计算机视觉——卡尔曼滤波

计算机视觉—卡尔曼滤波

brycezou@163.com

        整篇文章参考微信公众号【电子搬砖师】，有兴趣的读者可以关注一下，感谢作者！

        最近发现，只有理论推导而没有贴实用代码的博客阅读量都比较少，由此可见，技术博客也是快餐文化的一种。本人后面的博客尽量也贴一些典型代码，来满足更读者的胃口。卡尔曼滤波算法相关的代码后续补上\^_\^

0、卡尔曼滤波的核心内容

• 假设测量值和估计值都不是100%可信的，通过如下两步主要操作来估计真实值
  
  • 根据前一时刻的最优估计 $x(k-1|k-1)$ 得到当前时刻的普通估计 $x(k|k-1)$
  • 根据当前时刻的普通估计 $x(k|k-1)$ 和当前时刻的观测 $z(k)$，得到当前时刻的最优估计 $x(k|k)$

1、问题描述

        假设我们要测量房间的温度，用温度计进行了10分钟的测量，每隔1分钟测量一次，这10次的测量结果如下。我们还明白一个事实：由于测量设备（温度计）自身存在误差，测量过程也会引入干扰，因此，工程上通常不能直接使用测量设备的数据，而是先要对数据进行处理。那么问题来了，该如何进行处理？

25.3
26.5
24.0
35.0（温度计受到干扰）
22.9
25.6
23.5（开启暖气）
28.8
30.2
29.5

2、方案一：均值滤波

        均值滤波可简单描述为：连续采样 N 次，求均值后输出一次，再连续采样 N 次，求均值后输出一次，如此反复。对温度测量过程进行均值滤波，取 N=5，结果为：

T1=(25.3+26.5+24.0+35.0+22.9)/5=26.7
T2=(25.6+23.5+28.8+30.2+29.5)/5=27.5

均值滤波确实减缓了数据的跳变，但其更新频率太低，而且真实的变化过程（开启暖气）容易被淹没。

3、方案二：一阶滞后滤波

        为了提高更新频率，同时避免淹没真实的变化过程，可以采用一阶滞后滤波算法，
$$本次滤波结果=\alpha*本次采样值+(1-\alpha)*上次滤波结果$$

公式中的 $\alpha$ 表示对本次采样值的信任程度。设 $\alpha=0.6$，由于第1分钟时没有上次滤波结果，可以根据经验初始化为26.0，则结果为：

0.6*25.3+0.4*26.0=25.6
0.6*26.5+0.4*25.6=26.1
0.6*24.0+0.4*26.1=24.8
0.6*35.0+0.4*24.8=30.9
0.6*22.9+0.4*30.9=26.1
0.6*25.6+0.4*26.1=25.8
0.6*23.5+0.4*25.8=24.4
0.6*28.8+0.4*24.4=27.0
0.6*30.2+0.4*27.0=28.9
0.6*29.5+0.4*25.6=29.3

一阶滞后滤波算法既减缓了温度的跳变，又提高了更新频率。但是它仍然没能排除掉干扰，而且在房间开启暖气后温度变化有点滞后。

4、方案三：卡尔曼滤波

        一阶滞后滤波算法的权重 $\alpha$ 在计算过程中是保持不变的，而卡尔曼滤波算法的核心在于，可以根据测量值的变化实时更新权重。使用卡尔曼滤波需要知道的几个初始条件：

• 原始值：用于启动迭代，通常取传感器前 N 组测量数据的均值。本例中，原始温度取26.0度。
• 原始值误差：原始值通常是根据经验得到的，存在误差。本例中，假定原始温度误差是2度。
• 测量系统误差：假定温度计的测量误差为是3度。
• 下一次的最大偏差：由于房间温度相对恒定，即使开启暖气下一次最多也只能变化5度。

        使用卡尔曼滤波算法进行迭代：首先，用26.0作为原始温度，由于认为房间温度恒定，因此下一分钟的估计温度也是26.0度。其次，下一分钟的测量温度是25.3度。此时，我们有两个可以参考的温度值，26.0度和25.3度，究竟该如何权衡？定义测量值的信任比例为

$$kg=\sqrt{\frac{原始值误差^2+下一次的最大偏差^2}{原始值误差^2+下一次的最大偏差^2+测量系统误差^2}}$$

        代入本例中可得，测量值25.3的信任比例为

$$kg=\sqrt{\frac{2^2+5^2}{2^2+5^2+3^2}}=0.874$$

因此，第一分钟的最优温度估计为

T1=26.0*(1-0.874)+25.3*0.874=25.4

        这个25.4度将会成为下一轮迭代的原始值。此外，由于卡尔曼滤波的结果更加接近真实世界的数值，因此下一轮迭代的原始值误差也会被更新

$$下一轮原始值误差=\sqrt{(1-kg)*(本轮原始值误差^2+下一次的最大偏差^2)}$$

        代入本例中可得

$$原始值误差=\sqrt{(1-0.874)*(2^2+5^2)}=1.9$$

        下面进行第二轮迭代。首先，计算测量值26.5的信任比例

$$kg=\sqrt{\frac{1.9^2+5^2}{1.9^2+5^2+3^2}}=0.872$$

其次，计算第二分钟的最优温度估计

T2=25.4*(1-0.872)+26.5*0.872=26.4

最后，更新下一轮迭代的原始值误差

$$原始值误差=\sqrt{(1-0.872)*(1.9^2+5^2)}=1.9$$

        之后进行第三轮迭代 $\cdots$

        卡尔曼滤波经过多次迭代后会逐渐缩小原始值误差，输出结果也更加接近真实值。如果将本例中的下一次最大偏差从5度改为2度，那么传感器测量值的信任比例将会变小，滤波器输出的温度值跳动也会变小，对干扰的抑制作用会更加明显，但对于真实环境的响应也会变得更加滞后。

5、单变量形式推导

        参考： http://www.tina-vision.net/docs/memos/2003-003.pdf
        设温度的估计值服从正态分布
$$f(x)=\frac{1}{\sqrt{2\pi\sigma_估^2}}\exp\left(-\frac{(x-\mu_估)^2}{2\sigma_估^2}\right)$$

        设温度的测量值也服从正态分布

$$g(x)=\frac{1}{\sqrt{2\pi\sigma_测^2}}\exp\left(-\frac{(x-\mu_测)^2}{2\sigma_测^2}\right)$$

        融合两个正态分布，可得

h(x)=f(x)g(x)=12πσ