组合导航原理剖析（五）：kalman滤波在惯性导航中的应用相关推导过程

简明ESKF推导

1. 线性kalman滤波器与扩展kalman滤波器

本质上kalman滤波可以看成是一个递归滤波器。在信号处理领域，递归滤波器也被成为IIR滤波器，有着明确的系数和传递函数，而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滤波系统，也就是扩展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}$的雅克比矩阵，$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}$$

2. 误差状态扩展卡尔曼滤波器

导航的目的通常是估计或者测量系统的位置、姿态、速度状态，还可能包含一些未准确标定、待确定的参数，比如杆臂、零偏等等。常见的导航方式则采用误差状态估计的方式，去代替直接的状态估计，因此又被成为间接的误差状态估计。在文献Quaternion kinematics for the error-state Kalman filter中总结过这种方式的优势。

因此，整个组合导航过程分成了两个步骤：1.根据机械编排公式更新状态量，包括位置、速度、姿态等，得到标称值。2. kalman滤波估计误差状态值，将误差状态值补偿至标称值，得到修正之后的测量值。在第2步中，实际计算时将上面公式中的${\hat{x}}_k$替换成$\delta x=[\delta \theta ,\delta v,\delta p{]}^T$，那么状态转移矩阵就变成了

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

对于一般的非线性系统$x_k=f(x_{k-1},u_{k-1})$，应用泰勒展开，将状态量进行误差化可以表示为下式

$$\begin{gathered} x_k=f(x_{k-1},u_{k-1}) \\ {\hat{x}}_k+\delta x_k=f({\hat{x}}_{k-1}+\delta x_{k-1},u_{k-1}+n_{k-1}) \\ {\hat{x}}_k+\delta x_k=f({\hat{x}}_{k-1},{\hat{u}}_{k-1})+F\delta x_{k-1}+G{n}_{k-1} \end{gathered}$$

如果能够知道误差随时间变化的导数关系，也可以将其离散化之后得到误差传播方程

$$\delta \dot{x}=A\delta x+Bn$$

那么有关系：$F=(I+A\Delta t)G=B\Delta t$。

上面的表示方式有一定优势，也更加简洁。惯性测量中运动学方程或者比力方程是已知的，比如

$$\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}$$

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

3. 量测更新修正

观测方程中，由于采用误差状态量作为待估计对象，因此将观测方程对$\delta x$求偏导得到观测矩阵$H$。SLAM的方法中，求导方式可以采用链式法则。这一块的内容与运动学方程的计算方式接近，不再赘述。

$$\mathrm{H}=\frac{\partial h}{\partial \delta x}=\frac{\partial h}{\partial x}\frac{\partial x}{\partial \delta x}$$
以上，有助于理解在组合导航应用中kalman滤波计算的实际含义。