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

DCM: don't use the z accel sensor for drift correction 

the z accel is the noisest, and seems to do more harm than good. Using
just x and y is sufficient for drift correction by assuming the vector
length
This commit is contained in:
Andrew Tridgell 2012-03-01 22:56:42 +11:00
parent a78b00513b
commit 55413bfcc1
1 changed files with 31 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/AP_DCM/AP_DCM.cpp
View File

@ -369,29 +369,41 @@ AP_DCM::drift_correction(void)
//float mag_heading_x; //float mag_heading_x;
//float mag_heading_y; //float mag_heading_y;
float error_course = 0; float error_course = 0;
float accel_magnitude;
float accel_weight; float accel_weight;
float integrator_magnitude; float integrator_magnitude;
Vector3f error; Vector3f error;
float error_norm; float error_norm;
const float gravity_squared = (9.80665*9.80665);
//static float scaled_omega_P[3]; //static float scaled_omega_P[3];
//static float scaled_omega_I[3]; //static float scaled_omega_I[3];
//*****Roll and Pitch*************** //*****Roll and Pitch***************
// Calculate the magnitude of the accelerometer vector // calculate the z component of the accel vector assuming it
accel_magnitude = _accel_vector.length() / 9.80665f; // has a length of 9.8. This discards the z accelerometer
// sensor reading completely. Logs show that the z accel is
// the noisest, and it seems that using just the x and y accel
// values gives a better attitude estimate than including the
// z accel
// Dynamic weighting of accelerometer info (reliability filter) float zsquared = gravity_squared - ((_accel_vector.x * _accel_vector.x) + (_accel_vector.y * _accel_vector.y));
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0) if (zsquared < 0) {
accel_weight = constrain(1 - _clamp * fabs(1 - accel_magnitude), 0, 1); // upped to (<0.66G = 0.0, 1G = 1.0 , >1.33G = 0.0) accel_weight = 0;
} else {
if (_accel_vector.z > 0) {
_accel_vector.z = sqrt(zsquared);
} else {
_accel_vector.z = -sqrt(zsquared);
}
// this is arbitrary, and can be removed once we get
// ki and kp right
accel_weight = 0.6;
// We monitor the amount that the accelerometer based drift correction is deweighted for performance reporting
_health = constrain(_health+(0.02 * (accel_weight - .5)), 0, 1); _health = constrain(_health+(0.02 * (accel_weight - .5)), 0, 1);
// adjust the ground of reference error = _dcm_matrix.c % _accel_vector;
error = _dcm_matrix.c % _accel_vector; // Equation 27 *** sign changed from prev implementation???
// error_roll_pitch are in Accel m/s^2 units // error_roll_pitch are in Accel m/s^2 units
// Limit max error_roll_pitch to limit max omega_P and omega_I // Limit max error_roll_pitch to limit max omega_P and omega_I
@ -402,6 +414,7 @@ AP_DCM::drift_correction(void)
_omega_P = error * (_kp_roll_pitch * accel_weight); _omega_P = error * (_kp_roll_pitch * accel_weight);
_omega_I += error * (_ki_roll_pitch * accel_weight); _omega_I += error * (_ki_roll_pitch * accel_weight);
}
// these sums support the reporting of the DCM state via MAVLink // these sums support the reporting of the DCM state via MAVLink
_accel_weight_sum += accel_weight; _accel_weight_sum += accel_weight;