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()