kalman filter

卡尔曼滤波是一种在不确定状况下组合多源信息得到所需状态最优估计的一种方法。本文将简要介绍卡尔曼滤波的原理及推导。

kalman filter_第1张图片

什么是卡尔曼滤波

首先定义问题:对于某一系统,知道当前状态Xt,存在以下两个问题:

  1. 经过时间t后,下个状态Xt+1如何求出?
  2. 假定已求出Xt+1,在t+1时刻收到传感器的非直接信息Zt+1,如何对状态Xt+1进行更正?

这两个问题正是卡尔曼滤波要解决的问题,形式化两个问题如下:

  1. 预测未来
  2. 修正当下

下面,将以机器人导航为例,从预测未来修正当下两个角度介绍卡尔曼滤波器。

卡尔曼滤波的原理

问题场景如下:一个机器人,我们想知道它实时的状态x⃗ ,同时也想做到预测未来修正当下这两件事。

其状态x表示为一维大小为2的向量,元素分别表示位置信息与速度信息:

x⃗ =[pv]

可是状态x不一定是精准的,其不确定性用协方差表示:

Pk=[ΣppΣvpΣpvΣvv]

预测未来

只考虑自身状态

只考虑自身状态的情况下,根据物理公式,可得:

pkvk=pk1+Δt=vk1vk1

用矩阵表示如下:

x̂ k=[10Δt1]x̂ k1=Fkx̂ k1

在状态变化的过程中引入了新的不确定性,根据协方差的乘积公式可得:

Cov(x)Cov(Ax)=Σ=AΣAT

x̂ kPk=Fkx̂ k1=FkPk1FTk

考虑外部状态

外部状态,这里以加速度为例,引入变量a (uk)。

pkvk=pk1+Δt=vk1+vk1+12aΔt2aΔt

x̂ k=Fkx̂ k1+Δt22Δta=Fkx̂ k1+Bkuk

同时,环境仍然存在我们无法刻画的误差,以Qk表示,最终的预测公式如下:

x̂ kPk=Fkx̂ k1+Bkuk=FkPk1FTk+Qk

从上述式子可见:

  1. 的预测加上的修正。
  2. 加上

修正当下

我们已得到x̂ k,Pk,下面要通过观测到的测量值zkx̂ k,Pk进行更新。

因为x̂ k,Pkzk的数据尺度不一定相同,例如x̂ k,Pk包含了的坐标信息,使用radar得到的zk则包含信息。所以首先应该把两者放在相同的尺度下去比较,尺度转换使用Hk将预测信息转化为测量信息的尺度。

μ⃗ expectedΣexpected=Hkx̂ k=HkPkHTk

这样一来,便得到测量尺度上的两个分布:

  1. 测量值的分布(x,μ1,σ1)
  2. 预测值变换后的分布(x,μ0,σ0)

下面一个问题就是如何用这个两个分布组成新的分布。

(x,μ0,σ0)(x,μ1,σ1)=?(x,μ,σ)

kalman filter_第2张图片

简单的一维情况如下:

k=σ20σ20+σ21

高维情况下,针对测量值分布(μ1,Σ1)=(zk,Rk)与预测值的变化分布(μ0,Σ0)=(Hkx̂ k,HkPkHTk)组合的高斯分布如下:

x̂ kPk=x̂ k=Pk+K(zkHkx̂ k)KHkPk

K=PkHTk(HkPkHTk+Rk)1

总结

预测未来

  • 输入:过去的最优状态(x̂ k1Pk1)、外界对过程的影响uk,环境的不确定度Qk
  • 输出:预测的最优状态(x̂ kPk)。
  • 其他:对过程的描述(FkBk)跟时间有关。

x̂ kPk=Fkx̂ k1+Bkuk=FkPk1FTk+Qk

修正当下

  • 输入:预测的最优状态(x̂ kPk),测量的状态分布(zk,Rk),预测到测量的变换矩阵Hk
  • 输出:经过测量修正的最优状态(x̂ kPk)。

x̂ kPk=x̂ k=Pk+K(zkHkx̂ k)KHkPk

K=PkHTk(HkPkHTk+Rk)1

卡尔曼滤波需要内存少,计算速度快,适合实时性情况与嵌入式设备的需要。

参考

  1. How a Kalman filter works, in pictures
  2. Kalman Filter For Dummies

你可能感兴趣的:(kalman filter)