文章目录
- References
- 卡尔曼滤波的作用
- 世界中充满着不确定性
- 在工程中
- 整体感受
- 状态空间方程
- 结合例子理解公式
- 公式6-1说明
- 公式6-2说明
- 参数H的意义
- 总结
- 怎么融合？
- 从简单的例子入手-测量一枚硬币的直径
- 融合实例
- 卡尔曼公式详细推导
- 协方差矩阵
- 卡尔曼增益的推导
- 详细推导
- 误差协方差矩阵 P k − P_k^- Pk−
- 卡尔曼滤波的5个公式
- 扩展的卡尔曼滤波
- 线性化
- 线性化解决EKF
References
How a Kalman filter works, in pictures | Bzarg
详解卡尔曼滤波原理_清风莞尔的博客-CSDN博客
参考链接3：当然还有DR_CAN的视频
注：链接1是一位国外大神写的，所以是英文，链接2是其翻译版。
卡尔曼滤波的作用
你可以在任何含有不确定信息的动态系统中使用卡尔曼滤波，对系统下一步的走向做出有根据的预测，即使伴随着各种干扰，卡尔曼滤波总是能指出真实发生的情况。
世界中充满着不确定性
系统的运行过程中有过程噪音，仪器测量有测量误差。[就连喜欢一个人也都有不确定性，时而喜欢、时而无感觉，~嗯、渣男]。既然现实生活中充满着不确定性，那么有没有一种方法可以获得相对准确的结果呢？卡尔曼滤波就有这样的作用。
在工程中
我们常常根据系统的上一状态来计算出系统的当前状态，但是由于外界的影响，计算出来的结果和系统真实的结果是有差异的；我们又同时通过系统中的传感器获得系统的当前状态，但是由于传感器本身的误差，我们从传感器获得的结果和系统真实值也是有差异的。
即我们得到了系统两个具有误差的结果：
- 一个是根据系统的状态空间方程计算而得的计算值，真实值是在计算值的基础上会被加入过程噪声（比如小车在行驶过程中被风吹歪了，那么真实值就不是计算值了。）；
- 一个是根据传感器测量而得的、具有测量误差的测量值。
我们的目标是把两个具有误差的结果融合为一个误差不太大的、可信的结果。
整体感受
状态空间方程
系统的状态方程：
X
k
−
=
A
X
k
−
1
+
B
u
k
−
1
X_k^- = AX_{k-1} + Bu_{k-1}\\
Xk−=AXk−1+Buk−1
但系统会受到**环境噪声
w
k
−
1
w_{k-1}
wk−1**的影响，系统真实的状态并不是数学表达式所表示的结果，即系统真实的状态
X
k
X_k
Xk应该加上环境噪声，为：
X
k
=
X
k
−
+
w
k
−
1
=
A
X
k
−
1
+
B
u
k
−
1
+
w
k
−
1
\begin{align} X_k &= X_k^- + w_{k-1}\\ &= AX_{k-1} + Bu_{k-1} + w_{k-1}\\ \end{align}
Xk=Xk−+wk−1=AXk−1+Buk−1+wk−1
系统的计算值（先验估计）：
X
k
−
=
A
X
k
−
1
+
B
u
k
−
1
X_k^- = AX_{k-1} + Bu_{k-1}
Xk−=AXk−1+Buk−1。
传感器测量方程：
Z
k
=
H
X
k
{Z_k} = HX_k
Zk=HXk
系统的测量值：
X
k
M
e
a
s
u
r
e
=
H
−
Z
k
X_k^{Measure}=H^-Z_k
XkMeasure=H−Zk，记住：这个测量值包含了测量误差。因为传感器本身有误差，即**传感器读数值
Z
k
Z_k
Zk**包括了误差，即真实的传感器测量方程是：
Z
k
=
H
X
k
+
v
k
\begin{align} Z_k &= HX_k + {\color{green}v_k} \end{align}
Zk=HXk+vk
综上可得系统真实的状态方程为：
{
X
k
=
X
k
−
+
w
k
−
1
=
A
X
k
−
1
+
B
u
k
−
1
+
w
k
−
1
(
6
−
1
)
Z
k
=
H
X
k
+
v
k
(
6
−
2
)
\begin{cases} X_k = {\color{blue}X_k^-} + w_{k-1} = {\color{blue}AX_{k-1} + Bu_{k-1}} + w_{k-1}{\quad}(6-1)\\ Z_k = HX_k + v_k{\quad}(6-2) \end{cases}
{Xk=Xk−+wk−1=AXk−1+Buk−1+wk−1(6−1)Zk=HXk+vk(6−2)
式中：
- X k X_k Xk是当前系统真实的状态；
- X k − X_k^- Xk−是根据状态方程算出来当前系统本应该处于的状态（计算值）；
- u k − 1 u_{k-1} uk−1是外部控制输入；
- Z k Z_k Zk是当前传感器的读数值（包含了测量误差 v k v_k vk）；
- w k − 1 w_{k-1} wk−1和 v k v_k vk一个是过程噪声，一个是测量噪声，它们在卡尔曼滤波中都被认为服从高斯分布；
- H H H是一个系数，比如有的传感器测出的值是0~1024之间的值，还需要转换才能得到对应的物理量。
结合例子理解公式
一辆小车上装有位置传感器，并且向前匀速的运动。
公式6-1说明
系统的状态是可以用数学表达式表达出来的（或者这样说，系统的运行状态满足某些数学公式），也就意味着系统的当前状态可以根据上一状态计算而出，即通过系统的状态方程，可以计算出系统当前的状态 X k − X_k^- Xk−。但是由于具有过程噪声 w k − 1 w_{k-1} wk−1，所以这个结果不是系统当前的真实状态。
比如：上一时刻小车的位置是5，速度是2，那么通过计算可以知道下一时刻小车的位置是7。但是由于环境风的影响，导致小车真实的位置其实是6.8，所以小车的真实值 X k X_k Xk=计算值 X k − X_k^- Xk−+过程噪声 w k − 1 w_{k-1} wk−1（此处噪声是-0.2，计算值是7，真实值就是6.8了）。在卡尔曼滤波中认为过程噪声 w w w是服从 ( 0 , c o v ( w ) ) (0,cov(w)) (0,cov(w))的正态分布。
公式6-2说明
通过小车上的传感器，可以得到小车当前的测量位置，但是由于传感器具有测量误差，所以这个传感器传回的值包含了测量噪声，即真实值 X k X_k Xk+测量噪声 v k v_k vk=传感器测量的结果（读数值） Z k Z_k Zk。
比如：续上，小车当前的真实位置是6.8（传感器测量的就是小车的真实位置，只不过传感器自己有误差），由于测量误差，传感器测量得到的值是6.7（测量值才是我们可以得到的，是读数值），即 6.7 = 6.8 − 0.1 6.7=6.8-0.1 6.7=6.8−0.1，6.7是我们从传感器读数得知，0.1是测量误差（服从正态分布），6.8是真实值。
参数H的意义
这里首先要明白一点:传感器的值不一定就等于系统的状态，这里不是说误差的影响，而是传感器特性的影响。比如传感器的1代表了系统的100，这是生产传感器时就规定好的。所以由传感器上的值得到系统的状态还需要乘以100（就是H这个系数），如果传感器的值就代表了系统的状态，那么H=1，比如位置传感器直接给出小车位置，而速度传感器还需要转换才能得出位置。
总结
在公式7中，我们可以得到的是：
- 系统的计算值（计算值） X k − X_k^- Xk−；
- 系统的观测值（读数值） Z k Z_k Zk。
- 又基于实际生活和大量实验，我们可以大致得到过程噪声和测量噪声的分布情况（卡尔曼滤波认为是正态分布）。
最终我们就可以根据两个和正态误差相关的结果( X k − X_k^- Xk−和 Z k Z_k Zk)融合为一个误差较小的结果 X k + X_k^+ Xk+，这个融合值是最优估计。
怎么融合？
从简单的例子入手-测量一枚硬币的直径
由于尺子误差和认为读数误差，结果是不准确的，我们自然会测量多次以平均值作为其真实值。
x
k
~
=
1
k
(
z
1
+
z
2
+
.
.
.
+
z
k
)
=
1
k
(
z
1
+
z
2
+
.
.
.
+
z
k
−
1
)
+
1
k
z
k
=
1
k
k
−
1
k
−
1
(
z
1
+
z
2
+
.
.
.
+
z
k
−
1
)
+
1
k
z
k
=
k
−
1
k
x
k
−
1
~
+
1
k
z
k
=
x
k
−
1
~
−
1
k
x
k
−
1
~
+
1
k
z
k
=
x
k
−
1
~
+
1
k
(
z
k
−
x
k
−
1
~
)
\begin{align} \widetilde{x_k} &= \frac{1}{k}(z_1+z_2+...+z_k)\\ &=\frac{1}{k}(z_1+z_2+...+z_{k-1})+\frac{1}{k}z_k\\ &=\frac{1}{k}\frac{k-1}{\color{blue}{k-1}}{\color{blue}(z_1+z_2+...+z_{k-1})}+\frac{1}{k}z_k\\ &=\frac{k-1}{k}\widetilde{x_{k-1}}+\frac{1}{k}z_k\\ &=\widetilde{x_{k-1}}-\frac{1}{k}\widetilde{x_{k-1}}+\frac{1}{k}z_k\\ &={\color{red}\widetilde{x_{k-1}}+\frac{1}{k}(z_k-\widetilde{x_{k-1}})} \end{align}
xk
=k1(z1+z2+...+zk)=k1(z1+z2+...+zk−1)+k1zk=k1k−1k−1(z1+z2+...+zk−1)+k1zk=kk−1xk−1
+k1zk=xk−1
−k1xk−1
+k1zk=xk−1
+k1(zk−xk−1
)
z
k
z_k
zk表示第k次的测量值，
x
k
~
\widetilde{x_k}
xk
表示第k次的估计值（即前k次的平均值）。通过一个简单的变换，我们得到了一个公式，即：当前估计值=上一次估计值+比例(当前测量值-上一次估计值)。这里的比例*其实就是融合比例（卡尔曼增益），谁多一点、谁少一点。
比如：
- 当 k = 1 k=1 k=1时，此时估计值就等于测量值 x k ~ = z k \widetilde{x_k}=z_k xk =zk，因为目前就测量了一次，没有更多数据做平均嘛；
- 当 k → ∞ k{\to}\infin k→∞时，此时估计值就等于上一次的估计值 x k ~ = x k − 1 ~ \widetilde{x_k}=\widetilde{x_{k-1}} xk =xk−1 ，因为我有足够多的历史数据了，这一次的测量数据对我的影响很小。
融合实例
由两把不同的天平（误差都服从正态分布）得出不同的重量，天平1误差的标准差是2g，天平2误差的标准差是4g。现在天平1的结果是30g，天平2的结果是32g，怎么把两个结果融合为一个最优的估计值。从下面的正态分布图可以知道，最终的结果应该在30~32g之间，并且靠近30g更多一点。
z
1
=
30
g
σ
1
=
2
g
z
2
=
32
g
σ
2
=
4
g
z
~
=
z
1
+
k
(
z
2
−
z
1
)
z_1=30g {\quad} \sigma_1=2g \\ z_2=32g {\quad} \sigma_2=4g \\ \widetilde{z}=z_1 + {\color{red}k}(z_2 - z_1)
z1=30gσ1=2gz2=32gσ2=4gz
=z1+k(z2−z1)
融合目标：找到k，使得
v
a
r
(
z
~
)
var(\widetilde{z})
var(z
)最小，融合后误差最小就是使融合后方差最小（方差越小越接近真实值）。
σ
z
~
2
=
v
a
r
(
z
1
+
k
(
z
2
−
z
1
)
)
=
v
a
r
(
z
1
−
k
z
1
+
k
z
2
)
=
v
a
r
(
(
1
−
k
)
z
1
+
k
z
2
)
=
v
a
r
(
(
1
−
k
)
z
1
)
+
v
a
r
(
k
z
2
)
=
(
1
−
k
)
2
v
a
r
(
z
1
)
+
k
2
v
a
r
(
z
2
)
=
(
1
−
k
)
2
σ
1
2
+
k
2
σ
2
2
\begin{align} \sigma_{\widetilde{z}}^2&=var(z_1+k(z_2-z_1)) \\ &= var(z_1-kz_1+kz_2) \\ &= var((1-k)z_1+kz_2) \\ &= var((1-k)z_1) + var(kz_2) \\ &= (1-k)^2var(z_1)+k^2var(z_2) \\ &= (1-k)^2\sigma_1^2+k^2\sigma_2^2 \\ \end{align}
σz
2=var(z1+k(z2−z1))=var(z1−kz1+kz2)=var((1−k)z1+kz2)=var((1−k)z1)+var(kz2)=(1−k)2var(z1)+k2var(z2)=(1−k)2σ12+k2σ22
要使
σ
z
~
2
\sigma_{\widetilde{z}^2}
σz
2最小，求导令其为0，求其最小值。
d
σ
z
~
2
d
k
=
0
−
2
(
1
−
k
)
σ
1
2
+
2
k
σ
2
2
=
0
−
σ
1
2
+
k
σ
1
2
+
k
σ
2
2
=
0
k
(
σ
1
2
+
σ
2
2
)
=
σ
2
2
⟹
k
=
σ
1
2
σ
1
2
+
σ
2
2
\frac{d\sigma_{\widetilde{z}}^2}{dk}=0 \\ -2(1-k)\sigma_1^2+2k\sigma_2^2 = 0 \\ -\sigma_1^2+k\sigma_1^2+k\sigma_2^2 = 0 \\ \\ k(\sigma_1^2+\sigma_2^2)=\sigma_2^2 \\ {\Longrightarrow}{\color{red} k = \frac{\sigma_1^2}{\sigma_1^2+\sigma_2^2}}
dkdσz
2=0−2(1−k)σ12+2kσ22=0−σ12+kσ12+kσ22=0k(σ12+σ22)=σ22⟹k=σ12+σ22σ12
所以：
k
=
4
4
+
16
=
0.2
σ
z
~
2
=
(
1
−
0.2
)
2
×
2
2
+
0.
2
2
×
4
2
=
3.2
σ
z
~
=
3.2
=
1.79
g
k=\frac{4}{4+16}=0.2 \\ \sigma_{\widetilde{z}}^2=(1-0.2)^2\times2^2+0.2^2\times4^2=3.2 \\ \sigma_{\widetilde{z}}=\sqrt{3.2}=1.79g
k=4+164=0.2σz
2=(1−0.2)2×22+0.22×42=3.2σz
=3.2=1.79g
卡尔曼公式详细推导
可以不用看，推导较复杂！！！
协方差矩阵
E
[
w
w
T
]
E[w\ w^T]
E[w wT]就代表了
w
w
w的协方差矩阵，如下：
E
[
w
w
T
]
=
E
[
[
w
1
w
2
w
3
]
T
[
w
1
w
2
w
3
]
]
=
[
E
[
w
1
w
1
]
E
[
w
1
w
2
]
E
[
w
1
w
3
]
⋯
E
[
w
2
w
1
]
E
[
w
2
w
2
]
E
[
w
2
w
3
]
⋯
E
[
w
3
w
1
]
E
[
w
3
w
2
]
E
[
w
3
w
3
]
⋯
⋯
⋯
⋯
]
=
[
σ
1
2
σ
1
σ
2
σ
1
σ
3
⋯
σ
2
σ
1
σ
2
2
σ
2
σ
3
⋯
σ
3
σ
1
σ
3
σ
2
σ
3
2
⋯
⋯
⋯
⋯
]
=
c
o
v
(
w
)
\begin{align} E[w\ w^T] &= E\left[[w_1\ w_2\ w_3]^T[w_1\ w_2\ w_3]\right] \\ &= \left [\begin{array}{cccc|r} E[w_1\ w_1] & E[w_1\ w_2] & E[w_1\ w_3] & \cdots\\ E[w_2\ w_1] & E[w_2\ w_2] & E[w_2\ w_3] & \cdots\\ E[w_3\ w_1] & E[w_3\ w_2] & E[w_3\ w_3] & \cdots\\ \cdots & \cdots & \cdots \end{array} \right] \\ &= \left [\begin{array}{cccc|r} \sigma_1^2 & \sigma_1\sigma_2 & \sigma_1\sigma_3 & \cdots\\ \sigma_2\sigma_1 & \sigma_2^2 & \sigma_2\sigma_3 & \cdots\\ \sigma_3\sigma_1 & \sigma_3\sigma_2 & \sigma_3^2 & \cdots\\ \cdots & \cdots & \cdots \end{array} \right] \\ &=cov(w) \end{align}
E[w wT]=E[[w1 w2 w3]T[w1 w2 w3]]=
E[w1 w1]E[w2 w1]E[w3 w1]⋯E[w1 w2]E[w2 w2]E[w3 w2]⋯E[w1 w3]E[w2 w3]E[w3 w3]⋯⋯⋯⋯
=
σ12σ2σ1σ3σ1⋯σ1σ2σ22σ3σ2⋯σ1σ3σ2σ3σ32⋯⋯⋯⋯
=cov(w)
过程噪声协方差矩阵为
Q
Q
Q，服从
w
k
−
(
0
,
Q
)
w_k-(0,Q)
wk−(0,Q)，没有下标
k
k
k的原因是一般认为高斯噪声分布不变化。
E
[
w
k
w
k
T
]
=
Q
E[w_k w_k^T] = Q
E[wkwkT]=Q
测量噪声协方差矩阵为
R
R
R，服从
v
k
−
(
0
,
R
)
v_k-(0,R)
vk−(0,R)
E
[
v
k
v
k
T
]
=
R
E[v_k v_k^T] = R
E[vkvkT]=R
误差协方差矩阵为
P
k
P_k
Pk，服从
e
k
−
(
0
,
P
k
)
e_k-(0,P_k)
ek−(0,Pk)
E
[
e
k
e
k
T
]
=
P
k
E[e_k e_k^T] = P_k
E[ekekT]=Pk
卡尔曼增益的推导
状态空间方程：
{
X
k
=
X
k
−
+
w
k
−
1
=
A
X
k
−
1
+
B
u
k
−
1
+
w
k
−
1
(
29
−
1
)
Z
k
=
H
X
k
+
v
k
(
29
−
2
)
\begin{cases} X_k = {\color{blue}X_k^-} + w_{k-1} = {\color{blue}AX_{k-1} + Bu_{k-1}} + w_{k-1}{\quad}(29-1)\\ Z_k = HX_k + v_k{\quad}(29-2) \end{cases}
{Xk=Xk−+wk−1=AXk−1+Buk−1+wk−1(29−1)Zk=HXk+vk(29−2)
其中：
- X k − = A X k − 1 + B u k − 1 X_k^-=AX_{k-1} + Bu_{k-1} Xk−=AXk−1+Buk−1被称为先验估计（计算值），和真实值还是有误差（被风吹歪了）；
- X k M e a s u r e = H − Z k X_k^{Measure}=H^-Z_k XkMeasure=H−Zk是测量值，包含了测量噪声。
估计值/后验估计
X
k
+
X_k^+
Xk+：
融合值
=
计算值
+
G
(
测量值
−
计算值
)
X
k
+
=
X
k
−
+
G
(
X
k
M
e
a
s
u
r
e
−
X
k
−
)
=
X
k
−
+
G
(
H
−
Z
k
−
X
k
−
)
{
X
k
+
=
X
K
−
=
计算值
,
G
=
0
X
K
+
=
H
−
Z
k
=
测量值
,
G
=
1
令
G
=
K
k
H
X
k
+
=
X
k
−
+
K
k
(
Z
k
−
H
X
k
−
)
{
X
k
+
=
X
K
−
=
计算值
,
K
=
0
X
K
+
=
H
−
Z
k
=
测量值
,
K
=
H
−
{\color{red}融合值=计算值+G(测量值-计算值)} \\ X_k^+=X_k^-+G({\color{blue}X_k^{Measure}}-X_k^-) \\ =X_k^-+G({\color{blue}H^-Z_k}-X_k^-) \\ \begin{cases} X_k^+ = X_K^-=计算值 {\quad},G=0 \\ X_K^+ = H^-Z_k=测量值 {\quad},G=1 \\ \end{cases} \\ {\color{red}令{\quad} G = K_kH} \\ X_k^+=X_k^-+K_k(Z_k-HX_k^-) \\ \begin{cases} X_k^+ = X_K^-=计算值 {\quad},K=0 \\ X_K^+ = H^-Z_k=测量值 {\quad},K=H^- \\ \end{cases}
融合值=计算值+G(测量值−计算值)Xk+=Xk−+G(XkMeasure−Xk−)=Xk−+G(H−Zk−Xk−){Xk+=XK−=计算值,G=0XK+=H−Zk=测量值,G=1令G=KkHXk+=Xk−+Kk(Zk−HXk−){Xk+=XK−=计算值,K=0XK+=H−Zk=测量值,K=H−
- 定义误差（真实值-估计值） e k = X k − X k + e_k=X_k-X_k^+ ek=Xk−Xk+，寻找 K k K_k Kk使得 X k + → X k X_k^+{\to}X_k Xk+→Xk，即 e k → 0 e_k{\to}0 ek→0；
- 误差也服从高斯分布，即 P ( e k ) − ( 0 , P k ) P(e_k)-(0,P_k) P(ek)−(0,Pk)， P k P_k Pk为误差 e k e_k ek的协方差矩阵，即使得 P k P_k Pk的迹最小；
- 定义先验误差（真实值-先验估计） e k − = X k − X k − e_k^-=X_k-X_k^- ek−=Xk−Xk−；
- P k P_k Pk代表第k次的误差协方差矩阵， P k − P_k^- Pk−代表第k次的先验误差协方差矩阵。
详细推导
配合食用：
(
1
−
1
)
→
(
1
−
2
)
(1-1){\to}(1-2)
(1−1)→(1−2)：
X
k
−
X
k
+
=
X
k
−
[
X
k
−
+
K
k
(
Z
k
−
H
X
k
−
)
]
=
X
k
−
X
k
−
−
K
k
Z
k
+
K
k
H
X
k
−
(
∵
Z
k
=
H
X
k
+
v
k
)
=
X
k
−
X
k
−
−
K
k
H
X
k
−
K
k
v
k
+
K
k
H
X
k
−
=
(
X
k
−
X
k
−
)
−
K
k
H
(
X
k
−
X
k
−
)
−
K
k
v
k
=
(
I
−
K
k
H
)
(
X
k
−
X
k
−
)
−
K
k
v
k
=
(
I
−
K
k
H
)
e
k
−
−
K
k
v
k
\begin{align*} {\color{blue}X_k-X_k^+} &= X_k-[X_k^-+K_k(Z_k-HX_k^-)] \\ &=X_k-X_k^- - K_k{\color{red}Z_k} + K_kHX_k^- \\ &{\qquad}({\because}{\qquad}{\color{red}Z_k} = HX_k+v_k) \\ &=X_k-X_k^- - K_kHX_k - K_kv_k + K_kHX_k^- \\ &=(X_k - X_k^-) - K_kH(X_k - X_k^-) - K_kv_k \\ &=(I - K_kH)(X_k - X_k^-) - K_kv_k \\ &=(I - K_kH)e_k^- - K_kv_k \end{align*}
Xk−Xk+=Xk−[Xk−+Kk(Zk−HXk−)]=Xk−Xk−−KkZk+KkHXk−(∵Zk=HXk+vk)=Xk−Xk−−KkHXk−Kkvk+KkHXk−=(Xk−Xk−)−KkH(Xk−Xk−)−Kkvk=(I−KkH)(Xk−Xk−)−Kkvk=(I−KkH)ek−−Kkvk
(
1
−
2
)
→
(
1
−
3
)
(1-2){\to}(1-3)
(1−2)→(1−3)：
(
A
B
)
T
=
B
T
A
T
(
A
+
B
)
T
=
A
T
+
B
T
(AB)^T = B^TA^T \\ (A+B)^T = A^T + B^T
(AB)T=BTAT(A+B)T=AT+BT
(
1
−
4
)
→
(
1
−
5
)
(1-4){\to}(1-5)
(1−4)→(1−5)：
∵
E
[
(
I
−
K
k
H
)
e
k
−
v
k
T
K
k
T
]
=
(
I
−
K
k
H
)
K
k
T
E
[
e
k
−
v
k
T
]
e
k
−
,
v
k
T
互不相关
=
(
I
−
K
k
H
)
K
k
T
E
[
e
k
−
]
E
[
v
k
T
]
e
k
−
,
v
k
T
均服从
(
0
,
σ
2
)
=
0
{\because}{\quad}E{\color{green}[(I - K_kH)e_k^-v_k^TK_k^T]} \\ {\color{green}=(I - K_kH)K_k^TE[e_k^-v_k^T]} \\ e_k^-,v_k^T互不相关 \\ {\color{green}=(I - K_kH)K_k^TE[e_k^-]E[v_k^T]}\\ e_k^-,v_k^T均服从(0,\sigma^2) \\ =0
∵E[(I−KkH)ek−vkTKkT]=(I−KkH)KkTE[ek−vkT]ek−,vkT互不相关=(I−KkH)KkTE[ek−]E[vkT]ek−,vkT均服从(0,σ2)=0
(
1
−
6
)
→
(
1
−
7
)
(1-6){\to}(1-7)
(1−6)→(1−7)：测量噪声协方差矩阵
E
[
v
k
v
k
T
]
=
R
E[v_kv_k^T] = R
E[vkvkT]=R
(
1
−
8
)
→
(
1
−
9
)
(1-8){\to}(1-9)
(1−8)→(1−9)：
[
(
P
k
−
H
T
)
K
k
T
]
T
=
K
k
(
P
K
−
H
T
)
T
=
K
k
H
P
k
−
T
t
r
(
K
k
H
P
k
−
)
=
t
r
(
K
k
H
P
k
−
T
)
[(P_k^-H^T)K_k^T]^T = K_k(P_K^-H^T)^T = K_kHP_k^{-T} \\ tr(K_kHP_k^-) = tr(K_kHP_k^{-T})
[(Pk−HT)KkT]T=Kk(PK−HT)T=KkHPk−Ttr(KkHPk−)=tr(KkHPk−T)
(
1
−
9
)
→
(
1
−
10
)
(1-9){\to}(1-10)
(1−9)→(1−10)：
P
k
−
跟
K
k
无关
d
(
A
B
)
d
A
=
B
T
d
(
A
B
A
T
)
d
A
=
2
A
B
P_k^-跟K_k无关 \\ \frac{d(AB)}{dA} = B^T \\ \frac{d(ABA^T)}{dA} = 2AB
Pk−跟Kk无关dAd(AB)=BTdAd(ABAT)=2AB
卡尔曼滤波中最核心的公式：卡尔曼增益
K
k
=
P
k
−
H
T
H
P
K
−
H
T
+
R
K_k = \frac{P_k^-H^T}{HP_K^-H^T+R}
Kk=HPK−HT+RPk−HT
- R → ∞ R{\to}\infin R→∞， K k → 0 K_k{\to}0 Kk→0， R R R是测量噪声协方差矩阵，测量噪声很大，更愿意相信计算的结果；
- R → 0 R{\to}0 R→0， K k → H − K_k{\to}H^- Kk→H−，测量噪声小，更愿意相信测量的结果。
误差协方差矩阵 P k − P_k^- Pk−
先验估计：
X
k
−
=
A
X
k
−
1
+
B
u
k
−
1
X_k^- = AX_{k-1} + Bu_{k-1}
Xk−=AXk−1+Buk−1
后验估计：
X
k
+
=
X
k
−
+
K
k
(
Z
k
−
H
X
k
−
)
X_k^+=X_k^-+K_k(Z_k-HX_k^-) \\
Xk+=Xk−+Kk(Zk−HXk−)
卡尔曼增益：
K
k
=
P
k
−
H
T
H
P
K
−
H
T
+
R
K_k = \frac{P_k^-H^T}{HP_K^-H^T+R}
Kk=HPK−HT+RPk−HT
误差协方差矩阵的推导：
P
k
−
=
E
[
e
k
−
e
k
−
T
]
=
E
[
(
A
e
k
−
1
+
w
k
−
1
)
(
A
e
k
−
1
+
w
k
−
1
)
T
]
=
.
.
.
=
A
P
k
−
1
A
T
+
Q
\begin{align*} P_k^- &= E[{\color{blue}e_k^-} e_k^{-T}] \\ &=E[(Ae_{k-1}+w_{k-1})(Ae_{k-1}+w_{k-1})^T] \\ &= ... \\ &=AP_{k-1}A^T + Q \end{align*}
Pk−=E[ek−ek−T]=E[(Aek−1+wk−1)(Aek−1+wk−1)T]=...=APk−1AT+Q
配合食用（这里的
X
k
−
=
A
X
k
−
1
−
−
B
u
k
−
1
X_k^-=AX_{k-1}^--Bu_{k-1}
Xk−=AXk−1−−Buk−1），和前面的先验估计不一样，不好理解：
e
k
−
=
X
k
−
X
k
−
=
A
X
k
−
1
+
B
u
k
−
1
+
w
k
−
1
−
A
X
k
−
1
−
−
B
u
k
−
1
=
A
e
k
−
1
−
+
w
k
−
1
\begin{align*} {\color{blue}e_k^-} &= X_k - X_k^- \\ &=AX_{k-1} + Bu_{k-1} + w_{k-1} - AX_{k-1}^- - Bu_{k-1} \\ &=Ae_{k-1}^- + w_{k-1} \end{align*}
ek−=Xk−Xk−=AXk−1+Buk−1+wk−1−AXk−1−−Buk−1=Aek−1−+wk−1
卡尔曼滤波的5个公式
重要变量：先验估计（即计算值），测量值，先验误差，先验误差协方差矩阵，误差，误差协方差矩阵，卡尔曼增益，最优估计。
X
k
−
=
A
X
k
−
1
+
B
u
k
−
1
P
k
−
=
A
P
k
−
1
A
T
+
Q
K
k
=
P
k
−
H
T
H
P
K
−
H
T
+
R
X
k
+
=
X
k
−
+
K
k
(
Z
k
−
H
X
k
−
)
P
k
=
(
I
−
K
k
H
)
P
k
−
\begin{align*} & X_k^- = AX_{k-1} + Bu_{k-1} \\ & P_k^- = AP_{k-1}A^T + Q \\ & K_k = \frac{P_k^-H^T}{HP_K^-H^T+R} \\ & X_k^+ = X_k^- + K_k(Z_k - HX_k^-) \\ & P_k = (I - K_kH)P_k^- \end{align*}
Xk−=AXk−1+Buk−1Pk−=APk−1AT+QKk=HPK−HT+RPk−HTXk+=Xk−+Kk(Zk−HXk−)Pk=(I−KkH)Pk−
前两步是预测，后两步是校正，最后一步是更新。
扩展的卡尔曼滤波
参考1
参考2
前面是是标准的卡尔曼滤波，即线性卡尔曼滤波，不能作用于非线性系统，而扩展的卡尔曼滤波可用于非线性系统。
线性化
利用泰勒级数对
f
(
x
)
f(x)
f(x)在
x
0
x_0
x0处展开得：
f
(
x
)
=
f
(
x
0
)
+
f
′
(
x
0
)
1
!
(
x
−
x
0
)
+
f
′
′
(
x
0
)
2
!
(
x
−
x
0
)
2
+
⋯
+
f
n
(
x
0
)
n
!
(
x
−
x
0
)
n
f(x) = f(x_0) + \frac{f'(x_0)}{1!}(x-x_0) + \frac{f''(x_0)}{2!}(x-x_0)^2 + \cdots + \frac{f^n(x_0)}{n!}(x-x_0)^n
f(x)=f(x0)+1!f′(x0)(x−x0)+2!f′′(x0)(x−x0)2+⋯+n!fn(x0)(x−x0)n
如果
x
→
x
0
x{\to}x_0
x→x0，则
(
x
−
x
0
)
2
→
0
,
⋯
,
(
x
−
x
0
)
n
→
0
(x-x_0)^2{\to}0,\cdots,(x-x_0)^n{\to}0
(x−x0)2→0,⋯,(x−x0)n→0：
f
(
x
)
=
f
(
x
0
)
+
f
′
(
x
0
)
(
x
−
x
0
)
=
k
1
+
k
2
(
x
−
x
0
)
=
k
x
+
b
f(x) = f(x_0) + f'(x_0)(x-x_0) \\ = k_1 + k_2(x-x_0) \\ = kx+b
f(x)=f(x0)+f′(x0)(x−x0)=k1+k2(x−x0)=kx+b
如上，我们把
f
(
x
)
f(x)
f(x)线性化成了
k
x
+
b
kx+b
kx+b。
例：
f
(
x
)
=
sin
(
x
)
=
sin
(
x
0
)
+
cos
(
x
0
)
(
x
−
x
0
)
(
l
e
t
x
0
=
0
)
=
0
+
x
−
x
0
=
x
\begin{align*} f(x) &= \sin(x) \\ &= \sin(x_0) + \cos(x_0)(x-x_0) \\ &{\qquad}(let{\quad}x_0=0) \\ &= 0 + x-x_0 \\ &= x \end{align*}
f(x)=sin(x)=sin(x0)+cos(x0)(x−x0)(letx0=0)=0+x−x0=x
上式表明，
s
i
n
(
x
)
sin(x)
sin(x)在
x
=
0
x=0
x=0附近和
x
x
x是相等的，分析误差：
{
sin
(
π
6
)
=
1
/
2
,
π
6
=
0.52
,
e
r
r
=
0.52
−
0.5
0.5
=
4
%
sin
(
π
4
)
=
0.707
,
π
4
=
0.785
,
e
r
r
=
0.785
−
0.707
0.707
=
11
%
\begin{cases} \sin(\frac{\pi}{6}) = 1/2,{\quad}\frac{\pi}{6}=0.52,{\quad}err=\frac{0.52-0.5}{0.5}=4\% \\ \sin(\frac{\pi}{4}) = 0.707,{\quad}\frac{\pi}{4}=0.785,{\quad}err=\frac{0.785-0.707}{0.707}=11\% \\ \end{cases}
{sin(6π)=1/2,6π=0.52,err=0.50.52−0.5=4%sin(4π)=0.707,4π=0.785,err=0.7070.785−0.707=11%
线性化解决EKF
EKF的基本思想是利用泰勒级数展开将非线性系统线性化，然后采用卡尔曼滤波框架对信号进行滤波，因此它是一种次优滤波。
**标准(线性)**卡尔曼滤波KF的状态转移方程和观测方程为：
{
θ
k
=
A
θ
k
−
1
+
B
u
k
−
1
+
s
k
z
k
=
C
θ
k
+
v
k
\begin{cases} \theta_k=A\theta_{k-1}+Bu_{k-1}+s_k\\ z_k=C\theta_k+v_k \end{cases}
{θk=Aθk−1+Buk−1+skzk=Cθk+vk
**扩展(非线性)**卡尔曼滤波EKF的状态转移方程和观测方程为：
{
θ
k
=
f
(
θ
k
−
1
)
+
s
k
z
k
=
h
(
θ
k
)
+
v
k
\begin{cases} \theta_k=f(\theta_{k-1})+s_k\\ z_k=h(\theta_k)+v_k \end{cases}
{θk=f(θk−1)+skzk=h(θk)+vk
利用泰勒展开式对式xx在上一次的估计值
<
θ
k
−
1
>
<\theta_{k-1}>
<θk−1>处展开得：
θ
k
=
f
(
θ
k
−
1
)
+
s
k
=
f
(
<
θ
k
−
1
>
)
+
F
k
−
1
(
θ
k
−
1
−
<
θ
k
−
1
>
)
+
s
k
\theta_k = f(\theta_{k-1})+s_k \\ = f(<\theta_{k-1}>) + F_{k-1}(\theta_{k-1}-<\theta_{k-1}>) + s_k \\
θk=f(θk−1)+sk=f(<θk−1>)+Fk−1(θk−1−<θk−1>)+sk
再利用泰勒展开式对式xx在本轮得状态预测值
θ
k
′
\theta_k'
θk′处展开得：
z
k
=
h
(
θ
k
)
+
v
k
=
h
(
θ
k
′
)
+
H
k
(
θ
k
−
θ
k
′
)
+
v
k
z_k = h(\theta_k) + v_k = h(\theta_k') + H_k(\theta_k - \theta_k') + v_k
zk=h(θk)+vk=h(θk′)+Hk(θk−θk′)+vk
其中，
F
k
−
1
F_{k-1}
Fk−1和
H
k
H_k
Hk分别表示函数
f
(
θ
)
f(\theta)
f(θ)和
h
(
θ
)
h(\theta)
h(θ)在
<
θ
k
−
1
<\theta_{k-1}
<θk−1和
θ
k
′
\theta_k'
θk′处的雅可比矩阵。
注：这里对泰勒展开式只保留到一阶导，二阶导数以上的都舍去，噪声假设均为加性高斯噪声。
