卡尔曼滤波（一）

卡尔曼滤波（一）

我们将从理论和实际案例两个方面讨论卡尔曼滤波器，以下的讨论可能需要一些线性代数和概率论知识。

首先是理论方面：

1. 模型建立

卡尔曼滤波建立在线性代数和隐马尔可夫模型（hidden Markov model）上。我们在这里只讨论最简单的一种模型，其基本动态系统可以用一个马尔可夫链表示，该马尔可夫链建立在一个被高斯噪声（即正态分布的噪声）干扰的线性算子上的。系统的状态可以用一个元素为实数的向量表示。随着离散时间的每一个增加，这个线性算子就会作用在当前状态上，产生一个新的状态，并也会带入一些噪声，同时系统的一些已知的控制器的控制信息也会被加入。同时，另一个受噪声干扰的线性算子产生出这些隐含状态的可见输出。

为了从一系列有噪声的观察数据中用卡尔曼滤波器估计出被观察过程的内部状态，必须把这个过程在卡尔曼滤波的框架下建立模型。也就是说对于每一步 k，定义矩阵 F{k}, H{k}, Q{k}, R{k} ，有时也需要定义 B{k} ，如下。卡尔曼滤波模型假设k时刻的真实状态是从（k−1）时刻的状态演化而来，符合下式：

(1.1)

其中

• F{k} 是作用在 x{k−1} 上的状态变换模型（/矩阵/矢量）。
• B{k} 是作用在控制器向量 u{k} 上的输入－控制模型。
• w{k} 是过程噪声，并假定其符合均值为零，协方差矩阵为 Q{k} 的多元正态分布。w{k} ∼ N(0,Q{k})

在时刻k，对真实状态 x{k} 的一个测量 z{k} 满足下式：

(1.2)

其中

• H{k} 是观测模型，它把真实状态空间映射成观测空间
• v{k} 是观测噪声，其均值为零，协方差矩阵为 R{k} ，且服从正态分布。 v{k} ∼ N(0,R{k})

初始状态以及每一时刻的噪声{x0, w1, ..., wk, v1 ... vk}都认为是互相独立的。

2. 公式推导

定义两个矩阵 P{k|k} 、 P{k|k-1} 满足下式：

(1.3)

(1.4)

其中：

• P{k|k} 表示后验估计误差协方差矩阵， x'{k|k} 叫做后验状态估计量，表示对当前的状态估计量。
• P{k|k-1} 表示先验估计误差协方差矩阵， x'{k|k-1} 叫做先验状态预测量，表示根据第k-1次状态预测的状态量。（这里一定要分清预测量与估计量）

这里说一下先验与后验状态估计量之间的关系。

我们先定义一个 y'{k} 叫做测量余量，表示时刻 k 测量值与根据 k-1 时刻得到的预测值在观测空间中的差异，满足：

(1.5)

然后有，

(1.6)

这里的 K{k} 叫做卡尔曼增益，下面会给出推导。这里可以理解为估计量是预测量和测量值的线性组合，K{k} 决定了两者的权重。

(1.7)

卡尔曼滤波器是一个最小均方误差估计器，后验状态误差估计是

image

最小化这个矢量幅度平方的期望值，

image

这等同于最小化后验估计协方差矩阵P{k|k}的迹（trace）。将式1.7中的项展开，得到：

(1.8)

对未知数 K{k} 求导，导数为零处迹最小，从而解出卡尔曼增益：

image

image

将解出的卡尔曼增益带入式1.8可得：

image

根据式1.1、1.4可得：

image

至此，我们就已经完成了理论公式推导。

3. 总结

预测：

image

更新：

image

4. Tips

以上的公式推导主要是基于矩阵的性质以及矩阵运算的规则。

1. 协方差矩阵是对称正定的。

2. image

5. Reference

1. https://zh.wikipedia.org/wiki/卡尔曼滤波