mirror of https://github.com/ArduPilot/ardupilot
Quaternion: separate out drift correction from main updates
This commit is contained in:
Andrew Tridgell
parent
3e960f8ef9
commit
8b40c44522
|
|
@ -116,7 +116,13 @@ void AP_Quaternion::update_IMU(float deltat, Vector3f &gyro, Vector3f &accel)
|
|||
// our quaternion is bad! Reset based on roll/pitch/yaw
|
||||
// and hope for the best ...
|
||||
renorm_blowup_count++;
|
||||
if (_compass) {
|
||||
_compass->null_offsets_disable();
|
||||
}
|
||||
quaternion_from_euler(q, roll, pitch, yaw);
|
||||
if (_compass) {
|
||||
_compass->null_offsets_enable();
|
||||
}
|
||||
return;
|
||||
}
|
||||
SEq_1 *= norm;
|
||||
|
|
@ -213,10 +219,10 @@ void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, V
|
|||
J_64 = twob_xSEq_2;
|
||||
|
||||
// compute the gradient (matrix multiplication)
|
||||
SEqHatDot_1 = J_14or21 * f_2 - J_11or24 * f_1 - J_41 * f_4 - J_51 * f_5 + J_61 * f_6;
|
||||
SEqHatDot_1 = J_14or21 * f_2 - J_11or24 * f_1 - J_41 * f_4 - J_51 * f_5 + J_61 * f_6;
|
||||
SEqHatDot_2 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3 + J_42 * f_4 + J_52 * f_5 + J_62 * f_6;
|
||||
SEqHatDot_3 = J_12or23 * f_2 - J_33 * f_3 - J_13or22 * f_1 - J_43 * f_4 + J_53 * f_5 + J_63 * f_6;
|
||||
SEqHatDot_4 = J_14or21 * f_1 + J_11or24 * f_2 - J_44 * f_4 - J_54 * f_5 + J_64 * f_6;
|
||||
SEqHatDot_4 = J_14or21 * f_1 + J_11or24 * f_2 - J_44 * f_4 - J_54 * f_5 + J_64 * f_6;
|
||||
|
||||
// normalise the gradient to estimate direction of the gyroscope error
|
||||
norm = 1.0 / safe_sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4);
|
||||
|
|
@ -273,7 +279,9 @@ void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, V
|
|||
// our quaternion is bad! Reset based on roll/pitch/yaw
|
||||
// and hope for the best ...
|
||||
renorm_blowup_count++;
|
||||
_compass->null_offsets_disable();
|
||||
quaternion_from_euler(q, roll, pitch, yaw);
|
||||
_compass->null_offsets_disable();
|
||||
return;
|
||||
}
|
||||
SEq_1 *= norm;
|
||||
|
|
@ -299,6 +307,151 @@ void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, V
|
|||
}
|
||||
|
||||
|
||||
// Function to compute one quaternion iteration including magnetometer
|
||||
void AP_Quaternion::update_drift(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag)
|
||||
{
|
||||
// local system variables
|
||||
float norm; // vector norm
|
||||
float SEqDot_omega_1, SEqDot_omega_2, SEqDot_omega_3, SEqDot_omega_4; // quaternion rate from gyroscopes elements
|
||||
float f_1, f_2, f_3, f_4, f_5, f_6; // objective function elements
|
||||
float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33, // objective function Jacobian elements
|
||||
J_41, J_42, J_43, J_44, J_51, J_52, J_53, J_54, J_61, J_62, J_63, J_64; //
|
||||
float SEqHatDot_1, SEqHatDot_2, SEqHatDot_3, SEqHatDot_4; // estimated direction of the gyroscope error
|
||||
|
||||
// computed flux in the earth frame
|
||||
Vector3f flux;
|
||||
|
||||
// estimated direction of the gyroscope error (radians)
|
||||
Vector3f w_err;
|
||||
|
||||
// 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;
|
||||
float halfSEq_2 = 0.5f * SEq_2;
|
||||
float halfSEq_3 = 0.5f * SEq_3;
|
||||
float halfSEq_4 = 0.5f * SEq_4;
|
||||
float twoSEq_1 = 2.0f * SEq_1;
|
||||
float twoSEq_2 = 2.0f * SEq_2;
|
||||
float twoSEq_3 = 2.0f * SEq_3;
|
||||
float twoSEq_4 = 2.0f * SEq_4;
|
||||
float twob_x = 2.0f * b_x;
|
||||
float twob_z = 2.0f * b_z;
|
||||
float twob_xSEq_1 = 2.0f * b_x * SEq_1;
|
||||
float twob_xSEq_2 = 2.0f * b_x * SEq_2;
|
||||
float twob_xSEq_3 = 2.0f * b_x * SEq_3;
|
||||
float twob_xSEq_4 = 2.0f * b_x * SEq_4;
|
||||
float twob_zSEq_1 = 2.0f * b_z * SEq_1;
|
||||
float twob_zSEq_2 = 2.0f * b_z * SEq_2;
|
||||
float twob_zSEq_3 = 2.0f * b_z * SEq_3;
|
||||
float twob_zSEq_4 = 2.0f * b_z * SEq_4;
|
||||
float SEq_1SEq_2;
|
||||
float SEq_1SEq_3 = SEq_1 * SEq_3;
|
||||
float SEq_1SEq_4;
|
||||
float SEq_2SEq_3;
|
||||
float SEq_2SEq_4 = SEq_2 * SEq_4;
|
||||
float SEq_3SEq_4;
|
||||
Vector3f twom = mag * 2.0;
|
||||
|
||||
// compute the objective function and Jacobian
|
||||
f_1 = twoSEq_2 * SEq_4 - twoSEq_1 * SEq_3 - accel.x;
|
||||
f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 - accel.y;
|
||||
f_3 = 1.0f - twoSEq_2 * SEq_2 - twoSEq_3 * SEq_3 - accel.z;
|
||||
f_4 = twob_x * (0.5f - SEq_3 * SEq_3 - SEq_4 * SEq_4) + twob_z * (SEq_2SEq_4 - SEq_1SEq_3) - mag.x;
|
||||
f_5 = twob_x * (SEq_2 * SEq_3 - SEq_1 * SEq_4) + twob_z * (SEq_1 * SEq_2 + SEq_3 * SEq_4) - mag.y;
|
||||
f_6 = twob_x * (SEq_1SEq_3 + SEq_2SEq_4) + twob_z * (0.5f - SEq_2 * SEq_2 - SEq_3 * SEq_3) - mag.z;
|
||||
J_11or24 = twoSEq_3; // J_11 negated in matrix multiplication
|
||||
J_12or23 = 2.0f * SEq_4;
|
||||
J_13or22 = twoSEq_1; // J_12 negated in matrix multiplication
|
||||
J_14or21 = twoSEq_2;
|
||||
J_32 = 2.0f * J_14or21; // negated in matrix multiplication
|
||||
J_33 = 2.0f * J_11or24; // negated in matrix multiplication
|
||||
J_41 = twob_zSEq_3; // negated in matrix multiplication
|
||||
J_42 = twob_zSEq_4;
|
||||
J_43 = 2.0f * twob_xSEq_3 + twob_zSEq_1; // negated in matrix multiplication
|
||||
J_44 = 2.0f * twob_xSEq_4 - twob_zSEq_2; // negated in matrix multiplication
|
||||
J_51 = twob_xSEq_4 - twob_zSEq_2; // negated in matrix multiplication
|
||||
J_52 = twob_xSEq_3 + twob_zSEq_1;
|
||||
J_53 = twob_xSEq_2 + twob_zSEq_4;
|
||||
J_54 = twob_xSEq_1 - twob_zSEq_3; // negated in matrix multiplication
|
||||
J_61 = twob_xSEq_3;
|
||||
J_62 = twob_xSEq_4 - 2.0f * twob_zSEq_2;
|
||||
J_63 = twob_xSEq_1 - 2.0f * twob_zSEq_3;
|
||||
J_64 = twob_xSEq_2;
|
||||
|
||||
// compute the gradient (matrix multiplication)
|
||||
SEqHatDot_1 = J_14or21 * f_2 - J_11or24 * f_1 - J_41 * f_4 - J_51 * f_5 + J_61 * f_6;
|
||||
SEqHatDot_2 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3 + J_42 * f_4 + J_52 * f_5 + J_62 * f_6;
|
||||
SEqHatDot_3 = J_12or23 * f_2 - J_33 * f_3 - J_13or22 * f_1 - J_43 * f_4 + J_53 * f_5 + J_63 * f_6;
|
||||
SEqHatDot_4 = J_14or21 * f_1 + J_11or24 * f_2 - J_44 * f_4 - J_54 * f_5 + J_64 * f_6;
|
||||
|
||||
// normalise the gradient to estimate direction of the gyroscope error
|
||||
norm = 1.0 / safe_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;
|
||||
SEqHatDot_4 *= norm;
|
||||
|
||||
// compute angular estimated direction of the gyroscope error
|
||||
w_err.x = twoSEq_1 * SEqHatDot_2 - twoSEq_2 * SEqHatDot_1 - twoSEq_3 * SEqHatDot_4 + twoSEq_4 * SEqHatDot_3;
|
||||
w_err.y = twoSEq_1 * SEqHatDot_3 + twoSEq_2 * SEqHatDot_4 - twoSEq_3 * SEqHatDot_1 - twoSEq_4 * SEqHatDot_2;
|
||||
w_err.z = twoSEq_1 * SEqHatDot_4 - twoSEq_2 * SEqHatDot_3 + twoSEq_3 * SEqHatDot_2 - twoSEq_4 * SEqHatDot_1;
|
||||
|
||||
// keep track of the error rates
|
||||
_error_rp_sum += 0.5*(fabs(w_err.x) + fabs(w_err.y));
|
||||
_error_yaw_sum += fabs(w_err.z);
|
||||
_error_rp_count++;
|
||||
_error_yaw_count++;
|
||||
|
||||
// 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;
|
||||
|
||||
// compute flux in the earth frame
|
||||
// recompute axulirary variables
|
||||
SEq_1SEq_2 = SEq_1 * SEq_2;
|
||||
SEq_1SEq_3 = SEq_1 * SEq_3;
|
||||
SEq_1SEq_4 = SEq_1 * SEq_4;
|
||||
SEq_3SEq_4 = SEq_3 * SEq_4;
|
||||
SEq_2SEq_3 = SEq_2 * SEq_3;
|
||||
SEq_2SEq_4 = SEq_2 * SEq_4;
|
||||
flux.x = twom.x * (0.5f - SEq_3 * SEq_3 - SEq_4 * SEq_4) + twom.y * (SEq_2SEq_3 - SEq_1SEq_4) + twom.z * (SEq_2SEq_4 + SEq_1SEq_3);
|
||||
flux.y = twom.x * (SEq_2SEq_3 + SEq_1SEq_4) + twom.y * (0.5f - SEq_2 * SEq_2 - SEq_4 * SEq_4) + twom.z * (SEq_3SEq_4 - SEq_1SEq_2);
|
||||
flux.z = twom.x * (SEq_2SEq_4 - SEq_1SEq_3) + twom.y * (SEq_3SEq_4 + SEq_1SEq_2) + twom.z * (0.5f - SEq_2 * SEq_2 - SEq_3 * SEq_3);
|
||||
|
||||
// normalise the flux vector to have only components in the x and z
|
||||
b_x = sqrt((flux.x * flux.x) + (flux.y * flux.y));
|
||||
b_z = flux.z;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// Function to compute one quaternion iteration
|
||||
void AP_Quaternion::update(void)
|
||||
{
|
||||
|
|
@ -321,11 +474,15 @@ void AP_Quaternion::update(void)
|
|||
return;
|
||||
}
|
||||
|
||||
if (!_have_initial_yaw && _compass && _compass->use_for_yaw()) {
|
||||
if (!_have_initial_yaw && _compass &&
|
||||
_compass->use_for_yaw()) {
|
||||
// setup the quaternion with initial compass yaw
|
||||
_compass->null_offsets_disable();
|
||||
quaternion_from_euler(q, 0, 0, _compass->heading);
|
||||
_have_initial_yaw = true;
|
||||
_compass_last_update = _compass->last_update;
|
||||
gyro_bias.zero();
|
||||
_compass->null_offsets_enable();
|
||||
}
|
||||
|
||||
// get current IMU state
|
||||
|
|
@ -356,14 +513,43 @@ void AP_Quaternion::update(void)
|
|||
accel.z += (gyro.y - gyro_bias.y) * veloc;
|
||||
}
|
||||
|
||||
if (_compass == NULL || !_compass->use_for_yaw()) {
|
||||
update_IMU(deltat, gyro, accel);
|
||||
} else {
|
||||
// mag.z is reversed in quaternion code
|
||||
#if 1
|
||||
_gyro_sum += gyro;
|
||||
_accel_sum += accel;
|
||||
_sum_count++;
|
||||
|
||||
if (_compass != NULL && _compass->use_for_yaw() &&
|
||||
_compass->last_update != _compass_last_update &&
|
||||
_sum_count != 0) {
|
||||
// use new compass sample for drift correction
|
||||
float mag_deltat = 1.0e-6*(_compass->last_update - _compass_last_update);
|
||||
Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, - _compass->mag_z);
|
||||
update_MARG(deltat, gyro, accel, mag);
|
||||
Vector3f drift_accel = _accel_sum / _sum_count;
|
||||
Vector3f drift_gyro = _gyro_sum / _sum_count;
|
||||
_accel_sum.zero();
|
||||
_gyro_sum.zero();
|
||||
_sum_count = 0;
|
||||
update_drift(mag_deltat, drift_gyro, drift_accel, mag);
|
||||
_compass_last_update = _compass->last_update;
|
||||
}
|
||||
|
||||
// step the quaternion solution
|
||||
gyro -= gyro_bias;
|
||||
update_IMU(deltat, gyro, accel);
|
||||
#else
|
||||
if (_compass != NULL && _compass->use_for_yaw()) {
|
||||
// use new compass sample for drift correction
|
||||
Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, - _compass->mag_z);
|
||||
update_MARG(deltat, gyro, accel, mag);
|
||||
} else {
|
||||
// step the quaternion without drift correction
|
||||
gyro -= gyro_bias;
|
||||
_gyro_sum += gyro;
|
||||
_accel_sum += accel;
|
||||
_sum_count++;
|
||||
update_IMU(deltat, gyro, accel);
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef DESKTOP_BUILD
|
||||
if (q.is_nan()) {
|
||||
|
|
|
|||
|
|
@ -17,10 +17,9 @@ class AP_Quaternion
|
|||
{
|
||||
public:
|
||||
// Constructor
|
||||
AP_Quaternion(IMU *imu, GPS *&gps, Compass *compass = NULL) :
|
||||
AP_Quaternion(IMU *imu, GPS *&gps) :
|
||||
_imu(imu),
|
||||
_gps(gps),
|
||||
_compass(compass)
|
||||
_gps(gps)
|
||||
{
|
||||
// reference direction of flux in earth frame
|
||||
b_x = 0;
|
||||
|
|
@ -70,10 +69,16 @@ public:
|
|||
uint8_t renorm_blowup_count;
|
||||
float get_error_rp(void);
|
||||
float get_error_yaw(void);
|
||||
Matrix3f get_dcm_matrix(void) {
|
||||
Matrix3f ret;
|
||||
quaternion_to_rotation_matrix(q, ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
private:
|
||||
void update_IMU(float deltat, Vector3f &gyro, Vector3f &accel);
|
||||
void update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag);
|
||||
void update_drift(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag);
|
||||
|
||||
bool _have_initial_yaw;
|
||||
|
||||
|
|
@ -82,6 +87,8 @@ private:
|
|||
|
||||
// members
|
||||
Compass * _compass;
|
||||
// time in microseconds of last compass update
|
||||
uint32_t _compass_last_update;
|
||||
|
||||
// note: we use ref-to-pointer here so that our caller can change the GPS without our noticing
|
||||
// IMU under us without our noticing.
|
||||
|
|
@ -91,12 +98,12 @@ private:
|
|||
// true if we are doing centripetal acceleration correction
|
||||
bool _centripetal;
|
||||
|
||||
// maximum gyroscope measurement error in rad/s (set to 20 degrees/second)
|
||||
static const float gyroMeasError = 20.0 * (M_PI/180.0);
|
||||
// maximum gyroscope measurement error in rad/s (set to 40 degrees/second)
|
||||
static const float gyroMeasError = 10.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)
|
||||
static const float gyroMeasDrift = 0.02 * (PI/180.0);
|
||||
// maximum gyroscope drift rate in radians/s/s (set to 0.005
|
||||
// degrees/s/s, which is 0.3 degrees/s/minute)
|
||||
static const float gyroMeasDrift = 0.005 * (PI/180.0);
|
||||
|
||||
float beta;
|
||||
float zeta;
|
||||
|
|
@ -115,6 +122,11 @@ private:
|
|||
// the current corrected gyro vector
|
||||
Vector3f _gyro_corrected;
|
||||
|
||||
// accel and gyro accumulators for drift correction
|
||||
Vector3f _gyro_sum;
|
||||
Vector3f _accel_sum;
|
||||
uint32_t _sum_count;
|
||||
|
||||
// estimate of error
|
||||
float _error_rp_sum;
|
||||
uint16_t _error_rp_count;
|
||||
|
|
|
|||
Loading…
Reference in New Issue