大家好，又见面了，我是你们的朋友全栈君。
2021年6月23日更新:发现了一个讲卡尔曼滤波特别好的视频，但是需要访问国外网站。卡尔曼滤波视频
最近在学习卡尔曼滤波器，今天抽出点儿时间总结一下！
我的所有源码都放在Github的仓库里面了：eskf-gps-imu-fusion（记得要给我点star呀，哈哈）
在这篇博客中，我将会向你解释GPS融合IMU的扩展卡尔曼的推导过程，并且还会提供完整的源代码和数据。
首先来看一张实验结果，下图是我通过仿真gps和imu数据得到的融合结果，GPS的误差大约在5米，IMU的是中等精度，可以看到通过卡尔曼融合之后，误差降低非常多。
红色的GPS的测量数据，蓝色是轨迹的真值，绿色是融合之后的轨迹，可以看到融合了imu之后的轨迹，相比于只有GPS的情况提升了很多，甚至与真实值都非常接近了。
1.前言
卡尔曼滤波器在1958年被卡尔曼等人提出之后，经历了60多年，这期间有各种变体被提出来，但是核心的思想并没有变化。这其中比较突出的工作有，EKF、IEKF、ESKF、UKF等等。它们的方程有一些差异，但是用法基本区别不大，只要学会应用其中的一种，别的就问题不大了。
卡尔曼滤波器的公式推导，并不重要! 卡尔曼滤波器的公式推导，并不重要! 卡尔曼滤波器的公式推导，并不重要! 如果你初学卡尔曼滤波器，千万不要在公式推导和理解上花费太多的精力，这将会得不偿失，根据我的学习经验，想直接从卡尔曼的公式推到上去理解用法是比较难的，但是先不管原因，边应用边学习，将会是一个非常正确的做法，对于理解卡尔曼滤波器将会事半功倍！
2. 状态误差卡尔曼（Error-State Kalman Filter，ESKF）
本篇博客我们将探索一下状态误差卡尔曼(ESKF)的应用，它是卡尔曼滤波器的变种中应用最为广泛的一种，与EKF一样，它也是一种针对时变系统的非线性滤波器。但是与EKF不同的是，它的线性化是总是在0附近，因此线性化更准确。
根据我的经验，绝大部分的场景，ESKF就足够使用了。如果对于滤波有更高的要求，可以选择UKF，甚至PF(例子滤波)。
2.1 状态方程的推导
在融合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, omegacos 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} deltadot P_E \ deltadot P_N \ deltadot 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⎦⎥⎥⎤
2.2 观测方程的推导
前面说的预测值是来自于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_{3times 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的系统下的状态方程和测量方程。
2.3 构建ESKF滤波器
首先我们先写出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》
2.4 可观测性和可观测度分析
可观测性分析：在设计滤波器时，有些状态量是不可观，如果此时继续将这些状态量带入滤波器进行滤波，可能导致滤波器不收敛，或者收敛到一个错误的值。可观测度就是用来判断状态量是否都会收敛。
可观测度分析：可观测分析是为了评估状态量在滤波时收敛的快慢和收敛的精度，对于一些可观测度非常小的状态量可以考虑进行限制或者直接扔掉。
本文所讲的IMU GPS滤波系统是一种时变系统，对于时变系统它的可观测性和可观测度分析，可以参考论文《Observability Analysis of Piece-Wise Constant Systems》
这里我不再展示详细的推导过程，直接给出结论，如果你对这部分内容有兴趣，请阅读上面这篇论文，并且在我的代码中也给出了实现过程，点此进入，你可以参考。
IMU GPS的扩展卡尔曼滤波器系统，可观测度和可观测性分析结论：
- 载体静止或着匀速运动时：航向角， x 轴加速度bias和 y 轴加速度bias均不可观，而且z 轴角速度bias虽然收敛，但是收敛较慢；
- 载体加减速时： 所有状态量基本都可观，z轴角速度零偏，x 轴加速度bias，y轴加速度bias收敛较慢，需要几十秒，但是比静止和匀速运动时提升了很多；
- 载体转向时： 所有状态量基本都可观，但是航向角和加速度计bias以及角速度计bias参数收敛速度有一些慢，需要几十秒才能收敛，但是比静止和匀速运动时提升了很多。
2.5 总结
上面展示的这些流程，就是从IMU的误差方程到扩展卡尔曼滤波器的所有流程，其中很多的推导比较难理解，我能省略的就尽量省略了。
但是有以下的一些点，你需要注意：
- 上面的IMU误差方程是基于高精度的IMU推导的，考虑了地球的转速了，如果你的IMU精度一般，你可以忽略地球的转速，也就是上式（6）中的 F 33 F_{33} F33设置为0矩阵，并不会对精度产生很大的影响；
- 另外有一点你一定要格外的注意，上面方程的推导都是基于导航坐标系的，而你的IMU结果是针对body坐标系的，切记不要弄混了坐标系；
- 矩阵 F 33 F_{33} F33是加速度计敏感到的加速度在导航系下的表示，代码中不要忘记了这一步，另外还要注意这里的加速度不需要去掉重力加速度；
- 还要格外的注意单位的统一，GPS的测量值是经纬高，要转换到导航系下，表示成 x , y , z x,y,z x,y,z单位是米，并且IMU的角速度是以弧度为基础的；
- 卡尔曼滤波器公式（11）中的第一个等式是状态的传递方程，后面有一个噪声项，你可以加上，也可以忽略，根据你的实际情况去决定是否需要。我的实验结果是，加不加影响不大。
3.代码实现
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之后，使用如下命令可以显示运行之后的轨迹，
代码语言：javascript复制evo_traj kitti fused.txt gt.txt measured.txt -pd. 对于滤波器误差噪声的调节，是在config文件夹下的config.yaml文件中。
当你运行起我的代码之后，你可以或者如下的这样融合轨迹：
一定要把上面的公式和代码结合起来看，如果实在难以理解，可以先把这个流程和实现过程弄明白，然后再边用变理解，不然你可能很难学会！
发布者：全栈程序员栈长，转载请注明出处：https://javaforall.cn/150695.html原文链接：https://javaforall.cn
