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

Compass: implement Bills new offset nulling algorithm 

this seems to work much better than the old algorithm, converging
faster and more accurately. Even better, it has no linkage to DCM, so
no possibility of nasty feedback effects
This commit is contained in:
Andrew Tridgell 2012-03-27 15:16:00 +11:00
parent aea780e5ea
commit a3d3dd86cf
2 changed files with 48 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
libraries/AP_Compass/Compass.cpp
View File

@ -182,38 +182,62 @@ Compass::calculate(const Matrix3f &dcm_matrix)
#endif
}
/*
this offset nulling algorithm is based on a paper from
Bill Premerlani
http://gentlenav.googlecode.com/files/MagnetometerOffsetNullingRevisited.pdf
*/
void
Compass::null_offsets(const Matrix3f &dcm_matrix)
Compass::null_offsets(void)
{
if (_null_enable == false || _learn == 0) {
// auto-calibration is disabled
return;
}
// Update our estimate of the offsets in the magnetometer
Vector3f calc;
Matrix3f dcm_new_from_last;
float weight;
Vector3f mag_body_new = Vector3f(mag_x,mag_y,mag_z);
if(_null_init_done) {
dcm_new_from_last = dcm_matrix.transposed() * _last_dcm_matrix; // Note 11/20/2010: transpose() is not working, transposed() is.
weight = 3.0 - fabs(dcm_new_from_last.a.x) - fabs(dcm_new_from_last.b.y) - fabs(dcm_new_from_last.c.z);
if (weight > .001) {
calc = mag_body_new + _mag_body_last; // Eq 11 from Bill P's paper
calc -= dcm_new_from_last * _mag_body_last;
calc -= dcm_new_from_last.transposed() * mag_body_new;
if(weight > 0.5) weight = 0.5;
calc = calc * (weight);
_offset.set(_offset.get() - calc);
}
} else {
// this gain is set so we converge on the offsets in about 5
// minutes with a 10Hz compass
const float gain = 0.5;
const float max_change = 2.0;
if (!_null_init_done) {
// first time through
_null_init_done = true;
_mag_body_last = mag_body_new;
return;
}
_mag_body_last = mag_body_new - calc;
_last_dcm_matrix = dcm_matrix;
Vector3f delta, diff;
float diff_length, delta_length;
diff = mag_body_new - _mag_body_last;
diff_length = diff.length();
if (diff_length == 0.0) {
// the mag vector hasn't changed - we don't get any
// information from this
return;
}
// equation 6 of Bills paper
delta = diff * (mag_body_new.length() - _mag_body_last.length()) / diff_length;
// limit the change from any one reading. This is to prevent
// single crazy readings from throwing off the offsets for a long
// time
delta_length = delta.length();
if (delta_length > max_change) {
delta *= max_change / delta_length;
}
// set the new offsets
_offset.set(_offset.get() - (delta * gain));
// remember the last mag vector
_mag_body_last = mag_body_new;
}

9
libraries/AP_Compass/Compass.h
View File

@ -97,11 +97,9 @@ public:
///
void set_offsets(int x, int y, int z) { set_offsets(Vector3f(x, y, z)); }
/// Perform automatic offset updates using the results of the DCM matrix.
/// Perform automatic offset updates
///
/// @param dcm_matrix The DCM result matrix.
///
void null_offsets(const Matrix3f &dcm_matrix);
void null_offsets(void);
/// Enable/Start automatic offset updates
@ -134,7 +132,6 @@ protected:
bool _null_enable; ///< enabled flag for offset nulling
bool _null_init_done; ///< first-time-around flag used by offset nulling
Matrix3f _last_dcm_matrix; ///< previous DCM matrix used by offset nulling
Vector3f _mag_body_last; ///< ?? used by offset nulling
Vector3f _mag_body_last; ///< used by offset correction
};
#endif