State Estimation and Localization for Self-Driving Cars 第二周作业 EKF练习

最新推荐文章于 2021-12-31 15:39:46 发布

原创

最新推荐文章于 2021-12-31 15:39:46 发布 · 置顶 · 576 阅读

1 ·

CC 4.0 BY-SA版权

本文详细介绍了扩展卡尔曼滤波(EKF)在自动驾驶车辆定位中的应用，包括汽车运动模型、观测模型及滤波方程。通过python编程实现EKF，并分享了编程中常见错误及解决方法。

1.运动和测量模型

1.1汽车运动模型

$xk=xk−1+T[cos⁡θk−10sin⁡θk−1001]([vkωk]+wk),wk=N(0,Q)\mathbf{x}_{k}=\mathbf{x}_{k-1}+T\left[\begin{array}{cc} \cos \theta_{k-1} & 0 \\ \sin \theta_{k-1} & 0 \\ 0 & 1 \end{array}\right]\left(\left[\begin{array}{c} v_{k} \\ \omega_{k} \end{array}\right]+\mathbf{w}_{k}\right), \quad \mathbf{w}_{k}=\mathcal{N}(\mathbf{0}, \mathbf{Q})$

其中， $xk=[x，y，θ]T\mathbf{x}_{k}=[x ， y， \theta]^{T}$ 是目前汽车的2D位姿。 $v_{k}$ 和 $w_{k}$ 是速度和角速度里程计的度数。

1.2 观测模型

观测模型按照如下公式将LIDAR测量的距离和方位 $ykl=[r，ϕ]T\mathbf{y}_{k}^{l}=[r， \phi]^{T}$ 与汽车的位姿结合起来。
$ykl=[(xl−xk−dcos⁡θk)2+(yl−yk−dsin⁡θk)2atan⁡2(yl−yk−dsin⁡θk,xl−xk−dcos⁡θk)−θk]+nkl,nkl=N(0,R)\mathbf{y}_{k}^{l}=\left[\begin{array}{c} \sqrt{\left(x_{l}-x_{k}-d \cos \theta_{k}\right)^{2}+\left(y_{l}-y_{k}-d \sin \theta_{k}\right)^{2}} \\ \operatorname{atan} 2\left(y_{l}-y_{k}-d \sin \theta_{k}, x_{l}-x_{k}-d \cos \theta_{k}\right)-\theta_{k} \end{array}\right]+\mathbf{n}_{k}^{l}, \quad \mathbf{n}_{k}^{l}=\mathcal{N}(\mathbf{0}, \mathbf{R})$