目标跟踪之卡尔曼滤波---理解Kalman滤波的使用预测
Kalman滤波简介
Kalman滤波是一种线性滤波与预测方法，原文为：A New Approach to Linear Filtering and Prediction Problems。文章推导很复杂，看了一半就看不下去了，既然不能透彻理解其原理，但总可以通过实验来理解其具体的使用方法。
Kalman滤波分为2个步骤，预测(predict)和校正(correct)。预测是基于上一时刻状态估计当前时刻状态，而校正则是综合当前时刻的估计状态与观测状态，估计出最优的状态。预测与校正的过程如下：
预测：
校正：
公式1是状态预测，公式2是误差矩阵预测，公式3是kalman增益计算，公式4是状态校正，其输出即是最终的kalman滤波结果，公式5是误差矩阵更新。各变量说明如下表：
|
xk |
k时刻的状态 |
|
A |
状态转移矩阵，和具体的线性系统相关 |
|
uk |
K时刻外界对系统的作用 |
|
B |
输入控制矩阵，外界的影响如何转化为对状态的影响 |
|
P |
误差矩阵 |
|
Q |
预测噪声协方差矩阵 |
|
R |
测量噪声协方差矩阵 |
|
H |
观测矩阵 |
|
Kk |
K时刻的kalman增益 |
|
zk |
K时刻的观测值 |
算法实现与分析
Kalman滤波最复杂的计算应该就是公式3中的矩阵求逆，考虑到实现的方便性，采用matlab来简单实现，本文主要是分析kalman滤波中各个变量的作用和对滤波结果的影响。具体代码如下：
function filter = Kalman(filter)
%predict
predict_x = filter.A * filter.x + filter.B * filter.u;
filter.P = filter.A * filter.P * filter.A' + filter.Q;
%correct
filter.K = filter.P * filter.H' / (filter.H * filter.P * filter.H' + filter.R);
filter.x = predict_x + filter.K * (filter.z - filter.H * predict_x);
filter.P = filter.P - filter.K * filter.H * filter.P;
end
在matlab中，kalman滤波实际上就是上面那5个公式，而难点却是在测试代码中针对不同问题各个变量的初始化上，下面来逐个分析。
1.建立模型，明确观测量，系统状态以及其转移方程(下面这段公式太多，通过word写好后截图)
2.初始化噪声协方差矩阵
经过上面一步，只有PQRK四个矩阵还未确定了。显然增益矩阵K是不需要初始化的，P是误差矩阵，初始化可以是一个随机的矩阵或者0，只要经过几次的处理基本上就能调整到正常的水平，因此也就只会影响前面几次的滤波结果。
Q和R分别是预测和观测状态协方差矩阵，一般可以简单认为系统状态各维之间(即上面的a和b)相互独立，那么Q和R就可以设置为对角阵。而这两个对角线元素的大小将直接影响着滤波结果，若Q的元素远大于R的元素，则预测噪声大，从而更相信观测值，这样可能使得kalman滤波结果与观测值基本一致；反之，则更相信预测，kalman滤波结果会表现得比较规整和平滑；若二者接近，则滤波结果介于前面两者之间，根据实验效果看也缺乏实际使用价值。
以上几个矩阵确定后，对于状态x，由于0时刻我们没有任何关于该系统的知识，可以使用0时刻的测量值z0来初始x0，预测从k=1开始；也可以初始化-1时刻的状态，当然这个状态实际是未知的，也就可随机取。2种方式都可以，但使用0时刻测量值来初始化状态，可以使得前面几次预测更准确。
3.实验分析
首先使用下面代码生成一组数据存在z.mat中：
interval = pi/18;
t = 1:interval:100*pi;
len = size(t, 2);
a = t + 4 * (rand(1,len)-0.5);
b = t .* sin(t/10) + 10 * (rand(1,len)-0.5);
z = [a; b];
save('z.mat','z');
plot(z(1,:),z(2,:),'o')
可以看出其近似为一条振幅不断增大的正弦曲线叠加一个随机噪声。绘制出来如下：
如果使用上面推导的恒定状态系统模型，代码与实验结果如下：
clear
close all
clc
dim_observe = 2; %观测值维数
n = dim_observe; %状态维数，观测状态每个维度都有1个速度，故需乘2
filter.A = eye(n);%[1,0,1,0;0,1,0,1;0,0,1,0;0,0,0,1];
filter.B = 0;
filter.u = 0;
filter.P = eye(n);
filter.K = zeros(n);
filter.H = eye(n);%[1,0,0,0;0,1,0,0];
cQ = 1e-8;
cR = 1e-2;
filter.Q = eye(n) * cQ; %这里简单设置Q和R对角线元素都相等，设为不等亦可
filter.R = eye(dim_observe) * cR;
filter.x = zeros(n,1); %初始状态x0
load('z.mat');
figure(1),subplot(2,2,1),
t = 1;
out = [];
for i=1:size(z,2)
filter.z = z(:,i);
filter = Kalman(filter);
plot(filter.x(1),filter.x(2), 'r*');hold on
plot(filter.z(1),filter.z(2), 'bo'); hold on
out=[out filter.x];
% pause(.5)
end
figure(1),
str = sprintf('cQ = %e, cR = %e', cQ, cR);
title(str)
%画局部放大
subplot(2,2,2),
plot(out(1,:),out(2,:), 'r*');hold on
plot(z(1,:),z(2,:), 'bo'); hold on
axis([120 170 80 200])
可以看出滤波结果完全滞后于测量数据，其根本原因在于建立的模型存在问题。
如果采用上面推导的物体运动模型则只需要修改部分代码，主要是矩阵A和H，以及其他矩阵对应的维数，具体如下：
dim_observe = 2; %观测值维数 n = 2 * dim_observe; %状态维数，观测状态每个维度都有1个速度，故需乘2 filter.A = [1,0,1,0;0,1,0,1;0,0,1,0;0,0,0,1]; filter.B = 0; filter.u = 0; filter.P = eye(n); filter.K = zeros(n); filter.H = [1,0,0,0;0,1,0,0];
运行结果如下图，蓝色为观测数据，红色为kalman滤波数据，右侧为局部放大图。可以看出经过滤波后的数据相当平滑，这里Q和R中元素的量级分别为cQ和cR，下图结果可以看到cR比cQ多了6个数量级。
（1）
增加几组结果用于对比分析，对于的cQ和cR见图的标题。
（2）
（3）
（4）
（5）
（6）
首先看图1和2，cR与cQ大小均相差了3个数量级，而二者的比值相同，则kalman滤波结果相同。
再看图2~图6，cR/cQ在不断减小，kalman滤波结果的平滑性也在不断降低，到图5和6中，滤波结果完全和观测值相同，说明此时kalman滤波已经完全相信观测值了。原因在于cR/cQ过小，系统认为预测噪声的方差很大，不值得信赖，而观测值的噪声方差小，可信度高。
总结
根据上面的实验结果，可以看出Kalman滤波应用中的几个问题：
1.模型建立的正确性从根本上决定了滤波效果的正确性。
上面使用物体静止模型进行滤波，结果完全不对，而使用匀速运动模型则能达到较好的效果。从根本上讲，上面的数据也不是匀速运动的，为何结果会基本正确？看看第一个使用静止模型的滤波结果，虽然我们假定了物体是静止的，但由于观测数据的作用，kalman滤波结果也会有相应的运动而不是完全静止，也就是说滤波器在不停地修正这个状态，而在匀速运动模型中，物体的速度我们认为不变，但同样地kalman滤波器也会不停地修正这个速度，滤波器中计算的速度实质的偏离了真实速度的，因此最终也会有相应的偏差，不过这个偏差在我们容许范围内，也就可以大胆使用了。
如果能确定物体是匀变速直线运动，使用相应带加速度的模型会得到更准确的效果。但是越严格的模型其适用范围也相应越小。
2.影响滤波结果平滑性的因素是cR/cQ，这个值反映了我们对于预测和观测值的信任程度；其值越大则越相信预测结果，滤波结果平滑性好；反之则越相信观测结果，滤波结果越偏向于观测值。一般我们使用kalman滤波器是为了能平滑数据的波动，因此应尽量保证cR/cQ稍大，上面的测试结果该值在1e4以上数据较为平滑。
http://www.cnblogs.com/jcchen1987/p/4371439.html
http://blog.csdn.net/andrew659/article/details/4818988
