mirror of https://github.com/ArduPilot/ardupilot
AHRS: implement Bills new drift correction algorithm
this is an initial implementation of this paper: http://gentlenav.googlecode.com/files/RollPitchDriftCompensation.pdf
This commit is contained in:
parent
52d77407d4
commit
0f5c22bd8e
|
@ -38,8 +38,7 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
// Accessors
|
// Accessors
|
||||||
void set_centripetal(bool b) { _centripetal = b; }
|
void set_fly_forward(bool b) { _fly_forward = b; }
|
||||||
bool get_centripetal(void) { return _centripetal; }
|
|
||||||
void set_compass(Compass *compass) { _compass = compass; }
|
void set_compass(Compass *compass) { _compass = compass; }
|
||||||
|
|
||||||
// Methods
|
// Methods
|
||||||
|
@ -96,8 +95,9 @@ protected:
|
||||||
IMU *_imu;
|
IMU *_imu;
|
||||||
GPS *&_gps;
|
GPS *&_gps;
|
||||||
|
|
||||||
// true if we are doing centripetal acceleration correction
|
// true if we can assume the aircraft will be flying forward
|
||||||
bool _centripetal;
|
// on its X axis
|
||||||
|
bool _fly_forward;
|
||||||
|
|
||||||
// the limit of the gyro drift claimed by the sensors, in
|
// the limit of the gyro drift claimed by the sensors, in
|
||||||
// radians/s/s
|
// radians/s/s
|
||||||
|
|
|
@ -70,7 +70,7 @@ AP_AHRS_DCM::update(void)
|
||||||
void
|
void
|
||||||
AP_AHRS_DCM::matrix_update(float _G_Dt)
|
AP_AHRS_DCM::matrix_update(float _G_Dt)
|
||||||
{
|
{
|
||||||
// _omega_integ_corr is used for _centripetal correction
|
// _omega_integ_corr is used for centripetal correction
|
||||||
// (theoretically better than _omega)
|
// (theoretically better than _omega)
|
||||||
_omega_integ_corr = _gyro_vector + _omega_I;
|
_omega_integ_corr = _gyro_vector + _omega_I;
|
||||||
|
|
||||||
|
@ -90,29 +90,6 @@ AP_AHRS_DCM::matrix_update(float _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
|
reset the DCM matrix and omega. Used on ground start, and on
|
||||||
extreme errors in the matrix
|
extreme errors in the matrix
|
||||||
|
@ -269,83 +246,18 @@ AP_AHRS_DCM::normalize(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// perform drift correction. This function aims to update _omega_P and
|
// yaw drift correction
|
||||||
// _omega_I with our best estimate of the short term and long term
|
// we only do yaw drift correction when we get a new yaw
|
||||||
// gyro error. The _omega_P value is what pulls our attitude solution
|
// reference vector. In between times we rely on the gyros for
|
||||||
// back towards the reference vector quickly. The _omega_I term is an
|
// yaw. Avoiding this calculation on every call to
|
||||||
// attempt to learn the long term drift rate of the gyros.
|
// update() saves a lot of time
|
||||||
//
|
|
||||||
// This function also updates _omega_yaw_P with a yaw correction term
|
|
||||||
// from our yaw reference vector
|
|
||||||
void
|
void
|
||||||
AP_AHRS_DCM::drift_correction(float deltat)
|
AP_AHRS_DCM::drift_correction_yaw(float deltat)
|
||||||
{
|
{
|
||||||
float error_course = 0;
|
|
||||||
Vector3f accel;
|
|
||||||
Vector3f error;
|
Vector3f error;
|
||||||
float error_norm = 0;
|
float yaw_deltat;
|
||||||
float yaw_deltat = 0;
|
float error_course = 0;
|
||||||
|
|
||||||
accel = _accel_vector;
|
|
||||||
|
|
||||||
// if enabled, use the GPS to correct our accelerometer vector
|
|
||||||
// for centripetal forces
|
|
||||||
if(_centripetal &&
|
|
||||||
_gps != NULL &&
|
|
||||||
_gps->status() == GPS::GPS_OK) {
|
|
||||||
accel_adjust(accel);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
//*****Roll and Pitch***************
|
|
||||||
|
|
||||||
// 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;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 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;
|
|
||||||
|
|
||||||
// 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);
|
|
||||||
}
|
|
||||||
|
|
||||||
// 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;
|
|
||||||
|
|
||||||
// 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 && _compass->use_for_yaw()) {
|
||||||
if (_compass->last_update != _compass_last_update) {
|
if (_compass->last_update != _compass_last_update) {
|
||||||
yaw_deltat = 1.0e-6*(_compass->last_update - _compass_last_update);
|
yaw_deltat = 1.0e-6*(_compass->last_update - _compass_last_update);
|
||||||
|
@ -375,7 +287,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
|
||||||
error_course = 0;
|
error_course = 0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
} else if (_gps && _gps->status() == GPS::GPS_OK) {
|
} else if (_gps && _gps->status() == GPS::GPS_OK && _fly_forward) {
|
||||||
if (_gps->last_fix_time != _gps_last_update) {
|
if (_gps->last_fix_time != _gps_last_update) {
|
||||||
// Use GPS Ground course to correct yaw gyro drift
|
// Use GPS Ground course to correct yaw gyro drift
|
||||||
if (_gps->ground_speed >= GPS_SPEED_MIN) {
|
if (_gps->ground_speed >= GPS_SPEED_MIN) {
|
||||||
|
@ -485,6 +397,190 @@ check_sum_time:
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// this is our 'old' drift compensation method, left in here for now
|
||||||
|
// to make it easy to switch between them for comparison purposes. We
|
||||||
|
// also use it when we don't have a working GPS
|
||||||
|
void
|
||||||
|
AP_AHRS_DCM::drift_correction_old(float deltat)
|
||||||
|
{
|
||||||
|
Vector3f accel;
|
||||||
|
Vector3f error;
|
||||||
|
float error_norm = 0;
|
||||||
|
|
||||||
|
accel = _accel_vector;
|
||||||
|
|
||||||
|
// if enabled, use the GPS to correct our accelerometer vector
|
||||||
|
// for centripetal forces
|
||||||
|
if(_fly_forward &&
|
||||||
|
_gps != NULL &&
|
||||||
|
_gps->status() == GPS::GPS_OK) {
|
||||||
|
// account for linear acceleration on the X axis
|
||||||
|
float acceleration = _gps->acceleration();
|
||||||
|
accel.x -= acceleration;
|
||||||
|
|
||||||
|
// this is the old DCM drift correction method for
|
||||||
|
// centripetal forces. It assumes the aircraft is
|
||||||
|
// flying along its X axis
|
||||||
|
float velocity = _gps->ground_speed * 0.01;
|
||||||
|
accel.y -= _omega_integ_corr.z * velocity;
|
||||||
|
accel.z += _omega_integ_corr.y * velocity;
|
||||||
|
}
|
||||||
|
|
||||||
|
float accel_norm = accel.length();
|
||||||
|
if (accel_norm < 1) {
|
||||||
|
// we have no useful information about our attitude
|
||||||
|
// from this sample
|
||||||
|
_omega_P.zero();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// normalise the acceleration vector
|
||||||
|
accel *= (_gravity / accel_norm);
|
||||||
|
|
||||||
|
// 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;
|
||||||
|
|
||||||
|
// error from the above is in m/s^2 units.
|
||||||
|
|
||||||
|
// 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);
|
||||||
|
}
|
||||||
|
|
||||||
|
// 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;
|
||||||
|
|
||||||
|
// the _omega_I is the long term accumulated gyro
|
||||||
|
// error. This determines how much gyro drift we can
|
||||||
|
// handle.
|
||||||
|
Vector3f omega_I_delta = error * (_ki_roll_pitch * deltat);
|
||||||
|
|
||||||
|
// limit the slope of omega_I on each axis to
|
||||||
|
// the maximum drift rate
|
||||||
|
float drift_limit = _gyro_drift_limit * deltat;
|
||||||
|
omega_I_delta.x = constrain(omega_I_delta.x, -drift_limit, drift_limit);
|
||||||
|
omega_I_delta.y = constrain(omega_I_delta.y, -drift_limit, drift_limit);
|
||||||
|
omega_I_delta.z = constrain(omega_I_delta.z, -drift_limit, drift_limit);
|
||||||
|
|
||||||
|
_omega_I += omega_I_delta;
|
||||||
|
|
||||||
|
// these sums support the reporting of the DCM state via MAVLink
|
||||||
|
_error_rp_sum += error_norm;
|
||||||
|
_error_rp_count++;
|
||||||
|
|
||||||
|
// perform yaw drift correction if we have a new yaw reference
|
||||||
|
// vector
|
||||||
|
drift_correction_yaw(deltat);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// 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
|
||||||
|
void
|
||||||
|
AP_AHRS_DCM::drift_correction(float deltat)
|
||||||
|
{
|
||||||
|
Vector3f error;
|
||||||
|
float error_norm;
|
||||||
|
|
||||||
|
// if we don't have a working GPS then use the old style
|
||||||
|
// of drift correction
|
||||||
|
if (_gps == NULL || _gps->status() != GPS::GPS_OK) {
|
||||||
|
drift_correction_old(deltat);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// perform yaw drift correction if we have a new yaw reference
|
||||||
|
// vector
|
||||||
|
drift_correction_yaw(deltat);
|
||||||
|
|
||||||
|
// integrate the accel vector in the body 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;
|
||||||
|
|
||||||
|
// see if we have a new GPS reading
|
||||||
|
if (_gps->last_fix_time == _ra_sum_start) {
|
||||||
|
// we don't have a new GPS fix - nothing more to do
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// get GPS velocity vector in earth frame
|
||||||
|
Vector3f gps_velocity = Vector3f(_gps->velocity_north(), _gps->velocity_east(), 0);
|
||||||
|
|
||||||
|
// 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 = _gps->last_fix_time;
|
||||||
|
_gps_last_velocity = gps_velocity;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// get the corrected acceleration vector in earth frame
|
||||||
|
Vector3f ge = Vector3f(0,0, - (_gravity * _ra_deltat)) + (gps_velocity - _gps_last_velocity);
|
||||||
|
|
||||||
|
// calculate the error term in earth frame
|
||||||
|
error = _ra_sum % ge;
|
||||||
|
|
||||||
|
// limit the length of the error
|
||||||
|
error_norm = error.length();
|
||||||
|
if (error_norm > 2.0) {
|
||||||
|
error *= (2.0 / error_norm);
|
||||||
|
}
|
||||||
|
|
||||||
|
// convert the error term to body frame
|
||||||
|
error = _dcm_matrix.mul_transpose(error);
|
||||||
|
|
||||||
|
// 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;
|
||||||
|
|
||||||
|
// the _omega_I is the long term accumulated gyro
|
||||||
|
// error. This determines how much gyro drift we can
|
||||||
|
// handle.
|
||||||
|
Vector3f omega_I_delta = error * (_ki_roll_pitch * _ra_deltat);
|
||||||
|
|
||||||
|
// limit the slope of omega_I on each axis to
|
||||||
|
// the maximum drift rate
|
||||||
|
float drift_limit = _gyro_drift_limit * _ra_deltat;
|
||||||
|
omega_I_delta.x = constrain(omega_I_delta.x, -drift_limit, drift_limit);
|
||||||
|
omega_I_delta.y = constrain(omega_I_delta.y, -drift_limit, drift_limit);
|
||||||
|
omega_I_delta.z = constrain(omega_I_delta.z, -drift_limit, drift_limit);
|
||||||
|
|
||||||
|
// add in the limited omega correction into the long term
|
||||||
|
// drift correction
|
||||||
|
_omega_I += omega_I_delta;
|
||||||
|
|
||||||
|
// zero our accumulator ready for the next GPS step
|
||||||
|
_ra_sum.zero();
|
||||||
|
_ra_deltat = 0;
|
||||||
|
_ra_sum_start = _gps->last_fix_time;
|
||||||
|
|
||||||
|
// remember the GPS velocity for next time
|
||||||
|
_gps_last_velocity = gps_velocity;
|
||||||
|
|
||||||
|
// these sums support the reporting of the DCM state via MAVLink
|
||||||
|
_error_rp_sum += error_norm;
|
||||||
|
_error_rp_count++;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
// calculate the euler angles which will be used for high level
|
// calculate the euler angles which will be used for high level
|
||||||
// navigation control
|
// navigation control
|
||||||
|
|
|
@ -16,15 +16,13 @@ public:
|
||||||
// Constructors
|
// Constructors
|
||||||
AP_AHRS_DCM(IMU *imu, GPS *&gps) : AP_AHRS(imu, gps)
|
AP_AHRS_DCM(IMU *imu, GPS *&gps) : AP_AHRS(imu, gps)
|
||||||
{
|
{
|
||||||
_kp_roll_pitch = 0.13;
|
|
||||||
_kp_yaw.set(0.2);
|
_kp_yaw.set(0.2);
|
||||||
_dcm_matrix(Vector3f(1, 0, 0),
|
_kp_roll_pitch = 0.06;
|
||||||
Vector3f(0, 1, 0),
|
_dcm_matrix.identity();
|
||||||
Vector3f(0, 0, 1));
|
|
||||||
|
|
||||||
// base the ki values on the sensors drift rate
|
// base the ki values on the sensors drift rate
|
||||||
_ki_roll_pitch = _gyro_drift_limit * 5;
|
_ki_roll_pitch = _gyro_drift_limit * 4;
|
||||||
_ki_yaw = _gyro_drift_limit * 8;
|
_ki_yaw = _gyro_drift_limit * 6;
|
||||||
}
|
}
|
||||||
|
|
||||||
// return the smoothed gyro vector corrected for drift
|
// return the smoothed gyro vector corrected for drift
|
||||||
|
@ -52,12 +50,13 @@ private:
|
||||||
bool _have_initial_yaw;
|
bool _have_initial_yaw;
|
||||||
|
|
||||||
// Methods
|
// Methods
|
||||||
void accel_adjust(Vector3f &accel);
|
|
||||||
void matrix_update(float _G_Dt);
|
void matrix_update(float _G_Dt);
|
||||||
void normalize(void);
|
void normalize(void);
|
||||||
void check_matrix(void);
|
void check_matrix(void);
|
||||||
bool renorm(Vector3f const &a, Vector3f &result);
|
bool renorm(Vector3f const &a, Vector3f &result);
|
||||||
void drift_correction(float deltat);
|
void drift_correction(float deltat);
|
||||||
|
void drift_correction_old(float deltat);
|
||||||
|
void drift_correction_yaw(float deltat);
|
||||||
void euler_angles(void);
|
void euler_angles(void);
|
||||||
|
|
||||||
// primary representation of attitude
|
// primary representation of attitude
|
||||||
|
@ -86,6 +85,11 @@ private:
|
||||||
|
|
||||||
// time in millis when we last got a GPS heading
|
// time in millis when we last got a GPS heading
|
||||||
uint32_t _gps_last_update;
|
uint32_t _gps_last_update;
|
||||||
|
|
||||||
|
Vector3f _ra_sum;
|
||||||
|
Vector3f _gps_last_velocity;
|
||||||
|
float _ra_deltat;
|
||||||
|
uint32_t _ra_sum_start;
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif // AP_AHRS_DCM_H
|
#endif // AP_AHRS_DCM_H
|
||||||
|
|
Loading…
Reference in New Issue