【惯性/卫星组合导航】使用EKF拓展卡尔曼滤波求解惯性/卫星松组合模型

🚀 惯性/卫星组合导航有多种耦合模型，本文针对最简单的松组合模型进行讲解。该模型主要利用卫星端的数据对惯性端的数据进行更新，在更新中不断的消除累计误差。

本文先在基本微分方程理论中简要介绍EKF(Extended Kalman Filter)拓展卡尔曼滤波中会使用到的一些微分方程，接下来在系统/观测体系构建中讲解如何利用微分方程构建系统方程和观测方程，最后在具体EKF计算流程中讲解卫星/惯性组合导航的解算过程。

---

01
🌔基本微分方程理论

这里微分方程是以$\delta X_{eb}^n$为基准，即以变量$X$的变化量$\delta X$为目标，求取载体坐标系$b$相对于地心地固坐标系$e$在当地地理坐标系$n$下的投影表示。另外，由于涉及很多先验知识和复杂的理论推导，于是就直接给出结果，附带一点数学说明。

1.1 姿态误差微分方程

⭐️$\varphi$为x,y,z三个方向的姿态变化。

$\dot{\varphi }={\dot{\varphi }}_b^n=-w_{in}^n\times \varphi +\delta w_{in}^n+(-C_b^n\delta w_{ib}^b)$

$\varphi =\left[\begin{array}{ccc}\delta \alpha & \delta \beta & \delta \gamma \end{array}\right]$

$\{\begin{array}{l}{w}_{in}^n=w_{ie}^n+w_{en}^n={\left[\begin{array}{ccc}\Omega \cos L& 0& -\Omega \sin L\end{array}\right]}^T+{\left[\begin{array}{ccc}\frac{v_E}{R_E+h}& -\frac{v_N}{R_N+h}& -\frac{v_E\tan L}{R_E+h}\end{array}\right]}^T\\ ={\left[\begin{array}{ccc}\Omega \cos L+\frac{v_E}{R_E+h}& -\frac{v_N}{R_N+h}& -\Omega \sin L-\frac{v_E\tan L}{R_E+h}\end{array}\right]}^T\\ \delta w_{in}^n=\left[\begin{array}{ccc}0& \frac{1}{R_E+h}& 0\\ -\frac{1}{R_N+h}& 0& 0\\ 0& -\frac{\tan L}{R_E+h}& 0\end{array}\right]\left[\begin{array}{c}\delta v_N\\ \delta v_E\\ \delta v_D\end{array}\right]+\left[\begin{array}{ccc}-\Omega \sin L& 0& -\frac{v_E}{{(R_E+h)}^2}\\ 0& 0& \frac{v_N}{{(R_N+h)}^2}\\ -\Omega \cos L-\frac{v_E}{(R_E+h){\cos }^{2}L}& 0& \frac{v_E\tan L}{{(R_E+h)}^2}\end{array}\right]\left[\begin{array}{c}\delta L\\ \delta \lambda \\ \delta h\end{array}\right]\end{array}$

1.2 速度误差微分方程

⭐️$\delta v$为北向N、东向E、地向D三个维度的速度变化。

$\delta \dot{v}=\delta {\dot{v}}_{eb}^n=[f^n\times ]\varphi -[(2w_{ie}^n+w_{en}^n)\times ]\delta v_{eb}^n-[(2\delta w_{ie}^n+\delta w_{en}^n)\times ]v_{eb}^n+(C_b^n\delta f^b+\delta g^n)$

$[(2w_{ie}^n+w_{en}^n)\times ]=[{\left[\begin{array}{ccc}2\Omega \cos L+\frac{v_E}{R_E+h}& -\frac{v_N}{R_N+h}& -2\Omega \sin L-\frac{v_E\tan L}{R_E+h}\end{array}\right]}^T\times ]$

其中重力项$\delta g^n$常常忽略。

1.3 位置误差微分方程

⭐️$\delta p$为纬度、经度、高度三个维度的位置变化。

$\delta \dot{p}=\left[\begin{array}{c}\delta \dot{L}\\ \delta \dot{\lambda }\\ \delta \dot{h}\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{R_N+h}& 0& 0\\ 0& \frac{1}{(R_E+h)\cos L}& 0\\ 0& 0& -1\end{array}\right]\left[\begin{array}{c}\delta v_N\\ \delta v_E\\ \delta v_D\end{array}\right]+\left[\begin{array}{ccc}0& 0& -\frac{v_N}{(R_N+h{)}^2}\\ \frac{v_E\sin L}{(R_E+h){\cos }^{2}L}& 0& -\frac{v_E}{(R_E+h{)}^2\cos L}\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}\delta L\\ \delta \lambda \\ \delta h\end{array}\right]$

1.4 传感器误差微分方程

⭐️将陀螺仪(g)和加表(a)的系统误差模拟成随机游走过程：

$\{\begin{array}{l}\delta w_{ib}^b={ϵ}^b+w_g=ϵ+w_g\\ \delta f^b={\nabla }^b+w_a=\nabla +w_a\end{array}$

且为了方便考虑，取$\dot{ϵ}=0,\dot{\nabla }=0$。

---

02
🌔系统/观测体系构建

2.1 系统方程

⭐️以姿态误差/速度误差/姿态误差/传感器误差作为状态变量(INS-GNSS)：

$\delta x={\left[\begin{array}{c}\varphi \\ \delta v\\ \delta p\\ \delta ϵ\\ \delta \nabla \end{array}\right]}_{15\times 1}$

传感器噪声定义如下：

$w={\left[\begin{array}{cccccc}{w}_{gx}& w_{gy}& w_{gz}& w_{ax}& w_{ay}& w_{az}\end{array}\right]}_{6\times 1}^T$

结合上述微分方程的结果，可以推导出：
(所求量都是$b$相对于$e$在$n$下的表示，$F$为系统矩阵，$G$为噪声转移矩阵)

$\delta \dot{x}=F\delta x+Gw$

$F={\left[\begin{array}{ccccc}{F}_{11}& F_{12}& F_{13}& -C_b^n& 0_{3\times 3}\\ F_{21}& F_{22}& F_{23}& 0_{3\times 3}& C_b^n\\ F_{31}& F_{32}& F_{33}& 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}& 0_{3\times 3}\end{array}\right]}_{15\times 15}$

$G={\left[\begin{array}{cc}-C_b^n& 0_{3\times 3}\\ 0_{3\times 3}& 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}\end{array}\right]}_{15\times 6}$

F	$F_{i1}$	$F_{i2}$	$F_{i3}$
$F_{1j}$	$\left[\begin{array}{ccc}0& -\Omega \sin L-\frac{v_E\tan L}{R_E+h}& \frac{v_N}{R_N+h}\\ \Omega \sin L+\frac{v_E\tan L}{R_E+h}& 0& \Omega \cos L+\frac{v_E}{R_E+h}\\ -\frac{v_N}{R_N+h}& -\Omega \cos L-\frac{v_E}{R_E+h}& 0\end{array}\right]$	$\left[\begin{array}{ccc}0& \frac{1}{R_E+h}& 0\\ -\frac{1}{R_N+h}& 0& 0\\ 0& -\frac{\tan L}{R_E+h}& 0\end{array}\right]$	$\left[\begin{array}{ccc}-\Omega \sin L& 0& -\frac{v_E}{{(R_E+h)}^2}\\ 0& 0& \frac{v_N}{{(R_N+h)}^2}\\ -\Omega \cos L-\frac{v_E}{(R_E+h){\cos }^{2}L}& 0& \frac{v_E\tan L}{{(R_E+h)}^2}\end{array}\right]$
$F_{2j}$	$\left[\begin{array}{ccc}0& -f_D^n& f_E^n\\ f_D^n& 0& -f_N^n\\ -f_E^n& f_N^n& 0\end{array}\right]$	$\left[\begin{array}{ccc}\frac{v_D}{R_N+h}& -\frac{2v_E\tan L}{R_E+h}-2\Omega \sin L& \frac{v_N}{R_N+h}\\ 2\Omega \sin L+\frac{v_E\tan L}{R_E+h}& \frac{v_N\tan L}{R_E+h}+\frac{v_D}{R_E+h}& 2\Omega \cos L+\frac{v_E}{R_E+h}\\ -\frac{2v_N}{R_N+h}& -\frac{2v_E}{R_E+h}-2\Omega \cos L& 0\end{array}\right]$	$\left[\begin{array}{ccc}-2v_E\Omega \cos L-\frac{v_E^2}{(R_E+h){\cos }^{2}L}& 0& -\frac{v_N{v}_D}{(R_N+h{)}^2}+\frac{v_E^2\tan L}{(R_E+h{)}^2}\\ 2v_N\Omega \cos L+\frac{v_N{v}_E}{(R_E+h){\cos }^{2}L}-2v_D\Omega \sin L& 0& -\frac{v_N{v}_E\tan L}{(R_E+h{)}^2}-\frac{v_D{v}_E}{(R_E+h{)}^2}\\ 2\Omega v_E\sin L& 0& \frac{v_N^2}{(R_N+h{)}^2}+\frac{v_E^2}{(R_E+h{)}^2}\end{array}\right]$
$F_{3j}$	$\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]$	$\left[\begin{array}{ccc}\frac{1}{R_N+h}& 0& 0\\ 0& \frac{1}{(R_E+h)\cos L}& 0\\ 0& 0& -1\end{array}\right]$	$\left[\begin{array}{ccc}0& 0& -\frac{v_N}{R_N+h}\\ \frac{v_E\sin L}{(R_E+h){\cos }^{2}L}& 0& -\frac{v_E}{(R_E+h{)}^2\cos L}\\ 0& 0& 0\end{array}\right]$

2.2 观测方程

⭐️这里注意构造$\delta x^{\prime }$时的相减顺序, 这与后续EKF推导的加减号有关。

$\delta z=H\delta x^{\prime }+\upsilon ={\left[\begin{array}{c}{v}_{INS}^n-v_{GNSS}^n\\ L_{INS}^n-L_{GNSS}^n\\ {\lambda }_{INS}^n-{\lambda }_{GNSS}^n\\ h_{INS}^n-h_{GNSS}^n\end{array}\right]}_{6\times 1}+{\upsilon }_{6\times 1}$

$H={\left[\begin{array}{ccccc}{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}& I_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\end{array}\right]}_{6\times 15}$

其中，$H$是观测矩阵，$\upsilon$是观测噪声。

如果考虑倒惯导与接收机天线之间的杆臂长，则还需要对GNSS处的值进行补偿修改。

---

03
🌔具体EKF计算流程

经典的KF卡尔曼滤波可以参考这篇How a Kalman filter works, in pictures，他讲的很好。(需要科学上网工具)

KF卡尔曼滤波以$x$作为状态量，EKF拓展卡尔曼滤波以$\delta x$作为状态量。
为了说明大意，简化基础微分方程理论，而有以下设置：

状态向量(定义为INS-GNSS)	INS结果(除此外还有$C_b^n$)	GNSS结果
$\delta \hat{x}={\left[\begin{array}{c}\varphi \\ \delta v\\ \delta p\end{array}\right]}_{9\times 1}$	$\hat{x}={\left[\begin{array}{c}{p}_{INS}\\ v_{INS}\end{array}\right]}_{6\times 1}$	$z={\left[\begin{array}{c}{p}_{GNSS}\\ v_{GNSS}\end{array}\right]}_{6\times 1}$

3.1 状态预测

获取状态初值9x1(认为经过组合导航补偿后，下一轮迭代偏差已修正为0)：

$\delta {\hat{x}}_{k-1}=0_{9\times 1}$

根据微分方程，构造系统矩阵9x9：

$F={\left[\begin{array}{ccc}{F}_{11}& F_{12}& F_{13}\\ F_{21}& F_{22}& F_{23}\\ F_{31}& F_{32}& F_{33}\end{array}\right]}_{9\times 9}$

⭐获取状态预测值9x1(也为0，且驱动力$u_k=0$，驱动矩阵$B=0$)：

$\delta {\hat{x}}_k=F\delta {\hat{x}}_{k-1}+B{u}_k$

3.2 状态协方差预测

由陀螺仪噪声向量$w_g$和加表噪声向量$w_a$，构造传感器噪声矩阵6x6：

$W=diag([w_g{w}_a]\odot [w_g{w}_a]{)}_{6\times 6}$

由姿态，速度，位置的微分方程，构造系统噪声转移矩阵9x6：

$G={\left[\begin{array}{cc}-C_b^n& 0_{3\times 3}\\ 0_{3\times 3}& C_b^n\\ 0_{3\times 3}& 0_{3\times 3}\end{array}\right]}_{9\times 6}$

由传感器噪声和噪声转移矩阵，计算系统噪声9x9：

$Q=(GW{G}^T{)}_{9\times 9}$

⭐根据上一步迭代的状态协方差(最初的$P_0$按情况设置就行)，预测本次迭代的状态协方差9x9：

$P_k=F{P}_{k-1}{F}^T+Q$

3.3 卡尔曼增益计算

由观测噪声向量$\upsilon$，构造观测噪声矩阵6x6：

$R=diag(\upsilon \odot \upsilon {)}_{6\times 6}$

由观测向量$z_k$和状态向量$\delta x$的设置，构造观测矩阵6x9：

$H={\left[\begin{array}{ccc}{0}_{3\times 3}& 0_{3\times 3}& I_{3\times 3}\\ 0_{3\times 3}& I_{3\times 3}& 0_{3\times 3}\end{array}\right]}_{6\times 9}$

⭐构造卡尔曼增益9x9：

$K=P_k{H}^T(H{P}_k{H}^T+R{)}^{-1}$

3.4 状态更新

利用惯性导航解算，计算得此时的INS结果6x1：

${\hat{x}}_k=f({\hat{x}}_{k-1})$

⭐进行状态更新9x1(这里偏差计算的顺序与状态变量的定义有关)：

$\delta {\hat{x}}_k^{\prime }=\delta {\hat{x}}_k+K({\hat{x}}_k-z_k)$

注意将更新后的状态置零后再送入下一步迭代。

3.5 状态协方差更新

标准的状态协方差更新方程9x9：

$P_k^{\prime }=P_k-KH{P}_k$

⭐考虑到使用上式容易使得$P$非对称，而为了保持数值稳定性，在程序不报错条件下，采用Joseph形式的方程更新状态协方差：

$P_k^{\prime }=(1-KH)P_k(1-KH{)}^T+KR{K}^T$

注意将更新后的协方差送入下一步迭代。

3.6 补偿

⭐利用惯性导航结果和更新后的状态量，补偿得到组合导航结果(就此可以解释为何假设将送入下一轮的偏差设置为0)并送入下一次惯性解算(这里加减与状态定义有关)：

$x_{INS+GNSS}=x_{INS}-\delta \hat{x}$