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

Posted 2023-02-23 Young_Gy

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

KF与EKF

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

KF与EKF的区别如下：

1. 预测未来：$x\'=Fx+u$用$x\'=f(x,u)$代替；其余$F$用$F_j$代替。
2. 修正当下：将状态映射到测量的$Hx\'$用$h(x\')$代替；其余$H$用$H_j$代替。

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

$F_j，H_j$计算方式如下：

$$\\beginsplit F_j &= \\frac\\partial f(x, u)\\partial x \\\\ b &= \\frac\\partial h(x\')\\partial x \\endsplit$$

为什么要用EKF

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

将非线性系统线性化

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

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

$$f(x) \\approx f(\\mu) + \\frac \\partial f(\\mu)\\partial x(x-\\mu)$$

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

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

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

多传感器融合

lidar与radar

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

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

传感器融合步骤

步骤图如上所示，包括：

1. 收到第一个测量值，对状态$x$进行初始化。
2. 预测未来
3. 修正当下

初始化

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

对于radar来说，

⎡⎣⎢⎢⎢⎢pxpyvxv无损卡尔曼滤波UKF与多传感器融合

无损卡尔曼滤波UKF与多传感器融合

EKF定位基于传感器信息融合的EKF扩展卡尔曼滤波定位算法matlab仿真

扩展卡尔曼滤波EKF—目标跟踪中的应用(算法部分)

使用robot_pose_ekf对传感器信息融合

camera-lidar-radar基于卡尔曼滤波和扩展卡尔曼滤波的相机激光雷达毫米波雷达多传感器后融合