利用Kalman滤波器跟踪一个旋转的点
#include "cv.h" #include "highgui.h" #include <math.h> int main(int argc, char** argv) { const float A[] = { 1, 1, 0, 1 }; IplImage* img = cvCreateImage(cvSize(500, 500), 8, 3); CvKalman* kalman = cvCreateKalman(2, 1, 0); // 状态是角度和角度增量 CvMat* state = cvCreateMat(2, 1, CV_32FC1); CvMat* process_noise = cvCreateMat(2, 1, CV_32FC1); // 只有角度被测量 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)); // 选择随机的开始状态 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); // 预测点的方向 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); cvMatMulAdd(kalman->measurement_matrix, state, measurement, measurement); measurement_angle = measurement->data.fl[0]; measurement_pt = calc_point(measurement_angle); #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); // 调整Kalman滤波器的状态 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; } if (code == 27) break; } return 0; }