def run(self, set_of_input):
'''
main procedure of the algorithm
Args:
set_of_input is a tuple or list consistent with self.input
'''
# get input
self.dt = 1.0 / set_of_input[0]
gyro = set_of_input[1]
accel = set_of_input[2]
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))
self.vel_b = np.zeros((n, 3))
c_bn = np.eye((3))
for i in range(n):
#### initialize
if i == 0:
self.att[i, :] = self.att0
self.pos[i, :] = self.r0
self.vel_b[i, :] = self.v0
c_bn = attitude.euler2dcm(self.att[i, :])
self.vel[i, :] = c_bn.T.dot(self.v0)
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 + self.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(self.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
# 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]
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 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]