US 6,061,611 A · Closed-form integrator for the quaternion (euler angle) kinematics equations

Abstract

The invention is embodied in a method of integrating kinematics equations for updating a set of vehicle attitude angles of a vehicle using 3-dimensional angular velocities of the vehicle, which includes computing an integrating factor matrix from quantities corresponding to the 3-dimensional angular velocities, computing a total integrated angular rate from the quantities corresponding to a 3-dimensional angular velocities, computing a state transition matrix as a sum of (a) a first complementary function of the total integrated angular rate and (b) the integrating factor matrix multiplied by a second complementary function of the total integrated angular rate, and updating the set of vehicle attitude angles using the state transition matrix. Preferably, the method further includes computing a quanternion vector from the quantities corresponding to the 3-dimensional angular velocities, in which case the updating of the set of vehicle attitude angles using the state transition matrix is carried out by (a) updating the quanternion vector by multiplying the quanternion vector by the state transition matrix to produce an updated quanternion vector and (b) computing an updated set of vehicle attitude angles from the updated quanternion vector. The first and second trigonometric functions are complementary, such as a sine and a cosine. The quantities corresponding to the 3-dimensional angular velocities include respective averages of the 3-dimensional angular velocities over plural time frames. The updating of the quanternion vector preserves the norm of the vector, whereby the updated set of vehicle attitude angles are virtually error-free.

Claims21

Dependent claims

In a vehicle such as a flight vehicle, a control system comprising:

acceleration sensors with a angular velocity processor for providing 3-D angular velocities;
a navigation computer for computing trajectory corrections from attitude angles of said vehicle;
a kinematics equations integrator for updating a set of vehicle attitude angles of a vehicle using said 3-dimensional angular velocities of said vehicle, said integrator being connected to said navigation computer and to said angular velocity processor and comprising:
an integrating factor module which computes an integrating factor matrix from quantities corresponding to said 3-dimensional angular velocities;
a total integrated angular rate module which computes a total integrated angular rate from said quantities corresponding to 3-dimensional angular velocities;
a state transition matrix module, connected to said integrating factor module and said total integrated angular rate module, which computes a state transition matrix as a sum of (a) a first complementary function of said total integrated angular rate and (b) said integrating factor matrix multiplied by a second complementary function of said total integrated angular rate;
and
an updating module which updates said set of vehicle attitude angles using said state transition matrix for output to said navigation computer.

5 more elements

Dependencies are inferred from the claim text.

Closed-form integrator for the quaternion (euler angle) kinematics equations

Life of this patent

Closed-form integrator for the quaternion (euler angle) kinematics equations

Life of this patent

Abstract

Claims21