It's time to stop dithering around and actually do something: write up a Sensor Fusion module.

As discussed before, sensor fusion is using two different kinds of sensors to make a measurement. Perhaps both kinds can do it independently and we just want to cross-check. Perhaps neither can do it by itself but the combination can. Perhaps one sensor can do it in theory, but any slight perturbation will screw it up in practice.

Let's get down to concrete. My IMU has a three-axis rotation sensor, commonly called a gyro even though there are no spinning parts in it. It also contains a three-axis magnetic sensor, which I will call a compass. The compass by itself is great for absolute measurements, but by itself cannot determine the pointing of the IMU. To completely determine the orientation of an object from the outside like with a compass, you need two different reference vectors. I have a daydream about using something like an FM radio with three orthogonal loop antennas as another reference vector, but this is not practical. So, only one vector. You can tell that the IMU is pointing some part towards that vector, but it could be rolled at any angle around that vector.

The gyro by itself can in principle completely determine the orientation, if the initial orientation is known. However, because it integrates, if there is any tiny offset in the zero setting, the orientation will degrade at a linear rate, proportional to the zero offset. This is why I very carefully calibrated the gyros against temperature, but I still don't think it's enough.

However, the two together back up each other's weak points. The gyro is accurate in a relative sense, but has no absolute means to make sure it doesn't go wandering off. The compass is incomplete, but is nailed down to the external frame. Together, they can conquer the world! Or at least orient the IMU.

Skip the explanation of quaternion math. Go look up on the net for that. I may eventually write it myself, but today I am building.

State Vector:

The state vector is the four components of the quaternion <e> (equivalent to position vector), the three components of the body rate sensor measurement <ω> (equivalent to velocity).

<x>=<e.w,e.x,e.y,e.z,ω.x, ω.y,ω.z>

Physics function:

The physics function here is actually less physics than kinematics. You will notice no mention of moment of inertia or anything like that. Just how you integrate a body rate measurement into a quaternion.

A note on notation and convention first. A quaternion <e>, is shown as a vector because it is four related components. The quaternion conjugate is shown as <?~> for any quaternion <?>. This quaternion when used properly, transforms a point in the inertial reference frame into one in the body reference frame:

<v_ b>=<e~><v_i><e>

Conversely, we can transform a vector in the body frame to one in the inertial frame with:

<v_i>=<e><v_b><e~>

where these multiplications are conventional quaternion multiplications, and the scalar part of the pure vectors <v_?> are zero.

If you know the rotation rate of a body over time, you can integrate this to get the orientation over time, starting with some initial condition.

d<e>/dt=<e><ω>/2

where this multiplication is just as shown, not a vector transform, just a single quaternion multiplication. The vector <ω> is the body-frame rotation speed, measured in radians/sec. By components, we get:

F(<x>)=<F.ew,F.ex,F.ey,F.ez,0,0,0>

F.ew=de.w/dt=(-e.x ω.x-e.y ω.y -e.z ω.z)/2

F.ex=de.x/dt=(e.w ω.x-e.z ω.y +e.y ω.z)/2

F.ey=de.y/dt=(e.z ω.x+e.w ω.y -e.x ω.z)/2

F.ez=de.z/dt=(-e.y ω.x+e.x ω.y +e.w ω.z)/2

The physics matrix [Φ] is 7x7, but since the last three elements are zero, so are the last three rows of the matrix. Alpha reminded me that these are easy, it's just bookkeeping

Phi[0,0]=dF.ew/de.w =0

Phi[0,1]=dF.ew/de.x =-ω.x/2

Phi[0,2]=dF.ew/de.y =-ω.y/2

Phi[0,3]=dF.ew/de.z =-ω.z/2

Phi[0,4]=dF.ew/dω.x =-e.x/2

Phi[0,5]=dF.ew/dω.y =-e.y/2

Phi[0,6]=dF.ew/dω.z =-e.z/2

Phi[1,0]=dF.ex/de.w =+ω.x/2

Phi[1,1]=dF.ex/de.x =0

Phi[1,2]=dF.ex/de.y =+ω.z/2

Phi[1,3]=dF.ex/de.z =-ω.y/2

Phi[1,4]=dF.ex/dω.x =+e.w/2

Phi[1,5]=dF.ex/dω.y =-e.z/2

Phi[1,6]=dF.ex/dω.z =+e.y/2

Phi[2,0]=dF.ey/de.w =+ω.y/2

Phi[2,1]=dF.ey/de.x =-ω.z/2

Phi[2,2]=dF.ey/de.y =0

Phi[2,3]=dF.ey/de.z =+ω.x/2

Phi[2,4]=dF.ey/dω.x =+e.z/2

Phi[2,5]=dF.ey/dω.y =+e.w/2

Phi[2,6]=dF.ey/dω.z =-e.x/2

Phi[3,0]=dF.ez/de.w =+ω.z/2

Phi[3,1]=dF.ez/de.x =+ω.y/2

Phi[3,2]=dF.ez/de.y =-ω.x/2

Phi[3,3]=dF.ez/de.z =0

Phi[3,4]=dF.ez/dω.x =-e.y/2

Phi[3,5]=dF.ez/dω.y =+e.x/2

Phi[3,6]=dF.ez/dω.z =+e.w/2

The observation g(<x>) is as follows:

g(x)=<G.x,G.y,G.z,B_b.x,B_b.y,B_b.z> where G stands for

*gyro* reading and B stands for

*B-field* reading (the magnetic field is usually represented by <B> in most textbooks).

<G> is just the gyro reading transformed into radians per second, which presumably is already done, so we have

<G>=<ω>

Since the magnetic sensor is nailed to the body (we hope the parts haven't diverged yet) we measure <B_b> , the magnetic field in body coordinates. This is just the exterior magnetic field <B_i> transformed into body coordinates, so we use

<B_ b>=<e~><B_i><e>

When this is expressed in component form, we effectively transform this quaternion into a matrix and then matrix-multiply the external vector by this matrix:

B_b.x=(e_w^2+e.x^2-e.y^2-e.z^2)B_i.x+2(e.x e.y+e.w e.z)B_i.y+2(e.x e.z-e.w e.y)B_i.z

B_b.y=2(e.x e.y-e.w e.z)*B_i.x+(e.w^2-e.x^2+e.y^2-e.z^2)B_i.y+2(e.y e.z+e.w e.x)B_i.z

B_b.z=2(e.z e.x+e.w e.y)*B_i.x+2(e.y e.z-e.w e.x)B_i.y+(e.w^2-e.x^2-e.y^2+e.z^2)B_i.z

The observation matrix [H] will be six rows, one for each element of the observation, by 7 columns, one for each element of the state vector. First, the three rows with the gyro:

H[0,0]=dg.Gx/de.w=0

H[0,1]=dg.Gx/de.x=0

H[0,2]=dg.Gx/de.y=0

H[0,3]=dg.Gx/de.z=0

H[0,4]=dg.Gx/dω.x=1

H[0,5]=dg.Gx/dω.y=0

H[0,6]=dg.Gx/dω.z=0

H[1,0]=dg.Gy/de.w=0

H[1,1]=dg.Gy/de.x=0

H[1,2]=dg.Gy/de.y=0

H[1,3]=dg.Gy/de.z=0

H[1,4]=dg.Gy/dω.x=0

H[1,5]=dg.Gy/dω.y=1

H[1,6]=dg.Gy/dω.z=0

H[2,0]=dg.Gz/de.w=0

H[2,1]=dg.Gz/de.x=0

H[2,2]=dg.Gz/de.y=0

H[2,3]=dg.Gz/de.z=0

H[2,4]=dg.Gz/dω.x=0

H[2,5]=dg.Gz/dω.y=0

H[2,6]=dg.Gz/dω.z=1

Now the three elements with the compass. This will be a bit more complicated, but still manageable.

B_b.x=(e_w^2+e.x^2-e.y^2-e.z^2)B_i.x+2(e.x e.y+e.w e.z)B_i.y+2(e.x e.z-e.w e.y)B_i.z

B_b.y=2(e.x e.y-e.w e.z)*B_i.x+(e.w^2-e.x^2+e.y^2-e.z^2)B_i.y+2(e.y e.z+e.w e.x)B_i.z

B_b.z=2(e.z e.x+e.w e.y)*B_i.x+2(e.y e.z-e.w e.x)B_i.y+(e.w^2-e.x^2-e.y^2+e.z^2)B_i.z

H[3,0]=dg.Bx/de.w=2(+e.w B_i.x+e.z B_i.y-e.y B_i.z)

H[3,1]=dg.Bx/de.x=2(+e.x B_i.x+e.y B_i.y+e.z B_i.z)

H[3,2]=dg.Bx/de.y=2(-e.y B_i.x+e.x B_i.y-e.w B_i.z)

H[3,3]=dg.Bx/de.z=2(-e.z B_i.x+e.w B_i.y+e.x B_i.z)

H[3,4]=dg.Bx/dω.x=0

H[3,5]=dg.Bx/dω.y=0

H[3,6]=dg.Bx/dω.z=0

H[4,0]=dg.By/de.w=2(-e.z B_i.x+e.w B_i.y-e.x B_i.z)

H[4,1]=dg.By/de.x=2(+e.y B_i.x-e.x B_i.y+e.w B_i.z)

H[4,2]=dg.By/de.y=2(+e.x B_i.x+e.y B_i.y+e.z B_i.z);

H[4,3]=dg.By/de.z=2(-e.w B_i.x-e.z B_i.y+e.y B_i.z);

H[4,4]=dg.By/dω.x=0

H[4,5]=dg.By/dω.y=0

H[4,6]=dg.By/dω.z=0

H[5,0]=dg.Bz/de.w=2(+e.y B_i.x-e.x B_i.y+e.w B_i.z);

H[5,1]=dg.Bz/de.x=2(+e.z B_i.x-e.w B_i.y-e.x B_i.z);

H[5,2]=dg.Bz/de.y=2(+e.w B_i.x+e.z B_i.y-e.y B_i.z);

H[5,3]=dg.Bz/de.z=2(+e.x B_i.x+e.y B_i.y+e.z B_i.z);

H[5,4]=dg.Bz/dω.x=0

H[5,5]=dg.Bz/dω.y=0

H[5,6]=dg.Bz/dω.z=0

See, none of these are complicated, it's just a matter of bookkeeping.

Now for the fun part. If the observation vector elements are uncorrelated with each other, and they are if the measurement covariance is diagonal, then we can do things one element of one observation at a time. Further, we only have to use the one row of H relevant to that element, and with this, the bit inside of the matrix inverse in the filter is just a 1x1 matrix, or a scalar, and the inverse is just division. Score! We don't have to write a matrix inverter to run on our controller!