前面几节相继介绍的不同维度的卡尔曼滤波算法，在线性高斯模型的条件下，具有很好的跟踪效果，当时在实际生活中，不可能所有的运动都呈现一种线性关系，也存在着非线性关系，比如说：飞机的飞行状态，导弹的制导问题等等。为了保证结果的准确性和精确度，因此就需要有一种适用于非线性系统的滤波算法。
对于非线性系统的滤波问题，将非线性系统转换为一个近似线性的系统然后进行滤波是一种较为常用的方法，例如本节将介绍的扩展卡尔曼滤波EKF算法。
EKF实现思路
扩展Kalman滤波建立在线性Kalman滤波算法上，对一般的非线性系统，主要思路为：
- 围绕滤波值
X
^
k
\hat{X}_{k}
X^k将
非线性函数f(*)和h(*)展开成泰勒（Taylor）级数并略去二阶及以上项； - 然后得到一个近似的线性化模型，然后
应用Kalman滤波完成对自标的滤波估计等处理。
EKF滤波原理
在之前的线性Kalman滤波中，状态方程和量测方程如下所示：
X
k
=
F
X
k
−
1
+
V
k
(
1
)
X_k =FX_{k-1}+V_{k} \quad(1)
Xk=FXk−1+Vk(1)
Z
k
=
H
X
k
+
W
k
(
1
)
Z_k =HX_{k}+W_{k} \quad(1)
Zk=HXk+Wk(1)
由于此时的非线性模型，因此状态转移不再是严格的线性关系，因此，上述的两个方程变为了下式：
X
k
=
f
(
k
,
X
k
−
1
)
+
V
k
(
3
)
X_k =f(k,X_{k-1})+V_{k} \quad(3)
Xk=f(k,Xk−1)+Vk(3)
Z
k
=
h
(
k
,
X
k
)
+
W
k
(
4
)
Z_k =h(k,X_k)+W_{k} \quad(4)
Zk=h(k,Xk)+Wk(4)
按照EKF的思路，首先进行局部的线性化。
系统状态方程的局部线性化
将非线性函数$f(k,X_{k-1})$围绕滤波值$\hat X_k$进行一阶展开如下：
X
(
k
+
1
)
≈
f
[
k
,
X
^
(
k
)
]
+
∂
f
∂
X
^
(
k
)
[
X
(
k
)
−
X
^
(
k
)
]
+
V
k
(
5
)
X(k+1)\approx f[k,\hat{X}(k)]+\frac{\partial f}{\partial\hat{X}(k)}[X(k)-\hat{X}(k)]+V_k \quad(5)
X(k+1)≈f[k,X^(k)]+∂X^(k)∂f[X(k)−X^(k)]+Vk(5)
令其中的：
∂
f
∂
X
^
(
k
)
=
∂
f
[
X
^
,
k
]
,
(
k
)
∂
X
^
(
k
)
∣
X
^
(
k
)
=
X
(
k
)
=
Φ
(
k
+
1
∣
k
)
(
6
)
\left.\frac{\partial f}{\partial\hat{X}\left(k\right)}=\frac{\partial f\left[\hat{X},k\right],\left(k\right)}{\partial\hat{X}\left(k\right)}\right|_{\hat{X}\left(k\right)=X\left(k\right)}=\Phi(k+1|k)\quad(6)
∂X^(k)∂f=∂X^(k)∂f[X^,k],(k)
X^(k)=X(k)=Φ(k+1∣k)(6)
f
[
k
,
X
^
(
k
)
]
−
∂
f
∂
X
(
k
)
∣
X
(
k
)
=
X
^
(
k
)
X
^
(
k
)
=
ϕ
(
k
)
(
7
)
f\left[k,\hat{X}(k)\right]-\left.\frac{\partial f}{\partial X(k)}\right|_{X(k)=\hat{X}(k)}\hat{X}(k)=\phi(k)\quad(7)
f[k,X^(k)]−∂X(k)∂f
X(k)=X^(k)X^(k)=ϕ(k)(7)
此时上述的（6）式就可以看作是非线性模型下的状态转移方程，此时的状态方程可以写为：
X
(
k
+
1
)
=
Φ
(
k
+
1
∣
k
)
X
(
k
)
+
V
k
+
ϕ
(
k
)
(
8
)
X\left(k+1\right)=\Phi\left(k+1|k\right)X\left(k\right)+V_k+\phi\left(k\right)\quad(8)
X(k+1)=Φ(k+1∣k)X(k)+Vk+ϕ(k)(8)
可以看到，相对基本的Kalman滤波方式中的状态方程，此方程中多了一项，该项为非随机外作用项。
量测方程的局部线性化
采用同样的方法对量测方程进行局部线性化，如下：
Z
(
k
)
=
h
[
X
^
(
k
∣
k
−
1
)
,
k
]
+
∂
h
∂
X
^
(
k
)
∣
X
^
(
k
,
k
−
1
)
[
X
(
k
)
−
X
^
(
k
∣
k
−
1
)
]
+
W
k
(
9
)
Z(k)=h[\hat{X}(k\mid k-1),k]+\left.\frac{\partial h}{\partial\hat{X}(k)}\right|_{\hat{X}(k,k-1)}[X(k)-\hat{X}(k\mid k-1)]+W_k\quad(9)
Z(k)=h[X^(k∣k−1),k]+∂X^(k)∂h
X^(k,k−1)[X(k)−X^(k∣k−1)]+Wk(9)
令其中的：
∂
h
∂
X
^
(
k
)
∣
X
(
k
)
=
X
^
(
k
)
=
H
(
k
)
(
10
)
\left.\frac{\partial h}{\partial\hat{X}\left(k\right)}\right|_{X\left(k\right)=\hat{X}\left(k\right)}=H\left(k\right)\quad(10)
∂X^(k)∂h
X(k)=X^(k)=H(k)(10)
y
(
k
)
=
h
[
X
^
(
k
∣
k
−
1
)
,
k
]
−
∂
h
∂
X
^
(
k
)
∣
X
(
k
)
=
X
^
(
k
)
X
^
(
k
∣
k
−
1
)
(
11
)
y(k)=h[\hat{X}(k|k-1),k]-\frac{\partial h}{\partial\hat{X}(k)|_{X(k)=\hat{X}(k)}}\hat{X}(k|k-1)\quad(11)
y(k)=h[X^(k∣k−1),k]−∂X^(k)∣X(k)=X^(k)∂hX^(k∣k−1)(11)
同样整理得到非线性模型下的量测方程如下：
Z
(
k
)
=
H
(
k
)
X
(
k
)
+
y
(
k
)
+
W
k
(
12
)
Z\left(k\right)=H\left(k\right)X\left(k\right)+y\left(k\right)+W_k\quad(12)
Z(k)=H(k)X(k)+y(k)+Wk(12)
EKF滤波
经过了局部的初始化后，即可以按照卡尔曼滤波的基本共识与流程进行滤波，基本公式如下：
滤波的初值和初始协方差矩阵为：
状态转移矩阵与量测矩阵的计算
线性化后的系统方程中，状态转移矩阵
Φ
(
k
+
1
∣
k
)
\Phi(k+1|k)
Φ(k+1∣k)和量测矩阵
H
(
k
+
1
)
H(k+1)
H(k+1)可以通过f和h的雅各比矩阵代替，假设状态变量X有n维，也即
X
=
[
x
1
x
2
⋯
x
n
]
T
X = \left[ \begin{array} { l l l l } { x _ { 1 } } & { x _ { 2 } } & { \cdots } & { x _ { n } } \end{array} \right]^T
X=[x1x2⋯xn]T
对应的计算公式分别如下：
明日将会这部分内容进行仿真测试，上述内容即使今天的全部内容了，感谢大家的观看。
如果方便，辛苦大家点个赞和关注哦！
您的点赞或评论或关注是对我最大的肯定，谢谢大家！！！
