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

Quaternion: added more numerical safety in the quaternion code 

prevent infinities from creeping in and prevent large drift changes
This commit is contained in:
Andrew Tridgell 2012-03-05 15:07:04 +11:00
parent f2e6714598
commit 89a10c584d
2 changed files with 81 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

94
libraries/AP_Quaternion/AP_Quaternion.cpp
View File

@ -31,6 +31,13 @@ AP_Quaternion::set_compass(Compass *compass)
_compass = compass;
}
// to keep the code as close to the original as possible, we use these
// macros for quaternion access
#define SEq_1 q.q1
#define SEq_2 q.q2
#define SEq_3 q.q3
#define SEq_4 q.q4
// Function to compute one quaternion iteration without magnetometer
void AP_Quaternion::update_IMU(float deltat, Vector3f &gyro, Vector3f &accel)
{
@ -55,6 +62,11 @@ void AP_Quaternion::update_IMU(float deltat, Vector3f &gyro, Vector3f &accel)
// normalise accelerometer vector
accel.normalize();
if (accel.is_inf()) {
// discard this data point
renorm_range_count++;
return;
}
// Compute the objective function and Jacobian
f_1 = twoSEq_2 * SEq_4 - twoSEq_1 * SEq_3 - accel.x;
@ -75,12 +87,16 @@ void AP_Quaternion::update_IMU(float deltat, Vector3f &gyro, Vector3f &accel)
// Normalise the gradient
norm = 1.0/sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4);
if (!isinf(norm)) {
SEqHatDot_1 *= norm;
SEqHatDot_2 *= norm;
SEqHatDot_3 *= norm;
SEqHatDot_4 *= norm;
if (isinf(norm)) {
// we can't do an update - discard this data point and
// hope the next one is better
renorm_range_count++;
return;
}
SEqHatDot_1 *= norm;
SEqHatDot_2 *= norm;
SEqHatDot_3 *= norm;
SEqHatDot_4 *= norm;
// Compute the quaternion derrivative measured by gyroscopes
SEqDot_omega_1 = -halfSEq_2 * gyro.x - halfSEq_3 * gyro.y - halfSEq_4 * gyro.z;
@ -96,12 +112,17 @@ void AP_Quaternion::update_IMU(float deltat, Vector3f &gyro, Vector3f &accel)
// Normalise quaternion
norm = 1.0/sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4);
if (!isinf(norm)) {
SEq_1 *= norm;
SEq_2 *= norm;
SEq_3 *= norm;
SEq_4 *= norm;
if (isinf(norm)) {
// our quaternion is bad! Reset based on roll/pitch/yaw
// and hope for the best ...
renorm_blowup_count++;
quaternion_from_euler(q, roll, pitch, yaw);
return;
}
SEq_1 *= norm;
SEq_2 *= norm;
SEq_3 *= norm;
SEq_4 *= norm;
}
@ -124,9 +145,19 @@ void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, V
// normalise accelerometer vector
accel.normalize();
if (accel.is_inf()) {
// discard this data point
renorm_range_count++;
return;
}
// normalise the magnetometer measurement
mag.normalize();
if (mag.is_inf()) {
// discard this data point
renorm_range_count++;
return;
}
// auxiliary variables to avoid repeated calculations
float halfSEq_1 = 0.5f * SEq_1;
@ -189,6 +220,11 @@ void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, V
// normalise the gradient to estimate direction of the gyroscope error
norm = 1.0 / sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4);
if (isinf(norm)) {
// discard this data point
renorm_range_count++;
return;
}
SEqHatDot_1 *= norm;
SEqHatDot_2 *= norm;
SEqHatDot_3 *= norm;
@ -205,8 +241,18 @@ void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, V
_error_rp_count++;
_error_yaw_count++;
// compute and remove the gyroscope baises
gyro_bias += w_err * (deltat * zeta);
// compute the gyroscope bias delta
Vector3f drift_delta = w_err * (deltat * zeta);
// don't allow the drift rate to be exceeded. This prevents a
// sudden drift change coming from a outage in the compass
float max_change = gyroMeasDrift * deltat;
drift_delta.x = constrain(drift_delta.x, -max_change, max_change);
drift_delta.y = constrain(drift_delta.y, -max_change, max_change);
drift_delta.z = constrain(drift_delta.z, -max_change, max_change);
gyro_bias += drift_delta;
// correct the gyro reading for drift
gyro -= gyro_bias;
// compute the quaternion rate measured by gyroscopes
@ -223,12 +269,17 @@ void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, V
// normalise quaternion
norm = 1.0/sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4);
if (!isinf(norm)) {
SEq_1 *= norm;
SEq_2 *= norm;
SEq_3 *= norm;
SEq_4 *= norm;
if (isinf(norm)) {
// our quaternion is bad! Reset based on roll/pitch/yaw
// and hope for the best ...
renorm_blowup_count++;
quaternion_from_euler(q, roll, pitch, yaw);
return;
}
SEq_1 *= norm;
SEq_2 *= norm;
SEq_3 *= norm;
SEq_4 *= norm;
// compute flux in the earth frame
// recompute axulirary variables
@ -265,6 +316,13 @@ void AP_Quaternion::update(void)
return;
}
if (!_have_initial_yaw && _compass && _compass->use_for_yaw()) {
// setup the quaternion with initial compass yaw
quaternion_from_euler(q, 0, 0, _compass->heading);
_have_initial_yaw = true;
gyro_bias.zero();
}
// get current IMU state
gyro = _imu->get_gyro();
accel = _imu->get_accel();
@ -293,7 +351,7 @@ void AP_Quaternion::update(void)
accel.z += (gyro.y - gyro_bias.y) * veloc;
}
if (_compass == NULL) {
if (_compass == NULL || !_compass->use_for_yaw()) {
update_IMU(deltat, gyro, accel);
} else {
// mag.z is reversed in quaternion code

14
libraries/AP_Quaternion/AP_Quaternion.h
View File

@ -22,12 +22,6 @@ public:
_gps(gps),
_compass(compass)
{
// initial quaternion
SEq_1 = 1;
SEq_2 = 0;
SEq_3 = 0;
SEq_4 = 0;
// reference direction of flux in earth frame
b_x = 0;
b_z = -1;
@ -97,8 +91,8 @@ private:
// true if we are doing centripetal acceleration correction
bool _centripetal;
// maximum gyroscope measurement error in rad/s (set to 10 degrees/second)
static const float gyroMeasError = 10.0 * (M_PI/180.0);
// maximum gyroscope measurement error in rad/s (set to 20 degrees/second)
static const float gyroMeasError = 20.0 * (M_PI/180.0);
// maximum gyroscope drift rate in radians/s/s (set to 0.02
// degrees/s/s, which is 1.2 degrees/s/minute)
@ -108,8 +102,10 @@ private:
float zeta;
// quaternion elements
float SEq_1, SEq_2, SEq_3, SEq_4;
Quaternion q;
// magnetic flux estimates. These are used for the automatic
// magnetometer calibration
float b_x;
float b_z;