def script_random_trajectory_unscented_ivan():
dt = 0.025
print "script_unscented_trajectory"
l_points = [[-1000., 200.], [-1000., 800.], [-400., 1200.], [500., 500.], [1100., 180.]]
real_path = generateur_chemin.generate_path(l_points=l_points, velocity_translation=50,
velocity_rotation=0.5, dt=dt)
measures_pos = np.array(real_path)
real_path_point = []
for couple in real_path:
x, y = couple
pos = Point(x, y)
real_path_point.append(pos)
l_pos_filter = [real_path_point[0]]
measures_pos = np.asmatrix(measures_pos)
print measures_pos[0, :].T
filtering = IvanFilter(measures_pos[0, :].T, dt=dt)
var = 0.01
for i in range(1, measures_pos.shape[0]):
x = measures_pos[i, 0]
y = measures_pos[i, 1]
print 'x', x, 'y', y
conv = Converter()
m1, m2, m3 = conv.get_measures_from_state(x, y)
m1, m2, m3 = m1 + np.random.randn()*var, m2 + np.random.randn()*var, m3 + np.random.randn()*var
print m1, m2, m3
filtering.update(np.asmatrix([m1, m2, m3]).T)
l_pos_filter.append(filtering.get_state_position())
print get_mer(real_path, l_pos_filter)
def script_classic_trajectory():
"""
Script utilisant le filtre de Kalman avec des mesures simulées à partir d'une trajectoire inventée !
:return:
"""
print "script_classic_trajectory"
l_points = [[-1000., 200.], [-1000., 800.], [-400., 1200.], [500., 500.], [1100., 180.]]
dt = 0.025
real_path = generateur_chemin.generate_path(l_points=l_points, velocity_translation=25,
velocity_rotation=0.7, dt=dt)
measures_pos = np.array(real_path)
real_path_point = []
for couple in real_path:
x, y = couple
pos = Point(x, y)
real_path_point.append(pos)
l_pos_filtre = [real_path_point[0]]
vite = get_velocity(measures_pos, dt)
measures = np.concatenate((measures_pos, vite), axis=1)
measures = np.asmatrix(measures)
filtering = FiltrageKalman(measures[0, :].T, dt=dt)
var = 10
for i in range(1, measures.shape[0]):
x = measures[i, 0]
y = measures[i, 1]
# print "x et y", x, y, "i" ,i
x_bruite, y_bruite = x + np.random.randn()*var, y + np.random.randn()*var
filtering.update(x_bruite, y_bruite)
pos = filtering.get_current_position()
l_pos_filtre.append(pos)
print get_mer(real_path_point, l_pos_filtre)
def script_classic_trajectory(var=10):
"""
Script utilisant le filtre de Kalman avec des mesures simulées à partir d'une trajectoire inventée !
:return:
"""
print("script_classic_trajectory")
l_points = [[-1000., 200.], [-1000., 800.], [-400., 1200.], [500., 500.], [1100., 180.]]
dt = 0.025
real_path = generateur_chemin.generate_path(l_points=l_points, velocity_translation=25,
velocity_rotation=0.7, dt=dt)
measures_pos = np.array(real_path)
real_path_point = []
for couple in real_path:
x, y = couple
pos = Point(x, y)
real_path_point.append(pos)
l_pos_filtre = [real_path_point[0]]
l_pos_measured = [real_path_point[0]]
vite = get_velocity(measures_pos, dt)
measures = np.concatenate((measures_pos, vite), axis=1)
measures = np.asmatrix(measures)
filtering = FiltrageKalman(measures[0, :].T, dt=dt)
for i in range(1, measures.shape[0]):
x = measures[i, 0]
y = measures[i, 1]
# print "x et y", x, y, "i" ,i
x_bruite, y_bruite = x + np.random.randn()*var, y + np.random.randn()*var
filtering.update(x_bruite, y_bruite)
pos = filtering.get_current_position()
l_pos_filtre.append(pos)
l_pos_measured.append(Point(x_bruite, y_bruite))
print(get_mer(real_path_point, l_pos_filtre))
print(get_mer(real_path_point, l_pos_measured))
return real_path_point, l_pos_filtre, l_pos_measured
def script_unscented_trajectory_with_file():
"""
Script utilisant le filtre de Kalman unscented avec des mesures simulées à partir d'une trajectoire inventée !
:return:
"""
print "script_unscented_trajectory"
l_points = [[-1000., 200.], [-1000., 800.], [-400., 1200.], [500., 500.], [1100., 180.]]
dt = 0.025
real_path = generateur_chemin.generate_path(l_points=l_points, velocity_translation=1,
velocity_rotation=0.5, dt=dt)
measures_pos = np.array(real_path)
real_path_point = []
for couple in real_path:
x, y = couple
pos = Point(x, y)
real_path_point.append(pos)
l_pos_filter = [real_path_point[0]]
# vite = get_velocity(measures_pos, dt)
# measures = np.concatenate((measures_pos, vite), axis=1)
measures = np.asmatrix(measures_pos)
mesures_us = []
filtering = UnscentedKalmanFilter(measures[0, :].T, dt=dt, dime=2)
var = 0
# with open("mesures_chemin_determinites", "rb") as f:
# m = pickle.Unpickler(f)
# mesures_us = m.load()
for i in range(1, measures.shape[0]):
# print mesures_us[i-1]
x = measures[i, 0]
y = measures[i, 1]
conv = Converter()
m1, m2, m3 = conv.get_measures_from_state(x, y)
m1, m2, m3 = m1 + np.random.randn()*var, m2 + np.random.randn()*var, m3 + np.random.randn()*var
mesures_us.append([m1, m2, m3])
# m1, m2, m3 = mesures_us[i-1]
filtering.update(np.asmatrix([m1, m2, m3]).T)
l_pos_filter.append(filtering.get_state_position())
with open("mesures_chemin_determinites", "wb") as f:
m = pickle.Pickler(f)
m.dump(mesures_us)
print get_mer(real_path_point, l_pos_filter)
