锐单电子商城 , 一站式电子元器件采购平台!
  • 电话:400-990-0325

自动驾驶算法-滤波器系列(一)——详解卡尔曼滤波原理

时间:2022-12-26 19:30:00 2300bk温度传感器

详细说明卡尔曼滤波原理

  • 什么是卡尔曼滤波?
  • 卡尔曼滤波器能做什么?
  • 卡尔曼滤波器是如何看到你的问题的
  • 用矩阵来描述问题
  • 外部控制量
  • 外部干扰
  • 用测量来修正估计值
  • 整合高斯分布
  • 整合所有公式
  • 总结


什么是卡尔曼滤波?

您可以在任何包含不确定信息的动态系统中使用卡尔曼滤波器,并对系统的下一个方向做出基本的预测。即使伴随着各种干扰,卡尔曼滤波器也总出真实情况。

在连续变化的系统中使用卡尔曼滤波是非常理想的,它具有占用内存小的优点(除了前一个状态量外,不需要保留其它历史数据),并且速度很快,很适合应用于实时问题和嵌入式系统。

卡尔曼滤波器能做什么?

以玩具为例:你开发了一个可以在树林里跑来跑去的小机器人,需要知道它的确切位置才能导航。

我们可以说机器人有一个状态 x k ? \vec{x_k} xk ,表示位置和速度:
x ? k = ( p ? , v ? ) \vec{x}_k = (\vec{p}, \vec{v}) x k=(p ,v )

注意这个状态只是关于这个系统基本属性的一堆数字,它可以是任何其它的东西。在这个例子中是位置和速度,它也可以是一个容器中液体的总量,汽车发动机的温度,用户手指在触摸板上的位置坐标,或者任何你需要跟踪的信号。

这个机器人带有GPS,精度大约为10米,还算不错,但是,它需要将自己的位置精确到10米以内。树林里有很多沟壑和悬崖,如果机器人走错了一步,就有可能掉下悬崖,所以只有GPS是不够的。

或许我们知道一些机器人如何运动的信息:例如,机器人知道发送给电机的指令,知道自己是否在朝一个方向移动并且没有人干预,在下一个状态,机器人很可能朝着相同的方向移动。当然,机器人对自己的运动是一无所知的:它可能受到风吹的影响,轮子方向偏了一点,或者遇到不平的地面而翻倒。所以,轮子转过的长度并不能精确表示机器人实际行走的距离,预测也不是很完美。

GPS 传感器告诉了我们一些状态信息,我们的预测告诉了我们机器人会怎样运动,但都只是间接的,并且伴随着一些不确定和不准确性。但是,如果使用所有对我们可用的信息,我们能得到一个比任何依据自身估计更好的结果吗?回答当然是YES,这就是卡尔曼滤波的用处。

卡尔曼滤波是如何看到你的问题的

下面我们继续以只有位置和速度这两个状态的简单例子做解释。
x ⃗ = [ p v ] \vec{x} = \left[\begin{matrix}p\\ v \end{matrix}\right] x =[pv]

我们并不知道实际的位置和速度,它们之间有很多种可能正确的组合,但其中一些的可能性要大于其它部分:

卡尔曼滤波假设两个变量(位置和速度,在这个例子中)都是随机的,并且服从高斯分布。每个变量都有一个均值 μ \mu μ,表示随机分布的中心(最可能的状态),以及方差 σ 2 \sigma^2 σ2,表示不确定性。

在上图中,位置和速度是不相关的,这意味着由其中一个变量的状态无法推测出另一个变量可能的值。下面的例子更有趣:位置和速度是相关的,观测特定位置的可能性取决于当前的速度:

这种情况是有可能发生的,例如,我们基于旧的位置来估计新位置。如果速度过高,我们可能已经移动很远了。如果缓慢移动,则距离不会很远。跟踪这种关系是非常重要的,因为它带给我们更多的信息:其中一个测量值告诉了我们其它变量可能的值,这就是卡尔曼滤波的目的,尽可能地在包含不确定性的测量数据中提取更多信息!
Σ i j \Sigma_{ij} Σij

这种相关性用协方差矩阵来表示,简而言之,矩阵中的每个元素 Σ i j \Sigma_{ij} Σij表示第 i i i个和第 j j j个状态变量之间的相关度。(你可能已经猜到协方差矩阵是一个对称矩阵,这意味着可以任意交换 i i i j j j)。协方差矩阵通常用“ Σ \Sigma Σ”来表示,其中的元素则表示为“ Σ i j \Sigma_{ij} Σij”。

使用矩阵来描述问题

我们基于高斯分布来建立状态变量,所以在时刻 k k k需要两个信息:最佳估计 x ^ k \hat{x}_k x^k(即均值,其它地方常用 μ \mu μ 表示),以及协方差矩阵 P k P_k Pk
x ^ k = [ p o s i t i o n v e l o c i t y ] ,      P k = [ Σ p p Σ p v Σ v p Σ v v ] \hat{x}_k=\left[\begin{matrix}position\\ velocity \end{matrix}\right], \ \ \ \ P_k=\left[\begin{matrix} \Sigma_{pp} & \Sigma_{pv}\\ \Sigma_{vp} & \Sigma_{vv} \end{matrix}\right] x^k=[positionvelocity],    Pk=[ΣppΣvpΣpvΣvv]

(当然,在这里我们只用到了位置和速度,实际上这个状态可以包含多个变量,代表任何你想表示的信息)。接下来,我们需要根据当前状态(k-1 时刻)来预测下一状态(k 时刻)。记住,我们并不知道对下一状态的所有预测中哪个是“真实”的,但我们的预测函数并不在乎。它对所有的可能性进行预测,并给出新的高斯分布。

我们可以用矩阵 F k F_k Fk来表示这个预测过程:

它将我们原始估计中的每个点都移动到了一个新的预测位置,如果原始估计是正确的话,这个新的预测位置就是系统下一步会移动到的位置。那我们又如何用矩阵来预测下一个时刻的位置和速度呢?下面用一个基本的运动学公式来表示:
p k = p k − 1 + Δ t v k − 1 v k =                   v k − 1 p_k = p_{k-1}+\Delta t v_{k-1}\\ v_k = \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ v_{k-1} pk=pk1+Δtvk1vk=                 vk1
即,
x ^ k = [ 1 Δ t 0 1 ] x ^ k − 1 = F k x ^ k − 1 \hat{x}_k=\left[\begin{matrix} 1 & \Delta t\\ 0 & 1 \end{matrix}\right]\hat{x}_{k-1} =F_k \hat{x}_{k-1} x^k=[10Δt1]x^k1=Fkx^k1

现在,我们有了一个预测矩阵来表示下一时刻的状态,但是,我们仍然不知道怎么更新协方差矩阵。此时,我们需要引入另一个公式,如果我们将分布中的每个点都乘以矩阵 A A A,那么它的协方差矩阵 Σ \Sigma Σ会怎样变化呢?很简单,下面给出公式:
C o v ( x ) = Σ C o v ( A x ) = A Σ A T Cov(x)=\Sigma\\ Cov(Ax)=A\Sigma A^T Cov(x)=ΣCov(Ax)=AΣAT
综合以上公式,可得,
x ^ k = F k x ^ k − 1 P k = F k P k − 1 F k T \hat{x}_k=F_k \hat{x}_{k-1}\\ P_k = F_k P_{k-1}F_k^T x^k=Fkx^k1Pk=FkPk1FkT

外部控制量

我们并没有捕捉到一切信息,可能存在外部因素会对系统进行控制,带来一些与系统自身状态没有相关性的改变。

以火车的运动状态模型为例,火车司机可能会操纵油门,让火车加速。相同地,在我们机器人这个例子中,导航软件可能会发出一个指令让轮子转向或者停止。如果知道这些额外的信息,我们可以用一个向量 u ⃗ k \vec{u}_k u k来表示,将它加到我们的预测方程中做修正。

假设由于油门的设置或控制命令,我们知道了期望的加速度 a a a,根据基本的运动学方程可以得到:
p k = p k − 1 + Δ t v k − 1 + 1 2 a Δ t 2 v k = v k − 1 + a Δ t p_k = p_{k-1} + \Delta t v_{k-1} + \frac{1}{2}a \Delta t^2\\ v_k = v_{k-1} + a \Delta t pk=pk1+Δtvk1+21aΔt2vk=vk1+a

相关文章