def test_free_integration():
'''
test Sim
'''
#### do not need an IMU model
imu = None
#### Algorithm
from demo_algorithms import free_integration
ini_pos_vel_att = np.genfromtxt(log_dir+"ini.txt", delimiter=',')
ini_pos_vel_att[0:2] *= attitude.D2R # For Lat and Lon, deg to rad
ini_pos_vel_att[6:9] *= attitude.D2R # Attitude from deg to rad
if not using_external_g:
ini_pos_vel_att = ini_pos_vel_att[0:9]
# create the algorithm object
algo = free_integration.FreeIntegration(ini_pos_vel_att, earth_rot=False)
from demo_algorithms import inclinometer_acc
algo2 = inclinometer_acc.TiltAcc()
#### start simulation
sim = ins_sim.Sim([fs, 0.0, 0.0],
log_dir,
ref_frame=0,
imu=imu,
mode=None,
env=None,
algorithm=[algo, algo2])
# run the simulation
sim.run(1)
# generate simulation results, summary
sim.results('', end_point=True, extra_opt='ned')
# plot
sim.plot(['pos', 'vel', 'att_euler', 'accel', 'gyro'],
opt={'pos':'error', 'vel':'error', 'att_euler':'error'})
def test_free_integration():
'''
test Sim
'''
#### 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_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(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 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
# motion_path_filename = "\motion_def_linemerge.csv"
# motion_path_filename = "\motion_def-90deg_turn.csv"
# motion_path_filename = "\motion_def_linemerge.csv"
motion_path_filename = "\motion_def_70_mph_straight_20sec.csv"
# motion_path_filename = "\motion_def_70_mph_straight.csv"
# motion_path_filename = "//motion_def-long_drive.csv"
print(motion_def_path + motion_path_filename)
# Create IMU model with parameters and initial state vector
imu_model = imu_creation(arw_input=arw, bias_input=gyro_bias)
initial_state_vector = initial_state(motion_path_filename)
print(initial_state_vector)
# choose algorithm in library (here we use multiple)
algo1 = free_integration_odo.FreeIntegration(initial_state_vector)
algo2 = free_integration.FreeIntegration(initial_state_vector)
# Create the simulation object that will be simulated
sim_object_1 = create_sim_object(algo1, motion_path_filename)
# sim_object_2 = create_sim_object(algo2)
# run the simulation for a number of times
sim_object_1.run(5)
# sim_object_2.run(10)
# -----------------------------RESULTS SECTION--------------------------------------------
# generate simulation results, summary
# do not save data since the simulation runs for 1000 times and generates too many results
sim_object_1.results('', err_stats_start=-1, gen_kml=True)
# sim_object_2.results(err_stats_start=-1, gen_kml=True)
# sim_object_1.plot(['ref_pos'])
