《学习opencv》kalman.c详细注释

时间:2022-02-13 01:06:55
 
 

// 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;
}