卡尔曼滤波原理及实现
来源:互联网 发布:国际现货黄金软件 编辑:程序博客网 时间:2024/05/17 20:33
卡尔曼滤波原理及实现
前一段时间,做项目研究了一下卡尔曼滤波,并且在项目当中实现了一个物体跟踪的功能,所以,借着新鲜劲儿,本次博客对卡尔曼滤波进行一次整理。
- 卡尔曼滤波是什么
- 卡尔曼滤波能做什么
- 卡尔曼滤波的工作原理
- 举个栗子
卡尔曼滤波是什么
卡尔曼滤波适用于估计一个动态系统的最优状态。即便是观测到的系统状态参数含有噪声,观测值不准确,卡尔曼滤波也能够完成对状态真实值的最优估计。网上大多数的教程讲到卡尔曼的数学公式推导,会让人很头疼,难以把握其中的主线和思想。所以我参考了国外一位学者的文章,讲述卡尔曼滤波的工作原理,然后编写了一个基于OpenCV的小程序给大家做一下说明。下面的这个视频请大家先直观地看看热闹吧~
角度跟踪视频
卡尔曼滤波能做什么
假设我们手头有一辆DIY的移动小车。这辆车的外形是这样的:
这辆车可以在荒野移动,为了便于对它进行控制,需要知道它的位置以及移动速度。所以,建立一个向量,用来存储小车的位置和速度
卡尔曼滤波的工作原理
1.先验状态估计
以之前我们创建的状态变量为例,
下图表示的是一个状态空间图,为了研究方便,假如小车在一条绝对笔直的线路上面行驶,其位置和速度的方向是确定的,不确定的是大小。
卡尔曼滤波器发生作用的前提是小车的速度和位置量在其定义域内具有正态的高斯分布规律。每一个变量都具有一个平均值
下面请看另外一种情况:下图的位置和速度的分布不再像上图一样了,速度越大,位置也越大,这种情况下,两个变量相关(其实这种情况是很有可能发生的,因为速度越大的话,可能小车就远离我们,速度越小,表明靠近我们)。那么,通过协方差矩阵
要注意的是协方差矩阵是一个对称阵。感兴趣的童鞋可以深入研究一下,协方差矩阵的特征值和特征向量所具有的几何意义:the directions in which the data varies the most.啥意思,就是哪个方向变化快,特征向量指哪儿。
协方差的特征向量是什么
协方差的几何意义
两个链接提供给大家,有兴趣的好好看看,知识点融会贯通了才有意思!
下面是重头戏:数学描述部分:
定义:
下图描述了一个k-1时刻
定义
写成矩阵的形式就是:
状态向量的更新有了,但是还缺少状态向量之间相关性的更新,也就是协方差矩阵。
外部确定性影响
到目前为止,我们只是关心系统内部状态的更新,但是如果在外部对系统产生了确定影响,比如:小车的操控者发出一条刹车的指令,我们通过建模该指令,假如小车的加速度为
其中,
外部不确定性影响
现实世界往往是不那么好描述清楚的,就是存在不确定的外部影响,会对系统产生不确定的干扰。我们是无法对这些干扰进行准确的跟踪和量化的。所以,除了外界的确定项,还需要考虑不确定干扰项
从而,得到最终先验估计的更新方程:
每一次的状态更新,就是在原来的最优估计的基础上面,下一次的状态落在一个新的高斯分布区域,从坐标系上面看就像是一团云状的集合,最优的估计就在这个云团当中的某一处。所以,我们首先要弄清楚这个云团具有什么样的性质,也就是使用过程激励噪声协方差
考虑到不确定的干扰之后,和不考虑这一因素相比,状态的期望不变,只是协方差产生变化,即
到此,先验估计的过程结束,总结一下,形成如下的公式:
重点来了:
先验估计
2.后验估计(量测更新)
到此,利用
在这里需要说明一下,卡尔曼滤波器的观测系统的维数小于等于动态系统的维数,即观测量可以少于动态系统中状态向量所包含的元素个数。
注意,传感器的输出值不一定就是我们创建的状态向量当中的元素,有时候需要进行一下简单的换算。即使是,有可能单位也不对应,所以,需要一个转换。这个转换就是矩阵
得到的公式如下:
那么,传感器对系统某些状态的测量真的准确吗?是不是也会有偏差呢?答案是肯定的。系统在某一个状态下,传感器输出一组观测值,当系统在另一个状态下,传感器会输出另外的观测值。反过来想,就是说根据读取到的观测向量,可以推测系统的实际状态。由于的存在,不同的系统状态造成当前的观测值的概率是不同的。所以,还需要一个观测噪声向量以及观测噪声协方差来衡量测量水平,我们将它们分别命名为
观测向量
下图看到的是两个云团,一个是状态预测值,另一个是观测值。那么到底哪一个具体的结果才是最好的呢?现在需要做的是对这两个结果进行合理的取舍,通过一种方法完成最终的卡尔曼预测。即:从图中粉红色云团和绿色云团当中找到一个最合适的点。
问题来了,怎么找?
首先来直观理解一下:考察观测向量
如果我们具有两个事件,如果都发生的话,从直觉或者是理性思维上讲,是不是认定两个事件发生就找到了那个最理想的估计值?好了,抽象一下,得到:两个事件同时发生的可能性越大,我们越相信它!要想考察它们同时发生的可能性,就是将两个事件单独发生的概率相乘。
那么,下一步就是对两个云团进行重叠,找到重叠最亮的点(实际上我们能够把云团看做一帧图像,这帧图像上面的每一个像素具有一个灰度值,灰度值大小代表的是该事件发生在这个点的概率密度),最亮的点,从直觉上面讲,就是以上两种预测准确的最大化可能性。也就是得到了最终的结果。非常神奇的事情是,对两个高斯分布进行乘法运算,得到新的概率分布规律仍然符合高斯分布,然后就取下图当中蓝色曲线峰值对应的横坐标不就是结果了嘛。证明如下:
我们考察单随机变量的高斯分布,期望为
如果存在两个这样的高斯分布,只不过期望和方差不同,当两个分布相乘,得到什么结果?
是不是下式成立
将(4)带入(5),对照(4)的形式,求得
其中,
以上是单变量概率密度函数的计算结果,如果是多变量的,那么,就变成了协方差矩阵的形式:
K称为卡尔曼增益,在下一步将会起到非常重要的作用。
好了,马上就要接近真相!
卡尔曼估计
下面就是对传感器的量测结果和根据k-1时刻预测得到的结果进行融合。(由于刚才的推导是两个单变量,并且处在同一个空间内,下面的推导为了方便起见,我们将先验状态估计对应的结果映射到观测向量空间进行统一的运算)
第一个要解决的问题是:(7)和(8)两个式子中那个平均值和方差都对应多少?
将(9)和(10)代入(6)、(7)、(8)三个式子,整理得到:
其中,卡尔曼增益的表达式为:
最后一步,更新结果:
对卡尔曼滤波原理的理解,我参考了这篇文章,图片取自该文章,该文的图片和公式颜色区分是一大亮点,在此表示对作者的感谢。
举个栗子
这部分重点讲解一下利用OpenCV如何实现一个对三维空间内物体的二维平面跟踪。
背景:跟踪一个移动速度大小和方向大致保持不变的物体,检测该物体的传感器是通过对物体拍摄连续帧图像,经过逐个像素的分析计算,得到x、y方向的速度,将两个速度构成一个二维的列向量作为观测向量。
下面看一下OpenCV当中对卡尔曼滤波的支持和提供的接口说明。
OpenCV2.4.13-KalmanFilter
下面是参与到卡尔曼滤波的一些数据结构,它们代表的意义在其后面用英文进行了描述。
OpenCV将以下的成员封装在了KalmanFilter当中,我们使用时候,可以直接实例化一个对象,例如:
KalmanFilter m_KF;
//CV_PROP_RW Mat statePre; //!< predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k)//CV_PROP_RW Mat statePost; //!< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))//CV_PROP_RW Mat transitionMatrix; //!< state transition matrix (A)//CV_PROP_RW Mat controlMatrix; //!< control matrix (B) (not used if there is no control)//CV_PROP_RW Mat measurementMatrix; //!< measurement matrix (H)//CV_PROP_RW Mat processNoiseCov; //!< process noise covariance matrix (Q)//CV_PROP_RW Mat measurementNoiseCov;//!< measurement noise covariance matrix (R)//CV_PROP_RW Mat errorCovPre; //!< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*///CV_PROP_RW Mat gain; //!< Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)//CV_PROP_RW Mat errorCovPost; //!< posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)
KalmanFilter类的成员函数具有如下几个:
KalmanFilterinitpredictcorrect
方法很简单,但是需要知道如何使用:
程序当中,我单独写了一个类,在我的计算线程(就是获取到量测结果的线程)当中对该类进行实例化并且调用其中的方法。量测结果存放在
m_dispacementVector[0] = m_xSpeed;
m_dispacementVector[1] = m_ySpeed;
当中。
步骤一:
CV_WRAP KalmanFilter( int dynamParams, int measureParams, int controlParams = 0, int type = CV_32F ); /** @brief Re-initializes Kalman filter. The previous content is destroyed. @param dynamParams Dimensionality of the state. @param measureParams Dimensionality of the measurement. @param controlParams Dimensionality of the control vector. @param type Type of the created matrices that should be CV_32F or CV_64F. */
在卡尔曼滤波类的构造函数成员列表当中首先告知OpenCV过程状态向量的维度和观测向量的维度:
m_KF(4,2,0)
对m_KF
成员的初始化:
1状态向量初始化xk
我想对物体的位置信息和速度信息进行跟踪,由于是二维的,所以位置信息x、y方向两个变量,速度信息x、y方向两个变量,从而m_KF.statePre
和m_KF.statePost
是一个四维列向量。
该向量在初始化时设置为零。
2状态转移矩阵初始化Fk
在计算机屏幕上面,我自定义了一个该物体的运动空间,具有横纵坐标,后面会看到这个空间。由于相机的帧率是30fps,所以相邻帧时间间隔
m_KF.transitionMatrix = (Mat_<float>(4, 4) << 1, 0, 0.03, 0,0,1,0,0.03,0,0,1,0,0,0,0,1);
3过程噪声激励协方差矩阵Qk
setIdentity(m_KF.processNoiseCov, Scalar::all(1e-5));//Q
认为过程激励噪声比较弱,并且每个分量相互之间不存在相关系。
4观测矩阵Hk
setIdentity(m_KF.measurementMatrix);
初始化得到:
由于传感器只是检测到了两个方向的速度,对位移没有检测,所以要将矩阵前两列初始化为0。
5预测估计协方差矩阵Pk
setIdentity(m_KF.errorCovPost, Scalar::all(1));
初始化为单位阵。
6测量噪声协方差矩阵Rk
setIdentity(m_KF.measurementNoiseCov, Scalar::all(1e-1));//R
步骤二:
先验估计:
m_prediction = m_KF.predict();
步骤三:
后验估计:
首先需要告知卡尔曼滤波器最新的传感器数据:
m_dispacementVector[0] = m_xSpeed; m_dispacementVector[1] = m_ySpeed;
m_pKalman->updateMeasurements(m_dispacementVector);
void kalmanFilter::updateMeasurements(double p[]){ m_measurement = (Mat_<float>(2, 1) << p[0], p[1]);}
接着完成后验估计:
m_KF.correct(m_measurement);
KalmanFilter.h:
#include <QObject>#include "opencv2/video/tracking.hpp"#include "opencv2/highgui/highgui.hpp"using namespace cv;class kalmanFilter:public QObject{ Q_OBJECTpublic: kalmanFilter(); ~kalmanFilter(); void initKalman(); void kalmanPredict(); void updateMeasurements(double p[]); void kalmamCorrect(); double *returnResult(); void drawArrow(Point start, Point end, Scalar color, int alpha, int len);private: KalmanFilter m_KF; Mat m_state; Mat m_postCorrectionState; Mat m_processNoise; Mat m_measurement; Mat m_img; Mat m_prediction; Point2f m_PointCenter;};
KalmanFilter.cpp:
#include "kalmanFilter.h"#include <iostream>#include <fstream>//CV_PROP_RW Mat statePre; //!< predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k)//CV_PROP_RW Mat statePost; //!< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))//CV_PROP_RW Mat transitionMatrix; //!< state transition matrix (A)//CV_PROP_RW Mat controlMatrix; //!< control matrix (B) (not used if there is no control)//CV_PROP_RW Mat measurementMatrix; //!< measurement matrix (H)//CV_PROP_RW Mat processNoiseCov; //!< process noise covariance matrix (Q)//CV_PROP_RW Mat measurementNoiseCov;//!< measurement noise covariance matrix (R)//CV_PROP_RW Mat errorCovPre; //!< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*///CV_PROP_RW Mat gain; //!< Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)//CV_PROP_RW Mat errorCovPost; //!< posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)using namespace cv;using namespace std;kalmanFilter::kalmanFilter():m_KF(4,2,0), m_state(4,1,CV_32F), m_processNoise(4, 1, CV_32F), m_measurement(2,1,CV_32F), m_img(300, 300, CV_8UC3), m_PointCenter(m_img.cols*0.5f, m_img.rows){ m_KF.transitionMatrix = (Mat_<float>(4, 4) << 1, 0, 0.03, 0,0,1,0,0.03,0,0,1,0,0,0,0,1);//A setIdentity(m_KF.measurementMatrix); setIdentity(m_KF.processNoiseCov, Scalar::all(1e-5));//Q setIdentity(m_KF.measurementNoiseCov, Scalar::all(1e-1));//R setIdentity(m_KF.errorCovPost, Scalar::all(1));}kalmanFilter::~kalmanFilter(){}void kalmanFilter::initKalman(){ m_state = (Mat_<float>(4, 1) << 0,0,0,0); m_KF.statePre = (Mat_<float>(4, 1) << m_PointCenter.x, m_PointCenter.y, 0, 0); m_KF.statePost = (Mat_<float>(4, 1) << m_PointCenter.x, m_PointCenter.y, 0, 0); m_KF.measurementMatrix = (Mat_<float>(2, 4) << 0,0, 1, 0,0,0,0,1);}void kalmanFilter::updateMeasurements(double p[]){ m_measurement = (Mat_<float>(2, 1) << p[0], p[1]);}void kalmanFilter::kalmanPredict(){ m_prediction = m_KF.predict();}void kalmanFilter::kalmamCorrect(){ m_KF.correct(m_measurement); m_postCorrectionState = m_KF.statePost; //show the result m_img = Scalar::all(0); //measured result drawArrow(Point(m_KF.statePost.at<float>(0, 0), m_KF.statePost.at<float>(1, 0)), Point(m_KF.statePost.at<float>(0, 0) + m_measurement.at<float>(0, 0), m_KF.statePost.at<float>(1, 0) + m_measurement.at<float>(1, 0)), Scalar(0, 0, 255), 20, 15);//B G R putText(m_img, "measurement", Point(m_KF.statePost.at<float>(0, 0) + m_measurement.at<float>(0, 0), m_KF.statePost.at<float>(1, 0) + m_measurement.at<float>(1, 0)-5), FONT_HERSHEY_PLAIN, 1, Scalar(0, 0, 255), 2); //predicted result drawArrow(Point(m_KF.statePost.at<float>(0, 0), m_KF.statePost.at<float>(1, 0)), Point(m_KF.statePost.at<float>(0, 0) + m_KF.statePost.at<float>(2, 0), m_KF.statePost.at<float>(1, 0) + m_KF.statePost.at<float>(3, 0)), Scalar(0,255, 0), 20, 15); putText(m_img, "Kalman", Point(m_KF.statePost.at<float>(0, 0) + m_KF.statePost.at<float>(2, 0)-10, m_KF.statePost.at<float>(1, 0) + m_KF.statePost.at<float>(3, 0)-5), FONT_HERSHEY_PLAIN, 1, Scalar(0, 255, 0), 2); imshow("Kalman", m_img); ofstream myfile("example.txt", ios::app); myfile << "measure" << " "<<m_measurement.at<float>(0, 0) << " " << m_measurement.at<float>(1, 0)<<" "; myfile << "kalman" << " " << m_KF.statePost.at<float>(2, 0) << " " << m_KF.statePost.at<float>(3, 0) << endl;}double * kalmanFilter::returnResult(){ double result[4]; for (int i = 0; i < 4; i++) { result[i] = m_postCorrectionState.at<double>(i, 1); } return result;}void kalmanFilter::drawArrow(Point start, Point end, Scalar color,int alpha,int len){ const double PI = 3.1415926; Point arrow; double angle = atan2((double)(start.y - end.y), (double)(start.x - end.x)); line(m_img, start, end, color,2); arrow.x = end.x + len * cos(angle + PI * alpha / 180); arrow.y = end.y + len * sin(angle + PI * alpha / 180); line(m_img, end, arrow, color,1); arrow.x = end.x + len * cos(angle - PI * alpha / 180); arrow.y = end.y + len * sin(angle - PI * alpha / 180); line(m_img, end, arrow, color,1);}
下面是录制的程序运行效果图(抱歉物体未显示):
参考资料:
卡尔曼滤波器OpenCV自带例子
教程
卡尔曼滤波的直觉理解
- 卡尔曼滤波原理及实现
- 卡尔曼滤波原理及实例
- 卡尔曼滤波原理
- 卡尔曼滤波原理
- 卡尔曼滤波原理
- 卡尔曼滤波原理
- 卡尔曼滤波原理
- 卡尔曼滤波算法原理
- 卡尔曼滤波原理说明
- 卡尔曼滤波算法原理
- 卡尔曼滤波原理说明
- 卡尔曼滤波原理说明
- 卡尔曼滤波的原理
- 卡尔曼滤波的原理
- 详解卡尔曼滤波原理
- 扩展卡尔曼滤波的原理及应用
- 《卡尔曼滤波原理及应用-MATLAB仿真》程序-2.1
- 《卡尔曼滤波原理及应用-MATLAB仿真》程序-2.2
- Spring Data —— 完全统一的API?
- poj2017
- BLE Gatt Date Time Service Implimentation in nRF51/52
- 使用hive查询hdfs数据时报字段类型异常
- [硕.Love Python] QuickSort(快速排序)
- 卡尔曼滤波原理及实现
- poj2027
- LabVIEW崩溃关闭:Aeccess violation (0xC0000005)解决方法
- 串行和并行的区别
- 常用Linux命令小记
- poj2028
- 数据结构之 平衡二叉树的建立
- poj1258 Agri-Net
- 文件后缀名怎么显示