卡尔曼滤波(KF)

    博客分类: 知识归纳 阅读次数:

卡尔曼滤波(KF)

卡尔曼滤波与目标追踪

什么是卡尔曼滤波

为了获得一个事物的状态,我们往往会去测量它,但是我们不能完全相信测量,因为测量往往会存在一定的噪声,这个时候我们就要去估计事物的状态。卡尔曼滤波就是一种结合预测和测量的状态估计算法。

那么什么是预测?预测其实就是根据先验知识给出的状态。先验知识比如牛顿力学、运动模型等。卡尔曼滤波就是预测和测量之间不断循环迭代以获得对状态的最优估计。

过程模型

现在我们要追踪一个一维小车的运动,那么可以定义状态向量为小车的位置和速度 $\bold{x}_t = [x_t, \dot{x}_t]^T$。另外我们记小车的加速度为 $a_t$ 。根据物理知识(先验)有: \(\begin{align*} &x_t = x_{t_1} + \dot{x}_{t-1}\Delta t + \frac{1}{2}a_t\Delta t^2 \\ &\dot{x}_t = \dot{x}_{t-1} + a_t\Delta t \end{align*}\) 写成矩阵形式就是: \(\left[\begin{matrix} x_t \\ \dot{x}_t \end{matrix}\right] = \left[\begin{matrix} 1 & \Delta t \\ 0 & 1 \end{matrix}\right] \left[\begin{matrix} x_{t-1} \\ \dot{x}_{t-1} \end{matrix}\right] + \left[\begin{matrix} \frac{\Delta t^2}{2} \\ \Delta t \end{matrix}\right] a_t\) 在预测阶段,根据先验知识对状态进行建模,给出的模型称为过程模型。上式就是一维小车运动的过程模型。

更一般的,对于一个线性系统,过程模型通常可以写成如下形式: \(\bold{x}_t = \bold{F}_t\bold{x}_{t-1} + \bold{B}_t\bold{u}_{t} \tag{1}\) 其中:

为什么叫运动测量值?我的理解是,在实际问题中,除了要估计的量,另一些量是可以知道的。比如在一维小车运动中,我们认为加速度$a_t=f_t/m$, 推力和车的质量是我们知道的。

过程模型推导出的状态和真值之间存在差别,比如小车的运动还受风阻等影响,不完全符合上诉推导,因此,状态的真值通常可以如下表示: \(\bold{x}_t = \bold{F}_t\bold{x}_{t-1} + \bold{B}_t\bold{u}_{t} + \omega_t \tag{2}\)

观察(2)式,其三个部分分别表示:状态转换、外界作用力、外界噪声

预测更新

有了过程模型,我们就可以对状态进行更新: \(\bold{\hat{x}}_{t} = \bold{F}_t\bold{\hat{x}}_{t-1} + \bold{B}_t\bold{u}_{t} + \hat\omega_t = \bold{F}_t\bold{\hat{x}}_{t-1} + \bold{B}_t\bold{u}_{t}\tag{3}\) $\bold{\hat{x}}_{t}$ 则表示 $t$ 时刻卡尔曼滤波的状态估计。$\hat\omega_t$ 是对状过程噪声的估计,由于过程噪声均值为0,因此取0.

预测更新实际上相当于“加法”:将当前状态转换到下一时刻,再把外界的控制作用叠加上去。这两步都会导致估计值和真值之间的误差增加。我们可以通过状态向量的真值与估计的协方差变化来验证这一点。

假设 $\bold{x}_{t}$ 为 $t$ 时刻下状态向量的真值,那么有: \(\bold{x}_{t} = \bold{F}_t\bold{x}_{t-1} + \bold{B}_t\bold{u}_{t} + \omega_t \tag{4}\) 可以计算状态向量真值与估计值的协方差来表示预测误差: \(\begin{align*} \bold{P}_t &= E[(\bold{x}_t - \bold{\hat{x}_t})(\bold{x}_t - \bold{\hat{x}_t})^T] \\ &=E[(F_t(\bold{x}_{t-1} - \bold{\hat{x}}_{t-1})+ \omega_t)(F_t(\bold{x}_{t-1} - \bold{\hat{x}}_{t-1})+ \omega_t)^T]\\ &=\bold{F}_tE[(\bold{x}_{t-1} - \bold{\hat{x}}_{t-1})\cdot(\bold{x}_{t-1} - \bold{\hat{x}}_{t-1})^T]\bold{F}_t^T\\ &+\bold{F}_tE[(\bold{x}_{t-1} - \bold{\hat{x}}_{t-1})\omega_t^T]\bold{F}_t^T\\ &+\bold{F}_tE[(\omega_t\cdot(\bold{x}_{t-1} - \bold{\hat{x}}_{t-1})^T]\bold{F}_t^T\\ &+E[\omega_t\omega_t^T] \end{align*}\) 由于状态向量和噪声是不相关的,则上市中间两项为0,可以简化为: \(\begin{align*} \bold{P}_t &= \bold{F}_tE[(\bold{x}_{t-1} - \bold{\hat{x}}_{t-1})\cdot(\bold{x}_{t-1} - \bold{\hat{x}}_{t-1})^T]\bold{F}_t^T+E[\omega_t\omega_t^T]\\ &=\bold{F}_t\bold{P}_{t-1}\bold{F}_t^T+\bold{Q}_t \tag{5} \end{align*}\) 可以看到,误差差变大了。

测量更新

从上节可以看出,如果只使用过程模型来对状态进行更新,那么状态估计的误差会越来越大。因此,需要用测量值对预测值进行修正。这就是“测量更新”。

假设得到了状态的测量值 $\bold{z}_t$,可如下作状态更新: \(\begin{align*} \bold{\hat{x}}_t^{\prime} &= \bold{\hat{x}}_t + \bold{K}_t(\bold{z}_t - \bold{H}_t\bold{\hat{x}}_t ) \tag{6}\\ \bold{P}_t^{\prime} &= \bold{P}_t - \bold{K}_t\bold{H}_t\bold{P}_t \tag{7} \end{align*}\) 其中, \(\bold{K}_t = \bold{P}_t\bold{H}_t^T(\bold{H}_t\bold{P}_t\bold{H}_t^T + \bold{R}_t)^{-1} \tag{8}\) 为卡尔曼增益。

测量更新的推导

回到(3)式,如果我们不将过程噪声取均值,将 $\bold{\hat{x}}_{t}$ 看做一个随机变量,那么预测更新的结果可以如下表示: \(p_1(x;\mu_1, \sigma_1) = \frac{1}{\sqrt{2\pi\sigma_1^2}}e^{-\frac{(x-\mu_1)^2}{2\sigma_1^2}} \tag{9}\) $\mu_1$ 为这个高斯分布的均值, $\sigma_1$ 为方差,而 $x$ 为小车的可能位置, $p_1$ 为某个可能位置的概率。

假设在 $t$ 时刻,通过某测距仪测得小车距离原点的距离 $x$,由于测量包含噪声(且假设其为高斯噪声),因此该测量值也可以利用高斯概率分布来表示: \(p_2(x;\mu_2, \sigma_2) = \frac{1}{\sqrt{2\pi\sigma_2^2}}e^{-\frac{(x-\mu_2)^2}{2\sigma_2^2}} \tag{10}\) 现在有了两个关于小车位置的估计。我们希望能够融合两个估计来得到更精确的估计。卡尔曼滤波采用的融合方式是“乘法”,即将两个高斯分布乘起来: \(\begin{align*} p_3(x;\mu_3, \sigma_3) =p_3(x;\mu_1, \sigma_1,\mu_2, \sigma_2) = \frac{1}{\sqrt{2\pi\sigma_1^2}}e^{-\frac{(x-\mu_1)^2}{2\sigma_1^2}}\cdot\frac{1}{\sqrt{2\pi\sigma_2^2}}e^{-\frac{(x-\mu_2)^2}{2\sigma_2^2}}=S\cdot\frac{1}{\sqrt{2\pi\sigma_3^2}}e^{-\frac{(x-\mu_3)^2}{2\sigma_3^2}} \tag{11} \end{align*}\) 其中: \(\begin{align*} \mu_3 &= \frac{\mu_2\sigma_1^2+\mu_1\sigma_2^2}{\sigma_1^2+\sigma_2^2}=\mu_1+\frac{\sigma_1^2(\mu_2-\mu_1)}{\sigma_1^2+\sigma_2^2} \\ \sigma_3^2 &= \frac{\sigma_1^2\sigma_2^2}{\sigma_1^2+\sigma_2^2}=\sigma_1^2-\frac{\sigma_1^4}{\sigma_1^2+\sigma_2^2}\\ S &=\frac{1}{\sqrt{2\pi(\sigma_1^2+\sigma_2^2)}}e^{-\frac{(\mu_1-\mu_2)^2}{2(\sigma_1^2+\sigma_2^2)}} \end{align*}\)

详细推导参见两个高斯分布乘积的理论推导

可以看出,两个高斯分布相乘后是一个乘了缩放系数的高斯分布。卡尔曼滤波就采用这个新高斯分布的均值来做最优估计。

两个分布相乘,其实就是寻找他们的重叠部分

转换矩阵H的引入

上面假设测距仪直接给出了小车距原点的距离。但实际上测距仪给出的可能不是时间,而是光信号的飞行时间$t$,单位为秒(s)。即测量值是这样的: \(p_2^{\prime}(t;\mu_2, \sigma_2) = \frac{1}{\sqrt{2\pi\sigma_2^2}}e^{-\frac{(t-\mu_2)^2}{2\sigma_2^2}} \tag{12}\) 由于$x=c\cdot t$ (其中 $c$ 为光速),那么我们可以得到关于位置的测量值分布: \(p_2(x;\mu_2, \sigma_2) =\frac{1}{\sqrt{2\pi\sigma_2^2c^2}}e^{-\frac{(x-c\mu_2)^2}{2\sigma_2^2c^2}} \tag{13}\) 这样又可以相乘了,那么: \(p_3(x;\mu_3, \sigma_3) = S\cdot\frac{1}{\sqrt{2\pi\sigma_3^2}}e^{-\frac{(x-\mu_3)^2}{2\sigma_3^2}} \tag{14}\) 其中: \(\begin{align*} \mu_3 &=\mu_1+\frac{\sigma_1^2(c\mu_2-\mu_1)}{\sigma_1^2+\sigma_2^2c^2} = \mu_1+K(\mu_2-H\mu_1) \tag{15} \\ \sigma_3^2 &=\sigma_1^2-\frac{\sigma_1^4}{\sigma_1^2+c^2\sigma_2^2} = \sigma_1^2 - KH\sigma_1^2 \tag{16}\\ S &=\frac{1}{\sqrt{2\pi(\sigma_1^2+c^2\sigma_2^2)}}e^{-\frac{(\mu_1-c\mu_2)^2}{2(\sigma_1^2+c^2\sigma_2^2)}} \end{align*}\)

其中: \(\begin{align*} H &= \frac{1}{c} \\ K &= \frac{H\sigma_1^2}{H^2\sigma_1^2+\sigma_2^2} \tag{17} \end{align*}\) $K$ 称为卡尔曼增益

对比 (6)(7) 和 (15)(16) 式:

总结

卡尔曼滤波的过程可以总结如下:

  1. 通过先验知识对目标的状态进行预测:

    • 输入:过去的最优状态 $(\bold{\hat{x}}_{t-1}, \bold{P}_{t-1})$,外界对过程的影响 $(\bold{B}_t, \bold{u}_t)$,过程噪声的协方差 $\bold{Q}_t$,状态转移矩阵 $\bold{F}_t$
    • 输出:对状态的预测 $(\bold{\hat{x}}_{t}, \bold{P}_t)$
    \[\begin{align*} \bold{\hat{x}}_{t} &= \bold{F}_t\bold{\hat{x}}_{t-1} + \bold{B}_t\bold{u}_{t} + \hat\omega_t = \bold{F}_t\bold{\hat{x}}_{t-1} + \bold{B}_t\bold{u}_{t}\\ \bold{P}_t &=\bold{F}_t\bold{P}_{t-1}\bold{F}_t^T+\bold{Q}_t \end{align*}\]
  2. 通过观测值对预测值进行修正:

    • 输入:状态的预测 $(\bold{\hat{x}}_{t}, \bold{P}_t)$,测量值 $(\bold{z}_t, \bold{R}_t)$,预测到测量转换矩阵 $H_t$
    • 输出:经过测量修正的对状态的估计 $(\bold{\hat{x}}_{t}^{\prime}, \bold{P}_t^{\prime})$
    \[\begin{align*} \bold{\hat{x}}_t^{\prime} &= \bold{\hat{x}}_t + \bold{K}_t(\bold{z}_t - \bold{H}_t\bold{\hat{x}}_t ) \\ \bold{P}_t^{\prime} &= \bold{P}_t - \bold{K}_t\bold{H}_t\bold{P}_t \end{align*}\]

    其中, \(\bold{K}_t = \bold{P}_t\bold{H}_t^T(\bold{H}_t\bold{P}_t\bold{H}_t^T + \bold{R}_t)^{-1}\) 为卡尔曼增益。

回顾测量更新的推导过程,卡尔曼滤波实际上是融合两个高斯分布的方法,有没有过程模型不重要。只要有两个高斯分布,那么融合两者得到一个更精确的估计,就可以用卡尔曼滤波。这里过程噪声的存在,使得过程更新的结果正好是个高斯分布。

为什么叫滤波:融合两个分布得到更精确的估计,降低了噪声,因此可以叫滤波

卡尔曼滤波的局限性在于其只能拟合线性高斯系统。但其最大的优点在于计算量小,能够利用前一时刻的状态(和可能的测量值)来得到当前时刻下的状态的最优估计。

我们在做卡尔曼的时候最关键就是找出+两个高斯分布的均值和方差,然后就可以套公式了

参考