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

• 参考

https://blog.csdn.net/yongjiankuang/article/details/76218996

• 安装编译opencv

https://blog.csdn.net/quantum7/article/details/82881521

特别注意：

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

• gh_kalman.h

#ifndef __KALMAN_H__
#define __KALMAN_H__

#include 
#include 

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_SIZE	512
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_(4, 4)