Patent ID: 8645063

Claim:
A method of determining a quaternion for relating a vehicle coordinate frame to an inertial coordinate frame, the method comprising: receiving, by a processor, periodic indications of angular rates of the vehicle from a plurality of angular rate sensors associated with the vehicle during a first time period; receiving, by the processor, periodic indications of acceleration that the vehicle is experiencing during the first time period when acceleration that the vehicle is subject to is due to gravity; receiving, by the processor, periodic indications of an inertial reference vector during the first time period, the inertial reference vector indications being in a vehicle coordinate frame; transforming, using the processor, the received acceleration and inertial reference vector indications from the vehicle coordinate frame to an inertial navigation coordinate frame based on a current quaternion to provide transformed acceleration indications and transformed reference vector indications; determining, using the processor, an angular error vector in the inertial navigation coordinate frame by comparing the transformed acceleration indications to a predefined gravity vector in the inertial navigation coordinate frame and by comparing the transformed inertial reference vector indications to a predefined reference vector in the inertial navigation coordinate frame; determining, using the processor, an angular rate correction vector, to counter the determined angular error vector in the inertial navigation coordinate frame, based on the determined angular error vector and a first gain factor; transforming, using the processor, the angular rate correction vector from the inertial navigation coordinate frame to the vehicle coordinate frame, to produce vehicle angular rate corrections, based on the current quaternion; determining, using the processor, corrective vehicle rates based on the received angular rate indications and the vehicle angular rate corrections; and determining, using the processor, an updated quaternion by integrating a rotation of the current quaternion over a second time period based on the determined corrective vehicle rates, the second time period being a portion of the first time period.