In a previous post, I explained how to correct for gyro drift by using the accelerometer’s reading of the gravitational acceleration vector.
But how do we know the measured gravitational vector is accurate? Unless I build my tricopter with utmost precision and proper materials (i.e., not scrap wood cut with a handsaw), it’s impossible to mount the accelerometer perfectly horizontal to the chassis.
Fortunately, this is a problem that can be fixed in code. Recall that we update the DCM with a rotation vector w * dt, where w (omega) is the “instantaneous” rotation vector measured by the gyro and dt is the change in time, i.e., our system loop time. Recall also that we use the accelerometer’s reading of gravity to produce a correctional rotation vector wA that we add to wdt, keeping the DCM at least horizontal.
for (int i=0; i<3; i++) {
wdt[i] = (wdt[i] + ACC_WEIGHT*wA[i]) / (1.0 + ACC_WEIGHT);
}
When the accelerometer is installed in a skewed manner, wA will correct wdt to the wrong target rotation, so we need to correct the correction vector wA. (WRONG. Read my corrections at the bottom of this post.) The following is a visualization of the orientation my tricopter thought it was in when it was actually level (red line is X axis, blue is Z axis). What’s funnier is that I actually expected this thing to fly.
Inaccurate DCM due to tilted accelerometer.
To correct wA, we mount the accelerometer to the tricopter, and in its mounted state with the tricopter level, we take the accelerometer’s reading of gravity and store it temporarily as a vector. We then cross-multiply this vector with the body’s K vector (<0, 0, 1>) to produce a correctional rotation vector we will call wAOffset. Notice that this is in the opposite order of how we calculated wA by cross-multiplying the global K vector with the measured gravitational vector. This means that we can add wAOffset to wA to make wA accurately correct for gyro drift towards a level tricopter, not a level accelerometer. Same idea, just reversed! Here’s the commit of this change.
UPDATE 2/17/12: Scratch that, my math was wrong.
Because wdt and wA operate in the body frame of reference, correcting wA with wAOffset fails if the body tilts away from vertical. For example, if the body rotates 90 degrees along the X axis, wAOffset’s Y component is in the global Z direction, and the DCM starts spinning accordingly.
Here’s the right way to think about this (and this time, I validated my theory by fully testing it out on the tricopter):
We adjust our DCM according to whatever the accelerometer thinks is gravity. This means that the DCM will represent the orientation of the accelerometer, not the tricopter chassis, along the X and Y rotational axes. The rotational difference between the orientations of the accelerometer (gyroDCM) and the chassis (bodyDCM) is constant and can be represented as another rotation matrix we will call offsetDCM. We can obtain offsetDCM using wAOffset and rotate gyroDCM with offsetDCM to obtain bodyDCM.
Keep in mind, however, that we must still perform our IMU calculations using gyroDCM and not bodyDCM. This is because it is gyroDCM we are correcting with the accelerometer’s measurement of the gravity vector, and bodyDCM is only a transformation of that DCM based on offsetDCM. We use bodyDCM for flight calculations, but gyroDCM is what we keep track of within the IMU.
As a final note, this is a small-angle approximation! We could do something fancy with trigonometry, but if the hardware is so poorly built (or poorly designed) that small-angle approximations become insufficient, we can’t expect software to fix everything.
The updated IMU code is on my github repo.