四元數運動學筆記（5）IMU驅動的運動誤差方程

1.擴展卡爾曼濾波EKF
1.1線性化卡爾曼濾波
1.2偏差微分方程的推導
1.3線性化卡爾曼濾波的流程
1.4 離散EKF
2.誤差狀態的運動方程
2.1連續時間的IMU系統動態方程
2.1.1相關變量
2.1.2真實狀態運動方程(true-state kinematics equations )
2.1.3標稱狀態運動方程(normal-state kinematics equations )
2.1.4誤差狀態運動方程
2.2離散時間的IMU系統動態方程
2.2.1離散時間的IMU系統動態方程
2.2.2誤差狀態的雅克比矩陣和擾動矩陣
3.ESKF：Error state Kalman filter
3.1總體介紹
3.2ESKF vs KF
3.3fusing IMU with other complementary sensory data
3.3.1ESKF prediction
3.3.2ESKF correction
3.2.1 通過correction建立誤差的觀測
3.2.2 將觀測誤差注入標稱狀態
3.2.3ESKF reset
3.4 使用global angular errors的ESKF
3.4.1 連續時間誤差狀態運動方程
3.4.2 離散誤差狀態運動方程
3.4.3 globally ESKF vs locally ESKF

1.擴展卡爾曼濾波EKF

1.1線性化卡爾曼濾波

• 標稱值（normal value）：標稱值是基於系統軌跡的先驗猜測，比如系統方程代表飛行的動態性能，則標稱控制、狀態和輸出可能的飛行軌跡。標稱值是沒有任何噪聲和模型擾動的理想值，它和世界的軌跡（true-sate真實值）之間的各種因素產生的不確定性用誤差 $\delta x$表示。
• 系統的運動學方程一般采用狀態變量的微分形式表示，從而建立微分方程，將問題代數化。我的理解是，右邊的函數以系統相關參數表示出了左邊狀態變量的變化速率。
• 非線性系統方程的泰勒級數展開

  
  

1.2偏差微分方程的推導

• 需要注意的是這里假設真實的控制輸入是沒有不確定性的，即使有也將其歸於過程噪聲里。

  

1.3線性化卡爾曼濾波的流程

1.4 離散EKF

2.誤差狀態的運動方程

• 這里涉及到真實值(true-)，標稱值（nominal-），和誤差狀態值（error-state）三個量，真實值是有標稱值和誤差值組合而來。標稱值是有非線性方程而來的"大信號"，誤差值的線性方程而來滿足線性高斯濾波的“小信號”。

2.1連續時間的IMU系統動態方程

2.1.1相關變量

• 這里的相關變量中，輸入值是IMU的測量值 $a_m,b_m$，以及當地重力加速度 $g$;其他真值值變量的值都是由他們而來。其中normal值是一個抽象變量，表示理想值，是無法估計的。在融合中我們使用估計的真實值做為預測。
• 標稱值是完全理想值，我們把各種因素產生的真實值和標稱值的不確定性歸於變量 $\delta x$表示。
• 這里采用Hamilton 四元數的表述方法
• 這里的角速率 $w$使用的是locally的表述，這樣可以直接使用機體系b下的傳感器測量值 $w_m$。
• 這里的角度誤差 $\delta \theta$使用的也是locally的表述，這是很多文獻和算法采用的經典方式，不過有證明說采用globally的方式定義的角度誤差有更好的性質。
• 這里的加速度 $a$定義的是globally，我的理解是，方便計算導航系下的速度和位置。

  

2.1.2真實狀態運動方程(true-state kinematics equations )

• true-state 變量表示的運動方程.IMU bias的噪聲定義為隨機游走。

• 由於初始狀態姿態未知，這種不確定即重力加速度向量的不確定，但是實際工程中我們是制定初始狀態的，即重力加速度不確定不再存在，這里的方程也認為其為恆定值。

  

• IMU誤差來源分為兩種，內部bias $a_{b}t,w_{b}t$和測量噪聲 $a_n,w_n$.

• 這里的IMU真實值是沒有bias和測量噪聲的值,IMU加速度計測量加速度不包含重力加速度，即如果自由落體運動，不考慮噪聲加速度計的讀數是0。

  

• 利用IMU測量代替真實值的系統真實狀態運動方程 $\stackrel{\dot{}}{x}=f_t(x_t,u,w)$，該方程狀態為 $x_t$,由IMU的含噪聲測量值 $u_m$驅動，並受高斯白噪聲 $w$干擾.

  
  

2.1.3標稱狀態運動方程(normal-state kinematics equations )

• 標稱狀態對應系統沒有噪聲和擾動的情況。
• 可以這么理解，標稱值是把測量值 $a_m,w_m$和bias當做理想值計算出來的理想狀態；而實際值是在考慮測量值和bias有一部分應該不是我們想要的信號，是測量值減去噪聲和bias減去擾動計算得到的。我們的目的是優化兩者的誤差，誤差越小，測量值和bias包含的不確定性就越小。

  

2.1.4誤差狀態運動方程

• 方程的右邊變量是標稱狀態、IMU測量值和噪聲項。

  

• 其中速度誤差和角度誤差推導如下，主要技巧是省略小量的二次以上的項以及真實值和標稱值之間的數學關系。還有就是高斯噪聲的各向同性，使旋轉對其沒有影響。

  
  

2.2離散時間的IMU系統動態方程

2.2.1離散時間的IMU系統動態方程

• 誤差狀態運動方程的離散化主要是利用其微分方程進行積分，並將其表示為迭代形式。積分的方法有很多種。

  

• 離散化后的噪聲和連續狀態的噪聲存在差別，詳細見IMU Noise Model.

  

2.2.2誤差狀態的雅克比矩陣和擾動矩陣

• 通過觀察2.2.1很容易得到誤差狀態迭代方程的的雅克比矩陣；這里給出的是離散迭代方程的雅克比，和連續情況下誤差狀態的微分方程的雅克比，會有些不同。
• 這里的雅克比矩陣其實就是誤差狀態方程的狀態轉移矩陣。
• 這里的雅克比矩陣來源於真實狀態運動方程的離散化，即2.2.4誤差狀態運動方程的推導過程即是其離散化的過程。

  
  

3.ESKF：Error state Kalman filter

3.1總體介紹

• 這里介紹sola的ESKF，其中的誤差來源是IMU加速度計和陀螺儀讀數噪聲以及bias帶的擾動。首先推導了IMU的系統誤差動態方程，由於IMU造成的航位推算系統（DR:dead-reckoning position system）會隨着時間漂移(我的理解是由於在只有IMU作為測量傳感器的位姿測量系統里，航向角和位置都是靠積分得到的，因而會有航位的漂移問題)。所以將該方程做為KF的預測值，使用其他來源的數據如GPS或視覺作為矯正，構成融合數據的KF。
• 我的理解，作者這里提出的ESKF，和EKF其實是一回事。

3.2ESKF vs KF

• 作者比較了ESKF相對於KF的優點，有些不是完全理解
• 避免參數冗余，由於ESKF的參數和自由度一致，可以避免協方差矩陣的奇異性問題。
• ESKF的(誤差)變量操作一直在原點附近（誤差變量的值很小），可以避免可能地參數奇異性、萬向節鎖的問題，保證線性化的有效性。
• 誤差狀態值一致很小，意味着公式中誤差的二次以乘積都可以忽略，使得相應的雅克比矩陣比較簡單，提高計算速度。
• 可以讓KF的correction速度低於prediction，這點不是很理解，貼在這里吧。

  

3.3fusing IMU with other complementary sensory data

3.3.1ESKF prediction

• 具體變量定義見2.3.2，在2.2.3中，我們使用IMU含噪聲的測量值和bias去計算標稱值。並且沒有考慮測量值的噪聲項和擾動，從而產生了誤差。我們把所有不確定性引來的誤差放在誤差變量 $\delta x$中考慮，我們利用IMU測量值和高斯噪聲去計算誤差項，並將其作為ESKF的觀測，並利用其它來源的數據如GPS,視覺等來作為矯正（使誤差可觀）。
• 這里用IMU作為觀測，不是完全理解，我現在的理解是，IMU數據的引入使得預測項更加接近真實情況，參考《最優狀態估計》，這里的IMU數據即運動方程里的控制項，如果沒有測量值做控制項，我們需要進行比較強的假設，如恆速模型等；但是有了傳感器數據作為控制輸入，運動方程的模型就更加接近真實情況，有時我們也用里程計作為控制輸入，但相對於IMU，里程計的測量位置誤差會比較大（齒輪嚙合不理想等），而且對於飛行器來說，里程計是不能用的。
• 最后我們利用上次的狀態值、控制輸入、傳遞的誤差項來預測下一時刻的狀態。
• 預測階段均值和方差的更新公式：

  

3.3.2ESKF correction

3.2.1 通過correction建立誤差的觀測

• 這里使用另外的GPS或camera建立測量方程，並將觀測方程在標稱點附近進行線性化。

  

• 這里的雅克比矩陣推導使用了鏈式法則。

  
  

3.2.2 將觀測誤差注入標稱狀態

• 這樣做使得這次迭代標稱狀態和真實狀態相等，誤差重置為0；

  

3.2.3ESKF reset

• 重置的過程主要涉及到不確定性即方差的傳播,通常這里的雅克比可以近似為I。

  

• 這里的雅克比矩陣的推導

  

3.4 使用global angular errors的ESKF

• 之前有提到使用global angular error有更好的特性，這里進行相關的推導，所以涉及到的旋轉積分要采用globally的左擾動。
• 這里的角速度仍使用locally，考慮到測量值是在機體系下表示的

3.4.1 連續時間誤差狀態運動方程

• 相關推導和locally相似，在角度誤差微分方程推導里，四元數微分用的是locally，四元數積分用的是globally，這樣做的可行性在於角度誤差和角速率沒有耦合在兩者中使用。

  
  

3.4.2 離散誤差狀態運動方程

3.4.3 globally ESKF vs locally ESKF

null