def test_WrapToPi(self): """ Testcase for WrapToPi against MATLAB's function """ filename = "./test_data/WrapToPiTest.mat" data = sio.loadmat(filename, struct_as_record=True) data = np.hstack((data['input'], data['output'].T)) for (inp, out) in data: assert_almost_equal(ekf.wrapToPi(inp), out, decimal=12)
def test_WrapToPi(self): """ Testcase for WrapToPi against MATLAB's function """ filename = "./test_data/WrapToPiTest.mat" data = sio.loadmat(filename,struct_as_record=True) data = np.hstack((data['input'],data['output'].T)) for (inp,out) in data: assert_almost_equal(ekf.wrapToPi(inp),out,decimal=12)
def __init__(self): # Load Parameters self.odom_used = rospy.get_param("~odom_used",True) self.imu_used = rospy.get_param("~imu_used",True) self.gps_used = rospy.get_param("~gps_used",True) self.cutters_used = rospy.get_param("~cutters_used",False) self.decimate_ahrs = rospy.get_param("~decimate_ahrs_by_factor",0) self.publish_rate = 1.0/rospy.get_param("~output_publish_rate", 25) self.time_delay = rospy.get_param("~time_delay",0.0) self.output_frame = rospy.get_param("~output_frame","odom_combined") self.output_states = rospy.get_param("~output_states",False) self.output_states_dir = rospy.get_param("~output_states_dir","~/.ros/") self.adaptive_encoders = rospy.get_param("~adaptive_encoders",False) self.adaptive_cutters = rospy.get_param("~adaptive_cutters",False) self.publish_states = rospy.get_param("~publish_states",False) if self.output_states: import time self.output_states_file = self.output_states_dir + "ekf_states-" + \ time.strftime('%F-%H-%M') + ".csv" self.output_file = open(self.output_states_file,'w') self.location_initilized = False self.heading_initilized = False self.ahrs_count = 0 # Class Variables self.ekf = AutomowEKF.fromDefault() self.v = 0 self.w = 0 self.filter_time = rospy.Time.now() self.cutter_l = 0 self.cutter_r = 0 # Subscribers if self.odom_used: rospy.loginfo("Adding Encoders to the EKF") self.enc_sub = rospy.Subscriber("encoders", StampedEncoders, self.encoders_cb) else: rospy.loginfo("You aren't using the wheel encoders, expect bad results") rospy.loginfo("Adding IMU to the EKF") self.imu_sub = rospy.Subscriber("imu/data", Imu, self.imu_cb) rospy.loginfo("Adding GPS to the EKF") self.gps_sub = rospy.Subscriber("gps/odometry", Odometry, self.gps_cb) if self.cutters_used or self.adaptive_cutters: rospy.loginfo("Subscribing to the cutter status") self.cut_sub = rospy.Subscriber("/CutterControl", CutterControl, self.cutter_cb) if self.adaptive_cutters: rospy.loginfo("P matrix will change on cutter status") # Publishers self.odom_pub = rospy.Publisher("ekf/odom",Odometry) self.odometry_timer = threading.Timer(self.publish_rate, self.odometry_cb) self.odometry_timer.start() if self.publish_states: self.states_pub = rospy.Publisher("ekf/states",States)
def __init__(self): # Load Parameters self.odom_used = rospy.get_param("~odom_used", True) self.imu_used = rospy.get_param("~imu_used", True) self.gps_used = rospy.get_param("~gps_used", True) self.decimate_ahrs = rospy.get_param("~decimate_ahrs_by_factor", 0) self.publish_rate = 1.0 / rospy.get_param("~output_publish_rate", 25) self.time_delay = rospy.get_param("~time_delay", 0.0) self.output_tf = rospy.get_param("~output_tf", True) self.output_frame = rospy.get_param("~output_frame", "odom_combined") self.adaptive_encoders = rospy.get_param("~adaptive_encoders", False) self.publish_states = rospy.get_param("~publish_states", False) self.encoder_resolution = 1000 self.location_initilized = False self.heading_initilized = False self.ahrs_count = 0 # Class Variables self.ekf = AutomowEKF.fromDefault() self.v = 0 self.w = 0 self.filter_time = rospy.Time.now() self.cutter_l = 0 self.cutter_r = 0 # Subscribers if self.odom_used: rospy.loginfo("Adding Encoders to the EKF") self.enc_sub = rospy.Subscriber("/encoders", StampedEncoders, self.encoders_cb) self.enc_prev_time = rospy.Time.now() if self.imu_used: rospy.loginfo("Adding IMU to the EKF") self.imu_sub = rospy.Subscriber("/imu/data", Imu, self.imu_cb) else: rospy.loginfo("IMU will not be used.") self.ekf.measurementUpdateAHRS(0) self.filter_time = rospy.Time.now() self.heading_initilized = True if self.gps_used: rospy.loginfo("Adding GPS to the EKF") self.gps_sub = rospy.Subscriber("/magellan_dg14/odometry", Odometry, self.gps_cb) self.gps_fix_sub = rospy.Subscriber("/magellan_dg14/utm_fix", UTMFix, self.gps_fix_cb) self.gps_bad = False else: rospy.loginfo("GPS will not be used") y = np.array([0, 0], dtype=np.double) covar = np.diag(np.array([0.001, 0.001])) self.ekf.measurementUpdateGPS(y, covar) self.filter_time = rospy.Time.now() self.location_initilized = True # Publishers if self.publish_states: self.states_pub = rospy.Publisher("ekf/states", States) self.odom_pub = rospy.Publisher("ekf/odom", Odometry) self.odometry_timer = threading.Timer(self.publish_rate, self.odometry_cb) self.odometry_timer.start()