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 CvKalman is used to keep Kalman filter state. It is created by cvCreateKalman function, updated by cvKalmanPredict and cvKalmanCorrect functions and released by cvReleaseKalman functions. Normally, the structure is used for standard Kalman filter (notation and formulae are borrowed from excellent Kalman tutorial [Welch95]):

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 CvKalman structure is allocated via 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 cvCreateKalman allocates 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 cvReleaseKalman releases the structure 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 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 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 CvConDensation stores CONditional DENSity propagATION tracker state. The information about the algorithm can be found at http://www.dai.ed.ac.uk/CVonline/LOCAL_COP...ndensation.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 cvCreateConDensation creates 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 cvReleaseConDensation releases the structure CvConDensation (see 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 cvConDensInitSampleSet fills the samples arrays in the structure 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 cvConDensUpdateByTime estimates the subsequent stochastic model state from its current state.