kalman滤波

Kalman滤波器是一种用递归方法解决离散数据线性滤波问题的滤波器。得益于数 字信号处理技术和集成电路工艺的发展，kalman滤波器的应用场景日益广泛，在 通信系统的各个领域都有所应用。

kalman滤波器用于估计离散事件过程的状态变x∈Rn。这个 离散时间过程可以用一下离散随机差分方程描述：

xk=Axk−1+Buk−1+wk−1

另外对于观测系统，我们也需要定义一个观测方程：

zk=Hxk+vk

随机信号wk 和vk 分别表示过程噪声和观测噪声。通常假设这两 个随机变量相互独立，并且服从正态分布：

p(w)∼N(0,Q)p(v)∼N(0,R)

在实际系统中，过程噪声协方差矩阵Q和观测噪声协方差矩阵R可能会随 着每次迭代计算而变化。

定义ˆx−k∈Rn为一直第k步以前状态情况 下第k步的香烟估计。定义ˆxk∈Rn为一直测 量变量zk时第k步的后验状态估计。由此定义先验估计误差和后验估 计误差：

e−k=xk−ˆx−kek=xk−ˆxk

先验估计误差的协方差为：

P−k=E[e−ke−Tk]

后验估计的协方差为：

Rk=E[ekeTk]

我们用先验估计ˆx−k和加权的测量变量zk以及预测 Hˆx−k之差的线性组合构成了后验状态估计ˆxk。 即：

ˆxk=ˆx−k+K(zk−Hˆx−k)

式~(5) 构成了kalman滤波器的表达式。式中的zk−Hˆx−k被称为测量过程的更新或者残余。这个量反映了预测值和实际值之间 的不一致程度。更新为0说明二者完全相同。式中K叫做更新的增益，K 的作用是使式~(4)的后验估计误差协方差尽可能的小。可以把 式~(5)带入式~(4)，求得期望之后，将式~(4)对 K求导，可得K:

Kk=P−kHT(HP−kHT+R)−1

从式~(6)可知，观测噪声协方差R越小，更新的增益 K越小。特别地，P−k趋向于零时，有：

limRk→0Kk=H−1

从式~(5)可以看出，Kk=H−1时，有ˆxk=xk，即后验估计值和真实值相等。也就是说当观测方程的误差越来越小的 时候，后验估计值越来越接近真实值。也就是说随着测量噪声越来越小，我们需 要越来越相信测量到的zk，而不是基于测量方程的预测 Hˆx−k。

另一方面，先验估计误差协方差P−k越小，残余的增益K越小。特 别的，P−k趋于零时，有：

limP−k→0Kk=0

这说明先验估计很准的时候，后验估计要和先验估计保持一致。也就是说系统方 程的误差越来越小，我们要相信系统方程做出的先验估计，而不是相信测量变量 zk。最终的结果都要通过测量方程来输出。根据先验估计误差和测量误 差的变化，我们选择相信测量值还是测量方程的估计值。

kalman滤波用反馈的方式估计过程状态：滤波器估计过程某一时刻的状态，然后 以（含噪声）测量变量的方式获得反馈。kalman滤波器的精髓就在这里。具体体 现在先验估计协方差P−k和观测噪声的博弈。这是个此消彼长的过程： 系统输出比较精确我就倾向于使用系统的输出；观测是比较精确的我就倾向于使 用观测的结果。

kalman滤波器分两部分：1. 时间更新方程；2. 测量更新方程。时间更新方程负 责向前推算当前状态变量和误差协方差估计，为下一时刻构建先验估计做准备。 测量更新方程负责反馈，即把先验估计和新的测量变量结合以构造改进的后验估 计。所以时间更新方程是预估方程，测量更新方程是校正方程。这两个方程互相 迭代，不断提高后验估计的精度。

基于之前定义的变量，我们在这里不加推导的给出时间更新方程和测量方程对应 的内容。

离散kalman滤波器时间更新方程为：

ˆx−k=Aˆxk−1+Buk−1P−k=APk−1AT+Q

时间更新方程将状态估计x−k和协方差估计P−k从k−1 时刻向前推算到k时刻。A和B 来自~(1)， Q是过程 噪声方差。

离散kalman滤波器状态更新方程为：

Kk=P−kHT(HP−kHT+R)−1ˆxk=ˆx−k+Kk(zk−Hˆx−k)Pk=(I−KkH)P−k

测量更新方程首先要做的是计算kalman增益Kk。然后更新测量输出获得 zk，更新后验估计^xk。最后估计状态的后验协方差。

Kalman滤波器是一种迭代滤波器，在时间更新方程和状态更新方程之间迭代。上 一次得到的后验估计被作为下一次计算的先验估计。这种递推是kalman滤波器的 亮点。它比维也纳滤波器计算量更低。因为维也纳滤波必须计算全部数据，而 kalman滤波器每次只根据以前的测量变量递归计算当前的状态估计。

在实际实现时，测量噪声协方差R一般可以观测得到，是滤波器的已知条件。 观测测量噪声协方差R一般是可以实现的。因此我们通常我们离线获取一些 系统观测值以计算测量噪声协方差。

通常更难确定过程激励噪声协方差的Q值，因此我们无法直接观测到过程信 号xk。有时可以通过Q的选择给过程信号注入足够的不确定性来建立 一个简单的过程模型而产生可以接受的结果。在这两种情况下，不管我们是否有 一个合理的标准来选择系数，我们通常都可以通过调整滤波器系数来获得更好的 性能。调整的过程是离线完成的，并经常与另一个确定无误的在线滤波器对比。 这个过程称为系统识别。这里给了机器学习一个机会。

有时候我们假定Q和R都是常数，过程估计误差协方差R和kalman增 益Kk都会快速收敛并保持为常量。若实际情况如此，那么滤波器系数便 可以通过预先离线运行滤波器计算。然而，在实际应用中，观测误差R尤其 不易保持不变。不仅观测噪声会发生变化，有时候过程激励噪声协方差Q 也 会随着滤波器运行而发生变化，这样Q就是时变的了。

在前面两节，我们说明了kalman滤波器原理和迭代过程，在这一节，我们跟进一 步的细化这个过程，并辅以例子阐述几乎每个细节。毕竟，就一个主题进行深入 的逐步的解读才是我的风格。

在这个章节，我们准备分四步来完成对一个动态系统的预测：

1. 用数学语言描述一个动态系统，然后估计这个动态系统的状态；
2. 我们推导这个动态系统的状态均值和方差的变化；
3. 我们使用计算机模拟这个系统；
4. 每次我们获得一个测量，就更新这个系统状态的均值和方差。

在这一节我们描述的动态系统如下：

xk=Fk−1xk−1+Gk−1uk−1+wk−1yk=Hkxk+vk

其中wk和vk是零均值，白高斯噪声：

wk∼N(0,Qk)vk∼N(0,Rk)

我们的目的是基于我们对系统的了解和观测值yk 估计 xk。我们 的已知信息与要考虑的动态系统密切相关。如果我们有系统截止到（包含） k时刻的所有观测信息，我们可以获得一个对xk的后验估计 ˆx+k。我们用+表示后验估计。获取ˆx+k的 方法是计算后验概率：

ˆx+k=E[xk|y1,y2,…,yk]

如果我们有系统截止到（不包含）k时刻的所有观测值，我们可以获得一个 对xk的先验估计值ˆx−k，我们用−表示先验估计。 即：

ˆx−k=E[xk|y1,y2,…,yk−1]

需要注意的是：ˆx−k和ˆx+k是我们对同一状态 的两种估计。前者基于我们没有获得时刻k的观测值yk；后者基于我 们获得了时刻k的观测值yk。自然，我们期望ˆx+k 要比ˆx−k要准确。

如果我们有时刻k之后的一些观测，基于这些观测，我们要对xk做一 个估计，我们把这个估计叫做平滑估计，用数学描述就是：

ˆxk|k+N=E[xk|y1,y2,…,yk,yk+1,…,yk+N]

N是正整数。

如果我们相基于现有观测值对未来某个时刻进行估计，那么我们有一个预测估计：

ˆxk|k−M=E[xk|y1,y2,…,yk−M]

M是正整数。

对于初始时刻，没有任何观测值，我们用ˆx+0代表对x0 的一个初始估计。第一个观测值y1在k=1时到来，因为我们没有对 x0的任何观测值，所以一个合理的做法是：

ˆx+0=E(x0)

我们用Pk来表示估计误差的协方差矩阵。P−k表示 ˆx−k的估计误差的协方差矩阵； P+k表示 ˆx+k的估计误差的协方差矩阵。即：

P−k=E[(xk−x−k)(xk−x−k)T]P+k=E[(xk−x+k)(xk−x+k)T]

在我们收到yk之前我们有对xk的一个估计ˆx−k 以及这个估计的误差的协方差举着P−k。在时刻k，我们收到了 yk，此时我们有一个对xk的估计x+k和这个估计的误 差的协方差矩阵P+k

我们从时刻0开始，此时我们想有一个对x1状态的先验估计，即：

ˆx−1=F0x0+G0u0

这是一个初始估计，可以推而广之：

ˆx−k=Fk−1x+k−1+Gk−1uk−1

这个是ˆx的时间更新方程。接下来我们推导估计误差的协方差矩阵更 新过程，同样我们从P+0开始：

P+0=E[(x0−ˆx+0)(x0−ˆx+0)T]

给定了p+0，如何得到P−1?

P−1=F0P+0FT0+Q0

同样，我们可以推而广之：

P−k=Fk−1P+k−1FTk−1+Qk−1

这就是P的时间更新方程。

接下来，我们要获得ˆx和P的测量更新方程。给定 ˆx−k我们如何获得ˆx+k呢？我们知道 ˆx−k和ˆx+k是xk的两个估计，不过 一个考虑了yk，一个没有考虑yk。ˆx和P的更新过 程：

Kk=Pk−1HTk(HkPk−1HTk+Rk)−1=PkHTkR−1kˆxk=ˆxk−1+Kk(yk−Hkˆxk−1)Pk=(1−KkHk)Pk−1(1−KkHk)T+KkRkKTk=(P−1k−1+HTkR−1kHk)−1=(I−KkHk)Pk−1

其中ˆxk−1和Pk−1是yk到达之前的估计和协方差估 计；ˆxk和Pk是yk到达之后的估计和协方差估计。 我们用ˆx−k和P−k是yk到达之前的估计和协 方差。ˆx+k和P+k是yk到达之后的估计和协 方差。

我们用ˆx−k代替ˆxk−1，用P−k代替 Pk−1。用ˆx+k代替ˆxk，用P+k 代替Pk。

Kk=P−kHTk(HkP−kHTk+Rk)−1=P+kHTkR−1kˆx+k=ˆx−k+Kk(yk−Hkˆx−k)P+k=(1−KkHk)P−1k(1−KkHk)T+KkRkKTk=((P−k)−1+HTkR−1kHk)−1=(I−KkHk)P−k

上式P+k的第一个表达式叫做Joseph协方差更新矩阵，是一个叫做 Joseph的人做出的结果。这个结果要比第三个表达式更稳定。第一个表达式保证 P+k一定是对称的正定矩阵（只要P−k）是对称正定矩阵。 第三个表达式计算起来比较简单。但是这个表达式不保证P+k是正定 的，也不保证其是对称的。使用Kk的第二个表达式时，必须使用 P+k的第二个表达式。这是因为Kk的第二个表达式依赖于 P+k，所以P+k必须不能再依赖Kk。

Kalman滤波器还有一个非常重要的特性。我们发现P−k，Kk和 P+k的计算都不依赖yk，只依赖于系统参数 Fk,Hk,Qk和Rk。这意味着Kk可以离散计算，保存 起来。在实际的适时系统中，只有ˆxk才需要适时计算。

在之前我们发现，计算先验估计的时候需要上一次迭代的后验估计；计算后验估 计的时候需要上一次迭代的先验估计。在这一节，我们将会看到，如何分别只用 一个方程计算先验估计或者后验估计。即，在计算先验估计的时候只用到上一次 的先验估计；计算后验估计的时候只用到上一次的后验估计。

我们知道：

ˆx−k+1=Fkˆx+k+Gkuk

把ˆx+k的表达式带入式~(25)可以得到：

ˆx−k+1=Fk(ˆx−k+Kk(yk−Hkˆx−k))+Gkuk=Fk(I−KkHk)ˆx−k+FkKkyk+Gkuk

这说明一个先验估计可以根据上一时刻的先验估计直接算出来，不必计算后验估 计。同理，我们可以得到关于估计协方差的先验估计迭代公式。

P−k+1=FkP+kFTk+Qk

我们把P+k的计算公式带入，则有：

P−k+1=Fk(P−k−KkHkP−k)FTk+Qk=FkP−kFTk−FkKkHkP−kFTk+Qk

这个方程叫做Riccati方程，根据这个方程我们可以不用计算P+k而直 接从P−k计算到P−k+1。

同理，我们也可以推导ˆx+k+1和P+k+1的一步计算公 式。

ˆx+k=(I−KkHk)(Fk−1ˆx+k−1+Gk−1uk−1)+KkykP+k=(I−KkHk)(Fk−1P+k−1FTk−1+Qk−1)

本文给出了kalman滤波的基本原理以及一些简化计算的方法。kalman滤波于1960 年被发明。截止目前，已经有50多年。期间，出现了诸多变种。有些侧重于性能 提升，有些侧重于计算量的降低。在实际应用中需要酌情考虑，具体问题具体分 析。