OpenCV source code analysis (3): 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)
{
	/** full constructor
    	dynamParams: the dimension of the state vector
    	measureParams: the dimension of the observation matrix
    	controlParams: the dimension of the controller
    	type: matrix data type: default CV_32F
	*/
    init(dynamParams, measureParams, controlParams, type);
}

// initialize the filter
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); // A predicted value of the state, (a priori estimate), DPx1 dimension
    statePost = Mat::zeros(DP, 1, type); // updated state estimate, (posterior estimate)
    transitionMatrix = Mat::eye(DP, DP, type); // state transition matrix (A), DPxDP

    processNoiseCov = Mat::eye(DP, DP, type); // covariance matrix of process noise (Q)
    measurementMatrix = Mat::zeros(MP, DP, type); // measurement matrix (H)
    measurementNoiseCov = Mat::eye(MP, MP, type); // Covariance matrix of observation noise (R)

    errorCovPre = Mat::zeros(DP, DP, type); // covariance matrix P'(k) of the error of the prior estimate
    errorCovPost = Mat::zeros(DP, DP, type); // covariance matrix P(k) of the error of the posterior estimate
    gain = Mat::zeros(DP, MP, type); // Kalman gain matrix(K(k))

    if( CP > 0 )
        controlMatrix = Mat::zeros(DP, CP, type); // control matrix (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);
}

// Calculate the state predicted value, giving a priori estimate of the state
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)
    // Step 1: Pass the covariance matrix of the posterior error at the previous moment to the current moment through the state transition matrix
    temp1 = transitionMatrix*errorCovPost;

    // P'(k) = temp1*At + Q
    // Step 2: Add the current process noise 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;
}

// Update the one-step predicted value of the state based on the observation data and the observation error, and give the posterior estimate of the state
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;
}

}