实现你的kalman
本文结合opencv的Mat数据类型,实现了kalman跟踪,具体理论知识可以参考博客:http://blog.csdn.net/baimafujinji/article/details/50646814
#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;
#include "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;
}
#include "kalman.h"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);}
}void main()
{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 showImg(winWidth, winHeight, CV_8UC3);Mat measurement(mea_size,1,CV_32F);for (;;){setMouseCallback("Kalman", mouseEvent);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[256];sprintf_s(buf, 256, "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_s(buf, 256, "true position:(%3d,%3d)", mousePosition.x, mousePosition.y);putText(showImg, buf, cvPoint(10, 90), FONT_HERSHEY_SIMPLEX, 1, Scalar::all(255));imshow("Kalman", showImg);int key = waitKey(3);if (key == 27){break;}}
}
kalman论文看起来挺迷糊的,但是实现还是挺好实现的
实现你的kalman相关推荐
- Error-State Kalman filter (ESKF)
1. 原理 1.1. 变量定义 1.2. 名义量预测 1.3. 误差量预测 1.4. 更新 1.5. 重置 2. 主要优点 The orientation error-state is minimal ...
- 一文图解卡尔曼滤波(Kalman Filter)
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达 译者注:这恐怕是全网有关卡尔曼滤波最简单易懂的解释,如果你认真的读 ...
- [Math]理解卡尔曼滤波器 (Understanding Kalman Filter)
2. 基本模型 2.1 系统模型 卡尔曼滤波模型假设k时刻的真实状态是从(k − 1)时刻的状态演化而来,符合下式: (1) Fk 是作用在 Xk−1 上的状态变换模型(/矩阵/矢量). Bk 是作 ...
- Control~Kalman filter
常用控制算法(包括PID和卡尔曼滤波等)各有什么天然的局限乃至缺陷 回答都专业而又接地气 自平衡小车的探讨:卡尔曼滤波与PID算法 ...
- 理解Kalman滤波的使用
Kalman滤波简介 Kalman滤波是一种线性滤波与预测方法,原文为:A New Approach to Linear Filtering and Prediction Problems.文章推导很 ...
- 卡尔曼滤波(kalman)相关理论以及与HMM、最小二乘法关系 转
卡尔曼滤波(kalman)相关理论以及与HMM.最小二乘法关系_weixin_30527143的博客-CSDN博客
- c语言卡尔曼函数库,kalman之c语言实现
kalman的文章之前也写过一篇,具体参考:kalman,本文主要是基于c的实现,若有不当之处,望指教,谢谢! typedef struct image_double { double* pix; u ...
- matlab温度数据怎么滤波_卡尔曼滤波算法思想理解 Kalman filter 第一篇
卡尔曼滤波算法思想理解 Kalman filter 第一篇 最近在初步的理解目标跟踪的领域, 其中一个非常经典的算法卡尔曼滤波Kalman filter是需要有很好的理解才行, 由于已经脱离了学校,懂 ...
- kalman 滤波 演示与opencv代码
在机器视觉中追踪时常会用到预测算法,kalman是你一定知道的.它可以用来预测各种状态,比如说位置,速度等.关于它的理论有很多很好的文献可以参考.opencv给出了kalman filter的一个实现 ...
- kalman滤波的解释
文章目录 kalman滤波的解释 背景 信号经过线性变换后的分布 信号的相乘 一维信号 多维信号 贝叶斯估计 状态方程 先验分布 后验分布 总结 kalman滤波的解释 背景 这里的信号都是指服从正态 ...
最新文章
- Linux架构思维导图
- 小学计算机课调研问卷,小学生深度学习--调查问卷(教师专业素养维度)
- 30个非常有吸引力的黑色网站设计作品
- 解决Dialog 消失,输入法不消失的问题
- Android中BroadCast与Activity之间的通信
- 烂泥:通过vsphere给esxi添加本地硬盘
- adb 命令的个人记录
- 服务器升级中暂不可修改怎么回事,抖音服务器升级中,暂不支持本地区开播抖音怎么在法国直播?...
- sublime搭建C/C++编译环境
- iOS开发之WKWebview(淘宝链接不自动打开淘宝和天猫的app)
- 【Microsoft Azure 的1024种玩法】七.Azure云端搭建部署属于自己的维基百科
- 01 ArcGIS空间分析--水文分析--利用水文分析方法提取山谷山脊线
- 上天入地无所不能的C语言(一)
- live2d_碧蓝航线:老婆们在外服都长啥样?她惊艳四座,Live2D独此一家
- freetype的安装与使用
- 中基鸿业低收入家庭如何理财
- 2021-07-14
- 《圈外课程学习记录》3.2 数据化强力说服
- 为什么计算机编程以英语为主,为什么做编程都用英文,中文不行吗?听程序员解释完,总算明白了...
- 产业经济学古诺模型计算机,古诺模型概述及对多寡头垄断市场的推广研究
热门文章
- ascii码中的可见字符
- Prometheus + AlertManager + Prometheus-webhook-dingtalk的告警通知
- 双 JK 触发器 74LS112 逻辑功能。真值表_时序逻辑电路设计(一):同步计数器...
- Windows磁盘管理工具Diskpart之一 管理基本磁盘
- 怎么设置BIOS 启用硬件虚拟化
- H.266/VVC帧间预测技术学习:带有运动矢量差的Merge技术(Merge mode with MVD)
- c3p0详细配置(c3p0-config.xml)及使用
- 请问set JAVA_OPTS的各项参数是什么意思?
- 为什么80后集体缺席互联网
- 关于通用雷达信号的时频分析与图像绘制(Matlab)