神秘的 卡尔曼滤波器

$ 强大的卡尔曼滤波器

卡尔曼滤波器，又称为最佳线性滤波器，时域滤波器。卡尔曼滤波是控制系统理论，计算机视觉和图像处理和信道滤波都会有它的身影。

$ 为什么需要卡尔曼滤波器

• 参考：http://blog.csdn.net/MangZuo/article/details/71171137

1 概念

最近养了只小兔子，接下来拿小兔子做个例子。每天喂兔子吃得好好的，看它一天天在长大，就忍不住想了解它的体重增长情况。

那我们就以小兔子的体重作为研究对象。

假定我每周做一次观察，我有两个办法可以知道兔子的体重：

• 一个是拿体重计来称（兔子太调皮，不能老实呆着，弹簧秤因为小兔子的晃动会产生很大误差）。尽管有误差，那也是一个不可失的渠道来得到兔子的体重。
• 还有一个途径是参考书本上的资料，根据兔子的年龄可以估计一下我的小兔子应该会多重。

我们把用体重计称出来的叫观察量，用资料估计出来的叫估计值，无论是观察值还是估计值显然都是有误差的，假定误差是高斯分布。

---

2 加权平均的思想

现在问题来了，按照书本上说我的兔子该3斤重，称出来却只有2.5斤，我究竟该信哪个呢？再强调一遍是我们的现状是兔兔不够乖，体重计还很烂。
在这样恶劣的情景下，卡尔曼先生告诉我们一个办法，仍然可以估计出八九不离十的兔兔体重。
这个办法其实很直白，就是加权平均：把称出来的结果即观测值和按照书本经验估算出来的即估计值，分别加一个权值，再做平均。当然这两个权值加起来是等于 1 的。例如如果你有 70% 分相信称出来的体重，那么就只有 30% 相信书上的估计。

---

3 代表信任度的权值

说到这里大家一定会问，究竟该有几分相信书上的，有几分相信我自己称的呢？都怪体重计不争气，没法让我 100% 信赖它，还要根据书上的数据来做调整。
好在卡尔曼先生也发现了这个问题，于是找到一个办法来决定这个权值问题，这个办法也很直白，就是根据以往的表现来做决定，这听起来很公平：你以前表现好，我就相信你多一点，权值也就给的高一点，以前表现不好，权值自然低一点。
什么是好的表现呢，表现好就是测量结果稳定，方差很小，表现不好就是估计值或观测值不稳定，方差很大。假如你称兔子，第一次 1 斤第二次 10 斤，第三次 25 斤，肯定不能轻易相信这个称。但是如果第一次 3 斤第二次 3.2 斤，第三次 2.8 斤，自然我就相信它多一点 。

---

有了这个权值，接下来的事情就好办了。我们只能一步一步地调整我们的观察和估计值，来渐渐达到准确的测量。调整的过程也很简单，就是把实测值和估计值比较一下，如果估计值比测量值小，那就把估计值加上它们之间的偏差作为新的估计值，当然前面要添上加权系数，公式如下：
$$X_{t+1}=X_t+K(Z-X_t)$$
加权系数，又叫卡尔曼增益。
$$X_{t+1}=X_t+K(Z-X_t)=(1-K)\cdot X_t+KZ$$
即估计值的权值是 $1-K$，而观察值的权值是 $K$，而 $K$ 的取值取决于估计值和观察值以前的表现（方差）了。

4 卡尔曼滤波的分类

• Extended Kalman Filter：因为高斯模型有时不适用，于是有了Extended Kalman Filter。

• Particle Filter：这是用于多个对象的，比如除了兔子还有只猫，他们的体重有一个联合概率模型，每一个对象就是一个particle。

• Kalman Filter 传递的是高斯分布，Particle Filter 传递的是高斯混合分布。

$ 卡尔曼滤波的数学理解

卡尔曼滤波的默认假定：任何测量结果都有噪声，状态转移过程会有噪声，你想知道系统的真实值么，哪有那么容易！

===================

卡尔曼滤波建模：一个系统会处在各种不同的状态，并且会在状态之间转移。但是渺小的人类谁也不知道系统当前的状态，但幸运的是我们可以通过测量的结果猜测到系统当前的状态。

---

1 协方差矩阵

一维随机分布（以高斯分布为例），只需要简单的统计量即可描述：样本的均值，方差，或者再加个标准差。
但对于多维分布（如二维高斯分布），均值和方差是不够的。

一维分布 可以如下表述：

---

但对于二维高斯分布，就需要考虑两个维度是否独立或者相关：（下图第 1 幅表示独立）

$$cov(X,Y)=\frac{{\sum }_{i=1}^n(X_i-\bar{X})(Y_i-\bar{Y})}{n-1}$$
$$cov(X,Y)=cov(Y,X)$$
协方差矩阵：
$$C=\left[\begin{array}{cc}{\sigma }_{11}& {\sigma }_{12}\\ {\sigma }_{12}& {\sigma }_{22}\end{array}\right]=\left[\begin{array}{cc}cov(x,x)& cov(x,y)\\ cov(x,y)& cov(y,y)\end{array}\right]$$
协方差矩阵性质：
$$cov(Ax,By)=Acov(x,y)B^T$$

---

2 几个重要方程

状态预测公式：
$${\hat{x}}_t^{-}=F_t{\hat{x}}_{t-1}+B_t{u}_t$$
${\hat{x}}_t^{-}$有一个减号的上标，表示该估计欠准确，需要更新。$x_t$ 是状态，例如表示某时刻的位置和速度：
$$x_t=\left[\begin{array}{c}{p}_t\\ v_t\end{array}\right]$$

===============

预测不确定性传递公式：
$$P_t^{-}=F{P}_{t-1}{F}^T+Q$$
利用协方差矩阵的 性质 得到上式形式。$P$ 为代表不确定性的协方差矩阵，它是由上一时刻的协方差矩阵和外部噪声的协方差一起计算得到的。$F$ 是状态转移矩阵。$Q$ 是预测模型本身带来的噪声协方差矩阵。

===============

观测公式：
$$z_t=H{x}_t+R$$
$z_t $ 是观测值。$H$ 是观测矩阵，例如观测传感器返回的信息其单位和尺度可能与我们在预测步骤中所跟踪的状态量的单位和尺度有所不同。$R$ 是观测噪声协方差矩阵。

===============

由之前的卡尔曼增益 更新 ${\hat{x}}_t$：
$$X_{t+1}=X_t+K(Z-X_t)$$
$${\hat{x}}_t={\hat{x}}_t^{-}+K_t(z_t-H{\hat{x}}_t^{-})$$

===============

$K_t$ 是怎么得到的 ？
$$K_t=P_t^{-}{H}^T(H{P}_t^{-}{H}^T+R{)}^{-1}$$
$K$ 里面已经包含了协方差矩阵 $P$ 的信息。
参考文献：秦永元，张洪钺，汪叔华，卡尔曼滤波与组合导航原理，西北工业大学出版社

===============

不确定性的更新：
$$P_t=(I-K_{t}H){P_t}^{-}$$

3 流程

$$\begin{gathered} {\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_k(1) \\ {P}_k^{-}=A{P}_{k-1}{A}^T+Q(2) \\ {K}_k=P_k^{-}{H}^T(H{P}_k^{-}{H}^T+R{)}^{-1}(3) \\ {\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-})(4) \\ {P}_k=(I-K_{k}H)P_k^{-}(5) \end{gathered}$$

下图左边为预测组，右边为更新组。