#ifndef _KALMANFILTER_ #define _KALMANFILTER_ class CKalman { public: CKalman(int D, int M, int C); ~CKalman(); void KalmanPredict(double * control); void KalmanCorrect(double * measurement); void set_transition_matrix(const double*transition_matrix); public: int MP; //number of measure vector dimensions int DP; //number of state vector dimensions int CP; //number of control vector dimensions double* state_pre; //[DP * 1] double* state_post; //[DP * 1] double* transition_matrix; //[DP * DP] double* control_matrix; //[DP * CP] if (CP > 0) double* measurement_matrix; //[MP * DP] double* process_noise_cov; //[DP * DP] double* measurement_noise_cov;//[MP * MP] double* error_cov_pre; //[DP * DP] double* error_cov_post; //[DP * DP] double* gain; //[DP * MP] }; #endif
#include "stdafx.h" #include "kalmanfilter.h" #include "Matrix.h" #include<memory> #include<assert.h> using namespace std; CKalman::CKalman(int D, int M, int C){ if (D <= 0 || M <= 0){ assert(false);// "state and measurement vectors must have positive number of dimensions! "); } if (C < 0) { assert(false);// "No Control!"); } DP = D; MP = M; CP = C; state_pre = new double[DP * 1]; memset(state_pre, 0, sizeof(double)*DP); state_post = new double[DP * 1]; memset(state_post, 0, sizeof(*state_post)); transition_matrix = new double[DP * DP]; memset(transition_matrix, 0, sizeof(double)*DP*DP); process_noise_cov = new double[DP * DP]; memset(process_noise_cov, 0, sizeof(double)*DP*DP); measurement_matrix = new double[MP * DP]; memset(measurement_matrix, 0, sizeof(double)*MP*DP); measurement_noise_cov = new double[MP * MP]; memset(measurement_noise_cov, 0, sizeof(double)*MP*MP); error_cov_pre = new double[DP * DP]; memset(error_cov_pre, 0, sizeof(double)*DP*DP); error_cov_post = new double[DP * DP]; memset(error_cov_post, 0, sizeof(double)*DP*DP); gain = new double[DP * MP]; memset(gain, 0, sizeof(double)*DP*MP); if (CP > 0) { control_matrix = new double[DP * CP]; memset(control_matrix, 0, sizeof(double)*DP*CP); } } void CKalman::set_transition_matrix(const double*trans_matrix) { for (int i = 0; i < DP*DP; i++) { transition_matrix[i] = trans_matrix[i]; } } CKalman::~CKalman(){ delete[] state_pre; delete[] state_post; delete[] transition_matrix; if (CP >0){ delete[] control_matrix; } delete[] measurement_matrix; delete[] process_noise_cov; delete[] measurement_noise_cov; delete[] error_cov_pre; delete[] gain; delete[] error_cov_post; } void CKalman::KalmanPredict(double * control){ CMatrix matrix; /* update the state */ /* x'(k) = A*x(k) */ matrix.Mul(transition_matrix, DP, DP, state_post, DP, 1, state_pre); if (control && CP > 0){ /* x'(k) = x'(k) + B*u(k) */ double * temp1 = new double[DP * 1]; matrix.Mul(control_matrix, DP, CP, control, CP, 1, temp1); matrix.Add(state_pre, temp1, state_pre, DP, 1); delete[] temp1; temp1 = NULL; } /* update error covariance matrices */ /* temp2 = A*P(k) */ double * temp2 = new double[DP * DP]; matrix.Mul(transition_matrix, DP, DP, error_cov_post, DP, DP, temp2); /* P'(k) = temp2*At + Q */ double * temp3 = new double[DP * DP]; double * temp4 = new double[DP * DP]; matrix.Transpose(transition_matrix, DP, DP, temp3); matrix.Mul(temp2, DP, DP, temp3, DP, DP, temp4); matrix.Add(temp4, process_noise_cov, error_cov_pre, DP, DP); delete[] temp2; temp2 = NULL; delete[] temp3; temp3 = NULL; delete[] temp4; temp4 = NULL; } void CKalman::KalmanCorrect(double * measurement){ if (!measurement){ assert(false);// , "Measurement is Null!!!"); } CMatrix matrix; /* temp1 = Ht*/ double * temp1 = new double[DP * MP]; matrix.Transpose(measurement_matrix, MP, DP, temp1); /* temp2 = P'(k) * temp1 */ double * temp2 = new double[DP * MP]; matrix.Mul(error_cov_pre, DP, DP, temp1, DP, MP, temp2); /* temp3 = H*temp2 + R */ double * temp3 = new double[MP * MP]; matrix.Mul(measurement_matrix, MP, DP, temp2, DP, MP, temp3); matrix.Add(temp3, measurement_noise_cov, temp3, MP, MP); /* temp4 = inv(temp3) */ double * temp4 = new double[MP * MP]; matrix.ContraryMatrix(temp3, temp4, MP); /* K(k) = temp2 * temp4 */ matrix.Mul(temp2, DP, MP, temp4, MP, MP, gain); delete[] temp1; temp1 = NULL; delete[] temp2; temp2 = NULL; delete[] temp3; temp3 = NULL; delete[] temp4; temp4 = NULL; //update state_post /* temp5 = z(k) - H*x'(k) */ double * temp5 = new double[MP * 1]; matrix.Mul(measurement_matrix, MP, DP, state_pre, DP, 1, temp5); matrix.Sub(measurement, temp5, temp5, MP, 1); /* x(k) = x'(k) + K(k)*temp5 */ double * temp6 = new double[DP * 1]; matrix.Mul(gain, DP, MP, temp5, MP, 1, temp6); matrix.Add(state_pre, temp6, state_post, DP, 1); delete[] temp5; temp5 = NULL; delete[] temp6; temp6 = NULL; //update error_cov_post /* P(k) = P'(k) - K(k)*H* P'(k) */ double * temp7 = new double[MP * DP]; double * temp8 = new double[DP * DP]; matrix.Mul(measurement_matrix, MP, DP, error_cov_pre, DP, DP, temp7); matrix.Mul(gain, DP, MP, temp7, MP, DP, temp8); matrix.Sub(error_cov_pre, temp8, error_cov_post, DP, DP); delete[] temp7; temp7 = NULL; delete[] temp8; temp8 = NULL; }
// kalman-filter.cpp : 定义控制台应用程序的入口点。 // #include "stdafx.h" #include"kalmanfilter.h" #include<time.h> #include<cstdlib> #include <cv.h> #include <cxcore.h> #include <highgui.h> using namespace std; const int winHeight = 600; const int winWidth = 800; CvPoint mousePosition = cvPoint(winWidth >> 1, winHeight >> 1); //mouse event callback void mouseEvent(int event, int x, int y, int flags, void *param) { if (event == CV_EVENT_MOUSEMOVE) { mousePosition = cvPoint(x, y); } } int _tmain(int argc, _TCHAR* argv[]) { time_t t; srand(time(&t)); //1.kalman filter setup const int stateNum = 4; const int measureNum = 2; CKalman kf(stateNum, measureNum, 0);//state(x,y,detaX,detaY) double A[] = {//transition matrix 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1 }; kf.set_transition_matrix(A); kf.measurement_matrix[0] = 1;//for x,x is an observation kf.measurement_matrix[5] = 1;//for y,y is an observation kf.measurement_noise_cov[0] = 1e1; kf.measurement_noise_cov[3] = 1e1; kf.process_noise_cov[0] = 1e-1;//diag kf.process_noise_cov[5] = 1e-1; kf.process_noise_cov[10] = 1e-1; kf.process_noise_cov[15] = 1e-1; int nn = winHeight > winWidth ? winWidth : winHeight; kf.state_post[0] = double(rand()) / RAND_MAX*nn;//x kf.state_post[1] = double(rand()) / RAND_MAX*nn;//y kf.state_post[2]=rand();//deltaX kf.state_post[3]=rand();//deltaY kf.error_cov_post[0] = 1;//diag kf.error_cov_post[5] = 1; kf.error_cov_post[10] = 1; kf.error_cov_post[15] = 1; CvFont font; cvInitFont(&font, CV_FONT_HERSHEY_SCRIPT_COMPLEX, 1, 1); cvNamedWindow("kalman"); cvSetMouseCallback("kalman", mouseEvent); IplImage* img = cvCreateImage(cvSize(winWidth, winHeight), 8, 3); while (1){ //2.kalman prediction kf.KalmanPredict(NULL); CvPoint predict_pt = cvPoint((int)kf.state_pre[0], (int)kf.state_pre[1]); //3.update measurement double measure[2]; measure[0] = double(mousePosition.x); measure[1] = double(mousePosition.y); //4.update kf.KalmanCorrect(measure); //draw cvSet(img, cvScalar(255, 255, 255, 0)); cvCircle(img, predict_pt, 5, CV_RGB(0, 255, 0), 3);//predicted point with green cvCircle(img, mousePosition, 5, CV_RGB(255, 0, 0), 3);//current position with red char buf[256]; sprintf_s(buf, 256, "predicted position:(%3d,%3d)", predict_pt.x, predict_pt.y); cvPutText(img, buf, cvPoint(10, 30), &font, CV_RGB(0, 0, 0)); sprintf_s(buf, 256, "current position :(%3d,%3d)", mousePosition.x, mousePosition.y); cvPutText(img, buf, cvPoint(10, 60), &font, CV_RGB(0, 0, 0)); cvShowImage("kalman", img); int key = cvWaitKey(3); if (key == 27){//esc break; } } cvReleaseImage(&img); return 0; }