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 <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_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_<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;
}      

編譯

有兩個問題要注意：

opencv的編譯。如果提示Exception，重新編譯opencv。

需要的cv庫：

 -L /usr/local/lib -lopencv_core -lopencv_highgui -lopencv_imgproc -lopencv_imgcodecs