本博主的QQ：1154911397，欢饮大家与我交流！\(^o^)/~
转载请说明出处：http://blog.csdn.net/zhjm07054115/article/details/24735981
关于卡尔曼滤波器的原理，请看我的博客：卡尔曼滤波器原理
这篇博文主要给各位分析一下OpenCV2.x版本中的卡尔曼滤波器的实现代码。代码中的公式与我的博文卡尔曼滤波器原理中的公式是一致的，你可以参照。
首先看看KalmanFilter类的类型定义：（头文件）
/*!
卡尔曼滤波器类的声明.
该文件声明了标准卡尔曼滤波器类： \url{http://en.wikipedia.org/wiki/Kalman_filter}.
但是, 你可以自己修改 KalmanFilter::transitionMatrix, KalmanFilter::controlMatrix and
KalmanFilter::measurementMatrix 来获得扩展卡尔曼滤波器的功能.
说明：在该文件中，我只是将OpenCV中的KalmanFilter类名替换成了MyKalmanFilter,
代码内容没有变动，这么做只是为了避免自定义的滤波器类与OpenCV命名空间中的类重名
*/
#include
#include
#include
using namespace cv;
class CV_EXPORTS_W MyKalmanFilter
{
public:
//! 默认构造函数
CV_WRAP MyKalmanFilter();
//! 完全构造函数 参数：状态向量、观测矩阵、控制矩阵的维数
CV_WRAP MyKalmanFilter(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F);
//! 重新初始化滤波器. 以前的内容将被销毁.
void init(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F);
//! 计算状态的一步预测值，给出状态的先验估计
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; //!< 观测矩阵 (H)
Mat processNoiseCov; //!< 过程噪声的协方差矩阵 (Q)
Mat measurementNoiseCov;//!< 观测噪声的协方差矩阵 (R)
Mat errorCovPre; //!< 先验估计值的误差的协方差矩阵 (P'(k)): P'(k)=A*P(k-1)*At + Q)*/
Mat gain; //!< Kalman 增益矩阵 (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; temp2; temp3; temp4; temp5; }; < code>
接着看看KalmanFilter类的实现文件（实现文件）
/*!
卡尔曼滤波器类的实现.
该类实现了标准卡尔曼滤波器类： \url{http://en.wikipedia.org/wiki/Kalman_filter}.
但是, 你可以自己修改 KalmanFilter::transitionMatrix, KalmanFilter::controlMatrix and
KalmanFilter::measurementMatrix 来获得扩展卡尔曼滤波器的功能.
说明：在该文件中，我只是将OpenCV中的KalmanFilter类名替换成了MyKalmanFilter,
代码内容没有变动，这么做只是为了避免自定义的滤波器类与OpenCV命名空间中的类重名
*/
#include "MyKalmanFilter.h"
/*默认构造函数*/
MyKalmanFilter::MyKalmanFilter()
{
}
/**完整构造函数
dynamParams：状态向量的维数
measureParams：观测矩阵的维数
controlParams：控制器的维数
type：矩阵数据类型：默认CV_32F
*/
MyKalmanFilter::MyKalmanFilter(int dynamParams, int measureParams, int controlParams, int type)
{
init(dynamParams, measureParams, controlParams, type);
}
//!初始化滤波器. 以前的内容将被销毁.
void MyKalmanFilter::init(int DP, int MP, int CP, int type)
{
CV_Assert( DP > 0 && MP > 0 );
CV_Assert( type == CV_32F || type == CV_64F );
CP = std::max(CP, 0);
statePre = Mat::zeros(DP, 1, type);//!< 状态的一步预测值（先验估计值），DP×1维
statePost = Mat::zeros(DP, 1, type);//!< 更新之后的状态估计值（后验估计值），DP×1维
transitionMatrix = Mat::eye(DP, DP, type);//!< 状态转移矩阵 (A)，DP×DP维
processNoiseCov = Mat::eye(DP, DP, type);//!< 过程噪声的协方差矩阵(Q)，DP×DP维
measurementMatrix = Mat::zeros(MP, DP, type);//!< 观测矩阵 (H):MP×DP维
measurementNoiseCov = Mat::eye(MP, MP, type);//!< 观测噪声的协方差矩阵(R)：MP×MP维
errorCovPre = Mat::zeros(DP, DP, type); //!< 先验估计值的误差的协方差矩阵(P'(k)):DP×DP维
errorCovPost = Mat::zeros(DP, DP, type); //!
<后验估计值的误差的协方差矩阵 (p(k)):dp×dp维 gain="Mat::zeros(DP," mp, type); !< kalman 增益矩阵 (k(k)): dp×mp维 if( cp>
0 )
controlMatrix = Mat::zeros(DP, CP, type); //!< 控制矩阵 (B) :DP×CP维
else
controlMatrix.release();
temp1.create(DP, DP, type);
temp2.create(MP, DP, type);
temp3.create(MP, MP, type);
temp4.create(MP, DP, type);
temp5.create(MP, 1, type);
}
//计算状态的一步预测值，给出状态的先验估计
const Mat& MyKalmanFilter::predict(const Mat& control)
{
// 更新状态: x'(k) = A*x(k)
statePre = transitionMatrix*statePost;//将上一时刻的后验估计值根据状态转移方程转移到当前时刻
if( control.data )//如果有控制量的作用，则当前时刻的先验状态值还要叠加上当前时刻的控制量
// x'(k) = x'(k) + B*u(k)
statePre += controlMatrix*control;
// 更新先验估计值的误差的协方差矩阵 P'(k)：分两步估计errorCovPre
//第一步：将上一时刻后验误差的协方差阵通过状态转移矩阵传递到当前时刻：temp1 = A*P(k)
temp1 = transitionMatrix*errorCovPost;
//第二步：加上当前时刻的过程噪声矩阵 P'(k) = temp1*At + Q （At是A的转置）
gemm(temp1, transitionMatrix, 1, processNoiseCov, 1, errorCovPre, GEMM_2_T);
//处理这种情况：就是在下一次进行预测之前没有观测值的情况.
//也就是说在没有观测的情况下，该函数也能正常递推运行，不过只有不断地预测，没有观测校正过程
statePre.copyTo(statePost);//看看该函数的第一句就明白这一句了
return statePre; //返回当前时刻状态的先验估计值
}
//依据观测数据和观测误差更新状态的一步预测值，给出状态的后验估计值
const Mat& MyKalmanFilter::correct(const Mat& measurement)
{
// temp2 = H*P'(k)：将先验误差协方差矩阵通过观测矩阵传递到观测端
temp2 = measurementMatrix * errorCovPre;
// temp3 = temp2*Ht + R ：后验误差协方差 = 先验误差协方差通过观测矩阵传递到观测端 + 当前时刻的观测误差
gemm(temp2, measurementMatrix, 1, measurementNoiseCov, 1, temp3, GEMM_2_T);
// temp4 = inv(temp3)*temp2 = Kt(k)：滤波增益的转置 其实就是 先验误差方差与（先验误差方差 + 后验误差方差）的比值
solve(temp3, temp2, temp4, DECOMP_SVD);
// 计算k时刻的增益矩阵：K(k)：增益其实就是在先验和后验之间权衡利弊的权重系数，它的大小取决于先验误差方差和后验误差方差
gain = temp4.t();//后验误差方差越大，即表示观测数据越不可靠，则增益就越小，此时我们更应该相信先验估计值
// 真实观测值measurement与先验观测值之间的误差：
temp5 = measurement - measurementMatrix*statePre; //temp5 = z(k) - H*x'(k)
//当前时刻状态的后验估计值：==该时刻的先验估计值+真实观测值带来的误差乘以增益。
statePost = statePre + gain*temp5;// x(k) = x'(k) + K(k)*temp5
//更新状态的后验估计值的误差协方差矩阵 P(k) = P'(k) - K(k)*temp2
errorCovPost = errorCovPre - gain*temp2;
return statePost;
}
接着看看MAIN函数的测试实现吧：
下面是以上测试实例的代码实现：
/**
OpenCV2.4.5 Kalman Filter算法的学习
*/
#include
#include
#include
#include "MyKalmanFilter.h"
using namespace std;
using namespace cv;
//返回以给定的圆心和半径以及圆心角的圆周上的点
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( "\n OpenCV中 Kalman filter的例子.\n"
" 跟踪一个不断旋转的点：用卡尔曼滤波预测当前点的位置.\n"
" 旋转速度是固定的.\n"
" 状态向量和观测向量都是1维的，就是被跟踪的点的圆心角,\n"
" 角度的观测值等于真实的角度值加上高斯噪声.\n"
" 点的真实位置和估计位置用黄色的线段连起来,\n"
" 点的真实位置和观测位置用红色的线段连起来.\n"
" (如果kalman滤波器的工作是正常的,\n"
" 黄的线段应该比红色线段短).\n"
"\n"
" 除了ESC键之外，按下任何键都会使得点的速度加快.\n"
" 按ESC键退出程序.\n"
);
}
int main()
{
printf("OpenCV2.4.5中 Kalman Filter算法的学习!!!\n");
help();
Mat img(500, 500, CV_8UC3);
MyKalmanFilter KF(2, 1, 0); //声明一个kalman 滤波器的类的对象实例
Mat state(2, 1, CV_32F); /* 状态矩阵：2个状态，角度以及角度的差分(phi, delta_phi) */
Mat processNoise(2, 1, CV_32F); //过程噪声矩阵
Mat measurement = Mat::zeros(1, 1, CV_32F);//观测值，只能观测到phi,观测不到delta_phi
char code = (char)-1;//控制被跟踪点绕圆周运动的速度的标志
for(;;)
{
//根据指定的均值与标准差，产生高斯分布的随机状态向量
randn( state, Scalar::all(0), Scalar::all(0.1) );
//状态转移矩阵，因为有两个状态，所以是2×2维的，[1 1; 0 1]
KF.transitionMatrix = *(Mat_
(2, 2) << 1, 1, 0, 1);//把数组赋值给矩阵，还可以这样写啊
//cout<
<
(0);//状态向量中的第一个状态为角度 Point statePt = calcPoint(center, R, stateAngle);//将角度值转换为圆周上的对应点 //一步预测，得到当前这时刻状态的先验估计值 Mat prediction = KF.predict(); //得到 KF.statePre double predictAngle = prediction.at
(0);//取出先验估计的角度值 Point predictPt = calcPoint(center, R, predictAngle);//先验估计的角度值对应的圆周位置点 //生成当前时刻的观测值中的观测噪声 randn( measurement, Scalar::all(0), Scalar::all(KF.measurementNoiseCov.at
(0))); // 生成当前时刻的观测值 z = H*x + R,观测矩阵H = 1,观测噪声R属于N(0,1e-1)分布,所以 z = angle + N(0,1e-1) measurement += KF.measurementMatrix*state; //取出观测向量中的角度 double measAngle = measurement.at
(0); Point measPt = calcPoint(center, R, measAngle);//计算观测到的角度值对应的圆周位置点 // 画这种类型的“×”点的宏定义 #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 );//从上一时刻的位置到当前一步预测的位置的连线 if(theRNG().uniform(0,4) != 0) KF.correct(measurement); //利用观测值去修正先验估计值，得到真正的后验估计值 //产生过程噪声，过程噪声Q属于二维高斯分布，每一维都是零均值，标准差是1e-5. randn( processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at
(0, 0)))); //产生下一时刻的状态 state = 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; }
接着看看main函数的运行效果吧：
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
