Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

DCM: drop the 'drop z' method

Browse Source 
the 'drop z' method reduced the impact of noise on omegaI, but it also
made us more sensitive to errors in accelerometer calibration and
scaling, as demonstated by the logs from Gabor here:

  http://diydrones.com/xn/detail/705844:Comment:834373

Simulation testing shows that the other noise suppression methods
applied in the DCM code, in particular the slope limiting on omegaI
the removal of the weighting and the upcoming use of a _omega_I_sum
buffer have reduced the impact of noise enough that we can now safely
include z in the acceleration calculation.
This commit is contained in:
Andrew Tridgell 2012-04-16 10:26:16 +10:00
parent cb863dd483
commit 398a608b83
2 changed files with 46 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_AHRS/AP_AHRS.h
View File

@ -102,6 +102,9 @@ protected:
// the limit of the gyro drift claimed by the sensors, in
// radians/s/s
float _gyro_drift_limit;
// acceleration due to gravity in m/s/s
static const float _gravity = 9.80665;
};
#include <AP_AHRS_DCM.h>

95
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -279,7 +279,6 @@ AP_AHRS_DCM::drift_correction(float deltat)
Vector3f accel;
Vector3f error;
float error_norm = 0;
const float gravity_squared = (9.80665*9.80665);
float yaw_deltat = 0;
accel = _accel_vector;
@ -295,59 +294,51 @@ AP_AHRS_DCM::drift_correction(float deltat)
//*****Roll and Pitch***************
// calculate the z component of the accel vector assuming it
// has a length of 9.8. This discards the z accelerometer
// sensor reading completely. Logs show that the z accel is
// the noisest, plus it has a disproportionate impact on the
// drift correction result because of the geometry when we are
// mostly flat. Dropping it completely seems to make the DCM
// algorithm much more resilient to large amounts of
// accelerometer noise.
float zsquared = gravity_squared - ((accel.x * accel.x) + (accel.y * accel.y));
if (zsquared < 0) {
// 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();
} else {
if (accel.z > 0) {
accel.z = sqrt(zsquared);
} else {
accel.z = -sqrt(zsquared);
}
// 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;
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.
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++;
@ -470,7 +461,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
float omega_Iz_delta = error.z * (_ki_yaw * yaw_deltat);
// limit the slope of omega_I.z to the maximum gyro drift rate
float drift_limit = _gyro_drift_limit * yaw_deltat;
drift_limit = _gyro_drift_limit * yaw_deltat;
omega_Iz_delta = constrain(omega_Iz_delta, -drift_limit, drift_limit);
_omega_I.z += omega_Iz_delta;