0

我正在使用里程计数据估计车辆位置。下面是我编写的代码，用于使用偏航率、方向盘角度和车速的输入数据预测车辆位置。该代码采用 dt（不一致）、更新的航向角、x 位置和 y 位置来更新测量值。

我目前已将此代码生成的轨迹与使用 GPS 数据绘制的参考轨迹进行了比较，显示错误。

我想为这个里程计数据实现卡尔曼滤波器，但我不知道为我的状态矩阵和状态转换矩阵设置什么。如果我将状态转移矩阵设置为 x_pos, y_pos, 指向 k+1，我应该使用什么作为状态矩阵？简单地使用 x,y,yaw, x_speed, y_speed, yaw_rate 的状态矩阵似乎是不可能的，因为这些数据中的大部分都在不断更新。

请帮忙！

''' def convert_odometry_info_to_cartesian(odo_yaw_rate, odo_SAS_angle, odo_speed, dt, heading, x_pos, y_pos):

# Declare constants
SOF_S_W_RATIO = 17.0
SOF_L_R = 1.5155
SOF_L_F = 1.4945

# Convert units
odo_yaw_rate = np.deg2rad(odo_yaw_rate) # converted to deg/s
odo_SAS_angle = np.deg2rad(odo_SAS_angle) # coverted to deg
heading = np.deg2rad(heading)

# Based on bicycle model
abs_yaw_rate = abs(odo_yaw_rate)
sin_heading = np.sin(heading)
cos_heading = np.cos(heading)
sin_heading_wt = np.sin(odo_yaw_rate * dt)
cos_heading_wt = np.cos(odo_SAS_angle * dt)

# Calculate wheel angle
wheel_angle = odo_SAS_angle / SOF_S_W_RATIO 
# Calculate slip angle 
slip_angle = np.arctan(SOF_L_R * np.tan(wheel_angle) / (SOF_L_F + SOF_L_R))

# Discrete time velocity model
x_speed = odo_speed * np.cos(slip_angle) 
y_speed = odo_speed * np.sin(slip_angle)

# Discrete time position update models (constant velocity & turn rate model)

x_pos = x_pos + (x_speed / odo_yaw_rate) * (sin_heading *
    cos_heading_wt + cos_heading * sin_heading_wt - sin_heading)
y_pos = y_pos + (x_speed/odo_yaw_rate) * (cos_heading - cos_heading
    * cos_heading_wt + sin_heading * sin_heading_wt)
heading = heading + odo_yaw_rate * dt

# Convert units
heading = np.rad2deg(heading)
slip_angle = np.rad2deg(slip_angle)

return x_pos, y_pos, heading, slip_angle

'''

4

0 回答 0