def load_data(data_length,
correction=1.21,
conversion=21,
data_name='../data.txt'):
"""
:param data_length:
:param correction: 修正
:param conversion: 方向盘转角-->前轮转角
:param data_name:
头尾较好，中间（15000, 26000）波折：1.3和17
开头较好：1.21和21
:return:
"""
data_frame = pd.read_table(data_name,
header=0,
delim_whitespace=True,
nrows=data_length)
second_series = data_frame.loc[:, '秒数']
velocity_series = Compute.kmh_kms(data_frame.loc[:, '实际车速km/h']) # kms
steering_wheel_angle_series = data_frame.loc[:, '当前转角°'] # 方向盘转角,degree
steering_wheel_angle_series -= correction # 修正
wheel_angle_series = Compute.deg_rad(-steering_wheel_angle_series /
conversion) # 前轮转角，rad
longitude_series = data_frame.loc[:, '经度°']
latitude_series = data_frame.loc[:, '纬度°']
return second_series, velocity_series, wheel_angle_series, longitude_series, latitude_series
def load_wheel_data(data_length, wheelbase=0.0015, data_name='../data.txt'):
data_frame = pd.read_table(data_name,
header=0,
delim_whitespace=True,
nrows=data_length)
second_series = data_frame.loc[:, '秒数']
# velocity_series = Compute.kmh_kms(data_frame.loc[:, '实际车速km/h']) # kms
front_left_wheel_velocity_series = Compute.kmh_kms(
data_frame.loc[:, 'FL']) # kms
front_right_wheel_velocity_series = Compute.kmh_kms(data_frame.loc[:,
'FR'])
rear_left_wheel_velocity_series = Compute.kmh_kms(data_frame.loc[:, 'RL'])
rear_right_wheel_velocity_series = Compute.kmh_kms(data_frame.loc[:, 'RR'])
velocity_series = (
front_left_wheel_velocity_series + front_right_wheel_velocity_series +
rear_left_wheel_velocity_series + rear_right_wheel_velocity_series) / 4
front_wheel_steer_velocity_s = (
front_right_wheel_velocity_series -
front_left_wheel_velocity_series) / wheelbase
# car_steering_velocity_series = front_wheel_steer_velocity_s
rear_wheel_steer_velocity_s = (rear_right_wheel_velocity_series -
rear_left_wheel_velocity_series) / wheelbase
# car_steering_velocity_series = rear_wheel_steer_velocity_s
car_steering_velocity_series = (front_wheel_steer_velocity_s +
rear_wheel_steer_velocity_s) / 2
longitude_series = data_frame.loc[:, '经度°']
latitude_series = data_frame.loc[:, '纬度°']
return second_series, velocity_series, car_steering_velocity_series, longitude_series, latitude_series
def load_noise_data(data_length,
correction=1.21,
conversion=21,
data_name='../data.txt'):
data_frame = pd.read_table(data_name,
header=0,
delim_whitespace=True,
nrows=data_length)
second_series = data_frame.loc[:, '秒数']
velocity_series = Compute.kmh_kms(data_frame.loc[:, '实际车速km/h']) # kms
# 方向盘转角-->前轮转角
steering_wheel_angle_series = data_frame.loc[:, '当前转角°'] # 方向盘转角,degree
steering_wheel_angle_series -= correction
wheel_angle_series = Compute.deg_rad(-steering_wheel_angle_series /
conversion) # 前轮转角，rad
longitude_series = data_frame.loc[:, '经度°']
latitude_series = data_frame.loc[:, '纬度°']
# 模拟错误GPS信号：
# 1、纬度偏离
# latitude_series[6000:9000] += 0.001
# 2、逐渐偏离
# for i in range(2000):
# latitude_series[12000+i] -= 0.0000005*i
# latitude_series[14000:16000] -= 0.001
# for i in range(1000):
# latitude_series[16000+i] -= 0.000001*(1000-i)
# 3、noise
# for i in range(3000):
# latitude_series[12000+i] += random.gauss(0, 0.0001)
return second_series, velocity_series, wheel_angle_series, longitude_series, latitude_series
def predict(particles, u, std, dt=0.02):
"""
move participles，不需要准确的motion model
according to control input with noise Q
:param particles:
:param u: (heading change, velocity)
:param std: 标准差，Q(std heading change, std velocity)
:param dt: default 0.02， 根据data算的更精确
:return: none
"""
N = len(particles)
# update heading车辆转角，不准
particles[:, 2] += u[0] + (randn(N) * std[0])
particles[:, 2] %= 2 * np.pi
# move in the (noisy) commanded direction
# N个不同的distance
dist = (u[1] * dt) + (randn(N) * std[1]) # km
# predict时不知道true latitude
# 用各个particle的latitude——int/list
for i in range(len(particles)):
yaw = np.cos(particles[i, 2])
dist_lo = Compute.km_lo(dist[i], particles[i, 1])
particles[i, 0] += yaw * dist_lo # km-->经度
particles[:, 1] += np.sin(particles[:, 2]) * dist/111 # km-->纬度
def load_process_data(data_length,
correction=1.21,
conversion=21,
wheelbase=0.003):
"""
IMU bicycle：车辆转角<--前轮转角<--方向盘转角
用PF修正错误的bicycle预测
GPS：arctan(dy/dx)
dt时间差，0.02
:param data_length:
:param correction: 修正
:param conversion: 方向盘转角-->前轮转角
头尾较好，中间（15000, 26000）波折：1.3和17
开头较好：1.21和21
:param wheelbase: 0.003km
:return:
"""
data_frame = pd.read_table('../data.txt',
header=0,
delim_whitespace=True,
nrows=data_length)
second_series = data_frame.loc[:, '秒数']
velocity_series = Compute.kmh_kms(data_frame.loc[:, '实际车速km/h']) # km/s
longitude_series = data_frame.loc[:, '经度°']
latitude_series = data_frame.loc[:, '纬度°']
steering_wheel_angle_series = data_frame.loc[:, '当前转角°'] # 方向盘转角,degree
steering_wheel_angle_series -= correction # 修正
wheel_angle_series = Compute.deg_rad(-steering_wheel_angle_series /
conversion) # 前轮转角，rad
dt_series, turn_angle_series = [], []
for i in range(len(second_series) - 1):
dt = second_series[i + 1] - second_series[i]
dt_series.append(dt) # s
dist = velocity_series[i] * dt_series[i] # km
turn_angle_series.append(
(dist / wheelbase) * tan(wheel_angle_series[i]))
# 补一个没用的
dt_series.append(0.02)
turn_angle_series.append(0)
return dt_series, turn_angle_series, velocity_series, longitude_series, latitude_series
def move(self, x, u, dt=0.02):
latitude = x[1, 0]
yaw = x[2, 0] # rad
vel = u[0] # km/s
wheel_angle = u[1] # 前轮 angle, rad
dist = vel * dt # km
if abs(wheel_angle) > 0.000001: # is robot turning?
turn_angle = (dist / wheelbase) * tan(wheel_angle) # rad
turn_radius = wheelbase / tan(wheel_angle) # km
r_lo = Compute.km_lo(turn_radius, latitude)
r_la = Compute.km_la(turn_radius)
dx = np.array([[-r_lo * sin(yaw) + r_lo * sin(yaw + turn_angle)],
[r_la * cos(yaw) - r_la * cos(yaw + turn_angle)],
[turn_angle]])
else: # moving in straight line
dx = np.array([[Compute.km_lo(dist * cos(yaw), latitude)],
[Compute.km_la(dist * sin(yaw))], [0]])
return x + dx
latitude_pre_list = [latitude_series[0]]
yaw_pre_list = [yaw_init]
print('start')
for i in range(data_length - 1): # predict less first
dt = second_series[i + 1] - second_series[i]
distance = velocity_series[i] * dt # km
# print(i, 'steering angle', steering_angle_series[i])
# bicycle model
if abs(wheel_angle_series[i]) > 0.000001: # is turn? 1d=0.017rad
# 方向盘转角-->车辆转向角
turn_angle = distance / wheelbase * tan(
wheel_angle_series[i]) # rad, wheelbase?
turn_radius_km = wheelbase / tan(wheel_angle_series[i]) # km
turn_radius_lo = Compute.km_lo(turn_radius_km, latitude_series[i])
turn_radius_la = Compute.km_la(turn_radius_km)
# print('turn: ', turn_angle)
longitude_pre = longitude_pre_list[i] - turn_radius_lo * sin(yaw_pre_list[i]) \
+ turn_radius_lo * sin(yaw_pre_list[i] + turn_angle)
latitude_pre = latitude_pre_list[i] + turn_radius_la * cos(yaw_pre_list[i])\
- turn_radius_la * cos(yaw_pre_list[i] + turn_angle)
yaw_pre = yaw_pre_list[i] + turn_angle
# print('yaw', i+1, ': ', yaw_pre)
else: # no turn
longitude_pre = longitude_pre_list[i] + Compute.km_lo(
(distance * cos(yaw_pre_list[i])), latitude_series[i])
latitude_pre = latitude_pre_list[i] + Compute.km_la(
(distance * sin(yaw_pre_list[i])))
yaw_pre = yaw_pre_list[i]
= Data.load_data(data_length)
init_x = array([[longitude_series[0]], [latitude_series[0]], [yaw_init]])
ekf_ = RobotEKF(init_x, wheelbase)
longitude_pre_list = [longitude_series[0]]
latitude_pre_list = [latitude_series[0]]
yaw_pre_list = [yaw_init]
for i in range(data_length - 1): # predict less first
dt = second_series[i + 1] - second_series[i]
ekf_.x = ekf_.move(ekf_.x,
[velocity_series[i], wheel_steering_angle_series[i]],
dt)
# add
longitude_pre_list.append(ekf_.x[0, 0])
latitude_pre_list.append(ekf_.x[1, 0])
yaw_pre_list.append(ekf_.x[2, 0])
print('end')
print('longitude rmse: ',
Compute.root_mean_square_error(longitude_series, longitude_pre_list))
print('latitude rmse',
Compute.root_mean_square_error(latitude_series, latitude_pre_list))
plt.scatter(longitude_series, latitude_series, label='measure')
plt.scatter(longitude_pre_list, latitude_pre_list, label='predict')
plt.legend()
plt.show()
from math import tan
from tools import Compute
sum_ = 72.255300 + 72.427200 + 72.026100 + 72.198000
mean_ = sum_ / 4
print(mean_)
print(mean_ / 3.6)
dt = 0.970333 - 0.942257
wheel_radius = 0.316
wheelbase = 3 # m
wheel_steering_angle1 = wheel_radius * (72.427200 -
72.255300) / 3.6 / wheelbase # m/s
wheel_steering_angle2 = wheel_radius * (72.198000 -
72.026100) / 3.6 / wheelbase
print(wheel_steering_angle1)
print(wheel_steering_angle2)
steering_wheel_angle = -1.000000
wheel_steering_angle = Compute.deg_rad(-steering_wheel_angle / 21)
print(wheel_steering_angle)
distance = 72.226650 / 3.6 * dt # m
turn_angle = distance / wheelbase * tan(wheel_steering_angle)
print(turn_angle)
# zs = list(zip(longitude_series, latitude_series)) # matrix operations
zs = [] # array([[longitude], [latitude]])
for (longitude, latitude) in zip(longitude_series, latitude_series):
zs.append(array([[longitude], [latitude]]))
ekf_, predicts, estimates = run_localization(std_vel=0.01,
std_steer=np.radians(0.01),
std_lo=0.3,
std_la=0.1,
us=us,
zs=zs,
dts=second_series,
wheelbase=wheelbase,
data_length=data_length)
print('longitude rmse: ',
Compute.root_mean_square_error(longitude_series,
estimates[:, 0])) # 与正确GPS的差距
print('latitude rmse',
Compute.root_mean_square_error(latitude_series, estimates[:, 1])) #
print('Final P/covariance:', ekf_.P.diagonal()) # 协方差——越小越集中越确定
print('Final y/residual:', ekf_.y) # predict与measure的差
# plt.scatter(true_longitude_series, true_latitude_series, color='b', label='true')
plt.scatter(longitude_series, latitude_series, color='b', label='measure')
# plt.scatter(predicts[:, 0], predicts[:, 1], color='r', label='predict')
plt.scatter(estimates[:, 0], estimates[:, 1], color='y', label='estimate')
plt.title("EKF Robot localization")
plt.legend()
plt.show()
# yaw_init = radians(99.568)
yaw_init = radians(-61.219)
wheelbase = 0.0028 # km
# data_frame = pd.read_table('../log/log_gps_H9_BLACK_20180902 230927.txt',
# header=0, delim_whitespace=True, nrows=data_length)
# data_frame = pd.read_table('../log/log_gps_H9_BLACK_20180902 235108.txt',
# header=0, delim_whitespace=True, nrows=data_length)
data_frame = pd.read_table('../log/log_gps_H9_BLACK_20180903 013352.txt',
header=0,
delim_whitespace=True,
nrows=data_length)
second_series = data_frame.loc[:, '时间']
longitude_series = data_frame.loc[:, '经度']
latitude_series = data_frame.loc[:, '纬度']
velocity_series = Compute.kmh_kms(data_frame.loc[:, '速度']) # kms
steering_wheel_angle_series = Compute.deg_rad(
data_frame.loc[:, '实际转向角']) # 方向盘转角, rad
wheel_angle_series = -steering_wheel_angle_series / 20 # 前轮转角, rad
longitude_pre_list = [longitude_series[0]]
latitude_pre_list = [latitude_series[0]]
yaw_pre_list = [yaw_init]
# print(0, '时刻，经度：', longitude_series[0])
# print(0, '时刻，纬度：', latitude_series[0])
# print(0, '时刻，航向：', yaw_init)
print('start')
for i in range(data_length - 1): # predict less first
dt = second_series[i + 1] - second_series[i]
distance = velocity_series[i] * dt # km
print('start')
for i in range(data_length - 1): # predict less first
dt = second_series[i + 1] - second_series[i]
# dt = 0.02
# 对GPS测量的依赖过大：1、前一时刻的经纬度 2、方向
# cos_yaw = Compute.cos_yaw(longitude_series[i], latitude_series[i], longitude_series[i + 1], latitude_series[i + 1])
# sin_yaw = Compute.sin_yaw(longitude_series[i], latitude_series[i], longitude_series[i + 1], latitude_series[i + 1])
#
# longitude_pre = longitude_series[i] + Compute.km_lo(
# (velocity_series[i] * cos_yaw * dt), latitude_series[i])
# latitude_pre = latitude_series[i] + Compute.km_la(
# (velocity_series[i] * sin_yaw * dt))
# 方向依赖于GPS测量值
cos_yaw = Compute.cos_yaw(longitude_pre_list[i], latitude_pre_list[i],
longitude_series[i + 1], latitude_series[i + 1])
sin_yaw = Compute.sin_yaw(longitude_pre_list[i], latitude_pre_list[i],
longitude_series[i + 1], latitude_series[i + 1])
print('yaw', i, ': ', acos(cos_yaw))
print('yaw', i, ': ', asin(sin_yaw))
longitude_pre = longitude_pre_list[i] + Compute.km_lo(
(velocity_series[i] * cos_yaw * dt), latitude_series[i])
latitude_pre = latitude_pre_list[i] + Compute.km_la(
(velocity_series[i] * sin_yaw * dt))
longitude_pre_list.append(longitude_pre)
latitude_pre_list.append(latitude_pre)
print('end')
print('longitude rmse: ',
data_length = 20000
second_series, velocity_series, car_steering_velocity_series, longitude_series, latitude_series\
= Data.load_wheel_data(data_length, wheelbase=wheelbase, data_name='../log/log_gps_H9_BLACK_20180902 230927.txt')
longitude_pre_list = [longitude_series[0]]
latitude_pre_list = [latitude_series[0]]
yaw_pre_list = [yaw_init]
print('start')
for i in range(data_length-1): # predict less first
# motion cos sin
dt = second_series[i + 1] - second_series[i]
car_steer_angle = car_steering_velocity_series[i] * dt
longitude_pre = longitude_pre_list[i] + Compute.km_lo(
(velocity_series[i] * cos(yaw_pre_list[i]) * dt), latitude_series[i])
latitude_pre = latitude_pre_list[i] + Compute.km_la(
(velocity_series[i] * sin(yaw_pre_list[i]) * dt))
yaw_pre = yaw_pre_list[i] + car_steer_angle
# # bicycle， wheel
# dt = second_series[i + 1] - second_series[i]
# distance = velocity_series[i] * dt # km
#
# wheel_angle = car_steering_velocity_series[i] * dt
# # print(i, 'steering angle', steering_angle_series[i])
# # bicycle model
# if abs(wheel_angle) > 0.0000001: # is turn? 1d=0.017rad
# turn_angle = distance / wheelbase * tan(wheel_angle)
# turn_radius_km = wheelbase / tan(wheelbase) # km
# turn_radius_lo = Compute.km_lo(turn_radius_km, latitude_series[i])
# turn_radius_la = Compute.km_la(turn_radius_km)