[C++] Kalman算法C++实现代码（编译运行通过）

sudo apt-get install cmake libgtk2.0-dev pkg-config

• gh_kalman.h
  

#ifndef __KALMAN_H__
#define __KALMAN_H__
#include <iostream>
#include <opencv2/opencv.hpp>
using namespace std;
using namespace cv;
class KALMAN
{
public:
  KALMAN(int state_size, int mea_size);
  ~KALMAN();
public:
  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)) ： 1 * 8
  Mat transitionMatrix;  //转移矩阵(A)  ： 8 * 8
  Mat controMatrix;    //控制矩阵(B)
  Mat measurementMatrix;   //测量矩阵(H) ：4 * 8
  Mat processNoiseCov;   //预测模型噪声协方差矩阵(Q) ：8 * 8
  Mat measurementNoiseCov; //测量噪声协方差矩阵(R)  ： 4 * 4
  Mat errorCovPre;     //转移噪声矩阵(P'(k)) p'(k) = A * p(k - 1) * At + Q 
  Mat K;           //kalman增益矩阵 K = p'(k) * Ht * inv(H * p'(k) * Ht + R)
  Mat errorCovPost;    //转移噪声修正矩阵(p(k)) p(k) = (I - K(k) * H) * p'(k)  ： 8 * 8
public:
  void init();
  void update(Mat Y);
  Mat predicted(Mat Y);
};
#endif

• gh_kalman.cpp
  

#include "gh_kalman.h"
KALMAN::KALMAN(int state_size,int mea_size)
{
  transitionMatrix  = Mat::zeros(state_size, state_size, CV_32F);
  measurementMatrix   = Mat::zeros(mea_size,   state_size, CV_32F);
  processNoiseCov   = Mat::zeros(state_size, state_size, CV_32F);
  measurementNoiseCov = Mat::zeros(mea_size,   mea_size,   CV_32F);
  errorCovPre     = Mat::zeros(state_size, state_size, CV_32F);
  errorCovPost    = Mat::zeros(state_size, state_size, CV_32F);
  statePost       = Mat::zeros(state_size, 1,      CV_32F);
  statePre      = Mat::zeros(state_size, 1,      CV_32F);
  K           = Mat::zeros(state_size, mea_size,   CV_32F);
}
KALMAN::~KALMAN()
{
  //
}
void KALMAN::init()
{
  setIdentity(measurementMatrix,   Scalar::all(1));   //观测矩阵的初始化;
  setIdentity(processNoiseCov,   Scalar::all(1e-5));//模型本身噪声协方差矩阵初始化;
  setIdentity(measurementNoiseCov, Scalar::all(1e-1));//测量噪声的协方差矩阵初始化
  setIdentity(errorCovPost,    Scalar::all(1));   //转移噪声修正矩阵初始化
  randn(statePost,Scalar::all(0),  Scalar::all(5));   //kalaman状态估计修正矩阵初始化
}
void KALMAN::update(Mat Y)
{
  K      = errorCovPre * (measurementMatrix.t()) * ((measurementMatrix * errorCovPre * measurementMatrix.t() + measurementNoiseCov).inv());
  statePost  = statePre  + K * (Y - measurementMatrix * statePre);
  errorCovPost = errorCovPre - K * measurementMatrix * errorCovPre;
}
Mat KALMAN::predicted(Mat Y)
{
  statePre  = transitionMatrix * statePost;
  errorCovPre = transitionMatrix * errorCovPost * transitionMatrix.t() + processNoiseCov;
  update(Y);
  return statePost;
}

• gh_test.cpp
  

#include "gh_kalman.h"

#define WINDOW_NAME "Kalman"
#define BUFFER_SIZE512
const int winWidth  = 800;
const int winHeight = 600;
Point mousePosition = Point(winWidth >> 1, winHeight >> 1);
//mouse call back  
void mouseEvent(int event, int x, int y, int flags, void *param)
{
  if (event == CV_EVENT_MOUSEMOVE)
  {
    mousePosition = Point(x, y);
  }
}
int main(int argc, char** argv)
{
  int state_size = 4;
  int mea_size   = 2;
  KALMAN kalman(state_size,mea_size);
  kalman.init();
  kalman.transitionMatrix = (Mat_<float>(4, 4) <<
    1, 0, 1, 0,
    0, 1, 0, 1,
    0, 0, 1, 0,
    0, 0, 0, 1);//元素导入矩阵，按行; 
  Mat g_srcImage;
  Mat showImg(winWidth, winHeight, CV_8UC3);
  Mat measurement(mea_size,1,CV_32F);
  for (;;)
  {
    setMouseCallback(WINDOW_NAME, mouseEvent, 0);
    showImg.setTo(0);
    Point statePt = Point((int)kalman.statePost.at<float>(0), (int)kalman.statePost.at<float>(1));
    //3.update measurement  
    measurement.at<float>(0) = (float)mousePosition.x;
    measurement.at<float>(1) = (float)mousePosition.y;
    //2.kalman prediction   
    Mat   prediction  = kalman.predicted(measurement);
    Point predictPt   = Point((int)prediction.at<float>(0), (int)prediction.at<float>(1));

    //randn( processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0))));  
    //state = KF.transitionMatrix*state + processNoise;  
    //draw  
    circle(showImg, statePt,     5, CV_RGB(255,   0,   0), 1);//former point  
    circle(showImg, predictPt,   5, CV_RGB(  0, 255,   0), 1);//predict point  
    circle(showImg, mousePosition, 5, CV_RGB(  0,   0, 255), 1);//ture point  
    //      CvFont font;//字体  
    //      cvInitFont(&font, CV_FONT_HERSHEY_SCRIPT_COMPLEX, 0.5f, 0.5f, 0, 1, 8);  
    char buf[BUFFER_SIZE];
    sprintf(buf, "Green:predicted position:(%3d,%3d)", predictPt.x, predictPt.y);
    //putText(showImg, "Red: Former Point", cvPoint(10, 30), FONT_HERSHEY_SIMPLEX, 1, Scalar::all(255));
    putText(showImg, buf, cvPoint(10, 60), FONT_HERSHEY_SIMPLEX, 1, Scalar::all(255));
    sprintf(buf, "true position:(%3d,%3d)", mousePosition.x, mousePosition.y);
    putText(showImg, buf, cvPoint(10, 90), FONT_HERSHEY_SIMPLEX, 1, Scalar::all(255));
    imshow(WINDOW_NAME, showImg);
    int key = waitKey(3);
    if (key == 27)
    {
      break;
    }
  }
  return 0;
}

• 编译