图像处理之 _ 卡尔曼滤波

1. 用途

根据一些已知的量来预测未知的量。常用于运动预测。

2. 定义

卡尔曼滤波（Kalman filtering）一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。

由于观测数据中包括系统中的噪声和干扰的影响，最优估计也可看作是滤波过程，因此叫作卡尔曼滤波。

3. 原理分析

1) 第一部分：预测值与误差

  1. 假设你和我两个人每天给同一头猪测体重，但两个称都不准。你测 x1 斤，正负误差在σ1 斤左右（假设预测成高斯分布，标准差反映了测量值好坏程度的不确定性），我测 x2 斤，正负误差在σ2 斤左右。结合两个人的预测，简单计算猪的体重是 x1 和 x2 的平均值。
图片.png
  1. 假如我的称更准，即误差更小，σ2<σ1，结合两个人的测量，应该以我为主，因此根据误差加权组合。
图片.png

（通过概率函数导数的最大值推出上述公式，不在此详述）

由公式可知：如果你的不确定性σ1 越大，我的测量值 x2 在新均值中占的比例越大（权值更大）。

  1. 后来你走了，用上一次的加权平均（x1,σ1）替代你的测量，我的测量值仍是（x2, σ2）（此后我们把加权平均的结果称为预测值）。同样采取加权平均的算法，算出本次预测值（x2,σ2），这是一个迭代的过程。通过化简得到以下公式：
图片.png

请注意，现在公式中的 1,2 不再代表你和我，而代表时间点，带的为预测值，不带的为测量值。也就是说新的预测值（x2^, σ2）由前一个预测值（x1, σ1^）和本次测量值（x2, σ2）求得。为简化公式引入更新率Ｋ，即卡尔曼增益 (Kalman Gain)。将公式简化为以下形式：

图片.png

从简化之后的公式可知，K 是由误差算出的，K 用于预测新的 x 和误差σ。由公式可知：前次预测的不确定性σ1^越大，K 越大，我的预测 x2 越重要。

  1. 综上，我们已经看到了最简单（单值且预测与测量的值相同）情况下的测量值与前次的预测值，是如何预测之后的值 x 与误差σ的，下面我们将它推广到更复杂的情况。

2) 第二部分：如何预测

  1. 已知当前点的状态，如何预测下一时间点的状态。

首先要考虑猪前一天的状态 Xk-1，猪的成长变化，正常情况下每天长两斤（F）；它每跑步一圈减半斤（B），某天可能跑（uk）圈；还要考虑天气等不可控外力（wk）。因此，猪从第 k 天的体重 Xk 由下式预测：

图片.png

总之，想预测他的未来，先要知道它之前的状态，自身成长系数，培养教育，对培养教育的反应，以及不可控的机遇，哈哈，好像人一样。

  1. 后来，称猪的称丢了，现在只有一个尺子，我们通过量猪腰围 Zk。然后乘以系数 Hk，来估计猪的体重，同时还要考虑尺子的测量误差 vk。
图片.png

此时，测量值和预测值已经不是同一属性了。

  1. 再后来，发现只量腰围还不够，还需要量身高，也就是说 Zk 不再是单一变量，而是一组向量，此时 H 就变成了一个矩阵，用于转换测量维度 z 和预测维度 x。

同理我们可能预测更多的 x 状态，比如猪的大小，体脂；另外影响猪长肉的外在因素也不只跑圈，可能还有喂食多少。上述公式就从一维扩展到了 N 维。

公式中的 F,B,H,w,v 是一维到多维矩阵。x,u, z 是一维到多维向量。但本质没变，都是通过测量值和之前预测值的误差，求卡尔曼增益，然后进行下一次预测。

  1. 综上，引出卡尔曼方程如下：
图片.png

xk 是 k 时刻的系统状态，是一个 n 维状态向量。

F 是传递矩阵，它是一个 nxn 的矩阵，描述的是 xk-1 与 xk 间变化的关系。它是变化的内因。

uk 是 k 时刻的控制输入，它是变化的外因，它由 c 维向量组成。

B 是控制矩阵，它是一个 nxc 的矩阵，它是变化的外因。

wk 是过程噪声，它是具有高斯分布的随机事件，nxn 的协方差矩阵。

zk 是 k 时刻的测量值，是一个 m 维状态向量。

Hk 是测量矩阵，它是 mxn 的矩阵。描述的是 Xk 与 Zk 间的关系。

Vk 是测量误差，它是具有高斯分布的随机事件，mxm 的协方差矩阵。

3) 第三部分：复杂的多维数据代入加权平均算法

  1. 至此我们看到了 x 和 x^是如何变化的，下面来考虑σ以及 K
图片.png

Xk^是对 Xk 的预测，对应第一部分中的 x。

Pk^是误差协方差矩阵，对应第一部分中的误差σ。

Qk-1 是过程噪声。

Kk 是卡尔曼增益。

Rk 是测试噪声。

4. 相关程序

在 opencv 中使用 kalman 预测的效果，可通过例程 opencv/samples/cpp/kalman.cpp 调用。

程序基本分为三个部分：初始化，预测，更新。在初始化时，我们先指定传递矩阵，控制矩阵，测量矩阵，噪声矩阵；迭代过程中通过测量值更新；预测新的状态。

Kalman 的具体实现，请参考程序 opencv/modules/video/src/kalman.cpp，下面是其预测和更新部分（对应第三部分的公式）：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
const Mat& KalmanFilter::predict(constMat& control)
{
   CV_INSTRUMENT_REGION()
 
   // update the state: x'(k) = A*x(k)，通过xk-1^预测xk^
   statePre = transitionMatrix*statePost;
 
   if( !control.empty() )
       // x'(k) = x'(k) + B*u(k)，计算外因影响：控制矩阵
       statePre += controlMatrix*control;
 
   // update error covariance matrices: temp1 = A*P(k)
   temp1 = transitionMatrix*errorCovPost;
 
   // P'(k) = temp1*At + Q，更新误差Pk^
   gemm(temp1, transitionMatrix, 1, processNoiseCov, 1, errorCovPre,GEMM_2_T);
 
   // handle the case when there will be measurement before the nextpredict.
   statePre.copyTo(statePost);
   errorCovPre.copyTo(errorCovPost);

    //至此可以看到，预测后得到了xk^，以及误差Pk^
   return statePre;
}
 
const Mat& KalmanFilter::correct(constMat& measurement)
{
   CV_INSTRUMENT_REGION()
 
   // temp2 = H*P'(k)
   temp2 = measurementMatrix * errorCovPre;
 
   // temp3 = temp2*Ht + R
   gemm(temp2, measurementMatrix, 1, measurementNoiseCov, 1, temp3,GEMM_2_T);
 
   // temp4 = inv(temp3)*temp2 = Kt(k)
   solve(temp3, temp2, temp4, DECOMP_SVD);
 
   // K(k)，卡尔曼增益K
   gain = temp4.t(); 

   // temp5 = z(k) - H*x'(k)，见第一部分中的x2-x1^
   temp5 = measurement - measurementMatrix*statePre;
 
   // x(k) = x'(k) + K(k)*temp5，计算xk-1^
   statePost = statePre + gain*temp5;
 
   // P(k) = P'(k) - K(k)*temp2，计算误差
   errorCovPost = errorCovPre - gain*temp2;
 
   return statePost;
}