and the Arduino software:
// Sparkfun 9DOF Razor IMU AHRS
// 9 Degree of Measurement Attitude and Heading Reference System
// Firmware v1.0
// Released under Creative Commons License
// Code by Doug Weibel and Jose Julio
// Based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose Julio and Doug Weibel
I thought the drift correction was far too fast, so that the attitude
settled to the accelerometer angles, rather than the integrated gyro
angles. This gives problems in a turn, when the accelerometers dont
measure the bank angle, and also when doing straight-line accelerations,
when the accelerometers see the acceleration, and we see a pitch change,
even though the gyros dont see any attitude change.
When I slowed the gyro correction down by setting Kp_rollpitch smaller,
I saw the control loop go unstable. I think I finally see why. While I
have every respect for Bill Premerlani's math ability, I think he has
overlooked one crucial point in the control algorithm.
The classical PID control loop is for instance a thermostat, where the error
between the output temperature and the setpoint is computed, then
proportional + integral + ( usually ) derivative terms are computed from
the error, and fed back on each iteration to the control ( heater )
until the error goes to zero.
I set up a program just to drift correct one gyro, so I could see what
happened without all the complex matrix operations getting in the way.
In the case of a gyro, there is one crucial difference. We have the
gyro signal as (rate - offset), this is the omega term in the maths. We
then integrate this to get the angle, and compare the angle with the
accelerometer signal to get the error. But when we compute say the
proportional term, and feed this back to the offset, we are integrating
this to get the new angle. So our proportional signal is really an
integral correction. I could see this in reality, because when I ran
it, the output would run faster and faster back through zero, as the
error term integrated up. This is exactly what happens when you set the
integration constant in a classical PID controller too high - you get
oscillations. The only way I could get it stable was to drop the Ki
term entirely, and add a differential term Kd - which in reality is a
proportional term after the integration. Then the loop behaved like I
would expect a 'proper' control loop should.
In the existing implementation, the control appears to work, but it does
so for the wrong reasons. The feedback loop is very fast ( 20 msec )
and the 'proportional' constant is relatively very large ( Kp = 0.01 ),
so the loop stays 'locked' to the accelerometers. The gyro drift in 20
msec is tiny - well below the measurement resolution - so it appears to
work. But as soon as you reduce Kp to get the timeconstant up around 30
seconds ( Kp around 0.001 ) the loop goes unstable.
I dont have the math ability to insert the correct control system into
the existing code, my expertise is in control systems and programming,
not in math, but I can see that this change should make the system work
much better than it does at present.
Comments are welcome.