From 8600ad8d7fe437714d57be582d26fff3fd99d91c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 27 Jun 2012 18:16:41 +1000 Subject: [PATCH] AHRS: revert to the old drift correction algorithm we need to work out why Craigs quad flipped today before we enable this new drift correction --- libraries/AP_AHRS/AP_AHRS.h | 10 +- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 461 +++++++++++++++--------------- libraries/AP_AHRS/AP_AHRS_DCM.h | 43 +-- 3 files changed, 243 insertions(+), 271 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index f4d0bef687..9d97525d4e 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -28,8 +28,7 @@ public: // Constructor AP_AHRS(IMU *imu, GPS *&gps): _imu(imu), - _gps(gps), - _barometer(NULL) + _gps(gps) { // base the ki values by the sensors maximum drift // rate. The APM2 has gyros which are much less drift @@ -90,6 +89,9 @@ 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; @@ -97,10 +99,8 @@ protected: // IMU under us without our noticing. IMU *_imu; GPS *&_gps; - AP_Baro *_barometer; - // true if we can assume the aircraft will be flying forward - // on its X axis + // true if we are doing centripetal acceleration correction bool _fly_forward; // the limit of the gyro drift claimed by the sensors, in diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index f04f11ea19..eccfb88e32 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -23,12 +23,6 @@ // 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 @@ -76,13 +70,49 @@ AP_AHRS_DCM::update(void) void AP_AHRS_DCM::matrix_update(float _G_Dt) { - // Equation 16, adding proportional and integral correction terms - _omega = _gyro_vector + _omega_I + _omega_P + _omega_yaw_P; + // _omega_integ_corr is used for _centripetal correction + // (theoretically better than _omega) + _omega_integ_corr = _gyro_vector + _omega_I; - _dcm_matrix.rotate(_omega * _G_Dt); + // Equation 16, adding proportional and integral correction terms + _omega = _omega_integ_corr + _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); } +// 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 @@ -94,6 +124,7 @@ 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 @@ -228,262 +259,216 @@ 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 drift correction implementation is based on a paper -// by Bill Premerlani from here: -// http://gentlenav.googlecode.com/files/RollPitchDriftCompensation.pdf +// This function also updates _omega_yaw_P with a yaw correction term +// from our yaw reference vector void AP_AHRS_DCM::drift_correction(float deltat) { - Vector3f error; - Vector3f velocity; - uint32_t last_correction_time; - bool use_gps = true; + float error_course = 0; + Vector3f accel; + Vector3f error; + float error_norm = 0; + float yaw_deltat = 0; - // perform yaw drift correction if we have a new yaw reference - // vector - drift_correction_yaw(); + accel = _accel_vector; - // integrate the accel vector in the earth frame between GPS readings - _ra_sum += _dcm_matrix * (_accel_vector * deltat); + // 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); + } - // 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; - } + //*****Roll and Pitch*************** - // 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; - } + // 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(); + return; + } - 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; - } + // 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; - // even without GPS lock we can correct for the vertical acceleration - if (_barometer != NULL) { - // Z velocity is down - velocity.z = - _barometer->get_climb_rate(); - } + // 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); + } - // 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; - } + // 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; - // 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); + // 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; - // 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; - } + // these sums support the reporting of the DCM state via MAVLink + _error_rp_sum += error_norm; + _error_rp_count++; - _error_rp_sum += error.length(); - _error_rp_count++; + // yaw drift correction - // convert the error term to body frame - error = _dcm_matrix.mul_transpose(error); + // 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); - // base the P gain on the spin rate - float spin_rate = _omega.length(); + // 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; + } + } + } - // 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 * _P_gain(spin_rate) * _kp; + // 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; + } - // accumulate some integrator error - if (spin_rate < ToRad(SPIN_RATE_LIMIT)) { - _omega_I_sum += error * _ki * _ra_deltat; - _omega_I_sum_time += _ra_deltat; - } + // 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; + } - 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; - _omega_I_sum.zero(); - _omega_I_sum_time = 0; - } + // 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; - // zero our accumulator ready for the next GPS step - _ra_sum.zero(); - _ra_deltat = 0; - _ra_sum_start = last_correction_time; + // 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(); - // remember the velocity for next time - _last_velocity = velocity; + // 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); + + _omega_I += _omega_I_sum; + + // zero the sum + _omega_I_sum.zero(); + _omega_I_sum_time = 0; + } } diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index 2493133a6d..d4bb46f474 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -16,15 +16,15 @@ public: // Constructors AP_AHRS_DCM(IMU *imu, GPS *&gps) : AP_AHRS(imu, gps) { - _dcm_matrix.identity(); + _kp_roll_pitch = 0.13; + _kp_yaw.set(0.2); + _dcm_matrix(Vector3f(1, 0, 0), + Vector3f(0, 1, 0), + Vector3f(0, 0, 1)); - // 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); + // base the ki values on the sensors drift rate + _ki_roll_pitch = _gyro_drift_limit * 5; + _ki_yaw = _gyro_drift_limit * 8; } // return the smoothed gyro vector corrected for drift @@ -46,20 +46,18 @@ public: AP_Float _kp_yaw; private: - float _kp; - float _ki; + float _kp_roll_pitch; + float _ki_roll_pitch; 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 @@ -68,16 +66,14 @@ 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; // proportional yaw correction + Vector3f _omega_P; // accel Omega Proportional correction + Vector3f _omega_yaw_P; // yaw Omega Proportional correction Vector3f _omega_I; // Omega Integrator correction - Vector3f _omega_I_sum; + Vector3f _omega_I_sum; // summation vector for omegaI 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; @@ -90,15 +86,6 @@ 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