def test_path_gen():
'''
test only path generation in Sim.
'''
# imu_err
imu_err = {'gyro_b': np.array([1e-2, 2e-2, 3e-2]) / D2R * 3600 * 0,
'gyro_arw': np.array([1e-5, 1e-5, 1e-5]) / D2R * 60 * 0,
'gyro_b_stability': np.array([1e-5, 1e-5, 1e-5]) / D2R * 3600 * 1e-0,
'gyro_b_corr': np.array([100.0, 100.0, 100.0]),
'accel_b': np.array([2.0e-3, 1.0e-3, 5.0e-3]) * 0,
'accel_vrw': np.array([1e-4, 1e-4, 1e-4]) * 60.0 * 0,
'accel_b_stability': np.array([1e-4, 1e-4, 1e-4]) * 1.0 * 1e0,
'accel_b_corr': np.array([200.0, 200.0, 200.0]),
# 'mag_std': np.array([0.2, 0.2, 0.2]) * 1.0
}
# generate GPS and magnetometer data
gps_err = {
'stdp': np.array([1, 1, 1]) * 1e-6,
'stdv': np.array([0, 0, 0]),
}
imu = imu_model.IMU(accuracy=imu_err, axis=6, gps=True, gps_opt=gps_err)
# start simulation
sim = ins_sim.Sim([fs, fs_gps, fs_mag],
motion_def_path+"//imu_def7.csv",
ref_frame=0,
imu=imu,
mode=None,
env=None,
algorithm=None)
sim.run(1)
# save simulation data to files
sim.results('/imu/data7')
def test_dmu380_sim():
'''
test Sim with DMU380 algorithm.
'''
print("This demo only runs on Ubuntu x64.")
#### choose a built-in IMU model, typical for IMU380
imu_err = 'mid-accuracy'
# do not generate GPS and magnetometer data
imu = imu_model.IMU(accuracy=imu_err, axis=9, gps=False)
#### Algorithm
# DMU380 algorithm
from demo_algorithms import dmu380_sim
cfg_file = os.path.abspath(
'.//demo_algorithms//dmu380_sim_lib//ekfSim_tilt.cfg')
algo = dmu380_sim.DMU380Sim(cfg_file)
#### start simulation
sim = imu_sim.Sim([fs, 0.0, fs],
imu,
motion_def_path + "//motion_def-Komatsu_loaded_-5_0.csv",
ref_frame=1,
mode=None,
env=None,
algorithm=algo)
sim.run(10)
# generate simulation results, summary, and save data to files
sim.results() # do not save data
# plot data
sim.plot(['att_euler'])
def test_path_gen():
'''
test only path generation in Sim.
'''
#### imu_err
imu_err = {'gyro_b': np.array([1e-2, 2e-2, 3e-2]) / D2R * 3600 * 0,
'gyro_arw': np.array([1e-5, 1e-5, 1e-5]) / D2R * 60,
'gyro_b_stability': np.array([5e-6, 5e-6, 5e-6]) / D2R * 3600,
'gyro_b_corr': np.array([100.0, 100.0, 100.0]),
'accel_b': np.array([2.0e-3, 1.0e-3, 5.0e-3]) * 0,
'accel_vrw': np.array([1e-4, 1e-4, 1e-4]) * 60.0,
'accel_b_stability': np.array([1e-5, 1e-5, 1e-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
}
# generate GPS and magnetometer data
imu = imu_model.IMU(accuracy=imu_err, axis=6, gps=True)
#### start simulation
sim = ins_sim.Sim([fs, fs_gps, fs_mag],
motion_def_path+"//imu_def6.csv",
ref_frame=1,
imu=imu,
mode=None,
env=None,
algorithm=None)
sim.run(1)
# save simulation data to files
sim.results('/home/zsp/Desktop/imu/data5')
# plot data, 3d plot of reference positoin, 2d plots of gyro and accel
# sim.plot(['gyro', 'accel'])
sim.plot(['ref_pos', 'gyro', 'gps_visibility'], opt={'ref_pos': '3d'})
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_inclinometer_mahony():
'''
test Sim with a inclinometer algorithm based on Mahony's theory.
'''
#### choose a built-in IMU model, typical for IMU380
imu_err = 'low-accuracy'
# do not generate GPS and magnetometer data
imu = imu_model.IMU(accuracy=imu_err, axis=6, gps=False)
#### Algorithm
# ECF based inclinometer
from demo_algorithms import inclinometer_mahony
algo = inclinometer_mahony.MahonyFilter()
#### start simulation
sim = imu_sim.Sim([fs, 0.0, 0.0],
imu,
motion_def_path + "//motion_def.csv",
ref_frame=1,
mode=None,
env=None,
algorithm=algo)
sim.run()
# generate simulation results, summary, and save data to files
sim.results() # do not save data
# plot data
sim.plot(['att_euler', 'ref_att_euler'], opt={'att_euler': 'error'})
def test_path_gen():
'''
test ANS as GUI.
'''
#### simulation config
motion_def_path = os.path.abspath('.//demo_motion_def_files//')
fs = 100.0 # IMU sample frequency
fs_gps = 10.0 # GPS sample frequency
fs_mag = fs # magnetometer sample frequency, not used for now
#### 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)
#### start simulation
with open(motion_def_path+"//motion_def-3d.csv", 'r') as f:
tmp = f.read()
sim = ins_sim.Sim([fs, fs_gps, fs_mag],
tmp,
ref_frame=0,
imu=imu,
mode=None,
env=None,
algorithm=None)
sim.run(1)
# save simulation data to files
sim.results('')
#### GUI
gui = gui_ans.GuiAns()
gui.start(sim)
def odometer_sim():
#### IMU model, typical for IMU381
imu_err = {
'gyro_b': np.array([0.0, 0.0, 0.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([0.0e-3, 0.0e-3, 0.0e-3]),
'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]) * 1.0,
'stdv': np.array([0.05, 0.05, 0.05]) * 1.0
}
# generate GPS and magnetometer data
imu = imu_model.IMU(accuracy=imu_err, axis=6, gps=True, gps_opt=gps_err)
#### Algorithm
# Free integration based on odometer in NED frame
from demo_algorithms import gps_odo
'''
Initial states (position, velocity and attitude) are from motion_def csv file.
'''
ini_pos_vel_att = np.genfromtxt(motion_def_path+"//motion_def-ins_odo.csv",\
delimiter=',', skip_header=1, max_rows=1)
ini_pos_vel_att[0] = ini_pos_vel_att[
0] * D2R #将lat (deg),lon (deg), yaw (deg), pitch (deg), roll (deg)单位转为弧度 rad
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
# create the algorith object
algo = gps_odo.OdometerSim(ini_pos_vel_att)
#### start simulation
sim = ins_sim.Sim(
[fs, fs, 0.0], # imu, gps, no mag.
motion_def_path +
"//motion_def-ins_odo.csv", # motion_def-90deg_turn_gps motion_def-ins_odo
ref_frame=1,
imu=imu,
mode=None,
env=None,
algorithm=algo)
# run the simulation for 1 times
sim.run(1)
# generate simulation results, summary
# save results
sim.results(err_stats_start=-1, gen_kml=True)
sim.plot(['ref_pos', 'gyro', 'gps_visibility', 'accel'],
opt={'ref_pos': '3d'})
def test_free_integration():
'''
test Sim
'''
#### IMU model, typical for IMU381
imu_err = {'gyro_b': np.array([0.0, 0.0, 0.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([0.0e-3, 0.0e-3, 0.0e-3]),
'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
}
odo_err = {'scale': 0.999,
'stdv': 0.1}
# do not generate GPS and magnetometer data
imu = imu_model.IMU(accuracy=imu_err, axis=6, gps=False, odo=True, odo_opt=odo_err)
#### Algorithm
# Free integration in a virtual inertial frame
from demo_algorithms import free_integration_odo
from demo_algorithms import free_integration
'''
Free integration requires initial states (position, velocity and attitude). You should provide
theses values when you create the algorithm object.
'''
ini_pos_vel_att = np.genfromtxt(motion_def_path+"//motion_def-90deg_turn.csv",\
delimiter=',', skip_header=1, max_rows=1)
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
# create the algorith object
algo1 = free_integration_odo.FreeIntegration(ini_pos_vel_att)
algo2 = free_integration.FreeIntegration(ini_pos_vel_att)
#### start simulation
sim = ins_sim.Sim([fs, 0.0, 0.0],
motion_def_path+"//motion_def-90deg_turn.csv",
ref_frame=1,
imu=imu,
mode=None,
env=None,
algorithm=[algo1, algo2])
# run the simulation for 1000 times
sim.run(10)
# generate simulation results, summary
# do not save data since the simulation runs for 1000 times and generates too many results
sim.results(err_stats_start=-1, gen_kml=True)
def test_path_gen():
'''
test only path generation in Sim.
'''
#### choose a built-in IMU model, typical for IMU381
#imu_err = 'low-accuracy'
#imu_err = 'mid-accuracy'
imu_err = {
# gyro bias, deg/hr
'gyro_b': np.array([0.0014, 0.0014, 0.0014]),
# gyro angle random walk, deg/rt-hr
'gyro_arw': np.array([0.00016, 0.00016, 0.00016]),
# gyro bias instability, deg/hr
'gyro_b_stability': np.array([0.0, 0.0, 0.0]),
# gyro bias instability correlation, sec.
# set this to 'inf' to use a random walk model
# set this to a positive real number to use a first-order Gauss-Markkov model
'gyro_b_corr': np.array([np.Inf, np.Inf, np.Inf]),
# accelerometer bias, m/s^2
'accel_b': np.array([0.0e-3, 0.0e-3, 0.0e-3]),
# accelerometer velocity random walk, m/s/rt-hr
'accel_vrw': np.array([5.0e-6, 5.0e-6, 5.0e-6]),
# accelerometer bias instability, m/s^2
'accel_b_stability': np.array([0.60e-3, 0.60e-3, 0.80e-3]),
# accelerometer bias instability correlation, sec. Similar to gyro_b_corr
'accel_b_corr': np.array([np.Inf, np.Inf, np.Inf]),
# magnetometer noise std, uT
'mag_std': np.array([0.2, 0.2, 0.2])
}
gps_err = {'stdp': np.array([5.0, 5.0, 7.0]) * 0.2,
'stdv': np.array([0.05, 0.05, 0.05]) * 1.0}
odo_err = {'scale': 0.999,
'stdv': 0.01}
# generate GPS and magnetometer data
imu = imu_model.IMU(accuracy=imu_err, axis=9, gps=True, gps_opt=gps_err,
odo=True, odo_opt=odo_err)
#### start simulation
sim = ins_sim.Sim([fs, fs_gps, fs_mag],
motion_def_path+"motion_def-Holland_tunnel.csv",
ref_frame=1,
imu=imu,
mode=None,
env=None,
algorithm=None)
sim.run()
# plot data, 3d plot of reference positoin, 2d plots of gyro and accel
sim.plot(['ref_att_euler', 'gyro', 'mag', 'accel'], opt={'ref_pos': '2d'})
# save simulation data to files
res = []
res = sim.get_data(['time', 'gps_time', 'ref_gps', 'ref_pos', 'ref_vel', 'ref_accel', 'ref_gyro', 'odo', 'gyro', 'mag', 'accel'])
return res
def get_gnss_ins_sim(motion_def_file, fs_imu, fs_gps):
'''
Generate simulated GNSS/IMU data using specified trajectory.
'''
# set IMU model:
imu_err = 'low-accuracy'
# generate GPS and magnetometer data:
imu = imu_model.IMU(accuracy=imu_err, axis=9, gps=True)
# init simulation:
sim = ins_sim.Sim(
[fs_imu, fs_gps, fs_imu],
motion_def_file,
ref_frame=1,
imu=imu,
mode=None,
env=None,
algorithm=None
)
# run:
sim.run(1)
# get simulated data:
rospy.logwarn(
"Simulated data size {}".format(
len(sim.dmgr.get_data_all('gyro').data[0])
)
)
# imu measurements:
step_size = 1.0 / fs_imu
for i, (gyro, accel) in enumerate(
zip(
# a. gyro
sim.dmgr.get_data_all('gyro').data[0],
# b. accel
sim.dmgr.get_data_all('accel').data[0]
)
):
yield {
'stamp': i * step_size,
'data': {
# a. gyro:
'gyro_x': gyro[0],
'gyro_y': gyro[1],
'gyro_z': gyro[2],
# b. accel:
'accel_x': accel[0],
'accel_y': accel[1],
'accel_z': accel[2]
}
}
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 mahony_filter_test():
'''
test Sim
'''
#### IMU model, typical for IMU381
imu_err = {
'gyro_b': np.array([0.0, 0.0, 0.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([0.0e-3, 0.0e-3, 0.0e-3]),
'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
}
# do not generate GPS data
imu = imu_model.IMU(accuracy=imu_err, axis=9, gps=False)
#### Algorithm
# mahony filter in a virtual inertial frame.
from demo_algorithms import vg_mahony
'''
calculate pitch, roll (and yaw if input mag data) by mahony filter.
'''
# create the algorith object
algo = vg_mahony.MahonyTest()
#### start simulation
sim = ins_sim.Sim(
[fs, 0.0, 0.0],
# '/Users/songyang/project/code/github/gnss-ins-sim_learn/demo_data_files/bosch',
motion_def_path +
"//motion_def-90deg_turn_gps.csv", #motion_def.csv motion_def-90deg_turn.csv
ref_frame=1,
imu=imu,
mode=None,
env=None,
algorithm=[algo])
# run the simulation for 1000 times
sim.run(1)
# generate simulation results, summary
# do not save data since the simulation runs for 1000 times and generates too many results
sim.results(err_stats_start=-1, gen_kml=True)
# plot postion error
# sim.plot(['pos'], opt={'pos':'error'})
sim.plot(['att_euler'], opt={'att_euler': 'error'})
def test_mag_cal():
'''
test soft iron and hard iron calibration.
'''
print("This demo only runs on Ubuntu x64.")
#### IMU model, typical for IMU381
imu_err = 'mid-accuracy'
# do not generate GPS data
imu = imu_model.IMU(accuracy=imu_err, axis=9, gps=False)
mag_error = {
'si': np.eye(3) + np.random.randn(3, 3) * 0.1,
'hi': np.array([10.0, 10.0, 10.0]) * 1.0
}
imu.set_mag_error(mag_error)
#### Algorithm
from demo_algorithms import mag_calibrate
algo = mag_calibrate.MagCal()
#### start simulation
sim = ins_sim.Sim(
[fs, fs_gps, fs_mag],
motion_def_path + "//motion_def_mag_cal.csv",
# motion_def_path+"//test_mag_cal//",
ref_frame=1,
imu=imu,
mode=None,
env=None,
algorithm=algo)
sim.run(1)
# save simulation data to files
sim.results()
# plot data
sim.plot(['mag', 'mag_cal'],
opt={
'mag': 'projection',
'mag_cal': 'projection'
},
extra_opt='.')
# show calibration params:
print('true soft iron is:')
print(np.linalg.inv(mag_error['si']))
print('estimated soft iron is:')
print(sim.dmgr.soft_iron.data)
print('true hard iron is:')
print(mag_error['hi'])
print('estimated hard iron is:')
print(sim.dmgr.hard_iron.data)
def imu_creation(arw_input=.25, odo_scale=.999, odo_noise=.1, bias_input=3.5):
"""
Create the IMU object from the library. Currently setup to operate as a 6 axis IMU, allowing odometer input, without GPS.
There are three built-in IMU models: 'low-accuracy', 'mid-accuracy' and 'high accuracy'.
Inputs: Currently, the gyro angle random walk, odometer scale error, odometer noise, and gyro bias instability. These
could be expanded to include other sources of error, for instance in the accelerometer.
Returns: An IMU object
"""
# IMU model, typical for IMU381ZA
imu_err = {
'gyro_b': np.array([0.0, 0.0, 0.0]),
# gyro angle random walk, deg/rt-hr
'gyro_arw': np.array([arw_input, arw_input, arw_input]) * 1.0,
# gyro bias instability, deg/hr
'gyro_b_stability': np.array([bias_input, bias_input, bias_input]),
# gyro bias instability correlation, sec.
# set this to 'inf' to use a random walk model
# set this to a positive real number to use a first-order Gauss-Markkov model
'gyro_b_corr': np.array([100.0, 100.0, 100.0]),
# accelerometer bias, m/s^2
'accel_b': np.array([0.0e-3, 0.0e-3, 0.0e-3]),
# accelerometer velocity random walk, m/s/rt-hr
'accel_vrw': np.array([0.03119, 0.03009, 0.04779]) * 1.0,
# accelerometer bias instability, m/s^2
'accel_b_stability': np.array([4.29e-5, 5.72e-5, 8.02e-5]) * 1.0,
# accelerometer bias instability correlation, sec. Similar to gyro_b_corr
'accel_b_corr': np.array([200.0, 200.0, 200.0]),
# magnetometer noise std, uT
'mag_std': np.array([0.2, 0.2, 0.2]) * 1.0
}
odo_err = {'scale': odo_scale, 'stdv': odo_noise}
# create imu object
# do not generate GPS and magnetometer data
imu = imu_model.IMU(accuracy=imu_err,
axis=9,
gps=False,
odo=True,
odo_opt=odo_err)
return imu
def gen_data_first(data_dir, fileName="motion_tmp.csv"):
'''
Generate data that will be used by test_gen_data_from_files()
'''
# imu model
imu = imu_model.IMU(accuracy='mid-accuracy', axis=6, gps=False)
# start simulation
sim = ins_sim.Sim([fs, fs_gps, fs_mag],
motion_def_path + "//" + fileName,
ref_frame=0,
imu=imu,
mode=None,
env=None,
algorithm=None)
sim.run(10)
# save simulation data to files
sim.results(data_dir, stdout=False)
def test_dmu380_sim():
'''
test Sim with DMU380 algorithm.
'''
print("This demo only runs on Ubuntu x64.")
#### choose a built-in IMU model, typical for IMU380
imu_err = {
'gyro_b': np.array([1.0, -1.0, 0.5]) * 1.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([50.0e-3, 50.0e-3, 50.0e-3]),
'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
}
# do not generate GPS and magnetometer data
imu = imu_model.IMU(accuracy=imu_err, axis=9, gps=False)
#### Algorithm
# DMU380 algorithm
from demo_algorithms import dmu380_sim
cfg_file = os.path.abspath(
'.//demo_algorithms//dmu380_sim_lib//ekfSim_tilt.cfg')
algo = dmu380_sim.DMU380Sim(cfg_file)
#### start simulation
sim = ins_sim.Sim(
[fs, 0.0, fs],
# motion_def_path+"//motion_def-static.csv",
"//mnt//share//jd_figure8.csv",
ref_frame=1,
imu=imu,
mode=None,
env=None, #'[0.1 0.01 0.11]g-random',
algorithm=algo)
sim.run(1)
# generate simulation results, summary, and save data to files
sim.results('aa') # do not save data
# plot data
sim.plot(['att_euler', 'ref_pos'], opt={'ref_pos': 'projection'})
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)
#### start simulation
sim = imu_sim.Sim([fs, fs_gps, fs_mag], imu, motion_def_path+"//motion_def.csv",
ref_frame=1,
mode=None,
env=None,
algorithm=None)
sim.run(1)
# 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(['ref_pos', 'gyro', 'accel'], opt={'ref_pos': '3d'})
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]) * 1.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([50.0e-3, 50.0e-3, 50.0e-3]) * 0.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
}
# do not generate GPS and magnetometer data
imu = imu_model.IMU(accuracy=imu_err, axis=6, gps=False)
#### Algorithm
# DMU380 algorithm
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)
#### start simulation
sim = ins_sim.Sim(
[fs, 0.0, fs],
motion_def_path + "//motion_def-static.csv",
# ".//demo_saved_data//car_test_20180929//",
ref_frame=1,
imu=imu,
mode=None,
env=None, #'[0.1 0.01 0.11]g-random',
algorithm=algo)
sim.run(1)
# generate simulation results, summary, and save data to files
sim.results() # do not save data
# plot data
sim.plot(['att_euler', 'ref_pos'], opt={'att_euler': 'error'})
def run_and_save_results(args, motion_def):
resultsdir = args.outdir
stagingdir = os.path.join(resultsdir, "staging")
# Before running, warn if the resultsdir holds existing results.
if glob.glob("{}/dr_*.csv".format(resultsdir)):
print(
"WARNING: DR trajectory results already exist in the given results directory."
)
print(" You may end up with a mismatched set.")
# Setup and run N simulations.
imu = imu_model.IMU(accuracy=IMU_MODELS[args.imu], axis=6, gps=False)
ini_pos_vel_att = read_initial_condition(motion_def)
for i in range(args.N):
if args.enable_init_error:
init_cond = perturbed_initial_condition(ini_pos_vel_att)
else:
init_cond = ini_pos_vel_att
if args.enable_vibrations:
env = DEFAULT_VIBRATIONS
else:
env = None
algo = FreeIntegrationWithVel(init_cond,
meas_vel_stddev=args.odom_sigma)
sim = ins_sim.Sim([args.fs, 0.0, 0.0],
motion_def,
ref_frame=0,
imu=imu,
mode=None,
env=env,
algorithm=algo)
sim.run(1)
# We don't care for the printed results.
with open(os.devnull, 'w') as devnull:
stdout = sys.stdout
sys.stdout = devnull
sim.results(stagingdir, end_point=True)
sys.stdout = stdout
collate_sim_results(stagingdir,
os.path.join(resultsdir, "dr_{}.csv".format(i)))
shutil.rmtree(stagingdir)
def test_inclinometer_mahony():
'''
test Sim with a inclinometer algorithm based on Mahony's theory.
'''
#### choose a built-in IMU model, typical for IMU380
imu_err = {
'gyro_b': np.array([5.0, -5.0, 2.5]) * 3600.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([50.0e-3, 50.0e-3, 50.0e-3]) * 0.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
}
# do not generate GPS and magnetometer data
imu = imu_model.IMU(accuracy=imu_err, axis=6, gps=False)
#### Algorithm
# ECF based inclinometer
from demo_algorithms import inclinometer_mahony
algo = inclinometer_mahony.MahonyFilter()
#### start simulation
sim = ins_sim.Sim(
[fs, 0.0, 0.0],
motion_def_path + "//motion_def.csv",
ref_frame=1,
imu=imu,
mode=None,
env=None, #'[0.01 0.01 0.11]g-random',
algorithm=algo)
sim.run()
# generate simulation results, summary, and save data to files
sim.results() # do not save data
# plot data
sim.plot(['att_euler', 'wb', 'ab'], opt={'att_euler': 'error'})
def test_ins_loose():
'''
test Sim
'''
#### IMU model, typical for IMU381
imu_err = {
'gyro_b': np.array([0.0, 0.0, 0.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([0.0e-3, 0.0e-3, 0.0e-3]),
'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
}
# do not generate GPS and magnetometer data
imu = imu_model.IMU(accuracy=imu_err, axis=6, gps=True)
#### Algorithm
# loosely couple INS algorithm
from demo_algorithms import ins_loose
algo = ins_loose.InsLoose()
#### start simulation
sim = ins_sim.Sim([fs, fs_gps, 0.0],
motion_def_path + "//motion_def-90deg_turn.csv",
ref_frame=0,
imu=imu,
mode=None,
env=None,
algorithm=algo)
# run the simulation for 1000 times
sim.run()
# generate simulation results, summary
# do not save data since the simulation runs for 1000 times and generates too many results
sim.results(end_point=True)
sim.plot(['ref_pos', 'pos'], opt={'ref_pos': '3d'})
def test_allan():
'''
An Allan analysis demo for Sim.
'''
#### Customized IMU model parameters, typical for IMU381
imu_err = {
'gyro_b': np.array([0.0, 0.0, 0.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([0.0e-3, 0.0e-3, 0.0e-3]),
'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
}
# do not generate GPS and magnetometer data
imu = imu_model.IMU(accuracy=imu_err, axis=6, gps=False)
#### Allan analysis algorithm
from demo_algorithms import allan_analysis
algo = allan_analysis.Allan()
#### start simulation
sim = ins_sim.Sim([fs, 0.0, 0.0],
motion_def_path + "//motion_def-Allan.csv",
ref_frame=1,
imu=imu,
mode=None,
env=None,
algorithm=algo)
sim.run()
# generate simulation results, summary, and save data to files
sim.results() # save data files
# plot data
sim.plot(['ad_accel', 'ad_gyro'])
def get_gnss_ins_sim(motion_def_file, fs_imu, fs_gps):
'''
Generate simulated GNSS/IMU data using specified trajectory.
'''
# set IMU model:
D2R = math.pi / 180.0
# imu_err = 'low-accuracy'
imu_err = {
# 1. gyro:
# a. random noise:
# gyro angle random walk, deg/rt-hr
'gyro_arw': np.array([0., 0., 0.]),
# gyro bias instability, deg/hr
'gyro_b_stability': np.array([0.0, 0.0, 0.0]),
# gyro bias isntability correlation time, sec
'gyro_b_corr': np.array([100.0, 100.0, 100.0]),
# b. deterministic error:
'gyro_b': np.array([0.0, 0.0, 0.0]),
'gyro_k': np.array([1.0, 1.0, 1.0]),
'gyro_s': np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
# 2. accel:
# a. random noise:
# accel velocity random walk, m/s/rt-hr
'accel_vrw': np.array([0., 0., 0.]),
# accel bias instability, m/s2
'accel_b_stability': np.array([0., 0., 0.]),
# accel bias isntability correlation time, sec
'accel_b_corr': np.array([100.0, 100.0, 100.0]),
# b. deterministic error:
'accel_b': np.array([0.0, 0.0, 0.0]),
'accel_k': np.array([1.0, 1.0, 1.0]),
'accel_s': np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
# 3. mag:
'mag_si': np.eye(3) + np.random.randn(3, 3) * 0.0,
'mag_hi': np.array([10.0, 10.0, 10.0]) * 0.0,
'mag_std': np.array([0.1, 0.1, 0.1])
}
# generate GPS and magnetometer data:
imu = imu_model.IMU(accuracy=imu_err, axis=9, gps=True)
# init simulation:
sim = ins_sim.Sim([fs_imu, fs_gps, fs_imu],
motion_def_file,
ref_frame=1,
imu=imu,
mode=None,
env=None,
algorithm=None)
# true pos init xyz
init_lla = np.array([31.224361, 121.469170, 0])
init_xyz = geoparams.lla2ecef(init_lla)
print(init_xyz)
# run:
sim.run(1)
# get simulated data:
rospy.logwarn('Simulated data size: Gyro-{}, Accel-{}, pos-{}'.format(
len(sim.dmgr.get_data_all('gyro').data[0]),
len(sim.dmgr.get_data_all('accel').data[0]),
len(sim.dmgr.get_data_all('ref_pos').data)))
# imu measurements:
step_size = 1.0 / fs_imu
for i, (gyro, accel, ref_q, ref_pos, ref_vel) in enumerate(
zip(
# a. gyro
sim.dmgr.get_data_all('gyro').data[0],
# b. accel
sim.dmgr.get_data_all('accel').data[0],
# c. true pose
sim.dmgr.get_data_all('ref_att_quat').data,
sim.dmgr.get_data_all('ref_pos').data,
#d. true vel
sim.dmgr.get_data_all('ref_vel').data)):
yield {
'stamp': i * step_size,
'data': {
# a. gyro:
'gyro_x': gyro[0],
'gyro_y': gyro[1],
'gyro_z': gyro[2],
# b. accel:
'accel_x': accel[0],
'accel_y': accel[1],
'accel_z': accel[2],
# c. orientation:
'gt_quat_w': ref_q[0],
'gt_quat_x': ref_q[1],
'gt_quat_y': ref_q[2],
'gt_quat_z': ref_q[3],
#d. position
'gt_pos_x': ref_pos[0] + 2849886.61825,
'gt_pos_y': ref_pos[1] - 4656214.27294,
'gt_pos_z': ref_pos[2] - 3287190.60046,
#e. velocity
'gt_vel_x': ref_vel[0],
'gt_vel_y': ref_vel[1],
'gt_vel_z': ref_vel[2]
}
}
sim.results()
sim.plot(['ref_pos', 'ref_vel'], opt={'ref_pos': '3d'})
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
def get_gnss_ins_sim(motion_def_file, fs_imu, fs_gps):
'''
Generate simulated GNSS/IMU data using specified trajectory.
'''
# set IMU model:
D2R = math.pi/180.0
# imu_err = 'low-accuracy'
imu_err = {
# 1. gyro:
# a. random noise:
# gyro angle random walk, deg/rt-hr
'gyro_arw': np.array([0.75, 0.75, 0.75]),
# gyro bias instability, deg/hr
'gyro_b_stability': np.array([10.0, 10.0, 10.0]),
# gyro bias isntability correlation time, sec
'gyro_b_corr': np.array([100.0, 100.0, 100.0]),
# b. deterministic error:
'gyro_b': np.array([0.0, 0.0, 0.0]),
'gyro_k': np.array([1.0, 1.0, 1.0]),
'gyro_s': np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
# 2. accel:
# a. random noise:
# accel velocity random walk, m/s/rt-hr
'accel_vrw': np.array([0.05, 0.05, 0.05]),
# accel bias instability, m/s2
'accel_b_stability': np.array([2.0e-4, 2.0e-4, 2.0e-4]),
# accel bias isntability correlation time, sec
'accel_b_corr': np.array([100.0, 100.0, 100.0]),
# b. deterministic error:
'accel_b': np.array([0.0e-3, 0.0e-3, 0.0e-3]),
'accel_k': np.array([1.0, 1.0, 1.0]),
'accel_s': np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
# 3. mag:
'mag_si': np.eye(3) + np.random.randn(3, 3)*0.0,
'mag_hi': np.array([10.0, 10.0, 10.0])*0.0,
'mag_std': np.array([0.1, 0.1, 0.1])
}
# generate GPS and magnetometer data:
imu = imu_model.IMU(accuracy=imu_err, axis=9, gps=True)
# init simulation:
sim = ins_sim.Sim(
[fs_imu, fs_gps, fs_imu],
motion_def_file,
ref_frame=1,
imu=imu,
mode=None,
env=None,
algorithm=None
)
# run:
sim.run(1)
# get simulated data:
rospy.logwarn(
"Simulated data size {}".format(
len(sim.dmgr.get_data_all('gyro').data[0])
)
)
# imu measurements:
step_size = 1.0 / fs_imu
for i, (gyro, accel, ref_pos, ref_vel, ref_att_quat) in enumerate(
zip(
# a. gyro
sim.dmgr.get_data_all('ref_gyro').data,
# b. accel
sim.dmgr.get_data_all('ref_accel').data,
# c. ref_pos
sim.dmgr.get_data_all('ref_pos').data,
# d. ref_vel
sim.dmgr.get_data_all('ref_vel').data,
# e. ref_att_quat
sim.dmgr.get_data_all('ref_att_quat').data
)
):
yield {
'stamp': i * step_size,
'data': {
# a. gyro:
'gyro_x': gyro[0],
'gyro_y': gyro[1],
'gyro_z': gyro[2],
# b. accel:
'accel_x': accel[0],
'accel_y': accel[1],
'accel_z': accel[2],
# c. ref_pos:
'ref_pos_x': ref_pos[0],
'ref_pos_y': ref_pos[1],
'ref_pos_z': ref_pos[2],
# d. ref_vel:
'ref_vel_x': ref_vel[0],
'ref_vel_y': ref_vel[1],
'ref_vel_z': ref_vel[2],
# e. ref_att_quat:
'ref_att_quat_w': ref_att_quat[0],
'ref_att_quat_x': ref_att_quat[1],
'ref_att_quat_y': ref_att_quat[2],
'ref_att_quat_z': ref_att_quat[3]
}
}
def get_gnss_ins_sim(motion_def_file, fs_imu, fs_gps):
'''
Generate simulated GNSS/IMU data using specified trajectory.
'''
# set IMU model:
D2R = math.pi / 180.0
# imu_err = 'low-accuracy'
imu_err = {
# 1. gyro:
# a. random noise:
# gyro angle random walk, deg/rt-hr
# 'gyro_arw': np.array([0.75, 0.75, 0.75]),
'gyro_arw': np.array([0.00, 0.00, 0.00]),
# gyro bias instability, deg/hr
# 'gyro_b_stability': np.array([10.0, 10.0, 10.0]),
'gyro_b_stability': np.array([0.0, 0.0, 0.0]),
# gyro bias isntability correlation time, sec
# 'gyro_b_corr': np.array([100.0, 100.0, 100.0]),
# b. deterministic error:
'gyro_b': np.array([36.00, 36.00, 36.00]),
'gyro_k': np.array([0.98, 0.98, 0.98]),
'gyro_s': np.array([0.01, 0.01, 0.01, 0.01, 0.01, 0.01]),
# 2. accel:
# a. random noise:
# accel velocity random walk, m/s/rt-hr
'accel_vrw': np.array([0.05, 0.05, 0.05]),
# accel bias instability, m/s2
'accel_b_stability': np.array([2.0e-4, 2.0e-4, 2.0e-4]),
# accel bias isntability correlation time, sec
'accel_b_corr': np.array([100.0, 100.0, 100.0]),
# b. deterministic error:
'accel_b': np.array([0.01, 0.01, 0.01]),
'accel_k': np.array([0.98, 0.98, 0.98]),
'accel_s': np.array([0.01, 0.01, 0.01, 0.01, 0.01, 0.01]),
# 3. mag:
'mag_si': np.eye(3) + np.random.randn(3, 3) * 0.0,
'mag_hi': np.array([10.0, 10.0, 10.0]) * 0.0,
'mag_std': np.array([0.1, 0.1, 0.1])
}
# generate GPS and magnetometer data:
imu = imu_model.IMU(accuracy=imu_err, axis=9, gps=True)
# init simulation:
sim = ins_sim.Sim(
# here sync GPS with other measurements as marker:
[fs_imu, fs_imu, fs_imu],
motion_def_file,
ref_frame=1,
imu=imu,
mode=None,
env=None,
algorithm=None)
# run:
sim.run(1)
# get simulated data:
rospy.logwarn(
'Simulated data size: Gyro-{}, Accel-{}, GPS Availability-{}'.format(
len(sim.dmgr.get_data_all('gyro').data[0]),
len(sim.dmgr.get_data_all('accel').data[0]),
len(sim.dmgr.get_data_all('gps_visibility').data)))
# calibration stages:
STEP_SIZE = 1.0 / fs_imu
STAGES = [
'rotate_z_pos', 'rotate_z_neg', 'rotate_y_pos', 'rotate_y_neg',
'rotate_x_pos', 'rotate_x_neg', 'static_z_pos', 'static_z_neg',
'static_y_pos', 'static_y_neg', 'static_x_pos', 'static_x_neg'
]
stage_id = 0
last_gps_visibility = True
curr_gps_visibility = True
for i, (gyro, accel, ref_gyro, ref_accel, gps_visibility) in enumerate(
zip(
# a. gyro:
sim.dmgr.get_data_all('gyro').data[0],
# b. accel:
sim.dmgr.get_data_all('accel').data[0],
# c. ref gyro:
sim.dmgr.get_data_all('ref_gyro').data,
# d. ref accel:
sim.dmgr.get_data_all('ref_accel').data,
# e. gps visibility as marker:
sim.dmgr.get_data_all('gps_visibility').data)):
last_gps_visibility = curr_gps_visibility
curr_gps_visibility = gps_visibility
if (not last_gps_visibility) and curr_gps_visibility:
stage_id += 1
if not curr_gps_visibility:
continue
yield {
'stamp': i * STEP_SIZE,
# a. gyro:
'gyro_x': gyro[0],
'gyro_y': gyro[1],
'gyro_z': gyro[2],
# b. accel:
'accel_x': accel[0],
'accel_y': accel[1],
'accel_z': accel[2],
# c. gyro:
'ref_gyro_x': ref_gyro[0],
'ref_gyro_y': ref_gyro[1],
'ref_gyro_z': ref_gyro[2],
# d. accel:
'ref_accel_x': ref_accel[0],
'ref_accel_y': ref_accel[1],
'ref_accel_z': ref_accel[2],
# e. stage:
'stage': STAGES[stage_id]
}
def get_gnss_ins_sim(motion_def_file,
fs_imu,
fs_gps,
imu_error_level='high_accuracy',
mag_error_level='mid_accuracy',
gps_error_level='mid_accuracy',
odo_error_level='mid_accuracy'):
"""
Generate simulated
IMU,
magnetometer,
GPS,
odometer
data using specified trajectory
"""
#
# set IMU model:
#
# for error-state Kalman filter observability & the degree of observability analysis
# remove deterministic error and random noise:
imu_err = config['imu'][imu_error_level].copy()
imu_err.update(config['mag'][mag_error_level])
gps_err = config['gps'][gps_error_level]
odo_err = config['odo'][odo_error_level]
# show device error level config:
for k in imu_err:
rospy.logwarn("{}: {}".format(k, imu_err[k]))
for k in gps_err:
rospy.logwarn("{}: {}".format(k, gps_err[k]))
for k in odo_err:
rospy.logwarn("{}: {}".format(k, odo_err[k]))
# generate GPS and magnetometer data:
imu = imu_model.IMU(accuracy=imu_err,
axis=9,
gps=True,
gps_opt=gps_err,
odo=True,
odo_opt=odo_err)
# init simulation:
sim = ins_sim.Sim(
# here sync GPS with other measurements as marker:
[fs_imu, fs_imu, fs_imu],
motion_def_file,
# use NED frame:
ref_frame=0,
imu=imu,
mode=None,
env=None,
algorithm=None)
# run:
sim.run(1)
# get simulated data:
rospy.logwarn("""
GNSS-INS-Sim Summary:
\tIMU Measurement:
\t\tGyroscope: {}
\t\tAccelerometer: {}
\t\tMagnetometer: {}
\tGNSS Measurement:
\t\tLLA: {}
\t\tNED Velocity: {}
\tOdometry:
\t\tVelocity: {}
\tReference Trajectory:
\t\tPosition: {}
\t\tVelocity: {}
\t\tOrientation in Quaternion: {}
""".format(
# a. IMU:
repr(sim.dmgr.get_data_all('gyro').data[0].shape),
repr(sim.dmgr.get_data_all('accel').data[0].shape),
repr(sim.dmgr.get_data_all('mag').data[0].shape),
# b. GNSS:
repr(sim.dmgr.get_data_all('gps').data[0][:, :3].shape),
repr(sim.dmgr.get_data_all('gps').data[0][:, 3:].shape),
# c. odometry:
repr(sim.dmgr.get_data_all('odo').data[0].shape),
# d. reference trajectory:
repr(sim.dmgr.get_data_all('ref_pos').data.shape),
repr(sim.dmgr.get_data_all('ref_vel').data.shape),
repr(sim.dmgr.get_data_all('ref_att_quat').data.shape),
))
# init timer:
timestamp_start = rospy.Time.now()
STEP_SIZE = 1.0 / fs_imu
# yield init pose:
init_pose_msg = get_init_pose(timestamp_start, motion_def_file)
yield init_pose_msg
# yield measurements:
for i, (
# a. IMU:
gyro,
accel,
mag,
# b. GNSS:
gps_pos,
gps_vel,
# c. odometry:
odo_x,
# d. reference trajectory:
ref_pos,
ref_vel,
ref_att_quat) in enumerate(
zip(
# a. IMU:
sim.dmgr.get_data_all('gyro').data[0],
sim.dmgr.get_data_all('accel').data[0],
sim.dmgr.get_data_all('mag').data[0],
# b. GNSS:
sim.dmgr.get_data_all('gps').data[0][:, :3],
sim.dmgr.get_data_all('gps').data[0][:, 3:],
# c. odometry velocity:
sim.dmgr.get_data_all('odo').data[0],
# d. reference trajectory:
sim.dmgr.get_data_all('ref_pos').data,
sim.dmgr.get_data_all('ref_vel').data,
sim.dmgr.get_data_all('ref_att_quat').data)):
# generate timestamp:
stamp = timestamp_start + rospy.Duration.from_sec(i * STEP_SIZE)
# a. IMU:
imu_msg = get_imu_msg(stamp, gyro, accel)
# b. magnetometer:
mag_msg = get_mag_msg(stamp, mag)
# c. GNSS:
gps_pos_msg = get_gps_pos_msg(stamp, gps_pos)
gps_vel_msg = get_gps_vel_msg(stamp, gps_vel)
# d. odometer:
odo_msg = get_odo_msg(
stamp,
# measurement is only available at forward direction
np.array([odo_x, 0.0, 0.0]))
# e. reference trajectory:
reference_pose_msg = get_pose_msg(stamp, ref_pos, ref_vel,
ref_att_quat)
yield (imu_msg, mag_msg, gps_pos_msg, gps_vel_msg, odo_msg,
reference_pose_msg)
