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

AHRS: adapt the quaternion library to AHRS

This commit is contained in:
Andrew Tridgell 2012-03-11 19:00:19 +11:00
parent 2d12bdb412
commit fe63e79416
2 changed files with 46 additions and 273 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

236
libraries/AP_AHRS/AP_AHRS_Quaternion.cpp
View File

@ -1,5 +1,5 @@
/* /*
AP_Quaternion code, based on quaternion code from Jeb Madgwick AP_AHRS_Quaternion code, based on quaternion code from Jeb Madgwick
See http://www.x-io.co.uk/res/doc/madgwick_internal_report.pdf See http://www.x-io.co.uk/res/doc/madgwick_internal_report.pdf
@ -13,24 +13,7 @@
version. version.
*/ */
#include <FastSerial.h> #include <FastSerial.h>
#include <AP_Quaternion.h> #include <AP_AHRS.h>
#define ToRad(x) (x*0.01745329252) // *pi/180
#define ToDeg(x) (x*57.2957795131) // *180/pi
// this is the speed in cm/s above which we first get a yaw lock with
// the GPS
#define GPS_SPEED_MIN 300
// this is the speed in cm/s at which we stop using drift correction
// from the GPS and wait for the ground speed to get above GPS_SPEED_MIN
#define GPS_SPEED_RESET 100
void
AP_Quaternion::set_compass(Compass *compass)
{
_compass = compass;
}
// to keep the code as close to the original as possible, we use these // to keep the code as close to the original as possible, we use these
// macros for quaternion access // macros for quaternion access
@ -40,7 +23,7 @@ AP_Quaternion::set_compass(Compass *compass)
#define SEq_4 q.q4 #define SEq_4 q.q4
// Function to compute one quaternion iteration without magnetometer // Function to compute one quaternion iteration without magnetometer
void AP_Quaternion::update_IMU(float deltat, Vector3f &gyro, Vector3f &accel) void AP_AHRS_Quaternion::update_IMU(float deltat, Vector3f &gyro, Vector3f &accel)
{ {
// Local system variables // Local system variables
float norm; // vector norm float norm; // vector norm
@ -134,7 +117,7 @@ void AP_Quaternion::update_IMU(float deltat, Vector3f &gyro, Vector3f &accel)
// Function to compute one quaternion iteration including magnetometer // Function to compute one quaternion iteration including magnetometer
void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag) void AP_AHRS_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag)
{ {
// local system variables // local system variables
float norm; // vector norm float norm; // vector norm
@ -253,7 +236,7 @@ void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, V
// don't allow the drift rate to be exceeded. This prevents a // don't allow the drift rate to be exceeded. This prevents a
// sudden drift change coming from a outage in the compass // sudden drift change coming from a outage in the compass
float max_change = gyroMeasDrift * deltat; float max_change = _gyro_drift_limit * deltat;
drift_delta.x = constrain(drift_delta.x, -max_change, max_change); drift_delta.x = constrain(drift_delta.x, -max_change, max_change);
drift_delta.y = constrain(drift_delta.y, -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); drift_delta.z = constrain(drift_delta.z, -max_change, max_change);
@ -308,161 +291,8 @@ void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, V
} }
// Function to update our gyro_bias drift estimate using the accelerometer
// and magnetometer. This is a cut-down version of update_MARG(), but
// without the quaternion update.
void AP_Quaternion::update_drift(float deltat, Vector3f &mag)
{
// local system variables
float norm; // vector norm
float 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 the magnetometer measurement
mag.normalize();
if (mag.is_inf()) {
// discard this data point
renorm_range_count++;
return;
}
// auxiliary variables to avoid repeated calculations
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_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_41 * f_4 - J_51 * f_5 + J_61 * f_6;
SEqHatDot_2 = + J_42 * f_4 + J_52 * f_5 + J_62 * f_6;
SEqHatDot_3 = - J_43 * f_4 + J_53 * f_5 + J_63 * f_6;
SEqHatDot_4 = - 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 then integrate the estimated quaternion rate
SEq_1 -= (beta * SEqHatDot_1) * deltat;
SEq_2 -= (beta * SEqHatDot_2) * deltat;
SEq_3 -= (beta * SEqHatDot_3) * deltat;
SEq_4 -= (beta * SEqHatDot_4) * deltat;
// normalise quaternion
norm = 1.0/safe_sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4);
if (isinf(norm)) {
// our quaternion is bad! Reset based on roll/pitch/yaw
// and hope for the best ...
renorm_blowup_count++;
_compass->null_offsets_disable();
q.from_euler(roll, pitch, yaw);
_compass->null_offsets_disable();
return;
}
SEq_1 *= norm;
SEq_2 *= norm;
SEq_3 *= norm;
SEq_4 *= norm;
// 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 // Function to compute one quaternion iteration
void AP_Quaternion::update(void) void AP_AHRS_Quaternion::update(void)
{ {
Vector3f gyro, accel; Vector3f gyro, accel;
float deltat; float deltat;
@ -518,41 +348,14 @@ void AP_Quaternion::update(void)
accel.z += (gyro.y - gyro_bias.y) * veloc; accel.z += (gyro.y - gyro_bias.y) * veloc;
} }
#define SEPARATE_DRIFT 0 if (_compass != NULL && _compass->use_for_yaw()) {
#if SEPARATE_DRIFT
/*
The full Madgwick quaternion 'MARG' system assumes you get
gyro, accel and magnetometer updates at the same rate. On
APM we get them at very different rates, and re-calculating
our drift due to the magnetometer in the fast loop is very
wasteful of CPU.
Instead, we only update the gyro_bias vector when we get a
new magnetometer point, and use the much simpler
update_IMU() as the main quaternion update function.
*/
_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); Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, - _compass->mag_z);
_sum_count = 0; update_MARG(deltat, gyro, accel, mag);
update_drift(mag_deltat, mag); } else {
_compass_last_update = _compass->last_update;
}
// step the quaternion solution using just gyros and accels // step the quaternion solution using just gyros and accels
gyro -= gyro_bias; gyro -= gyro_bias;
update_IMU(deltat, gyro, accel); update_IMU(deltat, gyro, accel);
#else }
Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, - _compass->mag_z);
update_MARG(deltat, gyro, accel, mag);
#endif
#ifdef DESKTOP_BUILD #ifdef DESKTOP_BUILD
if (q.is_nan()) { if (q.is_nan()) {
@ -587,7 +390,7 @@ void AP_Quaternion::update(void)
} }
// average error in roll/pitch since last call // average error in roll/pitch since last call
float AP_Quaternion::get_error_rp(void) float AP_AHRS_Quaternion::get_error_rp(void)
{ {
float ret; float ret;
if (_error_rp_count == 0) { if (_error_rp_count == 0) {
@ -600,7 +403,7 @@ float AP_Quaternion::get_error_rp(void)
} }
// average error in yaw since last call // average error in yaw since last call
float AP_Quaternion::get_error_yaw(void) float AP_AHRS_Quaternion::get_error_yaw(void)
{ {
float ret; float ret;
if (_error_yaw_count == 0) { if (_error_yaw_count == 0) {
@ -611,3 +414,18 @@ float AP_Quaternion::get_error_yaw(void)
_error_yaw_count = 0; _error_yaw_count = 0;
return ret; return ret;
} }
// reset attitude system
void AP_AHRS_Quaternion::reset(bool recover_eulers)
{
if (recover_eulers) {
q.from_euler(roll, pitch, yaw);
} else {
q(1, 0, 0, 0);
}
gyro_bias.zero();
// reference direction of flux in earth frame
b_x = 0;
b_z = -1;
}

79
libraries/AP_AHRS/AP_AHRS_Quaternion.h
View File

@ -13,66 +13,41 @@
#include "WProgram.h" #include "WProgram.h"
#endif #endif
class AP_Quaternion class AP_AHRS_Quaternion : public AP_AHRS
{ {
public: public:
// Constructor // Constructor
AP_Quaternion(IMU *imu, GPS *&gps) : AP_AHRS_Quaternion(IMU *imu, GPS *&gps) : AP_AHRS(imu, gps)
_imu(imu),
_gps(gps)
{ {
// reference direction of flux in earth frame
b_x = 0;
b_z = -1;
// limit the drift to the drift rate reported by the
// sensor driver
gyroMeasDrift = imu->get_gyro_drift_rate();
// scaled gyro drift limits // scaled gyro drift limits
beta = sqrt(3.0f / 4.0f) * gyroMeasError; beta = sqrt(3.0f / 4.0f) * gyroMeasError;
zeta = sqrt(3.0f / 4.0f) * gyroMeasDrift; zeta = sqrt(3.0f / 4.0f) * _gyro_drift_limit;
}
// Accessors // reset attitude
void set_centripetal(bool b) {_centripetal = b;} reset();
bool get_centripetal(void) {return _centripetal;} }
void set_compass(Compass *compass);
// Methods // Methods
void update(void); void update(void);
void reset(bool recover_eulers=false);
// Euler angles (radians) // get corrected gyro vector
float roll;
float pitch;
float yaw;
// integer Euler angles (Degrees * 100)
int32_t roll_sensor;
int32_t pitch_sensor;
int32_t yaw_sensor;
// compatibility methods with DCM
void update_DCM(void) { update(); }
void update_DCM_fast(void) { update(); }
Vector3f get_gyro(void) { Vector3f get_gyro(void) {
// notice the sign reversals here // notice the sign reversals here
return Vector3f(-_gyro_corrected.x, -_gyro_corrected.y, _gyro_corrected.z); return Vector3f(-_gyro_corrected.x, -_gyro_corrected.y, _gyro_corrected.z);
} }
Vector3f get_gyro_drift(void) { Vector3f get_gyro_drift(void) {
// notice the sign reversals here // notice the sign reversals here. The quaternion
return Vector3f(-gyro_bias.x, -gyro_bias.y, gyro_bias.z); // system uses a -ve gyro bias, DCM uses a +ve
return Vector3f(gyro_bias.x, gyro_bias.y, -gyro_bias.z);
} }
float get_accel_weight(void) { return 0; }
float get_renorm_val(void) { return 0; }
float get_health(void) { return 0; }
void matrix_reset(void) { }
uint8_t gyro_sat_count;
uint8_t renorm_range_count;
uint8_t renorm_blowup_count;
float get_error_rp(void); float get_error_rp(void);
float get_error_yaw(void); float get_error_yaw(void);
// convert quaternion to a DCM matrix, used by compass
// null offsets code
Matrix3f get_dcm_matrix(void) { Matrix3f get_dcm_matrix(void) {
Matrix3f ret; Matrix3f ret;
q.rotation_matrix(ret); q.rotation_matrix(ret);
@ -82,31 +57,16 @@ public:
private: private:
void update_IMU(float deltat, Vector3f &gyro, Vector3f &accel); void update_IMU(float deltat, Vector3f &gyro, Vector3f &accel);
void update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag); void update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag);
void update_drift(float deltat, Vector3f &mag);
bool _have_initial_yaw; bool _have_initial_yaw;
// Methods // Methods
void accel_adjust(void); void accel_adjust(void);
// 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.
GPS *&_gps;
IMU *_imu;
// true if we are doing centripetal acceleration correction
bool _centripetal;
// maximum gyroscope measurement error in rad/s (set to 7 degrees/second) // maximum gyroscope measurement error in rad/s (set to 7 degrees/second)
static const float gyroMeasError = 20.0 * (M_PI/180.0); static const float gyroMeasError = 20.0 * (M_PI/180.0);
float gyroMeasDrift; // scaled tuning constants
float beta; float beta;
float zeta; float zeta;
@ -124,11 +84,6 @@ private:
// the current corrected gyro vector // the current corrected gyro vector
Vector3f _gyro_corrected; Vector3f _gyro_corrected;
// accel and gyro accumulators for drift correction
Vector3f _gyro_sum;
Vector3f _accel_sum;
uint32_t _sum_count;
// estimate of error // estimate of error
float _error_rp_sum; float _error_rp_sum;
uint16_t _error_rp_count; uint16_t _error_rp_count;