本文详细介绍了卡尔曼滤波的基本原理，包括状态空间模型、预测与更新步骤、卡尔曼增益计算以及误差协方差矩阵的处理。通过一个实例演示了如何应用这些概念进行状态估计和数据融合。
本文是对卡尔曼滤波学习的记录，主要参照了DR_CAN老师的视频进行学习，文章末尾有链接！（相关式子的定义见上文：详细推导卡尔曼滤波(Kalman Filter)状态估计、协方差矩阵、数据融合【非常详细推导，认真推理，包你看得懂】 上）
卡尔曼滤波
卡尔曼滤波（Kalman Filtering） 是一种用于估计系统状态的数学方法，它结合了传感器测量和系统模型的信息，以获得对系统状态的最佳估计。卡尔曼滤波最初是由Rudolf E. Kálmán在1960年代提出的，用于航空航天领域的导航和控制问题。自那时以来，它已经在许多领域得到了广泛应用，包括工程、经济学、生物学等。
卡尔曼滤波的主要思想是通过融合系统的动态模型（通常是线性系统）和实际测量值来估计系统的状态。它基于两个主要假设：
(1) 系统的动态行为可以用线性动态方程描述。
(2) 测量噪声和过程噪声都是高斯分布的，并且彼此独立。
卡尔曼滤波的过程可以概括为以下几个步骤：
(1) 预测（Predict）：根据系统的动态模型，使用先前的状态估计来预测下一个时刻的状态，并估计状态的不确定性。
(2) 更新（Update）：将新的测量值与预测值进行比较，根据它们的不确定性，更新状态的估计值和不确定性。这一步使用贝叶斯推断来融合测量和预测信息。
误差协方差矩阵
假设有离散系统的状态空间方程为：
X
k
=
A
X
k
−
1
+
B
U
k
−
1
+
W
k
−
1
Z
k
=
H
X
k
+
V
k
\begin{gather} \begin{aligned} &X_{k}=A X_{k-1}+B U_{k-1} +W_{k-1}\\ &Z_k=HX_{k}+V_k \end{aligned}\end{gather}
Xk=AXk−1+BUk−1+Wk−1Zk=HXk+Vk先验估计：
X
k
−
^
=
A
X
^
k
−
1
+
B
U
k
−
1
(
已知可求
)
\begin{gather}\hat{X_{k}^-} = A \hat{X}_{k-1}+B U_{k-1} \ \ \ \ (已知可求) \end{gather}
Xk−^=AX^k−1+BUk−1 (已知可求)后验估计：
X
^
k
=
X
^
k
−
+
k
k
(
Z
k
−
H
X
^
k
−
)
(
仅卡尔曼增益
k
k
未知
)
\begin{gather}\begin{array}{ll} \hat{X}_k=\hat{X}_{k}^{-}+k_k(Z_k-H\hat{X}_k^{-}) \ \ \ \ \ \ \ (仅卡尔曼增益k_k未知) \end{array}\end{gather}
X^k=X^k−+kk(Zk−HX^k−) (仅卡尔曼增益kk未知)卡尔曼增益：
k
k
=
P
k
−
H
T
H
P
k
−
H
T
+
R
(
仅先验协方差
P
k
−
未知
)
求
P
k
−
=
E
[
e
k
−
e
k
−
T
]
\begin{gather}k_k =\frac{P_k^-H^T}{HP_k^-H^T+R} \ \ \ \ \ \ \ (仅先验协方差P_k^-未知) \\ 求 \ \ P_k^-=E[e_k^- \ \ {e_k^-}^T]\end{gather}
kk=HPk−HT+RPk−HT (仅先验协方差Pk−未知)求 Pk−=E[ek− ek−T]先验误差
e
k
−
e_k^-
ek− , 将公式(1)(2)代入得:
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
(
X
k
−
1
−
X
^
k
−
1
)
+
W
k
−
1
=
A
e
k
−
1
+
W
k
−
1
\begin{gather} \begin{aligned}e_k^-=X_k-\hat{X_{k}^-} =&A X_{k-1}+B U_{k-1} +W_{k-1}-A \hat{X}_{k-1}-B U_{k-1}\\ =&A (X_{k-1}-\hat{X}_{k-1})+W_{k-1}\\ =&Ae_{k-1}+W_{k-1}\end{aligned}\end{gather}
ek−=Xk−Xk−^===AXk−1+BUk−1+Wk−1−AX^k−1−BUk−1A(Xk−1−X^k−1)+Wk−1Aek−1+Wk−1将(6)代入(5)得：
P
k
−
=
E
[
(
A
e
k
−
1
+
W
k
−
1
)
(
A
e
k
−
1
+
W
k
−
1
)
T
]
=
E
[
(
A
e
k
−
1
+
W
k
−
1
)
(
e
k
−
1
T
A
T
+
W
k
−
1
T
)
]
=
E
[
A
e
k
−
1
e
k
−
1
T
A
T
]
+
E
[
W
k
−
1
W
k
−
1
T
]
=
A
P
k
−
1
A
T
+
Q
(
注：误差协方差矩阵
P
k
−
1
和过程噪声协方差
Q
在上文有定义
)
\begin{gather} \begin{aligned}P_k^-&=E[(Ae_{k-1}+W_{k-1})(Ae_{k-1}+W_{k-1})^T]\\&=E[(Ae_{k-1}+W_{k-1})(e_{k-1}^TA^T+W_{k-1}^T)]\\&=E[Ae_{k-1}e_{k-1}^TA^T]+E[W_{k-1}W_{k-1}^T]\\&=AP_{k-1}A^T+Q \ \ \ \ (注：误差协方差矩阵P_{k-1}和过程噪声协方差 Q在上文有定义)\end{aligned}\end{gather}
Pk−=E[(Aek−1+Wk−1)(Aek−1+Wk−1)T]=E[(Aek−1+Wk−1)(ek−1TAT+Wk−1T)]=E[Aek−1ek−1TAT]+E[Wk−1Wk−1T]=APk−1AT+Q (注：误差协方差矩阵Pk−1和过程噪声协方差Q在上文有定义)至此，将(7)代入(4) 得后验估计所有项均已知：
X
^
k
=
X
^
k
−
+
k
k
(
Z
k
−
H
X
^
k
−
)
\begin{gather}\begin{array}{ll} \hat{X}_k=\hat{X}_{k}^{-}+k_k(Z_k-H\hat{X}_k^{-}) \end{array}\end{gather}
X^k=X^k−+kk(Zk−HX^k−)注：完成后验估计后，需要更新上一时刻的误差协方差矩阵
P
k
−
1
P_{k-1}
Pk−1为本时刻
P
k
P_{k}
Pk为：
P
k
=
(
I
−
k
k
H
)
P
k
−
\begin{gather}\begin{array}{ll} P_{k}=(I-k_kH)P_k^- \end{array}\end{gather}
Pk=(I−kkH)Pk−
自此，卡尔曼滤波的五个重要公式分别为：
1、预测：先验估计（2）、先验误差协方差矩阵（7）；
2、校正：卡尔曼增益（4）、后验估计（8）以及更新误差协方差（5）.
DR_CAN老师给出个例子： 密码是6.66（时间允许建议自己算一下，卡尔曼滤波(Kalman Filter)状态估计、协方差矩阵、数据融合，看这一文即可，【非常详细推导，认真推理，包你看得懂】 上，轻松解决）
链接: DR_CAN卡尔曼滤波例子
敲公式不易，麻烦各位看官一键三连！感谢！欢迎收藏以便后续用到的时候查公式！
扫一扫
6912
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
