图像处理之目标跟踪(一)之卡尔曼kalman滤波跟踪(主要为知识梳理)(转载)
内容不是一篇文章的转载而是几篇内容的综合梳理.
主要来源：
EmbeddedApp 运动目标跟踪（一）--搜索算法预测模型之KF,EKF,UKF（文献1，主要是原理的梳理，十分详细）
-Sherman- [opencv]Kalman滤波跟踪（文献2，主要是公式与opencv对应关系的讲解）
Markala 学习OpenCV2——卡尔曼滤波(KalmanFilter)详解（文献3，三个例子代码）
一、卡尔曼滤波的方程推导
直接从数学公式和概念入手来考虑卡尔曼滤波无疑是一件非常枯燥的事情。为了便于理解，我们仍然从一个现实中的实例开始下面的介绍，这一过程中你所需的预备知识仅仅是高中程度的物理学内容。
假如现在有一辆在路上做直线运动的小车（如下所示），该小车在 t 时刻的状态可以用一个向量来表示，其中pt 表示他当前的位置，vt表示该车当前的速度。当然，司机还可以踩油门或者刹车来给车一个加速度ut，ut相当于是一个对车的控制量。显然，如果司机既没有踩油门也没有踩刹车，那么ut就等于0。此时车就会做匀速直线运动。
如果我们已知上一时刻 t-1时小车的状态，现在来考虑当前时刻t 小车的状态。显然有
易知，上述两个公式中，输出变量都是输入变量的线性组合，这也就是称卡尔曼滤波器为线性滤波器的原因所在。既然上述公式表征了一种线性关系，那么我们就可以用一个矩阵来表示它，则有
如果另其中的
则得到卡尔曼滤波方程组中的第一条公式——状态预测公式，而F就是状态转移矩阵，它表示我们如何从上一状态来推测当前状态。而B则是控制矩阵，它表示控制量u如何作用于当前状态。
上式中x顶上的hat表示为估计值（而非真实值）。等式左端部分的右上标“-”表示该状态是根据上一状态推测而来的，稍后我们还将对其进行修正以得到最优估计，彼时才可以将“-”去掉。
既然我们是在对真实值进行估计，那么就理应考虑到噪声的影响。实践中，我们通常都是假设噪声服从一个0均值的高斯分布，即Noise~Guassian(0,σ)。例如对于一个一维的数据进行估计时，若要引入噪声的影响，其实只要考虑其中的方差σ即可。当我们将维度提高之后，为了综合考虑各个维度偏离其均值的程度，就需要引入协方差矩阵。
回到我们的例子，系统中每一个时刻的不确定性都是通过协方差矩阵 Σ 来给出的。而且这种不确定性在每个时刻间还会进行传递。也就是说不仅当前物体的状态（例如位置或者速度）是会（在每个时刻间）进行传递的，而且物体状态的不确定性也是会（在每个时刻间）进行传递的。这种不确定性的传递就可以用状态转移矩阵来表示，即（注意，这里用到了前面给出的关于协方差矩阵的性质）
但是我们还应该考虑到，预测模型本身也并不绝对准确的，所以我们要引入一个协方差矩阵 Q 来表示预测模型本身的噪声（也即是噪声在传递过程中的不确定性），则有
这就是卡尔曼滤波方程组中的第二条公式，它表示不确定性在各个时刻间的传递关系。
继续我们的小汽车例子。你应该注意到，前面我们所讨论的内容都是围绕小汽车的真实状态展开的。而真实状态我们其实是无法得知的，我们只能通过观测值来对真实值进行估计。所以现在我们在路上布设了一个装置来测定小汽车的位置，观测到的值记为V(t)。而且从小汽车的真实状态到其观测状态还有一个变换关系，这个变换关系我们记为h(•)，而且这个h(•)还是一个线性函数。此时便有（该式前面曾经给出过）
Y(t) = h[X(t)] + V(t)
其中V(t)表示观测的误差。既然h(•)还是一个线性函数，所以我们同样可以把上式改写成矩阵的形式，则有
Yt=Hxt + v
就本例而言，观测矩阵 H = [1 0]，这其实告诉我们x和Z的维度不一定非得相同。在我们的例子中，x是一个二维的列向量，而Z只是一个标量。此时当把x与上面给出的H相乘就会得出一个标量，此时得到的Y 就是x中的首个元素，也就是小车的位置。同样，我们还需要用一个协方差矩阵R来取代上述式子中的v来表示观测中的不确定性。当然，由于Z是一个一维的值，所以此时协方差矩阵R也只有一维，也就是只有一个值，即观测噪声之高斯分布的参数σ。如果我们有很多装置来测量小汽车的不同状态，那么Z就会是一个包含所有观测值的向量。
接下来要做的事情就是对前面得出的状态估计进行修正，具体而言就是利用下面这个式子
直观上来说，上式并不难理解。前面我们提到，
上式的推导比较复杂，有兴趣深入研究的读者可以参阅文献【2】（P35~P37）。如果有时间我会在后面再做详细推导。但是现在我们仍然可以定性地对这个系数进行解读：滤波增益阵首先权衡预测状态协方差矩阵Σ和观测值矩阵R的大小，并以此来觉得我们是更倾向于相信预测模型还是详细观测模型。如果相信预测模型多一点，那么这个残差的权重就会小一点。反之亦然，如果相信观察模型多一点，这个残差的权重就会大一点。不仅如此，滤波增益阵还负责把残差的表现形式从观测域转换到了状态域。例如本题中观测值Z 仅仅是一个一维的向量，状态 x 是一个二维的向量。所以在实际应用中，观测值与状态值所采用的描述特征或者单位都有可能不同，显然直接用观测值的残差去更新状态值是不合理的。而利用卡尔曼系数，我们就可以完成这种转换。例如，在小车运动这个例子中，我们只观察到了汽车的位置，但K里面已经包含了协方差矩阵P的信息（P里面就给出了速度和位置的相关性），所以它利用速度和位置这两个维度的相关性，从位置的残差中推算出了速度的残差。从而让我们可以对状态值 x 的两个维度同时进行修正。
最后，还需对最优估计值的噪声分布进行更新。所使用的公式为
至此，我们便获得了实现卡尔曼滤波所需的全部五个公式，我在前面分别用（1）~（5）的标记进行了编号。我现在把它们再次罗列出来：
我们将这五个公式分成预测组和更新组。预测组总是根据前一个状态来估计当前状态。更新组则根据观测信息来对预测信息进行修正，以期达到最优估计之目的。
二、公式和opencv对应关系的讲解：
首先对于离散控制过程的系统，其系统状态和系统测量值可进行以下表示：
X(k)： k时刻系统状态
A： 状态转移矩阵，对应opencv里kalman滤波器的transitionMatrix矩阵（关于opencv kalman滤波器详细定义会在2中给出）
B： 控制输入矩阵，对应opencv里kalman滤波器的controlMatrix矩阵
U(k)：k时刻对系统的控制量
Z(k)： k时刻的测量值
H： 系统测量矩阵，对应opencv里kalman滤波器的measurementMatrix矩阵
W(k)：系统过程噪声，为高斯白噪声，协方差为Q，对应opencv里的kalman滤波器的processNoiseCov矩阵
V(k)： 测量噪声，也为高斯白噪声，协方差为R，对应opencv里的kalman滤波器的measurementNoiseCov矩阵
对于满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声)，卡尔曼滤波器是最优的信息处理器。
接下来便是核心的5个公式。
这5个式子可分成3部分看。
第一部分：
式(1)-(2)：预测值的计算
式(1)：计算基于k-1时刻状态对k时刻系统状态的预测值
X(k|k-1)： 基于k-1时刻状态对k时刻状态的预测值，对应opencv里kalman滤波器的predict()输出，即statePre矩阵
X(k-1|k-1)：k-1时刻状态的最优结果，对应opencv里kalman滤波器的上一次状态的statePost矩阵
U(k)： k时刻的系统控制量，无则为0
A： 状态转移矩阵，对应opencv里kalman滤波器的transitionMatrix矩阵
B： 控制输入矩阵，对应opencv里kalman滤波器的controlMatrix矩阵
式(2)：计算X(k|k-1)对应的协方差的预测值
P(k|k-1)： 基于k-1时刻的协方差计算k时刻协方差的预测值，对应opencv里kalman滤波器的errorCovPre矩阵
P(k-1|k-1)：k-1时刻协方差的最优结果，对应opencv里kalman滤波器的上一次状态的errorCovPost矩阵
Q： 系统过程噪声协方差，对应opencv里kalman滤波器的processNoiseCov矩阵
第二部分：
式(3):增益的计算
Kg(k)：k时刻的kalman增益，为估计量的方差占总方差(估计量方差和测量方差)的比重，对应opencv里kalman滤波器的gain矩阵
H： 系统测量矩阵，对应opencv里kalman滤波器的measurementMatrix矩阵
R： 测量噪声协方差，对应opencv里的kalman滤波器的measurementNoiseCov矩阵
第三部分：
式(4)--(5):k时刻的更新
式(4)：计算k时刻系统状态最优值
X(k|k)：k时刻系统状态的最优结果，对应opencv里kalman滤波器的k时刻状态的statePost矩阵
Z(k)：k时刻系统测量值
式(5)：计算k时刻系统最优结果对应的协方差
P(k|k)：k时刻系统最优结果对应的协方差，对应opencv里kalman滤波器的errorCovPost矩阵
三、两个代码例子
1.这里将文献3里的第一个opencv自带例子进行了些许更改，原程序在VS2015+opencv3.2 contrib版中不能正常运行。
#include<opencv.hpp>
#include <iostream>
//#include <stdio.h>
using namespace std;
using namespace cv;
Mat img(500, 500, CV_8UC3);
//计算相对窗口的坐标值，因为坐标原点在左上角，所以sin前有个负号
static inline Point calcPoint(Point2f center, double R, double angle)
{
return center + Point2f((float)cos(angle), (float)-sin(angle))*(float)R;
}
void drawCross(Point center, Scalar color, int 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);
}
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();
KalmanFilter KF(2, 1, 0); //创建卡尔曼滤波器对象KF
Mat state(2, 1, CV_32F); //state(角度，△角度)
Mat processNoise(2, 1, CV_32F);
Mat measurement = Mat::zeros(1, 1, CV_32F); //定义测量值
char code = (char)-1;
Scalar color;
int d=5;
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); //测量矩阵H
setIdentity(KF.processNoiseCov, Scalar::all(1e-5)); //系统噪声方差矩阵Q
setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1)); //测量噪声方差矩阵R
setIdentity(KF.errorCovPost, Scalar::all(1)); //后验错误估计协方差矩阵P
randn(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 measurement
measurement += KF.measurementMatrix*state; //z = z + H*x;
double measAngle = measurement.at<float>(0);
Point measPt = calcPoint(center, R, measAngle);
// plot points
//定义了画十字的方法，值得学习下
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::all(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0)))); //vk
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.第二个是鼠标跟踪程序。（将转移矩阵A初始化进行更改，原程序在VS2015+opencv3.2 contrib版中不能正常运行）代码如下：（个人理解：这个程序中存在着一些问题，印证了卡尔曼滤波的基本操作，但是对于滤波中的参数有些没有体现,measurement的更新没有体现measurementNoiseCov，，而且进程中也没有体现processNoiseCov的作用）。
#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <stdio.h>
#include<iostream>
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)
{
Mat processNoise(2, 1, CV_32F);//新加程序
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(5)); //系统噪声方差矩阵Q
//cout << KF.processNoiseCov;
//waitKey(0);
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
randn(processNoise, Scalar::all(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0))));//新加程序
// cout << processNoise;//新加程序
//waitKey(0);//新加程序
//measurement.at<float>(0) = (float)mousePosition.x;
//measurement.at<float>(1) = (float)mousePosition.y;
//Point predict_pt = Point(prediction.at<float>(0), prediction.at<float>(1));
measurement.at<float>(0) = (float)(mousePosition.x+processNoise.at<float>(0,0));
measurement.at<float>(1) = (float)(mousePosition.y+processNoise.at<float>(1,0));
//cout << measurement.at<float>(0);
//waitKey(0);
Point p;
p.x = measurement.at<float>(0);
p.y = measurement.at<float>(1);
//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, "measure position :(%3d,%3d)", p.x, p.y);
putText(image, buf, cvPoint(10, 60), 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, 90), CV_FONT_HERSHEY_SCRIPT_COMPLEX, 1, Scalar(0, 0, 0), 1, 8);
imshow("kalman", image);
int key = waitKey(3);
if (key == 27) {//esc
break;
}
}
}
运行结果：
227
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
