Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 10:28: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 // the limit of the gyro drift claimed by the sensors, in
// radians/s/s // radians/s/s
float _gyro_drift_limit; float _gyro_drift_limit;
// acceleration due to gravity in m/s/s
static const float _gravity = 9.80665;
}; };
#include <AP_AHRS_DCM.h> #include <AP_AHRS_DCM.h>

31
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -279,7 +279,6 @@ AP_AHRS_DCM::drift_correction(float deltat)
Vector3f accel; Vector3f accel;
Vector3f error; Vector3f error;
float error_norm = 0; float error_norm = 0;
const float gravity_squared = (9.80665*9.80665);
float yaw_deltat = 0; float yaw_deltat = 0;
accel = _accel_vector; accel = _accel_vector;
@ -295,22 +294,17 @@ AP_AHRS_DCM::drift_correction(float deltat)
//*****Roll and Pitch*************** //*****Roll and Pitch***************
// calculate the z component of the accel vector assuming it // normalise the accelerometer vector to a standard length
// has a length of 9.8. This discards the z accelerometer // this is important to reduce the impact of noise on the
// sensor reading completely. Logs show that the z accel is // drift correction, as very noisy vectors tend to have
// the noisest, plus it has a disproportionate impact on the // abnormally high lengths. By normalising the length we
// drift correction result because of the geometry when we are // reduce their impact.
// mostly flat. Dropping it completely seems to make the DCM float accel_length = accel.length();
// algorithm much more resilient to large amounts of accel *= (_gravity / accel_length);
// accelerometer noise. if (accel.is_inf()) {
float zsquared = gravity_squared - ((accel.x * accel.x) + (accel.y * accel.y)); // we can't do anything useful with this sample
if (zsquared < 0) {
_omega_P.zero(); _omega_P.zero();
} else { return;
if (accel.z > 0) {
accel.z = sqrt(zsquared);
} else {
accel.z = -sqrt(zsquared);
} }
// calculate the error, in m/2^2, between the attitude // calculate the error, in m/2^2, between the attitude
@ -318,8 +312,6 @@ AP_AHRS_DCM::drift_correction(float deltat)
// in the current DCM matrix // in the current DCM matrix
error = _dcm_matrix.c % accel; 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 // Limit max error to limit the effect of noisy values
// on the algorithm. This limits the error to about 11 // on the algorithm. This limits the error to about 11
// degrees // degrees
@ -346,7 +338,6 @@ AP_AHRS_DCM::drift_correction(float deltat)
omega_I_delta.z = constrain(omega_I_delta.z, -drift_limit, drift_limit); omega_I_delta.z = constrain(omega_I_delta.z, -drift_limit, drift_limit);
_omega_I += omega_I_delta; _omega_I += omega_I_delta;
}
// these sums support the reporting of the DCM state via MAVLink // these sums support the reporting of the DCM state via MAVLink
_error_rp_sum += error_norm; _error_rp_sum += error_norm;
@ -470,7 +461,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
float omega_Iz_delta = error.z * (_ki_yaw * yaw_deltat); float omega_Iz_delta = error.z * (_ki_yaw * yaw_deltat);
// limit the slope of omega_I.z to the maximum gyro drift rate // 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_Iz_delta = constrain(omega_Iz_delta, -drift_limit, drift_limit);
_omega_I.z += omega_Iz_delta; _omega_I.z += omega_Iz_delta;