卡尔曼滤波的opencv源码及编程步骤分析

一、卡尔曼滤波的五个方程

二、opencv中卡尔曼滤波--KalmanFilter类的源码分析

class CV_EXPORTS_W KalmanFilter  {  public:          CV_WRAP KalmanFilter();                                                                           //构造默认KalmanFilter对象      CV_WRAP KalmanFilter(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F);  //完整构造KalmanFilter对象方法      void init(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F);              //初始化KalmanFilter对象，会替换原来的KF对象  //dynamParams过程状态向量的维度//measureParams观测向量的维度//controlParams控制向量的维度    CV_WRAP const Mat& predict(const Mat& control=Mat());           //计算预测的状态值          CV_WRAP const Mat& correct(const Mat& measurement);             //根据测量值更新状态值        Mat statePre;            //先验估计值 (x'(k)): x'(k)=A*x(k-1)+B*u(k-1) 方程1     Mat statePost;           //后验更新值 (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))  方程4    Mat transitionMatrix;    //状态转移矩阵 A      Mat controlMatrix;       //控制矩阵 B       Mat measurementMatrix;   //测量矩阵 H      Mat processNoiseCov;     //系统噪声协方差矩阵  Q    Mat measurementNoiseCov; //测量噪声协方差矩阵  R    Mat errorCovPre;         //先验协方差矩阵 (P'(k)): P'(k)=A*P(k-1)*At + Q  方程2    Mat gain;                //卡尔曼增益   (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)  方程3    Mat errorCovPost;        //后验协方差矩阵 (P(k)): P(k)=(I-K(k)*H)*P'(k)方程5      // 临时矩阵      Mat temp1;      Mat temp2;      Mat temp3;      Mat temp4;      Mat temp5;  };  

1、先看predict()函数的功能

//predict()函数的功能：//输入形参--控制向量u(k)，求出了 先验估计状态向量x'(k) 和 先验估计协方差矩阵p'(k)，即求解了方程1、2const Mat& KalmanFilter::predict(const Mat& control)//此处的control是控制向量u(k){    // update the state: x'(k) = A*x(k)    statePre = transitionMatrix*statePost;//当前的后验更新值 作为下一次的 先验估计值    if( !control.empty() )        // x'(k) = x'(k) + B*u(k)        statePre += controlMatrix*control;//到此，完成了方程1    // update error covariance matrices: temp1 = A*P(k)    temp1 = transitionMatrix*errorCovPost;    // P'(k) = temp1*At + Q    gemm(temp1, transitionMatrix, 1, processNoiseCov, 1, errorCovPre, GEMM_2_T);//至此，完成了方程2    // handle the case when there will be measurement before the next predict.    statePre.copyTo(statePost);    errorCovPre.copyTo(errorCovPost);    return statePre;}

其中用到了一个函数gemm()，先将gemm函数原型贴于下面

//gemm()函数的功能void cv::gemm( InputArray matA, InputArray matB, double alpha,           InputArray matC, double beta, OutputArray _matD, int flags )//_matD = matA * InputArray matB * alpha + matC * beta;//GEMM_2_T 表示对第2个参数矩阵求偏置

2、再看correct()函数原型

//correct()函数的功能：//输入形参--实际输出z(k)，求出了卡尔曼增益Kk、后验更新状态向量x(k) 和 后验更新协方差矩阵p(k)，即求解了方程3、4、5const Mat& KalmanFilter::correct(const Mat& measurement){    // 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)    gain = temp4.t();//至此，完成了方程3，求卡尔曼增益    // temp5 = z(k) - H*x'(k)    temp5 = measurement - measurementMatrix*statePre;    // x(k) = x'(k) + K(k)*temp5    statePost = statePre + gain*temp5;//至此，完成了方程4    // P(k) = P'(k) - K(k)*temp2    errorCovPost = errorCovPre - gain*temp2;//至此，完成了方程5    return statePost;}

其中用到了solve函数，将其原型贴于下面

//solve()函数原型bool cv::solve( InputArray _src, InputArray _src2arg, OutputArray _dst, int method )

3、KalmanFilter类的结构如下

//1.四个成员函数：CV_WRAP KalmanFilter(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F); void init(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F); CV_WRAP const Mat& predict(const Mat& control=Mat());CV_WRAP const Mat& correct(const Mat& measurement); //2.这五个成员变量是调用 上面两个成员函数后，自动求出来的Mat statePre;Mat statePost;Mat gain; Mat errorCovPre;      Mat errorCovPost; //需要用户自定义的成员变量Mat transitionMatrix;    //状态转移矩阵 A  Mat controlMatrix;       //控制矩阵 B   Mat measurementMatrix;   //测量矩阵 H  Mat processNoiseCov;     //系统噪声协方差矩阵  QMat measurementNoiseCov; //测量噪声协方差矩阵  R

理解了上述以后，现对卡尔曼滤波的opencv编程步骤做如下总结：
1、定义卡尔曼对象
KalmanFilter KF(int dP, int mP, int cP);//定义状态向量、观测向量、控制向量的维度

2、初始化五个参数 + 三个初始状态
五个参数：
KF.transitionMatrix; //状态转移矩阵 A  //方阵，与状态向量维度相同
KF.controlMatrix; //控制矩阵 B  //若无控制量，B就是0
KF.measurementMatrix; //测量矩阵 H  //行：观测向量的维度；列：状态向量的维度
KF.processNoiseCov; //系统噪声协方差矩阵 Q//方阵，与状态向量维度相同，若为对角阵，表示状态向量各维度之间互不相关
KF.measurementNoiseCov; //测量噪声协方差矩阵 R//方阵，与观测向量维度相同，若为对角阵，表示观测向量各维度之间互不相关
三个初始状态：
KF.statePost; //前一时刻的状态向量x方便迭代
KF.errorCovPost; //前一时刻的后验协方差矩阵P方便迭代
//这两个两可以随便给，因为随着卡尔曼滤波的进行，x最终会收敛，但是P初始值不要给0，否则卡尔曼会认为初始化的x(0)就是最优状态
Mat measurement; //当前的观测值y，这个量要自己定义

3、预测
KF.predict; //求出了x'(k) 和 p'(k)，该函数输入的是 控制向量u(k)，输出是x'(k) 

4、更新
measurement; //更新观测值
KF.correct(measurement); //求出了Kk、x(k) 和p(k)，该函数输入的是 观测向量y