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

首先，递归滤波器和kalman原理上滤波器有相似之处。
递归滤波器表示以下步骤kalman滤波器的想法相似。然而，随着时间的推移，卡尔曼滤波器采用的增益逐渐趋于稳定。当滤波器收敛时，系统理论上可以获得状态值的最佳后验估计。

s t e p 1 : 计算 k a l m a n 增益 : K k = e k ? 1 E S T e k ? 1 E S T e k M E A step1:计算kalman增益:K_k=\frac{e^{EST}_{k-1}}{e^{EST}_{k-1} e^{MEA}_k} step1:计算kalman增益:Kk=ek?1EST ekMEAek−1EST​​

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