EKF 学习（1）

背景¶

---

这是Cousera Self-Driving-Cars 系列课程中的Module2-State_Estimation-Linear_and_Nonlinear_Kalman_Filters 中的一次作业。

此作业是需要使用可用的测量值（measurements）和运动模型（motion model），递归估计车辆的位置。

该车辆配备了一种非常简单的激光雷达传感器，可返回与环境中各个地标相对应的距离（r）和方位测量值（b）。地标的全局位置假定事先已知（l）。

我们还将假设了已知的数据关联，即，哪个测量值属于哪个地标。如下截图，是对应的Motion model和Measurement model:

 1. 解压数据：

import pickle
import numpy as np
import matplotlib.pyplot as plt

with open('/Week2/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 landmarks center in the frame attached to the laser [rad]
r = data['r']  # range measurements [m]
l = data['l']  # x,y positions of landmarks [m]
d = data['d']  # distance between robot center and laser rangefinder [m]

2. 初始化参数

v_var = 0.01  # translation velocity variance  
om_var = 0.01  # rotational velocity variance 
r_var = 0.1  # range measurements variance
b_var = 0.1  # bearing measurement variance

Q_km = np.diag([v_var, om_var]) # input noise covariance 
cov_y = np.diag([r_var, b_var])  # measurement noise covariance 

x_est = np.zeros([len(v), 3])  # estimated states, x, y, and theta
P_est = np.zeros([len(v), 3, 3])  # state covariance matrices

x_est[0] = np.array([x_init, y_init, th_init]) # initial state
P_est[0] = np.diag([1, 1, 0.1]) # initial state covariance

# Wraps angle to (-pi,pi] range
def wraptopi(x):
    if x > np.pi:
        x = x - (np.floor(x / (2 * np.pi)) + 1) * 2 * np.pi
    elif x < -np.pi:
        x = x + (np.floor(x / (-2 * np.pi)) + 1) * 2 * np.pi
    return x

3. 修正步骤 - measurement update

import math
def measurement_update(lk, rk, bk, P_check, x_check):
    # 1. Compute measurement Jacobian
    xl = lk[0]
    yl = lk[1]
    xk = x_check[0]
    yk = x_check[1]
    theta_k = wraptopi(x_check[2])
    
    r_xk = (xk-xl)/(((xl-xk)**2 + (yl-yk)**2)**0.5)
    r_yk = (yk-yl)/(((xl-xk)**2 + (yl-yk)**2)**0.5)
    r_thk = 0
    ph_xk = -(yl-yk)/((xl-xk)**2 + (yl-yk)**2)
    ph_yk = -(xl-xk)/((xl-xk)**2 + (yl-yk)**2)
    ph_thk = -1
    h_jac = np.array([[r_xk, r_yk, r_thk],[ph_xk, ph_yk, ph_thk]])
    m_jac = 1
    # 2. Compute Kalman Gain
    Kk = P_check@h_jac.T@np.linalg.inv(h_jac@P_check@h_jac.T + Q_km)
    
    
    # 3. Correct predicted state (remember to wrap the angles to [-pi,pi])
    y_mear = np.array([rk,bk])
    y_est = np.array([((xl-xk)**2+(yl-yk)**2)**0.5, (wraptopi(math.atan2((yl-yk),(xl-xk))-theta_k))])
    x_check = x_check + Kk@(y_mear - y_est)

    # 4. Correct covariance
    P_check = (np.eye(3) - Kk@h_jac)@P_check
    return x_check, P_check

4. 预测步骤 - Main Filter Loop

#### 5. Main Filter Loop #######################################################################
for k in range(1, len(t)):  # start at 1 because we've set the initial prediciton

    delta_t = t[k] - t[k - 1]  # time step (difference between timestamps)

    # 1. Update state with odometry readings (remember to wrap the angles to [-pi,pi])
    x_check = np.zeros(3)
    odometry_r = np.array([v[k-1],om[k-1]])
    x_check = x_est[k-1] + (np.array([[math.cos(x_est[k-1][2]),0],[math.sin(x_est[k-1][2]),0],[0,1]])@odometry_r).T
    x_check[2] = wraptopi(x_check[2])
    
    # 2. Motion model jacobian with respect to last state
    F_km = np.zeros([3, 3])
    F_km = np.array([[1,0,v[k]*(-math.sin(x_est[k-1][2]))],[0,1,v[k]*(math.cos(x_est[k-1][2]))],[0,0,1]])
    # 3. Motion model jacobian with respect to noise
    L_km = np.zeros([3, 2])
    L_km = np.array([[math.cos(x_est[k-1][2]),0],[math.sin(x_est[k-1][2]),0],[0,1]])
    # 4. Propagate uncertainty
    P_check = F_km@P_est[k-1]@F_km.T + L_km@cov_y@L_km.T
    # 5. Update state estimate using available landmark measurements
    

    for i in range(len(r[k])):
        x_check, P_check = measurement_update(l[i], r[k, i], b[k, i], P_check, x_check)
    

    # Set final state predictions for timestep
    x_est[k, 0] = x_check[0]
    x_est[k, 1] = x_check[1]
    x_est[k, 2] = x_check[2]
    P_est[k, :, :] = P_check

5. 画图，

        1）真实值：

        2）如果注释掉measurement_update，留下motion model的估计：

      3）EKF 融合motion model 和 measurements估计结果：