本文基本上可以看做 https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/ 一文的中文翻译。
Kalman Filter 卡尔曼滤波
卡尔曼滤波器结合了 根据上一次状态的预测值 和这一次 状态的观测值（含噪声），来合理推测这一次状态的真实值。
其目的在于得到当前状态的准确值，避免各种噪声的影响。
建模系统状态为随机变量
假设系统的各个状态变量都是随机变量，且符合各自的正态分布。
因此，我们将k时刻系统的状态用各个随机变量的均值和他们的协方差矩阵表示，即 x k x_k xk 和 P k P_k Pk 。
协方差矩阵中的各个变量线性相关性，可以帮助我们通过一个状态变量估计其他的状态变量，达到更好的估计。
状态预测建模
假设系统的状态 x x x 满足马尔科夫性， F k F_k Fk 表示状态预测矩阵，我们根据上一次的预测值 x ^ k − 1 \hat{x}_{k-1} x^k−1 推测当前系统的预测值 x ^ k \hat{x}_{k} x^k。
x ^ k = F k x ^ k − 1 P k = C o n v ( x ^ k ) = C o n v ( F k x ^ k − 1 ) = F k C o n v ( x ^ k − 1 ) F k T = F k P k − 1 F k T \begin{align} \hat{x}_{k} =& \space F_k \hat{x}_{k-1} \\ P_k =& \space Conv(\hat{x}_{k}) \\ =& \space Conv(F_k \hat{x}_{k-1}) \\ =& \space F_k Conv(\hat{x}_{k-1}) F_k^T \\ =& \space F_k P_{k-1} F_k^T \end{align} x^k=Pk==== Fkx^k−1 Conv(x^k) Conv(Fkx^k−1) FkConv(x^k−1)FkT FkPk−1FkT
状态预测矩阵 F k F_k Fk 仅仅考虑系统状态，即是系统状态的函数，不考虑其他外力。
初始化
通过收集过往系统状态 x x x 的数据计算协方差，以初始化 P 0 P_0 P0 。
外部影响建模（控制和噪声）修正状态预测模型
外部控制
外部控制指的是不考虑在状态预测矩阵
F
k
F_k
Fk 中的变量，表示为影响系统状态的外力。
如果存在外力，则需要修改预测模型：
x ^ k = F k x ^ k − 1 + B k u k \begin{align} \hat{x}_{k} =& \space F_k \hat{x}_{k-1} + B_k u_k \\ \end{align} x^k= Fkx^k−1+Bkuk
B
k
B_k
Bk 称为控制矩阵，指的是某个外力影响系统状态的模型；
u
k
u_k
uk 称为控制向量，指的是外力的参数。
如果将外力也视为系统状态的一维，则外力影响加入到状态转移矩阵中。
环境影响（噪声）
外力控制模型 B k B_k Bk 只是简化的物理建模，实际上由于环境潜在因素的影响，不可能完全代表实际情况，因此将其他潜在的影响因素建模为噪声 Q k Q_k Qk。
为了简化问题，我们简单的将 Q k Q_k Qk 加入到原来的协方差矩形 P k P_k Pk 中作为环境影响：
P k = F k P k − 1 + Q k \begin{align} P_k =& \space F_k P_{k-1} + Q_k \\ \end{align} Pk= FkPk−1+Qk
方程 (6) (7) 就是完整的系统状态预测模型。
初始化
重复实验，计算状态预测值和实际值的差，然后计算其协方差，以初始化噪声的协方差矩阵Q。
测量/观察建模
将测量系统状态得到读数的过程建模为
H
k
H_k
Hk ，
则预测的测量读数（随机变量）的分布为：
μ
e
x
c
e
p
t
e
d
=
H
k
x
^
k
Σ
e
x
c
e
p
t
e
d
=
H
k
P
k
H
k
T
\begin{align} \mu_{excepted} =& \space H_k \hat{x}_k \\ \Sigma_{excepted} =& \space H_k P_k H_k^T \\ \end{align}
μexcepted=Σexcepted= Hkx^k HkPkHkT
最简单的 H k H_k Hk 就是进行系统状态物理量和传感器读数物理量的量纲转换。
将实际测量值视为一个随机变量，服从正态分布，均值
z
k
z_k
zk 等于当前实际测量量，协方差矩阵为
R
k
R_k
Rk 。
μ
m
e
a
s
u
r
e
d
=
z
k
Σ
m
e
a
s
u
r
e
d
=
R
k
\begin{align} \mu_{measured} =& \space z_k \\ \Sigma_{measured} =& \space R_k \\ \end{align}
μmeasured=Σmeasured= zk Rk
初始化
重复实验，计算传感器测量值和实际值的差，然后计算其协方差，以初始化传感器协方差矩阵 R k R_k Rk 。
结合预测模型和测量模型
解释一：概率乘法
方程 (8) (9) 和方程 (10) (11) 分别表达了系统状态预测值和测量值的分布，
将两个分布相乘，表达“使两分布都为真的系统状态真实值”的概率分布。
将两个正态分布相乘，得到一个新的正态分布（数学上不满足，因为存在一个缩放因子A）：
k = μ 1 2 μ 1 2 + μ 2 2 μ 0 = μ 1 + k ( μ 2 − μ 1 ) σ 0 2 = σ 1 2 − k σ 1 2 \begin{align} k =& \space \frac{\mu_1^2}{\mu_1^2 + \mu_2^2} \\ \mu_0 =& \space \mu_1 + k (\mu_2-\mu_1) \\ \sigma_0^2 =& \space \sigma_1^2 - k \sigma_1^2 \\ \end{align} k=μ0=σ02= μ12+μ22μ12 μ1+k(μ2−μ1) σ12−kσ12
矩阵版本：
K = Σ 1 ( Σ 1 + Σ 2 ) − 1 μ 0 = μ 1 + K ( μ 2 − μ 1 ) Σ 0 = Σ 1 − K Σ 1 \begin{align} K =& \space \Sigma_1 (\Sigma_1 + \Sigma_2)^{-1} \\ \mu_0 =& \space \mu_1 + K (\mu_2-\mu_1) \\ \Sigma_0 =& \space \Sigma_1 - K \Sigma_1 \\ \end{align} K=μ0=Σ0= Σ1(Σ1+Σ2)−1 μ1+K(μ2−μ1) Σ1−KΣ1
称 K 为卡尔曼增益。
解释二：贝叶斯定理
方程 (8) (9) 视为先验概率 P ( z ∣ x ) P(z|x) P(z∣x) ，方程 (10) (11) 视为 P ( x ) P(x) P(x)，通过两者相乘得到后验概率 P ( x ∣ z ) P(x|z) P(x∣z) 。
整合版
结合两个正态分布 (8, 9) 和 (10, 11) ，将他们带入方程 (15, 16, 17) 中，得到：
K = H k P k H k T ( H k P k H k T + R k ) − 1 H k x ^ k ′ = H k x ^ k + K ( z k − H k x ^ k ) H k P k ′ H k T = H k P k H k T − K H k P k H k T \begin{align} K =& \space H_k P_k H_k^T (H_k P_kH_k^T + R_k)^{-1} \\ H_k \hat{x}_k' =& \space H_k \hat{x}_k + K (z_k - H_k \hat{x}_k) \\ H_k P_k' H_k^T =& \space H_k P_k H_k^T - K H_k P_k H_k^T \\ \end{align} K=Hkx^k′=HkPk′HkT= HkPkHkT(HkPkHkT+Rk)−1 Hkx^k+K(zk−Hkx^k) HkPkHkT−KHkPkHkT
将 (19, 20) 左侧除掉 H k H_k Hk ，并修改 K K K 的定义，再将 (20) 右侧除掉 H k T H_k^T HkT ，得到最终的预测与更新方程：
K ′ = P k H k T ( H k P k H k T + R k ) − 1 x ^ k ′ = x ^ k + K ′ ( z k − H k x ^ k ) P k ′ = P k − K ′ H k P k \begin{align} K' =& \space P_k H_k^T (H_k P_kH_k^T + R_k)^{-1} \\ \hat{x}_k' =& \space \hat{x}_k + K' (z_k - H_k \hat{x}_k) \\ P_k' =& \space P_k - K' H_k P_k \\ \end{align} K′=x^k′=Pk′= PkHkT(HkPkHkT+Rk)−1 x^k+K′(zk−Hkx^k) Pk−K′HkPk
整个流程如下：
3544
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
