def test_path_gen(): ''' test only path generation in Sim. ''' #### choose a built-in IMU model, typical for IMU381 imu_err = 'mid-accuracy' # generate GPS and magnetometer data imu = imu_model.IMU(accuracy=imu_err, axis=9, gps=True) #### Algorithm # ECF based inclinometer from demo_algorithms import inclinometer_mahony algo1 = inclinometer_mahony.MahonyFilter() from demo_algorithms import inclinometer_acc algo2 = inclinometer_acc.TiltAcc() #### start simulation sim = ins_sim.Sim( [fs, fs_gps, fs_mag], motion_def_path + "//motion_def-static.csv", # 'e://Projects//gnss-ins-sim//demo_saved_data//2018-04-24-14-55-53//', ref_frame=1, imu=imu, mode=None, env=None, algorithm=[algo1, algo2]) sim.run(3) # save simulation data to files sim.results('.//demo_saved_data//') # plot data, 3d plot of reference positoin, 2d plots of gyro and accel sim.plot(['att_euler'], opt={'att_euler': 'error'})
def test_free_integration(): ''' test Sim ''' #### do not need an IMU model imu = None #### Algorithm from demo_algorithms import free_integration ini_pos_vel_att = np.genfromtxt(log_dir+"ini.txt", delimiter=',') ini_pos_vel_att[0:2] *= attitude.D2R # For Lat and Lon, deg to rad ini_pos_vel_att[6:9] *= attitude.D2R # Attitude from deg to rad if not using_external_g: ini_pos_vel_att = ini_pos_vel_att[0:9] # create the algorithm object algo = free_integration.FreeIntegration(ini_pos_vel_att, earth_rot=False) from demo_algorithms import inclinometer_acc algo2 = inclinometer_acc.TiltAcc() #### start simulation sim = ins_sim.Sim([fs, 0.0, 0.0], log_dir, ref_frame=0, imu=imu, mode=None, env=None, algorithm=[algo, algo2]) # run the simulation sim.run(1) # generate simulation results, summary sim.results('', end_point=True, extra_opt='ned') # plot sim.plot(['pos', 'vel', 'att_euler', 'accel', 'gyro'], opt={'pos':'error', 'vel':'error', 'att_euler':'error'})
def test_free_integration(): ''' test Sim ''' #### do not need an IMU model imu = None #### Algorithm # Free integration in a virtual inertial frame from demo_algorithms import free_integration ini_pos_vel_att = np.genfromtxt(log_dir + "ini.txt") # add initial states error if needed ini_vel_err = np.array([0.0, 0.0, 0.0 ]) # initial velocity error in the body frame, m/s ini_att_err = np.array([0.0, 0.0, 0.0]) # initial Euler angles error, deg ini_pos_vel_att[0:2] = ini_pos_vel_att[0:2] * attitude.D2R ini_pos_vel_att[3:6] += ini_vel_err ini_pos_vel_att[6:9] = (ini_pos_vel_att[6:9] + ini_att_err) * attitude.D2R if not using_external_g: ini_pos_vel_att = ini_pos_vel_att[0:9] # create the algorithm object algo = free_integration.FreeIntegration(ini_pos_vel_att, earth_rot=False) from demo_algorithms import inclinometer_acc algo2 = inclinometer_acc.TiltAcc() #### start simulation sim = ins_sim.Sim([fs, 0.0, 0.0], log_dir, ref_frame=0, imu=imu, mode=None, env=None, algorithm=[algo, algo2]) # run the simulation sim.run(1) # generate simulation results, summary sim.results('', end_point=True, extra_opt='ned') # plot sim.plot(['pos', 'vel', 'att_euler', 'accel'], opt={ 'pos': 'error', 'vel': 'error', 'att_euler': 'error' })