def __init__(self, ini_pos_vel_att):
'''
Args:
ini_pos_vel_att: 9x1 initial position, velocity and attitude.
3x1 position in the form of LLA, units: [rad, rad, m];
3x1 velocity in the body frame, units: m/s;
3x1 Euler anels [yaw, pitch, roll], rotation sequency is ZYX, rad.
'''
# algorithm description
self.input = ['fs', 'gyro', 'accel']
self.output = ['att_euler', 'pos', 'vel']
self.batch = True
self.results = None
# algorithm vars
self.dt = 1.0
self.att = None
self.pos = None
self.vel = None
self.vel_b = None
# ini state
self.r0 = geoparams.lla2xyz(ini_pos_vel_att[0:3])
self.v0 = ini_pos_vel_att[3:6]
self.att0 = ini_pos_vel_att[6:9]
# Earth gravity
earth_param = geoparams.geo_param(
ini_pos_vel_att[0:3]) # geo parameters
self.g_n = np.array([0, 0, earth_param[2]])
def get_init_pose(stamp, motion_def_file):
"""
Get init pose from motion def file as ROS nav_msgs::Odometry
"""
# parse:
(lat, lon, alt, vx, vy, vz, yaw, pitch,
roll) = np.genfromtxt(motion_def_file,
delimiter=',',
skip_header=True,
max_rows=1)
(rm, rn, g, sl, cl, w_ie) = geoparams.geo_param(np.array([lat, lon, alt]))
rospy.logwarn("""
Earth Params:
\tRm: {}
\tRn: {}
\tG: {}
\tsin(Lat): {}
\tcos(Lat): {}
\tw_ie: {}
""".format(rm, rn, g, sl, cl, w_ie))
# init:
init_pose_msg = Odometry()
# set header:
init_pose_msg.header.stamp = stamp
init_pose_msg.header.frame_id = '/map'
init_pose_msg.child_frame_id = '/imu_link'
# a. navigation frame, position:
"""
(
init_pose_msg.pose.pose.position.x,
init_pose_msg.pose.pose.position.y,
init_pose_msg.pose.pose.position.z
) = geoparams.lla2ecef(
[
lat*D2R, lon*D2R, alt
]
)
"""
(init_pose_msg.pose.pose.position.x, init_pose_msg.pose.pose.position.y,
init_pose_msg.pose.pose.position.z) = [0.0, 0.0, 0.0]
# b. navigation frame, orientation
(init_pose_msg.pose.pose.orientation.w,
init_pose_msg.pose.pose.orientation.x,
init_pose_msg.pose.pose.orientation.y,
init_pose_msg.pose.pose.orientation.z) = attitude.euler2quat(
np.asarray([yaw * D2R, pitch * D2R, roll * D2R]), rot_seq='zyx')
# c. body frame, velocity:
(init_pose_msg.twist.twist.linear.x, init_pose_msg.twist.twist.linear.y,
init_pose_msg.twist.twist.linear.z) = (vx, vy, vz)
# finally:
return init_pose_msg
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]