卡尔曼滤波(二)高级运动模型和扩展卡尔曼滤波EKF

正文索引 [隐藏]

本节主要讲解非线性系统中广泛使用的扩展卡尔曼滤波算法,我们通常将该算法应用于实际的车辆状态估计(或者说车辆追踪)中。另外,实际的车辆追踪运动模型显然不能使用简单的恒定速度模型来建模,在本节中会介绍几种应用于车辆追踪的高级运动模型。并且已经其中的CTRV模型来构造我们的扩展卡尔曼滤波。最后,在代码实例中,我会介绍如何使用EKF做多传感器融合。

应用于车辆追踪的高级运动模型

首先要明确的一点是,不管是什么运动模型,本质上都是为了帮助我们简化问题,所以我们可以根据运动模型的复杂程度(次数)来给我们常用的运动模型分一下类。
一次运动模型(也别称为线性运动模型):

  • 恒定速度模型(Constant Velocity, CV)

  • 恒定加速度模型(Constant Acceleration, CA)

这些线性运动模型假定目标是直线运动的,并不考虑物体的转弯。

二次运动模型:

  • 恒定转率和速度模型(Constant Turn Rate and Velocity,CTRV)
  • 恒定转率和加速度模型(Constant Turn Rate and Acceleration,CTRA)

CTRV目前多用于机载追踪系统(飞机),这些二次运动模型大多假定速度 vv 和 偏航角速度(yaw rate) ωω 没有关系,因此,在这类运动模型中,由于偏航角速度测量的扰动(不稳定),即使车辆没有移动,我们的运动模型下的角速度也会发生细微的变化。

为了解决这个问题,速度 vv 和 偏航角速度 ωω 的关联可以通过设定转向角 ΦΦ 恒定来建立,这样就引出了 恒定转向角和速度模型(Constant Steering Angle and Velocity,CSAV), 另外,速度可以别假定为线性变化的,进而引出了常曲率和加速度模型(Constant Curvature and Acceleration,CCA)。

各大模型之间的关系:

运动模型的状态转移公式

由于除CCA以外,以上的运动模型都非常著名,故本文不提供详细的推导过程。本文提供CV和CTRV模型的状态转移公式。

状态转移公式:就是我们的处理模型由上一状态的估计计算下一个状态的先验分布的计算公式,可以理解为我们基于一定的先验知识总结出来的运动公式。

  1. CV模型

    CV模型的状态空间可以表示为:

    \vec{x}(t)=\left(\begin{matrix}x&y&v_x&v_y\end{matrix}\right)^T

    那么转移函数为:

    \vec{x}(t+\Delta{t})=\left(\begin{matrix}x(t)+\Delta{t}v_x\\y(t)+\Delta{t}v_y\\v_x\\v_y\end{matrix}\right)

  2. CTRV模型

    CTRV中,目标状态量为:

    \vec{x}(t)=\left(\begin{matrix}x&y&v&\theta&\omega\end{matrix}\right)^T

    其中\theta为偏航角,是追踪目标车辆在当前车辆坐标系下与x轴的夹角,逆时针方向为正,取值范围是[0,2\pi),\omega是偏航角速度。

    CTRV状态转移函数为:
    \vec{x}(t+\Delta{t})= \left(\begin{matrix} {v\over\omega}sin(\omega\Delta{t}+\theta)-{v\over\omega}sin(\theta)+x(t)\\ -{v\over\omega}cos(\omega\Delta{t}+\theta)-{v\over\omega}cos(\theta)+y(t)\\ v\\\omega\Delta{t}+\theta\\\omega \end{matrix}\right)

下面的内容将以CTRV模型作为我们的运动模型。使用CTRV还存在一个问题,那就是ω=0 的情况,此时我们的状态转移函数公式中的 (x,y) 将变成无穷大。为了解决这个问题,我们考察一下ω=0 的情况,此时我们追踪的车辆实际上是直线行驶的,所以我们的 (x,y) 的计算公式就变成了:
x(t+\Delta{t})=vcos(\theta)\Delta{t}+x(t)\\y(t+\Delta{t})=vsin(\theta)\Delta{t}+y(t)\\
那么现在问题来了,我们知道,卡尔曼滤波仅仅用于处理线性问题,那么很显然我们现在的处理模型是非线性的,这个时候我们就不能简单使用卡尔曼滤波进行预测和更新了,此时预测的第一步变成了如下非线性函数:
x_{k+1}=g(x_k,u)

其中,g()g() 表示CTRV运动模型的状态转移函数,u 表示处理噪声。为了解决非线性系统下的问题,我们引入扩展卡尔曼滤波(Extended Kalman Filter,EKF)

扩展卡尔曼滤波

雅可比矩阵

扩展卡尔曼滤波使用线性变换来近似非线性线性变换,具体来说,EKF使用一阶泰勒展式来进行线性化:

h\approx h(u)+{\partial{x}\over\partial y}(x-u)

“`数学中,泰勒公式是一个用函数在某点的信息描述其附近取值的公式。如果函数足够平滑的话,在已知函数在某一点的各阶导数值的情况之下,泰勒公式可以用这些导数值做系数构建一个多项式来近似函数在这一点的邻域中的值。泰勒公式还给出了这个多项式和实际的函数值之间的偏差。“

那么我们的处理模型中,状态转移函数为:
\vec{x}(t+\Delta t)=g(x(t))=\left(\begin{matrix} {v\over\omega}sin(\omega\Delta{t}+\theta)-{v\over\omega}sin(\theta)+x(t)\\ -{v\over\omega}cos(\omega\Delta{t}+\theta)-{v\over\omega}cos(\theta)+y(t)\\ v\\\omega\Delta{t}+\theta\\\omega \end{matrix}\right),w\neq0

\vec{x}(t+\Delta t)=g(x(t))=\left(\begin{matrix} v\cos(\theta)\Delta t+x(t)\\ v\sin(\theta)\Delta t+y(t)\\ v\\\omega\Delta{t}+\theta\\\omega \end{matrix}\right),w=0

那么,对于这个多元函数,我们需要使用多元泰勒级数:

T(x)=f(u)+(x-u)Df(u)+{1\over 2!}(x-u)^2D^2f(u)+\ldots

其中,Df(a)雅克比矩阵,他是多元函数中各个因变量关于各个自变量的一阶偏导数构成的矩阵。
J = [{\partial f \over \partial x_1}\cdots{\partial f \over \partial x_n}]= \left[\begin{matrix} {\partial f_1 \over \partial x_1}&\cdots&{\partial f_1 \over \partial x_n} \\ \vdots &\ddots&\vdots\\ {\partial f_m \over \partial x_1}&\cdots&{\partial f_m \over \partial x_n} \\ \end{matrix}\right]
在向量微积分中,雅可比矩阵是一阶偏导数以一定方式排列成的矩阵,其行列式称为雅可比行列式。雅可比矩阵的重要性在于它体现了一个可微方程与给出点的最优线性逼近。因此,雅可比矩阵类似于多元函数的导数。

在扩展卡尔曼滤波中,由于(x−u)本身数值很小,那么 (x−u)^2 就更小了,所以更高阶的级数在此问题中忽略不计,我们只考虑到利用雅可比矩阵进行线性化。

那么接下来就是求解雅可比矩阵,在CTRV模型中,对各个元素求偏导数可以得到雅可比矩阵(\omega \neq 0)
J_A= \left[\begin{matrix} 1&0&{1\over\omega}(sin(\omega\Delta t+\theta)-sin(\theta))&{v\over\omega}(cos(\omega\Delta t+\theta)-cos(\theta))&{\Delta t v\over\omega}cos(\omega\Delta t +\theta)-{v\over\omega^2}(sin(\omega\Delta t+\theta)-sin(\theta))\\ 0&1&{v\over\omega}(sin(\omega\Delta t+\theta)-sin(\theta))&{1\over\omega}(cos(\theta)-cos(\omega\Delta t+\theta))&{\Delta t v\over\omega}sin(\omega\Delta t +\theta)-{v\over\omega^2}(cos(\theta)+cos(\omega\Delta t+\theta))\\ 0&0&1&0&0\\ 0&0&0&1&\Delta t\\ 0&0&0&0&1 \end{matrix}\right]
\omega=0时,雅克比矩阵为:
J_A =\left[\begin{matrix} 1&0&\Delta t\cos(\theta)&-\Delta t v \sin(\theta)&0\\ 0&1&\Delta t\sin(\theta) &\Delta tv\cos(0)&0\\ 0&0&1&0&0\\ 0&0&0&1&\Delta t\\0&0&0&0&1 \end{matrix}\right]
在我们后面的Python实现中,我们将使用numdifftools包直接计算雅可比矩阵,而不需要我们使用代码写这个雅可比矩阵。在得到我们CTRV模型的雅可比矩阵以后,我们的处理模型就可以写成:
x_{k+1}=g(x_k,u) \\P_{k+1}=J_AP_kJ_A^T+Q

处理噪声

处理噪声模拟了运动模型中的扰动,我们引入运动模型的出发点就是要简化我们要处理的问题,这个简化是建立在多个假设的基础上(在CTRV中,这些假设就是恒定偏航角速度和速度),但是在现实问题中这些假设就会带来一定的误差,处理噪声实际上描述了当我们的系统在运行一个指定时间段以后可能面临多大的这样的误差。在CTRV模型中噪声的引入主要来源于两处:直线加速度和偏航角加速度噪声,我们假定直线加速度和偏航角加速度满足均值为 0,方差分别为\sigma_a^2,\sigma_\omega^2的高斯分布,由于均值为 0, 我们在状态转移公式中的 u 就可以不予考虑,我们来看噪声带来的不确定性 Q,直线加速度和偏航角加速度将影响我们的状态量 (x,y,v,\theta,ω),这两个加速度量对我们的状态的影响的表达式如下:
noise_{term} = \left[ \begin{matrix} {1\over2}\Delta t\mu_acos(\theta)\\{1\over2}\Delta t^2\mu_asin(\theta)\\\Delta t\mu_a\\{1\over2}\Delta t^2\mu_\omega\\\Delta t \mu_\omega \end{matrix} \right]
其中,\mu_a,\mu_\omega为直线和转角上的加速度(在这个模型中,我们把他们看做处理噪声),我们分解这个矩阵:
noise_{term} = \left[ \begin{matrix} {1\over2}\Delta t\cos(\theta)&0\\{1\over2}\Delta t^2sin(\theta)&0\\\Delta t\\0&{1\over2}\Delta t^2\\0&\Delta t \end{matrix} \right]\cdot\left[\begin{matrix}\mu_a\\\mu_\omega\end{matrix}\right]=G\cdot \mu
我们知道Q就是处理噪声的协方差矩阵,其表达式为:
Q=E[noise_{term}\cdot noist_{term}^T]=E[G_{\mu\mu^T}]=G\cdot E[\mu\mu^t]\cdot G^T
其中:
E[\mu \mu^T]=\left(\begin{matrix}\sigma_a^2&0\\0&\sigma_\omega^2\end{matrix}\right)
所以,我们在CTRV模型中的处理噪声的协方差矩阵Q的计算公式为:
Q = \left[\begin{matrix} ({1\over2}\Delta t^2\sigma_acos(\theta)^2) & {1\over4}\Delta t^4\sigma_a^2sin(\theta)cos(\theta) & {1\over2}\Delta t^3\sigma_a^2cos(\theta) & 0 & 0\\ {1\over4}\Delta t^4\sigma_a^2sin(\theta)cos(\theta) & ({1\over2}\Delta t^2\sigma_asin(\theta)^2) & {1\over2}\Delta t^3\sigma_a^2sin(\theta) & 0 & 0\\ 0&0&0&({1\over2}\Delta t^2\sigma_\omega)^2&{1\over2}\Delta t^3\sigma_\omega^2\\ 0&0&0&\Delta t^3\sigma_\omega&\Delta t^2\sigma_\omega \end{matrix}\right]

测量

假设我们有激光雷达和雷达两个传感器,它们分别以一定的频率来测量如下数据:

  • 激光雷达:测量目标车辆的坐标 (x,y) 。这里的x,y是相对于我们的车辆的坐标系的,即我们的车辆为坐标系的原点,我们的车头为x轴,车的左侧为y轴。

  • 雷达:测量目标车辆在我们车辆坐标系下与本车的距离 \rho,目标车辆与x轴的夹角 \psi,以及目标车辆与我们自己的相对距离变化率 \dot\rho(本质上就是目标车辆的实际速度在我们和目标车辆的连线上的分量)

前面的卡尔曼滤波器中,我们使用一个测量矩阵 H 将预测的结果映射到测量空间,那是因为这个映射本身就是线性的,现在,我们使用雷达和激光雷达来测量目标车辆(我们把这个过程称为传感器融合),这个时候会有两种情况,即:

  1. 激光雷达的测量模型仍然是线性的,其测量矩阵为:
    H_L= \left[\begin{matrix}1&0&0&0&0\\0&1&0&0&0\end{matrix}\right]

    将预测映射到激光雷达测量空间:

H_L\vec x = (x,y)^T

  1. 雷达的预测映射到测量空间是非线性的,其表达式为:
    \left(\begin{matrix}\rho\\\psi\\\dot\rho\end{matrix}\right) = \left(\begin{matrix}\sqrt{x^2+y^2}\\a\tan_2(y,x)\\{{vx+vy}\over{\sqrt{x^2+y^2}}}\end{matrix}\right)
    此时我们使用 h(x)h(x) 来表示这样一个非线性映射,那么在求解卡尔曼增益时我们也要将该非线性过程使用泰勒公式将其线性化,参照预测过程,我们也只要求解 h(x)h(x)的雅可比矩阵:
    J_H = \left[\begin{matrix}{x\over{\sqrt{x^2+y^2}}}&{y\over{\sqrt{x^2+y^2}}}&0&0&0\\ -{y\over{{x^2+y^2}}}&{x\over{{x^2+y^2}}}&0&0&0\\ {v\over{\sqrt{x^2+y^2}}}-{x(vx+vy)\over{(x^2+y^2)^{3\over2}}}&{v\over{\sqrt{x^2+y^2}}}-{y(vx+vy)\over{(x^2+y^2)^{3\over2}}}&{x+y\over{\sqrt{x^2+y^2}}}&0&0 \end{matrix}\right]

虽然这个雅可比矩阵看似非常复杂,但是我们待会编程的时候并不需要完整的推导出这个雅可比矩阵的表达式,在本篇中,我们采用numdifftools这个公式来直接求解雅可比矩阵。

综上,EKF的整个过程为:

参考 http://blog.csdn.net/adamshan/article/details/78265754