OpenCV--卡尔曼滤波(KalmanFilter)详解【转载】
本文将简要回顾一下卡尔曼滤波理论，然后详细介绍如何在OpenCV中使用卡尔曼滤波进行跟踪，最后给两个程序实例。
1. 卡尔曼滤波理论回顾
对于一个动态系统，我们首先定义一组状态空间方程
状态方程：
测量方程：
xkx_kxk是状态向量，zkz_kzk是测量向量，AkA_kAk是状态转移矩阵，uku_kuk是控制向量，BkB_kBk是控制矩阵，wkw_kwk是系统误差（噪声），HkH_kHk是测量矩阵，vkv_kvk是测量误差（噪声）。wkw_kwk和vkv_kvk都是高斯噪声，即
整个卡尔曼滤波的过程就是个递推计算的过程，不断的“预测——更新——预测——更新……”
1.1 预测
预测状态值：
预测最小均方误差：
1.2 更新
测量误差：
测量协方差：
最优卡尔曼增益：
修正状态值：
修正最小均方误差：
2.OpenCV中的KalmanFilter详解
OpenCV中有两个版本的卡尔曼滤波方法KalmanFilter(C++)和CvKalman©，用法差不太多，这里只介绍KalmanFilter。
C++版本中将KalmanFilter封装到一个类中，其结构如下所示：
class CV_EXPORTS_W KalmanFilter
{public: CV_WRAP KalmanFilter(); //构造默认KalmanFilter对象CV_WRAP KalmanFilter(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F); //完整构造KalmanFilter对象方法void init(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F); //初始化KalmanFilter对象，会替换原来的KF对象CV_WRAP const Mat& predict(const Mat& control=Mat()); //计算预测的状态值 CV_WRAP const Mat& correct(const Mat& measurement); //根据测量值更新状态值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))Mat transitionMatrix; //状态转移矩阵 (A)Mat controlMatrix; //控制矩阵 B Mat measurementMatrix; //测量矩阵 HMat processNoiseCov; //系统误差 QMat measurementNoiseCov; //测量误差 RMat errorCovPre; //最小均方误差 (P'(k)): P'(k)=A*P(k-1)*At + Q)Mat gain; //卡尔曼增益 (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)Mat errorCovPost; //修正的最小均方误差 (P(k)): P(k)=(I-K(k)*H)*P'(k)// 临时矩阵Mat temp1;Mat temp2;Mat temp3;Mat temp4;Mat temp5;
};enum
{OPTFLOW_USE_INITIAL_FLOW = CV_LKFLOW_INITIAL_GUESSES,OPTFLOW_LK_GET_MIN_EIGENVALS = CV_LKFLOW_GET_MIN_EIGENVALS,OPTFLOW_FARNEBACK_GAUSSIAN = 256
};
函数原型见：……\OpenCV2\sources\modules\ocl\src\kalman.cpp
只有四个方法: 构造KF对象KalmanFilter(DP,MP,CP)、初始化KF对象init(DP,MP,CP)、预测predict( )、更新correct( )。除非你要重新构造KF对象，否则用不到init( )。
KalmanFilter(DP,MP,CP)和init( )就是赋值，没什么好说的。
注意：KalmanFilter结构体中并没有测量值，测量值需要自己定义，而且一定要定义，因为后面要用。
编程步骤
step1：定义KalmanFilter类并初始化//构造KF对象KalmanFilter KF(DP, MP, 0);//初始化相关参数KF.transitionMatrix 转移矩阵 AKF.measurementMatrix 测量矩阵 HKF.processNoiseCov 过程噪声 QKF.measurementNoiseCov 测量噪声 RKF.errorCovPost 最小均方误差 PKF.statePost 系统初始状态 x(0) Mat measurement 定义初始测量值 z(0) step2：预测KF.predict( ) //返回的是下一时刻的状态值KF.statePost (k+1) step3：更新更新measurement; //注意measurement不能通过观测方程进行计算得到，要自己定义！更新KF KF.correct(measurement)最终的结果应该是更新后的statePost.
相关参数的确定
对于系统状态方程，简记为Y=AX+B，X和Y是表示系统状态的列向量，A是转移矩阵，B是其他项。
状态值（向量）只要能表示系统的状态即可，状态值的维数决定了转移矩阵A的维数，比如X和Y是N×1的，则A是N×N的。
A的确定跟X有关，只要保证方程中不相干项的系数为0即可，看下面例子
X和Y是二维的，
X和Y是三维的，
X和Y是三维的，但c和△ c是相关项
上面的1也可以是其他值
下面对predict( ) 和correct( )函数介绍下，可以不用看，不影响编程。
CV_EXPORTS const oclMat& KalmanFilter::predict(const oclMat& control)
{gemm(transitionMatrix, statePost, 1, oclMat(), 0, statePre);oclMat temp;if(control.data)gemm(controlMatrix, control, 1, statePre, 1, statePre);gemm(transitionMatrix, errorCovPost, 1, oclMat(), 0, temp1);gemm(temp1, transitionMatrix, 1, processNoiseCov, 1, errorCovPre, GEMM_2_T);statePre.copyTo(statePost);return statePre;
}
gemm( )是矩阵的广义乘法
void gemm(const GpuMat& src1, constGpuMat& src2, double alpha, const GpuMat& src3, double beta,GpuMat& dst, int flags=0, Stream& stream=Stream::Null())
dst = alpha · src1 · src2 +beta· src3
上面，oclMat()其实是uku_kuk，只不过默认为0，所以没赋值。整个过程就计算了x′x'x′和P’P’P’。（用x′x'x′代表xxx的预测值，用P′P'P′代表PPP的预测值）。GEMM_2_T表示对第2个参数转置。
x’(k)=1⋅A⋅x(k−1)x’(k)=1·A·x(k-1)x’(k)=1⋅A⋅x(k−1)
如果B非空， x′(k)=1⋅B⋅u+1⋅x′(k−1)x'(k) = 1·B·u + 1·x'(k-1)x′(k)=1⋅B⋅u+1⋅x′(k−1)
temp1=1⋅A⋅P(k−1)+0⋅u(k)temp1 = 1·A·P(k-1) + 0·u(k)temp1=1⋅A⋅P(k−1)+0⋅u(k)
P’(k)=1⋅temp1⋅AT+1⋅Qk=A⋅P(k−1)⋅AT+1⋅QkP’(k) = 1· temp1·A^T + 1· Q_k= A·P(k-1)·A^T + 1· Q_kP’(k)=1⋅temp1⋅AT+1⋅Qk=A⋅P(k−1)⋅AT+1⋅Qk
可见，和第一部分的理论介绍完全一致。
CV_EXPORTS const oclMat& KalmanFilter::correct(const oclMat& measurement)
{CV_Assert(measurement.empty() == false);gemm(measurementMatrix, errorCovPre, 1, oclMat(), 0, temp2);gemm(temp2, measurementMatrix, 1, measurementNoiseCov, 1, temp3, GEMM_2_T);Mat temp;solve(Mat(temp3), Mat(temp2), temp, DECOMP_SVD);temp4.upload(temp);gain = temp4.t();gemm(measurementMatrix, statePre, -1, measurement, 1, temp5);gemm(gain, temp5, 1, statePre, 1, statePost);gemm(gain, temp2, -1, errorCovPre, 1, errorCovPost);return statePost;
}
bool solve(InputArray src1, InputArray src2, OutputArray dst, int flags=DECOMP_LU)
求解线型最小二乘估计
temp2=1⋅H⋅P’+0⋅u(k)temp2 = 1· H·P’ + 0·u(k)temp2=1⋅H⋅P’+0⋅u(k)
temp3=1⋅temp2⋅HT+1⋅R=H⋅P’⋅HT+1⋅R也就是上面的Sktemp3 = 1· temp2·H^T + 1·R = H·P’·H^T+ 1· R 也就是上面的S_ktemp3=1⋅temp2⋅HT+1⋅R=H⋅P’⋅HT+1⋅R也就是上面的Sk
temp=argmin∣∣tem2−temp3∣∣temp = argmin||tem2- temp3||temp=argmin∣∣tem2−temp3∣∣
K=tempK=tempK=temp
temp5=−1⋅H⋅x’+1⋅zk就是上面的y’。temp5 = -1· H·x’ + 1·z_k 就是上面的y’。temp5=−1⋅H⋅x’+1⋅zk就是上面的y’。
x=1⋅K⋅temp5+1⋅x’=KT⋅y’+x’x = 1·K·temp5 + 1·x’ = K^T·y’ +x’x=1⋅K⋅temp5+1⋅x’=KT⋅y’+x’
P=−1⋅K⋅temp2+1⋅P’=−K⋅H⋅P’+P’=(I−K⋅H)P’P =-1·K·temp2 + 1·P’ = -K·H·P’+P’ = (I- K·H) P’P=−1⋅K⋅temp2+1⋅P’=−K⋅H⋅P’+P’=(I−K⋅H)P’
也和第一部分的理论完全一致。
通过深入函数内部，学到了两个实用的函数哦。矩阵广义乘法gemm( )、最小二乘估计solve( )。
补充：
1）以例2为例，为什么状态值一般都设置成（x,y,△x,△y）？我们不妨设置成(x,y,△x)，对应的转移矩阵也改成3×3的。可以看到仍能跟上，不过在x方向跟踪速度快，在y方向跟踪速度慢。进一步设置成(x,y)和2×2的转移矩阵，程序的跟踪速度简直是龟速。所以，简单理解，△x和△y严重影响对应方向上的跟踪速度。
3.实例
例1 OpenCV自带的示例程序
#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <iostream>
#include <stdio.h>
using namespace std;
using namespace cv;//计算相对窗口的坐标值，因为坐标原点在左上角，所以sin前有个负号
static inline Point calcPoint(Point2f center, double R, double angle)
{return center + Point2f((float)cos(angle), (float)-sin(angle))*(float)R;
}static void help()
{printf( "\nExamle of c calls to OpenCV's Kalman filter.\n"
" Tracking of rotating point.\n"
" Rotation speed is constant.\n"
" Both state and measurements vectors are 1D (a point angle),\n"
" Measurement is the real point angle + gaussian noise.\n"
" The real and the estimated points are connected with yellow line segment,\n"
" the real and the measured points are connected with red line segment.\n"
" (if Kalman filter works correctly,\n"
" the yellow segment should be shorter than the red one).\n""\n"
" Pressing any key (except ESC) will reset the tracking with a different speed.\n"
" Pressing ESC will stop the program.\n");
}int main(int, char**)
{help();Mat img(500, 500, CV_8UC3);KalmanFilter KF(2, 1, 0); //创建卡尔曼滤波器对象KFMat state(2, 1, CV_32F); //state(角度，△角度)Mat processNoise(2, 1, CV_32F);Mat measurement = Mat::zeros(1, 1, CV_32F); //定义测量值char code = (char)-1;for(;;){//1.初始化randn( state, Scalar::all(0), Scalar::all(0.1) ); //KF.transitionMatrix = *(Mat_<float>(2, 2) << 1, 1, 0, 1); //转移矩阵A[1,1;0,1] //将下面几个矩阵设置为对角阵setIdentity(KF.measurementMatrix); //测量矩阵HsetIdentity(KF.processNoiseCov, Scalar::all(1e-5)); //系统噪声方差矩阵QsetIdentity(KF.measurementNoiseCov, Scalar::all(1e-1)); //测量噪声方差矩阵RsetIdentity(KF.errorCovPost, Scalar::all(1)); //后验错误估计协方差矩阵Prandn(KF.statePost, Scalar::all(0), Scalar::all(0.1)); //x(0)初始化for(;;){Point2f center(img.cols*0.5f, img.rows*0.5f); //center图像中心点float R = img.cols/3.f; //半径double stateAngle = state.at<float>(0); //跟踪点角度Point statePt = calcPoint(center, R, stateAngle); //跟踪点坐标statePt//2. 预测Mat prediction = KF.predict(); //计算预测值，返回x'double predictAngle = prediction.at<float>(0); //预测点的角度Point predictPt = calcPoint(center, R, predictAngle); //预测点坐标predictPt//3.更新//measurement是测量值randn( measurement, Scalar::all(0), Scalar::all(KF.measurementNoiseCov.at<float>(0))); //给measurement赋值N(0,R)的随机值// generate measurementmeasurement += KF.measurementMatrix*state; //z = z + H*x;double measAngle = measurement.at<float>(0);Point measPt = calcPoint(center, R, measAngle);// plot points//定义了画十字的方法，值得学习下#define drawCross( center, color, d ) \line( img, Point( center.x - d, center.y - d ), \Point( center.x + d, center.y + d ), color, 1, CV_AA, 0); \line( img, Point( center.x + d, center.y - d ), \Point( center.x - d, center.y + d ), color, 1, CV_AA, 0 )img = Scalar::all(0);drawCross( statePt, Scalar(255,255,255), 3 );drawCross( measPt, Scalar(0,0,255), 3 );drawCross( predictPt, Scalar(0,255,0), 3 );line( img, statePt, measPt, Scalar(0,0,255), 3, CV_AA, 0 );line( img, statePt, predictPt, Scalar(0,255,255), 3, CV_AA, 0 );//调用kalman这个类的correct方法得到加入观察值校正后的状态变量值矩阵if(theRNG().uniform(0,4) != 0)KF.correct(measurement);//不加噪声的话就是匀速圆周运动，加了点噪声类似匀速圆周运动，因为噪声的原因，运动方向可能会改变randn( processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0)))); //vkstate = KF.transitionMatrix*state + processNoise; imshow( "Kalman", img );code = (char)waitKey(100);if( code > 0 )break;}if( code == 27 || code == 'q' || code == 'Q' )break;}return 0;
}
运行结果
例2 跟踪鼠标位置
在我介绍粒子滤波的博文“学习Opencv2——粒子滤波Condensation算法”里，有个例3，是跟踪鼠标位置。现在我们用卡尔曼滤波来实现。
#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <stdio.h>
using namespace cv;
using namespace std;const int winHeight=600;
const int winWidth=800;Point mousePosition= Point(winWidth>>1,winHeight>>1);//mouse event callback
void mouseEvent(int event, int x, int y, int flags, void *param )
{if (event==CV_EVENT_MOUSEMOVE) {mousePosition = Point(x,y);}
}int main (void)
{RNG rng;//1.kalman filter setupconst int stateNum=4; //状态值4×1向量(x,y,△x,△y)const int measureNum=2; //测量值2×1向量(x,y) KalmanFilter KF(stateNum, measureNum, 0); KF.transitionMatrix = *(Mat_<float>(4, 4) <<1,0,1,0,0,1,0,1,0,0,1,0,0,0,0,1); //转移矩阵AsetIdentity(KF.measurementMatrix); //测量矩阵HsetIdentity(KF.processNoiseCov, Scalar::all(1e-5)); //系统噪声方差矩阵QsetIdentity(KF.measurementNoiseCov, Scalar::all(1e-1)); //测量噪声方差矩阵RsetIdentity(KF.errorCovPost, Scalar::all(1)); //后验错误估计协方差矩阵Prng.fill(KF.statePost,RNG::UNIFORM,0,winHeight>winWidth?winWidth:winHeight); //初始状态值x(0)Mat measurement = Mat::zeros(measureNum, 1, CV_32F); //初始测量值x'(0)，因为后面要更新这个值，所以必须先定义namedWindow("kalman");setMouseCallback("kalman",mouseEvent);Mat image(winHeight,winWidth,CV_8UC3,Scalar(0));while (1){//2.kalman predictionMat prediction = KF.predict();Point predict_pt = Point(prediction.at<float>(0),prediction.at<float>(1) ); //预测值(x',y')//3.update measurementmeasurement.at<float>(0) = (float)mousePosition.x;measurement.at<float>(1) = (float)mousePosition.y; //4.updateKF.correct(measurement);//draw image.setTo(Scalar(255,255,255,0));circle(image,predict_pt,5,Scalar(0,255,0),3); //predicted point with greencircle(image,mousePosition,5,Scalar(255,0,0),3); //current position with red char buf[256];sprintf_s(buf,256,"predicted position:(%3d,%3d)",predict_pt.x,predict_pt.y);putText(image,buf,Point(10,30),CV_FONT_HERSHEY_SCRIPT_COMPLEX,1,Scalar(0,0,0),1,8);sprintf_s(buf,256,"current position :(%3d,%3d)",mousePosition.x,mousePosition.y);putText(image,buf,cvPoint(10,60),CV_FONT_HERSHEY_SCRIPT_COMPLEX,1,Scalar(0,0,0),1,8);imshow("kalman", image);int key=waitKey(3);if (key==27){//esc break; } }
}
运行结果
例3
#include "opencv2/video/tracking.hpp"
#include <opencv2/legacy/legacy.hpp> //#include "cvAux.h"
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include <stdio.h>using namespace cv;
using namespace std;int main( )
{ float A[10][3] = {10, 50, 15.6,12, 49, 16,11, 52, 15.8,13, 52.2, 15.8,12.9, 50, 17,14, 48, 16.6,13.7, 49, 16.5,13.6, 47.8, 16.4,12.3, 46, 15.9,13.1, 45, 16.2}; const int stateNum=3;const int measureNum=3;KalmanFilter KF(stateNum, measureNum, 0); KF.transitionMatrix = *(Mat_<float>(3, 3) <<1,0,0,0,1,0,0,0,1); //转移矩阵A setIdentity(KF.measurementMatrix); //测量矩阵H setIdentity(KF.processNoiseCov, Scalar::all(1e-5)); //系统噪声方差矩阵Q setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1)); //测量噪声方差矩阵R setIdentity(KF.errorCovPost, Scalar::all(1)); Mat measurement = Mat::zeros(measureNum, 1, CV_32F); //初始状态值KF.statePost = *(Mat_<float>(3, 1) <<A[0][0],A[0][1],A[0][2]); cout<<"state0="<<KF.statePost<<endl;for(int i=1;i<=9;i++){//预测Mat prediction = KF.predict(); //计算测量值measurement.at<float>(0) = (float)A[i][0]; measurement.at<float>(1) = (float)A[i][1]; measurement.at<float>(2) = (float)A[i][2]; //更新KF.correct(measurement); //输出结果cout<<"predict ="<<"\t"<<prediction.at<float>(0)<<"\t"<<prediction.at<float>(1)<<"\t"<<prediction.at<float>(2)<<endl;cout<<"measurement="<<"\t"<<measurement.at<float>(0)<<"\t"<<measurement.at<float>(1)<<"\t"<<measurement.at<float>(2)<<endl;cout<<"correct ="<<"\t"<<KF.statePost.at<float>(0)<<"\t"<<KF.statePost.at<float>(1)<<"\t"<<KF.statePost.at<float>(2)<<endl;}system("pause");
}
运行结果
这里预测值和上一个状态值一样，原因是转移矩阵A是单位阵，如果改成非单位阵，结果就不一样了。
以上博文内容转载自：
- Markala_学习OpenCV2——卡尔曼滤波(KalmanFilter)详解【CSDN】
OpenCV--卡尔曼滤波(KalmanFilter)详解【转载】相关推荐
- 学习OpenCV2——卡尔曼滤波(KalmanFilter)详解
本文将简要回顾一下卡尔曼滤波理论,然后详细介绍如何在OpenCV中使用卡尔曼滤波进行跟踪,最后给两个程序实例. 1. 卡尔曼滤波理论回顾 对于一个动态系统,我们首先定义一组状态空间方程 状态方程: ...
- 【OpenCV 4开发详解】分割图像——分水岭法
本文首发于"小白学视觉"微信公众号,欢迎关注公众号 本文作者为小白,版权归人民邮电出版社发行所有,禁止转载,侵权必究! 经过几个月的努力,小白终于完成了市面上第一本OpenCV 4 ...
- 【OpenCV 4开发详解】QR二维码检测
本文首发于"小白学视觉"微信公众号,欢迎关注公众号 本文作者为小白,版权归人民邮电出版社发行所有,禁止转载,侵权必究! 经过几个月的努力,小白终于完成了市面上第一本OpenCV 4 ...
- 【OpenCV 4开发详解】深度神经网络应用实例
本文首发于"小白学视觉"微信公众号,欢迎关注公众号 本文作者为小白,版权归人民邮电出版社发行所有,禁止转载,侵权必究! 经过几个月的努力,小白终于完成了市面上第一本OpenCV 4 ...
- 【OpenCV 4开发详解】图像修复
本文首发于"小白学视觉"微信公众号,欢迎关注公众号 本文作者为小白,版权归人民邮电出版社发行所有,禁止转载,侵权必究! 经过几个月的努力,小白终于完成了市面上第一本OpenCV 4 ...
- 【OpenCV 4开发详解】分割图像——Mean-Shift分割算法
本文首发于"小白学视觉"微信公众号,欢迎关注公众号 本文作者为小白,版权归人民邮电出版社发行所有,禁止转载,侵权必究! 经过几个月的努力,小白终于完成了市面上第一本OpenCV 4 ...
- 【OpenCV 4开发详解】分割图像——Grabcut图像分割
本文首发于"小白学视觉"微信公众号,欢迎关注公众号 本文作者为小白,版权归人民邮电出版社发行所有,禁止转载,侵权必究! 经过几个月的努力,小白终于完成了市面上第一本OpenCV 4 ...
- 【OpenCV 4开发详解】漫水填充法
本文首发于"小白学视觉"微信公众号,欢迎关注公众号 本文作者为小白,版权归人民邮电出版社发行所有,禁止转载,侵权必究! 经过几个月的努力,小白终于完成了市面上第一本OpenCV 4 ...
- 【OpenCV 4开发详解】点集拟合
本文首发于"小白学视觉"微信公众号,欢迎关注公众号 本文作者为小白,版权归人民邮电出版社发行所有,禁止转载,侵权必究! 经过几个月的努力,小白终于完成了市面上第一本OpenCV 4 ...
最新文章
- SharePoint 2013 工作流之使用Visio设计篇
- 随机数生成算法-二谈
- python32bit改64bit,从32位Python更改64位注册表
- 【深度学习】Dropout、正反向传播、计算图等的介绍和实现（Pytorch）
- 知识点讲解八：Python中的尾递归
- 杨国福或夺“麻辣烫第一股”，是否名副其实？
- hdu 3065 AC自动机
- DGL_图的创建、保存、加载
- django配置随时执行的脚本，使用运行时的manage环境执行程序
- 移植qt常见错误修改方法
- 为提升管理效率 蔚来汽车美国办公室裁员70名
- kmp算法字符串匹配C语言实现
- 推荐系统（十六）多任务学习：腾讯PLE模型（Progressive Layered Extraction model）
- 微服务使用模块化时候导入公共模块导致maven更新依赖错误问题
- Cruehead-CrackMe-3
- 深信服上网行为管理设备登录方法、恢复出厂、恢复控制台密码
- 给Revit中的Button添加动画和图片
- 邯郸一中高考2021成绩查询,邯郸一中2019高考成绩喜报、本一本二上线人数情况...
- 计算FLT_MAX的整数位长度
- android view保存成背景,Android用VideoView实现MP4作为页面背景