def state_estimation():
se = StateEst()
# initialize node
rospy.init_node('state_estimation_sensor', anonymous=True)
# topic subscriptions / publications
rospy.Subscriber('imu/data', Imu, se.imu_callback)
rospy.Subscriber('vel_est', Vel_est, se.encoder_vel_callback)
rospy.Subscriber('ecu', ECU, se.ecu_callback)
rospy.Subscriber('hedge_pos', hedge_pos, se.gps_callback, queue_size=1)
state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=1)
# get vehicle dimension parameters
L_f = rospy.get_param("L_a") # distance from CoG to front axel
L_r = rospy.get_param("L_b") # distance from CoG to rear axel
vhMdl = (L_f, L_r)
# set node rate
loop_rate = 50
dt = 1.0 / loop_rate
rate = rospy.Rate(loop_rate)
se.t0 = rospy.get_rostime().to_sec() # set initial time
z_EKF = zeros(14) # x, y, psi, v, psi_drift
P = eye(14) # initial dynamics coveriance matrix
qa = 1000.0
qp = 1000.0
# x, y, vx, vy, ax, ay, psi, psidot, psidrift, x, y, psi, v
#Q = diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp,0.01, 0.01,0.01,1.0,1.0,0.1])
#R = diag([0.5,0.5,0.5,0.1,10.0,1.0,1.0, 5.0,5.0,0.1,0.5, 1.0, 1.0])
Q = diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp,0.1, 0.2,0.2,1.0,1.0,0.1])
R = diag([5.0,5.0,1.0,10.0,100.0,1000.0,1000.0, 5.0,5.0,10.0,1.0, 10.0,10.0])
#R = diag([20.0,20.0,1.0,10.0,100.0,1000.0,1000.0, 20.0,20.0,10.0,1.0, 10.0,10.0])
# x,y,v,psi,psiDot,a_x,a_y, x, y, psi, v
# Set up track parameters
l = Localization()
l.create_track()
#l.prepare_trajectory(0.06)
d_f_hist = [0.0]*10 # assuming that we are running at 50Hz, array of 10 means 0.2s lag
d_f_lp = 0.0
a_lp = 0.0
t_now = 0.0
# Estimation variables
(x_est, y_est, a_x_est, a_y_est, v_est_2) = [0]*5
bta = 0.0
v_est = 0.0
psi_est = 0.0
est_counter = 0
acc_f = 0.0
vel_meas_est = 0.0
while not rospy.is_shutdown():
t_now = rospy.get_rostime().to_sec()-se.t0
se.x_meas = polyval(se.c_X, t_now)
se.y_meas = polyval(se.c_Y, t_now)
se.gps_updated = False
se.imu_updated = False
se.vel_updated = False
# define input
d_f_hist.append(se.cmd_servo) # this is for a 0.2 seconds delay of steering
d_f_lp = d_f_lp + 0.5*(se.cmd_servo-d_f_lp) # low pass filter on steering
a_lp = a_lp + 1.0*(se.cmd_motor-a_lp) # low pass filter on acceleration
#u = [a_lp, d_f_hist.pop(0)]
u = [se.cmd_motor, d_f_hist.pop(0)]
bta = 0.5 * u[1]
# print "V, V_x and V_y : (%f, %f, %f)" % (se.vel_meas,cos(bta)*se.vel_meas, sin(bta)*se.vel_meas)
# get measurement
y = array([se.x_meas, se.y_meas, se.vel_meas, se.yaw_meas, se.psiDot_meas, se.a_x_meas, se.a_y_meas,
se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, cos(bta)*se.vel_meas, sin(bta)*se.vel_meas])
# build extra arguments for non-linear function
args = (u, vhMdl, dt, 0)
# apply EKF and get each state estimate
(z_EKF, P) = ekf(f_SensorKinematicModel, z_EKF, P, h_SensorKinematicModel, y, Q, R, args)
# Read values
(x_est, y_est, v_x_est, v_y_est, a_x_est, a_y_est, psi_est, psi_dot_est, psi_drift_est,
x_est_2, y_est_2, psi_est_2, v_est_2, psi_drift_est_2) = z_EKF # note, r = EKF estimate yaw rate
se.x_est = x_est_2
se.y_est = y_est_2
#print "V_x and V_y : (%f, %f)" % (v_x_est, v_y_est)
# Update track position
l.set_pos(x_est_2, y_est_2, psi_est_2, v_x_est, v_y_est, psi_dot_est)
# Calculate new s, ey, epsi (only 12.5 Hz, enough for controller that runs at 10 Hz)
if est_counter%4 == 0:
l.find_s()
#l.s = 0
#l.epsi = 0
#l.s_start = 0
# and then publish position info
ros_t = rospy.get_rostime()
state_pub_pos.publish(pos_info(Header(stamp=ros_t), l.s, l.ey, l.epsi, v_est_2, l.s_start, l.x, l.y, l.v_x, l.v_y,
l.psi, l.psiDot, se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, se.psiDot_meas,
psi_drift_est, a_x_est, a_y_est, se.a_x_meas, se.a_y_meas, se.cmd_motor, se.cmd_servo,
(0,), (0,), (0,), l.coeffCurvature.tolist()))
# wait
est_counter += 1
rate.sleep()
def main():
# node initialization
rospy.init_node("state_estimation")
a_delay = 0.0
df_delay = 0.0
loop_rate = 200.0
counter = 0
t0 = rospy.get_rostime().to_sec()
imu = ImuClass(t0)
gps = GpsClass(t0)
enc = EncClass(t0)
ecu = EcuClass(t0)
sim = SimulatorClass(t0)
est = Estimator(t0, loop_rate, a_delay, df_delay)
estMsg = pos_info()
saved_x_est = []
saved_y_est = []
saved_vx_est = []
saved_vy_est = []
saved_psi_est = []
saved_psiDot_est = []
saved_ax_est = []
saved_ay_est = []
saved_switch = []
rospy.sleep(0.1) # Soluciona los problemas de inicializacion
while not rospy.is_shutdown():
est.estimateState(imu, gps, enc, ecu, sim, est.GS_LPV_Est,
est.Continuous_AB_Comp, est.L_Gain_Comp)
# Save estimator Input.
saved_x_est.append(estMsg.x)
saved_y_est.append(estMsg.y)
saved_vx_est.append(estMsg.v_x)
saved_vy_est.append(estMsg.v_y)
saved_psi_est.append(estMsg.psi)
saved_psiDot_est.append(estMsg.psiDot)
saved_ax_est.append(estMsg.a_x)
saved_ay_est.append(estMsg.a_y)
# Save estimator output
est.saveHistory()
estMsg.header.stamp = rospy.get_rostime()
estMsg.v = np.sqrt(est.vx_est**2 + est.vy_est**2)
estMsg.x = est.x_est
estMsg.y = est.y_est
estMsg.v_x = est.vx_est
estMsg.v_y = est.vy_est
estMsg.psi = est.yaw_est
estMsg.psiDot = est.psiDot_est
estMsg.a_x = est.ax_est
estMsg.a_y = est.ay_est
estMsg.u_a = ecu.a
estMsg.u_df = ecu.df
est.state_pub_pos.publish(estMsg)
est.rate.sleep()
print "gps x package lost:", float(est.x_count) / float(est.pkg_count)
print "gps y package lost:", float(est.y_count) / float(est.pkg_count)
print "enc v package lost:", float(est.v_meas_count) / float(
est.pkg_count)
print "imu ax package lost:", float(est.ax_count) / float(
est.pkg_count)
print "imu ay package lost:", float(est.ay_count) / float(
est.pkg_count)
print "imu psiDot package lost:", float(est.psiDot_count) / float(
est.pkg_count)
homedir = os.path.expanduser("~")
pathSave = os.path.join(homedir, "barc_data/estimator_output.npz")
np.savez(pathSave,
yaw_est_his=saved_psi_est,
psiDot_est_his=saved_psiDot_est,
x_est_his=saved_x_est,
y_est_his=saved_y_est,
vx_est_his=saved_vx_est,
vy_est_his=saved_vy_est,
ax_est_his=saved_ax_est,
ay_est_his=saved_ay_est,
gps_time=est.gps_time,
imu_time=est.imu_time,
enc_time=est.enc_time,
inp_x_his=est.x_his,
inp_y_his=est.y_his,
inp_v_meas_his=est.v_meas_his,
inp_ax_his=est.ax_his,
inp_ay_his=est.ay_his,
inp_psiDot_his=est.psiDot_his,
inp_a_his=est.inp_a_his,
inp_df_his=est.inp_df_his,
flagVy=saved_switch,
roll_his=est.roll_his,
pitch_his=est.pitch_his,
wx_his=est.wx_his,
wy_his=est.wy_his,
wz_his=est.wz_his,
v_rl_his=est.v_rl_his,
v_rr_his=est.v_rr_his,
v_fl_his=est.v_fl_his,
v_fr_his=est.v_fr_his,
psi_raw_his=est.psi_raw_his)
pathSave = os.path.join(homedir, "barc_data/estimator_imu.npz")
np.savez(pathSave,
psiDot_his=imu.psiDot_his,
roll_his=imu.roll_his,
pitch_his=imu.pitch_his,
yaw_his=imu.yaw_his,
ax_his=imu.ax_his,
ay_his=imu.ay_his,
imu_time=imu.time_his)
pathSave = os.path.join(homedir, "barc_data/estimator_gps.npz")
np.savez(pathSave, x_his=gps.x_his, y_his=gps.y_his, gps_time=gps.time_his)
pathSave = os.path.join(homedir, "barc_data/estimator_enc.npz")
np.savez(pathSave,
v_fl_his=enc.v_fl_his,
v_fr_his=enc.v_fr_his,
v_rl_his=enc.v_rl_his,
v_rr_his=enc.v_rr_his,
enc_time=enc.time_his)
pathSave = os.path.join(homedir, "barc_data/estimator_ecu.npz")
np.savez(pathSave,
a_his=ecu.a_his,
df_his=ecu.df_his,
ecu_time=ecu.time_his)
print "Finishing saveing state estimation data"
def main():
# node initialization
rospy.init_node("state_estimation")
a_delay = 0.0
df_delay = 0.0
loop_rate = 100.0
# Tuning for estimator at high speed
Q_hs = eye(7)
Q_hs[0, 0] = rospy.get_param("/state_estimator/Qx_hs") # 0.5 # x
Q_hs[1, 1] = rospy.get_param("/state_estimator/Qy_hs") # 0.5 # y
Q_hs[2, 2] = rospy.get_param("/state_estimator/Qvx_hs") #10.0 # vx
Q_hs[3, 3] = rospy.get_param("/state_estimator/Qvy_hs") #10.0 # vy
Q_hs[4, 4] = rospy.get_param("/state_estimator/Qax_hs") #1.0 # ax
Q_hs[5, 5] = rospy.get_param("/state_estimator/Qay_hs") #1.0 # ay
Q_hs[6, 6] = rospy.get_param(
"/state_estimator/Qpsi_hs") #10 + 80.0 # psi
R_hs = eye(6)
R_hs[0,
0] = rospy.get_param("/state_estimator/Rx_hs") # 10 + 40.0 # x
R_hs[1, 1] = rospy.get_param("/state_estimator/Ry_hs") #10 + 40.0 # y
R_hs[2, 2] = rospy.get_param("/state_estimator/Rvx_hs") # 0.1 # vx
R_hs[3,
3] = rospy.get_param("/state_estimator/Rax_hs") #30 + 10.0 # ax
R_hs[4, 4] = rospy.get_param("/state_estimator/Ray_hs") #40.0 # ay
R_hs[5, 5] = rospy.get_param("/state_estimator/Rvy_hs") # 0.01 # vy
# Tuning for estimator at low speed
Q_ls = eye(7)
Q_ls[0, 0] = rospy.get_param("/state_estimator/Qx_ls") # 0.5 # x
Q_ls[1, 1] = rospy.get_param("/state_estimator/Qy_ls") #0.5 # y
Q_ls[2, 2] = rospy.get_param("/state_estimator/Qvx_ls") #10.0 # vx
Q_ls[3, 3] = rospy.get_param("/state_estimator/Qvy_ls") #10.0 # vy
Q_ls[4, 4] = rospy.get_param("/state_estimator/Qax_ls") #1.0 # ax
Q_ls[5, 5] = rospy.get_param("/state_estimator/Qay_ls") #1.0 # ay
Q_ls[6, 6] = rospy.get_param(
"/state_estimator/Qpsi_ls") #10 + 80.0 # psi
R_ls = eye(6)
R_ls[0,
0] = rospy.get_param("/state_estimator/Rx_ls") # 10 + 40.0 # x
R_ls[1,
1] = rospy.get_param("/state_estimator/Ry_ls") # 10 + 40.0 # y
R_ls[2, 2] = rospy.get_param("/state_estimator/Rvx_ls") # 0.1 # vx
R_ls[3,
3] = rospy.get_param("/state_estimator/Rax_ls") # 30 + 10.0 # ax
R_ls[4, 4] = rospy.get_param("/state_estimator/Ray_ls") # 40.0 # ay
R_ls[5, 5] = rospy.get_param("/state_estimator/Rvy_ls") # 0.01 # vy
thReset = rospy.get_param("/state_estimator/thReset") # 0.4
vSwitch = rospy.get_param("/state_estimator/vSwitch") # 1.0
psiSwitch = rospy.get_param("/state_estimator/psiSwitch") # 0.5 * 2.0
# Q = eye(7)
# Q[0,0] = 0.001**2 # Q_x
# Q[1,1] = 0.001**2 # Q_y
# Q[2,2] = 0.01**2 # Q_vx
# Q[3,3] = 0.001**2 # Q_vy
# Q[4,4] = 0.1**2 # Q_ax
# Q[5,5] = 0.1**2 # Q_ay
# Q[6,6] = 0.001**2 # Q_psi
# R = eye(6)
# R[0,0] = 10.0#**2 # R_x
# R[1,1] = 10.0#**2 # R_y
# R[2,2] = 0.01**2 # R_vx
# R[3,3] = 0.016 # R_ax
# R[4,4] = 0.024 # R_ay
# R[5,5] = 0.001**2 # R_vy
# Q_hs = Q
# R_ls = R
# Q_hs = Q
# R_ls = R
t0 = rospy.get_rostime().to_sec()
imu = ImuClass(t0)
gps = GpsClass(t0)
enc = EncClass(t0)
ecu = EcuClass(t0)
est = Estimator(t0, loop_rate, a_delay, df_delay, Q_hs, Q_ls, R_hs, R_ls,
thReset, vSwitch, psiSwitch)
estMsg = pos_info()
saved_x_est = []
saved_y_est = []
saved_vx_est = []
saved_vy_est = []
saved_psi_est = []
saved_psiDot_est = []
saved_ax_est = []
saved_ay_est = []
saved_switch = []
while not rospy.is_shutdown():
# if ecu.a != 0:
est.estimateState(imu, gps, enc, ecu, est.ekf)
# Save estimator Input.
saved_x_est.append(estMsg.x)
saved_y_est.append(estMsg.y)
saved_vx_est.append(estMsg.v_x)
saved_vy_est.append(estMsg.v_y)
saved_psi_est.append(estMsg.psi)
saved_psiDot_est.append(estMsg.psiDot)
saved_ax_est.append(estMsg.a_x)
saved_ay_est.append(estMsg.a_y)
saved_switch.append(int(est.flagVy))
# Save estimator output
est.saveHistory()
estMsg.header.stamp = rospy.get_rostime()
estMsg.v = np.sqrt(est.vx_est**2 + est.vy_est**2)
estMsg.x = est.x_est
estMsg.y = est.y_est
estMsg.v_x = est.vx_est
estMsg.v_y = est.vy_est
estMsg.psi = est.yaw_est
estMsg.psiDot = est.psiDot_est
estMsg.a_x = est.ax_est
estMsg.a_y = est.ay_est
estMsg.u_a = ecu.a
estMsg.u_df = ecu.df
est.state_pub_pos.publish(estMsg)
est.rate.sleep()
print "gps x package lost:", float(est.x_count) / float(est.pkg_count)
print "gps y package lost:", float(est.y_count) / float(est.pkg_count)
print "enc v package lost:", float(est.v_meas_count) / float(
est.pkg_count)
print "imu ax package lost:", float(est.ax_count) / float(
est.pkg_count)
print "imu ay package lost:", float(est.ay_count) / float(
est.pkg_count)
print "imu psiDot package lost:", float(est.psiDot_count) / float(
est.pkg_count)
homedir = os.path.expanduser("~")
pathSave = os.path.join(homedir, "barc_data/estimator_output.npz")
np.savez(pathSave,
yaw_est_his=saved_psi_est,
psiDot_est_his=saved_psiDot_est,
x_est_his=saved_x_est,
y_est_his=saved_y_est,
vx_est_his=saved_vx_est,
vy_est_his=saved_vy_est,
ax_est_his=saved_ax_est,
ay_est_his=saved_ay_est,
gps_time=est.gps_time,
imu_time=est.imu_time,
enc_time=est.enc_time,
inp_x_his=est.x_his,
inp_y_his=est.y_his,
inp_v_meas_his=est.v_meas_his,
inp_ax_his=est.ax_his,
inp_ay_his=est.ay_his,
inp_psiDot_his=est.psiDot_his,
inp_a_his=est.inp_a_his,
inp_df_his=est.inp_df_his,
flagVy=saved_switch,
roll_his=est.roll_his,
pitch_his=est.pitch_his,
wx_his=est.wx_his,
wy_his=est.wy_his,
wz_his=est.wz_his,
v_rl_his=est.v_rl_his,
v_rr_his=est.v_rr_his,
v_fl_his=est.v_fl_his,
v_fr_his=est.v_fr_his,
psi_raw_his=est.psi_raw_his)
pathSave = os.path.join(homedir, "barc_data/estimator_imu.npz")
np.savez(pathSave,
psiDot_his=imu.psiDot_his,
roll_his=imu.roll_his,
pitch_his=imu.pitch_his,
yaw_his=imu.yaw_his,
ax_his=imu.ax_his,
ay_his=imu.ay_his,
imu_time=imu.time_his)
pathSave = os.path.join(homedir, "barc_data/estimator_gps.npz")
np.savez(pathSave, x_his=gps.x_his, y_his=gps.y_his, gps_time=gps.time_his)
pathSave = os.path.join(homedir, "barc_data/estimator_enc.npz")
np.savez(pathSave,
v_fl_his=enc.v_fl_his,
v_fr_his=enc.v_fr_his,
v_rl_his=enc.v_rl_his,
v_rr_his=enc.v_rr_his,
enc_time=enc.time_his)
pathSave = os.path.join(homedir, "barc_data/estimator_ecu.npz")
np.savez(pathSave,
a_his=ecu.a_his,
df_his=ecu.df_his,
ecu_time=ecu.time_his)
print "Finishing saveing state estimation data"
def main():
# node initialization
rospy.init_node("state_estimation")
a_delay = rospy.get_param("state_estimator/delay_a")
df_delay = rospy.get_param("state_estimator/delay_df")
loop_rate = 50.0
# Initialize Map
map = Map()
Q = eye(8)
Q[0,0] = rospy.get_param("state_estimator/Q_x")
Q[1,1] = rospy.get_param("state_estimator/Q_y")
Q[2,2] = rospy.get_param("state_estimator/Q_vx")
Q[3,3] = rospy.get_param("state_estimator/Q_vy")
Q[4,4] = rospy.get_param("state_estimator/Q_ax")
Q[5,5] = rospy.get_param("state_estimator/Q_ay")
Q[6,6] = rospy.get_param("state_estimator/Q_psi")
Q[7,7] = rospy.get_param("state_estimator/Q_psiDot")
R = eye(6)
R[0,0] = rospy.get_param("state_estimator/R_x")
R[1,1] = rospy.get_param("state_estimator/R_y")
R[2,2] = rospy.get_param("state_estimator/R_vx")
R[3,3] = rospy.get_param("state_estimator/R_ax")
R[4,4] = rospy.get_param("state_estimator/R_ay")
R[5,5] = rospy.get_param("state_estimator/R_psiDot")
# R[6,6] = rospy.get_param("state_estimator/R_vy")
t0 = rospy.get_rostime().to_sec()
imu = ImuClass(t0)
gps = GpsClass(t0)
enc = EncClass(t0)
ecu = EcuClass(t0)
est = Estimator(t0,loop_rate,a_delay,df_delay,Q,R)
estMsg = pos_info()
while not rospy.is_shutdown():
est.estimateState(imu,gps,enc,ecu,est.ekf)
estMsg.s, estMsg.ey, estMsg.epsi, _ = map.getLocalPosition(est.x_est, est.y_est, est.yaw_est)
estMsg.header.stamp = rospy.get_rostime()
estMsg.v = np.sqrt(est.vx_est**2 + est.vy_est**2)
estMsg.x = est.x_est
estMsg.y = est.y_est
estMsg.v_x = est.vx_est
estMsg.v_y = est.vy_est
estMsg.psi = est.yaw_est
estMsg.psiDot = est.psiDot_est
estMsg.a_x = est.ax_est
estMsg.a_y = est.ay_est
estMsg.u_a = ecu.a
estMsg.u_df = ecu.df
est.state_pub_pos.publish(estMsg)
est.saveHistory()
est.rate.sleep()
homedir = os.path.expanduser("~")
pathSave = os.path.join(homedir,"barc_debugging/estimator_output.npz")
np.savez(pathSave,yaw_est_his = est.yaw_est_his,
psiDot_est_his = est.psiDot_est_his,
x_est_his = est.x_est_his,
y_est_his = est.y_est_his,
vx_est_his = est.vx_est_his,
vy_est_his = est.vy_est_his,
ax_est_his = est.ax_est_his,
ay_est_his = est.ay_est_his,
estimator_time = est.time_his)
pathSave = os.path.join(homedir,"barc_debugging/estimator_imu.npz")
np.savez(pathSave,psiDot_his = imu.psiDot_his,
roll_his = imu.roll_his,
pitch_his = imu.pitch_his,
yaw_his = imu.yaw_his,
ax_his = imu.ax_his,
ay_his = imu.ay_his,
imu_time = imu.time_his)
pathSave = os.path.join(homedir,"barc_debugging/estimator_gps.npz")
np.savez(pathSave,x_his = gps.x_his,
y_his = gps.y_his,
gps_time = gps.time_his)
pathSave = os.path.join(homedir,"barc_debugging/estimator_enc.npz")
np.savez(pathSave,v_fl_his = enc.v_fl_his,
v_fr_his = enc.v_fr_his,
v_rl_his = enc.v_rl_his,
v_rr_his = enc.v_rr_his,
enc_time = enc.time_his)
pathSave = os.path.join(homedir,"barc_debugging/estimator_ecu.npz")
np.savez(pathSave,a_his = ecu.a_his,
df_his = ecu.df_his,
ecu_time = ecu.time_his)
print "Finishing saveing state estimation data"
def state_estimation():
global dt_v_enc
global v_meas, psi_meas
global x_local, y_local
global new_enc_meas, new_gps_meas, new_imu_meas
global vhMdl
# initialize node
rospy.init_node('state_estimation_bm', anonymous=True)
# topic subscriptions / publications
rospy.Subscriber('imu/data', Imu, imu_callback)
rospy.Subscriber('encoder', Encoder, enc_callback)
# rospy.Subscriber('vel_est', Encoder, vel_est_callback)
rospy.Subscriber('ecu', ECU, ecu_callback)
rospy.Subscriber('ecu_pwm', ECU, ecu_pwm_callback)
rospy.Subscriber('fix', NavSatFix, gps_callback)
state_pub = rospy.Publisher('state_estimate', Z_KinBkMdl, queue_size = 10)
state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size = 10)
# for debugging
meas_pub = rospy.Publisher('meas', Z_KinBkMdl, queue_size = 10)
u_pub = rospy.Publisher('u', ECU, queue_size = 10)
# get vehicle dimension parameters
L_a = rospy.get_param("L_a") # distance from CoG to front axel
L_b = rospy.get_param("L_b") # distance from CoG to rear axel
vhMdl = (L_a, L_b)
# get encoder parameters
dt_v_enc = rospy.get_param("state_estimation/dt_v_enc") # time interval to compute v_x from encoders
# get EKF observer properties
q_std = rospy.get_param("state_estimation/q_std") # std of process noise
r_std = rospy.get_param("state_estimation/r_std") # std of measurementnoise
# get measurement model type according to incorporated sensors
est_mode = rospy.get_param("state_estimation/est_mode")
##################################################################################################################################
# Set up track parameters (for simulation, this needs to be changed according to the track the sensor data was recorded for)
l = Localization()
l.create_track()
est_counter = 0
##################################################################################################################################
# set node rate
loop_rate = 50
dt = 1.0 / loop_rate
rate = rospy.Rate(loop_rate)
t0 = time.time()
# initial state vector (mean)
x_EKF = zeros(4)
# estimation variables for the filter (covariances)
# make the uncertainty in the initial psi and/or psi_drift bigger the less we know about the relationship between the relationship
# between the GPS coordinate system (namely NESW) and the IMU coordinate system
# Precisely: where is the car heading initially w.r.t. NESW?
P = diag([0.1,0.1,50*q_std**2,0.01]) # initial state covariance matrix
Q = diag([q_std**2,q_std**2,10*q_std**2,2*q_std**2]) # process noise covariance matrix (drift: 5)
R = diag([r_std**2,r_std**2,2*r_std**2,r_std**2]) # measurement noise covariance matrix
# publish initial state estimate
(x, y, psi, psi_drift) = x_EKF
# print x,y,psi,v
v = acc
state_pub.publish( Z_KinBkMdl(x, y, psi, v) )
# collect input
u = [ d_f, acc ]
# u_pub.publish( ECU(u[1],u[0]) )
# wait
rate.sleep()
while not rospy.is_shutdown():
# collect measurements
z = array([x_local, y_local, psi_meas, v_meas])
# print
# print z
z_new = array([new_gps_meas, new_gps_meas, new_imu_meas, new_enc_meas])
meas_pub.publish(Z_KinBkMdl(x_local,y_local,psi_meas,v_meas))
z = z[z_new]
# Reset booleans for new measurements for the next iteration
new_enc_meas = False
new_gps_meas = False
new_imu_meas = False
# print u
# reshape covariance matrix according to number of received measurements
R_k = R[:,z_new][z_new,:]
# decide which measurement model to use based on which measurements came in
if z_new[0] and z_new[2] and z_new[3]:
est_mode = 1
elif not z_new[0] and z_new[2] and z_new[3]:
est_mode = 2
elif z_new[0] and not z_new[2] and not z_new[3]:
est_mode = 3
elif z_new[0] and not z_new[2] and z_new[3]:
est_mode = 4
elif z_new[0] and z_new[2] and not z_new[3]:
est_mode = 5
elif not z_new[0] and not z_new[2] and z_new[3]:
est_mode = 6
elif not z_new[0] and z_new[2] and not z_new[3]:
est_mode = 7
elif not z_new[0] and not z_new[2] and not z_new[3]:
est_mode = 8
else:
est_mode = 0 # a case is not covered -> error
args = (u,vhMdl,dt,est_mode)
# collect input for the next iteration
u = [ d_f, acc ]
# u_pub.publish( ECU(u[1],u[0]) )
# print est_mode
# apply EKF and get each state estimate
(x_EKF,P) = ekf(f_KinBkMdl, x_EKF, P, h_KinBkMdl, z, Q, R_k, args )
# publish state estimate
# (x, y, psi, v) = x_EKF
v = acc
(x, y, psi, psi_drift) = x_EKF
# print x,y,psi,psi_drift
# publish information
state_pub.publish( Z_KinBkMdl(x, y, psi, v) )
# Update track position (x,y,psi,v_x,v_y,psi_dot)
l.set_pos(x, y, psi, v, 0, 0)
# Comment: in find_s, the linear and angular velocities are not needed,
# therefore we can just ignore splitting up v here
# Calculate new s, ey, epsi (only 12.5 Hz, enough for controller that runs at 10 Hz)
if est_counter%4 == 0:
l.find_s()
# and then publish position info
ros_t = rospy.get_rostime()
state_pub_pos.publish(pos_info(Header(stamp=ros_t), l.s, l.ey, l.epsi, v, l.s_start, l.x, l.y, l.v_x, l.v_y,
l.psi, l.psiDot, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0,
(0,), (0,), (0,), l.coeffCurvature.tolist()))
est_counter += 1
# wait
rate.sleep()