mirror of https://github.com/ArduPilot/ardupilot
Compass: implement noise resistant varient of offset learning
This adds a large amount of noise robustness to the compass offset learning algorithm, at a cost of 120 bytes of memory. The changes are based on a long discussion with Bill Premerlani.
This commit is contained in:
parent
c3a82247d3
commit
2a011578f9
|
@ -184,10 +184,24 @@ Compass::calculate(const Matrix3f &dcm_matrix)
|
|||
|
||||
|
||||
/*
|
||||
this offset nulling algorithm is based on a paper from
|
||||
Bill Premerlani
|
||||
this offset nulling algorithm is inspired by this paper from Bill Premerlani
|
||||
|
||||
http://gentlenav.googlecode.com/files/MagnetometerOffsetNullingRevisited.pdf
|
||||
|
||||
The base algorithm works well, but is quite sensitive to
|
||||
noise. After long discussions with Bill, the following changes were
|
||||
made:
|
||||
|
||||
1) we keep a history buffer that effectively divides the mag
|
||||
vectors into a set of N streams. The algorithm is run on the
|
||||
streams separately
|
||||
|
||||
2) within each stream we only calculate a change when the mag
|
||||
vector has changed by a significant amount.
|
||||
|
||||
This gives us the property that we learn quickly if there is no
|
||||
noise, but still learn correctly (and slowly) in the face of lots of
|
||||
noise.
|
||||
*/
|
||||
void
|
||||
Compass::null_offsets(void)
|
||||
|
@ -197,47 +211,67 @@ Compass::null_offsets(void)
|
|||
return;
|
||||
}
|
||||
|
||||
Vector3f mag_body_new = Vector3f(mag_x,mag_y,mag_z);
|
||||
|
||||
// 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;
|
||||
const float gain = 0.01;
|
||||
const float max_change = 10.0;
|
||||
const float min_diff = 50.0;
|
||||
Vector3f ofs;
|
||||
|
||||
ofs = _offset.get();
|
||||
|
||||
if (!_null_init_done) {
|
||||
// first time through
|
||||
_null_init_done = true;
|
||||
_mag_body_last = mag_body_new;
|
||||
for (uint8_t i=0; i<_mag_history_size; i++) {
|
||||
// fill the history buffer with the current mag vector,
|
||||
// with the offset removed
|
||||
_mag_history[i] = Vector3i((mag_x+0.5) - ofs.x, (mag_y+0.5) - ofs.y, (mag_z+0.5) - ofs.z);
|
||||
}
|
||||
_mag_history_index = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
Vector3f delta, diff;
|
||||
float diff_length, delta_length;
|
||||
Vector3f b1, b2, diff;
|
||||
float 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
|
||||
// get a past element
|
||||
b1 = Vector3f(_mag_history[_mag_history_index].x,
|
||||
_mag_history[_mag_history_index].y,
|
||||
_mag_history[_mag_history_index].z);
|
||||
// the history buffer doesn't have the offsets
|
||||
b1 += ofs;
|
||||
|
||||
// get the current vector
|
||||
b2 = Vector3f(mag_x, mag_y, mag_z);
|
||||
|
||||
// calculate the delta for this sample
|
||||
diff = b2 - b1;
|
||||
length = diff.length();
|
||||
if (length < min_diff) {
|
||||
// the mag vector hasn't changed enough - we don't get
|
||||
// enough information from this vector to use it
|
||||
_mag_history_index = (_mag_history_index + 1) % _mag_history_size;
|
||||
return;
|
||||
}
|
||||
|
||||
// put the vector in the history
|
||||
_mag_history[_mag_history_index] = Vector3i((mag_x+0.5) - ofs.x, (mag_y+0.5) - ofs.y, (mag_z+0.5) - ofs.z);
|
||||
_mag_history_index = (_mag_history_index + 1) % _mag_history_size;
|
||||
|
||||
// equation 6 of Bills paper
|
||||
delta = diff * (mag_body_new.length() - _mag_body_last.length()) / diff_length;
|
||||
diff = diff * (gain * (b2.length() - b1.length()) / 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;
|
||||
length = diff.length();
|
||||
if (length > max_change) {
|
||||
diff *= max_change / length;
|
||||
}
|
||||
|
||||
// set the new offsets
|
||||
_offset.set(_offset.get() - (delta * gain));
|
||||
|
||||
// remember the last mag vector
|
||||
_mag_body_last = mag_body_new;
|
||||
_offset.set(_offset.get() - diff);
|
||||
}
|
||||
|
||||
|
||||
|
|
|
@ -132,6 +132,10 @@ protected:
|
|||
|
||||
bool _null_enable; ///< enabled flag for offset nulling
|
||||
bool _null_init_done; ///< first-time-around flag used by offset nulling
|
||||
Vector3f _mag_body_last; ///< used by offset correction
|
||||
|
||||
///< used by offset correction
|
||||
static const uint8_t _mag_history_size = 20;
|
||||
uint8_t _mag_history_index;
|
||||
Vector3i _mag_history[_mag_history_size];
|
||||
};
|
||||
#endif
|
||||
|
|
Loading…
Reference in New Issue