def test_motion_model_circular_right_1(self): ''' Move from origin (0,0,0) to the RIGHT With v = 1, on a circle with R = 2.0 meters with dT = 1 seconds for 1 cycle. x = Rsin(v*dT/R) = 0.47 y = Rcos(v*dT/R) = 0.94 y needs a shifting with R after that ''' x = 0.0 y = 0.0 o = 0.0 v = 1.0 a = 0.0 s = 0.0 # R = 2 dT = 1 u = np.array([[a], [s]]) kf = EKF(initial_x=x, initial_y=y, initial_o=o, initial_v=v, dT=dT) R = get_icr_radius(s) x = kf.motion_model(u) self.assertLessEqual(kf.posx, R) self.assertLessEqual(kf.posy, 0) self.assertGreaterEqual(kf.posx, -R) self.assertGreaterEqual(kf.posy, -2 * R) if self.debug is True: print(kf.x)
def __init__(self, t): self.dt = t self.R = np.diag([params.sigma_r**2, params.sigma_theta**2]) self.lm_filters = [ [EKF(params.dt, i) for i in range(params.num_lms)] for i in range(params.M) ] #List of lists. Inside list is EKF for each LM. Outer list is each particle
def __init__(self): self.icp = ICP() self.ekf = EKF() self.extraction = Extraction() # odom robot init states self.robot_x = rospy.get_param('/icp/robot_x',0) self.robot_y = rospy.get_param('/icp/robot_y',0) self.robot_theta = rospy.get_param('/icp/robot_theta',0) self.sensor_sta = [self.robot_x,self.robot_y,self.robot_theta] self.isFirstScan = True self.src_pc = [] self.tar_pc = [] # State Vector [x y yaw] self.xOdom = np.zeros((3,1)) self.xEst = np.zeros((3,1)) self.PEst = np.eye(3) # map observation self.obstacle = [] # radius self.obstacle_r = 10 # init map self.updateMap() # ros topic self.laser_sub = rospy.Subscriber('/course_agv/laser/scan',LaserScan,self.laserCallback) self.location_pub = rospy.Publisher('ekf_location',Odometry,queue_size=3) self.odom_pub = rospy.Publisher('icp_odom',Odometry,queue_size=3) self.odom_broadcaster = tf.TransformBroadcaster() self.landMark_pub = rospy.Publisher('/landMarks',MarkerArray,queue_size=1)
def test_motion_model_circular_left_4(self): ''' Move from origin (0,0,0) to the LEFT With v = 1, on a circle with R = 1.75 meters with dT = 4 seconds for 1 cycle. ''' x = 0.0 y = 0.0 o = 0.0 v = 1.0 a = 0.0 s = 14.0 # R = 1.75 dT = 4 u = np.array([[a], [s]]) kf = EKF(initial_x=x, initial_y=y, initial_o=o, initial_v=v, dT=dT) R = get_icr_radius(s) x = kf.motion_model(u) self.assertLessEqual(kf.posx, R) self.assertLessEqual(kf.posy, 2 * R) self.assertGreaterEqual(kf.posx, -R) self.assertGreaterEqual(kf.posy, 0) if self.debug is True: print(kf.x)
def test_motion_model_circular_loop_left_5times(self): ''' Move from origin (0,0,0) to the LEFT With v = 1, on a circle with R = 1.75 meters with dT = 1 seconds with 5 cycles ''' x = 0.0 y = 0.0 o = 0.0 v = 1.0 a = 0.0 s = 14.0 # R = 1.75 u = np.array([[a], [s]]) dT = 1.0 kf = EKF(initial_x=x, initial_y=y, initial_o=o, initial_v=v, dT=dT) R = get_icr_radius(s) for i in range(1, 6): x = kf.motion_model(u) # if self.debug is True: # print(kf.x) self.assertLessEqual(kf.posx, R) self.assertLessEqual(kf.posy, 2 * R) self.assertGreaterEqual(kf.posx, -R) self.assertGreaterEqual(kf.posy, 0) if self.debug is True: print(kf.x)
def test_motion_model_circular_right_0(self): ''' Move from origin (0,0,0) to the RIGHT With v = PI, on a circle with R = 2.0 meters with dT = 1 seconds for 1 cycle. x = [2, -2, pi, pi] ''' x = 0.0 y = 0.0 o = 0.0 v = 0.0 a = 0.0 s = 0.0 # R = 2 dT = 1 u = np.array([[a], [s]]) R = get_icr_radius(s) v = np.pi * R / 2 kf = EKF(initial_x=x, initial_y=y, initial_o=o, initial_v=v, dT=dT) x = kf.motion_model(u) self.assertLessEqual(kf.posx, R) self.assertLessEqual(kf.posy, 0) self.assertGreaterEqual(kf.posx, -R) self.assertGreaterEqual(kf.posy, -2 * R) self.assertAlmostEqual(kf.posx, R, None, None, 0.2) self.assertAlmostEqual(kf.posy, -R, None, None, 0.2) if self.debug is True: print(kf.x)
def __init__(self, acc_n=1e-2, gyr_n=1e-4, acc_w=1e-6, gyr_w=1e-8): self.initialized = False self.init_lla_ = None self.I_p_Gps_ = np.array([.0, .0, .0]) self.imu_buff = deque([], maxlen=100) self.ekf = EKF(acc_n, gyr_n, acc_w, gyr_w) rospy.Subscriber("imu/data", Imu, self.imu_callback) rospy.Subscriber("/fix", NavSatFix, self.gnss_callback) self.pub_path = rospy.Publisher("nav_path", Path, queue_size=10) self.pub_odom = rospy.Publisher("nav_odom", Odometry, queue_size=10) self.nav_path = Path() self.last_imu = None self.p_G_Gps_ = None
def main(): global ekf, pose_pub rospy.init_node("localization") ekf = EKF() pose_pub = rospy.Publisher("pose", Pose) rospy.Subscriber("odom", Odometry, odom_callback) # odom_combined comes from robot_pose_ekf, different message type from # regular odom, but seems to have the same basic contents, so we can use # the same callback (yay dynamic typing!) #rospy.Subscriber("odom_combined", PoseWithCovarianceStamped, odom_callback) #rospy.Subscriber("/corobot_pose_ekf/odom_semicombined", PoseWithCovarianceStamped, odom_callback) #rospy.Subscriber("laser_pose", Pose, laser_callback) rospy.Subscriber("qrcode_pose", Pose, qrcode_callback) rospy.spin()
def test_init_ekf(self): ''' Create an instance of the EKF class and initialize it. ''' x = 0.0 y = 0.0 o = 0.0 v = 0.1 kf = EKF(initial_x=x, initial_y=y, initial_o=o, initial_v=v, dT=1) self.assertEqual(kf.posx, x) self.assertEqual(kf.posy, y) self.assertEqual(kf.yaw, o) self.assertEqual(kf.vel, v)
def __init__(self, host='localhost', port=4567): # Connection details self._host = host self._port = port self.ekf = EKF() # Server callbacks definition self._sio = socketio.AsyncServer() self._sio.on('connect', self._on_sim_connect) self._sio.on('disconnect', self._on_sim_disconnect) self._sio.on('telemetry', self._on_telemetry) # History stacking self.estimations_history = [] self.groundtruth_history = []
def main(): global pose_pub, ekf, particles, map, num_particles rospy.init_node("localization") rospy.wait_for_service('get_map') map_srv = rospy.ServiceProxy('get_map', GetCoMap) map = map_srv().map roslib.load_manifest("corobot_localization") ekf = EKF() pose_pub = rospy.Publisher("pose", Pose) num_particles = 500 particles = [] if len(particles) == 0: rospy.Subscriber("qrcode_pose", Pose, pf_initialize) if len(particles) > 0: rospy.Subscriber("odom", Odometry, prediction) rospy.Subscriber("scan", LaserScan, update_model) rospy.spin()
def test_motion_model_straight(self): ''' Check motion model for straight forward movement. ''' x = 0.0 y = 0.0 o = 0.0 v = 0.1 a = 0.0 # 7.0 means straight forward s = 7.0 dT = 1 u = np.array([[a], [s]]) kf = EKF(initial_x=x, initial_y=y, initial_o=o, initial_v=v, dT=dT) x = kf.motion_model(u) self.assertEqual(kf.posx, 0.1) self.assertEqual(kf.posy, 0.0) self.assertEqual(kf.yaw, o) self.assertEqual(kf.vel, v)
count_list.append(count) # Extracting simulation data and storing it in a list for plotting S_truth, S1_meas, S2_meas = nl_simulation(count) alt_truth_list.append(S_truth[0]) v_truth_list.append(S_truth[1]) alt_meas1_list.append(S1_meas[0]) v_meas1_list.append(S1_meas[1]) alt_meas2_list.append(S2_meas[0]) v_meas2_list.append(S2_meas[1]) # Initializing KF for two gps sensor measurements if count == 0: S0 = S1_meas P0 = np.eye((2)) two_sensor_ekf = EKF(S0, P0) # Running KF with 2 gps sensor measurements F = [[1, pi / 180], [0, 1]] H = np.eye((2)) Q = np.power(q, 2) * np.eye((2)) R = np.power(r, 2) * np.eye((2)) two_sensor_ekf.ekf(S1_meas, F, H, Q, R) # print("alt truth %f ekf alt_est %f " % (S_truth[0], two_sensor_ekf.S_est[0])) # print("v truth %f ekf v_est %f " % (S_truth[1], two_sensor_ekf.S_est[1])) ekf_alt_est_list.append(two_sensor_ekf.S_est[0]) count += 1 print("----plotting NL EKF----") plot_two_sensors(count_list, alt_truth_list, alt_meas1_list,
initial_transform = vehicle.actor_list[0].get_transform() initial_rotation = initial_transform.rotation initial_location = initial_transform.location ## Convert rotation and location to right-handed r_right_handed = R.from_matrix( carla_rotation_to_numpy_rotation_matrix(initial_rotation)) t_right_handed = np.array( [initial_location.x, initial_location.y * -1, initial_location.z]).T ## Convert the right-handed rotation to a quaternion, roll it to get the form w,x,y,z from x,y,z,w initial_quat = np.roll(r_right_handed.as_quat(), 1) ## Initialize the EKF system #TODO check initial values vio_ekf = EKF(np.array([0, 0, 0]), np.array([0, 0, 0]), initial_quat, debug=False) vio_ekf.use_new_data = False # --- Set instrument noise parameters vio_ekf.setSigmaAccel(0.0) vio_ekf.setSigmaGyro(0.0) ### Location transform ############################################### H_local_to_global = np.eye(4) H_local_to_global[:3, :3] = r_right_handed.as_matrix() H_local_to_global[:3, 3] = t_right_handed H_global_to_local = np.linalg.inv(H_local_to_global) r_right_handed = R.from_matrix(H_global_to_local[:3, :3]) t_right_handed = H_global_to_local[:3, 3]
def setUp(self): self.empty_ekf = EKF(landmark_threshold=0)
trajectory_vo = [np.array([0, 0, 0])] # initialize first ground truth position # transform from global frame to robot start frame initial_pos = H_global_to_start @ np.append(t_right_handed, 1) trajectory_gt = [initial_pos] # EKF Initialization #TODO ######################################################### # Convert the right-handed rotation to a quaternion, roll it to get the form w,x,y,z from x,y,z,w initial_quat_wxyz = np.roll(r_right_handed.as_quat(), 1) # initial_quat_wxyz = np.roll(R.from_matrix(car_start_r).as_quat(), 1) ## Initialize the EKF system #TODO check initial values # vio_ekf = EKF(np.array([0,0,0]), np.array([0, 0, 0]), initial_quat_wxyz, debug=False) vio_ekf = EKF(np.array([0, 0, 0]), np.array([0, 0, 0]), np.array([1, 0, 0, 0]), debug=False) vio_ekf.use_new_data = True vio_ekf.setSigmaAccel(1.) vio_ekf.setSigmaGyro(0.5) vio_ekf.setSigmaVO(100.) accel_list = [] gyro_list = [] # Main Loop ############################################################# max_length = 500 first = True step = 0 while True: step += 1
[twr.Getx()[1], twr.Getx()[1] + body_radius * np.sin(twr.Getx()[2])]) robot_head.set_xdata(headx) robot_head.set_ydata(heady) fig.canvas.draw() plt.pause(0.01) if __name__ == "__main__": # Two-Wheeled Robot Init twr = TWR() # TWR model object # Kalman Filter Init ekf = EKF(twr.c, twr.nl) body_radius = 0.3 fig, lines, lines_est, msensor, robot_body, robot_head = InitPlot( twr, body_radius) mu = ekf.mu K = ekf.K two_sig_x = np.array([[2 * np.sqrt(ekf.cov.item((0, 0)))], [-2 * np.sqrt(ekf.cov.item((0, 0)))]]) two_sig_y = np.array([[2 * np.sqrt(ekf.cov.item((1, 1)))], [-2 * np.sqrt(ekf.cov.item((1, 1)))]]) two_sig_theta = np.array([[2 * np.sqrt(ekf.cov.item((2, 2)))], [-2 * np.sqrt(ekf.cov.item((2, 2)))]]) for i in range(int(twr.t_end / twr.dt)): # truth model updates twr.Propagate()
#!/usr/bin/env python import rospy import sys import os import numpy as np from std_msgs.msg import String from nav_msgs.msg import Odometry from marker_msgs.msg import MarkerDetection from tf.transformations import euler_from_quaternion sys.path.insert(1, '/home/ros/catkin_ws/src/moro_g12/src/') from ekf import EKF from csv_writer import csv_writer previous_timestamp = None ekf = EKF(3, 2, 2) beacons = [(7.3, 3), (1, 1), (9, 9), (1, 8), (5.8, 8)] # xy coordinates def get_rotation(quaternion): global roll, pitch, yaw orientation_list = [quaternion.x, quaternion.y, quaternion.z, quaternion.w] (roll, pitch, yaw) = euler_from_quaternion(orientation_list) return wrap_to_pi(yaw) def odom_callback(msg): timestamp = msg.header.stamp x = msg.pose.pose.position.x y = msg.pose.pose.position.y theta = get_rotation(msg.pose.pose.orientation) # radians
robot_sim.add_landmark(4.0, -4.0) robot_sim.add_landmark(-2.0, -2.0) robot_sim.add_landmark(-4.0, 4.0) robot_sim.set_odom_noises(0.33, 0.1, 0.1, 0.33) robot_sim.set_max_measurement_range(max_measurement_range) robot_sim.set_measurement_variances(measurement_range_variance_sim, measurement_angle_variance_sim) robot_sim.set_plot_sizes(max_measurement_range, max_measurement_range) robot_sim.set_random_measurement_rate(0.05) robot_sim.set_sim_time_step(0.1) # ekf parameters measurement_range_variance = 0.3 * 0.3 measurement_angle_variance = 5.0 * math.pi / 180.0 * 5.0 * math.pi / 180.0 ekf = EKF(start_x, start_y, start_yaw) ekf.add_landmark(2.0, 2.0) ekf.add_landmark(4.0, -4.0) ekf.add_landmark(-2.0, -2.0) ekf.add_landmark(-4.0, 4.0) ekf.set_odom_noises(5.0, 2.0, 2.0, 8.0) ekf.set_measurement_variances(measurement_range_variance, measurement_angle_variance) ekf.set_min_trace(0.001) ekf.set_plot_sizes(5.0, 5.0) while True: # simulate robot behaviors robot_sim.update_pose(0.2, -0.2) delta_dist = robot_sim.sim_v * robot_sim.sim_time_step delta_yaw = robot_sim.sim_w * robot_sim.sim_time_step
def gsf_fill_dict(self, key, x_0, P_0): self.ekf_dict[key] = EKF(x_0, P_0)
#!/usr/bin/env python3 import os, sys sys.path.append("/home/jc/Downloads/test_ws/src/imu_gnss_fusion/src") from ekf import EKF import numpy as np class ImuData: def __init__(self): self.timestamp = 0.0 self.acc = np.array([.0, .0, .0]) self.gyr = np.array([.0, .0, .0]) ekf = EKF(acc_n=1e-2, gyr_n=1e-4, acc_w=1e-6, gyr_w=1e-8) last_imu = ImuData() last_imu.timestamp = 1 curr_imu = ImuData() curr_imu.timestamp = 1.1 curr_imu.acc = np.array([0.1, 0, 0.]) curr_imu.gyr = np.array([0.0, 0., 0]) ekf.predict(last_imu, curr_imu) H = np.array([[1., 0., 0., 0., 0., 0., -0., -0., -0., 0., 0., 0., 0., 0., 0.], [0., 1., 0., 0., 0., 0., -0., -0., -0., 0., 0., 0., 0., 0., 0.], [0., 0., 1., 0., 0., 0., -0., -0., -0., 0., 0., 0., 0., 0., 0.]]) V = np.array([[ 0.002304, 0., 0., ], [
import animation reload(animation) from animation import Animation import plotData reload(plotData) from plotData import plotData import ekf reload(ekf) from ekf import EKF dynamics = Dynamics() animation = Animation() dataPlot = plotData() ekf = EKF() t = P.t_start while t < P.t_end: t_next_plot = t + P.t_plot while t < t_next_plot: t = t + P.Ts vc = 1 + 0.5 * np.cos(2 * np.pi * (0.2) * t) omegac = -0.2 + 2 * np.cos(2 * np.pi * (0.6) * t) noise_v = vc + np.random.normal( 0, np.sqrt(P.alpha1 * vc**2 + P.alpha2 * omegac**2)) noise_omega = omegac + np.random.normal( 0, np.sqrt(P.alpha3 * vc**2 + P.alpha4 * omegac**2)) u = [noise_v, noise_omega] dynamics.propagateDynamics(u)
training_size, T)) all_y = [] all_x = [] ukf_mses = [] ekf_mses = [] lstm_mses = [] lstm_stacked_mses = [] bar = ProgressBar() for s in bar(range(num_sims)): #x_0 = np.random.normal(0, x_var, 1) x_0 = np.array([0]) sim = UNGM(x_0, R, Q, x_var) ukf = UKF(sim.f, sim.F, sim.h, sim.H, sim.Q, sim.R, 5., first_x_0, 1) ekf = EKF(sim.f, sim.F, sim.h, sim.H, sim.Q, sim.R, first_x_0, 1) for t in range(T): x, y = sim.process_next() ukf.predict() ukf.update(y) ekf.predict() ekf.update(y) ukf_mses.append(MSE(ukf.get_all_predictions(), sim.get_all_x())) ekf_mses.append(MSE(ekf.get_all_predictions(), sim.get_all_x())) all_x.append(np.array(sim.get_all_x())) all_y.append(np.array(sim.get_all_y())) X = np.array(all_y)[:, :-1, :] y = np.array(all_x)[:, 1:, :]
import random #from matplotlib.pyplot import * #from mpl_toolkits.mplot3d import Axes3D from path_finding import PATH from median_filter_special import myfilter from cost_matrix import COSTMtrix from cost_matrix import Window_LEN, Overall_shiftting_WinLen, Standard_LEN from scipy.ndimage import gaussian_filter1d from time import time import scipy.io from parrallel_thread import Dual_thread_Overall_shift_NURD from shift_deploy import Shift_Predict from basic_trans import Basic_oper from Path_post_pcs import PATH_POST from ekf import EKF myekf = EKF() device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu") Resample_size = Window_LEN R_len = 20 #read_start = 100 read_start = 0 Debug_flag = True global intergral_flag intergral_flag = 0 Branch_flag = 0 # 0 fusion, 1 A, 2 B if (Save_signal_flag == True):
ind = np.argwhere(np.abs(z[1]) < params.fov) z = z[:, ind][:, :, 0] return z, ind if __name__ == "__main__": t = np.arange(0, params.tf, params.dt) vc, wc = generateVelocities(t) v = vc + np.sqrt(params.alpha1 * vc**2 + params.alpha2 * wc**2) * np.random.randn(vc.size) w = wc + np.sqrt(params.alpha3 * vc**2 + params.alpha4 * wc**2) * np.random.randn(wc.size) Car = CarAnimation() ekf = EKF(params.dt) x_hist = [] mu_hist = [] err_hist = [] x_covar_hist = [] y_covar_hist = [] psi_covar_hist = [] K_hist = [] state = np.zeros(3) dead_reckon = np.zeros(3) mu = ekf.mu Sigma = ekf.Sigma for i in range(t.size):
# GPS and local heading hdg = radians(0) psi_k = radians(0) # Driving simulation last_xk = [ radians(35.210467), radians(-97.441811), hdg, 0, 0, psi_k, 0, 0, 0, 0 ] # Commanded wheel velocities ctrl_signal = [3.14, 6.3] # Init the ekf ekf = EKF(last_xk) print(ekf.motion.get_jac()) exit(0) dt = 1.0 / 20.0 sim_time = 4 sim_steps = int(round(sim_time / dt)) points = [] # Simulate the EKF start_time = time.time() for _ in range(0, sim_steps): # last_xk = motion.run_filter(last_xk, ctrl_signal, dt) last_xk = ekf.run_filter(last_xk, ctrl_signal, dt)