參考《概率機器人》、《Principles of GNSS，lnertial and Multisensor lntegrated Navigation Systems
(Second Edition)》

一、卡爾曼濾波(KF)

卡爾曼濾波(KF,kalman filter）是一種線性濾波方法。
狀態方程：噪聲為Rt

觀測方程：

預測更新：

1、如果 P/R小，相應的卡爾曼增益會很小并且狀態估計收斂到真值的速度會更慢 對應到
系統的變化，狀態估計過程會變慢 相反，如果 P/R 太大，卡爾曼增益會非常
大，這將使濾波更側重于最近的觀測值，從而會因為觀測噪聲對狀態估計產生了
太大的影響而導致濾波不穩，或產生估計偏差。有時，通過系統模型狀態估計與
觀測噪聲會形成正反饋，并最終導致濾波器的迅速發散，遠離真值。這里假設觀測是比較準的，預測步方差P過大/觀測噪聲過小，類似于比例因子K比較大，超調量會比較大，容易振蕩；預測步方差P過小/觀測噪聲過大，K較小，收斂速度比較慢

二、擴展卡爾曼濾波(EKF)

《概率機器人》P40
擴展卡爾曼濾波（EKF，extend kalman filter）是一種非線性濾波方法。先將非線性狀態轉移和觀測方程使用一階泰勒展開線性化，然后使用卡爾曼方法預測和更新即可。
非線性方程：

線性化后：

預測：

三、誤差狀態卡爾曼濾波(ESKF)

誤差傳播
1）慣性傳感器誤差，即陀 螺和加速度計的噪聲、零偏、比例因子誤差和交軸耦合等各類誤差。
2）導航初始化誤差，包括位置、速度和姿態等參數的初值誤差。
3）重力誤差，主要是重力模型誤差和位置誤差帶來的重力計算誤差等
4）算法和計算誤差
基于擾動方法推導誤差狀態方程和觀測方程
狀態方程推導
1）首先將有誤差的變量建模為誤差狀態δx=x_true - x,x_true為真值，x為估計值，姿態誤差需要單獨定義，在計算或得到變量 x 的過程中是否引入含有誤差的變量，或者誤差幅度是否可以小到忽略不計。
2）求解誤差狀態δx導數，得到連續狀態的狀態轉移函數，其導數與x_true 和x導數差相等，根據x導數解析解，x_true誤差變量加上擾動δx，x導數不變，再將兩者相減得到誤差狀態轉移方程；姿態根據q_true導數解析解和擾動方差2種不同的求導方式，然后取等號進行求解
3）離散化：δx導數=(δx_k-δx_k-1)/δt,根據中值法、歐拉法不同方法離散化，如VINS推導；或采用如下圖牛小驥團隊《組合導航》課程方式離散化。注意一般給定的是連續狀態的噪聲，需要轉為離散化誤差狀態噪聲再套卡爾曼公式，噪聲為一段時間的累積

觀測方程：
4）觀測誤差更新方程不需要求導，將觀測減去估計值構建，需要把觀測噪聲轉為誤差觀測狀態噪聲，得到誤差狀態協方差更新方程與狀態協方差不同，噪聲是某個時刻的

5)狀態反饋：使用誤差狀態δx_k更新當前狀態