卡尔曼滤波器：
卡尔曼滤波是一种递归的估计，即只要获知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值，因此不需要记录观测或者估计的历史信息。卡尔曼滤波器与大多数滤波器不同之处，在于它是一种纯粹的时域滤波器，它不需要像低通滤波器等频域滤波器那样，需要在频域设计再转换到时域实现。
卡尔曼滤波器的状态由以下两个变量表示：
- ，在时刻k 的状态的估计；
- ，误差相关矩阵，度量估计值的精确程度。
对于一个动态系统，我们首先定义一组状态空间方程
状态方程： 
测量方程： 
Xk： k时刻系统状态向量
Ak： 状态转移矩阵，对应opencv里kalman滤波器的transitionMatrix矩阵（关于opencv kalman滤波器详细定义会在2中给出）
Bk： 控制输入矩阵，对应opencv里kalman滤波器的controlMatrix矩阵
U(k)： k时刻对系统的控制向量
Z(k)： k时刻的测量值向量
Hk： 系统测量矩阵，对应opencv里kalman滤波器的measurementMatrix矩阵
W(k)：系统过程噪声，为高斯白噪声，协方差为Q，对应opencv里的kalman滤波器的processNoiseCov矩阵
V(k)： 测量噪声，也为高斯白噪声，协方差为R，对应opencv里的kalman滤波器的measurementNoiseCov矩阵
一般情况下：W(k)和V(k)都是高斯噪声，即
整个卡尔曼滤波的过程就是个递推计算的过程，不断的“预测——更新——预测——更新……”
卡尔曼滤波器的操作包括两个阶段：预测与更新。在预测阶段，滤波器使用上一状态的估计，做出对当前状态的估计。在更新阶段，滤波器利用对当前状态的观测值优化在预测阶段获得的预测值，以获得一个更精确的新估计值。
预测 predict:
//预测 predict:
Mat prediction = KF.predict(); 预测状态值： 
（1） 预测最小均方误差： 
（2）式(1)-(2)：预测值的计算
式(1)： 计算基于k-1时刻状态对k时刻系统状态的预测值
： 基于k-1时刻状态对k时刻状态的预测值，对应opencv里kalman滤波器的predict()输出，即statePre矩阵
：k-1时刻状态的最优结果，对应opencv里kalman滤波器的上一次状态的statePost矩阵
u(k)： k时刻的系统控制量，无则为0
Ak： 状态转移矩阵，对应opencv里kalman滤波器的transitionMatrix矩阵
Bk： 控制输入矩阵，对应opencv里kalman滤波器的controlMatrix矩阵
式(2)：计算X(k|k-1)对应的协方差的预测值
： 基于k-1时刻的协方差计算k时刻协方差的预测值，对应opencv里kalman滤波器的errorCovPre矩阵
：k-1时刻协方差的最优结果，对应opencv里kalman滤波器的上一次状态的errorCovPost矩阵
Q： 系统过程噪声协方差，对应opencv里kalman滤波器的processNoiseCov矩阵
更新过程 updata:
KF.correct(measurement); 测量误差：
测量协方差：
最优卡尔曼增益：
（3） 修正更新的状态估计值：
(4) 修正最小均方误差：
(更新的协方差估计)式(3):增益的计算
K(k)： k时刻的kalman增益，为估计量的方差占总方差(估计量方差和测量方差
)的比重，对应opencv里kalman滤波器的gain矩阵
Hk： 系统测量矩阵，对应opencv里kalman滤波器的measurementMatrix矩阵
Rk： 测量噪声协方差，对应opencv里的kalman滤波器的measurementNoiseCov矩阵
式(4)--(5):k时刻的更新
式(4)：计算k时刻系统状态最优值 
：k时刻系统状态的最优结果，对应opencv里kalman滤波器的k时刻状态的statePost矩阵Z(k)：k时刻系统测量值
式(5)：计算k时刻系统最优结果对应的协方差
：k时刻系统最优结果对应的协方差，对应opencv里kalman滤波器的errorCovPost矩阵
以上便是Kalman的核心部分，运行(1)(2)-(3)-(4)(5)-(1)(2)......输出即为，k时刻系统状态最优估计值。
运行流程见下图所示:
OpenCV中的KalmanFilter详解
OpenCV中有两个版本的卡尔曼滤波方法KalmanFilter(C++)和CvKalman(C)，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; //测量矩阵 H
Mat processNoiseCov; //系统误差 Q
Mat measurementNoiseCov; //测量误差 R
Mat 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
};Parameters:
1. dynamParams – Dimensionality of the state.
2. measureParams – Dimensionality of the measurement.
3. controlParams – Dimensionality of the control vector.
4. type – Type of the created matrices that should be CV_32F or CV_64FKalmanFilter类有四个方法:
- 构造KF对象KalmanFilter(DP,MP,CP)
- 初始化KF对象init(DP,MP,CP)
- 预测predict( )
- 更新correct( )
KalmanFilter(DP,MP,CP)和init( )就是赋值，此外除非你要重新构造KF对象，否则用不到init( )。。
注意：KalmanFilter结构体中并没有测量值，测量值需要自己定义，而且一定要定义，因为后面要用。
编程步骤：
step1：定义KalmanFilter类并初始化
//构造KF对象
int dynamParams = 4;//状态数，这里鼠标坐标X,Y和速度DX,DY
int measureParams = 2;//观测量数量，这里只写了X,Y
KalmanFilter KF(dynamParams,measureParams,0);//第三个为控制量参数，这里没有//初始化相关参数
KF.transitionMatrix = *(Mat_<float>(4, 4) <<
1, 0, 1, 0,
0, 1, 0, 1,
0, 0, 1, 0,
0 ,0, 0, 1);setIdentity(KF.measurementMatrix); //测量矩阵H
setIdentity(KF.processNoiseCov,Scalar::all(1e-5)); //过程噪声协方差矩阵Q，标准差为1e-5
setIdentity(KF.measurementNoiseCov,Scalar::all(1e-1));//测量噪声协方差矩阵R，标准差为1e-1
setIdentity(KF.errorCovPost,Scalar::all(1)); //P(1|1),估计协方差矩阵P
Mat state(dynamParams, 1, CV_32F);
Mat processNoise(dynamParams, 1, CV_32F);
Mat measurement = Mat::zeros(measureParams, 1, CV_32F); // 定义初始测量值 z(0)
randn(KF.statePost,Scalar::all(0),Scalar::all(0.1));//系统初始状态值X(1|1)
randn(state,Scalar::all(0),Scalar::all(0.1));step2：预测
KF.predict( ) //返回的是下一时刻的状态值KF.statePost (k+1)
step3：更新
更新measurement; //注意measurement不能通过观测方程进行计算得到，要自己定义！
更新KF KF.correct(measurement) //最终的结果应该是更新后的statePost.
Mat prediction = KF.predict();
Point predictPt = Point(prediction.at<float>(0),prediction.at<float>(1));
measurement.at<float>(0) = (float)mousepoint.x;//自定义测量值
measurement.at<float>(1) = (float)mousepoint.y;//自定义测量值
KF.correct(measurement);
Point statePt = Point(KF.statePost.at<float>(0),KF.statePost.at<float>(1));//最终的结果应该是更新后的statePost.得到GIF效果如下：
currentpoint为setMouseCallback获取鼠标值
predictpoint为基于上一次最优结果计算的预测坐标值，即X(k|k-1)
Kalmanpoint为现在最优结果坐标，即X(k|k)
实例
1 、OpenCV自带的示例程序
#include <opencv2\video\tracking.hpp>
#include <opencv2\highgui\highgui.hpp>
#include <stdio.h>
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("\nExample 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);
Mat state(2, 1, CV_32F); /* (phi, delta_phi) */
Mat processNoise(2, 1, CV_32F);
Mat measurement = Mat::zeros(1, 1, CV_32F);
char code = (char)-1;
for (;;)
{
randn(state, Scalar::all(0), Scalar::all(0.1));
KF.transitionMatrix = *(Mat_<float>(2, 2) << 1, 1, 0, 1);
setIdentity(KF.measurementMatrix);
setIdentity(KF.processNoiseCov, Scalar::all(1e-5));
setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));
setIdentity(KF.errorCovPost, Scalar::all(1));
randn(KF.statePost, Scalar::all(0), Scalar::all(0.1));
for (;;)
{
Point2f center(img.cols*0.5f, img.rows*0.5f);
float R = img.cols / 3.f;
double stateAngle = state.at<float>(0);
Point statePt = calcPoint(center, R, stateAngle);
Mat prediction = KF.predict();
double predictAngle = prediction.at<float>(0);
Point predictPt = calcPoint(center, R, predictAngle);
randn(measurement, Scalar::all(0), Scalar::all(KF.measurementNoiseCov.at<float>(0)));
// generate measurement
measurement += KF.measurementMatrix*state;
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);
if (theRNG().uniform(0, 4) != 0)
KF.correct(measurement);
randn(processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(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;
}程序结果:
2、跟踪鼠标位置
#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 setup
const 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); //转移矩阵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)); //后验错误估计协方差矩阵P
rng.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 prediction
Mat prediction = KF.predict();
Point predict_pt = Point(prediction.at<float>(0),prediction.at<float>(1) ); //预测值(x',y')
//3.update measurement
measurement.at<float>(0) = (float)mousePosition.x;
measurement.at<float>(1) = (float)mousePosition.y;
//4.update
KF.correct(measurement);
//draw
image.setTo(Scalar(255,255,255,0));
circle(image,predict_pt,5,Scalar(0,255,0),3); //predicted point with green
circle(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;
}
}
}结果：
