卡尔曼滤波是最优线性估计器，然而现实世界中被估计过程和对过程的测量绝大多数都是非线性的，这个时候就要用到EKF。
EKF的思想是使用泰勒展开来对当前的估计状态进行局部线性化。假设过程有一个状态向量
x
∈
R
n
x\in R^{n}
x∈Rn,有一个测量量
z
∈
R
m
z\in R^{m}
z∈Rm该过程可用如下的非线性方程表示：
x
k
=
f
(
x
k
−
1
,
u
k
,
w
k
−
1
)
(
1
)
z
k
=
h
(
x
k
,
v
k
)
(
2
)
x_{k} = f(x_{k-1},u_{k},w_{k-1})\space(1) \newline z_{k} = h(x_{k},v_{k})\space (2)
xk=f(xk−1,uk,wk−1) (1)zk=h(xk,vk) (2)
其中：
- w k , v k w_{k},v_{k} wk,vk分别代表过程噪声和测量噪声。
- f , h f,h f,h均为非线性函数
实际上，没有人能够知道噪声
w
k
,
v
k
w_{k},v_{k}
wk,vk在每一个时间点上的值究竟是什么，所以我们将状态和测量向量近似如下：
x
~
k
=
f
(
x
^
k
−
1
,
u
k
,
0
)
(
3
)
z
~
k
=
h
(
x
~
k
,
0
)
(
4
)
\tilde{x}_{k} = f(\hat{x}_{k-1},u_{k},0)\space (3) \newline \tilde{z}_{k} = h(\tilde{x}_{k},0)\space (4)
x~k=f(x^k−1,uk,0) (3)z~k=h(x~k,0) (4)
其中：
- x ^ k \hat{x}_{k} x^k是状态的后验估计
我们对方程(3)和(4)在
x
k
−
1
x_{k-1}
xk−1处使用泰勒展开，并忽略其二阶及二阶以上的导数，可得如下方程：
x
k
≈
x
~
k
+
A
(
x
k
−
1
−
x
^
k
−
1
)
+
W
w
k
−
1
(
5
)
z
k
≈
z
~
k
+
H
(
x
k
−
x
~
k
)
+
V
v
k
(
6
)
x_{k} \approx \tilde{x}_{k}+A(x_{k-1}-\hat{x}_{k-1})+Ww_{k-1}\space (5) \newline z_{k} \approx \tilde{z}_{k}+H(x_{k}-\tilde{x}_{k})+Vv_{k}\space (6)
xk≈x~k+A(xk−1−x^k−1)+Wwk−1 (5)zk≈z~k+H(xk−x~k)+Vvk (6)
其中：
- x k 和 z k x_{k}和z_{k} xk和zk是真实值
- x ~ k 和 z ~ k \tilde{x}_{k}和 \tilde{z}_{k} x~k和z~k是由方程(3)和(4)得到的近似值
- x ^ k \hat{x}_{k} x^k是一个后验估计值
-
A
A
A是函数f对x计算偏微分得到的雅可比矩阵：
A [ i , j ] = ∂ f [ i ] ∂ x [ j ] ( x ^ k − 1 , u k , 0 ) A_{[i,j]} = \frac{\partial{f}_{[i]}}{\partial{x}_{[j]}}(\hat{x}_{k-1},u_{k},0) A[i,j]=∂x[j]∂f[i](x^k−1,uk,0)
W W W同理 -
H
H
H是函数h对x计算偏微分得到的雅可比矩阵：
H [ i , j ] = ∂ h [ i ] ∂ x [ j ] ( x ~ k , 0 ) H_{[i,j]} = \frac{\partial{h}_{[i]}}{\partial{x}_{[j]}}(\tilde{x}_{k},0) H[i,j]=∂x[j]∂h[i](x~k,0)
V V V同理
定义预测误差为：
e
~
x
k
=
x
k
−
x
~
k
(
7
)
\tilde{e}_{x_{k}} = x_{k}-\tilde{x}_{k}\space (7)
e~xk=xk−x~k (7)
定义测量残差为：
e
~
z
k
=
z
k
−
z
~
k
(
8
)
\tilde{e}_{z_{k}} = z_{k}-\tilde{z}_{k}\space (8)
e~zk=zk−z~k (8)
其中：
- x k x_{k} xk是我们永远也无法得知的真实状态
- z k z_{k} zk是可以得到的实际测量值
将方程(5)和(6)代入方程(7)和(8)，可得到：
e
~
x
k
≈
A
(
x
k
−
1
−
x
^
k
−
1
)
+
ε
k
e
~
z
k
≈
H
e
~
x
k
+
η
k
\tilde{e}_{x_{k}} \approx A(x_{k-1}-\hat{x}_{k-1}) + \varepsilon_{k} \newline \tilde{e}_{z_{k}} \approx H \tilde{e}_{x_{k}}+\eta_{k}
e~xk≈A(xk−1−x^k−1)+εke~zk≈He~xk+ηk
其中
ε
k
∼
N
(
0
,
W
Q
k
W
T
)
\varepsilon_{k}\sim N(0,WQ_{k}W^{T})
εk∼N(0,WQkWT),
η
k
∼
N
(
0
,
V
R
k
V
T
)
\eta_{k}\sim N(0,VR_{k}V^{T})
ηk∼N(0,VRkVT)
x
^
k
=
x
~
k
+
e
^
k
e
^
k
=
K
k
e
^
z
k
\hat{x}_{k}=\tilde{x}_{k}+\hat{e}_{k} \newline \hat{e}_{k} = K_{k}\hat{e}_{z_{k}}
x^k=x~k+e^ke^k=Kke^zk
故而EKF预测方程为：
x
^
k
−
=
f
(
x
^
k
−
1
,
u
k
,
0
)
P
k
−
=
A
k
P
k
−
1
A
k
T
+
W
k
Q
k
−
1
W
k
T
\hat{x}^{-}_{k} = f(\hat{x}_{k-1},u_{k},0) \newline P^{-}_{k}=A_{k}P_{k-1}A^{T}_{k}+W_{k}Q_{k-1}W^{T}_{k}
x^k−=f(x^k−1,uk,0)Pk−=AkPk−1AkT+WkQk−1WkT
EKF校正方程为：
K
k
=
P
k
−
H
k
T
(
H
k
P
k
−
H
k
T
+
V
k
R
k
V
k
T
)
−
1
x
^
k
=
x
^
k
−
+
K
k
(
z
k
−
z
~
k
)
P
k
=
(
I
−
K
k
H
k
)
P
k
−
K_{k}=P^{-}_{k}H^{T}_{k}(H_{k}P^{-}_{k}H^{T}_{k}+V_{k}R_{k}V^{T}_{k})^{-1} \newline \hat{x}_{k}=\hat{x}^{-}_{k}+K_{k}(z_{k}-\tilde{z}_{k}) \newline P_{k}=(I-K_{k}H_{k})P^{-}_{k}
Kk=Pk−HkT(HkPk−HkT+VkRkVkT)−1x^k=x^k−+Kk(zk−z~k)Pk=(I−KkHk)Pk−
参考文献：
[1]Welch G, Bishop G. An introduction to the Kalman filter[J]. 1995.