} # u = np.array([0.4, 0]) mup_data = [ekf_data['mu']] S_data = [ekf_data['S']] prev_t = 1289870797.29 for data in sorted_data: if len(data) == 5: # continue ekf_data = controller.ekf( ekf_data['mu'], np.array([data[1], data[2], data[3]]), ekf_data['S'], EKF_CONSTS_GPS['Q'], np.array([0.4, data[4]]), EKF_CONSTS['R'], EKF_CONSTS['G'], EKF_CONSTS['mu_p'], EKF_CONSTS_GPS['H'], EKF_CONSTS_GPS['h'](43.472294833, -80.539653), data[0], prev_t, ) else: # continue ekf_data = controller.ekf( ekf_data['mu'], np.array([data[1]]), ekf_data['S'], EKF_CONSTS_ENC['Q'], np.array([0.4, data[2]]), EKF_CONSTS['R'],
if callback_type == ENCODER: # Encoder Message Processing # Get the current time, and the number of ticks recorded cur_time = msg.header.stamp cur_ticks = msg.ticks[0] rospy.loginfo('ENCODER:I got: [%d] as encoder ticks at [%s]', cur_ticks, cur_time) csv_writers['enc'].writerow([cur_time, cur_ticks, pid_data['linear_velocity_cmd'], steering_angle['angle']]) # EKF Update if not pid_data['prev_ticks'] is None: ekf_data = controller.ekf( x=ekf_data['mu'], y=np.array([(cur_ticks - pid_data['prev_ticks']) * controller.METER_PER_TICK]), S=ekf_data['S'], u=np.array([ REF_SPEED, steering_angle['angle']]), t=cur_time.to_sec(), prev_t=ekf_data['prev_t'], **EKF_CONSTS_ENC) # Q, H, h, R, G, mu_p csv_writers['ekf'].writerow([cur_time, ekf_data['mu'], ekf_data['S'], 'ENC']) csv_writers['ekf_est'].writerow(ekf_data['mu']) # Update the velocity with a PID controller pid_data.update(encoder_pid_processing(cur_time, cur_ticks, pid_data)) csv_writers['pid'].writerow([cur_time, cur_ticks, pid_data['linear_velocity_cmd']]) # Limit the movement to a specific distance if initial_ticks is None: initial_ticks = cur_ticks if cur_ticks - initial_ticks > (20.0 / controller.METER_PER_TICK):