OpenCV源码分析（三）：KalmanFilter

/*!
 Kalman filter.

 The class implements standard Kalman filter \url{http://en.wikipedia.org/wiki/Kalman_filter}.
 However, you can modify KalmanFilter::transitionMatrix, KalmanFilter::controlMatrix and
 KalmanFilter::measurementMatrix to get the extended Kalman filter functionality.
*/
class CV_EXPORTS_W KalmanFilter
{
public:
    //! the default constructor
    CV_WRAP KalmanFilter();
    //! the full constructor taking the dimensionality of the state, of the measurement and of the control vector
    CV_WRAP KalmanFilter(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F);
    //! re-initializes Kalman filter. The previous content is destroyed.
    void init(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F);

    //! computes predicted state
    CV_WRAP const Mat& predict(const Mat& control=Mat());
    //! updates the predicted state from the measurement
    CV_WRAP const Mat& correct(const Mat& measurement);

    Mat statePre;           //!< predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k)
    Mat statePost;          //!< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))
    Mat transitionMatrix;   //!< state transition matrix (A)
    Mat controlMatrix;      //!< control matrix (B) (not used if there is no control)
    Mat measurementMatrix;  //!< measurement matrix (H)
    Mat processNoiseCov;    //!< process noise covariance matrix (Q)
    Mat measurementNoiseCov;//!< measurement noise covariance matrix (R)
    Mat errorCovPre;        //!< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/
    Mat gain;               //!< Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)
    Mat errorCovPost;       //!< posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)

    // temporary matrices
    Mat temp1;
    Mat temp2;
    Mat temp3;
    Mat temp4;
    Mat temp5;
};

namespace cv
{

KalmanFilter::KalmanFilter() {}
KalmanFilter::KalmanFilter(int dynamParams, int measureParams, int controlParams, int type)
{
	/**完整构造函数 
    	dynamParams：状态向量的维数 
    	measureParams：观测矩阵的维数 
    	controlParams：控制器的维数 
    	type：矩阵数据类型：默认CV_32F 
	*/ 
    init(dynamParams, measureParams, controlParams, type);
}

// 初始化滤波器
void KalmanFilter::init(int DP, int MP, int CP, int type)
{
    CV_Assert( DP > 0 && MP > 0 );
    CV_Assert( type == CV_32F || type == CV_64F );
    CP = std::max(CP, 0);

    statePre = Mat::zeros(DP, 1, type);		// 状态的一部预测值，(先验估计值), DPx1维
    statePost = Mat::zeros(DP, 1, type);	// 更新后的状态估计值,(后验估计值)
    transitionMatrix = Mat::eye(DP, DP, type);	// 状态转移矩阵(A), DPxDP

    processNoiseCov = Mat::eye(DP, DP, type);	// 过程噪声的协方差矩阵(Q)
    measurementMatrix = Mat::zeros(MP, DP, type);	// 观测矩阵(H)
    measurementNoiseCov = Mat::eye(MP, MP, type);	// 观测噪声的协方差矩阵(R)

    errorCovPre = Mat::zeros(DP, DP, type);	// 先验估计值的误差的协方差矩阵 P'(k)
    errorCovPost = Mat::zeros(DP, DP, type);	// 后验估计值的误差的协方差矩阵 P(k)
    gain = Mat::zeros(DP, MP, type);		// Kalman增益矩阵(K(k))

    if( CP > 0 )
        controlMatrix = Mat::zeros(DP, CP, type); // 控制矩阵 (B)
    else
        controlMatrix.release();

    temp1.create(DP, DP, type);
    temp2.create(MP, DP, type);
    temp3.create(MP, MP, type);
    temp4.create(MP, DP, type);
    temp5.create(MP, 1, type);
}

// 计算状态预测值， 给出状态的先验估计
const Mat& KalmanFilter::predict(const Mat& control)
{
    // update the state: x'(k) = A*x(k)
    statePre = transitionMatrix*statePost;

    if( control.data )
        // 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
    // 第二步: 加上当前时刻的过程噪声 P'(k) = temp1*At + Q
    gemm(temp1, transitionMatrix, 1, processNoiseCov, 1, errorCovPre, GEMM_2_T);

    // handle the case when there will be measurement before the next predict.
    statePre.copyTo(statePost);
    errorCovPre.copyTo(errorCovPost);

    return statePre;
}

// 依据观测数据和观测误差更新状态的一步预测值，给出状态的后验估计值
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();

    // temp5 = z(k) - H*x'(k)
    temp5 = measurement - measurementMatrix*statePre;

    // x(k) = x'(k) + K(k)*temp5
    statePost = statePre + gain*temp5;

    // P(k) = P'(k) - K(k)*temp2
    errorCovPost = errorCovPre - gain*temp2;

    return statePost;
}

}