【附源码+代码注释】误差状态卡尔曼滤波(error-state Kalman Filter),扩展卡尔曼滤波,实现GPS+IMU融合,EKF ESKF GPS+IMU

2021年6月23日更新:发现了一个讲卡尔曼滤波特别好的视频,但是需要科学上网。卡尔曼滤波视频

最近在学习卡尔曼滤波器,今天抽出点儿时间总结一下!

我的所有源码都放在Github的仓库里面了:eskf-gps-imu-fusion(记得要给我点star呀,哈哈)

在这篇博客中,我将会向你解释GPS融合IMU的扩展卡尔曼的推导过程,并且还会提供完整的源代码和数据。

首先来看一张实验结果,下图是我通过仿真gps和imu数据得到的融合结果,GPS的误差大约在5米,IMU的是中等精度,可以看到通过卡尔曼融合之后,误差降低非常多。

【附源码+代码注释】误差状态卡尔曼滤波(error-state Kalman Filter),扩展卡尔曼滤波,实现GPS+IMU融合,EKF ESKF GPS+IMU_第1张图片

红色的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, \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ϕE00ωcosLωsinLCbnε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=0fUfNfU0fEfNfE0ϕEϕNϕU+Cbnxyz(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εTT]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=[xyz]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×3Cbn03×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=0fUfNfU0fEfNfE0 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×3Cbn06×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_{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的系统下的状态方程和测量方程。

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^k1,vk,0)=Fk1P^k1Fk1T+Bk1QkBk1T=PˇkGkT(GkPˇKGkT+CkRkCkT)1=(IKkGk)Pˇk=xˇk+Kk(ykg(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 Fk1=I15×15+Ft(X^k1,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} Bk1=Bt(X^k1,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=Fk1X^k1+Bk1Wk=Fk1P^k1Fk1T+Bk1QkBk1T=PˇkGkT(GkPˇKGkT+CkRkCkT)1=(IKkGk)Pˇk=Xˇk+Kk(YkGkXˇ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之后,使用如下命令可以显示运行之后的轨迹,

evo_traj kitti fused.txt gt.txt measured.txt -p

d. 对于滤波器误差噪声的调节,是在config文件夹下的config.yaml文件中。

当你运行起我的代码之后,你可以或者如下的这样融合轨迹:
【附源码+代码注释】误差状态卡尔曼滤波(error-state Kalman Filter),扩展卡尔曼滤波,实现GPS+IMU融合,EKF ESKF GPS+IMU_第2张图片

一定要把上面的公式和代码结合起来看,如果实在难以理解,可以先把这个流程和实现过程弄明白,然后再边用变理解,不然你可能很难学会!

你可能感兴趣的:(传感器融合定位,算法,定位,滤波器,传感器融合,卡尔曼滤波算法)