classCV_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. voidinit(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; };
staticinline Point calcPoint(Point2f center, double R, double angle) { return center + Point2f((float)cos(angle), (float)-sin(angle))*(float)R; }
voidhelp() { printf( " Tracking of rotating point.\n" " Rotation speed is constant.\n" " Both state and measurements vectors are 1D (a point angle),\n" " Measurement is the real point angle + gaussian noise.\n" " The real and the estimated points are connected with yellow line segment,\n" " the real and the measured points are connected with red line segment.\n" " (if Kalman filter works correctly,\n" " the yellow segment should be shorter than the red one).\n" "\n" " Pressing any key (except ESC) will reset the tracking with a different speed.\n" " Pressing ESC will stop the program.\n" ); }
intmain(int, char**) { help(); Mat img(500, 500, CV_8UC3); KalmanFilter KF(2, 1, 0); Mat state(2, 1, CV_32F); /* (phi, delta_phi) */ Mat processNoise(2, 1, CV_32F); Mat measurement = Mat::zeros(1, 1, CV_32F); char code = (char)-1;
KalmanFilter KF(stateNum, measureNum, 0); //KalmanFilter::KalmanFilter(intdynamParams, intmeasureParams, int controlParams=0, inttype=CV_32F) Parameters: * dynamParams – Dimensionality of the state. * measureParams – Dimensionality of the measurement. * controlParams – Dimensionality of the control vector. * type – Type of the created matrices that should beCV_32F orCV_64F.
Mat state(stateNum, 1, CV_32FC1); // state(x,y,detaX,detaY) Mat processNoise(stateNum, 1, CV_32F); // processNoise(x,y,detaX,detaY) Mat measurement = Mat::zeros(measureNum, 1, CV_32F); //measurement(x,y)
//!< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k)) //initialize post state of kalman filter at random randn(KF.statePost, Scalar::all(0), Scalar::all(0.1)); Mat showImg(winWidth, winHeight,CV_8UC3);
