完整的卡尔曼滤波器推导、参数分析以及C++实现(带有目标跟踪效果对比)
完整的卡尔曼滤波器推导、参数分析以及仿真(带有目标跟踪效果对比)
1. 引言
之前看过卡尔曼滤波器的介绍,但是学得比较迷惑,只知道它有预测作用,不知道怎么去使用。这
篇博客将从理论、代码、实际效果几个方面解释卡尔曼滤波器,帮助大家在项目中使用卡尔曼滤波器。
比较有启发的几个教程为:
卡尔曼滤波器OpenCV的具体实现:https://www.geek-share.com/detail/2669109669.html
Matlab的讲解什么是卡尔曼滤波器:https://www.zhihu.com/question/23971601
卡尔曼滤波器里的具体参数的作用:https://zhuanlan.zhihu.com/p/110107365
接下来我将针对各大神的博客以及我个人的理解进行卡尔曼滤波器的介绍,这其中肯定存在一些错
误,希望大家批评指正,能跟大家一起讨论学习进步~
2. 算法效果
直接附上我在跟踪器中加入卡尔曼滤波器后跟踪框的效果:
两个图分别为原始跟踪器的跟踪结果与加上卡尔曼滤波以后的跟踪框效果,发现当加上卡尔曼滤波
以后跟踪框变得非常顺滑~~~,从视觉观感上来说看着更加丝滑了。
3. 卡尔曼滤波的迭代
借用Matlab的例子:当一辆汽车直线匀速,可以使用GPS对其进行定位,但是测量结果存在一定误
差,所以车辆位置不够准确,但是在这个过程中,我们还知道汽车的运行速度,这可以帮助我们得到汽
车的位置的预测值。
通过预测值与测量值的计算我们可以得到汽车位置的最优值。
3.1 问题描述
3.2 卡尔曼滤波器的计算
3.3 卡尔曼滤波器迭代过程:
/////////////////////////////////////////////
同学们可能对F状态矩阵和H观测矩阵有比较大的迷惑,不知道这两个矩阵到底有什么作
用,不要着急,接下来我会用例子对参数进行讲解,哪些参数需要初始化,如何初始化。
////////////////////////////////////////////
3.4 参数分析
我们再来看看这张图,卡尔曼滤波可以用一句话总结:通过汽车预测结果的高斯分布以及测量
值的高斯分布,通过高斯相乘可以得到汽车位置的最佳值。
如果我们有多个传感器,获得了多个位置的测量值,我们仍然可以用高斯滤波进行目标最佳位置估
计:
4. 目标跟踪中卡尔曼滤波器的简单使用
在目标跟踪中,我想对跟踪框的中心点坐标进行卡尔曼滤波,减小跟踪框的“卡顿”现象,状态量包括
中心点x和y坐标,以及x和y的变化率。状态量X为:
实时观测的仅观测中心点的坐标,所以观测矩阵H为:
目标跟踪中假设目标为匀速直线运动,所以状态矩阵F为:
Z矩阵为实际每次的测量矩阵:
P估计量协方差,R测量协方差,Q预估过程协方差的初始化(R和Q可以自行进行初始化),在目标跟踪中我初始化为:
针对R和Q的协方差初始化,不同的初始化有着不同的效果。如果测量协方差的越小,最终的结果更
趋向于测量的结果,反之则趋向于预测结果。这里不再推导公式了~~
(链接https://www.zhihu.com/question/23971601 第4个视频4分钟有结果的数学推导;
链接https://zhuanlan.zhihu.com/p/109859418 对R和Q协方差矩阵的推导)
5. OpenCV中卡尔曼滤波的使用
我自己实现了一下卡尔曼滤波器,并且这个可以正常使用,我的项目里用的就是这个
//头文件 #include <opencv2/highgui/highgui.hpp> #include <opencv2/core/core.hpp> #include <opencv2/imgproc/imgproc.hpp> #include<iostream> using namespace cv; class use_kalman_filter { public: Mat x_state; Mat z; //真实值 int stateNum; //状态值个数 int measureNum; //测量值个数 void set_Q(float x); void set_R(float y); void set_P(float z); void get_F(Mat FF); void init(); //卡尔曼滤波器的初始化 void correct(); //用于对预测值的更新 private: Mat Q; //系统噪声方差矩阵Q Mat R; //测量噪声方差矩阵R Mat F; //状态转移矩阵,物理方程 Mat H; //观测矩阵 Mat K; //卡尔曼系数 Mat P; //预测的协方差矩阵 Mat P_predict; //最终的协方差矩阵 Mat x_hat_prect; Mat temp; };
#include "kalman_filter.h" void::use_kalman_filter::set_Q(float x) { Q = x*Mat::eye(stateNum, stateNum, CV_32F); } void::use_kalman_filter::set_R(float y) { R = y*Mat::eye(measureNum, measureNum, CV_32F); } void::use_kalman_filter::set_P(float z) { P_predict = z*Mat::eye(stateNum, stateNum, CV_32F); } void::use_kalman_filter::get_F(Mat FF) { F = FF.clone(); } void use_kalman_filter::init() { K = Mat::zeros(stateNum, stateNum, CV_32F); H = Mat::zeros(measureNum, stateNum, CV_32F); temp = Mat::zeros(stateNum, stateNum, CV_32F); for (int i = 0; i < measureNum; i++) { H.at<float>(i, i) = 1; } } void::use_kalman_filter::correct() { //公式均为blog中的公式 //predict x_hat_prect = F*x_state; P = F*P_predict*F.t() + Q; //Update temp = H*P*H.t() + R; temp = temp.inv(); K = P*H.t() *temp; x_state = x_hat_prect+ K*(z - H *x_hat_prect); //预测值 P_predict = (Mat::eye(stateNum, stateNum, CV_32F) - K*H)*P; //预测值协方差 }
#include"kalman_filter.h" #include<iostream> using namespace std; use_kalman_filter KF; void main() { Mat trans; ///=================卡尔曼滤波器的初始化=============================== //初始化的值有状态量的数值 stateNum //真实测量值的个数 measureNum //系统噪声方差矩阵 Q //测量噪声方差矩阵 R //预测值的协方差初始化 P //X的初始化 x_state //转移矩阵(运动方程) F //kalman_filter_initial KF.stateNum = 4; KF.measureNum = 2; KF.init(); KF.set_Q(0.0001); KF.set_R(0.01); KF.set_P(1); KF.x_state = (Mat_<float>(4, 1) << 100, 100, 0, 0); //预测阶段 KF.get_F((Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1)); KF.z = (Mat_<float>(2, 1) << 50, 90); //测量值 KF.correct(); cout<<x_state.at<float>(1)<<" "<< x_state.at<float>(2)<<endl; //预测值 }
- 点赞 2
- 收藏
- 分享
- 文章举报
- jQuery实现仿淘宝带有指示条的图片转动切换效果完整实例
- 霍夫变换检测直线的公式推导以及基于opencv的源代码分析并实例实现
- 完整的C++实现算法导论十三章红黑树以及十四章中的顺序统计树
- 三种工厂模式的分析以及C++实现
- 三种工厂模式的分析以及C++实现
- 三种工厂模式的分析以及C++实现
- jQuery实现仿淘宝带有指示条的图片转动切换效果完整实例
- 【排序算法】九大内部排序算法总结(附对比分析,C++实现)
- 数据库shard中间件对比,以及sharding-jdbc 实现原理分析
- 用C++,C语言实现几种排序以及性能对比(sort,qsort,冒泡)
- python实现自动监测目标网站的爬取速度_以及整体网络环境分析
- Java基础---基础加强---增强for循环、自动拆装箱及享元、枚举的作用、实现带有构造方法、透彻分析反射的基础_Class类、成员变量的反射、数组参数的成员方法进行反射、数组的反射应用
- 三种工厂模式的分析以及C++实现
- sobel算子实现边缘检测及其c++实现及与matlab效果对比
- 三种工厂模式的分析以及C++实现
- Java基础---基础加强---增强for循环、自动拆装箱及享元、枚举的作用、实现带有构造方法、透彻分析反射的基础_Class类、成员变量的反射、数组参数的成员方法进行反射、数组的反射应用
- 三种工厂模式的分析以及C++实现
- opencv3/C++ 使用Tracker实现简单目标跟踪
- 三种工厂模式的分析以及C++实现