卡尔曼滤波的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、2
	
const 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、5
const 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;     //系统噪声协方差矩阵  Q
Mat 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