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 * 8Mat transitionMatrix;    //转移矩阵(A)  ： 8 * 8Mat controMatrix;        //控制矩阵(B)Mat measurementMatrix;   //测量矩阵(H) ：4 * 8Mat processNoiseCov;     //预测模型噪声协方差矩阵(Q) ：8 * 8Mat measurementNoiseCov; //测量噪声协方差矩阵(R)  ： 4 * 4Mat 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 * 8public: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

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

在线代码编译运行工具
在线代码编译运行工具如果需要学习语言,比如练习一些算法,或者跑一些别人写的代码,有一些语言特性不太了解需要写一些简单的 demo 做一些验证,那么先搭建一个环境去跑就有一点麻烦了,无需搭建本地环境的 ...
该段代码编译运行后显示的是蓝色背景，白色闪烁字的效果
int main(int argc,char **argv) { printf("\033[44;37;5m hello world\033[0m\n"); ...
caffe SSD 代码编译运行流程及问题解决
caffe SSD 代码编译运行流程及问题解决该文基于以下代码: https://github.com/weiliu89/caffe/tree/ssd down下来后,进入目录 -rw-rw-r-- ...
cmd命令行进行C++代码编译运行；实现进程调度和存储管理
最近刚考完一门,也算是有点闲暇时间,写个博客,就当做操作系统实验的笔记吧. 客官先别急着白嫖点个赞再看吧求求了文章目录一.在cmd命令行下编译运行C++源代码 1.进入目标目录 2. ...
VTK Example代码编译运行
在VTK源码包的Example中,每个模块的例子都有一个CMakeList文件,可之间编译运行. 但VTK版本更新快,模块经常小改,而对于VTK6.x的Example的CMakeLists却没有改动 ...
OpenCV神经网络ANN代码编译运行与解读（一）
运行环境搭建:参考<VS2013安装OpenCV4.1版本并搭建一个小程序> 基于OpenCV4.1.0中neural_network.cpp的例子代码. 参考了<OpenCV3[神 ...
OpenCV神经网络ANN代码编译运行与解读（二）
[转载自]<OpenCV笔记(4)实现神经网络(ANN)> 作者的原文参考了:<OpenCV中使用神经网络 CvANN_MLP> 实际上的预测结果是并非一个确定值,所以即使用训 ...
java floyd_百度百科里面的floyd算法java的代码，总是无法运行。请问是代码有问题吗，如何编译啊？...
展开全部不能编译运行的说法是错误,但是结果是否正确,我就32313133353236313431303231363533e59b9ee7ad9431333335303539不知道了,我不懂这个算法p ...
目标跟踪算法 - CMT、TLD、KCF现成代码编译
本文章将统计多种目标跟踪算法,并编译运行,比较不同算法的优劣势. 下文我会将每个算法用到的环境都会仔细备注,以免有些算法对OpenCV版本挑剔.请先自行下载并配置好OpenCV环境,安装CMAKE.由 ...
TVM yolov3优化代码修改（编译运行OK）
TVM yolov3优化代码修改(编译运行OK) yolov3_quantize_sample.py 附https://github.com/makihiro/tvm_yolov3_sample代码: ...

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

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

最新文章

热门文章