卡尔曼滤波介绍与原理讲解

首先我们要利用系统的过程模型，来预测下一状态的系统。假设现在的系统状态是k，根据系统的模型，可以基于系统的上一状态而预测出现在状态：
X(k|k-1)=AX(k-1|k-1)+BU(k)………..(1)

式(1)中，X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)是上一状态最优的结果，U(k)为现在状态的控制量，如果没有控制量，它可以为0。

到现在为止，我们的系统结果已经更新了，可是，对应于X(k|k-1)的covariance还没更新。我们用P表示covariance：

P(k|k-1)=AP(k-1|k-1)A’+Q(2)

式(2)中，P(k|k-1)是X(k|k-1)对应的covariance，P(k-1|k-1)是X(k-1|k-1)对应的covariance，A’

表示A的转置矩阵，Q是系统过程的covariance。式子1，2就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。

现在我们有了现在状态的预测结果，然后我们再收集现在状态的测量值。结合预测值和测量值，我们可以得到现在状态(k)的最优化估算值X(k|k)：
X(k|k)=X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1))(3)
其中Kg为卡尔曼增益(KalmanGain)：

Kg(k)=P(k|k-1)H’/(HP(k|k-1)H’+R)(4)

到现在为止，我们已经得到了k状态下最优的估算值X(k|k)。但是为了要另卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新k状态下X(k|k)的covariance：
P(k|k)=（I-Kg(k)H）P(k|k-1)(5)

其中I为1的矩阵，对于单模型单测量，I=1。当系统进入k+1状态时，P(k|k)就是式子(2)的P(k-1|k-1)。这样，算法就可以自回归的运算下去。

卡尔曼滤波器的原理基本描述了，式子1，2，3，4和5就是他的5个基本公式。根据这5个公式，可以很容易的实现计算机的程序。

下面，我会用程序举一个实际运行的例子。。。

4．简单例子
（ASimpleExample）
这里我们结合第二第三节，举一个非常简单的例子来说明卡尔曼滤波器的工作过程。所举的例子是进一步描述第二节的例子，而且还会配以程序模拟结果。

根据第二节的描述，把房间看成一个系统，然后对这个系统建模。当然，我们建的模型不需要非常地精确。我们所知道的这个房间的温度是跟前一时刻的温度相同的，所以A=1。没有控制量，所以U(k)=0。因此得出：
X(k|k-1)=X(k-1|k-1)………(6)
式子（2）可以改成：

P(k|k-1)=P(k-1|k-1)+Q………(7)

因为测量的值是温度计的，跟温度直接对应，所以H=1。式子3，4，5可以改成以下：
X(k|k)=X(k|k-1)+Kg(k)(Z(k)-X(k|k-1))………(8)
Kg(k)=P(k|k-1)/(P(k|k-1)+R)………(9)
P(k|k)=（1-Kg(k)）P(k|k-1)???(10)

现在我们模拟一组测量值作为输入。假设房间的真实温度为25度，我模拟了200个测量值，这些测量值的平均值为25度，但是加入了标准偏差为几度的高斯白噪声（在图中为蓝线）。

为了令卡尔曼滤波器开始工作，我们需要告诉卡尔曼两个零时刻的初始值，是X(0|0)和P(0|0)。他们的值不用太在意，随便给一个就可以了，因为随着 卡尔曼的工作，X会逐渐的收敛。但是对于P，一般不要取0，因为这样可能会令卡尔曼完全相信你给定的X(0|0)是系统最优的，从而使算法不能收敛。我选 了X(0|0)=1度，P(0|0)=10。

该系统的真实温度为25度，图中用黑线表示。图中红线是卡尔曼滤波器输出的最优化结果（该结果在算法中设置了Q=1e-6，R=1e-1）。

××××××××××××××××××

附matlab下面的kalman滤波程序：
clear
N=200;
w(1)=0;

w=randn(1,N) //产生一个均值为0，方差为1的1*n维向量（白噪声、正态分布而非均匀分布）
x(1)=0;

a=1;//即A
fork=2:N; //FOR

x(k)=a*x(k-1)+w(k-1); //200个X(k)赋值，初始值？
end //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;
p(1)=0;
s(1)=0;

fort=2:N; //FOR
p1(t)=a.^2*p(t-1)+Rww;

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 //END

t=1:N; //循环？

plot(t,s,'r',t,Y,'g',t,x,'b'); //RGB?  s,Y,x都是向量