卡尔曼滤波器是一个“最优化自回归数据处理算法”。对于解决大部分的问题，它是最优，效率最高甚至是最有用的。其广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等。近年来更被应用于计算机图像处理，列入，面部识别，图像分割，图像边缘检测等方面。
卡尔曼滤波原理
首先要引入一个离散控制过程的系统，该系统可用一个线性随机微分方程来秒速：
X（k）=AX（k-1）+BU（k）+W（k） （1）
再加上系统的测量值：
Z（k）=HX（k）+V（k） （2）
上两式子中，X（k）是k时刻的系统状态，U（k）是k时刻对系统的控制量。A和B是系统参数，对于多模型系统，它们为矩阵。Z（k）是k时刻的测量值，H是测量系统的参数，对于多测量系统，H为矩阵。W（k）和V(k)分别表示过程和测量的噪声。它们被假设成高斯白噪声，其协方差分别是Q,R,这里假设它们不随系统状态变化而变化。
由于满足上面的条件（线性随机微分系统，过程和测量都是高斯白噪声），卡尔曼滤波器是最优的信息处理器。下面来估算系统的最优化输出。
首先利用系统的过程模型预测下个状态的系统。假设现在的系统状态是k，根据系统的模型，可以基于系统的上一状态而预测出现在状态：
X（k|k-1）=AX（k-1|k-1）+BU（k） （3）
式（3）中，X（k|k-1）是利用上一个状态预测的结果，X（k-1|k-1）是上一个状态最优的结果，U（k）为现在状态的控制量，如果没有控制量，它可以为0.
到现在为止，系统结果已经更新了，可是对应于X(k|k-1)的协方差还没有更新。用P表示协方差：
P（k|k-1）=AP（k|k-1）A’+Q （4）
式子（4）中P（k|k-1）是X（k|k-1）对应的协方差，P（k-1|k-1）是X（k-1|k-1）对应的协方差，A’表示A的转置矩阵，Q时系统过程的协方差。式（3），式（4）就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。
有了现在状态的预测结果，再收集现在状态的测量值。结合预测值和测量值，可以得到现在状态（k）的最优化估算值X（k|k）：
X（k|k）=X（k|k-1）+Kg（k）（Z（k）-HX（k|k-1）） （5）
其中Kg为卡尔曼增益：
Kg(k)=P(k|k-1)H’|(HP(k|k-1)H’+R) （6）
到此为止，已经得到了k状态下最优的估算值X（k|k）。但是，为了要令卡尔曼滤波器不断地运行下去，直到系统过程结束，还要更新k状态下X（k|k）的covariance:
P（k|k）=（I-Kg(k)H）P(k|k-1) （7）
其中I为1的矩阵，对于单模型单测量，I=1。当系统进入式（k+1）状态时，P(k|k)就是式（4）中的P（k-1|k-1）。这样，算法就可以自回归地运算下去。
卡尔曼滤波器的原理基本描述了，式（3），（4），（5），（6），（7）是5个基本公式。根据这5个公式，可以很容易地实现计算机程序。
仿真思路
把房间看成是一个系统，然后对这个系统建模。已知房间的温度跟前一时刻的温度相同，所以A=1.没有控制量，所以U(k)=0。因此得出：
X（k|k-1）=X（k|-1k-1）
P（k|k-1）=P（k-1|k-1）+Q
因为测量的值是温度计的，跟温度直接对应，所以H=1。
X（k|k）=X（k|k-1）+Kg（k）（Z（k）-X（k|k-1））
Kg(k)=P(k|k-1)/(P(k|k-1)+R)
P（k|k）=（I-Kg(k)）P(k|k-1)
现在模拟一组测量值作为输入。假设房间的真实温度为25度，模拟200个测量值，这些测量值的平均值为25度，但是加入了标准偏差为几度的高斯白噪声。
为了令卡尔曼滤波器开始工作，需要告诉卡尔曼两个零时刻的初始值X(0|0)和P(0|0)。
它们的值不用太在意，随便给一个就可以了，因为随着卡尔曼的工作，X会逐渐收敛。 但是对于P，一般不要取0，因为这样可能会令卡尔曼完全相信给定的X(0|0)是系统最优的，从而使算法不能收敛。本列选了X(0|0)=1度，P(0|0)=10。
该系统的真实温度为25度，图中蓝线表示。图中红线是卡尔曼滤波器输出的最优化结果。
程序代码
clear
N=200;
w(1)=0;
w=randn(1,N);
x(1)=0;
a=1;
for k=2:N;
x(k)=a*x(k-1)+w(k-1);
end
V=randn(1,N);
q1=std(V);
Rvv=q1.^2;
q2=std(x);
Rxx=q2.^2;
q3=std(w);
Rww=q3.^2;
c=0.2;
Y=c*x+v;
Y=c*x+V;
p(1)=0;
s(1)=0;
for t=2:N;
p1(t)=a.^2*p(t-1)+Rvv;
b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);
s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1));
p(t)=p1(t)-c*b(t)*p1(t);
end
t=1:N;
plot(t,s,'y',t,Y,'g',t,x,'b');
clear
clc
cear
clc
clear
N=200;
w(1)=0;
w=randn(1,N);
x(1)=0;
a=1;
for k=2:N;
x(k)=a*x(k-1)+w(k-1);
end
V=randn(1,N);
q1=std(V);
Rvv=q1.^2;
q2=std(x);
Rxx=q2.^2;
q3=std(w);
Rww=q3.^2;
c=0.2;
Y=c*x+v;
Y=c*x+V;
p(1)=0;
s(1)=0;
for t=2:N;
p1(t)=a.^2*p(t-1)+Rvv;
b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);
s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1));
p(t)=p1(t)-c*b(t)*p1(t);
end
t=1:N;
plot(t,s,'y',t,Y,'g',t,x,'b');
图1
运行结果与分析
从图1中可以看出，卡尔曼滤波的估计值能较好地逼近要估计的真实温度值。该列表明，卡尔曼滤波具有很好的效果。下载本文
