Blog文章的打印版本

Estimators

CvKalman
Kalman filter state

typedef struct CvKalman{ int MP; /* number of measurement vector dimensions 测量向量维数*/ int DP; /* number of state vector dimensions 状态向量维数*/ int CP; /* number of control vector dimensions 控制向量维数*/ /* backward compatibility fields */#if 1 float* PosterState; /* =state_pre->data.fl 前驱*/ float* PriorState; /* =state_post->data.fl 后继*/ float* DynamMatr; /* =transition_matrix->data.fl 转移矩阵*/ float* MeasurementMatr; /* =measurement_matrix->data.fl 测量矩阵*/ float* MNCovariance; /* =measurement_noise_cov->data.fl 测量噪声协方差*/ float* PNCovariance; /* =process_noise_cov->data.fl 系统噪声协方差*/ float* KalmGainMatr; /* =gain->data.fl 卡尔曼增益*/ float* PriorErrorCovariance;/* =error_cov_pre->data.fl */ float* PosterErrorCovariance;/* =error_cov_post->data.fl */ float* Temp1; /* temp1->data.fl */ float* Temp2; /* temp2->data.fl */#endif CvMat* state_pre; /* predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k) 先验状态*/ CvMat* state_post; /* corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k)) 后验状态*/ CvMat* transition_matrix; /* state transition matrix (A) */ CvMat* control_matrix; /* control matrix ( (it is not used if there is no control)*/ CvMat* measurement_matrix; /* measurement matrix (H) */ CvMat* process_noise_cov; /* process noise covariance matrix (Q) */ CvMat* measurement_noise_cov; /* measurement noise covariance matrix ® */ CvMat* error_cov_pre; /* priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/ CvMat* gain; /* Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)*/ CvMat* error_cov_post; /* posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k) */ CvMat* temp1; /* temporary matrices */ CvMat* temp2; CvMat* temp3; CvMat* temp4; CvMat* temp5;}CvKalman;The structure http://www.itl.k.u-tokyo.ac.jp/~nakamura/opencv/opencvref_motionanalysis.html#decl_CvKalman is used to keep Kalman filter state. It is created by http://www.itl.k.u-tokyo.ac.jp/~nakamura/opencv/opencvref_motionanalysis.html#decl_cvCreateKalman function, updated by http://www.itl.k.u-tokyo.ac.jp/~nakamura/opencv/opencvref_motionanalysis.html#decl_cvKalmanPredict and http://www.itl.k.u-tokyo.ac.jp/~nakamura/opencv/opencvref_motionanalysis.html#decl_cvKalmanCorrect functions and released by http://www.itl.k.u-tokyo.ac.jp/~nakamura/opencv/opencvref_motionanalysis.html#decl_cvReleaseKalman functions. Normally, the structure is used for standard Kalman filter (notation and formulae are borrowed from excellent Kalman tutorial http://www.itl.k.u-tokyo.ac.jp/~nakamura/opencv/opencvref_motionanalysis.html#kalman_paper):

xk=A•xk-1+B•uk+wkzk=H•xk+vk,where:

xk (xk-1) - state of the system at the moment k (k-1)zk - measurement of the system state at the moment kuk - external control applied at the moment kwk and vk are normally-distributed process and measurement noise, respectively:p(w) ~ N(0,Q)p(v) ~ N(0,R),that is,Q - process noise covariance matrix, constant or variable,R - measurement noise covariance matrix, constant or variableIn case of standard Kalman filter, all the matrices: A, B, H, Q and R are initialized once after http://www.itl.k.u-tokyo.ac.jp/~nakamura/opencv/opencvref_motionanalysis.html#decl_CvKalman structure is allocated via http://www.itl.k.u-tokyo.ac.jp/~nakamura/opencv/opencvref_motionanalysis.html#decl_cvCreateKalman. However, the same structure and the same functions may be used to simulate extended Kalman filter by linearizing extended Kalman filter equation in the current system state neighborhood, in this case A, B, H (and, probably, Q and R) should be updated on every step.

[Welch95] Greg Welch, Gary Bishop. An Introduction To the Kalman Filter. Technical Report TR95-041, University of North Carolina at Chapel Hill, 1995. Online version is available at [url="http://www.cs.unc.edu/~welch/kalman/kalman_filter/kalman.html"]http://www.cs.unc.edu/~welch/kalman/kalman_filter/kalman.html


CreateKalman
Allocates Kalman filter structure

CvKalman* cvCreateKalman( int dynamParams, int measureParams, int controParams=0 );

dynamParams dimensionality of the state vector measureParams dimensionality of the measurement vector controlParams dimensionality of the control vector

The function http://www.itl.k.u-tokyo.ac.jp/~nakamura/opencv/opencvref_motionanalysis.html#decl_cvCreateKalman allocates http://www.itl.k.u-tokyo.ac.jp/~nakamura/opencv/opencvref_motionanalysis.html#decl_CvKalman and all its matrices and initializes them somehow.


ReleaseKalman
Deallocates Kalman filter structure

void cvReleaseKalman(CvKalman** kalman );

kalman double pointer to the Kalman filter structure.

The function http://www.itl.k.u-tokyo.ac.jp/~nakamura/opencv/opencvref_motionanalysis.html#decl_cvReleaseKalman releases the structure http://www.itl.k.u-tokyo.ac.jp/~nakamura/opencv/opencvref_motionanalysis.html#decl_CvKalman and all underlying matrices.


KalmanPredict
Estimates subsequent model state

const CvMat* cvKalmanPredict( CvKalman* kalman, const CvMat* control=NULL );#define cvKalmanUpdateByTime cvKalmanPredict

kalman Kalman filter state. control Control vector (uk), should be NULL iff there is no external control (controlParams=0).

The function http://www.itl.k.u-tokyo.ac.jp/~nakamura/opencv/opencvref_motionanalysis.html#decl_cvKalmanPredict estimates the subsequent stochastic model state by its current state and stores it at kalman->state_pre:

x'k=A•xk+B•uk P'k=A•Pk-1*AT + Q,wherex'k is predicted state (kalman->state_pre),xk-1 is corrected state on the previous step (kalman->state_post) (should be initialized somehow in the beginning, zero vector by default),uk is external control (control parameter),P'k is priori error covariance matrix (kalman->error_cov_pre)Pk-1 is posteriori error covariance matrix on the previous step (kalman->error_cov_post) (should be initialized somehow in the beginning, identity matrix by default),The function returns the estimated state.


KalmanCorrect
Adjusts model state

void cvKalmanCorrect( CvKalman* kalman, const CvMat* measurement=NULL );#define cvKalmanUpdateByMeasurement cvKalmanCorrect

kalman Pointer to the structure to be updated. measurement Pointer to the structure CvMat containing the measurement vector.

The function http://www.itl.k.u-tokyo.ac.jp/~nakamura/opencv/opencvref_motionanalysis.html#decl_cvKalmanCorrect adjusts stochastic model state on the basis of the given measurement of the model state:

Kk=P'k•HT•(H•P'k•HT+R)-1xk=x'k+Kk•(zk-H•x'k)Pk=(I-Kk•H)•P'kwherezk - given measurement (mesurement parameter)Kk - Kalman "gain" matrix.The function stores adjusted state at kalman->state_post and returns it on output.


Example. Using Kalman filter to track a rotating point
#include "cv.h"#include "highgui.h"#include <math.h>int main(int argc, char** argv){ /* A matrix data */ const float A[] = { 1, 1, 0, 1 }; IplImage* img = cvCreateImage( cvSize(500,500), 8, 3 ); CvKalman* kalman = cvCreateKalman( 2, 1, 0 ); /* state is (phi, delta_phi) - angle and angle increment */ CvMat* state = cvCreateMat( 2, 1, CV_32FC1 ); CvMat* process_noise = cvCreateMat( 2, 1, CV_32FC1 ); /* only phi (angle) is measured */ CvMat* measurement = cvCreateMat( 1, 1, CV_32FC1 ); CvRandState rng; int code = -1; cvRandInit( &rng, 0, 1, -1, CV_RAND_UNI ); cvZero( measurement ); cvNamedWindow( "Kalman", 1 ); for(;;) { cvRandSetRange( &rng, 0, 0.1, 0 ); rng.disttype = CV_RAND_NORMAL; cvRand( &rng, state ); memcpy( kalman->transition_matrix->data.fl, A, sizeof(A)); cvSetIdentity( kalman->measurement_matrix, cvRealScalar(1) ); cvSetIdentity( kalman->process_noise_cov, cvRealScalar(1e-5) ); cvSetIdentity( kalman->measurement_noise_cov, cvRealScalar(1e-1) ); cvSetIdentity( kalman->error_cov_post, cvRealScalar(1)); /* choose random initial state */ cvRand( &rng, kalman->state_post ); rng.disttype = CV_RAND_NORMAL; for(;;) { #define calc_point(angle) \ cvPoint( cvRound(img->width/2 + img->width/3*cos(angle)), \ cvRound(img->height/2 - img->width/3*sin(angle))) float state_angle = state->data.fl[0]; CvPoint state_pt = calc_point(state_angle); /* predict point position */ const CvMat* prediction = cvKalmanPredict( kalman, 0 ); float predict_angle = prediction->data.fl[0]; CvPoint predict_pt = calc_point(predict_angle); float measurement_angle; CvPoint measurement_pt; cvRandSetRange( &rng, 0, sqrt(kalman->measurement_noise_cov->data.fl[0]), 0 ); cvRand( &rng, measurement ); /* generate measurement */ cvMatMulAdd( kalman->measurement_matrix, state, measurement, measurement ); measurement_angle = measurement->data.fl[0]; measurement_pt = calc_point(measurement_angle); /* plot points */ #define draw_cross( center, color, d ) \ cvLine( img, cvPoint( center.x - d, center.y - d ), \ cvPoint( center.x + d, center.y + d ), color, 1, 0 ); \ cvLine( img, cvPoint( center.x + d, center.y - d ), \ cvPoint( center.x - d, center.y + d ), color, 1, 0 ) cvZero( img ); draw_cross( state_pt, CV_RGB(255,255,255), 3 ); draw_cross( measurement_pt, CV_RGB(255,0,0), 3 ); draw_cross( predict_pt, CV_RGB(0,255,0), 3 ); cvLine( img, state_pt, predict_pt, CV_RGB(255,255,0), 3, 0 ); /* adjust Kalman filter state */ cvKalmanCorrect( kalman, measurement ); cvRandSetRange( &rng, 0, sqrt(kalman->process_noise_cov->data.fl[0]), 0 ); cvRand( &rng, process_noise ); cvMatMulAdd( kalman->transition_matrix, state, process_noise, state ); cvShowImage( "Kalman", img ); code = cvWaitKey( 100 ); if( code > 0 ) /* break current simulation by pressing a key */ break; } if( code == 27 ) /* exit by ESCAPE */ break; } return 0;}
CvConDensation
ConDenstation state

typedef struct CvConDensation { int MP; //Dimension of measurement vector int DP; // Dimension of state vector float* DynamMatr; // Matrix of the linear Dynamics system float* State; // Vector of State int SamplesNum; // Number of the Samples float** flSamples; // array of the Sample Vectors float** flNewSamples; // temporary array of the Sample Vectors float* flConfidence; // Confidence for each Sample float* flCumulative; // Cumulative confidence float* Temp; // Temporary vector float* RandomSample; // RandomVector to update sample set CvRandState* RandS; // Array of structures to generate random vectors } CvConDensation;The structure http://www.itl.k.u-tokyo.ac.jp/~nakamura/opencv/opencvref_motionanalysis.html#decl_CvConDensation stores CONditional DENSity propagATION tracker state. The information about the algorithm can be found at http://www.dai.ed.ac.uk/CVonline/LOCAL_COPIES/ISARD1/condensation.html


CreateConDensation
Allocates ConDensation filter structure

CvConDensation* cvCreateConDensation( int DynamParams, int MeasureParams, int SamplesNum );

DynamParams Dimension of the state vector. MeasureParams Dimension of the measurement vector. SamplesNum Number of samples.

The function http://www.itl.k.u-tokyo.ac.jp/~nakamura/opencv/opencvref_motionanalysis.html#decl_cvCreateConDensation creates http://www.itl.k.u-tokyo.ac.jp/~nakamura/opencv/opencvref_motionanalysis.html#decl_CvConDensation structure and returns pointer to the structure.


ReleaseConDensation
Deallocates ConDensation filter structure

void cvReleaseConDensation( CvConDensation** ConDens );

ConDens Pointer to the pointer to the structure to be released.

The function http://www.itl.k.u-tokyo.ac.jp/~nakamura/opencv/opencvref_motionanalysis.html#decl_cvReleaseConDensation releases the structure http://www.itl.k.u-tokyo.ac.jp/~nakamura/opencv/opencvref_motionanalysis.html#decl_CvConDensation (see http://www.itl.k.u-tokyo.ac.jp/~nakamura/opencv/opencvref_motionanalysis.html#decl_CvConDensation) and frees all memory previously allocated for the structure.


ConDensInitSampleSet
Initializes sample set for condensation algorithm

void cvConDensInitSampleSet( CvConDensation* ConDens, CvMat* lowerBound, CvMat* upperBound );

ConDens Pointer to a structure to be initialized. lowerBound Vector of the lower boundary for each dimension. upperBound Vector of the upper boundary for each dimension.

The function http://www.itl.k.u-tokyo.ac.jp/~nakamura/opencv/opencvref_motionanalysis.html#decl_cvConDensInitSampleSet fills the samples arrays in the structure http://www.itl.k.u-tokyo.ac.jp/~nakamura/opencv/opencvref_motionanalysis.html#decl_CvConDensation with values within specified ranges.


ConDensUpdateByTime
Estimates subsequent model state

void cvConDensUpdateByTime( CvConDensation* ConDens );

ConDens Pointer to the structure to be updated.

The function http://www.itl.k.u-tokyo.ac.jp/~nakamura/opencv/opencvref_motionanalysis.html#decl_cvConDensUpdateByTime estimates the subsequent stochastic model state from its current state.