def run(self, set_of_input):
'''
main procedure of the algorithm
Args:
set_of_input is a tuple or list consistent with self.input
'''
self.run_times += 1
# get input
if set_of_input[0] == 0:
self.ref_frame = 0
self.dt = 1.0 / set_of_input[1]
gyro = set_of_input[2]
accel = set_of_input[3]
n = accel.shape[0]
# Free IMU integration
self.att = np.zeros((n, 3))
self.pos = np.zeros((n, 3))
self.vel = np.zeros((n, 3)) # NED vel
self.vel_b = np.zeros((n, 3)) # body vel
c_bn = np.eye((3))
if self.ref_frame == 1:
# which set of initial states to use
idx = self.run_times - 1
if self.run_times > self.set_of_inis:
idx = 0
# Earth gravity
if self.gravity is None:
earth_param = geoparams.geo_param(self.r0) # geo parameters
g_n = np.array([0, 0, earth_param[2]])
else:
g_n = np.array([0, 0, self.gravity[idx]])
for i in range(n):
#### initialize
if i == 0:
self.att[i, :] = self.att0[:, idx]
self.pos[i, :] = geoparams.lla2ecef(self.r0[:, idx])
self.vel_b[i, :] = self.v0[:, idx]
c_bn = attitude.euler2dcm(self.att[i, :])
self.vel[i, :] = c_bn.T.dot(self.vel_b[i, :])
continue
#### propagate Euler angles
self.att[i, :] = attitude.euler_update_zyx(self.att[i-1, :], gyro[i-1, :], self.dt)
#### propagate velocity in the navigation frame
# accel_n = c_nb.dot(accel[i-1, :])
# self.vel[i, :] = self.vel[i-1, :] + (accel_n + g_n) * self.dt
#### propagate velocity in the body frame
# c_bn here is from last loop (i-1), and used to project gravity
self.vel_b[i, :] = self.vel_b[i-1, :] +\
(accel[i-1, :] + c_bn.dot(g_n)) * self.dt -\
attitude.cross3(gyro[i-1, :], self.vel_b[i-1, :]) * self.dt
# c_bn (i)
c_bn = attitude.euler2dcm(self.att[i, :])
self.vel[i, :] = c_bn.T.dot(self.vel_b[i, :]) # velocity in navigation frame
self.pos[i, :] = self.pos[i-1, :] + self.vel[i-1, :] * self.dt
else:
# which set of initial states to use
idx = self.run_times - 1
if self.run_times > self.set_of_inis:
idx = 0
w_en_n = np.zeros(3)
w_ie_n = np.zeros(3)
for i in range(n):
#### initialize
if i == 0:
self.att[i, :] = self.att0[:, idx]
self.pos[i, :] = self.r0[:, idx]
self.vel_b[i, :] = self.v0[:, idx]
c_bn = attitude.euler2dcm(self.att[i, :])
self.vel[i, :] = c_bn.T.dot(self.vel_b[i, :])
continue
#### geo parameters
earth_param = geoparams.geo_param(self.pos[i-1, :])
rm = earth_param[0]
rn = earth_param[1]
g = earth_param[2]
sl = earth_param[3]
cl = earth_param[4]
w_ie = earth_param[5]
rm_effective = rm + self.pos[i-1, 2]
rn_effective = rn + self.pos[i-1, 2]
if self.gravity is None:
g_n = np.array([0, 0, g])
else:
g_n = np.array([0, 0, self.gravity[idx]])
w_en_n[0] = self.vel[i-1, 1] / rn_effective # wN
w_en_n[1] = -self.vel[i-1, 0] / rm_effective # wE
w_en_n[2] = -self.vel[i-1, 1] * sl /cl / rn_effective # wD
if self.earth_rot:
w_ie_n[0] = w_ie * cl
w_ie_n[2] = -w_ie * sl
#### propagate euler angles
w_nb_b = gyro[i-1, :] - c_bn.dot(w_en_n + w_ie_n)
self.att[i, :] = attitude.euler_update_zyx(self.att[i-1, :], w_nb_b, self.dt)
#### propagate velocity
# vel_dot_b = accel[i-1, :] + c_bn.T.dot(g_n) -\
# attitude.cross3(c_bn.dot(w_ie_n)+gyro[i-1,:], self.vel_b[i-1,:])
# self.vel_b[i,:] = self.vel_b[i-1,:] + vel_dot_b*self.dt
vel_dot_n = c_bn.T.dot(accel[i-1, :]) + g_n -\
attitude.cross3(2*w_ie_n + w_en_n, self.vel[i-1, :])
self.vel[i, :] = self.vel[i-1, :] + vel_dot_n * self.dt
#### propagate position
lat_dot = self.vel[i-1, 0] / rm_effective
lon_dot = self.vel[i-1, 1] / rn_effective / cl
alt_dot = -self.vel[i-1, 2]
self.pos[i, 0] = self.pos[i-1, 0] + lat_dot * self.dt
self.pos[i, 1] = self.pos[i-1, 1] + lon_dot * self.dt
self.pos[i, 2] = self.pos[i-1, 2] + alt_dot * self.dt
#### output
c_bn = attitude.euler2dcm(self.att[i, :])
self.vel_b[i, :] = c_bn.dot(self.vel[i, :])
# results
self.results = [self.att, self.pos, self.vel_b]
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(self, set_of_input):
'''
main procedure of the algorithm
Args:
set_of_input is a tuple or list consistent with self.input
'''
self.run_times += 1
# get input
if set_of_input[0] == 0:
self.ref_frame = 0
self.dt = 1.0 / set_of_input[1]
gyro = set_of_input[2]
odo = set_of_input[3]
n = gyro.shape[0]
# Free IMU integration
self.att = np.zeros((n, 3))
self.pos = np.zeros((n, 3))
self.vel = np.zeros((n, 3)) # NED vel
self.vel_b = np.zeros((n, 3)) # body vel
c_bn = np.eye((3))
if self.ref_frame == 1:
# which set of initial states to use
idx = self.run_times - 1
if self.run_times > self.set_of_inis:
idx = 0
# free integration
for i in range(n):
#### initialize
if i == 0:
self.att[i, :] = self.att0[:, idx]
self.pos[i, :] = geoparams.lla2ecef(self.r0[:, idx])
self.vel_b[i, :] = self.v0[:, idx]
c_bn = attitude.euler2dcm(self.att[i, :])
self.vel[i, :] = c_bn.T.dot(self.vel_b[i, :])
continue
#### propagate Euler angles
self.att[i, :] = attitude.euler_update_zyx(
self.att[i - 1, :], gyro[i - 1, :], self.dt)
#### velocity is from odometer
self.vel_b[i,
0] = odo[i -
1] # body axis is the direction of heading
self.vel_b[i, 1] = 0.0
self.vel_b[i, 2] = 0.0
# c_bn (i)
c_bn = attitude.euler2dcm(self.att[i, :])
self.vel[i, :] = c_bn.T.dot(
self.vel_b[i, :]) # velocity in navigation frame
self.pos[i, :] = self.pos[i -
1, :] + self.vel[i - 1, :] * self.dt
else:
# which set of initial states to use
idx = self.run_times - 1
if self.run_times > self.set_of_inis:
idx = 0
w_en_n = np.zeros(3)
w_ie_n = np.zeros(3)
for i in range(n):
#### initialize
if i == 0:
self.att[i, :] = self.att0[:, idx]
self.pos[i, :] = self.r0[:, idx]
self.vel_b[i, :] = self.v0[:, idx]
c_bn = attitude.euler2dcm(self.att[i, :])
self.vel[i, :] = c_bn.T.dot(self.vel_b[i, :])
continue
#### geo parameters
earth_param = geoparams.geo_param(self.pos[i - 1, :])
rm = earth_param[0]
rn = earth_param[1]
sl = earth_param[3]
cl = earth_param[4]
w_ie = earth_param[5]
rm_effective = rm + self.pos[i - 1, 2]
rn_effective = rn + self.pos[i - 1, 2]
w_en_n[0] = self.vel[i - 1, 1] / rn_effective # wN
w_en_n[1] = -self.vel[i - 1, 0] / rm_effective # wE
w_en_n[2] = -self.vel[i - 1, 1] * sl / cl / rn_effective # wD
if self.earth_rot:
w_ie_n[0] = w_ie * cl
w_ie_n[2] = -w_ie * sl
#### propagate euler angles
w_nb_b = gyro[i - 1, :] - c_bn.dot(w_en_n + w_ie_n)
self.att[i, :] = attitude.euler_update_zyx(
self.att[i - 1, :], w_nb_b, self.dt)
#### velocity is from odometer
self.vel_b[i,
0] = odo[i -
1] # body axis is the direction of heading
self.vel_b[i, 1] = 0.0
self.vel_b[i, 2] = 0.0
# c_bn (i)
c_bn = attitude.euler2dcm(self.att[i, :])
self.vel[i, :] = c_bn.T.dot(
self.vel_b[i, :]) # velocity in navigation frame
#### propagate position
lat_dot = self.vel[i - 1, 0] / rm_effective
lon_dot = self.vel[i - 1, 1] / rn_effective / cl
alt_dot = -self.vel[i - 1, 2]
self.pos[i, 0] = self.pos[i - 1, 0] + lat_dot * self.dt
self.pos[i, 1] = self.pos[i - 1, 1] + lon_dot * self.dt
self.pos[i, 2] = self.pos[i - 1, 2] + alt_dot * self.dt
#### output
c_bn = attitude.euler2dcm(self.att[i, :])
self.vel_b[i, :] = c_bn.dot(self.vel[i, :])
# results
self.results = [self.att, self.pos, self.vel_b]
def run(self, set_of_input):
'''
main procedure of the algorithm
Args:
set_of_input is a tuple or list consistent with self.input
'''
self.run_times += 1
# get input
if set_of_input[0] == 0:
self.ref_frame = 0
self.dt = 1.0 / set_of_input[1]
gyro = set_of_input[2]
accel = set_of_input[3]
gps = set_of_input[4]
n = accel.shape[0]
# Free IMU and odometer integration
self.att = np.zeros((n, 3))
self.pos = np.zeros((n, 3))
self.vel = np.zeros((n, 3)) # NED vel
self.vel_b = np.zeros((n, 3)) # body vel
c_bn = np.eye((3))
if self.ref_frame == 1:
# which set of initial states to use
idx = self.run_times - 1
if self.run_times > self.set_of_inis:
idx = 0
# Earth gravity
if self.gravity is None:
earth_param = geoparams.geo_param(self.r0) # geo parameters
g_n = np.array([0, 0, earth_param[2]]) # r0[LLA]处的g_n
else:
g_n = np.array([0, 0, self.gravity[idx]])
for i in range(n):
#### initialize
if i == 0:
# 实际应用中，取GPS信号丢失前，系统的att, pos, vel作为初始状态，开始IMU+odometer组合导航的递推。
self.att[i, :] = self.att0[:, idx]
self.pos[i, :] = geoparams.lla2ecef(self.r0[:, idx])
self.vel_b[i, :] = self.v0[:, idx]
c_bn = attitude.euler2dcm(
self.att[i, :]) #c_bn: body frame 到 nav frame的旋转矩阵。
self.vel[i, :] = c_bn.T.dot(self.vel_b[i, :])
continue
#### propagate Euler angles
self.att[i, :] = attitude.euler_update_zyx(
self.att[i - 1, :], gyro[i - 1, :], self.dt)
# self.att[i, :] = np.zeros((1, 3))
#### cal vel_b
gps_vel_N = gps[i - 1][3]
gps_vel_E = gps[i - 1][4]
# gps_vel_D = gps[i-1][5]
# self.vel_b[i][0] = math.sqrt(pow(gps_vel_N, 2) + pow(gps_vel_E, 2) + pow(gps_vel_D, 2))
self.vel_b[i][0] = math.sqrt(
pow(gps_vel_N, 2) + pow(gps_vel_E, 2))
self.vel_b[i][1] = 0
self.vel_b[i][2] = 0
# c_bn (i)
c_bn = attitude.euler2dcm(self.att[i, :])
self.vel[i, :] = c_bn.T.dot(
self.vel_b[i, :]) # velocity in navigation frame
self.pos[i, :] = self.pos[i -
1, :] + self.vel[i - 1, :] * self.dt
else:
pass
# results
self.results = [self.att, self.pos, self.vel_b]