namespace cv{class CV_EXPORTS_W KalmanFilter
{public://缺省构造函数：无论构造函数是自动生成的还是用户定义的，它都是可以调用的构造函数，无需提供任何参数。CV_WRAP KalmanFilter();//完整构造函数CV_WRAP KalmanFilter(int dynamparams,int measureparamsint, int controlParams=0, int type=CV_32F);  //dynamparams:状态维度//measureparamsint：观测维度//controlParams：控制维度void init(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F);//计算预测状态CV_WRAP const Mat& predict(const Mat& control=Mat());  //依据观测值更新预测状态 CV_WRAP const Mat& correct(const Mat& measurement);  Mat statePre;           //!< 预测状态 (x'(k)): x(k)=A*x(k-1)+B*u(k)  Mat statePost;          //!< 纠正状态 (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))  Mat transitionMatrix;   //!< 状态转换矩阵 (F)  Mat controlMatrix;      //!< 控制矩阵(B) (not used if there is no control)  Mat measurementMatrix;  //!< 观测矩阵(H)  Mat processNoiseCov;    //!< 处理过程噪声协方差矩阵 (Q)  Mat measurementNoiseCov;//!< 观测噪声协方差矩阵 (R)  Mat errorCovPre;        //!< 先验误差估计协方差矩阵 (P'(k)): P'(k)=A*P(k-1)*At + Q)*/  Mat gain;               //!< 卡尔曼增益矩阵 (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)  Mat errorCovPost;       //!< 后验误差估计协方差矩阵 (P(k)): P(k)=(I-K(k)*H)*P'(k)  // temporary matrices  Mat temp1;  Mat temp2;  Mat temp3;  Mat temp4;  Mat temp5;
};
}

namespace cv
{KalmanFilter::KalmanFilter() {}//dynamparams:状态维度//measureparamsint：观测维度//controlParams：控制维度
KalmanFilter::KalmanFilter(int dynamParams, int measureParams, int controlParams, int type)
{init(dynamParams, measureParams, controlParams, type);
}void KalmanFilter::init(int DP, int MP, int CP, int type)
{CV_Assert( DP > 0 && MP > 0 );CV_Assert( type == CV_32F || type == CV_64F );CP = std::max(CP, 0);statePre = Mat::zeros(DP, 1, type);//状态向量维度DP X 1statePost = Mat::zeros(DP, 1, type);transitionMatrix = Mat::eye(DP, DP, type);// 状态转移矩阵F的大小为DP X DPprocessNoiseCov = Mat::eye(DP, DP, type);// 处理过程噪声协方差矩阵Q的大小为DP X DPmeasurementMatrix = Mat::zeros(MP, DP, type);//观测矩阵H的大小为MP X DPmeasurementNoiseCov = Mat::eye(MP, MP, type);// 观测噪声协方差矩阵R大小为MP X MPerrorCovPre = Mat::zeros(DP, DP, type); // 先验误差估计协方差矩阵P'大小为DP X DPerrorCovPost = Mat::zeros(DP, DP, type);// 后验误差估计协方差矩阵P大小为DP X DPgain = Mat::zeros(DP, MP, type); // 卡尔曼增益矩阵if( CP > 0 )controlMatrix = Mat::zeros(DP, CP, type);// 控制矩阵BelsecontrolMatrix.release();temp1.create(DP, DP, type);temp2.create(MP, DP, type);temp3.create(MP, MP, type);temp4.create(MP, DP, type);temp5.create(MP, 1, type);
}const Mat& KalmanFilter::predict(const Mat& control)
{// 更新预测状态 e: x'(k) = F*x(k)statePre = transitionMatrix*statePost;//判断是否添加控制，是否添加控制矩阵影响更新的预测状态if( !control.empty() )// x'(k) = x'(k) + B*u(k)statePre += controlMatrix*control;// temp1 = F*P(k)temp1 = transitionMatrix*errorCovPost;// 更新先验误差估计协方差矩阵P'(k) = temp1*F_T + Q// gemm(a,b,n,c,m,ans,flags): ans = n*a*b+m*c// flags=GEMM_2_T:b transposesgemm(temp1, transitionMatrix, 1, processNoiseCov, 1, errorCovPre, GEMM_2_T);// handle the case when there will be measurement before the next predict.//a.copyTo(b):得到与a一样的b，两者可进行互不相关的操作。statePre.copyTo(statePost);errorCovPre.copyTo(errorCovPost);return statePre;
}const Mat& KalmanFilter::correct(const Mat& measurement)
{// K(k) = p'(k)*H_T*inv(H*p'(k)*H_T+R)// temp2 = F*P'(k)temp2 = measurementMatrix * errorCovPre;// temp3 = temp2*H_T + Rgemm(temp2, measurementMatrix, 1, measurementNoiseCov, 1, temp3, GEMM_2_T);//// solve(MA,MB,MX,CV_LU) 可以求AX=B for X 等价于X=A-1 *B// temp4 = inv(temp3)*temp2 = K(k)_Tsolve(temp3, temp2, temp4, DECOMP_SVD);// K(k)// 转置过来得到真正的Kgain = temp4.t();// temp5 = z(k) - H*x'(k)temp5 = measurement - measurementMatrix*statePre;// x(k) = x'(k) + K(k)*temp5statePost = statePre + gain*temp5;// P(k) = P'(k) - K(k)*temp2errorCovPost = errorCovPre - gain*temp2;return statePost;
}
}

#include "opencv2/video/tracking.hpp"
#include <iostream>
#include <stdio.h>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/imgproc.hpp>
using namespace cv;
using namespace std;
void on_mouse(int e, int x, int y, int d, void *ptr)
{Point*p = (Point*)ptr;p->x = x;p->y = y;
}int main()
{// 初始化int stateSize = 4;  // [x, y, v_x, v_y]int measSize = 2;   // [z_x, z_y] int contrSize = 0;  // no control inputunsigned int F_type = CV_32F;//or CV_64FKalmanFilter KF(stateSize, measSize, contrSize, F_type);Mat state(stateSize, 1, F_type);  // [x, y, v_x, v_y] Mat meas(measSize, 1, F_type);    // [z_x, z_y] setIdentity(KF.transitionMatrix);setIdentity(KF.measurementMatrix);setIdentity(KF.processNoiseCov, Scalar::all(1e-4));setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));setIdentity(KF.errorCovPost, Scalar::all(.1));Mat display_image(600, 800, CV_8UC3);namedWindow("Mouse Kalman");char ch = 0;double ticks = 0;Point mousePos;vector<Point> mousev, kalmanv;while (ch != 'q' && ch != 'Q'){//预测state = KF.predict(); Point predictPt(state.at<float>(0), state.at<float>(1));// 鼠标观测setMouseCallback("Mouse Kalman", on_mouse, &mousePos);meas.at<float>(0) = mousePos.x;meas.at<float>(1) = mousePos.y;// >更新Mat estimated = KF.correct(meas);Point statePt(estimated.at<float>(0), estimated.at<float>(1));Point measPt(meas.at<float>(0), meas.at<float>(1));char mousep[20];sprintf(mousep, "(%0.2f,%0.2f)", meas.at<float>(0), meas.at<float>(1));putText(display_image, "mouse point:",Point(0,10), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 0), 2,false );putText(display_image, mousep, Point(120,10), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 0), 2,false);char prept[20];sprintf(prept, "(%0.2f,%0.2f)", state.at<float>(0), state.at<float>(1));putText(display_image, "pre point:",Point(0,50), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 0), 2,false );putText(display_image, prept,Point(120,50), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 0), 2,false );imshow("Mouse Kalman", display_image);display_image = Scalar::all(0);//push_back 将新元素移动拷贝到vector里mousev.push_back(measPt);kalmanv.push_back(statePt);//以点坐标画园circle(display_image,statePt,5,(255,0,0),1);circle(display_image,measPt,5,(0,0,255),1);//画出运动轨迹for (int i = 0; i < mousev.size() - 1; i++)line(display_image, mousev[i], mousev[i + 1], Scalar(0, 0, 255), 1);for (int i = 0; i < kalmanv.size() - 1; i++)line(display_image, kalmanv[i], kalmanv[i + 1], Scalar(255, 0, 0), 1);ch = cv::waitKey(10);}
}