Kalman原理介绍

1. 卡尔曼滤波器的介绍
为了可以更加容易的理解卡尔曼滤波器, 这里会应用形象的描述方法来讲解, 而不是像大多 数参考书那样罗列一大堆的数学公式和数学符号。但是,他的 5 条公式是其核心内容。结合 现代的计算机,其实卡尔曼的程序相当的简单,只要你理解了他的那 5 条公式。 在介绍他的 5 条公式之前,先让我们来根据下面的例子一步一步的探索。 假设我们要研究的对象是一个房间的温度。根据你的经验判断,这个房间的温度是恒定的, 也就是下一分钟的温度等于现在这一分钟的温度(假设我们用一分钟来做时间单位)。假设 你对你的经验不是 100%的相信,可能会有上下偏差几度。我们把这些偏差看成是高斯白噪 声(White Gaussian Noise),也就是这些偏差跟前后时间是没有关系的而且符合高斯分配 (Gaussian Distribution)。另外,我们在房间里放一个温度计,但是这个温度计也不准确 的,测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。 好了,现在对于某一分钟我们有两个有关于该房间的温度值:你根据经验的预测值(系统的 预测值)和温度计的值(测量值)。下面我们要用这两个值结合他们各自的噪声来估算出房 间的实际温度值。 假如我们要估算 k 时刻的是实际温度值。首先你要根据 k-1 时刻的温度值,来预测 k 时刻的 温度。因为你相信温度是恒定的,所以你会得到 k 时刻的温度预测值是跟 k-1 时刻一样的, 假设是 23 度,同时该值的高斯噪声的偏差是 5 度(5 是这样得到的:如果 k-1 时刻估算出 的最优温度值的偏差是 3, 你对自己预测的不确定度是 4 度, 他们平方相加再开方, 就是 5) 。 然后,你从温度计那里得到了 k 时刻的温度值,假设是 25 度,同时该值的偏差是 4 度。 由于我们用于估算 k 时刻的实际温度有两个温度值,分别是 23 度和 25 度。究竟实际温度 是多少呢?相信自己还是相信温度计呢?究竟相信谁多一点, 我们可以用他们的 covariance 来判断。因为 Kg^2=5^2/(5^2+4^2),所以 Kg=0.78,我们可以估算出 k 时刻的实际温度 值是:23+0.78*(25-23)=24.56 度。可以看出,因为温度计的 covariance 比较小(比较相 信温度计),所以估算出的最优温度值偏向温度计的值。 现在我们已经得到 k 时刻的最优温度值了,下一步就是要进入 k+1 时刻,进行新的最优估 算。到现在为止,好像还没看到什么自回归的东西出现。对了,在进入 k+1 时刻之前,我 们还要算出 k 时刻那个最优值(24.56 度)的偏差。算法如下:((1-Kg)*5^2)^0.5=2.35。 这里的 5 就是上面的 k 时刻你预测的那个 23 度温度值的偏差,得出的 2.35 就是进入 k+1 时刻以后 k 时刻估算出的最优温度值的偏差(对应于上面的 3)。 就是这样,卡尔曼滤波器就不断的把 covariance 递归,从而估算出最优的温度值。他运行 的很快, 而且它只保留了上一时刻的 covariance。 上面的 Kg, 就是卡尔曼增益 (Kalman Gain) 。 他可以随不同的时刻而改变他自己的值,是不是很神奇! 下面就要言归正传,讨论真正工程系统上的卡尔曼。

2. 卡尔曼滤波器算法

在这一部分,我们就来描述源于 Dr Kalman 的卡尔曼滤波器。下面的描述,会涉及一些基 本的概念知识,包括概率(Probability),随即变量(Random Variable),高斯或正态分 配 (Gaussian Distribution) 还有 State-space Model 等等。 但对于卡尔曼滤波器的详细证明, 这里不能一一描述。 首先,我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程(Linear Stochastic Difference equation)来描述: X(k)=A X(k-1)+B U(k)+W(k) 再加上系统的测量值: Z(k)=H X(k)+V(k) 上两式子中,X(k)是 k 时刻的系统状态,U(k)是 k 时刻对系统的控制量。A 和 B 是系统参数, 对于多模型系统,他们为矩阵。Z(k)是 k 时刻的测量值,H 是测量系统的参数,对于多测量 系统,H 为矩阵。W(k)和 V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise),他们的 covariance 分别是 Q,R(这里我们假设他们不随系统状态变化而 变化)。 对于满足上面的条件(线性随机微分系统,过程和测量都是高斯白噪声),卡尔曼滤波器是最 优的信息处理器。下面我们来用他们结合他们的 covariances 来估算系统的最优化输出(类 似上一节那个温度的例子)。 首先我们要利用系统的过程模型,来预测下一状态的系统。假设现在的系统状态是 k,根据 系统的模型,可以基于系统的上一状态而预测出现在状态: X(k|k-1)=A X(k-1|k-1)+B U(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)=A P(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)-H X(k|k-1)) ……… (3) 其中 Kg 为卡尔曼增益(Kalman Gain): Kg(k)= P(k|k-1) H’ / (H P(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 个公式,可以很容易的实现计算机的程序。 下面,我会用程序举一个实际运行的例子。。。

3. 简单例子
这里我们结合第二第三节, 举一个非常简单的例子来说明卡尔曼滤波器的工作过程。 所举的 例子是进一步描述第二节的例子,而且还会配以程序模拟结果。 根据第二节的描述,把房间看成一个系统,然后对这个系统建模。当然,我们见的模型不需 要非常地精确。我们所知道的这个房间的温度是跟前一时刻的温度相同的,所以 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)。


相关文档

Kalman滤波原理及仿真手册-简介
KALMAN理论分析
Kalman滤波原理及程序(手册)
kalman滤波器算法原理
基于Kalman滤波原理的运动目标跟踪
Kalman滤波概述
基于数据融合估计理论的Kalman滤波
基于Kalman滤波的白噪声估计理论
Kalman滤波原理及算法
基于KALMAN滤波理论的数字微分信号降噪处理
电脑版