python如何实现小车行走_[详细推导]基于EKF的小车运动模型的python编程实现

一、任务介绍

在本任务中，您将使用可用的测量值和运动模型来递归估计车辆沿轨迹的位置。车辆有了非常简单的LIDAR传感器，可以返回与环境中各个地标相对应的范围(range)和方位测量值(bearing)。假定地标的所有位置是已知的并假设他们已知的数据关联，即哪个度量属于哪个界标。

数据链接：https://pan.baidu.com/s/10jQlD0fOyJ81NWP2FOoxAA​pan.baidu.com

提取码：dzkl

二、运动和测量模型

1.运动模型

车辆运动模型将线速度和角速度里程计读数作为输入，并输出车辆的状态(即2D状态)：

是当前车辆的状态，

和

是线速度和角速度的里程表读数，并作为模型的输入值，过程噪声

服从正太分布(均值为0)和常数协方差

2.测量模型

测量模型将车辆的当前姿态与激光雷达范围和方位测量值相关联

.

和

是 标记值

的真实值

,

和

表示当前车辆的状态

表示机器人中心和前轮激光测距仪(laser rangefinde)的距离 (LIDAR)

标记值的测量噪声

服从正太分布(均值为0)和常数协方差

.

由于上述模型是非线性的，因此我们建议使用扩展卡尔曼滤波器(EKF)作为状态估计器。具体来说，需要实现两个环节：预测环节，使用测距法测量和运动模型在给定的时间步长产生状态和协方差估计值

修正环节，使用范围和方位角激光雷达提供的测量值以纠正状态和他们的协方差估计

三、加载数据

import pickle

import numpy as np

import matplotlib.pyplot as plt

import math

with open('data/data.pickle', 'rb') as f:

data = pickle.load(f)

t = data['t'] # timestamps [s]

x_init = data['x_init'] # initial x position [m]

y_init = data['y_init'] # initial y position [m]

th_init = data['th_init'] # initial theta position [rad]

# input signal

v = data['v'] # translational velocity input [m/s]

om = data['om'] # rotational velocity input [rad/s]

# bearing and range measurements, LIDAR constants

b = data['b'] # bearing to each landm