Example #1
0
    def test_position(self):
        """
        Test position integration pof inscomputer.

        We just integrate position, velocity in INS state replaced by ideal 
        one. Other variables in state vector is not changed coz they do not 
        influence on position integration.
        """

        self.skipTest('temp')
        test_pos = np.empty
        for t in self.time:
            rez = self.ins._integrate_position()

            self.ins._state[0:3] = rez
            self.ins._state[3:6] = self.pf.state(t)[3:6]

            try:
                test_pos = np.vstack((test_pos, rez))
            except ValueError:
                test_pos = rez


        # get ideal values for: latitude, longitude and height
        tru_pos = np.array([self.pf.position(t) for t in self.time])

        lgnd = ['phi', 'lam', 'h']
        plot_compare_states(self.time, test_pos, tru_pos, lgnd)
        plot_compare_states_diff(self.time, 6378137.0*test_pos, 6378137.0*tru_pos, lgnd)

        np.testing.assert_allclose(test_pos, tru_pos, rtol=1e-4, atol=1e-2 )
Example #2
0
    def test_velocity_pos(self):
        """
        Test velocity integration of inscomputer.

        Here we integrate only velocity and check acceleration so, position
        and orientation replaced by ideal values.
        """
        self.skipTest('temp')
        vel = np.empty
        acc = np.empty
        pos = np.empty
        quat = np.empty
        quati = np.empty
        test_state = np.empty

        for t in self.time:

#            print 't = ', t
            if t != self.time[0]:
                # replace INS variables with ideal one
#                q = dcm2quat(self.pf.dcmbn(t))
#                self.ns._state[9:] = q

                q = self.ins._integrate_orientation(self.pf.gyros(t))
                qi = self.pf.orientation_q(t)

                self.ins._state[9:] = qi
                rez, aned = self.ins._integrate_velocity(self.pf.accs(t))
                self.ins._state[3:6] = rez
                self.ins._state[6:9] = aned
                self.ins._state[0:3] = self.ins._integrate_position()
#                self.ns._state[9:] = q

                # save result in next vectors
                vel = np.vstack((vel, self.ins._state[3:6]))
                acc = np.vstack((acc, aned))
                pos = np.vstack((pos, self.ins._state[0:3]))
                quat = np.vstack((quat, q))
                quati = np.vstack((quati, qi))
                test_state =  np.vstack((test_state, self.ins._state[:]))

            else:
                vel = self.ins._state[3:6]
                acc = self.ins._state[6:9]
                pos = self.ins._state[0:3]
                quat = self.ins._state[9:]
                quati = self.ins._state[9:]
                test_state =  self.ins._state[:]


        lgnd = ['phi', 'lam', 'h', 'vn', 've', 'vd', 'an', 'ae', 'ad',
                'q0', 'q1', 'q2', 'q3']

        ref_state = np.array([self.pf.state(t) for t in self.time])
        print np.shape(ref_state[1:,:])
        print np.shape(test_state[1:,:])

        plot_compare_states(self.time[1:], test_state[1:,:], ref_state[1:,:], lgnd)
        plot_compare_states_diff(self.time[1:], test_state[1:,:], ref_state[1:,:], lgnd)
    def test_functions(self):
        ref_state = np.array([self.pf_reg.state(t) for t in self.time])
        test_state = np.array([self.pf_opt.state(t) for t in self.time])

        lgnd = ['phi', 'lam', 'h', 'vn', 've', 'vd', 'an', 'ae', 'ad',
                 'q0', 'q1', 'q2', 'q3']
        plot_compare_states(self.time, test_state, ref_state, lgnd)
        plot_compare_states_diff(self.time, test_state, ref_state, lgnd)

        np.testing.assert_almost_equal(test_state, ref_state, decimal=10)
    def test_accs(self):
        ref_accs = np.array([self.pf_reg.accs(t) for t in self.time])
        test_accs = np.array([self.pf_opt.accs(t) for t in self.time])

        ## uncomment lines if you to check the plot and diff between
        ## test and ref values
        lgnd = ['ax', 'ay', 'az']
        plot_compare_states(self.time, test_accs, ref_accs, lgnd)
        plot_compare_states_diff(self.time, test_accs, ref_accs, lgnd)

        np.testing.assert_almost_equal(test_accs, ref_accs, decimal=10)
Example #5
0
    def test_complex_system(self):
        """
        Test full functional system.
        """
        self.skipTest('temp')
        test_state = np.array([self.ins(self.pf.gyros(t), self.pf.accs(t))
                      for t in self.time])
        ref_state = np.array([self.pf.state(t) for t in self.time])


        lgnd = ['phi', 'lam', 'h', 'vn', 've', 'vd', 'an', 'ae', 'ad',
                'q0', 'q1', 'q2', 'q3']
        plot_compare_states(self.time, test_state, ref_state, lgnd)
        plot_compare_states_diff(self.time, test_state, ref_state, lgnd)
Example #6
0
    def test_velocity_acc(self):
        """
        Test velocity integration of inscomputer.

        Here we integrate only velocity and check acceleration so, position
        and orientation replaced by ideal values.
        """

        self.skipTest('temp')
        vel = np.empty
        acc = np.empty

        for t in self.time:

            if t != self.time[0]:
                # replace INS variables with ideal one
                q = self.pf.orientation_q(t)
                self.ins._state[9:] = q

                rez, aned = self.ins._integrate_velocity(self.pf.accs(t))
                self.ins._state[3:6] = rez
                self.ins._state[6:9] = aned
                self.ins._state[0:3] = self.pf.position(t)

                # save result in next vectors
                vel = np.vstack((vel, rez))
                acc = np.vstack((acc, aned))

            else:
                vel = self.ins._state[3:6]
                acc = self.ins._state[6:9]


        tru_v = np.array([self.pf.velocity_ned(t) for t in self.time])
        tru_acc = np.array([self.pf.acceleration_ned(t) for t in self.time])

        lgnd = ['vn', 've', 'vd']
        plot_compare_states(self.time, vel, tru_v, lgnd)
        plot_compare_states_diff(self.time, vel, tru_v, lgnd)

        lgnd = ['an', 'ae', 'ad']
        plot_compare_states(self.time, acc, tru_acc, lgnd)
        plot_compare_states_diff(self.time, acc, tru_acc, lgnd)

        np.testing.assert_allclose(vel, tru_v, rtol=0.001, atol=0.01 )
        np.testing.assert_allclose(acc, tru_acc, rtol=1e-4, atol=0.01)
Example #7
0
    def test_attitude(self):
        """
        Test position integration of inscomputer.
        """
#        self.skipTest('temp')
        q = self.ins._state[9:]
        test_quat = self.ins._state[9:]
        for t in self.time:

            if t != 0.:


                self.ins._state[0:3] = self.pf.position(t)
                self.ins._state[3:6] = self.pf.velocity_ned(t)
#                q = self.ins._integrate_orientation(self.pf.gyros_inc_angle(t, self.dt))
                q = self.ins._integrate_orientation(self.pf.gyros(t))
                self.ins._state[9:] = q
                test_quat = np.vstack((test_quat, q))

            else:
                q = self.ins._state[9:]


#        pitch = np.rad2deg(np.array([self.pf.theta(t) for t in self.time]))
#        roll = np.rad2deg(np.array([self.pf.gamma(t) for t in self.time]))
#        yaw = np.rad2deg(np.array([self.pf.psi(t) for t in self.time]))

        ref_quat = np.array([self.pf.orientation_q(t) for t in self.time])

        lgnd = ['q0', 'q1', 'q2', 'q3']



        plot_compare_states(self.time, test_quat[:-1], ref_quat, lgnd)
        plot_compare_states_diff(self.time, test_quat[:-1], ref_quat, lgnd)

        lgnd = ['roll', 'pitch', 'yaw']
        plot_compare_quat2euler(self.time, test_quat[:-1], ref_quat, lgnd)
Example #8
0
from openins.visualisation.plotter import plot_compare_states
from openins.visualisation.plotter import plot_compare_states_diff
from openins.trajectory.navtrajectory_opt import NavTrajectoryOpt

# init trajectory of body
pf = NavTrajectoryOpt()
# set sample time and time vector
dt = 0.005
time = np.arange(0., 100.,  dt)

# init INS Computer
ins = SimpleINSComputer()
ins.dt = dt

# set initial state of INS
init_state = pf.init_state()
ins.set_state(init_state)

# save ins state
ins_state = np.array([ins(pf.gyros(t), pf.accs(t))
                       for t in time])
# save trajectory
ref_state = np.array([pf.state(t) for t in time])

# plot each variable versus time
lgnd = ['phi', 'lam', 'h', 'vn', 've', 'vd', 'an', 'ae', 'ad',
        'q0', 'q1', 'q2', 'q3']

plot_compare_states(time, ins_state, ref_state, lgnd)
plot_compare_states_diff(time, ins_state, ref_state, lgnd)