Thread Tools
This thread is privately moderated by Jack Crossfire, who may elect to delete unwanted replies.
Oct 21, 2013, 07:51 PM
Registered User
Jack Crossfire's Avatar
Thread OP

Accelerometer & GPS fusion

Was surprised to find someone was still studying bias cancellation in gyros. Only Analog Devices still produces bias cancelling gyros & they only produce a handful. The world has migrated to super cheap, high bias gyros because position sensing has gotten good enough to compensate for bad gyro sensing.

They're still selling the original analog, single axis gyros at the original price, but have been selling a digital version in an easily soldered SOIC package. They still advertize an extremely low 16 deg drift over an hour for the digital version. It still costs 10x the Invensense version. For a GPS denied environment with real bad position sensing, it might be worth it.

If you have a real high resolution optical flow system that updates really slowly because of processing power, there might be a benefit to using the accelerometer data to get short term updates.

Went over more of the Arducopter source code. They call the fusion of position sensing with accerometer data "inertial navigation." It's in AP_InertialNav.cpp. It seems to add earth frame acceleration data to historic GPS data through a convoluted system of weights & delay lines. Its advantage relies on GPS having a 400ms lag.

_position_correction & _position_error are the key variables. _position_correction is updated when the accelerometer is updated. _position_error is updated when the GPS is updated. They both feed back into each other. The earth frame acceleration data comes from the AHRS, which itself relies on GPS.

The direction cosine matrix is defined by 3 functions in matrix3.cpp. from_euler writes the matrix. rotate applies body frame gyro inputs. to_euler reads the matrix. It's all 32 bit floating point trig, no sqrt. It's probably worth converting to the DCM. It could probably be done easily in fixed point.

The drift correction is all applied to the arguments of the rotation function. They calculate the reference attitude in earth frame, subtract it from the integrated attitude, convert the error to body frame, & add it to the rotation function. The reference attitude is calculated from the accelerometer & acceleration from position sensing.

They also calculate a gyro bias. It adds the sum of the body frame error over a very long time to a gyro bias accumulator. The body frame error is clamped to a low value.

There are a lot of fudge factors for bogus position changes, high turn rates. A direction in body frame is converted to earth frame by multiplying it by the DCM matrix. It's converted to body frame by calling Matrix3::mul_transpose.
Sign up now
to remove ads between posts

Quick Reply
Thread Tools