from math import sin, cos, tan
import numpy as np
import matplotlib.pyplot as plt
from tools import Compute, Data
''' GPS不准，process不准/未知？
PF，lidar，landmark'''
''' bicycle model
steering wheel angle-->front tire angle-->turn angle
'''
yaw_init = 3.00
wheelbase = 0.003 # km
data_length = 15000
second_series, velocity_series, wheel_angle_series, longitude_series, latitude_series\
= Data.load_data(data_length, correction=1.3, conversion=17)
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
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?
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
yaw_init = 3.00
wheelbase = 0.003 # km
data_length = 15000
second_series, velocity_series, wheel_steering_angle_series, longitude_series, latitude_series\
= 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
import matplotlib.pyplot as plt
from tools import Data, Compute
from filter_ekf.Fusion import *
wheelbase = 0.003 # km
# dt = 1/64 #
data_length = 20000
# load data
_, _, _, true_longitude_series, true_latitude_series = Data.load_data(
data_length)
# second_series, velocity_series, wheel_angle_series, longitude_series, latitude_series\
# = Data.load_noise_data(data_length, correction=1.3, conversion=17)
second_series, velocity_series, wheel_angle_series, longitude_series, latitude_series\
= Data.load_noise_data(data_length, correction=0, conversion=16.3, data_name='../log/log_gps_H9_BLACK_20180902 230927.txt')
us = list(zip(velocity_series, wheel_angle_series)) # just for index 拿着用
# 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,
from math import sin, cos, tan
import matplotlib.pyplot as plt
from tools import Compute, Data
''' bicycle model
steering wheel angle-->front tire angle-->turn angle
'''
yaw_init = 3.33 # 3.33, 16.3
wheelbase = 0.003 # km
data_length = 20000
second_series, velocity_series, wheel_angle_series, longitude_series, latitude_series\
= Data.load_data(data_length, correction=0, conversion=16.3, 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(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
# 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
import matplotlib.pyplot as plt
from tools import Data
""" GPS不准时，如何定位？
使用IMU
例： 6s内纬度偏差了0.001即偏差100m"""
data_length = 20000
_, _, _, longitude_series, latitude_series = Data.load_data(
data_length, data_name='../log/log_gps_H9_BLACK_20180902 230927.txt')
_, _, _, noise_longitude_series, noise_latitude_series = Data.load_noise_data(
data_length, data_name='../log/log_gps_H9_BLACK_20180902 230927.txt')
plt.scatter(longitude_series, latitude_series, label='true_measure')
plt.scatter(noise_longitude_series,
noise_latitude_series,
label='noise_measure')
plt.legend()
plt.show()
import matplotlib.pyplot as plt
from math import acos, asin
from tools import Compute, Data
''' measure based model
x_ = x + v * cos_yaw * dt
yaw is based on measurements
'''
data_length = 130000
second_series, velocity_series, _, longitude_series, latitude_series = Data.load_data(
data_length)
longitude_pre_list = [longitude_series[0]]
latitude_pre_list = [latitude_series[0]]
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])