4.2.1 Attitude Estimation

The first goal is to find the tank coordinate frame axes in the vehicle frame. The tank z-axis is calculated in the vehicle frame by forming a unit vector in the direction of the acceleration sensed by the IMU, .

.


This is assumed to point down (i.e. aligned with the gravity vector), although noise, bias, and accelerations due to vehicle motion cause possibly significant perturbations.
To find the tank y-axis in vehicle coordinates, the Earth's magnetic field vector , sensed by the ESU, is crossed with the "down" vector ( ) to form a vector that points East.


(4.13)

The magnetic field vector lies in the plane between the North and "down" vectors.
Finally, the tank x-axis in vehicle coordinates is determined by using the Right Hand Rule.


(4.14)

Once the estimates of the tank frame axes have been calculated in the vehicle frame, a direction cosine matrix from vehicle to tank coordinates may be formed [4].


(4.15)

This is then converted into a quaternion. It is desirable to use equations that produce good numerical results across all possible values [7]. Expressed in terms of the quaternion, the direction cosine matrix is given by


,

(4.16)

where is defined in (4.11). Multiplying this out we get


.

(4.17)

The magnitude of each of the quaternion elements may be determined (4.18) - (4.21) from the diagonal elements of (4.17).


(4.18)


(4.19)


(4.20)


(4.21)

From this point there four possible solutions for the quaternion (4.22) - (4.25), each producing the same result, and each requiring only one of the quaternion element magnitudes from (4.18) - (4.21) along with all of the off-diagonal elements of (4.17).


(4.22)


(4.23)


(4.24)


(4.25)

Even those each of these equations theoretically produces the same quaternion, numerically it is more accurate to divide by numbers with larger magnitudes. Since each of the equations (4.22) - (4.25) divides by only one of the calculated quaternion element magnitudes, the quaternion estimation algorithm chooses the equation that uses the quaternion element with the largest magnitude.
This formulation of the attitude estimate is only as accurate as the data supplied to it. The algorithm is based on the premise that it is supplied with one vector pointing straight down, and another vector in the North-down plane such that the first vector crossed with the second points directly East. While it will be shown in Section 4.3 that this premise is not completely true, from an engineering standpoint it is often close enough to produce the good results.