chanong -- []追踪（文档2，主要是公式和对应关系的解释）
学习- ()详解（文档3，三个示例代码）
1.卡尔曼滤波器方程的推导
直接从数学公式和概念来考虑卡尔曼滤波器无疑是一件非常枯燥的事情。 为了便于理解，我们还是以一个现实生活中的例子来开始下面的介绍。 在这个过程中你需要的先决知识只是高中物理内容。
如果道路上有一辆直线运动的汽车（如下图），那么汽车在t时刻的状态可以用一个向量来表示，其中pt代表它的当前位置，vt代表汽车当前的速度。 当然，驾驶员也可以踩油门或者刹车，给汽车一个加速度ut，相当于对汽车的一个控制量。 显然，如果驾驶员既不踩油门也不踩刹车，则ut等于0。此时，汽车将以匀速直线行驶。
如果我们知道汽车在最后时间 t-1 的状态，现在让我们考虑汽车在当前时间 t 的状态。显然有
容易知道，在上面两个公式中，输出变量是输入变量的线性组合，这就是卡尔曼滤波器被称为线性滤波器的原因。由于上式表示的是线性关系，所以我们可以用一个矩阵来表示代表它，那么我们有
如果再有一个
然后我们得到卡尔曼滤波器方程组中的第一个公式——状态预测公式，F是状态转移矩阵，它代表我们如何从之前的状态推断出当前的状态。 B为控制矩阵，表示控制量u如何作用于当前状态。
(1)
在上面的公式中，x上面的帽子代表估计值（不是真实值）。 等式左边的右上标“-”表示这个状态是根据之前的状态估计出来的。 稍后我们会修正它，得到最优的估计，然后把“-”去掉就可以了。
由于我们正在估计真实值，因此应考虑噪声的影响。 实际中，我们通常假设噪声服从均值为0的高斯分布，即Noise~(0,σ)。 例如，在估计一维数据时，如果要引入噪声的影响，只需要考虑方差σ即可。 当我们增加维度时，为了综合考虑每个维度偏离均值的程度，需要引入协方差矩阵。
回到我们的例子，系统中每个时刻的不确定性由协方差矩阵 Σ 给出。 而这种不确定性会时时刻刻传递下去。 也就是说，不仅会（在每一时刻）传递物体的当前状态（如位置或速度），还会传递（在每一时刻）物体状态的不确定性。 这种不确定性的转移可以用状态转移矩阵来表示，即（注意这里使用了前面给出的协方差矩阵的性质）
但我们也应该考虑到预测模型本身并不是绝对准确的，所以我们需要引入一个协方差矩阵Q来表示预测模型本身的噪声（即噪声传播过程中的不确定性），那么我们有
(2)
这是卡尔曼滤波器方程组中的第二个公式，代表了每个时刻不确定性的传递关系。
继续我们的汽车示例。 您应该注意到，我们上面讨论的都是围绕汽车的真实状态。 我们实际上没有办法知道真实的状态。 我们只能通过观测值来估计真实值。 所以现在我们在路上架设一个装置来测量汽车的位置，观测值记为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 将是一个包含所有观测值的向量。
接下来要做的就是修改之前获得的状态估计。 具体来说，使用下面的公式
(4)
直观上，上式并不难理解。 我们之前提到过，
从之前的状态推断出来，那么它与“最佳”估计之间的差异现在就是等式右侧加号右侧的部分。
表示实际观测值与估计观测值之间的残差。 该残差乘以系数 K，可用于校正估计值。 K称为卡尔曼系数，也是一个矩阵。 它是残差的加权矩阵。 有些材料将其称为滤波器增益矩阵。
(3)
上述公式的推导比较复杂。 有兴趣深入研究的读者可以参考文献[2]（P35~P37）。 如果以后有时间我会做详细的推导。 但现在我们仍然可以定性地解释这个系数：滤波后的增益矩阵首先权衡预测状态协方差矩阵Σ和观测值矩阵R的大小，并用它来感受我们是否更倾向于相信预测模型或详细的观察模型。 如果你更信任预测模型，那么这个残差的权重就会更小。 反之亦然，如果你认为观测模型越多，这个残差的权重就会越大。 不仅如此，滤波器增益矩阵还负责将残差的表达从观察域转换到状态域。 例如，本题中的观测值Z只是一个一维向量，而状态x是一个二维向量。 因此，在实际应用中，观测值和状态值所使用的描述特征或单位可能会有所不同。 显然，直接利用观测值的残差来更新状态值是不合理的。 利用卡尔曼系数，我们可以完成这个转换。 例如，在汽车运动的例子中，我们只观察了汽车的位置，但是K已经包含了协方差矩阵P的信息（P给出了速度和位置之间的相关性），所以它利用了速度和位置的相关性这两个维度之间的差值从位置残差推导出速度残差。 这允许我们同时校正状态值 x 的两个维度。
最后，需要更新最优估计的噪声分布。使用的公式为
(5)
至此，我们已经获得了实现卡尔曼滤波所需的全部五个公式，我分别用标记（1）到（5）编号。 我现在再次列出它们：
我们将这五个公式分为预测组和更新组。 预测组总是根据先前的状态来估计当前的状态。 更新组根据观测信息对预测信息进行修正，以达到最优估计的目的。
2、公式及对应关系解释：
首先，对于具有离散控制过程的系统，系统状态和系统测量值可以表示为：
X(k)：k时刻的系统状态
A：状态转移矩阵，对应滤波器的矩阵（滤波器的详细定义将在2中给出）
B：控制输入矩阵，对应滤波器的矩阵
U(k)：k时刻系统的控制量
Z(k)：k时刻的测量值
H：系统测量矩阵，对应滤波器的矩阵
W(k)：系统过程噪声，是协方差为Q的高斯白噪声，对应于滤波器的矩阵
V(k)：测量噪声，也是高斯白噪声，协方差为R，对应于滤波器的矩阵
对于满足上述条件（线性随机微分系统，过程和测量均为高斯白噪声），卡尔曼滤波器是最优的信息处理器。
接下来是五个核心公式。
这5个公式可以分为3部分。
第一部分：
公式(1)-(2)：预测值的计算
公式（1）：根据k-1时刻的状态计算k时刻系统状态的预测值
X(k|k-1)：基于k-1时刻状态对k时刻状态的预测值，对应滤波器的()输出，即矩阵
X(k-1|k-1)：k-1时刻状态的最优结果，滤波器之前状态对应的矩阵
U(k)：k时刻的系统控制变量，如果没有则为0
A：状态转移矩阵，对应滤波器的矩阵
B：控制输入矩阵，对应滤波器的矩阵
公式（2）：计算X(k|k-1)对应的协方差的预测值
P(k|k-1)：根据k-1时刻的协方差计算k时刻协方差的预测值，对应滤波器的矩阵
P(k-1|k-1)：k-1时刻协方差的最优结果，对应滤波器之前状态的矩阵
Q：系统过程噪声协方差，对应滤波器的矩阵
第二部分：
公式（3）：增益的计算
Kg(k)：k时刻的增益，是估计器的方差占总方差（估计器的方差和测量方差）的比例，对应滤波器的增益矩阵
H：系统测量矩阵，对应滤波器的矩阵
R：测量噪声协方差，对应滤波器的矩阵
第三部分：
公式(4)--(5)：在k时刻更新
公式（4）：计算k时刻系统状态最优值
X(k|k)：k时刻系统状态的最优结果，对应k时刻滤波器状态的矩阵
Z(k)：k时刻系统测量值
式(5)：计算k时刻系统最优结果对应的协方差
P(k|k)：k时刻系统最优结果对应的协方差，对应滤波器的矩阵
3. 两个代码示例
1.文档3中的第一个内置示例此处略有更改。 原程序在+.2版本中无法正常运行。
#include
#include
//#include
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_(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(0); //跟踪点角度
Point statePt = calcPoint(center, R, stateAngle); //跟踪点坐标statePt
//2. 预测
Mat prediction = KF.predict(); //计算预测值，返回x'
double predictAngle = prediction.at(0); //预测点的角度
Point predictPt = calcPoint(center, R, predictAngle); //预测点坐标predictPt
//3.更新
//measurement是测量值
randn(measurement, Scalar::all(0), Scalar::all(KF.measurementNoiseCov.at(0))); //给measurement赋值N(0,R)的随机值
// generate measurement
measurement += KF.measurementMatrix*state; //z = z + H*x;
double measAngle = measurement.at(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(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的初始化，原程序在+.2版本中无法正常运行）代码如下：（个人理解：这个程序存在一些问题，印证了卡尔曼滤波的基本操作，但是对于过滤部分参数没有反映，更新没有反映，过程中没有反映效果）。
#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"
#include
#include
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_(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(0), prediction.at(1)); //预测值(x',y')
//3.update measurement
randn(processNoise, Scalar::all(0), Scalar::all(sqrt(KF.processNoiseCov.at(0, 0))));//新加程序
// cout << processNoise;//新加程序
//waitKey(0);//新加程序
//measurement.at(0) = (float)mousePosition.x;
//measurement.at(1) = (float)mousePosition.y;
//Point predict_pt = Point(prediction.at(0), prediction.at(1));
measurement.at(0) = (float)(mousePosition.x+processNoise.at(0,0));
measurement.at(1) = (float)(mousePosition.y+processNoise.at(1,0));
//cout << measurement.at(0);
//waitKey(0);
Point p;
p.x = measurement.at(0);
p.y = measurement.at(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;
}
}
} 运行结果：
微信扫一扫打赏
支付宝扫一扫打赏
