轻松一下看一个例子：

一片绿油油的草地上有一条曲折的小径，通向一棵大树。一个要求被提出：从起点沿着小径走到树下。
“很简单。” A说，于是他丝毫不差地沿着小径走到了树下。
现在，难度被增加了：蒙上眼。
“也不难，我当过特种兵。” B说，于是他歪歪扭扭地走到了树 ………. 旁。“唉，好久不练，生疏了。”
“看我的，我有 DIY 的 GPS！” C说，于是他像个醉汉似地走到了树………. 旁。“唉，这个 GPS 软件没做好，漂移太大。”
“我来试试。” 旁边一人拿过 GPS, 蒙上眼，居然沿着小径走到了树下。
“这么厉害！你是什么人?”
“卡尔曼 ! ”
“卡尔曼？！你是卡尔曼？”众人大吃一惊。
“我是说这个 GPS 卡而慢。”
抄自https://blog.csdn.net/bbs.21ic.com/icview-292853-1-1.html

综述：

卡尔曼滤波是一种最优线性估计算法而不是滤波器，其基于隐马尔可夫模型，其动态系统可用一个马尔科夫链表示，该马尔科夫链链被高斯噪声干扰。卡尔曼滤波分为两个步骤：1.状态预测。2状态更新

最简洁的方式介绍卡尔曼滤波：

图片来自:https://zhuanlan.zhihu.com/p/25598462?utm_source=com.miui.notes&utm_medium=social

详解：

卡尔曼滤波器的模型。圆圈代表向量，方块代表矩阵卡尔曼滤波模型假设k时刻的真实状态是从（k − 1）时刻的状态演化而来，符合下式：那么真实状态如下

${\textbf {x}}_{{k}}={\textbf {F}}_{{k}}{\textbf {x}}_{{k-1}}+{\textbf {B}}_{{k}}{\textbf {u}}_{{k}}+{\textbf {w}}_{{k}}$

其中

Fk是作用在xk−1上的状态变换模型（/矩阵/矢量）。
Bk是作用在控制器向量uk上的输入－控制模型。
wk是过程噪声，并假定其符合均值为零，协方差矩阵为Qk的多元正态分布。

${\textbf {w}}_{{k}}\sim N(0,{\textbf {Q}}_{k})$

时刻k，对真实状态xk的一个测量zk满足下式：

${\textbf {z}}_{{k}}={\textbf {H}}_{{k}}{\textbf {x}}_{{k}}+{\textbf {v}}_{{k}}$

其中Hk是观测模型，它把真实状态空间映射成观测空间，vk是观测噪声，其均值为零，协方差矩阵为Rk,且服从正态分布。

${\textbf {v}}_{{k}}\sim N(0,{\textbf {R}}_{k})$

初始状态以及每一时刻的噪声{x0, w1, ..., wk, v1 ... vk}都认为是互相独立的。

实际上，很多真实世界的动态系统都并不确切的符合这个模型；但是由于卡尔曼滤波器被设计在有噪声的情况下工作，一个近似的符合已经可以使这个滤波器非常有用了。更多其它更复杂的卡尔曼滤波器的变种。

卡尔曼滤波器详解：

卡尔曼滤波是一种递归的估计，即只要获知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值，因此不需要记录观测或者估计的历史信息。卡尔曼滤波器与大多数滤波器不同之处，在于它是一种纯粹的时域滤波器，它不需要像低通滤波器等频域滤波器那样，需要在频域设计再转换到时域实现。

卡尔曼滤波器的状态由以下两个变量表示：

${\hat {{\textbf {x}}}}_{{k|k}}$ ，在时刻k的状态的估计；
${\textbf {P}}_{{k|k}}$ ，后验估计误差协方差矩阵，度量估计值的精确程度。

卡尔曼滤波器的操作包括两个阶段：预测与更新。在预测阶段，滤波器使用上一状态的估计，做出对当前状态的估计。在更新阶段，滤波器利用对当前状态的观测值优化在预测阶段获得的预测值，以获得一个更精确的新估计值。

预测

${\hat {{\textbf {x}}}}_{{k|k-1}}={\textbf {F}}_{{k}}{\hat {{\textbf {x}}}}_{{k-1|k-1}}+{\textbf {B}}_{{k}}{\textbf {u}}_{{k}}$ (预测状态)

${\textbf {P}}_{{k|k-1}}={\textbf {F}}_{{k}}{\textbf {P}}_{{k-1|k-1}}{\textbf {F}}_{{k}}^{{T}}+{\textbf {Q}}_{{k}}$ （预测估计协方差矩阵）

可参考：http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf

更新

首先要算出以下三个量：

${\tilde {{\textbf {y}}}}_{{k}}={\textbf {z}}_{{k}}-{\textbf {H}}_{{k}}{\hat {{\textbf {x}}}}_{{k|k-1}}$ (测量余量，measurement residual)

${\textbf {S}}_{{k}}={\textbf {H}}_{{k}}{\textbf {P}}_{{k|k-1}}{\textbf {H}}_{{k}}^{{T}}+{\textbf {R}}_{{k}}$ (测量余量协方差)

${\textbf {K}}_{{k}}={\textbf {P}}_{{k|k-1}}{\textbf {H}}_{{k}}^{{T}}{\textbf {S}}_{{k}}^{{-1}}$ （最优卡尔曼增益）

然后用它们来更新滤波器变量x与P：

${\hat {{\textbf {x}}}}_{{k|k}}={\hat {{\textbf {x}}}}_{{k|k-1}}+{\textbf {K}}_{{k}}{\tilde {{\textbf {y}}}}_{{k}}$ (更新的状态估计)

${\textbf {P}}_{{k|k}}=(I-{\textbf {K}}_{{k}}{\textbf {H}}_{{k}}){\textbf {P}}_{{k|k-1}}$ （更新的协方差估计）

使用上述公式计算 ${\textbf {P}}_{{k|k}}$ 仅在最优卡尔曼增益的时候有效。使用其他增益的话，公式要复杂一些，请参见推导。

不变量（Invariant）

如果模型准确，而且 ${\hat {{\textbf {x}}}}_{{0|0}}$ 与 ${\textbf {P}}_{{0|0}}$ 的值准确的反映了最初状态的分布，那么以下不变量就保持不变：所有估计的误差均值为零

${\textrm {E}}[{\textbf {x}}_{k}-{\hat {{\textbf {x}}}}_{{k|k}}]={\textrm {E}}[{\textbf {x}}_{k}-{\hat {{\textbf {x}}}}_{{k|k-1}}]=0$
${\textrm {E}}[{\tilde {{\textbf {y}}}}_{k}]=0$

且协方差矩阵准确的反映了估计的协方差：

${\textbf {P}}_{{k|k}}={\textrm {cov}}({\textbf {x}}_{k}-{\hat {{\textbf {x}}}}_{{k|k}})$
${\textbf {P}}_{{k|k-1}}={\textrm {cov}}({\textbf {x}}_{k}-{\hat {{\textbf {x}}}}_{{k|k-1}})$
${\textbf {S}}_{{k}}={\textrm {cov}}({\tilde {{\textbf {y}}}}_{k})$

请注意，其中 ${\textrm {E}}[{\textbf {a}}]$ 表示 ${a}$ 的期望值, ${\textrm {cov}}({\textbf {a}})={\textrm {E}}[{\textbf {a}}{\textbf {a}}^{T}]$ 。

补充：

https://blog.csdn.net/u010720661/article/details/63253509

详解卡尔曼滤波

轻松一下看一个例子：

综述：

最简洁的方式介绍卡尔曼滤波：

详解：

卡尔曼滤波器详解：

猜你喜欢