卡尔曼滤波(一)

正文索引 [隐藏]

公式加载不出请刷新页面

为什么学习卡尔曼滤波

  • 卡尔曼滤波以及其扩展算法能够应用于目标状态估计,如果这个目标是行人,那么就是行人状态估计(或者说行人追踪)
  • 如果这个目标是自身,那么就是车辆自身的追踪(结合一些地图的先验,GPS等数据的话就是自身的定位)。
  • 在很多的无人驾驶汽车项目中,都能找到卡尔曼滤波的扩展算法的身影(比如说EKF,UKF等等)。

什么是卡尔曼滤波

  • 我们通常要对一些事物的状态去做估计,为什么要做估计呢?因为我们通常无法精确的知道物体当前的状态。
  • 为了估计一个事物的状态,我们往往会去测量它,但是我们不能完全相信我们的测量,因为我们的测量是不精准的,它往往会存在一定的噪声,这个时候我们就要去估计我们的状态。
  • 卡尔曼滤波就是一种结合预测(先验分布)和测量更新(似然)的状态估计算法。

卡尔曼滤波推导

1. 例子

  • 干年后,我们的可回收火箭要降落到地球,我们比较关心的状态就是我们的飞行器的高度了,飞行器的高度就是我们想要估计的状态,我们会通过一些传感器去测量我们当前的高度信息,比如说使用气压计。

  • 假如我们每一次测量,当前高度都变成上一次测量的 95%,那么我们就可以得到如下关系:
    Height^{(t)} = 0.95\times Height^{(t-1)}\cdots (Function 1)

  • 我们可以使用递归来表示这样的公式——为了计算我们当前的高度,我们必须知道我们上一个测量的高度,以此递推,我们就会推到我们的飞行器的初始高度。

  • 但是,由于我们的测量往往来自于一些传感器(比如说GPS,气压计),所以测量的结果总是带有噪声的,这个噪声是有传感器本身引起的,那么我们的表达式就变成了:
    Measurement = Height^{(t)} + Noise^{(t)} \cdots(Function 2)

  • 这个噪声我们称为测量噪声(Measurement Noise)。

  • 我们用符号表示以上公式:

x_k :Height; z_k : Measurement; v_k : Noise

x_k = ax_{k-1} \cdots (Function 3)

z_k = x_k +v_k\cdots (Function 4)

  • 第一个式子是我们的 处理模型,是我们的经验(比如说一些运动模型,牛顿力学等等),我们用这种处理模型去 预测 我们考察的事物的状态(在没有任何信息的情况下,或者说在没有任何测量数据的情况下);
  • 第二个式子是测量的表达式,它大致描述了我们测量的组成成分,我们用这个测量去 更新 我们对状态的估计。
  • 其中x_k是我们飞行器当前的状态,x_{k-1}是飞行器上一个状态 (注意!状态为实际值而非测量值)
  • z_k是我们当前对飞行器的测量值,v_k是当前测量到的噪声。a是一个常数,在这个example里是0.95
  • 显然,现实中的运动不会像我们这个简单的处理模型一样按高度比例缩小。
  • 为了简化,我们假设模型很完美,能够大致描述出这颗“神奇火箭”的运动规律,只是偶尔会存在一定的偏差(比如所空气湍流的影响)
  • 那么我们在计算x_k时在加一个噪声来描述我们的处理模型与实际运动的差异,这个噪声我们称之为处理噪声 (Process Noise),我们用 w_t 来表示这种噪声,那么 x_k 的计算公式就变成:

x_k = ax_{k-1} + w_t \cdots (Function 5)

为了简化,后面的分析我们会先忽略处理噪声,但是我们在传感器融合的部分会将处理噪声重新考虑进来。

2. 状态估计

  • 因为我们是要估计飞行器的状态(高度),所以我们把测量的公式变换成(交换律):

x_k = z_k – v_k \cdots(Function 6)

  • 显然,在这里我们没法知道v_k,卡尔曼滤波通过同时考虑上一状态和当前的测量值来获得对当前状态的估计,一般我们用 hat{x} 表示对当前状态x_k的估计,下面我们可以用公式来描述卡尔曼滤波如何结合上一个估计和现在的测量产生对当前的估计的:

hat{x_k} = \hat{x}_{k-1} + g_k(z_k-\hat{x}_{k-1}) \cdots (Function 7)

  • 这里g_k叫做卡尔曼增益(Kalman Gain),它描述的是之前的估计和当前的测量对当前的估计的影响的分配权重,为了理解,我们考虑极端的例子,如果g_k=0,也就是增益为0,那么

  • \hat{x_k} = \hat{x}_{k-1} \cdots (Function 8)

  • 也就是硕我们非常不信任我们当前测量的值,所以直接保留了我们上一次的估计作为我们当前状态的估计。

  • 如果g_k=1即增益为1

  • \hat{x_k} = z_k \cdots(Function 9)

  • 即我们认为当前的测量值十分可靠,我们彻底接受它作为我们当前状态的估计。那么党g_k介于(0,1)之间时,则表示对两者的权益分配。

3. 计算卡尔曼增益

那么如何计算卡尔曼增益呢?

我们使用一种间接的方法,我们虽然不知道测量噪声 v_k 的值,但是我们知道它的均值,前面提到,测量噪声来自传感器本身,并符合高斯分布,所以我们能够从传感去厂商那里获得测量噪声的均值 r **,那么 g_k 可以表示为:
g_k = \frac{p_{k-1}} {(p_{k-1}+r)}\cdots (Function 10)
其中 p_k
测量误差**,表达式为:
p_k = (1-g_k)p_{k-1}\cdots (Function 11)
g_kp_k 的理d解

对于公式10,假设前一次的预测误差 p_{k-1}=0,那么根据公式,当前的增益 g_k=0,这意味着舍弃掉当前的测量而完全采用上一个时刻的估计(上一次的预测数据误差很小,为0,所以我们保留上一次的测量数据)。如果 p_{k-1}=1 那么增益变成 \frac{1}{1+r} 通常r是个很小的数值,所以增益 g_k =1,所以这次我们完全接受测量值作为我们的估计(因为上一次的预测误差太大了,为1,所以一旦拿到了新的测量数据,就干脆吧上次不准确的估计舍弃掉了) 。

对于下面的公式11,分析是一样的,我们考虑极端的例子,当增益为0, p_k = p_{k-1},因为我们舍弃了本次的测量,所以本次的预测误差只能接受上一次的。当增益g_t=1p_k=0

4. 预测和更新

那么现在我们有两个关于火箭高度的公式了,他们分别是:
x_k = ax_{k-1}\cdots(Function12)

\hat{x_k} = \hat{x}_{k-1}+g_k(z_{k}-\hat{x}_{k-1} )\cdots(Function13)

那么我们到底要用哪一个呢?

答案是我们都用!第一个公式我们称之为预测,是基于一些先验的知识(比如说运动模型,牛顿力学等等),觉得我们的状态应该是这样的。第二个公式,就是我们基于我们“不完美的”传感器的测量数据来更新我们对状态的估计。另外,预测,理论上只考虑了一个固定的处理模型和处理噪声,但是由于我们现在是对机械的状态进行估计,在预测的过程中需要对机械本身的控制建模,我们在预测部分再新增一个控制信号,用 bu_k 表示,实际的传感器测量除了会有测量噪声v_k 以外,还会存在一定的关于真实状态的缩放,因此我们使用 x_k 表示测量时通常还会在其前面加一个缩放系数 c 。结合这些我们就可以得到科尔曼滤波预测和更新过程了。

预测 根据上一时刻( k - 1 时刻) 的后验估计值来估计当前时刻( k 时刻) 的状态,得到 k 时刻的先验估计值;
\hat{x_k} = a\hat{x_{k-1}}+bu_k \cdots(Function14)

p_k =ap_{k-1}a\cdots(Function15)

卡尔曼滤波更新过程 使用当前时刻的测量值来更正预测阶段估计值,得到当前时刻的后验估计值
g_k={p_kc\over{cp_kc+r}}\cdots(Function16)

\hat{x_k}\leftarrow\hat{x{k}}+g_k(z_k-c\hat{x{k}})\cdots(Function17)

p_k\leftarrow(1-g_kc)p_{k}\cdots(Function18)

5. 使用线性代数方法来表示预测和更新

卡尔曼滤波器可以分为时间更新方程测量更新方程。时间更新方程(即预测阶段)根据前一时刻的状态估计值推算当前时刻的状态变量先验估计值和误差协方差先验估计值; 测量更新方程(即更新阶段)负责将先验估计和新的测量变量结合起来构造改进的后验估计。时间更新方程和测量更新方程也被称为预测方程校正方程。因此卡尔曼算法是一个递归的预测—校正方法。

注意:相比较上面的公式,我们这里考虑了噪声

预测
\hat{x_k} = A\hat{x_{k-1}}+Bu_k \cdots(Function19)

p_k =Ap_{k-1}A+Q\cdots(Function20)

卡尔曼滤波更新过程
G_k={P_kC^T\over{CP_kC^T+R}}\cdots(Function21)

\hat{x_k}\leftarrow\hat{x_{k‘}}+G_k(z_k-C\hat{x_{k‘}})\cdots(Function22)

P_k\leftarrow(1-G_kC)P_{k’}\cdots(Function23)

至此卡尔曼滤波的完整就推到完了。

以上的19-23五个公式,我们称之为卡尔曼滤波五大公式。

下面来一个个详细剖析每个参数:

  1. \hat{x}_{k-1}\hat{x_k}:分别表示 k - 1 时刻和 k 时刻的后验状态估计值,是滤波的结果之一,即更新后的结果,也叫最优估计(估计的状态,根据理论,我们不可能知道每时刻状态的确切结果所以叫估计)。
  2. \hat{x_{k’}} : k 时刻的先验状态估计值,是滤波的中间计算结果,即根据上一时刻(k-1时刻)的最优估计预测的k时刻的结果,是预测方程的结果。
  3. P_{k-1}P_k:分别表示 k - 1 时刻和 k 时刻的后验估计协方差(即\hat{x}_{k-1}\hat{x_k}的协方差,表示状态的不确定度),是滤波的结果之一。
  4. P_{k‘}:k 时刻的先验估计协方差 (\hat{x_{k’}}的协方差),是滤波的中间计算结果。
  5. C :是状态变量到测量(观测)的转换矩阵,表示将状态和观测连接起来的关系,卡尔曼滤波里为线性关系,它负责将 m 维的测量值转换到 n 维,使之符合状态变量的数学形式,是滤波的前提条件之一。
  6. z_k:测量值(观测值),是滤波的输入。
  7. G_k:滤波增益矩阵,是滤波的中间计算结果,卡尔曼增益,或卡尔曼系数。
  8. A:状态转移矩阵,实际上是对目标状态转换的一种猜想模型。例如在机动目标跟踪中, 状态转移矩阵常常用来对目标的运动建模,其模型可能为匀速直线运动或者匀加速运动。当状态转移矩阵不符合目标的状态转换模型时,滤波会很快发散。
  9. Q:过程激励噪声协方差(系统过程的协方差)。该参数被用来表示状态转换矩阵与实际过程之间的误差。因为我们无法直接观测到过程信号, 所以 Q 的取值是很难确定的。是卡尔曼滤波器用于估计离散时间过程的状态变量,也叫预测模型本身带来的噪声。状态转移协方差矩阵
  10. R:测量噪声协方差。滤波器实际实现时,测量噪声协方差 R一般可以观测得到,是滤波器的已知条件。
  11. B :是将输入转换为状态的矩阵
  12. (z_k-C\hat{x_{k‘}}):实际观测和预测观测的残差,和卡尔曼增益一起修正先验(预测),得到后验。
卡尔曼滤波算法为什么叫滤波算法?
- 一维卡尔曼滤波为例,如果我们单纯的相信测量的信号,那么这个信号是包含噪声的,是很毛糙的
- 但是当我们运行卡尔曼滤波算法去做估计,我们估计的信号会很光滑,看起来似乎滤掉了噪声的影响,所以称之为滤波算法。
- 实际上,卡尔曼滤波不仅仅过滤掉了测量信号的噪声,它同时也结合了以往的估计,卡尔曼滤波在线性问题中被证明是最优估计。

卡尔曼滤波在无人驾驶汽车感知模块的应用

  • 无人驾驶汽车要安全的在道路上行驶,需要“耳听六路,眼观八方”。
  • 那么无人车的耳朵和眼睛是什么呢?那就是安装在无人车上的各种各样的传感器了。
  • 无人车上的传感器能够多达几十个,而且是不同种类的,比如:
    • 立体摄像机
    • 交通标志摄像机
    • 雷达(RADAR)
    • 激光雷达(LIDAR)

立体摄像机往往用于获取图像和距离信息;交通标志摄像机用于交通标志的识别;

雷达一般安装在车辆的前保险杆里面,用于测量相对于车辆坐标系下的物体,可以用来定位,测距,测速等等,容易受强反射物体的干扰,通常不用于静止物体的检测

激光雷达往往安装在车顶,使用红外激光束来获得物体的距离和位置,但是空间分辨率高,但是笨重,容易被天气影响。

由此可知,各种传感器都有其优点和缺点,在实际的无人驾驶汽车里,我们往往结合多种传感器的数据去感知我们的车辆周边的环境。

这种结合各种传感器的测量数据的过程我们称之为传感器融合(Sensor Fusion)。

基于卡尔曼滤波的行人位置估算

卡尔曼滤波虽然简单,确是无人驾驶汽车的技术树中非常重要的一部分,当然,在真实的无人驾驶汽车项目中使用到的技术是相对更加复杂的,但是其基本原理仍然是本博客介绍的这些内容。

在无人驾驶中,卡尔曼滤波主要可以用于一些状态的估计,主要应用于行人,自行车以及其他汽车的状态估计。

下面,我们以行人的状态估计为例展开。

当我们要估计一个行人的状态时,首先就是要建立起要估计的状态的表示。这里,行人的状态大致可以表示为 x=(p,v) ,其中 p 为行人的位置,v 为行人此时的速度。我们用一个向量来表示一个状态:
x=(p_x,p_y,v_x,v_y)^T
在确定了我们要估计的状态以后,我们需要确定一个处理模型,如前文所说,处理模型用于预测阶段来产生一个对当前估计的先验。在本文中,我们先以一个最简单的处理模型来描述行人的运动—恒定速度模型
x_{k+1}=Ax_k+v
即:
x_{k+1 }= \left[ \begin{matrix} 1&0&Delta{t}&0\\ 0&1&0&Delta{t}\\ 0&0&1&0\\ 0&0&0&1 \end{matrix}\right]\cdot \left[ \begin{matrix} p_x\\p_y\\v_x\\v_y \end{matrix} \right]_k+v
之所以称之为恒定速度模型,是因为我们将上面这个行列式展开可以得到:
p_{x}^{k+1}=p_x^k+v_x^k\Delta{t}\\ p_y^{k+1}=p_y^k+v_y^k\Delta{t}\\ v_x^{k+1}=v_x^k\\ v_y^{k+1}=v_y^k
恒定速度处理模型,假定预测的目标的运动规律是恒定的速度的,在行人状态预测这个问题中,很显然行人并不一定会以恒定的速度运动,所以我们的处理模型包含了一定的 处理噪声,在这个问题中处理噪声也被考虑了进来,其中的 v 是我们这个处理模型的处理噪声。在行人状态估计中的处理噪声其实就是行人的加减速,那么我们原来的处理过程就变成了:
x_{k+1 }= \left[ \begin{matrix} 1&0&Delta{t}&0\\ 0&1&0&Delta{t}\\ 0&0&1&0\\ 0&0&0&1 \end{matrix} \right] \cdot \left[ \begin{matrix} p_x\\p_y\\v_x\\v_y \end{matrix} \right]_k+ \left[ \begin{matrix} {1\over2}a_x\Delta{t^2}\\ {1\over2}a_x\Delta{t^2}\\ a_x\Delta{t}\\ a_x\Delta{t} \end{matrix} \right]
我们预测的第二步变成了:
P’=APA^T+Q
这是我们的P的更新过程,它本质上是我们的估计状态概率分布的协方差矩阵。Q 是我们的处理噪声的协方差矩阵,由于处理噪声是随机带入的,所以 ν 本质是一个高斯分布:v~N(0,Q),Q是处理噪声的协方差,Q的形式为
Q = \left[ \begin{matrix} \sigma^2_{p_x}&\sigma_{p_xp_y}&\sigma_{p_xv_x}&\sigma_{p_xv_y}\\ \sigma_{p_yp_x}&\sigma^2_{p_y}&\sigma_{p_yv_x}&\sigma_{p_yv_y}\\ \sigma_{v_xp_x}&\sigma_{v_xp_y}&\sigma^2_{v_x}&\sigma_{v_xv_y}\\ \sigma_{v_yp_x}&\sigma_{v_yp_y}&\sigma_{v_yv_x}&\sigma^2_{v_y}\\ \end{matrix} \right]
进而表示为:
Q=G\cdot G^T\cdot \sigma^2_{v}
其中G=[0.5Δt^2,0.5Δt^2,Δt,Δt]^T, σ^2_{v} 对于行人我们可以设置为0.5m/s

测量步骤中,我们直接可以测量到速度 v_xv_y,所以我们的测量矩阵 C 可以表示为:
C=\left[ \begin{matrix} 0&0&1&0\\ 0&0&0&1 \end{matrix} \right]
测量噪声的协方差矩阵R为:
R= \left[ \begin{matrix} \sigma_{v_x}^2 & 0 \\ 0 & \sigma_{v_y}^2 \end{matrix} \right]
其中 \sigma_{v_x}^2 \sigma_{v_y}^2 描述了我们的传感器的测量能够有多“差”,这是传感器固有的性质,所以往往是传感器厂商提供。

最后,在测量的最后一步,我们使用单位矩阵 I 来代替原式中的1:
P\leftarrow(I-G_kC)P_k
至此,基于恒定速度处理模型卡尔曼滤波的行人状态估计的整个流程我们就讲完了。
参考 http://blog.csdn.net/AdamShan/article/details/78248421