Closed-form integrator for the quaternion (euler angle) kinematics equations
First Claim
1. 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.
1 Assignment
0 Petitions
Accused Products
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.
41 Citations
21 Claims
-
1. 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. - View Dependent Claims (2, 4, 5, 6, 7, 8, 9)
-
-
3. A method of integrating kinematics equations for updating a set of vehicle attitude angles of a vehicle using 3-dimensional angular velocities of said vehicle, comprising:
-
computing an integrating factor matrix from quantities corresponding to said 3-dimensional angular velocities; computing a total integrated angular rate from said quantities corresponding to 3-dimensional angular velocities; computing 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 updating said set of vehicle attitude angles using said state transition matrix.
-
-
10. Apparatus for use in integrating kinematics equations for updating a set of vehicle attitude angles of a vehicle using 3-dimensional angular velocities of said vehicle, said apparatus comprising:
-
a processor; a memory having executable instructions stored therein; and wherein said processor, in response to said instructions stored in said memory; computes an integrating factor matrix from quantities corresponding to said 3-dimensional angular velocities; computes a total integrated angular rate from said quantities corresponding to 3-dimensional angular velocities; computes a state transition matrix as a sum of (a) a first trigonometric function of said total integrated angular rate and (b) said integrating factor matrix multiplied by a second trigonometric function of said total integrated angular rate; and updates said set of vehicle attitude angles using said state transition matrix. - View Dependent Claims (11, 12, 13, 14, 15)
-
-
16. A kinematics equations integrator for updating a set of vehicle attitude angles of a vehicle using 3-dimensional angular velocities of said vehicle, 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. - View Dependent Claims (17, 18, 19, 20, 21)
-
Specification