Kalman Filter

Posted on Edited on In Waline:

本文结合网络资源对经典卡尔曼滤波算法进行整理。

卡尔曼滤波器

1 算法介绍

考虑一种状态估计算法，已知两个值：

  • 前一时刻状态的估计值 X^k−1\hat{X}_{k-1}

  • 当前状态的测量值 ZkZ_k

要求解当前时刻的估计值 X^k\hat{X}_k。

考虑到已知的两个值都带有噪声，一种思路则是结合这两个已知值，例如使用加权的方式：

X^k=αkZk+(1−αk)X^k−1\hat{X}_k=\alpha_k Z_k + (1-\alpha_k) \hat{X}_{k-1}

其中：

  • kk 为离散的时间间隔，例如第 kk 帧。
  • ZkZ_k 为测量值，通常伴随着噪声。
  • αk\alpha_k 为卡尔曼增益（Kalman Gain)，是未知量，用于将已知量加权。可以是固定值，但是如果根据当前状态变换这个增益，将会对当前状态的估算有很好的帮助。

卡尔曼滤波算法的思想就是利用上一状态以及当前状态测量去估计当前状态，但是模型会比上面更加复杂。

2 算法模型

卡尔曼滤波利用线性随机差分方程（Linear Stochastic Difference equation），根据上一个系统状态估计当前系统状态：

xk=Axk−1+Buk−1+wx_k = Ax_{k-1} + Bu_{k-1} + w

使用时一般忽略 uu 得到

xk=Axk−1+w(1)x_k = Ax_{k-1} + w \tag{1}

测量值和状态值的线性关系描述为

zk=Hxk+v(2)z_k = Hx_k + v \tag{2}

其中：

  • k−1k-1 和 kk 标识上一状态和当前状态
  • x∈Rnx\in R^n 表示状态估计值
  • A∈Rn×nA\in R^{n\times n} 表示上一状态到当前状态的转换矩阵
  • u∈Rlu\in R^l 表示可选择的状态输入值，可以忽略
  • B∈Rn×lB\in R^{n\times l} 表示控制输入到当前状态的转换矩阵
  • w∈Rnw\in R^n 表示从上一状态到当前状态的过程噪声
  • z∈Rmz\in R^m 表示状态测量值
  • H∈Rm×nH\in R^{m\times n} 当前状态到当前测量的转换矩阵
  • v∈Rmv\in R^m 表示仪器产生的测量噪声
  • 式中 A,B,H,w,vA, B, H, w, v 随着状态变化而变化，式中表示时没有添加下标，假设这些值不变

式中的噪声 ww 和 vv 是相互独立的，且是高斯白噪声，服从如下分布：

p(w)∼N(0,Q)p(v)∼N(0,R)Q=E(wwT), R=E(vvT)E(w)=0, E(v)=0(3)p(w)\sim N(0,Q)\\ p(v)\sim N(0,R)\\ Q=E(ww^T),\ R=E(vv^T)\\ E(w)=0,\ E(v)=0 \tag{3}

其中：

  • Q∈Rn×nQ\in R^{n\times n} 和 R∈Rm×mR\in R^{m\times m} 分别表示噪声 ww 和 vv 的协方差矩阵，也会随着状态的变化而改变

注（高斯白噪声）：

  • 高斯白噪声（White Gaussian Noise）中的高斯是指概率分布是正态函数，而白噪声是指它的二阶矩不相关，一阶矩为常数，是指先后信号在时间上的相关性。
  • 定义：如果一个噪声，它的瞬时值服从高斯分布，而它的功率谱密度又是均匀分布的，则称它为高斯白噪声。

3 算法推导

定义：

  • x^ˉk\bar{\hat{x}}_k 表示不考虑过程噪声的情况下，仅利用过程先验知识求出的当前状态的先验状态估计。
  • x^k\hat{x}_k 表示利用测量值 zkz_k 求出的当前状态的后验状态估计，也是最终求得的状态。卡尔曼滤波的最终估计就是求得 x^k\hat{x}_k，即对状态的最优估计。
  • zˉk\bar{z}_k 示忽略测量噪声根据当前先验状态得到的无噪声测量值。

由定义，在不考虑噪声的情况下，有

x^ˉk=Ax^k−1+Buk−1\bar{\hat{x}}_k = A\hat{x}_{k-1} + Bu_{k-1}

不考虑 uu，有

x^ˉk=Ax^k−1(4)\bar{\hat{x}}_k = A\hat{x}_{k-1} \tag{4}

同样，忽略测量噪声有

zˉk=Hx^ˉk\bar{z}_k=H\bar{\hat{x}}_k

有测量值减去无噪声由上一状态预测的测量值

z˙k=zk−zˉk=zk−Hx^ˉk\dot{z}_k = z_k - \bar{z}_k = z_k - H \bar{\hat{x}}_k

  • z˙k\dot{z}_k 表示实际测量值（包含过程噪声及测量噪声等）减去无噪声测量值，表示预测的测量值和实际测量值的差异。实际得到是过程噪声和测量噪声对当前测量值的影响，也就是包含了 ww 和 vv。

现在，可以把无噪声的状态估计 x^ˉk\bar{\hat{x}}_k 和噪声变量 z˙k\dot{z}_k 按照一定的组合方式得到后验状态估计，卡尔曼滤波中使用了按照比例组合的方式：

x^k=x^ˉk+αkz˙k=x^ˉk+αk(zk−Hx^ˉk)(5)\hat{x}_k = \bar{\hat{x}}_k + \alpha_k \dot{z}_k = \bar{\hat{x}}_k + \alpha_k (z_k - H \bar{\hat{x}}_k) \tag{5}

  • αk∈Rn×m\alpha_k\in R^{n\times m} 表示卡尔曼增益。

为求解 x^k\hat{x}_k，由于式中其他量已知，因此仅需要求出 αk\alpha_k。先定义先验和后验误差：

eˉk=xk−x^ˉkek=xk−x^k\bar{e}_k = x_k - \bar{\hat{x}}_k\\ e_k = x_k - \hat{x}_k

对应的误差协方差矩阵为：

Pˉk=E[eˉkeˉkT]Pk=E[ekekT]\bar{P}_k = E[\bar{e}_k \bar{e}_k^T]\\ P_k = E[e_k e_k^T]

为了使后验状态估计 x^k\hat{x}_k 最优，目标是最小化后验状态估计的误差协方差矩阵 PkP_k。

4 后验误差

根据公式 (1)，(2)，(4)，(5) 有

ek=xk−x^k=(I−αkH)eˉk−αkv\begin{aligned} e_k &= x_k - \hat{x}_k\\ &= (I-\alpha_kH) \bar{e}_k - \alpha_k v \end{aligned}

可得

ekekT=[(I−αkH)eˉk−αkv]⋅[(I−αkH)eˉk−αkv]T=(I−αkH)ekˉeˉkT(I−αkH)T−αkveˉkT(I−αkH)T−(I−αkH)eˉkvTαkT+αkvvTαkT\begin{aligned} e_k e_k^T &= [(I-\alpha_kH) \bar{e}_k - \alpha_k v] \cdot [(I-\alpha_kH) \bar{e}_k - \alpha_k v]^T\\ &= (I-\alpha_k H) \bar{e_k} \bar{e}_k^T (I-\alpha_k H)^T - \alpha_k v \bar{e}_k^T (I-\alpha_kH)^T - (I-\alpha_k H) \bar{e}_k v^T \alpha_k^T + \alpha_k v v^T \alpha_k^T \end{aligned}

又由于 eˉk\bar{e}_k 是 z1,...,zk−1z_1, ..., z_{k-1} 的线性函数，与当前 zkz_k 无关，因此

E(eˉkvT)=0E(veˉkT)=0E(\bar{e}_k v^T) = 0\\ E(v \bar{e}_k^T) = 0

带入协方差中，整理得到

Pk=(I−αkH)Pˉk(I−αkH)T+αkRαkT=Pˉk−αkHPˉk−PˉkHTαkT+αkHPˉkHT+αkRαkT\begin{aligned} P_k &= (I-\alpha_k H) \bar{P}_k (I-\alpha_k H)^T + \alpha_k R \alpha_k^T\\ &= \bar{P}_k - \alpha_k H \bar{P}_k - \bar{P}_k H^T \alpha_k^T + \alpha_k H \bar{P}_k H^T + \alpha_k R \alpha_k^T \end{aligned}

合并包含 αk\alpha_k 的项

Pk=Pˉk−PˉkHT(HPˉkHT+R)−1HPˉk+[αk−PˉkHT(HPˉkHT+R)−1](HPˉkHT+R)[αk−PˉkHT(HPˉkHT+R)−1]T\begin{aligned} P_k &= \bar{P}_k - \bar{P}_k H^T (H \bar{P}_k H^T + R)^{-1} H \bar{P}_k\\ &+[\alpha_k - \bar{P}_k H^T (H \bar{P}_k H^T + R)^{-1}](H \bar{P}_k H^T + R) [\alpha_k - \bar{P}_k H^T (H \bar{P}_k H^T + R)^{-1}]^T \end{aligned}

由上式，前两项中不包含 αk\alpha_k，因此最小化 PkP_k 只需

αk−PˉkHT(HPˉkHT+R)−1=0\alpha_k - \bar{P}_k H^T (H \bar{P}_k H^T + R)^{-1} = 0

即

αk=PˉkHT(HPˉkHT+R)−1(6)\alpha_k = \bar{P}_k H^T (H \bar{P}_k H^T + R)^{-1} \tag{6}

从而有

Pk=(I−αkH)Pˉk(7)P_k = (I - \alpha_k H) \bar{P}_k \tag{7}

5 先验误差

由公式 (1) 和 (4) 可得

eˉk=xk−x^ˉk=Aek−1+weˉkeˉkT=Aek−1ek−1TAT+wwT+wek−1TAT+Aek−1wT\bar{e}_k = x_k - \bar{\hat{x}}_k = Ae_{k-1} + w\\ \bar{e}_k \bar{e}_k^T = A e_{k-1} e_{k-1}^T A^T + w w^T + w e_{k-1}^T A^T + A e_{k-1} w^T

又

E(ek−1wT)=0E(wek−1T)=0E(e_{k-1} w^T) = 0\\ E(w e_{k-1}^T) = 0

因此

Pˉk=E[eˉkeˉkT]=APk−1AT+Q(8)\bar{P}_k = E[\bar{e}_k \bar{e}_k^T] = A P_{k-1} A^T + Q \tag{8}

6 分析

若 Pˉk=0\bar{P}_k = 0，那么有 αk=0\alpha_k = 0，可以得到 x^k=x^ˉk=Ax^k−1\hat{x}_k = \bar{\hat{x}}_k = A\hat{x}_{k-1}，即完全相信之前的估计值，这也是不能设置 P0=0P_0 = 0 的原因。

7 公式

通过上面公式 (3) — (8)，可以完成卡尔曼滤波的求解过程。将迭代过程分为两个方程集合

Time Update Measurement Update
x^ˉk=Ax^k−1+(Buk−1)\bar{\hat{x}}_k = A\hat{x}_{k-1} + (B u_{k-1}) αk=PˉkHT(HPˉkHT+R)−1\alpha_k = \bar{P}_k H^T (H \bar{P}_k H^T + R)^{-1}
Pˉk=APk−1AT+Q\bar{P}_k = A P_{k-1} A^T + Q x^k=x^ˉk+αk(zk−Hx^ˉk)\hat{x}_k = \bar{\hat{x}}_k + \alpha_k (z_k - H \bar{\hat{x}}_k)
Pk=(I−αkH)PˉkP_k = (I - \alpha_k H) \bar{P}_k

8 迭代过程

通过上面的公式，就可以计算状态估计，但是公式中一些参数未知，需要先定义：

  • uu 一般被忽略
  • A,B,HA, B, H 对于每个状态都是相同的，大部分情况下是单位阵
  • RR 是仪器的噪声协方差
  • QQ 较难获得，一般系统过程较为稳定，即认为没有过程噪声
  • zz 所有状态的测量值都是由仪器测量得到而已知的

RR 和 QQ 的取值不是特别重要，虽然两个参数估计结果不准确，但实际情况中卡尔曼滤波算法也可以得到比较准确的状态估计。当然对噪声参数的精确也会对结果的准确度有帮助。

因此只需要传入基本的 x^0\hat{x}_0 和 P0P_0 即可以开始进行迭代。需要注意的是，P0P_0 一般不取 00。

算法迭代过程如下：

0. 初始化 k=0Time Update1. 估计状态x^ˉk=Ax^k−1+Buk2. 计算先验误差协方差Pˉk=APk−1AT+QMeasurement Update3. 计算卡尔曼增益αk=PˉkHT(HPˉkHT+R)−14. 用 zk 更新状态估计x^k=x^ˉk+αk(zk−Hx^ˉk)5. 更新误差协方差Pk=(I−αkH)Pˉk6. 第 k 步的输出作为第 k+1 步的输入0. \ 初始化\ k = 0\\ \\ \mathrm{Time\ Update}\\ 1.\ 估计状态\\ \bar{\hat{x}}_k = A \hat{x}_{k-1} + B u_k\\ 2.\ 计算先验误差协方差\\ \bar{P}_k = AP_{k-1}A^T + Q\\ \\ \mathrm{Measurement\ Update}\\ 3.\ 计算卡尔曼增益\\ \alpha_k = \bar{P}_k H^T (H \bar{P}_k H^T + R)^{-1}\\ 4.\ 用\ z_k\ 更新状态估计\\ \hat{x}_k = \bar{\hat{x}}_k + \alpha_k (z_k - H \bar{\hat{x}}_k)\\ 5.\ 更新误差协方差\\ P_k = (I-\alpha_k H) \bar{P}_k\\ \\ 6.\ 第\ k\ 步的输出作为第\ k+1\ 步的输入\\

可以理解为 x^ˉk\bar{\hat{x}}_k 为依靠前一时刻的状态 x^k−1\hat{x}_{k-1} 得到的当前时刻的估计值，但是存在误差和噪声，可以描述为 x^ˉk=x^k∣k−1\bar{\hat{x}}_k = \hat{x}_{k|k-1}；先验误差同理可以描述为 Pˉk=Pk∣k−1\bar{P}_k = P_{k|k-1}。然后，经过计算 Kalman 增益，更新修正当前状态的估计值 x^k=x^k∣k\hat{x}_k = \hat{x}_{k|k}，同时得到后验误差 Pk=Pk∣kP_k = P_{k|k}。

参考链接