}
# 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):
