def test_dmu380_sim():
'''
test Sim with DMU380 algorithm.
'''
#### choose a built-in IMU model, typical for IMU380
imu_err = {
'gyro_b': np.array([1.0, -1.0, 0.5]) * 1800.0,
'gyro_arw': np.array([0.25, 0.25, 0.25]) * 1.0,
'gyro_b_stability': np.array([3.5, 3.5, 3.5]) * 1.0,
'gyro_b_corr': np.array([100.0, 100.0, 100.0]),
'accel_b': np.array([5.0e-3, 5.0e-3, 5.0e-3]) * 10.0,
'accel_vrw': np.array([0.03119, 0.03009, 0.04779]) * 1.0,
'accel_b_stability': np.array([4.29e-5, 5.72e-5, 8.02e-5]) * 1.0,
'accel_b_corr': np.array([200.0, 200.0, 200.0]),
'mag_std': np.array([0.2, 0.2, 0.2]) * 1.0
}
gps_err = {
'stdp': np.array([5.0, 5.0, 7.0]) * 0.2,
'stdv': np.array([0.05, 0.05, 0.05]) * 1.0
}
# generate GPS and magnetometer data
imu = imu_model.IMU(accuracy=imu_err, axis=9, gps=True, gps_opt=gps_err)
#### Algorithm
# DMU380 algorithm
from demo_algorithms import aceinna_ins
cfg_file = os.path.abspath(
'.//demo_algorithms//dmu380_sim_lib//ekfSim_ins.cfg')
algo = aceinna_ins.DMU380Sim(cfg_file)
#### start simulation
sim = ins_sim.Sim(
[fs, 1, fs],
motion_def_path + "//motion_def-long_drive.csv",
# ".//demo_saved_data//2019-08-27-10-26-14//",
ref_frame=0,
imu=imu,
mode=None,
env='[0.01 0.01 0.11]g-random',
algorithm=algo)
sim.run(1)
# generate simulation results, summary, and save data to files
sim.results('.//demo_saved_data//tmp',
err_stats_start=210,
gen_kml=True,
extra_opt='ned')
# plot data
sim.plot(['pos', 'vel', 'wb', 'ab', 'att_euler'],
opt={
'pos': 'error',
'vel': 'error',
'att_euler': 'error'
})
def run_simulation(data, fileName, request_body):
'''
An Allan analysis demo for Sim.
'''
#### Customized IMU model parameters, typical for IMU381
rateJSON = json.loads(data.rateJSON, object_hook=JSONObject)
accelJSON = json.loads(data.accelJSON, object_hook=JSONObject)
axisNum = 6
gpsFlag = False
gpsObj = {}
imu_err = {
'gyro_b':
np.array(
[float(rateJSON.b[0]),
float(rateJSON.b[1]),
float(rateJSON.b[2])]),
'gyro_arw':
np.array([
float(rateJSON.arw[0]),
float(rateJSON.arw[1]),
float(rateJSON.arw[2])
]) * 1.0,
'gyro_b_stability':
np.array([
float(rateJSON.b_drift[0]),
float(rateJSON.b_drift[1]),
float(rateJSON.b_drift[2])
]) * 1.0,
'gyro_b_corr':
np.array([
float(rateJSON.b_corr[0]),
float(rateJSON.b_corr[1]),
float(rateJSON.b_corr[2])
]),
'accel_b':
np.array([
float(accelJSON.b[0]),
float(accelJSON.b[1]),
float(accelJSON.b[2])
]),
'accel_vrw':
np.array([
float(accelJSON.vrw[0]),
float(accelJSON.vrw[1]),
float(accelJSON.vrw[2])
]) * 1.0,
'accel_b_stability':
np.array([
float(accelJSON.b_drift[0]),
float(accelJSON.b_drift[1]),
float(accelJSON.b_drift[2])
]) * 1.0,
'accel_b_corr':
np.array([
float(accelJSON.b_corr[0]),
float(accelJSON.b_corr[1]),
float(accelJSON.b_corr[2])
])
}
if 'magJSON' in request_body:
axisNum = 9
magJSON = json.loads(data.magJSON, object_hook=JSONObject)
imu_err['mag_std'] = np.array([
float(magJSON.std[0]),
float(magJSON.std[1]),
float(magJSON.std[2])
]) * 1.0
if 'si' in data.magJSON:
imu_err['mag_si'] = np.array([[
float(magJSON.si[0]),
float(magJSON.si[1]),
float(magJSON.si[2])
],
[
float(magJSON.si[3]),
float(magJSON.si[4]),
float(magJSON.si[5])
],
[
float(magJSON.si[6]),
float(magJSON.si[7]),
float(magJSON.si[8])
]])
if 'hi' in data.magJSON:
imu_err['mag_hi'] = np.array([
float(magJSON.hi[0]),
float(magJSON.hi[1]),
float(magJSON.hi[2])
])
if 'gpsJSON' in request_body:
gpsFlag = True
gpsJSON = json.loads(data.gpsJSON, object_hook=JSONObject)
gpsObj['stdp'] = np.array([
float(gpsJSON.stdp[0]),
float(gpsJSON.stdp[1]),
float(gpsJSON.stdp[2])
])
gpsObj['stdv'] = np.array([
float(gpsJSON.stdv[0]),
float(gpsJSON.stdv[1]),
float(gpsJSON.stdv[2])
])
gpsObj['avail'] = 0.95
# do not generate GPS and magnetometer data
imu = imu_model.IMU(accuracy=imu_err,
axis=axisNum,
gps=gpsFlag,
gps_opt=gpsObj)
if data.algorithmName == 'Allan':
Allanfs = 100.0 # IMU sample frequency
#### Allan analysis algorithm
from demo_algorithms import allan_analysis
algo = allan_analysis.Allan()
#### start simulation
sim = ins_sim.Sim([Allanfs, Allanfs, 0.0],
motion_def_path + "//motion_def-Allan.csv",
ref_frame=data.ref_frame,
imu=imu,
mode=None,
env=None,
algorithm=algo,
fileName=fileName,
data=data)
sim.run(data.algorithmRunTimes)
# generate simulation results, summary, and save data to files
sim.results(fileName, gen_kml=True,
update_flag=True) # save data files
# plot data
#sim.plot(['ad_accel', 'ad_gyro'])
elif data.algorithmName == 'FreeIntegration':
Freefs = 100.0 # IMU sample frequency
# Free integration in a virtual inertial frame
ini_pos_vel_att = np.fromstring(data.algorithmParams,
dtype=float,
sep=',')
#ini_pos_vel_att[0] = ini_pos_vel_att[0] * D2R
#ini_pos_vel_att[1] = ini_pos_vel_att[1] * D2R
#ini_pos_vel_att[6:9] = ini_pos_vel_att[6:9] * D2R
# 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[3:6] += ini_vel_err
#ini_pos_vel_att[6:9] += ini_att_err * D2R
from demo_algorithms import free_integration
algo = free_integration.FreeIntegration(ini_pos_vel_att)
staticsFlag = data.algorithmStatistics == 'end-point'
sim = ins_sim.Sim([Freefs, Freefs, 0.0],
motion_def_path + "//motion_def-90deg_turn.csv",
ref_frame=data.ref_frame,
imu=imu,
mode=None,
env=None,
algorithm=algo,
fileName=fileName,
data=data)
# run the simulation for 1000 times
sim.run(data.algorithmRunTimes)
# generate simulation results, summary
# do not save data since the simulation runs for 1000 times and generates too many results
sim.results(fileName,
gen_kml=True,
end_point=staticsFlag,
update_flag=True,
extra_opt='ned')
elif data.algorithmName == 'VG':
VGfs = 200.0 # IMU sample frequency
from demo_algorithms import aceinna_vg
cfg_file = os.path.abspath(
'.//demo_algorithms//dmu380_sim_lib//ekfSim_tilt.cfg')
algo = aceinna_vg.DMU380Sim(cfg_file)
sim = ins_sim.Sim(
[VGfs, VGfs, VGfs],
"//mnt//share//jd_figure8.csv",
ref_frame=data.ref_frame,
imu=imu,
mode=None,
env=None, #'[0.1 0.01 0.11]g-random',
algorithm=algo,
fileName=fileName,
data=data)
sim.run(data.algorithmRunTimes)
# generate simulation results, summary, and save data to files
sim.results(
fileName,
gen_kml=True,
update_flag=True,
) # do not save data
elif data.algorithmName == 'INS':
INSfs = 200.0 # IMU sample frequency
from demo_algorithms import aceinna_ins
cfg_file = os.path.abspath(
'.//demo_algorithms//dmu380_sim_lib//ekfSim_ins.cfg')
algo = aceinna_ins.DMU380Sim(cfg_file)
sim = ins_sim.Sim(
[INSfs, INSfs, INSfs],
"//mnt//share//jd_figure8.csv",
ref_frame=data.ref_frame,
imu=imu,
mode=None,
env=None, #'[0.1 0.01 0.11]g-random',
algorithm=algo,
fileName=fileName,
data=data)
sim.run(data.algorithmRunTimes)
# generate simulation results, summary, and save data to files
sim.results(
fileName,
gen_kml=True,
update_flag=True,
) # do not save data