Extended Kalman Filter（扩展卡尔曼滤波）是卡尔曼滤波的非线性版本。在状态转移方程确定的情况下，EKF已经成为了非线性系统状态估计的事实标准。本文将简要介绍EKF，并介绍其在无人驾驶多传感器融合上的应用。

KF与EKF

本文假定读者已熟悉KF，若不熟悉请参考卡尔曼滤波简介。

KF与EKF的区别如下：

预测未来：x′=Fx+ux'=Fx+u用x′=f(x,u)x'=f(x,u)代替；其余FF用FjF_j代替。
修正当下：将状态映射到测量的Hx′Hx'用h(x′)h(x')代替；其余HH用HjH_j代替。

其中，非线性函数f(x,u)，h(x′)f(x,u)，h(x')用非线性得到了更精准的状态预测值、映射后的测量值；线性变换Fj，HjF_j，H_j通过线性变换使得变换后的x，zx，z仍满足高斯分布的假设。

Fj，HjF_j，H_j计算方式如下：

Fjb=∂f(x,u)∂x=∂h(x′)∂x

\begin{split} F_j &= \frac{\partial f(x, u)}{\partial x} \\ b &= \frac{\partial h(x')}{\partial x} \end{split}

为什么要用EKF

KF的假设之一就是高斯分布的xx预测后仍服从高斯分布，高斯分布的xx变换到测量空间后仍服从高斯分布。可是，假如F、HF、H是非线性变换，那么上述条件则不成立。

将非线性系统线性化

既然非线性系统不行，那么很自然的解决思路就是将非线性系统线性化。

对于一维系统，采用泰勒一阶展开即可得到：

f(x)≈f(μ)+∂f(μ)∂x(x−μ)

f(x) \approx f(\mu) + \frac{ \partial f(\mu)}{\partial x}(x-\mu)

对于多维系统，仍旧采用泰勒一阶展开即可得到：

T(x)≈f(a)+(x−a)TDf(a)

T(x) \approx f(a) + (x-a)^T Df(a)

其中，Df(a)Df(a)是Jacobian矩阵。

多传感器融合

lidar与radar

本文将以汽车跟踪为例，目标是知道汽车时刻的状态x=(px,py,vx,vy)x = (p_x, p_y, v_x, v_y)。已知的传感器有lidar、radar。

lidar：笛卡尔坐标系。可检测到位置，没有速度信息。其测量值z=(px,py)z=(p_x, p_y)。
radar：极坐标系。可检测到距离，角度，速度信息，但是精度较低。其测量值z=(ρ,ϕ,ρ˙)z=(\rho, \phi, \dot{\rho})，图示如下。

传感器融合步骤

步骤图如上所示，包括：

收到第一个测量值，对状态xx进行初始化。
预测未来
修正当下

初始化

初始化，指在收到第一个测量值后，对状态xx进行初始化。初始化如下，同时加上对时间的更新。

对于radar来说，

⎡⎣⎢⎢⎢⎢pxpyvxvy⎤⎦⎥⎥⎥⎥=⎡⎣⎢⎢⎢⎢10000100⎤⎦⎥⎥⎥⎥[pxpy]

\left[\begin{matrix}p_x \\p_y \\v_x \\v_y\end{matrix}\right]= \left[\begin{matrix}1 & 0 \\0 & 1 \\0 & 0 \\0 & 0\end{matrix}\right] \left[\begin{matrix}p_x \\p_y \end{matrix}\right]

对于radar来说，

⎡⎣⎢⎢⎢⎢pxpyvxvy⎤⎦⎥⎥⎥⎥=⎡⎣⎢⎢⎢⎢ρcosϕρsinϕρ˙cosϕρ˙sinϕ⎤⎦⎥⎥⎥⎥

\left[\begin{matrix}p_x \\p_y \\v_x \\v_y\end{matrix}\right]= \left[\begin{matrix}\rho \cos{\phi} \\\rho \sin{\phi} \\\dot{\rho} \cos{\phi}\\\dot{\rho} \sin{\phi}\end{matrix}\right]

预测未来

预测主要涉及的公式是：

x′P′=Fx=FPFT+Q

\begin{split} x' &= Fx \\ P' &= FPF^T + Q \end{split}

需要求解的有三个变量：F、P、QF、P、Q。

FF表明了系统的状态如何改变，这里仅考虑线性系统，F易得：

Fx=⎡⎣⎢⎢⎢⎢10000100dt0100dt01⎤⎦⎥⎥⎥⎥⎡⎣⎢⎢⎢⎢pxpyvxvy⎤⎦⎥⎥⎥⎥

Fx = \left[\begin{matrix}1 & 0 & dt & 0 \\0 & 1 & 0& dt \\0 & 0 & 1& 0 \\0 & 0 & 0& 1\end{matrix}\right]\left[\begin{matrix}p_x \\p_y \\v_x \\v_y\end{matrix}\right]

PP表明了系统状态的不确定性程度，用xx的协方差表示，这里自己指定为：

P=⎡⎣⎢⎢⎢⎢1000010000100000001000⎤⎦⎥⎥⎥⎥

P = \left[\begin{matrix}1 & 0 & 0 & 0 \\0 & 1 & 0& 0 \\0 & 0 & 1000& 0 \\0 & 0 & 0& 1000\end{matrix}\right]

QQ表明了x′=Fxx' = Fx未能刻画的其他外界干扰。本例子使用线性模型，因此加速度变成了干扰项。x′=Fxx' = Fx中未衡量的额外项目vv为：

v=⎡⎣⎢⎢⎢⎢⎢⎢⎢axdt22aydt22axdtaydt⎤⎦⎥⎥⎥⎥⎥⎥⎥=⎡⎣⎢⎢⎢⎢⎢⎢dt220dt00dt220dt⎤⎦⎥⎥⎥⎥⎥⎥[axay]=Ga

v = \left[\begin{matrix}\frac{a_x dt^2}{2} \\\frac{a_y dt^2}{2} \\a_x dt \\a_y dt\end{matrix}\right] = \left[\begin{matrix}\frac{dt^2}{2} & 0 \\0 & \frac{dt^2}{2} \\dt & 0 \\0 & dt\end{matrix}\right]\left[\begin{matrix}a_x\\ a_y\end{matrix}\right] = Ga

vv服从高斯分布N(0,Q)N(0, Q)。

Q=E[vvT]=E[GaaTGT]=GE[aaT]GT=G[σ2ax00σ2ay]GT=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢dt44σ2ax0dt32σ2ax00dt44σ2ay0dt32σ2aydt32σ2ax0dt2σ2ax00dt32σ2ay0dt2σ2ay⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥

\begin{split} Q &= E[v v^T]= E[Gaa^TG^T] = GE[aa^T]G^T \\ &= G\left[\begin{matrix}\sigma_{ax}^2 & 0\\ 0 & \sigma_{ay}^2\end{matrix}\right] G^T \\&= \left[\begin{matrix}\frac{{dt}^4}{4} \sigma_{ax}^2 & 0 & \frac{{dt}^3}{2} \sigma_{ax}^2 & 0 \\0 & \frac{{dt}^4}{4} \sigma_{ay}^2 & 0 & \frac{{dt}^3}{2} \sigma_{ay}^2 \\\frac{{dt}^3}{2} \sigma_{ax}^2 & 0 ^2 \sigma_{ax}^2 & 0 \\0 & \frac{{dt}^3}{2} \sigma_{ay}^2 & 0^2 \sigma_{ay}^2\end{matrix}\right]\end{split}

修正当下

lidar

lidar使用了KF。修正当下这里牵涉到的公式主要是：

ySKx′P′=z−Hx=HPHT+R=PHTS−1=x+Ky=(I−KH)P

\begin{split} y &= z - Hx \\ S &= HPH^T+R \\ K &= PH^TS^{-1} \\ x' &= x+Ky \\ P' &= (I-KH)P \end{split}

需要求解的有两个变量：H、RH、R。

HH表示了状态空间到测量空间的映射。

Hx=[10010000]⎡⎣⎢⎢⎢⎢pxpyvxvy⎤⎦⎥⎥⎥⎥

Hx = \left[\begin{matrix}1 & 0 & 0 & 0\\0 & 1 & 0 & 0\end{matrix}\right] \left[\begin{matrix}p_x \\p_y \\v_x \\v_y\end{matrix}\right]

RR表示了测量值的不确定度，一般由传感器的厂家提供，这里lidar参考如下：

Rlaser=[0.0225000.0225]

R_{laser} = \left[\begin{matrix}0.0225 & 0 \\0 & 0.0225 \end{matrix}\right]

radar

radar使用了EKF。修正当下这里牵涉到的公式主要是：

ySKx′P′=z−f(x)=HjPHTj+R=PHTjS−1=x+Ky=(I−KHj)P

\begin{split} y &= z - f(x) \\ S &= H_jPH_j^T+R \\ K &= PH_j^TS^{-1} \\ x' &= x+Ky \\ P' &= (I-KH_j)P \end{split}

区别与上面lidar的主要有：

状态空间到测量空间的非线性映射f(x)f(x)
非线性映射线性化后的Jacob矩阵
radar的RradarR_{radar}

状态空间到测量空间的非线性映射f(x)f(x)如下

f(x)=⎡⎣⎢⎢ρϕρ˙⎤⎦⎥⎥=⎡⎣⎢⎢⎢⎢⎢⎢⎢p2x+p2y‾‾‾‾‾‾‾√arctanpypxpxvx+pyvyp2x+p2y‾‾‾‾‾‾‾√⎤⎦⎥⎥⎥⎥⎥⎥⎥

f(x) = \left[\begin{matrix}\rho \\\phi \\\dot{\rho} \end{matrix}\right] =\left[\begin{matrix}\sqrt{p_x^2+p_y^2} \\\arctan{\frac{p_y}{p_x}} \\\frac{p_x v_x + p_y v_y}{\sqrt{p_x^2+p_y^2} }\end{matrix}\right]

非线性映射线性化后的Jacob矩阵HjH_j

Hj=∂f(x)∂x=⎡⎣⎢⎢⎢⎢⎢⎢⎢∂ρ∂px∂ϕ∂px∂ρ˙∂px∂ρ∂py∂ϕ∂py∂ρ˙∂py∂ρ∂vx∂ϕ∂vx∂ρ˙∂vx∂ρ∂vy∂ϕ∂vy∂ρ˙∂vy⎤⎦⎥⎥⎥⎥⎥⎥⎥

H_j = \frac{\partial f(x)}{\partial x} = \left[\begin{matrix}\frac{\partial \rho}{\partial p_x} & \frac{\partial \rho}{\partial p_y} & \frac{\partial \rho}{\partial v_x} & \frac{\partial \rho}{\partial v_y} \\\frac{\partial \phi}{\partial p_x} & \frac{\partial \phi}{\partial p_y} & \frac{\partial \phi}{\partial v_x} & \frac{\partial \phi}{\partial v_y} \\\frac{\partial \dot{\rho}}{\partial p_x} & \frac{\partial \dot{\rho}}{\partial p_y} & \frac{\partial \dot{\rho}}{\partial v_x} & \frac{\partial \dot{\rho}}{\partial v_y} \end{matrix}\right]

RR表示了测量值的不确定度，一般由传感器的厂家提供，这里radar参考如下：

Rlaser=⎡⎣⎢⎢0.090000.00090000.09⎤⎦⎥⎥

R_{laser} = \left[\begin{matrix}0.09 & 0 & 0\\0 & 0.0009 & 0 \\0 & 0 & 0.09\end{matrix}\right]

传感器融合实例

多传感器融合的示例如下，需要注意的有：

lidar和radar的预测部分是完全相同的
lidar和radar的参数更新部分是不同的，不同的原因是不同传感器收到的测量值是不同的
当收到lidar或radar的测量值，依次执行预测、更新步骤
当同时收到lidar和radar的测量值，依次执行预测、更新1、更新2步骤

多传感器融合的效果如下图所示，红点和蓝点分别表示radar和lidar的测量位置，绿点代表了EKF经过多传感器融合后获取到的测量位置，取得了较低的RMSE。

扩展卡尔曼滤波EKF与多传感器融合相关推荐

基于扩展卡尔曼滤波(EKF)和低成本传感器的AHRS实现
文章目录传感器观测向量观测矩阵状态向量系统矩阵测试结果传感器使用华为honor7手机采集加速度计,陀螺仪和磁力计数据. 加速计数据在使用前经过低通滤波器.滤波结果如下图: 观测向量如 ...
无损卡尔曼滤波UKF与多传感器融合
非线性系统状态估计是一大难点.KF(Kalman Filter)只适用于线性系统.EKF(Extended Kalman Filter)利用泰勒展开将非线性系统线性化.可是,EKF在强非线性系统下的误 ...
基于扩展卡尔曼滤波EKF和模型预测控制MPC，自动泊车场景建模开发
基于扩展卡尔曼滤波EKF和模型预测控制MPC,自动泊车场景建模开发,文复现. MATLAB 基于扩展卡尔曼滤波EKF和模型预测控制MPC,自动泊车场景建模开发,文复现. MATLAB(工程项目线上支持 ...
控制算法学习四、扩展卡尔曼滤波EKF
控制算法学习四.扩展卡尔曼滤波EKF 前言非线性系统状态/观测方程线性化扩展卡尔曼滤波EKF 后记前言经典卡尔曼滤波的使用场景是线性系统,但现实应用时,大多数系统都是非线性的.扩展卡尔曼滤 ...
基于扩展卡尔曼滤波(EKF)的机器人状态估计
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达 EKF的目的是使卡尔曼滤波器能够应用于机器人等非线性运动系统,EK ...
卫星轨道的估计问题(Matlab)(二):扩展卡尔曼滤波(EKF)对新问题的尝试
前言在前面的问题中我们已经考虑到了用微分方程来描述卫星运动轨迹的方法: r¨=rθ˙2−GMr−2θ¨=−2r−1r˙θ˙\ddot r = r\dot \theta^2-GMr^{-2}\\\dd ...
卡尔曼滤波KF与扩展卡尔曼滤波EKF算法
最近学习了一些预测算法卡尔曼滤波具体推导可以查看B站的教程:贝叶斯滤波与卡尔曼滤波第一讲很有必要的绪论_哔哩哔哩_bilibili 建议从第一讲开始看看上述教程花不了多少时间,可能看一个电影的 ...
概率机器人总结——(扩展)卡尔曼滤波先实践再推导
概率机器人总结--卡尔曼滤波先实践再推导概率机器人总结--(扩展)卡尔曼滤波先实践再推导 (1)卡尔曼.扩展卡尔曼.粒子滤波到底什么关系? (2)实践--扩展卡尔曼滤波 (3)推导--卡尔曼滤波 ( ...
卡尔曼滤波（KF）和扩展卡尔曼滤波（EKF）相应推导
从上个世纪卡尔曼滤波理论被提出,卡尔曼滤波在控制论与信息论的连接上做出了卓越的贡献.为了得出准确的下一时刻状态真值,我们常常使用卡尔曼滤波.扩展卡尔曼滤波.无迹卡尔曼滤波.粒子滤波等等方法,这些方法在 ...

扩展卡尔曼滤波EKF与多传感器融合