图像处理之 _ 卡尔曼滤波

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)求得。为简化公式引入更新率K,即卡尔曼增益 (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;
}