Пример #1
0
def test_body2hor():

    # Test with a pitch rotation
    vector_body = np.array([1, 1, 1])
    theta, phi, psi = np.deg2rad(45), 0, 0

    vector_hor = body2hor(vector_body, theta, phi, psi)

    vector_hor_expected = np.array([2 * 0.70710678118654757, 1, 0])

    assert_array_almost_equal(vector_hor, vector_hor_expected)

    # Test with a roll rotation
    vector_body = np.array([1, 1, 1])
    theta, phi, psi = 0, np.deg2rad(45), 0

    vector_hor = body2hor(vector_body, theta, phi, psi)

    vector_hor_expected = np.array([1, 0, 2 * 0.70710678118654757])

    assert_array_almost_equal(vector_hor, vector_hor_expected)

    # Test with a yaw rotation
    vector_body = np.array([1, 1, 1])
    theta, phi, psi = 0, 0, np.deg2rad(45)

    vector_hor = body2hor(vector_body, theta, phi, psi)

    vector_hor_expected = np.array([0, 2 * 0.70710678118654757, 1])

    assert_array_almost_equal(vector_hor, vector_hor_expected)
Пример #2
0
    def propagate(self, aircraft, dt=0.01):
        """Propagate the state vector and update the rest of variables.

        Parameters
        ----------
        aircraft : Aircraft
            Aircraft model for simulation is used to get forces.
        """
        self.dt = dt
        self._propagate_state_vector(aircraft, dt)

        # TODO: update the rest of variables.
        self.vel_body = self.state_vector[0:3]
        self.vel_ang = self.state_vector[3:6]
        self.euler_angles[0] = self.state_vector[8]  # psi
        self.euler_angles[1] = self.state_vector[6]  # theta
        self.euler_angles[2] = self.state_vector[7]  # phi
        self.coord_earth = self.state_vector[9:12]

        # Set psi between 0 and 2*pi
        # FIXME: check the conversion to keep angle between 0 and 2pi again
        self.euler_angles[0] = np.arctan2(np.sin(self.psi), np.cos(
            self.psi)) % (2*np.pi)
        self.euler_angles[2] = np.arctan2(np.sin(self.phi), np.cos(self.phi))

        self.vel_NED = body2hor(self.vel_body, theta=self.theta, phi=self.phi,
                                psi=self.psi)
Пример #3
0
    def propagate(self, aircraft, dt=0.01):
        """Propagate the state vector and update the rest of variables.

        Parameters
        ----------
        environment
        """
        self.dt = dt
        self._propagate_state_vector(aircraft, dt)

        # TODO: update the rest of variables.
        self.vel_body = self.state_vector[0:3]
        self.vel_ang = self.state_vector[3:6]
        self.euler_angles[0] = self.state_vector[8]  # psi
        self.euler_angles[1] = self.state_vector[6]  # theta
        self.euler_angles[2] = self.state_vector[7]  # phi
        self.coord_earth = self.state_vector[9:12]

        # Set psi between 0 and 2*pi
        # FIXME: check the conversion to keep angle between 0 and 2pi again
        self.euler_angles[0] = np.arctan2(np.sin(self.psi), np.cos(
            self.psi)) % (2*np.pi)
        self.euler_angles[2] = np.arctan2(np.sin(self.phi), np.cos(self.phi))

        self.vel_NED = body2hor(self.vel_body, theta=self.theta, phi=self.phi,
                                psi=self.psi)
Пример #4
0
    def set_initial_state_vector(self):
        """
        Set the initial values of the required variables
        """

        self.vel_NED = body2hor(self.vel_body, theta=self.theta,
                                phi=self.phi, psi=self.psi)
        self.state_vector = np.array([
            self.u, self.v, self.w,
            self.p, self.q, self.r,
            self.theta, self.phi, self.psi,
            self.x_earth, self.y_earth, self.z_earth
        ])

        self._ode_lamceq.set_initial_value(y=self.state_vector[0:6])
        self._ode_kaqeq.set_initial_value(y=self.state_vector[6:9])
        self._ode_kleq.set_initial_value(y=self.state_vector[9:12])
Пример #5
0
    def set_initial_state_vector(self):
        """
        Set the initial values of the state vector for system integration
        once the values for the involved variables have been assigned or the
        system has been trimmed.
        """

        self.vel_NED = body2hor(self.vel_body, theta=self.theta,
                                phi=self.phi, psi=self.psi)
        self.state_vector = np.array([
            self.u, self.v, self.w,
            self.p, self.q, self.r,
            self.theta, self.phi, self.psi,
            self.x_earth, self.y_earth, self.z_earth
        ])

        self._ode_lamceq.set_initial_value(y=self.state_vector[0:6])
        self._ode_kaqeq.set_initial_value(y=self.state_vector[6:9])
        self._ode_kleq.set_initial_value(y=self.state_vector[9:12])
Пример #6
0
    def set_initial_state_vector(self):
        """
        Set the initial values of the state vector for system integration
        once the values for the involved variables have been assigned or the
        system has been trimmed.
        """

        self.vel_NED = body2hor(self.vel_body, theta=self.theta,
                                phi=self.phi, psi=self.psi)
        self.state_vector = np.array([
            self.u, self.v, self.w,
            self.p, self.q, self.r,
            self.theta, self.phi, self.psi,
            self.x_earth, self.y_earth, self.z_earth
        ])

        self._ode_lamceq.set_initial_value(y=self.state_vector[0:6])
        self._ode_kaqeq.set_initial_value(y=self.state_vector[6:9])
        self._ode_kleq.set_initial_value(y=self.state_vector[9:12])
Пример #7
0
 def update(self, value, attitude):
     self._vel_body[:] = value
     self._vel_NED = body2hor(value, attitude.theta, attitude.phi,
                              attitude.psi)  # m/s
Пример #8
0
 def update(self, coords, attitude):
     self._accel_body[:] = coords
     self._accel_NED = body2hor(coords, attitude.theta, attitude.phi,
                                attitude.psi)
Пример #9
0
 def update(self, value, attitude):
     self._vel_body[:] = value
     self._vel_NED = body2hor(value,
                              attitude.theta,
                              attitude.phi,
                              attitude.psi)  # m/s
Пример #10
0
 def update(self, coords, attitude):
     self._accel_body[:] = coords
     self._accel_NED = body2hor(coords,
                                attitude.theta,
                                attitude.phi,
                                attitude.psi)