《學習opencv》kalman.c詳細註釋

// Example 10-2. Kalman filter sample code
//
//  Use Kalman Filter to model particle in circular trajectory.

//
#include "cv.h"
#include "highgui.h"
#include "cvx_defs.h"

#define phi2xy(mat)                                                  \
  cvPoint( cvRound(img->width/2 + img->width/3*cos(mat->data.fl[0])),\
    cvRound( img->height/2 - img->width/3*sin(mat->data.fl[0])) )

int main(int argc, char** argv) {

    // Initialize, create Kalman Filter object, window, random number
    // generator etc.
    //
    cvNamedWindow( "Kalman", 1 );
    CvRandState rng;
    //typedef struct CvRandState
    //{
    //CvRNG     state;    /* RNG state (the current seed and carry)*/
    //int       disttype; /* distribution type */
    //CvScalar  param[2]; /* parameters of RNG */
    //} CvRandState;
    cvRandInit( &rng, 0, 1, -1, CV_RAND_UNI );
    //cvRandInit(CvRandState資料結構,隨機上界,隨機下界,均勻分佈但參數,指定分佈類型（CV_RAND_UNI）)
    //cvRandInit(CvRandState資料結構,平均數,標準差,正態分佈參數,正態分佈類型（CV_RAND_NORMAL）)

    IplImage* img = cvCreateImage( cvSize(500,500), 8, 3 );
    CvKalman* kalman = cvCreateKalman( 2, 1, 0 );
    // state is (phi, delta_phi) - angle and angular velocity
    // Initialize with random guess.
    //
    CvMat* x_k = cvCreateMat( 2, 1, CV_32FC1 );
    cvRandSetRange( &rng, 0, 0.1, 0 );
    //cvRandSetRange(CvRandState數據結構,均勻分佈上界,均勻分佈下界,目標信道數據)
    //cvRandSetRange(CvRandState數據結構,常態分佈平均數,常態分佈標準偏差,目標信道數據)
    rng.disttype = CV_RAND_NORMAL;//設爲正態分佈
    cvRand( &rng, x_k );//將x_k以正態分佈隨機化

    //狀態x_k,過程噪聲w_k,測量值z_k
    //x_k=F*x_k-1+B*u_k+w_k
    //z_k=H*x_k+v_k

    // process noise
    //
    CvMat* w_k = cvCreateMat( 2, 1, CV_32FC1 );

    // measurements, only one parameter for angle
    //
    CvMat* z_k = cvCreateMat( 1, 1, CV_32FC1 );
    cvZero( z_k );

    // Transition matrix 'F' describes relationship between
    // model parameters at step k and at step k+1 (this is
    // the "dynamics" in our model.
    //
    const float F[] = { 1, 1, 0, 1 };
    memcpy( kalman->transition_matrix->data.fl, F, sizeof(F));//memcpy (void*, const void*, size_t)
    // Initialize other Kalman filter parameters.
    //
    cvSetIdentity( kalman->measurement_matrix,    cvRealScalar(1) ); /* measurement matrix (H) */
    cvSetIdentity( kalman->process_noise_cov,     cvRealScalar(1e-5) );/* process noise covariance matrix (Q) */
    cvSetIdentity( kalman->measurement_noise_cov, cvRealScalar(1e-1) );/* measurement noise covariance matrix (R) */
    cvSetIdentity( kalman->error_cov_post,        cvRealScalar(1));/* priori error estimate covariance matrix (P'(k)):
                                                                      P'(k)=A*P(k-1)*At + Q)*/

    // choose random initial state
    //
    cvRand( &rng, kalman->state_post );

    while( 1 ) {
        // predict point position下一個時間點的預測值
        const CvMat* y_k = cvKalmanPredict( kalman, 0 );

        // generate measurement (z_k)
        //
        cvRandSetRange(
            &rng,
            0,
            sqrt(kalman->measurement_noise_cov->data.fl[0]),
            0
        );
        cvRand( &rng, z_k ); //實際上爲v_k
        cvMatMulAdd( kalman->measurement_matrix, x_k, z_k, z_k );
        //cvMatMulAdd(src1,src2,src3,dst) dist=src1*src2+src3;
        //z_k=H*x_k+v_k
        // plot points (eg convert to planar co-ordinates and draw)
        //
        cvZero( img );
        cvCircle( img, phi2xy(z_k), 4, CVX_YELLOW );   // observed state
        cvCircle( img, phi2xy(y_k), 4, CVX_WHITE, 2 ); // "predicted" state
        cvCircle( img, phi2xy(x_k), 4, CVX_RED );      // real state
        cvShowImage( "Kalman", img );
        // adjust Kalman filter state
        //
        cvKalmanCorrect( kalman, z_k );

        // Apply the transition matrix 'F' (eg, step time forward)
        // and also apply the "process" noise w_k.
        //x_k=F*x_k-1+B*u_k+w_k
        //z_k=H*x_k+v_k
        //
        cvRandSetRange(
            &rng,
            0,
            sqrt(kalman->process_noise_cov->data.fl[0]),  //????
            0
            );
        cvRand( &rng, w_k );
        cvMatMulAdd( kalman->transition_matrix, x_k, w_k, x_k );
        //不計u_k
        // exit if user hits 'Esc'
        if( cvWaitKey( 100 ) == 27 ) break;
    }

    return 0;
}