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]
Beispiel #2
0
 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]
Beispiel #3
0
 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]
Beispiel #4
0
    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]