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;
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
文档来源：51CTO技术博客https://blog.51cto.com/u_13161667/3309489
页:
[1]