组合导航原理剖析（一）：从扩展kalman滤波到ESKF算法

首先，递归滤波器与kalman滤波器在原理上有相似之处。
递归滤波器表示为下面几步，本质上与kalman滤波的思想是比较相似的。但是卡尔曼滤波所采用的增益是随时间变化逐渐趋近于稳定的，当滤波器收敛之后，理论上系统可以得到状态值的最优后验估计。

$$step1:计算kalman增益:K_k=\frac{e_{k-1}^{EST}}{e_{k-1}^{EST}+e_k^{MEA}}$$

$$step2:计算：{\hat{x}}_k={\hat{x}}_{k-1}+K_k(z_k-{\hat{x}}_{k-1})$$

$$e_k^{EST}=(1-K_k)e_{k-1}^{EST}$$

首先，定义线性系统表示为：

$$\begin{array}{l}{\mathrm{x}}_k=A{x}_{k-1}+B{u}_{k-1}+w_{k-1}\\ z_k=H_{k-1}+v_k\end{array}$$

误差可以表示为：

$$\begin{array}{l}P(w)\sim N(0,Q)\\ P(v)\sim N(0,R)\end{array}$$

预测方程表示为：

$$\begin{array}{l}{\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_{k-1}\\ P_k^{-}=A{P}_{k-1}{A}^T+Q\end{array}$$

更新方程表示为：

$$\begin{array}{l}{K}_k=\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R}\\ {\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-})\\ P_k=(I-K_{k}H)P_k^{-}\end{array}$$

实际情况下，系统常常是非线性的。假设非线性系统表示为

$$\begin{gathered} x_k=f(x_{k-1},u_{k-1},w_{k-1}) \\ {z}_k=h(x_k,v_k) \end{gathered}$$

设计非线性kalman滤波系统，首先需要对非线性系统进行线性化，其中线性化的点被称为**“operating point**”，通常认为选取真实点作为**“operating point**”是最合适的。但由于误差的存在真实值往往是未知的。因此采用泰勒展开的方式将更新矩阵在${\hat{x}}_{k-1}$处线性化，可以表示为**（其中${\hat{x}}_{k-1}$为k-1时刻的后验估计）**

$$x_k=f({\hat{x}}_{k-1},u_{k-1},w_{k-1})+A(x_k-{\hat{x}}_{k-1})+W_k{w}_{k-1}$$

那么假设$w_{k-1}$为0，有${\tilde{x}}_k=f({\hat{x}}_{k-1},u_{k-1},0)$，假设$v_k$为0，有${\tilde{z}}_k=h({\tilde{x}}_k,0)$。另外，雅克比矩阵表示为

$$\begin{gathered} A=\frac{\partial f}{\partial x}|{\hat{x}}_{k-1},u_{k-1} \\ {W}_k=\frac{\partial f}{\partial w}|{\hat{x}}_{k-1},u_{k-1} \end{gathered}$$

其中A为状态量$x_k$对状态量$x_{k-1}$的雅克比矩阵，$\ast \ast W_k$为状态量$x_k$对状态量$w_{k-1}$的雅克比矩阵**。

观测方程在${\tilde{x}}_k$处线性化，可以表示为

$$z_k={\tilde{z}}_k+H(x_k-{\tilde{x}}_{k-1})+V_k{v}_{k-1}$$

雅克比矩阵表示为

$$\begin{gathered} H_k=\frac{\partial h}{\partial x}|{\tilde{x}}_k \\ {V}_k=\frac{\partial h}{\partial v}|{\tilde{x}}_k \end{gathered}$$

协方差矩阵表示为

$$\begin{gathered} P(w)-N(0,Q) \\ P(Ww)-N(0,WQ{W}^T) \\ P(v)-N(0,R) \\ P(Vv)-N(0,VR{V}^T) \end{gathered}$$

扩展卡尔曼滤波可以表示为

$$\begin{gathered} {\hat{x}}_k^{-}=f({\hat{x}}_{k-1},u_{k-1},0) \\ {P}_k^{-}=A{P}_{k-1}{A}^T+WQ{W}^T \end{gathered}$$

更新方程表示为

$$K_k=\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+VR{V}^T}$$

$$\begin{gathered} {\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-h({\hat{x}}_k^{-},0)) \\ {P}_k=(I-K_{k}H)P_k^{-} \end{gathered}$$

结合组合导航原理，从上面的公式推导中可以得到下面的结论：

1. 求导获得雅克比矩阵，不光需要对状态量进行求导，还需要对噪声进行求导；
2. 一般认为上式中的${\hat{x}}_k$为后验估计值，与误差状态kalman滤波中对“真实值”和“标称值”的定义并不一致。

简明ESKF推导

下面以组合导航为例讨论EKF的用法。在组合导航中，常用误差状态量表示状态更新，具有诸多优点不赘述。

首先，对于一般的非线性系统，将状态量进行误差化可以表示为下式，就可以使用扩展

$$\begin{gathered} x_k=f({\hat{x}}_{k-1},u_{k-1},w_{k-1})+A(x_k-{\hat{x}}_{k-1})+W_k{w}_{k-1} \\ {\tilde{x}}_k+\delta x_k=f({\hat{x}}_{k-1},u_{k-1},w_{k-1}=0)+A({\tilde{x}}_{k-1}-{\hat{x}}_{k-1})+W_k{w}_{k-1} \\ \delta x_k=A\delta x_{k-1}+W_k{w}_{k-1} \end{gathered}$$

但是在组合导航的状态量运行学预测更新的计算中，已知状态导数与状态之间的转移矩阵，比如

$$\dot{\mathbf{v}}=\mathbf{R}{\mathbf{a}}^b+\mathbf{g}$$

可以推导出误差状态的微分公式如下：（这里是忽略了加速度计的零偏值，不是特别准确）

$$\begin{array}{rl}\dot{\mathbf{v}}+\dot{\delta \mathbf{v}}& =\mathbf{R}\left(\mathbf{I}+[\delta \mathbf{\theta }{]}_{\times }\right)\left({\mathbf{a}}^b+\delta {\mathbf{a}}^b\right)+\mathbf{g}+\delta \mathbf{g}\\ \dot{\delta \mathbf{v}}& =\mathbf{R}\delta {\mathbf{a}}^b+\mathbf{R}[\delta \mathbf{\theta }{]}_{\times }\left({\mathbf{a}}^b+\delta {\mathbf{a}}^b\right)+\delta \mathbf{g}\\ \dot{\delta }\mathbf{v}& =\mathbf{R}\delta {\mathbf{a}}^{\mathbf{b}}-\mathbf{R}{\left[{\mathbf{a}}^b\right]}_{\times }\delta \mathbf{\theta }+\delta \mathbf{g}\end{array}$$

这里面的表示有点问题，需要注意正负。

💡 最后一步推导：**两个小量相乘，最后的结果就是无穷小，可以忽略。在上式中，转移方程需要对delta_x进行求导。**

再将上式进行离散化，同样可以得到误差状态量的离散传递方程。

下面讨论观测方程。由于采用误差状态量作为待估计对象，因此将观测方程对$\delta x$求导得到观测矩阵H，求导方式可以采用链式法则。

$$\mathrm{H}=\frac{\partial h}{\partial \delta x}=\frac{\partial h}{\partial x}\frac{\partial x}{\partial \delta x}$$