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 = ins_sim.Sim([fs, 0.0, fs],
motion_def_path+"//motion_def-Komatsu_level_50m_0_0.csv",
ref_frame=1,
imu=imu,
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.
'''
#### 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 = ins_sim.Sim([fs, 0.0, 0.0],
motion_def_path + "//motion_def.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() # do not save data
# plot data
sim.plot(['att_euler', 'ref_att_euler'], opt={'att_euler': 'error'})
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_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_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 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 create_sim_object(algorithm,
motion_path_filename,
fs=100,
fs_gps=0.0,
fs_mag=0.0,
imu=imu_creation()):
"""
'ref_frame' Reference frame used as the navigation frame and the attitude reference.
0: NED (default), with x axis pointing along geographic north, y axis pointing eastward, z axis pointing downward. Position will be expressed in LLA form, and the velocity of the vehicle relative to the ECEF frame will be expressed in local NED frame.
1: a virtual inertial frame with constant g, x axis pointing along geographic or magnetic north, z axis pointing along g, y axis completing a right-handed coordinate system. Position and velocity will both be in the [x y z] form in this frame.
**Notice: For this virtual inertial frame, position is indeed the sum of the initial position in ecef and the relative position in the virutal inertial frame. Indeed, two vectors expressed in different frames should not be added. This is done in this way here just to preserve all useful information to generate .kml files. Keep this in mind if you use this result.
'fs' Sample frequency of IMU, units: Hz
'fs_gps' Sample frequency of GNSS, units: Hz
'fs_mag' Sample frequency of magnetometer, units: Hz
'time' Time series corresponds to IMU samples, units: sec.
'gps_time' Time series corresponds to GNSS samples, units: sec.
'algo_time' Time series corresponding to algorithm output, units: ['s']. If your algorithm output data rate is different from the input data rate, you should include 'algo_time' in the algorithm output.
'gps_visibility' Indicate if GPS is available. 1 means yes, and 0 means no.
'ref_pos' True position in the navigation frame. When users choose NED (ref_frame=0) as the navigation frame, positions will be given in the form of [Latitude, Longitude, Altitude], units: ['rad', 'rad', 'm']. When users choose the virtual inertial frame, positions (initial position + positions relative to the origin of the frame) will be given in the form of [x, y, z], units: ['m', 'm', 'm'].
'ref_vel' True velocity w.r.t the navigation/reference frame expressed in the NED frame, units: ['m/s', 'm/s', 'm/s'].
'ref_att_euler' True attitude (Euler angles, ZYX rotation sequency), units: ['rad', 'rad', 'rad']
'ref_att_quat' True attitude (quaternions)
'ref_gyro' True angular velocity in the body frame, units: ['rad/s', 'rad/s', 'rad/s']
'ref_accel' True acceleration in the body frame, units: ['m/s^2', 'm/s^2', 'm/s^2']
'ref_mag' True geomagnetic field in the body frame, units: ['uT', 'uT', 'uT'] (only available when axis=9 in IMU object)
'ref_gps' True GPS position/velocity, ['rad', 'rad', 'm', 'm/s', 'm/s', 'm/s'] for NED (LLA), ['m', 'm', 'm', 'm/s', 'm/s', 'm/s'] for virtual inertial frame (xyz) (only available when gps=True in IMU object)
'gyro' Gyroscope measurements, 'ref_gyro' with errors
'accel' Accelerometer measurements, 'ref_accel' with errors
'mag' Magnetometer measurements, 'ref_mag' with errors
'gps' GPS measurements, 'ref_gps' with errors
'ad_gyro' Allan std of gyro, units: ['rad/s', 'rad/s', 'rad/s']
'ad_accel' Allan std of accel, units: ['m/s2', 'm/s2', 'm/s2']
'pos' Simulation position from algo, units: ['rad', 'rad', 'm'] for NED (LLA), ['m', 'm', 'm'] for virtual inertial frame (xyz).
'vel' Simulation velocity from algo, units: ['m/s', 'm/s', 'm/s']
'att_euler' Simulation attitude (Euler, ZYX) from algo, units: ['rad', 'rad', 'rad']
'att_quat' Simulation attitude (quaternion) from algo
'wb' Gyroscope bias estimation, units: ['rad/s', 'rad/s', 'rad/s']
'ab' Accelerometer bias estimation, units: ['m/s^2', 'm/s^2', 'm/s^2']
'gyro_cal' Calibrated gyro output, units: ['rad/s', 'rad/s', 'rad/s']
'accel_cal' Calibrated acceleromter output, units: ['m/s^2', 'm/s^2', 'm/s^2']
'mag_cal' Calibrated magnetometer output, units: ['uT', 'uT', 'uT']
'soft_iron' 3x3 soft iron calibration matrix
'hard_iron' Hard iron calibration, units: ['uT', 'uT', 'uT']
Returns:
"""
sim = ins_sim.Sim(
[fs, fs_gps, fs_mag],
motion_def_path + motion_path_filename,
ref_frame=1,
imu=imu,
# vehicle maneuver capability
# [max accel, max angular accel, max angular rate]
mode=None,
env=None,
algorithm=algorithm)
return 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 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'
})
def test_gen_data_from_files():
'''
test data generation from files.
'''
#### start simulation
sim = ins_sim.Sim(
[fs, fs_gps, fs_mag],
'e://Projects//gnss-ins-sim//demo_saved_data//2018-05-16-14-23-34//',
ref_frame=1,
imu=None,
mode=None,
env=None,
algorithm=None)
sim.run()
# save simulation data to files
sim.results()
# plot data
sim.plot(['ref_att_euler', 'att_euler'], opt={'att_euler': 'error'})
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_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 test_gen_data_from_files():
'''
test only path generation in Sim.
'''
#### choose a built-in IMU model, typical for IMU381
imu = None
#### start simulation
sim = ins_sim.Sim(
[fs, fs_gps, fs_mag],
'e://Projects//gnss-ins-sim//demo_saved_data//2018-05-16-14-23-34//',
ref_frame=1,
imu=imu,
mode=None,
env=None,
algorithm=None)
sim.run()
# save simulation data to files
sim.results()
# plot data, 3d plot of reference positoin, 2d plots of gyro and accel
sim.plot(['ref_att_euler', 'att_euler'], 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_gen_data_from_files(data_dir):
'''
test data generation from files.
'''
#### start simulation
#### Algorithm
# Free integration in a virtual inertial frame
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
algo = free_integration.FreeIntegration(ini_pos_vel_att)
#### start simulation
sim = ins_sim.Sim([fs, 0.0, 0.0],
data_dir,
ref_frame=0,
imu=None,
mode=None,
env=None,
algorithm=None)
# 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)
sim.plot(['att_euler'])
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 = ins_sim.Sim([fs, fs_gps, fs_mag],
motion_def_path + "//my_test.csv",
ref_frame=0,
imu=imu,
mode=None,
env=None,
algorithm=None)
sim.run(1)
# save simulation data to files
sim.results('')
# plot data, 3d plot of reference positoin, 2d plots of gyro and accel
sim.plot(['ref_pos', 'gyro', "ref_vel"], opt={'ref_pos': '3d'})
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_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_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 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,
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)