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]