def run_pf1(N, iters=13, sensor_std_err=0.0000001,
plot_particles=False, is_lim=False,
xlim=(117.21, 117.31), ylim=(39.11, 39.21),
initial_x=None):
'''
:param N: 粒子数量，越大越可能落在正确位置。
:param iters: data length
:param sensor_std_err: measurement/sensor standard测量的准不准，越小测量越准。
:param plot_particles: 是否画出粒子堆
:param is_lim: 是否规范刻度
:param xlim: x刻度
:param ylim: y刻度
:param initial_x: 知道初始位置就gaussian分布，不知道就均匀分布
:return: none
'''
landmarks = np.array([[117.3005, 39.1160], [117.2995, 39.1160], [117.2985, 39.1160], [117.2975, 39.1160],
[117.3005, 39.1170], [117.2995, 39.1170], [117.2985, 39.1170], [117.2975, 39.1170],
[117.2965, 39.1160], [117.2960, 39.1160], [117.2955, 39.1160], [117.2950, 39.1160],
[117.2965, 39.1170], [117.2960, 39.1170], [117.2955, 39.1170], [117.2950, 39.1170]])
NL = len(landmarks)
plt.figure()
# create particles and weights
# 是否gaussian
if initial_x is not None: # 有最初的x就创建gaussian，std与Q
particles = create_gaussian_particles(
mean=initial_x, std=(0.00001, 0.00001, np.pi / 8), N=N)
else:
particles = create_uniform_particles((117.21, 117.31), (39.11, 39.21), (0, np.pi*2), N)
weights = np.zeros(N)
# 画出初始化的透明粒子，画出地标
if plot_particles:
alpha = .20
if N > 5000:
alpha *= np.sqrt(5000) / np.sqrt(N)
plt.scatter(particles[:, 0], particles[:, 1],
alpha=alpha, color='g')
plt.scatter(landmarks[:, 0], landmarks[:, 1], marker='s', s=60)
dt_series, turn_angle_series, velocity_series, longitude_series, latitude_series\
= Data.load_process_data(data_length=iters, correction=1.3, conversion=17) # true position
true_pos, us = [], [] # array([[longitude], [latitude]])
for (turn_angle, velocity, longitude, latitude) in zip(turn_angle_series, velocity_series, longitude_series, latitude_series):
true_pos.append([longitude, latitude])
us.append([turn_angle, velocity])
true_pos = np.array(true_pos)
us = np.array(us)
xs = [] # estimate x
for i in range(iters):
# 模拟测量distance from robot to each landmark
zs = (norm(landmarks - true_pos[i], axis=1) +
(randn(NL) * sensor_std_err))
# move
predict(particles, u=us[i], std=(0.2, 0.05), dt=dt_series[i]) # predict的置信度
# incorporate measurements
update(particles, weights, z=zs, R=sensor_std_err, # measure的置信度
landmarks=landmarks)
# resample if too few effective particles
if neff(weights) < N / 2:
indexes = systematic_resample(weights)
resample_from_index(particles, weights, indexes) # 重采样
mu, var = estimate(particles, weights)
xs.append(mu)
# 画出estimate后的粒子堆
if plot_particles:
plt.scatter(particles[:, 0], particles[:, 1],
color='k', marker=',', s=1)
# 画出真实位置点+
p1 = plt.scatter(true_pos[i, 0], true_pos[i, 1], marker='+',
color='k', s=180, lw=1, label='true')
# 画出estimate位置点
p2 = plt.scatter(mu[0], mu[1], marker='s', color='r', label='estimate')
# xs = np.array(xs)
# plt.plot(xs[:, 0], xs[:, 1])
plt.legend([p1, p2], ['Actual', 'PF'], loc=4, numpoints=1)
if is_lim:
plt.xlim(*xlim)
plt.ylim(*ylim)
print('final position error, variance:\n\t', mu - np.array([iters, iters]), var)
plt.show()
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?
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,
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
def chi2(w, sigma, cv, predictions):
w = np.ravel(w)
diff = w@predictions - cv
chi2 = diff@sigma@diff/len(diff)
return chi2
if True:
# load data, invcovmat and predictions
dt = Data('data/data.csv.tgz', 'data/invcovmat.csv.tgz')
th = Predictions('data/predictions.csv.tgz')
# retreive cv and invcovmat as numpy arrays
cv = dt.get_data()
sigma = dt.get_invcovmat()
predictions = th.get_predictions()
"""The chi² function is
v = (w@P) - cv
chi² = v@sigma@v
Therefore we need to minimize 1/2*w@P@W + q@w as a function of w
"""
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])
import matplotlib.pyplot as plt
from math import acos, asin, sin, cos, tan
from tools import Compute, Data
''' wheel velocity
turn angle = (wheel_right - wheel_left) / wheelbase *dt
'''
yaw_init = 3.50 # rad,3.33
wheelbase = 0.0015 # km，轮距
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))