Matrices

Idea from Van Sickle's Matrix page but re-thought to make sure the convention matches.

Our IMU filter tracks a quaternion. This quaternion is interpreted as one which transforms a vector from body to inertial coordinates. For instance, if the quaternion is <e> and we have a measurement of the direction from the vehicle to some way point in inertial coordinates <Ai>. To convert the vector to body coordinates <Ab>, we use the quaternion from the state vector like this:

<Ab>=<e~><Ai><e>

Or, you could use a matrix

<Ab>=[E]<Ai>.

To convert back the other way, we do either
<Ai>=<e><Ab><e~>

or


<Ai>=[E']<Ab>


To form matrix [E], we follow the derivation in Aircraft.pdf. The vector part of quaternion <?> we will call <?.v> and it will naturally be a column vector. The row vector form will be the transpose <?.v'> . The scalar part we will call ?.w . All of these operations are conventional matrix multiplication, addition, and scaling a matrix. The cross-product matrix [?.c] is [0,-?.z,?.y;?.z,0,-?.x;-?.y,?.x,0] and is the matrix such that <?> cross <@> is always equal to [?.c]<@> for any vector <@>.

[E]=2*<e.v>*<e.v'>+(e.w^2-<e.v'>*<e.v>)*[1]-2*e.w*[e.c]

Anyway, this is all just convention stuff. If we know the quaternion, we can find the matrix.

Now the best way I have heard to visualize a matrix is like this. The axes of the reference frame point east (x), north (y), and up (z). From the point of view of the vehicle, in the body reference frame, east is in the direction indicated by the first column, north by the second column, and up by the third. Conversely, in the inertial frame, the first row is the body x axis, the second row is y, and the third row is z.

From the calibration experiments, we know the axes of the wooden box in the gyroscope sensor frame. Since we were rotating the box around an axis parallel to one of the box axes (that's what we used the bubble level to verify) we know the rotation axis, and therefore each box axis, in the sensor body frame - it is directly the vector reported by the gyroscope readout. For instance, when we rotate around +z, we get the lion's share of rotation in the z sensor, but also a bit in x and y. If you put those measurements together into a vector, that vector points along the rotation axis, and its length is the speed of rotation.

So, put this together with what we said before. The reference Z axis is measured in the body frame, so that is the third column of the matrix from the gyro sensor axis to the body axis. Likewise what we measured around X and Y are the first and second columns respectively. So from the calibration measurements, we can measure the orientation of the gyro sensor relative to the box. We can easily do the same thing with the acceleration measurements. The box vectors, in sensor space, are the colums of the matrix which transforms from box to sensor. So, the inverse, not necessarily transpose because the sensors might not be orthogonal, transforms from sensor to box.

So the calibration process is to stimulate each box axis, by spinning it for the gyro or setting it facing up for the accelerometer. Suck the data into a spreadsheet, and average and normalize things for each axis, then build the matrix. In my case, in the spreadsheet, I have the reference axes on rows, so suck those into matlab and transpose it, then take the inverse and use those for coefficients in the calibration onboard.