Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AHRS: re-instate new DCM drift correction code 

This reverts commit 078489638d47fbaffde7c51249e36b5a8fc4ef9d.
This commit is contained in:
Andrew Tridgell 2012-06-28 11:09:22 +10:00
parent 2d71a2affc
commit e531061caa
3 changed files with 270 additions and 242 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_AHRS/AP_AHRS.h
View File

@ -28,7 +28,8 @@ public:
// Constructor
AP_AHRS(IMU *imu, GPS *&gps):
_imu(imu),
_gps(gps)
_gps(gps),
_barometer(NULL)
{
// base the ki values by the sensors maximum drift
// rate. The APM2 has gyros which are much less drift
@ -89,9 +90,6 @@ protected:
// pointer to compass object, if enabled
Compass * _compass;
// pointer to compass object, if enabled
AP_Baro * _barometer;
// time in microseconds of last compass update
uint32_t _compass_last_update;
@ -99,8 +97,10 @@ protected:
// IMU under us without our noticing.
IMU *_imu;
GPS *&_gps;
AP_Baro *_barometer;
// true if we are doing centripetal acceleration correction
// true if we can assume the aircraft will be flying forward
// on its X axis
bool _fly_forward;
// the limit of the gyro drift claimed by the sensors, in

449
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -23,6 +23,12 @@
// from the GPS and wait for the ground speed to get above GPS_SPEED_MIN
#define GPS_SPEED_RESET 100
// the limit (in degrees/second) beyond which we stop integrating
// omega_I. At larger spin rates the DCM PI controller can get 'dizzy'
// which results in false gyro drift. See
// http://gentlenav.googlecode.com/files/fastRotations.pdf
#define SPIN_RATE_LIMIT 20
// table of user settable parameters
const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
// @Param: YAW_P
@ -70,49 +76,13 @@ AP_AHRS_DCM::update(void)
void
AP_AHRS_DCM::matrix_update(float _G_Dt)
{
// _omega_integ_corr is used for _centripetal correction
// (theoretically better than _omega)
_omega_integ_corr = _gyro_vector + _omega_I;
// Equation 16, adding proportional and integral correction terms
_omega = _omega_integ_corr + _omega_P + _omega_yaw_P;
_omega = _gyro_vector + _omega_I + _omega_P + _omega_yaw_P;
// this is a replacement of the DCM matrix multiply (equation
// 17), with known zero elements removed and the matrix
// operations inlined. This runs much faster than the original
// version of this code, as the compiler was doing a terrible
// job of realising that so many of the factors were in common
// or zero. It also uses much less stack, as we no longer need
// two additional local matrices
Vector3f r = _omega * _G_Dt;
_dcm_matrix.rotate(r);
_dcm_matrix.rotate(_omega * _G_Dt);
}
// adjust an accelerometer vector for known acceleration forces
void
AP_AHRS_DCM::accel_adjust(Vector3f &accel)
{
float veloc;
// compensate for linear acceleration. This makes a
// surprisingly large difference in the pitch estimate when
// turning, plus on takeoff and landing
float acceleration = _gps->acceleration();
accel.x -= acceleration;
// compensate for centripetal acceleration
veloc = _gps->ground_speed * 0.01;
// We are working with a modified version of equation 26 as
// our IMU object reports acceleration in the positive axis
// direction as positive
// Equation 26 broken up into separate pieces
accel.y -= _omega_integ_corr.z * veloc;
accel.z += _omega_integ_corr.y * veloc;
}
/*
reset the DCM matrix and omega. Used on ground start, and on
extreme errors in the matrix
@ -124,7 +94,6 @@ AP_AHRS_DCM::reset(bool recover_eulers)
_omega_I.zero();
_omega_P.zero();
_omega_yaw_P.zero();
_omega_integ_corr.zero();
_omega.zero();
// if the caller wants us to try to recover to the current
@ -259,216 +228,262 @@ AP_AHRS_DCM::normalize(void)
}
// produce a yaw error value. The returned value is proportional
// to sin() of the current heading error in earth frame
float
AP_AHRS_DCM::yaw_error_compass(void)
{
Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, _compass->mag_z);
// get the mag vector in the earth frame
Vector3f rb = _dcm_matrix * mag;
rb.normalize();
if (rb.is_inf()) {
// not a valid vector
return 0.0;
}
// get the earths magnetic field (only X and Y components needed)
Vector3f mag_earth = Vector3f(cos(_compass->get_declination()),
sin(_compass->get_declination()), 0);
// calculate the error term in earth frame
Vector3f error = rb % mag_earth;
return error.z;
}
// produce a yaw error value using the GPS. The returned value is proportional
// to sin() of the current heading error in earth frame
float
AP_AHRS_DCM::yaw_error_gps(void)
{
return sin(ToRad(_gps->ground_course * 0.01) - yaw);
}
float
AP_AHRS_DCM::_P_gain(float spin_rate)
{
if (spin_rate < ToDeg(50)) {
return 1.0;
}
if (spin_rate > ToDeg(500)) {
return 10.0;
}
return spin_rate/ToDeg(50);
}
// yaw drift correction using the compass or GPS
// this function prodoces the _omega_yaw_P vector, and also
// contributes to the _omega_I.z long term yaw drift estimate
void
AP_AHRS_DCM::drift_correction_yaw(void)
{
bool new_value = false;
float yaw_error;
float yaw_deltat;
if (_compass && _compass->use_for_yaw()) {
if (_compass->last_update != _compass_last_update) {
yaw_deltat = (_compass->last_update - _compass_last_update) * 1.0e-6;
_compass_last_update = _compass->last_update;
if (!_have_initial_yaw) {
float heading = _compass->calculate_heading(_dcm_matrix);
_dcm_matrix.from_euler(roll, pitch, heading);
_omega_yaw_P.zero();
_have_initial_yaw = true;
}
new_value = true;
yaw_error = yaw_error_compass();
}
} else if (_gps && _gps->status() == GPS::GPS_OK) {
if (_gps->last_fix_time != _gps_last_update &&
_gps->ground_speed >= GPS_SPEED_MIN) {
yaw_deltat = (_gps->last_fix_time - _gps_last_update) * 1.0e-3;
_gps_last_update = _gps->last_fix_time;
if (!_have_initial_yaw) {
_dcm_matrix.from_euler(roll, pitch, ToRad(_gps->ground_course*0.01));
_omega_yaw_P.zero();
_have_initial_yaw = true;
}
new_value = true;
yaw_error = yaw_error_gps();
}
}
if (!new_value) {
// we don't have any new yaw information
// slowly decay _omega_yaw_P to cope with loss
// of our yaw source
_omega_yaw_P.z *= 0.97;
return;
}
// the yaw error is a vector in earth frame
Vector3f error = Vector3f(0,0, yaw_error);
// convert the error vector to body frame
error = _dcm_matrix.mul_transpose(error);
// the spin rate changes the P gain, and disables the
// integration at higher rates
float spin_rate = _omega.length();
// update the proportional control to drag the
// yaw back to the right value. We use a gain
// that depends on the spin rate. See the fastRotations.pdf
// paper from Bill Premerlani
_omega_yaw_P = error * _P_gain(spin_rate) * _kp_yaw.get();
// don't update the drift term if we lost the yaw reference
// for more than 2 seconds
if (yaw_deltat < 2.0 && spin_rate < ToRad(SPIN_RATE_LIMIT)) {
// also add to the I term
_omega_I_sum.z += error.z * _ki_yaw * yaw_deltat;
}
_error_yaw_sum += fabs(yaw_error);
_error_yaw_count++;
}
// perform drift correction. This function aims to update _omega_P and
// _omega_I with our best estimate of the short term and long term
// gyro error. The _omega_P value is what pulls our attitude solution
// back towards the reference vector quickly. The _omega_I term is an
// attempt to learn the long term drift rate of the gyros.
//
// This function also updates _omega_yaw_P with a yaw correction term
// from our yaw reference vector
// This drift correction implementation is based on a paper
// by Bill Premerlani from here:
// http://gentlenav.googlecode.com/files/RollPitchDriftCompensation.pdf
void
AP_AHRS_DCM::drift_correction(float deltat)
{
float error_course = 0;
Vector3f accel;
Vector3f error;
float error_norm = 0;
float yaw_deltat = 0;
Vector3f velocity;
uint32_t last_correction_time;
bool use_gps = true;
accel = _accel_vector;
// perform yaw drift correction if we have a new yaw reference
// vector
drift_correction_yaw();
// if enabled, use the GPS to correct our accelerometer vector
// for centripetal forces
if(_fly_forward &&
_gps != NULL &&
_gps->status() == GPS::GPS_OK) {
accel_adjust(accel);
// integrate the accel vector in the earth frame between GPS readings
_ra_sum += _dcm_matrix * (_accel_vector * deltat);
// keep a sum of the deltat values, so we know how much time
// we have integrated over
_ra_deltat += deltat;
// check if we have GPS lock
if (_gps == NULL || _gps->status() != GPS::GPS_OK) {
use_gps = false;
}
// a copter (which has _fly_forward false) which doesn't have a
// compass for yaw can't rely on the GPS velocity lining up with
// the earth frame from DCM, so it needs to assume zero velocity
// in the drift correction
if (!_fly_forward && !(_compass && _compass->use_for_yaw())) {
use_gps = false;
}
//*****Roll and Pitch***************
if (use_gps == false) {
// no GPS, or no lock. We assume zero velocity. This at
// least means we can cope with gyro drift while sitting
// on a bench with no GPS lock
if (_ra_deltat < 0.1) {
// not enough time has accumulated
return;
}
velocity.zero();
_last_velocity.zero();
last_correction_time = millis();
} else {
if (_gps->last_fix_time == _ra_sum_start) {
// we don't have a new GPS fix - nothing more to do
return;
}
velocity = Vector3f(_gps->velocity_north(), _gps->velocity_east(), 0);
last_correction_time = _gps->last_fix_time;
}
// normalise the accelerometer vector to a standard length
// this is important to reduce the impact of noise on the
// drift correction, as very noisy vectors tend to have
// abnormally high lengths. By normalising the length we
// reduce their impact.
float accel_length = accel.length();
accel *= (_gravity / accel_length);
if (accel.is_inf()) {
// we can't do anything useful with this sample
_omega_P.zero();
// even without GPS lock we can correct for the vertical acceleration
if (_barometer != NULL) {
// Z velocity is down
velocity.z = - _barometer->get_climb_rate();
}
// see if this is our first time through - in which case we
// just setup the start times and return
if (_ra_sum_start == 0) {
_ra_sum_start = last_correction_time;
_last_velocity = velocity;
return;
}
// calculate the error, in m/2^2, between the attitude
// implied by the accelerometers and the attitude
// in the current DCM matrix
error = _dcm_matrix.c % accel;
// get the corrected acceleration vector in earth frame. Units
// are m/s/s
Vector3f ge;
float v_scale = 1.0/(_ra_deltat*_gravity);
ge = Vector3f(0, 0, -1.0) + ((velocity - _last_velocity) * v_scale);
// Limit max error to limit the effect of noisy values
// on the algorithm. This limits the error to about 11
// degrees
error_norm = error.length();
if (error_norm > 2) {
error *= (2 / error_norm);
// calculate the error term in earth frame.
ge.normalize();
_ra_sum.normalize();
if (_ra_sum.is_inf() || ge.is_inf()) {
// the _ra_sum length is zero - we are falling with
// no apparent gravity. This gives us no information
// about which way up we are, so treat the error as zero
error = Vector3f(0,0,0);
} else {
error = _ra_sum % ge;
}
_error_rp_sum += error.length();
_error_rp_count++;
// convert the error term to body frame
error = _dcm_matrix.mul_transpose(error);
// base the P gain on the spin rate
float spin_rate = _omega.length();
// we now want to calculate _omega_P and _omega_I. The
// _omega_P value is what drags us quickly to the
// accelerometer reading.
_omega_P = error * _kp_roll_pitch;
_omega_P = error * _P_gain(spin_rate) * _kp;
// the _omega_I is the long term accumulated gyro
// error. This determines how much gyro drift we can
// handle.
_omega_I_sum += error * (_ki_roll_pitch * deltat);
_omega_I_sum_time += deltat;
// these sums support the reporting of the DCM state via MAVLink
_error_rp_sum += error_norm;
_error_rp_count++;
// yaw drift correction
// we only do yaw drift correction when we get a new yaw
// reference vector. In between times we rely on the gyros for
// yaw. Avoiding this calculation on every call to
// update_DCM() saves a lot of time
if (_compass && _compass->use_for_yaw()) {
if (_compass->last_update != _compass_last_update) {
yaw_deltat = 1.0e-6*(_compass->last_update - _compass_last_update);
if (_have_initial_yaw && yaw_deltat < 2.0) {
// Equation 23, Calculating YAW error
// We make the gyro YAW drift correction based
// on compass magnetic heading
float heading_x, heading_y;
float heading = _compass->calculate_heading(_dcm_matrix);
heading_x = cos(heading);
heading_y = sin(heading);
error_course = (_dcm_matrix.a.x * heading_y) - (_dcm_matrix.b.x * heading_x);
_compass_last_update = _compass->last_update;
} else {
// this is our first estimate of the yaw,
// or the compass has come back online after
// no readings for 2 seconds.
//
// construct a DCM matrix based on the current
// roll/pitch and the compass heading.
// First ensure the compass heading has been
// calculated
float heading = _compass->calculate_heading(_dcm_matrix);
// now construct a new DCM matrix
_dcm_matrix.from_euler(roll, pitch, heading);
_have_initial_yaw = true;
_compass_last_update = _compass->last_update;
error_course = 0;
}
}
} else if (_gps && _gps->status() == GPS::GPS_OK) {
if (_gps->last_fix_time != _gps_last_update) {
// Use GPS Ground course to correct yaw gyro drift
if (_gps->ground_speed >= GPS_SPEED_MIN) {
yaw_deltat = 1.0e-3*(_gps->last_fix_time - _gps_last_update);
if (_have_initial_yaw && yaw_deltat < 2.0) {
float course_over_ground_x = cos(ToRad(_gps->ground_course/100.0));
float course_over_ground_y = sin(ToRad(_gps->ground_course/100.0));
// Equation 23, Calculating YAW error
error_course = (_dcm_matrix.a.x * course_over_ground_y) - (_dcm_matrix.b.x * course_over_ground_x);
_gps_last_update = _gps->last_fix_time;
} else {
// when we first start moving, set the
// DCM matrix to the current
// roll/pitch values, but with yaw
// from the GPS
_dcm_matrix.from_euler(roll, pitch, ToRad(_gps->ground_course));
_have_initial_yaw = true;
error_course = 0;
_gps_last_update = _gps->last_fix_time;
}
} else if (_gps->ground_speed >= GPS_SPEED_RESET) {
// we are not going fast enough to use GPS for
// course correction, but we won't reset
// _have_initial_yaw yet, instead we just let
// the gyro handle yaw
error_course = 0;
} else {
// we are moving very slowly. Reset
// _have_initial_yaw and adjust our heading
// rapidly next time we get a good GPS ground
// speed
error_course = 0;
_have_initial_yaw = false;
}
}
// accumulate some integrator error
if (spin_rate < ToRad(SPIN_RATE_LIMIT)) {
_omega_I_sum += error * _ki * _ra_deltat;
_omega_I_sum_time += _ra_deltat;
}
// see if there is any error in our heading relative to the
// yaw reference. This will be zero most of the time, as we
// only calculate it when we get new data from the yaw
// reference source
if (yaw_deltat == 0 || error_course == 0) {
// we don't have a new reference heading. Slowly
// decay the _omega_yaw_P to ensure that if we have
// lost the yaw reference sensor completely we don't
// keep using a stale offset
_omega_yaw_P *= 0.97;
goto check_sum_time;
}
// ensure the course error is scaled from -PI to PI
if (error_course > PI) {
error_course -= 2*PI;
} else if (error_course < -PI) {
error_course += 2*PI;
}
// Equation 24, Applys the yaw correction to the XYZ rotation of the aircraft
// this gives us an error in radians
error = _dcm_matrix.c * error_course;
// Adding yaw correction to proportional correction vector. We
// allow the yaw reference source to affect all 3 components
// of _omega_yaw_P as we need to be able to correctly hold a
// heading when roll and pitch are non-zero
_omega_yaw_P = error * _kp_yaw.get();
// add yaw correction to integrator correction vector, but
// only for the z gyro. We rely on the accelerometers for x
// and y gyro drift correction. Using the compass or GPS for
// x/y drift correction is too inaccurate, and can lead to
// incorrect builups in the x/y drift. We rely on the
// accelerometers to get the x/y components right
_omega_I_sum.z += error.z * (_ki_yaw * yaw_deltat);
// we keep the sum of yaw error for reporting via MAVLink.
_error_yaw_sum += error_course;
_error_yaw_count++;
check_sum_time:
if (_omega_I_sum_time > 10) {
// every 10 seconds we apply the accumulated
// _omega_I_sum changes to _omega_I. We do this to
// prevent short term feedback between the P and I
// terms of the controller. The _omega_I vector is
// supposed to refect long term gyro drift, but with
// high noise it can start to build up due to short
// term interactions. By summing it over 10 seconds we
// prevent the short term interactions and can apply
// the slope limit more accurately
float drift_limit = _gyro_drift_limit * _omega_I_sum_time;
_omega_I_sum.x = constrain(_omega_I_sum.x, -drift_limit, drift_limit);
_omega_I_sum.y = constrain(_omega_I_sum.y, -drift_limit, drift_limit);
_omega_I_sum.z = constrain(_omega_I_sum.z, -drift_limit, drift_limit);
if (_omega_I_sum_time >= 5) {
// limit the rate of change of omega_I to the hardware
// reported maximum gyro drift rate. This ensures that
// short term errors don't cause a buildup of omega_I
// beyond the physical limits of the device
float change_limit = _gyro_drift_limit * _omega_I_sum_time;
_omega_I_sum.x = constrain(_omega_I_sum.x, -change_limit, change_limit);
_omega_I_sum.y = constrain(_omega_I_sum.y, -change_limit, change_limit);
_omega_I_sum.z = constrain(_omega_I_sum.z, -change_limit, change_limit);
_omega_I += _omega_I_sum;
// zero the sum
_omega_I_sum.zero();
_omega_I_sum_time = 0;
}
// zero our accumulator ready for the next GPS step
_ra_sum.zero();
_ra_deltat = 0;
_ra_sum_start = last_correction_time;
// remember the velocity for next time
_last_velocity = velocity;
}

43
libraries/AP_AHRS/AP_AHRS_DCM.h
View File

@ -16,15 +16,15 @@ public:
// Constructors
AP_AHRS_DCM(IMU *imu, GPS *&gps) : AP_AHRS(imu, gps)
{
_kp_roll_pitch = 0.13;
_kp_yaw.set(0.2);
_dcm_matrix(Vector3f(1, 0, 0),
Vector3f(0, 1, 0),
Vector3f(0, 0, 1));
_dcm_matrix.identity();
// base the ki values on the sensors drift rate
_ki_roll_pitch = _gyro_drift_limit * 5;
_ki_yaw = _gyro_drift_limit * 8;
// these are experimentally derived from the simulator
// with large drift levels
_ki = 0.0087;
_ki_yaw = 0.01;
_kp = 0.4;
_kp_yaw.set(0.4);
}
// return the smoothed gyro vector corrected for drift
@ -46,18 +46,20 @@ public:
AP_Float _kp_yaw;
private:
float _kp_roll_pitch;
float _ki_roll_pitch;
float _kp;
float _ki;
float _ki_yaw;
bool _have_initial_yaw;
// Methods
void accel_adjust(Vector3f &accel);
void matrix_update(float _G_Dt);
void normalize(void);
void check_matrix(void);
bool renorm(Vector3f const &a, Vector3f &result);
void drift_correction(float deltat);
void drift_correction_yaw(void);
float yaw_error_compass();
float yaw_error_gps();
void euler_angles(void);
// primary representation of attitude
@ -66,14 +68,16 @@ private:
Vector3f _gyro_vector; // Store the gyros turn rate in a vector
Vector3f _accel_vector; // current accel vector
Vector3f _omega_P; // accel Omega Proportional correction
Vector3f _omega_yaw_P; // yaw Omega Proportional correction
Vector3f _omega_P; // accel Omega proportional correction
Vector3f _omega_yaw_P; // proportional yaw correction
Vector3f _omega_I; // Omega Integrator correction
Vector3f _omega_I_sum; // summation vector for omegaI
Vector3f _omega_I_sum;
float _omega_I_sum_time;
Vector3f _omega_integ_corr; // Partially corrected Gyro_Vector data - used for centrepetal correction
Vector3f _omega; // Corrected Gyro_Vector data
// P term gain based on spin rate
float _P_gain(float spin_rate);
// state to support status reporting
float _renorm_val_sum;
uint16_t _renorm_val_count;
@ -86,6 +90,15 @@ private:
// time in millis when we last got a GPS heading
uint32_t _gps_last_update;
// state of accel drift correction
Vector3f _ra_sum;
Vector3f _last_velocity;
float _ra_deltat;
uint32_t _ra_sum_start;
// current drift error in earth frame
Vector3f _drift_error_earth;
};
#endif // AP_AHRS_DCM_H