您的位置: 首页  >  文章  >  卡尔曼滤波简介

卡尔曼滤波简介

分类: 文章 • 2023-12-28 07:50:46
如何求出？假定已求出Xt+1，在t+1时刻收到传感器的非直接信息Zt+1，如何对状态Xt+1
  1. 进行更正？

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

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

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

卡尔曼滤波的原理

问题场景如下：一个机器人，我们想知道它实时的状态x⃗ 

，同时也想做到预测未来和修正当下这两件事。

其状态x

表示为一维大小为2的向量，元素分别表示位置信息与速度信息：

x⃗ =[pv]

可是状态x

不一定是精准的，其不确定性用协方差表示：

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

预测未来

只考虑自身状态

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

pkvk=pk−1+Δt=vk−1vk−1

用矩阵表示如下：

x̂ k=[10Δt1]x̂ k−1=Fkx̂ k−1

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

Cov(x)Cov(Ax)=Σ=AΣAT
x̂ kPk=Fkx̂ k−1=FkPk−1FTk

考虑外部状态

外部状态，这里以加速度为例，引入变量a

(uk→

)。

pkvk=pk−1+Δt=vk−1+vk−1+12aΔt2aΔt
x̂ k=Fkx̂ k−1+[Δt22Δt]a=Fkx̂ k−1+Bkuk→

同时，环境仍然存在我们无法刻画的误差，以Qk

表示，最终的预测公式如下：

x̂ kPk=Fkx̂ k−1+Bkuk→=FkPk−1FTk+Qk

从上述式子可见：

  1. 新的最优估计
是之前最优估计的预测加上已知的外界影响的修正。新的不确定度是预测的不确定度加上环境的不确定度
  1. 。

修正当下

我们已得到x̂ k,Pk

，下面要通过观测到的测量值zk→对x̂ k,Pk

进行更新。

因为x̂ k,Pk

和zk→的数据尺度不一定相同，例如x̂ k,Pk包含了笛卡尔的坐标信息，使用radar得到的zk→则包含极坐标信息。所以首先应该把两者放在相同的尺度下去比较，尺度转换使用Hk

将预测信息转化为测量信息的尺度。

μ⃗ expectedΣexpected=Hkx̂ k=HkPkHTk

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

  1. 测量值的分布(x,μ1,σ1)

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

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

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

卡尔曼滤波简介

简单的一维情况如下：

k=σ20σ20+σ21

高维情况下，针对测量值分布(μ1,Σ1)=(zk→,Rk)

与预测值的变化分布(μ0,Σ0)=(Hkx̂ k,HkPkHTk)

组合的高斯分布如下：

x̂ ′kP′k=x̂ k=Pk+–K′(zk→–Hkx̂ k)K′HkPk
K′=PkHTk(HkPkHTk+Rk)−1

总结

预测未来

  • 输入：过去的最优状态（x̂ k−1
，Pk−1）、外界对过程的影响uk→，环境的不确定度Qk。输出：预测的最优状态（x̂ k，Pk）。其他：对过程的描述（Fk，Bk
  • ）跟时间有关。
x̂ kPk=Fkx̂ k−1+Bkuk→=FkPk−1FTk+Qk

修正当下

  • 输入：预测的最优状态（x̂ k
，Pk），测量的状态分布(zk→,Rk)，预测到测量的变换矩阵Hk。输出：经过测量修正的最优状态（x̂ ′k，P′k
  • ）。
x̂ ′kP′k=x̂ k=Pk+–K′(zk→–Hkx̂ k)K′HkPk
K′=PkHTk(HkPkHTk+Rk)−1

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

参考

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

相关推荐