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)