![](https://csdnimg.cn/release/download_crawler_static/87551100/bg4.jpg)
就是式子(2)的 P(k-1|k-1)。这样,算法就可以自回归的运算下去。
卡尔曼滤波器的原理基本描述了,式子 1,2,3,4 和 5 就是他的 5 个基本公式。
根据这 5 个公式,可以很容易的实现计算机的程序。
4. 简单例子
(A Simple Example)
这里我们结合第二第三节,举一个非常简单的例子来说明卡尔曼滤波器的工作
过程。所举的例子是进一步描述第二节的例子,而且还会配以程序模拟结果。
根据第二节的描述,把房间看成一个系统,然后对这个系统建模。当然,我们
见的模型不需要非常地精确。我们所知道的这个房间的温度是跟前一时刻的温
度相同的,所以 A=1。没有控制量,所以 U(k)=0。因此得出:
X(k|k-1)=X(k-1|k-1) ……….. (6)
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)。
最佳线性滤波理论起源于 40 年代美国科学家 Wiener 和前苏联科学家K
олмогоров 等人的研究工作,后人统称为维纳滤波理论。从理论上说,
维纳滤波的最大缺点是必须用到无限过去的数据,不适用于实时处理。为了克
服这一缺点, 60 年代 Kalman 把状态空间模型引入滤波理论,并导出了一套
递推估计算法,后人称之为卡尔曼滤波理论。卡尔曼滤波是以最小均方误差为
估计的最佳准则,来寻求一套递推估计的算法,其基本思想是:采用信号与噪