本文介绍 Kalmanfilter深入理解

Kalmanfilter深入理解

This article was original written by Jin Tian, welcome re-post, first come with https://jinfagang.github.io . but please keep this copyright info, thanks, any question could be asked via wechat: jintianiloveu

kalman filter 理解

深入理解卡尔曼滤波。其实在理解卡尔曼滤波之前,最好不要去想kalmanfilter,让我们先理解一下运动这个东西。

什么叫运动?一个球在二维平面滚来滚去,这个就叫做运动。一个点在一个一维的直线上前进或者后退,这个也是运动。那么什么是运动方程呢?

我问一个问题,假如说,要你预测一个二维平面的小球,在下一个时间状态的位置,你需要知道哪些变量?有人说,我只要知道它当前时刻的位置以及当前的速度,不就知道了下一个时刻的位置了吗?是的没有错,这个想法是正确的,如果你知道了这些,那么,你也就是理解了卡尔曼滤波。

卡尔曼滤波用来估计运动状态其实就是刚才我们说的预测小球未知的问题。而其中的你需要知道的变量就是里面所讲的状态空间.

我们从最简单的一维运动来考虑率一下,一个小球围绕着一个圆弧运动,那么它是不是一维的呢?很显然,它其实就是掰弯了的直线,本身上是一维的,那么我们要描述它的运动,需要哪些物理量呢?

很显然,我只要知道当前的角度,以及角速度,就可以了,也就是说,我只需要[θ,v], 就可以了。那么运动方程显然就可以这么写:

$\theta(t+1) = \theta(t) + \omega(t)*T$

$\omega(t+1) =\omega(t) (assuming avergae velocity)$

显然,由于匀速的运动,我们的运动方程可以这样表示。注意了,这里面的核心其实就是两个:

  • 你有多少个状态量才能确定状态空间;
  • 你的状态量是怎么跟时间联系的。

后面大家做跟踪的时候就会遇到这个问题,为什么会有一个delta time?其实就是说白了,就是速度和路程的关系,小学学的?

当我们要估计一些更加复杂的运动的时候,比如说,四个坐标的框,此时四个点都有速度,遇到这种问题,第一件事情就是先把运动方程写出来。然后就知道如何构建一个卡尔曼滤波器来进行状态的预测了。

OpenCV 卡尔曼滤波例子

千说不如一站,一个简单的例子足可以让你完全明白这个操作。opencv使用卡尔曼滤波器,首先你需要明确,你的状态空间有哪些变量,然后有哪些变量是可以测量的(在上面的一维例子中,你的角速度其实是无法测量的,你不知道,你只知道当前的角度)。

第一件事情当然就是初始化一个卡尔曼滤波器,这个初始化需要初始化这些量:

  • 状态转移矩阵,这个很简单,其实就是运动方程的矩阵提取出来,你能写出运动方程的矩阵形式就得到了状态转移矩阵,这个矩阵是常量(?);
  • 测量矩阵,就是你能拿到的实际值,如上面的角度你可以拿到但是角速度你不一定拿得到;
  • 控制向量,一般是0;
  • 过程噪声、控制噪声;
  • 当前初始化的状态

代码:

KalmanFilter KF(2, 1, 0);
// 初始化转移矩阵
KF.transitionMatrix = *(Mat_<float>(2, 2) << 1, 1, 0, 1);
// 这个转移矩阵其实就是上面的运动方程的矩阵形式,你会发现它其实就是一个常量

// 状态,有两个,角度和角速度
state.create(2, 1, CV_32F);
// 测量噪声
processNoise.create(2, 1, CV_32F);
// 可测量的量
measurement = Mat::zeros(1, 1, CV_32F);

// ???这个测量矩阵是什么?
setIdentity(KF.measurementMatrix);
//系统过程噪声方差矩阵
setIdentity(KF.processNoiseCov, Scalar::all(1e-5));
//测量过程噪声方差矩阵
setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));
//后验错误估计协方差矩阵
setIdentity(KF.errorCovPost, Scalar::all(1));