2021年6月23日更新:发现了一个讲卡尔曼滤波特别好的视频,但是需要科学上网。卡尔曼滤波视频
最近在学习卡尔曼滤波器,今天抽出点儿时间总结一下!
我的所有源码都放在Github的仓库里面了:eskf-gps-imu-fusion(记得要给我点star呀,哈哈)
在这篇博客中,我将会向你解释GPS融合IMU的扩展卡尔曼的推导过程,并且还会提供完整的源代码和数据。
首先来看一张实验结果,下图是我通过仿真gps和imu数据得到的融合结果,GPS的误差大约在5米,IMU的是中等精度,可以看到通过卡尔曼融合之后,误差降低非常多。
红色的GPS的测量数据,蓝色是轨迹的真值,绿色是融合之后的轨迹,可以看到融合了imu之后的轨迹,相比于只有GPS的情况提升了很多,甚至与真实值都非常接近了。
卡尔曼滤波器在1958年被卡尔曼等人提出之后,经历了60多年,这期间有各种变体被提出来,但是核心的思想并没有变化。这其中比较突出的工作有,EKF、IEKF、ESKF、UKF等等。它们的方程有一些差异,但是用法基本区别不大,只要学会应用其中的一种,别的就问题不大了。
卡尔曼滤波器的公式推导,并不重要!
卡尔曼滤波器的公式推导,并不重要!
卡尔曼滤波器的公式推导,并不重要!
如果你初学卡尔曼滤波器,千万不要在公式推导和理解上花费太多的精力,这将会得不偿失,根据我的学习经验,想直接从卡尔曼的公式推到上去理解用法是比较难的,但是先不管原因,边应用边学习,将会是一个非常正确的做法,对于理解卡尔曼滤波器将会事半功倍!
本篇博客我们将探索一下状态误差卡尔曼(ESKF)的应用,它是卡尔曼滤波器的变种中应用最为广泛的一种,与EKF一样,它也是一种针对时变系统的非线性滤波器。但是与EKF不同的是,它的线性化是总是在0附近,因此线性化更准确。
根据我的经验,绝大部分的场景,ESKF就足够使用了。如果对于滤波有更高的要求,可以选择UKF,甚至PF(例子滤波)。
在融合IMU和GPS的数据时,因为IMU的频率更高,所以常常用IMU的姿态解算作为轨迹增量的预测,如果使用EKF滤波器,那么就是这种做法。由于我们这里介绍的是更为复杂的ESKF,所以这里并不是对导航信息做滤波,而是对导航信息中的误差进行滤波,因为误差是小量,线性化时更精确。
这里直接给出IMU的误差方程,由于误差方程的推导比较复杂,需要的知识比较多,而且这里主要强调的是ESKF的用法,所以就忽略IMU误差方程的推导。
如果你想系统学习IMU误差方程的推导,推荐你看《捷联惯导算法与组合导航原理》(严恭敏)
你也可以直接记住下面的结论,先继续往后学习,当你把整个流程都走一遍之后,你就明白了!
IMU误差方程:
ϕ ˙ = ϕ × ω i e n δ V ˙ = f n × ϕ + δ f n δ P ˙ = δ V (0) \begin {aligned} \dot \phi &= \phi \times \omega_{ie}^{n} \\ \delta \dot V &= f^n \times \phi + \delta f^n \\ \delta \dot P &= \delta V \end {aligned} \tag 0 ϕ˙δV˙δP˙=ϕ×ωien=fn×ϕ+δfn=δV(0)其中,
ϕ = [ ϕ E , ϕ N , ϕ U ] T \phi = [\phi_E, \phi_N, \phi_U]^T ϕ=[ϕE,ϕN,ϕU]T称为失准角
ω i e n = [ 0 , ω cos L , ω sin L ] T \omega_{ie}^n = [0, \omega\cos L, \omega \sin L]^T ωien=[0,ωcosL,ωsinL]T地球自转角速度的分量, ω = 7.292115 e ( − 5 ) r a d / s \omega = 7.292115e(-5)rad/s ω=7.292115e(−5)rad/s
δ w i b n = C b n ∗ [ ε x ε y ε z ] T \delta w_{ib}^n = C_b^n * [\varepsilon_x \ \varepsilon_y \ \varepsilon_z]^T δwibn=Cbn∗[εx εy εz]T, ε \varepsilon ε是陀螺仪的bias在body frame坐标系下的分量, C b n C_b^n Cbn是当前IMU body frame到导航系的变换矩阵
f n = [ f E f N f U ] T f^n = [f_E \ f_N \ f_U]^T fn=[fE fN fU]T 是IMU测量的加速度在导航系下的表示
δ f n = C b n ∗ [ ∇ x ∇ y ∇ z ] T \delta f^n = C_b^n * [\nabla_x \ \nabla_y\ \nabla_z]^T δfn=Cbn∗[∇x ∇y ∇z]T, ∇ \nabla ∇是加速度计的bias在body frame下的分量
然后,将以上变量全部带入公式(0),得到如下式子:
[ ϕ ˙ E ϕ ˙ E ϕ ˙ E ] = [ 0 − ϕ U ϕ N ϕ U 0 − ϕ E − ϕ N ϕ E 0 ] [ 0 ω cos L ω sin L ] − C b n [ ε x ε y ε z ] (1) \left[\begin{matrix} \dot \phi_E \\ \dot \phi_E\\ \dot \phi_E \end{matrix}\right] = \left[\begin{matrix} 0 & -\phi_U & \phi_N \\ \phi_U & 0 & -\phi_E \\ -\phi_N & \phi_E & 0 \end{matrix}\right] \left[\begin{matrix} 0 \\ \omega \cos L \\ \omega \sin L \end{matrix}\right] - C_b^n \left[\begin{matrix} \varepsilon_x \\ \varepsilon_y \\ \varepsilon_z \end{matrix}\right] \tag 1 ⎣⎡ϕ˙Eϕ˙Eϕ˙E⎦⎤=⎣⎡0ϕU−ϕN−ϕU0ϕEϕN−ϕE0⎦⎤⎣⎡0ωcosLωsinL⎦⎤−Cbn⎣⎡εxεyεz⎦⎤(1)
[ δ V ˙ E δ V ˙ N δ V ˙ U ] = [ 0 − f U f N f U 0 − f E − f N f E 0 ] [ ϕ E ϕ N ϕ U ] + C b n [ ∇ x ∇ y ∇ z ] (2) \left[\begin{matrix} \delta \dot V_E \\ \delta \dot V_N \\ \delta \dot V_U \end{matrix}\right] = \left[\begin{matrix} 0 & -f_U & f_N \\ f_U & 0 & -f_E \\ -f_N & f_E & 0 \end{matrix}\right] \left[\begin{matrix} \phi_E \\ \phi_N \\ \phi_U \end{matrix}\right] + C_b^n \left[\begin{matrix} \nabla_x \\ \nabla_y \\ \nabla_z \end{matrix}\right] \tag 2 ⎣⎡δV˙EδV˙NδV˙U⎦⎤=⎣⎡0fU−fN−fU0fEfN−fE0⎦⎤⎣⎡ϕEϕNϕU⎦⎤+Cbn⎣⎡∇x∇y∇z⎦⎤(2)
[ δ P ˙ E δ P ˙ N δ P ˙ U ] = [ δ V E δ V N δ V U ] (3) \left[ \begin{matrix} \delta\dot P_E \\ \delta\dot P_N \\ \delta\dot P_U \end{matrix} \right] = \left[ \begin{matrix} \delta V_E \\ \delta V_N \\ \delta V_U \end{matrix} \right] \tag 3 ⎣⎡δP˙EδP˙NδP˙U⎦⎤=⎣⎡δVEδVNδVU⎦⎤(3)
这里稍微解释一下误差方程,由于在做IMU的解算时,做了一些近似,而那些舍掉的东西就给IMU解算带来了误差。ESKF要做的就是对误差进行滤波,获得当前情况下误差的最优估计,然后将计算出来的位移、速度、姿态等量减去这个误差,修正由于近似带来的不准确。
IMU误差方程的推导可以参考《捷联惯导算法与组合导航原理》(严恭敏)
在滤波器中,状态方程一般都是写为如下形式:
X ˙ = F t X + B t W (4) \dot X = F_t X + B_t W \tag 4 X˙=FtX+BtW(4)其中, X X X就是我们要估计的状态量,对于IMU+GPS的融合系统,需要估计的状态量可以有多种选择,这里我们选:位移误差、速度误差、姿态误差、陀螺仪的bias误差、加速度计的bias误差,将其写成向量如下:
X = [ δ P T δ V T δ ϕ T ε T ∇ T ] T (5) X = \left[ \begin{matrix} \delta P^T& \delta V^T&\delta \phi^T& \varepsilon^T& \nabla^T \end{matrix} \right] ^T \tag 5 X=[δPTδVTδϕTεT∇T]T(5)其中:
δ P T = [ δ P E δ P N δ P U ] T \delta P^T=\left[\begin{matrix} \delta P_E & \delta P_N & \delta P_U \end{matrix}\right]^T δPT=[δPEδPNδPU]T
δ V T = [ δ V E δ V N δ V U ] T \delta V^T=\left[\begin{matrix} \delta V_E & \delta V_N & \delta V_U \end{matrix}\right]^T δVT=[δVEδVNδVU]T
δ ϕ T = [ δ ϕ E δ ϕ N δ ϕ U ] T \delta \phi^T=\left[\begin{matrix} \delta \phi_E &\delta \phi_N &\delta \phi_U \end{matrix}\right]^T δϕT=[δϕEδϕNδϕU]T
ε T = [ δ ε x ε y ε z ] T \varepsilon^T=\left[\begin{matrix} \delta \varepsilon_x & \varepsilon_y & \varepsilon_z \end{matrix}\right]^T εT=[δεxεyεz]T
∇ T = [ ∇ x ∇ y ∇ z ] T \nabla^T=\left[\begin{matrix} \nabla_x & \nabla_y & \nabla_z \end{matrix}\right]^T ∇T=[∇x∇y∇z]T
根据(1)(2)(3)等式,将状态量 X X X带入方程(4),则可以得到一个完整的 F t F_t Ft矩阵:
F t = [ 0 3 × 3 I 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 F 23 0 3 × 3 C b n 0 3 × 3 0 3 × 3 F 33 − C b n 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 ] (6) F_t = \left[ \begin{matrix} 0_{3 \times 3} & I_{3 \times 3} & 0_{3 \times 3} &0_{3 \times 3} &0_{3 \times 3} & \\ 0_{3 \times 3} & 0_{3 \times 3} & F_{23} & 0_{3 \times 3} & C_b^n \\ 0_{3 \times 3} & 0_{3 \times 3} & F_{33} & -C_b^n & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} &0_{3 \times 3} &0_{3 \times 3} &0_{3 \times 3} & \\ 0_{3 \times 3} &0_{3 \times 3} &0_{3 \times 3} &0_{3 \times 3} &0_{3 \times 3} \end{matrix} \right] \tag 6 Ft=⎣⎢⎢⎢⎢⎡03×303×303×303×303×3I3×303×303×303×303×303×3F23F3303×303×303×303×3−Cbn03×303×303×3Cbn03×303×303×3⎦⎥⎥⎥⎥⎤(6)
其中:
F 23 = [ 0 − f U f N f U 0 − f E − f N f E 0 ] F_{23} = \left[ \begin{matrix} 0 & -f_U & f_N \\ f_U & 0 & -f_E \\ -f_N & f_E & 0 \end{matrix} \right]\hspace{3cm} F23=⎣⎡0fU−fN−fU0fEfN−fE0⎦⎤ F 33 = [ 0 ω sin L − ω cos L − ω sin L 0 0 ω cos L 0 0 ] F_{33} = \left[ \begin{matrix} 0 & \omega \sin L & -\omega \cos L \\ -\omega \sin L & 0 & 0 \\ \omega \cos L & 0 & 0 \end{matrix} \right]\hspace{2cm} F33=⎣⎡0−ωsinLωcosLωsinL00−ωcosL00⎦⎤
C b n C_b^n Cbn是当前t时刻,IMU body frame到导航系 n n n的变换
对于(4)式已经给出了 F t F_t Ft的完整形式,对于 B t B_t Bt和 W W W,它们分别代表了噪声转移矩阵和IMU中陀螺仪与加速度计的bias噪声,它们的完整形式如下:
W = [ ω g x ω g y ω g z ω a x ω a y ω a z ] W = \left[ \begin{matrix} \omega_{gx} & \omega_{gy} &\omega_{gz} &\omega_{ax} &\omega_{ay} &\omega_{az} \end{matrix} \right] W=[ωgxωgyωgzωaxωayωaz]其中,前三个是陀螺仪在三个轴上的bias噪声,后三个是加速度计在三个轴上的bias噪声。
而 B t B_t Bt的完整形式如下:
B t = [ 0 3 × 3 0 3 × 3 0 3 × 3 C b n − C b n 0 3 × 3 0 6 × 3 0 6 × 3 ] B_t = \left[ \begin{matrix} 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & C_b^n \\ -C_b^n & 0_{3 \times 3} \\ 0_{6 \times 3} & 0_{6 \times 3} \end{matrix} \right] Bt=⎣⎢⎢⎡03×303×3−Cbn06×303×3Cbn03×306×3⎦⎥⎥⎤
前面说的预测值是来自于IMU,而对于观测值我们采用的是GPS的观测。
在滤波器中,观测方程的形式比较统一,一般写为:
Y = G t X + C t N (7) Y = G_t X + C_tN \tag 7 Y=GtX+CtN(7)
在IMU和GPS的融合中,GPS观测量一般之后位置,所以 Y Y Y的完整形式如下:
Y = [ δ P E δ P N δ P U ] T Y =\left[\begin{matrix} \delta P_E & \delta P_N & \delta P_U \end{matrix}\right]^T Y=[δPEδPNδPU]T
由(5)式 X X X状态量的形式,可以推算出 G t G_t Gt和 C t C_t Ct分别如下:
G t = [ I 3 × 3 I 3 × 12 ] G_t =\left[\begin{matrix} I_{3 \times 3} & I_{3\times 12} \end{matrix}\right] Gt=[I3×3I3×12] C t = [ I 3 × 3 ] C_t =\left[\begin{matrix} I_{3 \times 3} \end{matrix}\right] Ct=[I3×3]
等式(7)中的 N N N是观测噪声,它是由于GPS传感器不准确带来的,它的完整形式如下:
C t = [ n P E n P N n P U ] C_t =\left[\begin{matrix}n_{P_E}&n_{P_N}&n_{P_U} \end{matrix}\right] Ct=[nPEnPNnPU]
等式(4)和(7)就是IMU+GPS的系统下的状态方程和测量方程。
首先我们先写出ESKF滤波器的五个经典公式:
x ˇ k = f ( x ^ k − 1 , v k , 0 ) P ˇ k = F k − 1 P ^ k − 1 F k − 1 T + B k − 1 Q k B k − 1 T K k = P ˇ k G k T ( G k P ˇ K G k T + C k R k C k T ) − 1 P ^ k = ( I − K k G k ) P ˇ k x ^ k = x ˇ k + K k ( y k − g ( x ˇ k , 0 ) ) (8) \begin{aligned} \check x_k & = f(\hat x_{k-1}, v_k, 0) \\ \check P_k &= F_{k-1}\hat P_{k-1}F_{k-1}^T + B_{k-1}Q_kB_{k-1}^T \\ K_k &= \check P_k G_k^T(G_k \check P_K G_k^T + C_k R_k C_k^T)^{-1} \\ \hat P_k &= (I-K_kG_k)\check P_k \\ \hat x_k & = \check x_k + K_k(y_k - g(\check x_k, 0)) \end{aligned} \tag 8 xˇkPˇkKkP^kx^k=f(x^k−1,vk,0)=Fk−1P^k−1Fk−1T+Bk−1QkBk−1T=PˇkGkT(GkPˇKGkT+CkRkCkT)−1=(I−KkGk)Pˇk=xˇk+Kk(yk−g(xˇk,0))(8)
ESKF和EKF本身没有区别,只不过ESKF是使用的EKF滤波器对误差进行滤波
这五个公式就是ESKF滤波的基础。
根据2.1节和2.2节的推导,我们已经获得了IMU+GPS系统的状态方程和测量方程,现在我们要做的就是将状态方程和测量方程,应用到卡尔曼滤波器的五个公式中。
对于状态方程和测量方程的推导,都是在连续时间下完成的,想要应用到滤波器中,必须要离散化,离散化时按照采样时间进行离散化。对于(4)式中的 F t F_t Ft采用一阶泰勒近似,可以得到如下形式:
F k − 1 = I 15 × 15 + F t ( X ^ k − 1 , v k , 0 ) T (9) F_{k-1} = I_{15 \times 15} + F_t(\hat X_{k-1}, v_k, 0)T \tag 9 Fk−1=I15×15+Ft(X^k−1,vk,0)T(9) 公式(9)的实现过程:点此进入
而 B t B_t Bt的离散化形式为:
B k − 1 = B t ( X ^ k − 1 , v k , 0 ) T (10) B_{k-1} = B_t(\hat X_{k-1}, v_k, 0)T \tag {10} Bk−1=Bt(X^k−1,vk,0)T(10)其中, T T T为卡尔曼的滤波周期
公式(10)的实现过程:点此进入
对于观测方程,本生就是离散化的,所以可以直接使用。
将式子(9)(10)带入到(8)式中,得到最终的卡尔曼滤波器方程为:
X ˇ k = F k − 1 X ^ k − 1 + B k − 1 W k P ˇ k = F k − 1 P ^ k − 1 F k − 1 T + B k − 1 Q k B k − 1 T K k = P ˇ k G k T ( G k P ˇ K G k T + C k R k C k T ) − 1 P ^ k = ( I − K k G k ) P ˇ k X ^ k = X ˇ k + K k ( Y k − G k X ˇ k ) (11) \begin{aligned} \check X_k & = F_{k-1}\hat X_{k-1} + B_{k-1}W_k \\ \check P_k &= F_{k-1}\hat P_{k-1}F_{k-1}^T + B_{k-1}Q_kB_{k-1}^T \\ K_k &= \check P_k G_k^T(G_k \check P_K G_k^T + C_k R_k C_k^T)^{-1} \\ \hat P_k &= (I-K_kG_k)\check P_k \\ \hat X_k & = \check X_k + K_k(Y_k - G_k \check X_k) \end{aligned} \tag {11} XˇkPˇkKkP^kX^k=Fk−1X^k−1+Bk−1Wk=Fk−1P^k−1Fk−1T+Bk−1QkBk−1T=PˇkGkT(GkPˇKGkT+CkRkCkT)−1=(I−KkGk)Pˇk=Xˇk+Kk(Yk−GkXˇk)(11)等式组(11)中的前两个等式对应的是预测过程,它主要用来预测状态量和状态量对应的协方差,第三个等式的 K k K_k Kk是卡尔曼增益,它是用来决策当前次的预测和测量中,更应该相信谁。第四个和第五个等式则是矫正前两个方程预测时的误差。
公式组(11)的五个公式的代码实现过程分别是:①、②、③、④、⑤
至此,整个基于IMU和GPS的状态误差卡尔曼滤波(ESKF)推导完成。
ESKF是对状态的误差进行矫正,所以ESKF每次迭代完成之后,必须要将状态量 X X X重新设置为零,但是协方差矩阵 P P P中的数值需要保留,代码在此。
对于公式(11)中的 Y K Y_K YK它对应的就是测量误差,对于GPS和IMU的系统,它的计算方法是:GPS的测量值减去IMU的预测值,代码在此
每次使用卡尔曼迭代完成之后,都需要将最终得到的误差,重新带回到预测的位置、速度、方向中消除掉这个误差,代码在此
上面只是把结论给出来了,如果确实需要了解其中卡尔曼滤波器的推导过程,可以参考《机器人学中的状态估计》或者《Quaternion kinematics for the error-state Kalman filter》
可观测性分析:在设计滤波器时,有些状态量是不可观,如果此时继续将这些状态量带入滤波器进行滤波,可能导致滤波器不收敛,或者收敛到一个错误的值。可观测度就是用来判断状态量是否都会收敛。
可观测度分析:可观测分析是为了评估状态量在滤波时收敛的快慢和收敛的精度,对于一些可观测度非常小的状态量可以考虑进行限制或者直接扔掉。
本文所讲的IMU+GPS滤波系统是一种时变系统,对于时变系统它的可观测性和可观测度分析,可以参考论文《Observability Analysis of Piece-Wise Constant Systems》
这里我不再展示详细的推导过程,直接给出结论,如果你对这部分内容有兴趣,请阅读上面这篇论文,并且在我的代码中也给出了实现过程,点此进入,你可以参考。
IMU+GPS的扩展卡尔曼滤波器系统,可观测度和可观测性分析结论:
上面展示的这些流程,就是从IMU的误差方程到扩展卡尔曼滤波器的所有流程,其中很多的推导比较难理解,我能省略的就尽量省略了。
但是有以下的一些点,你需要注意:
a. 我的所有源码都放在Github的仓库里面了:eskf-gps-imu-fusion
b. 在代码的文件夹/data/raw_data
中我提供了方针的IMU和GPS数据,它们的生成是通过gnss-ins-sim,关于gnss-ins-sim的使用方法,作者提供了比较详细的使用教程,你按照教程就能学会它的使用方法,另外你需要注意的是,不要把坐标系弄混了。我在eskf-gps-imu-fusion文件夹下的gnss-ins-sim文件中放了我所使用的脚本和运动控制文件,你按照readme中的方法操作,就能实现数据仿真了。
c. 融合过程中产生的轨迹和融合数据都保存在data
文件夹下,它们都是以kitti的姿态格式保存的,如果你想可视化它们,我推荐你使用evo,我写过一篇介绍它的使用方法的博客:点击入口,如果你不会使用,可以参考一下。
安装好evo之后,使用如下命令可以显示运行之后的轨迹,
evo_traj kitti fused.txt gt.txt measured.txt -p
d. 对于滤波器误差噪声的调节,是在config
文件夹下的config.yaml
文件中。
一定要把上面的公式和代码结合起来看,如果实在难以理解,可以先把这个流程和实现过程弄明白,然后再边用变理解,不然你可能很难学会!