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.
You need to be a member of diydrones to add comments!
Replies
I finally got around to determining what values of Kp and Ki we are actually using in MatrixPilot for roll-pitch compensaton.
They are:
Kp = 0.0755
Ki = 0.00943
The actual Kp is about 1/3 of of what I recommended in a previous post.
I did not determine what we are using for yaw compensation.
When converting from the binary values in the code to the real loop values, there are several factors that enter into the picture, including the fact that we do not normalize the gravity vector, and several factors of 2 that pop up.
I verified the values using a special test that I set up.
These values were selected by Paul Bizard by simulation. They are engineering compromises. If you set the gains too high, the plane will dive during a hand launch. If you set them too low, it takes too long to achieve the initial lock.
I noticed quite a bit of discussion in this thread about signs, gains, PID, etc. I hope everyone finally achieved what they want. It is very easy to get the signs wrong. Obviously, if you do, then you have an oscillator.
The scheme really should work ok with PI, no D.
Also, there was some discussion about the use of the value of the time step, dt. It should only be included with integrators.
Best regards,
Bill
I have been told that I did not answer the core of your question, so here is one more reply:
Yes, you are correct that the DCM acts like an integrator.
However, in general, a PID controller can be used to lock the loop, where PID act on angle errors and produces an output that is an adjustment for the gyro rates.
The integral term in the PID is necessary, because it is the only way to cancel the gyro rate errors without having a steady state angle error. In steady state, the average value of the input to an integrator is zero. So, the integrators in the PID eventually takes on the exact values of the gyro rate offsets.
The proportional term in the PID is necessary to provide damping. Without it, the loop would oscillate because you would simply have two integrators directly in series in the feedback loop.
The derivative (D) term in the PID in this case is undesireable for exactly one of the reasons you cited for keeping the gains low: we do not want the transient errors from the accelerometers to contaminate the accuracy of our attitude estimates. If you add a derivative term, the accelerations of the plane will give you large errors. For example, with a D term would tend to make the plane dive during the acceleration of takeoff.
Best regards,
Bill
By the way, personally, I recommend a 10 second time constant, critically damped.
For that case, you would use the following values of KP and KI:
KP = 0.2
KI = 0.01
If you want a 20 second time constant, critically damped, you would use:
KP = 0.1
KI = 0.0025
Just for the fun of it, I suggest you try these values. You should see stable behavior.
In general, for a critically damped response, with time constant tau, you set:
KP = 2/tau
KI = 1/(tau*tau)
Regarding errors from the accelerometers, you can compensate for most of the low and medium frequency errors by computing the centrifugal effects from the velocity and the yaw and pitch rate, and the forward acceleration from the GPS.
Best regards,
Bill
I am a little puzzled with your results, I wonder if something else is going on, either with the hardware, or maybe a software bug.
I have extensiviely bench-tested the DCM algorithm with the PI feedback controller, and found it to be stable for a wide range of values of P and I. It does get into trouble with very large feedback gains, but I have never had any trouble with low value of feedback gains. What can happen, however, if the gains are too low and the gyros have too much drift, is the "locking" action can be broken.
You are correct that the DCM algorithm is a (nonlinear) integrator, so the combination of the DCM algorithm plus the PI controller constitutes a second order feedback system. The continuous version of the loop, in the Laplace domain, is stable for all values of P and I. The digital version of the loop, in the Z domain, is stable for a wide range of P and I, and only gets in trouble for very large or very small values of P or I.
For very large values of P and I, you get oscillations. For very small values of P and I, the loop does not lock.
In the "MatrixPilot" version of the firmware, we use very small values of P and I, to keep the transient accelerometer error from creeping in. The gyros we use are low drift, and we take pains to measure the initial offset, so we can get away with the low gains.
The values of P and I that we selected in MatrixPilot are selected to produce two real poles that are approximately equal, so we get a "locking" response that is critically damped, with about a 10 second time constant.
Both Robert Mahoney and myself prefer not to use a PID controller for the drift compensation, because the D term introduces excessive noise.
Best regards,
Bill