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'
})