Accelerometer:
This sensor generates three values based on the acceleration sensed in three orthogonal directions. If the sensor is sitting still, it should read 1g in the direction parallel to gravity. Regardless of how the sensor is oriented, it will always point parallel to gravity (as long at it standing still or moving at a constant velocity). This ability to always point in the same direction makes the accelerometer a very good tool for countering gyro drift in what might be considered the pitch and roll directions of an aircraft. We can compare the direction of gravity to our current estimate of down or up and use the difference to detect and counter error in our estimate.
Where this gets more complicated is in dealing with the real world. When used on an aircraft, the accelerometer measures a lot more than just gravity. It also measures the vibration that travel through the airframe, changes in the aircraft's path and finally changes to it speed. We generally don't get the luxury of taking the accelerometer readings at face value. Although over a long period of time the accelerometer will generally point down, we can't assume that any one reading will be correct. It is necessary to filter them in such a way as to emphasize values that "look" correct while minimizing readings that appear to be heavily influenced by noise. There are a number of methods and theories relating to how this is done. I am going to keep it simple for this example and only correct a very small part of the error each sensor reading. Over a long period of time, these small "nudges" will add up and correctly counter the gyro drift. A future posting my go into more detail on advanced ways to handle this.
Drift Correction Operations:
Accelerometer sensor readings are taken in the aircraft's reference frame (body frame). If the aircraft is not accelerating and perfectly level, then the reading will point straight down toward gravity. In a similar situation, if the aircraft's attitude estimate indicates that the body is level, but the accelerometer does not point straight down, then we might conclude that the attitude estimate has some error. In this very simple example, we could then adjust the attitude estimate to correct this error. Unfortunately, aircraft rarely are in this situation, so we need to be able to detect attitude estimate errors even if the aircraft is not perfectly level. Dealing with non-zero body acceleration will be ignored for now, to keep things simple.
1.) The first step in accounting for orientation involves rotating the accelerometer vector from the body frame into the world frame. Vector rotation can be done easily with a 3x3 rotation matrix or quaternion. In my case I use a quaternion. I won't go into the exact math of this operation as there are many good online references showing how to use either method to rotate a vector.
V_accel_earth = Rotate(OrientationQuat, V_accel_body) (1)
Once the accelerometer reading is in the earth frame, you can apply filters to it to reduce noise. By filtering in the earth frame, sensor reading changes due to real body movement are not effected by the filter. This reduces false transient corrections to the orientation caused by filter delay.
2.) The second step is to "compare" this vector in the earth frame to the the expected result (pure vertical vector). This is accomplished by simply taking the cross product of the earth frame accelerometer vector with the vector 0,0,1.0f.
V_correction = CrossProduct( V_accel_earth, Vector(0,0,1.0f)) (2)
If the accelerometer reading points perfectly vertical (with a magnitude of 1), the result of the cross product will be 0,0,0 - indicating no correction is needed. As the accelerometer reading deviates from vertical, the correction vector will be larger.
3.) The third step is to scale the correction vector to reduce correction errors. Over time, consistent errors will result in a series of small corrections that will nudge the orientation estimate error towards zero. Random noise will cancel out with minimal effect. In this example I multiply by a constant 0.01. Increasing this number will correct for error faster but allow more accelerometer noise through. Reducing it will slow corrections while reducing the effect of noise. This step is not always needed, as will be explained in the next step.
V_correction = V_correction * 0.1f (3)
4.) The fourth step is to rotate the correction vector back to the body frame and add it to the next gyro sensor reading. Adding the correction vector to gyro reading is playing a little loose with the units. The gyro data is in radians per second while the correction vector has units of radians. This solution still works very well because when we integrate the combined gyro reading and correction vector, we will divide it by the control loop rate (see my post on fast quaternion integration). In my system I have a control loop of 400Hz, so only 1/400th of the indicated error is applied each cycle. This is the primary mechanism that allows this method to slowly "nudge" the attitude estimate towards what the accelerometer believes to be normal. Since were only taking a very small portion of the indicated error, vibration has reduced effect on our attitude estimate (although controlling vibration is still critical to best performance, as like every other system: garbage in = garbage out).
V_gyro_next += Rotate(V_correction, OrientationQuat) (4)
This adjusted gyro reading will then be integrated to produce the next orientation estimate. By adjusting the next gyro reading we can counter the detected orientation error without adding a significant amount of time to our cycle (compared to directly adjusting the orientation estimate independent of the next gyro reading integration).
It should be noted that this method does rely on the small angle approximation. This should never be a problem for any system with moderate to high sensor sample rates and normal flight characteristics. Improvements may involve dynamically reducing the correction vector when the accelerometer data looks suspect (very large correction values or accelerometer magnitudes that deviate significantly from 1.0).
Hopefully this is of value to anyone looking to make a their own fully 3D orientation estimate complimentary filter. Future subjects will include using a magnetometer to counter yaw gyro drift and also velocity and position estimation using a IMU + GPS complimentary filter. I will soon start including links to my code so that people interested in experimenting with this method can do so easily. (Edit: Go here to see post with example IMU code)
Regards,
Phillip
p.s. -- Since originally posting, I have updated step 4 to better explain how we can "get away" with adding the correction vector to the gyro data to counter drift.
No comments:
Post a Comment