慣性導航一般集成在GPS設備中，都是由供應商集成，那在這裏有什麼討論的必要呢，要知道在車輛行駛中，我們可以拿到GPS的yawrate和speed信號，而且車輛本身還有一套傳感器獲取yawrate和speed，又因爲航跡推算是自動駕駛很重要的一部分，所以理解慣性導航的工作原理，能很好地幫助我們做基於車身的航跡推算。
1. 慣性導航
目前GNSS+IMU構成的組合導航系統是主流的定位系統方案，慣性導航系統是唯一可以輸出完備的六自由度數據的設備、數據更新頻率高、是定位信息的融合中心。
慣導中使用的核心算法主要包括3種：1. 慣性導航解算算法；2. 組合導航的卡爾曼濾波器的耦合。3. 環境特徵信息與慣性導航融合。
2. 硬件及原理
慣性導航系統（INS）是利用慣性傳感器（IMU）測量載體的比力及角速度信息，結合給定的初始條件，與 GNSS等系統的信息融合，從而進行實時推算速度、位置、姿態等參數的自主式導航系統。具體來說慣性導航系統屬於一種推算導航方式。即從一已知點的位置根據連續測得的運載體航向角和速度推算出其下一點的位置，因而可連續測出運動體的當前位置。
慣性導航系統採用加速度計和陀螺儀傳感器來測量載體的運動參數。其中三個垂直佈置的陀螺儀用於測量載體繞自身三個座標軸的轉動角速度，同時也敏感地球自轉的角速度。
加速度計基於牛頓第二定律，採用電容式、壓阻式或熱對流原理，通過在加速過程中對質量塊對應慣性力的測量來獲得加速度值。用來測量運動體座標系上各軸的加速度。
慣導通過對陀螺儀測量的角速度進行積分運算和座標變換，計算車體的姿態角（橫滾、俯仰角）和方位角。根據姿態角可以計算出重力加速度在各個座標軸上的分量，加速度計測量得的各軸加速度，減去重力加速度分量後積分，得到速度和位置。慣導計算得到的狀態，用於預測車輛當前的位置，再和衛星定位接收機得到的位置（或觀測數據）進行比較。比較的偏差包含了慣導的推算誤差和衛星接收機的定位誤差，通過數據融合算法進行加權後，用於修正慣導的預測，讓慣導的預測越來越準確。
3. 慣性導航解算算法
通常分以下幾步：
- 姿態更新：對陀螺儀輸出的角速度進行積分得到姿態增量，疊加到上次的姿態上；
- 座標轉換：從IMU載體座標系到位置、速度求解座標系（慣性座標系）；
- 速度更新：需要考慮重力加速度的去除，得到慣性系下的加速度，通過積分得到速度；
- 位置更新：通過速度積分得到位置。
在慣性導航中，導航方程的每一次迭代都需要利用上一次的導航結果作爲初始值，因此慣導的初始化是比較重要的部分之一。姿態對準是指得到IMU的roll, pitch, yaw。roll, pitch的對準過程一般稱爲調平。使當車靜止時，加速度計測量的比力僅由重力導致，可以通過f=C*g來求解；對於非常高精度的IMU可通過羅經對準的方式，車靜止時，通過測量載體系中的地球自轉來確定載體的方位(yaw)。
4. 組合導航的卡爾曼濾波器的耦合
使用Kalman濾波器的耦合，對IMU和GNSS即點雲定位結果進行融合。可分爲松耦合和緊耦合兩種方法。
松耦合濾波器採用位置、速度量測值和解算的位置速度之差作爲組合導航濾波器輸入，也即卡爾曼濾波器的量測量。緊耦合的數據包括GNSS的導航參數、定位中的僞距、距離變化等。
以百度阿波羅使用的慣導系統爲例，採用了松耦合的方式，並且使用了一個誤差卡爾曼濾波器。慣性導航解算的結果用於Kalman濾波器的時間更新，即預測；而GNSS、點雲定位結果用於Kalman濾波器的量測更新。Kalman濾波會輸出位置、速度、姿態的誤差用來修正慣導模塊，IMU期間誤差用來補償IMU原始數據。
5. 環境特徵信息與慣性導航融合
目前常用的GNSS+IMU組合慣導方案在一些場景的定位精度穩定性仍不能完全滿足自動駕駛的要求。例如，城市樓宇羣、地下車庫等GNSS長時間信號微弱的場景下，依靠GNSS信號更新精確定位穩定性不足，因此必須引入新的精確定位更新數據源，在組合慣導中引入並融合激光雷達/視覺傳感定位等環境信息進行融合定位成爲必然趨勢。
以百度阿波羅的多傳感器融合定位系統解決方案爲例，慣性導航系統處於定位模塊的中心位置，模塊將IMU、GNSS、Lidar等定位信息進行融合，通過慣性導航系統解算修正後最終輸出滿足自動駕駛需求的6個自由度的高精度位置信息。
