图像处理之_卡尔曼滤波
图像处理之 _ 卡尔曼滤波
1. 用途
根据一些已知的量来预测未知的量。常用于运动预测。
2. 定义
卡尔曼滤波(Kalman filtering)一种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法。
由于观测数据中包括系统中的噪声和干扰的影响,最优估计也可看作是滤波过程,因此叫作卡尔曼滤波。
3. 原理分析
1) 第一部分:预测值与误差
- 假设你和我两个人每天给同一头猪测体重,但两个称都不准。你测 x1 斤,正负误差在σ1 斤左右(假设预测成高斯分布,标准差反映了测量值好坏程度的不确定性),我测 x2 斤,正负误差在σ2 斤左右。结合两个人的预测,简单计算猪的体重是 x1 和 x2 的平均值。
- 假如我的称更准,即误差更小,σ2<σ1,结合两个人的测量,应该以我为主,因此根据误差加权组合。
(通过概率函数导数的最大值推出上述公式,不在此详述)
由公式可知:如果你的不确定性σ1 越大,我的测量值 x2 在新均值中占的比例越大(权值更大)。
- 后来你走了,用上一次的加权平均(x1,σ1)替代你的测量,我的测量值仍是(x2, σ2)(此后我们把加权平均的结果称为预测值)。同样采取加权平均的算法,算出本次预测值(x2,σ2),这是一个迭代的过程。通过化简得到以下公式:
请注意,现在公式中的 1,2 不再代表你和我,而代表时间点,带的为预测值,不带的为测量值。也就是说新的预测值(x2^, σ2)由前一个预测值(x1, σ1^)和本次测量值(x2, σ2)求得。为简化公式引入更新率K,即卡尔曼增益 (Kalman Gain)。将公式简化为以下形式:
从简化之后的公式可知,K 是由误差算出的,K 用于预测新的 x 和误差σ。由公式可知:前次预测的不确定性σ1^越大,K 越大,我的预测 x2 越重要。
- 综上,我们已经看到了最简单(单值且预测与测量的值相同)情况下的测量值与前次的预测值,是如何预测之后的值 x 与误差σ的,下面我们将它推广到更复杂的情况。
2) 第二部分:如何预测
- 已知当前点的状态,如何预测下一时间点的状态。
首先要考虑猪前一天的状态 Xk-1,猪的成长变化,正常情况下每天长两斤(F);它每跑步一圈减半斤(B),某天可能跑(uk)圈;还要考虑天气等不可控外力(wk)。因此,猪从第 k 天的体重 Xk 由下式预测:
总之,想预测他的未来,先要知道它之前的状态,自身成长系数,培养教育,对培养教育的反应,以及不可控的机遇,哈哈,好像人一样。
- 后来,称猪的称丢了,现在只有一个尺子,我们通过量猪腰围 Zk。然后乘以系数 Hk,来估计猪的体重,同时还要考虑尺子的测量误差 vk。
此时,测量值和预测值已经不是同一属性了。
- 再后来,发现只量腰围还不够,还需要量身高,也就是说 Zk 不再是单一变量,而是一组向量,此时 H 就变成了一个矩阵,用于转换测量维度 z 和预测维度 x。
同理我们可能预测更多的 x 状态,比如猪的大小,体脂;另外影响猪长肉的外在因素也不只跑圈,可能还有喂食多少。上述公式就从一维扩展到了 N 维。
公式中的 F,B,H,w,v 是一维到多维矩阵。x,u, z 是一维到多维向量。但本质没变,都是通过测量值和之前预测值的误差,求卡尔曼增益,然后进行下一次预测。
- 综上,引出卡尔曼方程如下:
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) 第三部分:复杂的多维数据代入加权平均算法
- 至此我们看到了 x 和 x^是如何变化的,下面来考虑σ以及 K
Xk^是对 Xk 的预测,对应第一部分中的 x。
Pk^是误差协方差矩阵,对应第一部分中的误差σ。
Qk-1 是过程噪声。
Kk 是卡尔曼增益。
Rk 是测试噪声。
4. 相关程序
在 opencv 中使用 kalman 预测的效果,可通过例程 opencv/samples/cpp/kalman.cpp 调用。
程序基本分为三个部分:初始化,预测,更新。在初始化时,我们先指定传递矩阵,控制矩阵,测量矩阵,噪声矩阵;迭代过程中通过测量值更新;预测新的状态。
Kalman 的具体实现,请参考程序 opencv/modules/video/src/kalman.cpp,下面是其预测和更新部分(对应第三部分的公式):
1 | const Mat& KalmanFilter::predict(constMat& control) |