fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 00:34:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Enable GPS flight without magnetometer (#770)

Browse Source 
* EKF: Enable GPS flight without magnetometer

Enables takeoff in a non-GPS flight mode with mag fusion type set to MAG_FUSE_TYPE_NONE. After sufficient movement the EKF will reset the yaw tot he EKF-GSF estimate. After that GPS fusion will commence.

* EKF: Fix unconstrained yaw and yaw variance growth when on ground

* EKF: Ensure first yaw alignment can't be blocked

* EKF: Increase yaw variance limit allowed for alignment

Flight test data indicates that an uncertainty value of 15 deg still provides a reliable yaw estimate and enables an earlier alignment/reset if required.

* EKF: Remove unexecutable code

* EKF: Restructure heading fusion

* EKF: parameterise quarter variance check and retune default value

* EKF: Pass by reference instead of pointer

* EKF: Clarify reset logic

* EKF: Remove incorrect setting of mag field alignment flag

* EKF: Non-functional tidy up

* EKF: Fix non-use of function argument

The updateQuaternion function was using the _heading_innovation class variable instead of setting it using the innovation argument.

* EKF: Fix undefined variable

* EKF: Use single precision atan2

* EKF: remove unnecessary timer reset and unwanted execution of reset function

* EKF: Don't declare a mag fault when non-use is user selected

Doing so produces unnecessary user alerts.
This commit is contained in:
Paul Riseborough 2020-03-30 20:05:38 +11:00 committed by GitHub
parent 3bd09fdcd3
commit 89b5c77d5d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
8 changed files with 442 additions and 295 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
EKF/common.h
View File

@ -177,7 +177,7 @@ struct auxVelSample {
#define MASK_USE_DRAG (1<<5) ///< set to true to use the multi-rotor drag model to estimate wind
#define MASK_ROTATE_EV (1<<6) ///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used
#define MASK_USE_GPSYAW (1<<7) ///< set to true to use GPS yaw data if available
#define MASK_USE_EVVEL (1<<8) ///< sset to true to use external vision velocity data
#define MASK_USE_EVVEL (1<<8) ///< set to true to use external vision velocity data
enum TerrainFusionMask : int32_t {
TerrainFuseRangeFinder = (1 << 0),
@ -265,6 +265,7 @@ struct parameters {
int32_t mag_fusion_type{0}; ///< integer used to specify the type of magnetometer fusion used
float mag_acc_gate{0.5f}; ///< when in auto select mode, heading fusion will be used when manoeuvre accel is lower than this (m/sec**2)
float mag_yaw_rate_gate{0.25f}; ///< yaw rate threshold used by mode select logic (rad/sec)
float quat_max_variance{0.0001f}; ///< zero innovation yaw measurements will not be fused when the sum of quaternion variance is less than this
// airspeed fusion
float tas_innov_gate{5.0f}; ///< True Airspeed innovation consistency gate size (STD)
@ -365,7 +366,7 @@ struct parameters {
// Parameters used to control when yaw is reset to the EKF-GSF yaw estimator value
float EKFGSF_tas_default{15.0f}; ///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s)
unsigned EKFGSF_reset_delay{1000000}; ///< Number of uSec of bad innovations on main filter inpost takeoff phase before yaw is reset to EKF-GSF value
float EKFGSF_yaw_err_max{0.2f}; ///< Composite yaw 1-sigma uncertainty threshold used to check for convergence (rad)
float EKFGSF_yaw_err_max{0.262f}; ///< Composite yaw 1-sigma uncertainty threshold used to check for convergence (rad)
};
struct stateSample {

26
EKF/control.cpp
View File

@ -644,17 +644,27 @@ void Ekf::controlGpsFusion()
ECL_WARN_TIMESTAMPED("GPS data quality poor - stopping use");
}
// handle case where we are not currently using GPS, but need to align yaw angle using EKF-GSF before
// we can start using GPS
bool align_yaw_using_gsf = !_control_status.flags.gps && _do_ekfgsf_yaw_reset && isTimedOut(_ekfgsf_yaw_reset_time, 5000000);
if (align_yaw_using_gsf) {
if (resetYawToEKFGSF()) {
_ekfgsf_yaw_reset_time = _time_last_imu;
_do_ekfgsf_yaw_reset = false;
}
}
// handle the case when we now have GPS, but have not been using it for an extended period
if (_control_status.flags.gps) {
// We are relying on aiding to constrain drift so after a specified time
// with no aiding we need to do something
bool do_reset = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
bool do_vel_pos_reset = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
&& isTimedOut(_time_last_delpos_fuse, _params.reset_timeout_max)
&& isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
&& isTimedOut(_time_last_of_fuse, _params.reset_timeout_max);
// We haven't had an absolute position fix for a longer time so need to do something
do_reset = do_reset || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max);
do_vel_pos_reset = do_vel_pos_reset || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max);
// A reset to the EKF-GSF estimate can be performed after a recent takeoff which will enable
// recovery from a bad magnetometer or field estimate.
@ -667,14 +677,14 @@ void Ekf::controlGpsFusion()
_time_last_on_ground_us = _time_last_imu;
}
const bool recent_takeoff = _control_status.flags.in_air && !isTimedOut(_time_last_on_ground_us, 30000000);
const bool reset_yaw_to_EKFGSF = (do_reset || _do_emergency_yaw_reset || stopped_following_gps_velocity) &&
const bool do_yaw_vel_pos_reset = (do_vel_pos_reset || _do_ekfgsf_yaw_reset || stopped_following_gps_velocity) &&
recent_takeoff &&
isTimedOut(_emergency_yaw_reset_time, 5000000);
isTimedOut(_ekfgsf_yaw_reset_time, 5000000);
if (reset_yaw_to_EKFGSF) {
if (do_yaw_vel_pos_reset) {
if (resetYawToEKFGSF()) {
_emergency_yaw_reset_time = _time_last_imu;
_do_emergency_yaw_reset = false;
_ekfgsf_yaw_reset_time = _time_last_imu;
_do_ekfgsf_yaw_reset = false;
// Reset the timeout counters
_time_last_hor_pos_fuse = _time_last_imu;
@ -683,7 +693,7 @@ void Ekf::controlGpsFusion()
_time_last_of_fuse = _time_last_imu;
}
} else if (do_reset) {
} else if (do_vel_pos_reset) {
// use GPS velocity data to check and correct yaw angle if a FW vehicle
if (_control_status.flags.fixed_wing && _control_status.flags.in_air) {
// if flying a fixed wing aircraft, do a complete reset that includes yaw

33
EKF/ekf.h
View File

@ -373,14 +373,14 @@ private:
uint64_t _time_yaw_started{0}; ///< last system time in usec that a yaw rotation manoeuvre was detected
uint8_t _num_bad_flight_yaw_events{0}; ///< number of times a bad heading has been detected in flight and required a yaw reset
uint64_t _mag_use_not_inhibit_us{0}; ///< last system time in usec before magnetometer use was inhibited
bool _mag_use_inhibit{false}; ///< true when magnetometer use is being inhibited
bool _mag_use_inhibit_prev{false}; ///< true when magnetometer use was being inhibited the previous frame
bool _mag_inhibit_yaw_reset_req{false}; ///< true when magnetometer inhibit has been active for long enough to require a yaw reset when conditions improve.
float _last_static_yaw{0.0f}; ///< last yaw angle recorded when on ground motion checks were passing (rad)
bool _mag_yaw_reset_req{false}; ///< true when a reset of the yaw using the magnetometer data has been requested
bool _mag_decl_cov_reset{false}; ///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event.
bool _synthetic_mag_z_active{false}; ///< true if we are generating synthetic magnetometer Z measurements
bool _yaw_use_inhibit{false}; ///< true when yaw sensor use is being inhibited
matrix::SquareMatrix<float, _k_num_states> P; ///< state covariance matrix
Vector3f _delta_vel_bias_var_accum; ///< kahan summation algorithm accumulator for delta velocity bias variance
@ -540,6 +540,28 @@ private:
// fuse the first euler angle from either a 321 or 312 rotation sequence as the observation (currently measures yaw using the magnetometer)
void fuseHeading();
// fuse the yaw angle defined as the first rotation in a 321 Tait-Bryan rotation sequence
// yaw : angle observation defined as the first rotation in a 321 Tait-Bryan rotation sequence (rad)
// yaw_variance : variance of the yaw angle observation (rad^2)
// zero_innovation : Fuse data with innovation set to zero
void fuseYaw321(const float yaw, const float yaw_variance, bool zero_innovation);
// fuse the yaw angle defined as the first rotation in a 312 Tait-Bryan rotation sequence
// yaw : angle observation defined as the first rotation in a 312 Tait-Bryan rotation sequence (rad)
// yaw_variance : variance of the yaw angle observation (rad^2)
// zero_innovation : Fuse data with innovation set to zero
void fuseYaw312(const float yaw, const float yaw_variance, bool zero_innovation);
// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
// innovation : prediction - measurement
// variance : observaton variance
// gate_sigma : innovation consistency check gate size (Sigma)
// jacobian : 4x1 vector of partial derivatives of observation wrt each quaternion state
void updateQuaternion(const float innovation, const float variance, const float gate_sigma, const float (&yaw_jacobian)[4]);
// shrinks the yaw axis uncertainty of quaternion covariances by fusing a zero innovation yaw observation
void shrinkYawVariance();
// fuse the yaw angle obtained from a dual antenna GPS unit
void fuseGpsAntYaw();
@ -849,14 +871,15 @@ private:
// Declarations used to control use of the EKF-GSF yaw estimator
int64_t _emergency_yaw_reset_time{0}; ///< timestamp of last emergency yaw reset (uSec)
int64_t _ekfgsf_yaw_reset_time{0}; ///< timestamp of last emergency yaw reset (uSec)
uint64_t _time_last_on_ground_us{0}; ///< last tine we were on the ground (uSec)
bool _do_emergency_yaw_reset{false}; // true when an emergency yaw reset has been requested
bool _do_ekfgsf_yaw_reset{false}; // true when an emergency yaw reset has been requested
// Call once per _imu_sample_delayed update after all main EKF data fusion oeprations have been completed
void runYawEKFGSF();
// Resets the main Nav EKf yaw to the esitmator from the EKF-GSF yaw estimator
// Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator
// Resets the horizontal velocity and position to the default navigation sensor
// Returns true if the reset was successful
bool resetYawToEKFGSF();

39
EKF/ekf_helper.cpp
View File

@ -544,8 +544,8 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
yaw_new_variance = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
}
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _mag_use_inhibit) {
// we are operating without knowing the earth frame yaw angle
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _yaw_use_inhibit) {
// we are operating temporarily without knowing the earth frame yaw angle
return true;
} else {
@ -1723,7 +1723,9 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer)
_state_reset_status.quat_counter++;
}
// Reset main nav filter yaw to value from EKF-GSF and reset velocity and position to GPS
// Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator
// Resets the horizontal velocity and position to the default navigation sensor
// Returns true if the reset was successful
bool Ekf::resetYawToEKFGSF()
{
// don't allow reet using the EKF-GSF estimate until the filter has started fusing velocity
@ -1737,14 +1739,18 @@ bool Ekf::resetYawToEKFGSF()
resetVelocity();
resetPosition();
// stop using the magnetometer in the main EKF otherwise it's fusion could drag the yaw around
// and cause another navigation failure
_control_status.flags.mag_fault = true;
// record a magnetic field alignment event to prevent possibility of the EKF trying to reset the yaw to the mag later in flight
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
_control_status.flags.yaw_align = true;
ECL_INFO_TIMESTAMPED("Emergency yaw reset - magnetometer use stopped");
if (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE) {
ECL_INFO_TIMESTAMPED("Yaw aligned using IMU and GPS");
} else {
// stop using the magnetometer in the main EKF otherwise it's fusion could drag the yaw around
// and cause another navigation failure
_control_status.flags.mag_fault = true;
ECL_INFO_TIMESTAMPED("Emergency yaw reset - magnetometer use stopped");
}
return true;
}
@ -1755,7 +1761,7 @@ bool Ekf::resetYawToEKFGSF()
void Ekf::requestEmergencyNavReset()
{
_do_emergency_yaw_reset = true;
_do_ekfgsf_yaw_reset = true;
}
bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF])
@ -1803,3 +1809,18 @@ void Ekf::runYawEKFGSF()
yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), _gps_sample_delayed.vacc);
}
}
void Ekf::shrinkYawVariance()
{
if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) {
// rolled more than pitched so use 321 rotation order to define yaw angle
// and fuse a zero innovation yaw to shrink quaternion yaw variance
fuseYaw321(0.0f, 0.25f, true);
} else {
// pitched more than rolled so use 312 rotation order to define yaw angle
// and fuse a zero innovation yaw to shrink quaternion yaw variance
fuseYaw312(0.0f, 0.25f, true);
}
}

8
EKF/gps_checks.cpp
View File

@ -85,7 +85,13 @@ bool Ekf::collect_gps(const gps_message &gps)
_mag_strength_gps = 0.01f * get_mag_strength(lat, lon);
// request a reset of the yaw using the new declination
_mag_yaw_reset_req = true;
if (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE) {
// try to reset the yaw using the EKF-GSF yaw esitimator
_do_ekfgsf_yaw_reset = true;
_ekfgsf_yaw_reset_time = 0;
} else {
_mag_yaw_reset_req = true;
}
// save the horizontal and vertical position uncertainty of the origin
_gps_origin_eph = gps.eph;
_gps_origin_epv = gps.epv;

35
EKF/mag_control.cpp
View File

@ -41,20 +41,31 @@
void Ekf::controlMagFusion()
{
// handle undefined behaviour
if (_params.mag_fusion_type > MAG_FUSE_TYPE_NONE) {
return;
}
// When operating without a magnetometer, yaw fusion is run selectively to prevent
// enable yaw gyro bias learning hen stationary on ground and to prevent uncontrolled
// yaw variance growth
if (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE) {
_yaw_use_inhibit = true;
fuseHeading();
return;
}
updateMagFilter();
checkMagFieldStrength();
// If we are on ground, store the local position and time to use as a reference
// Also reset the flight alignment flag so that the mag fields will be re-initialised next time we achieve flight altitude
// If we are on ground, reset the flight alignment flag so that the mag fields will be
// re-initialised next time we achieve flight altitude
if (!_control_status.flags.in_air) {
_last_on_ground_posD = _state.pos(2);
_control_status.flags.mag_aligned_in_flight = false;
_num_bad_flight_yaw_events = 0;
}
if ((_params.mag_fusion_type >= MAG_FUSE_TYPE_NONE)
|| _control_status.flags.mag_fault
|| !_control_status.flags.yaw_align) {
if (_control_status.flags.mag_fault || !_control_status.flags.yaw_align) {
stopMagFusion();
return;
}
@ -143,12 +154,12 @@ void Ekf::runOnGroundYawReset()
bool Ekf::isYawResetAuthorized() const
{
return !_mag_use_inhibit;
return !_yaw_use_inhibit;
}
bool Ekf::canResetMagHeading() const
{
return !isStrongMagneticDisturbance();
return !isStrongMagneticDisturbance() && (_params.mag_fusion_type != MAG_FUSE_TYPE_NONE);
}
void Ekf::runInAirYawReset()
@ -255,8 +266,8 @@ void Ekf::checkMagDeclRequired()
void Ekf::checkMagInhibition()
{
_mag_use_inhibit = shouldInhibitMag();
if (!_mag_use_inhibit) {
_yaw_use_inhibit = shouldInhibitMag();
if (!_yaw_use_inhibit) {
_mag_use_not_inhibit_us = _imu_sample_delayed.time_us;
}
@ -271,7 +282,8 @@ bool Ekf::shouldInhibitMag() const
// If the user has selected auto protection against indoor magnetic field errors, only use the magnetometer
// if a yaw angle relative to true North is required for navigation. If no GPS or other earth frame aiding
// is available, assume that we are operating indoors and the magnetometer should not be used.
// Also inhibit mag fusion when a strong magnetic field interference is detected
// Also inhibit mag fusion when a strong magnetic field interference is detected or the user
// has explicitly stopped magnetometer use.
const bool user_selected = (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR);
const bool heading_not_required_for_navigation = !_control_status.flags.gps
@ -324,7 +336,6 @@ void Ekf::runMagAndMagDeclFusions()
{
if (_control_status.flags.mag_3D) {
run3DMagAndDeclFusions();
} else if (_control_status.flags.mag_hdg) {
fuseHeading();
}

586
EKF/mag_fusion.cpp
View File

@ -435,7 +435,7 @@ void Ekf::fuseMag()
}
}
void Ekf::fuseHeading()
void Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation)
{
// assign intermediate state variables
const float q0 = _state.quat_nominal(0);
@ -443,296 +443,206 @@ void Ekf::fuseHeading()
const float q2 = _state.quat_nominal(2);
const float q3 = _state.quat_nominal(3);
float R_YAW = 1.0f;
float predicted_hdg;
float R_YAW = fmaxf(yaw_variance, 1.0e-4f);
float measurement = wrap_pi(yaw);
float H_YAW[4];
Vector3f mag_earth_pred;
float measured_hdg;
// determine if a 321 or 312 Euler sequence is best
if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) {
// calculate observation jacobian when we are observing the first rotation in a 321 sequence
float t9 = q0*q3;
float t10 = q1*q2;
float t2 = t9+t10;
float t3 = q0*q0;
float t4 = q1*q1;
float t5 = q2*q2;
float t6 = q3*q3;
float t7 = t3+t4-t5-t6;
// calculate observation jacobian when we are observing the first rotation in a 321 sequence
float t9 = q0*q3;
float t10 = q1*q2;
float t2 = t9+t10;
float t3 = q0*q0;
float t4 = q1*q1;
float t5 = q2*q2;
float t6 = q3*q3;
float t7 = t3+t4-t5-t6;
float t16 = q3*t3;
float t17 = q3*t5;
float t18 = q0*q1*q2*2.0f;
float t19 = t16+t17+t18-q3*t4+q3*t6;
float t16 = q3*t3;
float t17 = q3*t5;
float t18 = q0*q1*q2*2.0f;
float t19 = t16+t17+t18-q3*t4+q3*t6;
float t24 = q2*t4;
float t25 = q2*t6;
float t26 = q0*q1*q3*2.0f;
float t27 = t24+t25+t26-q2*t3+q2*t5;
float t28 = q1*t3;
float t29 = q1*t5;
float t30 = q0*q2*q3*2.0f;
float t31 = t28+t29+t30+q1*t4-q1*t6;
float t32 = q0*t4;
float t33 = q0*t6;
float t34 = q1*q2*q3*2.0f;
float t35 = t32+t33+t34+q0*t3-q0*t5;
float t24 = q2*t4;
float t25 = q2*t6;
float t26 = q0*q1*q3*2.0f;
float t27 = t24+t25+t26-q2*t3+q2*t5;
float t28 = q1*t3;
float t29 = q1*t5;
float t30 = q0*q2*q3*2.0f;
float t31 = t28+t29+t30+q1*t4-q1*t6;
float t32 = q0*t4;
float t33 = q0*t6;
float t34 = q1*q2*q3*2.0f;
float t35 = t32+t33+t34+q0*t3-q0*t5;
// two computational paths are provided to work around singularities in calculation of the Jacobians
float t8 = t7*t7;
float t15 = t2*t2;
if (t8 > t15 && t8 > 1E-6f) {
// this path has a singularities at yaw = +-90 degrees
t8 = 1.0f/t8;
float t11 = t2*t2;
float t12 = t8*t11*4.0f;
float t13 = t12+1.0f;
float t14 = 1.0f/t13;
// two computational paths are provided to work around singularities in calculation of the Jacobians
float t8 = t7*t7;
float t15 = t2*t2;
if (t8 > t15 && t8 > 1E-6f) {
// this path has a singularities at yaw = +-90 degrees
t8 = 1.0f/t8;
float t11 = t2*t2;
float t12 = t8*t11*4.0f;
float t13 = t12+1.0f;
float t14 = 1.0f/t13;
H_YAW[0] = t8*t14*t19*(-2.0f);
H_YAW[1] = t8*t14*t27*(-2.0f);
H_YAW[2] = t8*t14*t31*2.0f;
H_YAW[3] = t8*t14*t35*2.0f;
H_YAW[0] = t8*t14*t19*(-2.0f);
H_YAW[1] = t8*t14*t27*(-2.0f);
H_YAW[2] = t8*t14*t31*2.0f;
H_YAW[3] = t8*t14*t35*2.0f;
} else if (t15 > 1E-6f) {
// this path has singularities at yaw = 0 and +-180 deg
t15 = 1.0f/t15;
float t20 = t7*t7;
float t21 = t15*t20*0.25f;
float t22 = t21+1.0f;
} else if (t15 > 1E-6f) {
// this path has singularities at yaw = 0 and +-180 deg
t15 = 1.0f/t15;
float t20 = t7*t7;
float t21 = t15*t20*0.25f;
float t22 = t21+1.0f;
if (fabsf(t22) > 1E-6f) {
float t23 = 1.0f/t22;
if (fabsf(t22) > 1E-6f) {
float t23 = 1.0f/t22;
H_YAW[0] = t15*t19*t23*(-0.5f);
H_YAW[1] = t15*t23*t27*(-0.5f);
H_YAW[2] = t15*t23*t31*0.5f;
H_YAW[3] = t15*t23*t35*0.5f;
H_YAW[0] = t15*t19*t23*(-0.5f);
H_YAW[1] = t15*t23*t27*(-0.5f);
H_YAW[2] = t15*t23*t31*0.5f;
H_YAW[3] = t15*t23*t35*0.5f;
} else {
return;
}
} else {
return;
}
} else {
return;
// rotate the magnetometer measurement into earth frame
}
// calculate the yaw innovation and wrap to the interval between +-pi
float innovation;
if (zero_innovation) {
innovation = 0.0f;
} else {
Eulerf euler321(_state.quat_nominal);
predicted_hdg = euler321(2); // we will need the predicted heading to calculate the innovation
// calculate the observed yaw angle
if (_control_status.flags.mag_hdg) {
// Set the yaw angle to zero and rotate the measurements into earth frame using the zero yaw angle
euler321(2) = 0.0f;
const Dcmf R_to_earth(euler321);
// rotate the magnetometer measurements into earth frame using a zero yaw angle
if (_control_status.flags.mag_3D) {
// don't apply bias corrections if we are learning them
mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
} else {
mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B);
}
// the angle of the projection onto the horizontal gives the yaw angle
measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
} else if (_control_status.flags.ev_yaw) {
// calculate the yaw angle for a 321 sequence
// Expressions obtained from yaw_input_321.c produced by https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw321.m
const float Tbn_1_0 = 2.0f*(_ev_sample_delayed.quat(0)*_ev_sample_delayed.quat(3)+_ev_sample_delayed.quat(1)*_ev_sample_delayed.quat(2));
const float Tbn_0_0 = sq(_ev_sample_delayed.quat(0))+sq(_ev_sample_delayed.quat(1))-sq(_ev_sample_delayed.quat(2))-sq(_ev_sample_delayed.quat(3));
measured_hdg = atan2f(Tbn_1_0,Tbn_0_0);
} else if (_mag_use_inhibit) {
// Special case where we either use the current or last known stationary value
// so set to current value as a safe default
measured_hdg = predicted_hdg;
} else {
// Should not be doing yaw fusion
return;
}
} else {
// calculate observation jacobian when we are observing a rotation in a 312 sequence
float t9 = q0*q3;
float t10 = q1*q2;
float t2 = t9-t10;
float t3 = q0*q0;
float t4 = q1*q1;
float t5 = q2*q2;
float t6 = q3*q3;
float t7 = t3-t4+t5-t6;
float t16 = q3*t3;
float t17 = q3*t4;
float t18 = t16+t17-q3*t5+q3*t6-q0*q1*q2*2.0f;
float t23 = q2*t3;
float t24 = q2*t4;
float t25 = t23+t24+q2*t5-q2*t6-q0*q1*q3*2.0f;
float t26 = q1*t5;
float t27 = q1*t6;
float t28 = t26+t27-q1*t3+q1*t4-q0*q2*q3*2.0f;
float t29 = q0*t5;
float t30 = q0*t6;
float t31 = t29+t30+q0*t3-q0*t4-q1*q2*q3*2.0f;
// two computational paths are provided to work around singularities in calculation of the Jacobians
float t8 = t7*t7;
float t15 = t2*t2;
if (t8 > t15 && t8 > 1E-6f) {
// this path has a singularities at yaw = +-90 degrees
t8 = 1.0f/t8;
float t11 = t2*t2;
float t12 = t8*t11*4.0f;
float t13 = t12+1.0f;
float t14 = 1.0f/t13;
H_YAW[0] = t8*t14*t18*(-2.0f);
H_YAW[1] = t8*t14*t25*(-2.0f);
H_YAW[2] = t8*t14*t28*2.0f;
H_YAW[3] = t8*t14*t31*2.0f;
} else if (t15 > 1E-6f) {
// this path has singularities at yaw = 0 and +-180 deg
t15 = 1.0f/t15;
float t19 = t7*t7;
float t20 = t15*t19*0.25f;
float t21 = t20+1.0f;
if (fabsf(t21) > 1E-6f) {
float t22 = 1.0f/t21;
H_YAW[0] = t15*t18*t22*(-0.5f);
H_YAW[1] = t15*t22*t25*(-0.5f);
H_YAW[2] = t15*t22*t28*0.5f;
H_YAW[3] = t15*t22*t31*0.5f;
} else {
return;
}
} else {
return;
}
// Use a Tait-Bryan 312 rotation sequence
Vector3f rotVec312;
predicted_hdg = atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1));
rotVec312(0) = 0.0f; // first rotation (yaw) set to zero for alter use when rotating the mag field into earth frame
rotVec312(1) = asinf(_R_to_earth(2, 1)); // second rotation (roll)
rotVec312(2) = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)); // third rotation (pitch)
// calculate the observed yaw angle
if (_control_status.flags.mag_hdg) {
// Calculate the body to earth frame rotation matrix from the euler angles using a 312 rotation sequence
// with yaw angle set to to zero
const Dcmf R_to_earth = taitBryan312ToRotMat(rotVec312);
// rotate the magnetometer measurements into earth frame using a zero yaw angle
if (_control_status.flags.mag_3D) {
// don't apply bias corrections if we are learning them
mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
} else {
mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B);
}
// the angle of the projection onto the horizontal gives the yaw angle
measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
} else if (_control_status.flags.ev_yaw) {
// calculate the yaw angle for a 312 sequence
// Values from yaw_input_312.c file produced by https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw312.m
float Tbn_0_1_neg = 2.0f*(_ev_sample_delayed.quat(0)*_ev_sample_delayed.quat(3)-_ev_sample_delayed.quat(1)*_ev_sample_delayed.quat(2));
float Tbn_1_1 = sq(_ev_sample_delayed.quat(0))-sq(_ev_sample_delayed.quat(1))+sq(_ev_sample_delayed.quat(2))-sq(_ev_sample_delayed.quat(3));
measured_hdg = atan2f(Tbn_0_1_neg,Tbn_1_1);
} else if (_mag_use_inhibit) {
// Special case where we either use the current or last known stationary value
// so set to current value as a safe default
measured_hdg = predicted_hdg;
} else {
// Should not be doing yaw fusion
return;
}
innovation = wrap_pi(atan2f(_R_to_earth(1, 0), _R_to_earth(0, 0)) - measurement);
}
// Calculate the observation variance
if (_control_status.flags.mag_hdg) {
// using magnetic heading tuning parameter
R_YAW = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
} else if (_control_status.flags.ev_yaw) {
// using error estimate from external vision data
R_YAW = fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f));
} else {
// default value
R_YAW = 0.01f;
}
// wrap the heading to the interval between +-pi
measured_hdg = wrap_pi(measured_hdg);
// calculate the innovation and define the innovation gate
// define the innovation gate size
float innov_gate = math::max(_params.heading_innov_gate, 1.0f);
if (_mag_use_inhibit) {
// The magnetometer cannot be trusted but we need to fuse a heading to prevent a badly
// conditioned covariance matrix developing over time.
if (!_control_status.flags.vehicle_at_rest) {
// Vehicle is not at rest so fuse a zero innovation and record the
// predicted heading to use as an observation when movement ceases.
_heading_innov = 0.0f;
_last_static_yaw = predicted_hdg;
// Update the quaternion states and covariance matrix
updateQuaternion(innovation, R_YAW, innov_gate, H_YAW);
}
void Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation)
{
// assign intermediate state variables
const float q0 = _state.quat_nominal(0);
const float q1 = _state.quat_nominal(1);
const float q2 = _state.quat_nominal(2);
const float q3 = _state.quat_nominal(3);
float R_YAW = fmaxf(yaw_variance, 1.0e-4f);
float measurement = wrap_pi(yaw);
float H_YAW[4];
// calculate observation jacobian when we are observing a rotation in a 312 sequence
float t9 = q0*q3;
float t10 = q1*q2;
float t2 = t9-t10;
float t3 = q0*q0;
float t4 = q1*q1;
float t5 = q2*q2;
float t6 = q3*q3;
float t7 = t3-t4+t5-t6;
float t16 = q3*t3;
float t17 = q3*t4;
float t18 = t16+t17-q3*t5+q3*t6-q0*q1*q2*2.0f;
float t23 = q2*t3;
float t24 = q2*t4;
float t25 = t23+t24+q2*t5-q2*t6-q0*q1*q3*2.0f;
float t26 = q1*t5;
float t27 = q1*t6;
float t28 = t26+t27-q1*t3+q1*t4-q0*q2*q3*2.0f;
float t29 = q0*t5;
float t30 = q0*t6;
float t31 = t29+t30+q0*t3-q0*t4-q1*q2*q3*2.0f;
// two computational paths are provided to work around singularities in calculation of the Jacobians
float t8 = t7*t7;
float t15 = t2*t2;
if (t8 > t15 && t8 > 1E-6f) {
// this path has a singularities at yaw = +-90 degrees
t8 = 1.0f/t8;
float t11 = t2*t2;
float t12 = t8*t11*4.0f;
float t13 = t12+1.0f;
float t14 = 1.0f/t13;
H_YAW[0] = t8*t14*t18*(-2.0f);
H_YAW[1] = t8*t14*t25*(-2.0f);
H_YAW[2] = t8*t14*t28*2.0f;
H_YAW[3] = t8*t14*t31*2.0f;
} else if (t15 > 1E-6f) {
// this path has singularities at yaw = 0 and +-180 deg
t15 = 1.0f/t15;
float t19 = t7*t7;
float t20 = t15*t19*0.25f;
float t21 = t20+1.0f;
if (fabsf(t21) > 1E-6f) {
float t22 = 1.0f/t21;
H_YAW[0] = t15*t18*t22*(-0.5f);
H_YAW[1] = t15*t22*t25*(-0.5f);
H_YAW[2] = t15*t22*t28*0.5f;
H_YAW[3] = t15*t22*t31*0.5f;
} else {
// Vehicle is at rest so use the last moving prediction as an observation
// to prevent the heading from drifting and to enable yaw gyro bias learning
// before takeoff.
_heading_innov = predicted_hdg - _last_static_yaw;
innov_gate = 5.0f;
return;
}
} else {
_heading_innov = predicted_hdg - measured_hdg;
_last_static_yaw = predicted_hdg;
return;
}
_mag_use_inhibit_prev = _mag_use_inhibit;
// wrap the innovation to the interval between +-pi
_heading_innov = wrap_pi(_heading_innov);
float innovation;
if (zero_innovation) {
innovation = 0.0f;
} else {
// calculate the the innovation and wrap to the interval between +-pi
innovation = wrap_pi(atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1)) - measurement);
}
// define the innovation gate size
float innov_gate = math::max(_params.heading_innov_gate, 1.0f);
// Update the quaternion states and covariance matrix
updateQuaternion(innovation, R_YAW, innov_gate, H_YAW);
}
// update quaternion states and covariances using the yaw innovation, yaw observation variance and yaw Jacobian
void Ekf::updateQuaternion(const float innovation, const float variance, const float gate_sigma, const float (&yaw_jacobian)[4])
{
// Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 4 elements in H are non zero
// calculate the innovation variance
float PH[4];
_heading_innov_var = R_YAW;
_heading_innov_var = variance;
for (unsigned row = 0; row <= 3; row++) {
PH[row] = 0.0f;
for (uint8_t col = 0; col <= 3; col++) {
PH[row] += P(row,col) * H_YAW[col];
PH[row] += P(row,col) * yaw_jacobian[col];
}
_heading_innov_var += H_YAW[row] * PH[row];
_heading_innov_var += yaw_jacobian[row] * PH[row];
}
float heading_innov_var_inv;
// check if the innovation variance calculation is badly conditioned
if (_heading_innov_var >= R_YAW) {
if (_heading_innov_var >= variance) {
// the innovation variance contribution from the state covariances is not negative, no fault
_fault_status.flags.bad_hdg = false;
heading_innov_var_inv = 1.0f / _heading_innov_var;
@ -755,7 +665,7 @@ void Ekf::fuseHeading()
Kfusion[row] = 0.0f;
for (uint8_t col = 0; col <= 3; col++) {
Kfusion[row] += P(row,col) * H_YAW[col];
Kfusion[row] += P(row,col) * yaw_jacobian[col];
}
Kfusion[row] *= heading_innov_var_inv;
@ -766,7 +676,7 @@ void Ekf::fuseHeading()
Kfusion[row] = 0.0f;
for (uint8_t col = 0; col <= 3; col++) {
Kfusion[row] += P(row,col) * H_YAW[col];
Kfusion[row] += P(row,col) * yaw_jacobian[col];
}
Kfusion[row] *= heading_innov_var_inv;
@ -774,7 +684,7 @@ void Ekf::fuseHeading()
}
// innovation test ratio
_yaw_test_ratio = sq(_heading_innov) / (sq(innov_gate) * _heading_innov_var);
_yaw_test_ratio = sq(innovation) / (sq(gate_sigma) * _heading_innov_var);
// we are no longer using 3-axis fusion so set the reported test levels to zero
memset(_mag_test_ratio, 0, sizeof(_mag_test_ratio));
@ -791,12 +701,13 @@ void Ekf::fuseHeading()
} else {
// constrain the innovation to the maximum set by the gate
float gate_limit = sqrtf((sq(innov_gate) * _heading_innov_var));
_heading_innov = math::constrain(_heading_innov, -gate_limit, gate_limit);
float gate_limit = sqrtf((sq(gate_sigma) * _heading_innov_var));
_heading_innov = math::constrain(innovation, -gate_limit, gate_limit);
}
} else {
_innov_check_fail_status.flags.reject_yaw = false;
_heading_innov = innovation;
}
// apply covariance correction via P_new = (I -K*H)*P
@ -807,10 +718,10 @@ void Ekf::fuseHeading()
for (unsigned row = 0; row < _k_num_states; row++) {
KH[0] = Kfusion[row] * H_YAW[0];
KH[1] = Kfusion[row] * H_YAW[1];
KH[2] = Kfusion[row] * H_YAW[2];
KH[3] = Kfusion[row] * H_YAW[3];
KH[0] = Kfusion[row] * yaw_jacobian[0];
KH[1] = Kfusion[row] * yaw_jacobian[1];
KH[2] = Kfusion[row] * yaw_jacobian[2];
KH[3] = Kfusion[row] * yaw_jacobian[3];
for (unsigned column = 0; column < _k_num_states; column++) {
float tmp = KH[0] * P(0,column);
@ -858,6 +769,165 @@ void Ekf::fuseHeading()
}
}
void Ekf::fuseHeading()
{
Vector3f mag_earth_pred;
float measured_hdg;
float predicted_hdg;
// Calculate the observation variance
float R_YAW;
if (_control_status.flags.mag_hdg) {
// using magnetic heading tuning parameter
R_YAW = sq(_params.mag_heading_noise);
} else if (_control_status.flags.ev_yaw) {
// using error estimate from external vision data
R_YAW = _ev_sample_delayed.angVar;
} else {
// default value
R_YAW = 0.01f;
}
// update transformation matrix from body to world frame using the current state estimate
_R_to_earth = Dcmf(_state.quat_nominal);
// determine if a 321 or 312 Euler sequence is best
if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) {
// rolled more than pitched so use 321 rotation order to calculate the observed yaw angle
Eulerf euler321(_state.quat_nominal);
predicted_hdg = euler321(2);
if (_control_status.flags.mag_hdg) {
// Set the yaw angle to zero and rotate the measurements into earth frame using the zero yaw angle
euler321(2) = 0.0f;
const Dcmf R_to_earth(euler321);
if (_control_status.flags.mag_3D) {
// don't apply bias corrections if we are learning them
mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
} else {
mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B);
}
// the angle of the projection onto the horizontal gives the yaw angle
measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
} else if (_control_status.flags.ev_yaw) {
// calculate the yaw angle for a 321 sequence
// Expressions obtained from yaw_input_321.c produced by https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw321.m
const float Tbn_1_0 = 2.0f*(_ev_sample_delayed.quat(0)*_ev_sample_delayed.quat(3)+_ev_sample_delayed.quat(1)*_ev_sample_delayed.quat(2));
const float Tbn_0_0 = sq(_ev_sample_delayed.quat(0))+sq(_ev_sample_delayed.quat(1))-sq(_ev_sample_delayed.quat(2))-sq(_ev_sample_delayed.quat(3));
measured_hdg = atan2f(Tbn_1_0,Tbn_0_0);
} else {
measured_hdg = predicted_hdg;
}
// handle special case where yaw measurement is unavailable
bool fuse_zero_innov = false;
if (_yaw_use_inhibit) {
// The yaw measurement cannot be trusted but we need to fuse something to prevent a badly
// conditioned covariance matrix developing over time.
if (!_control_status.flags.vehicle_at_rest) {
// Vehicle is not at rest so fuse a zero innovation if necessary to prevent
// unconstrained quaterniion variance growth and record the predicted heading
// to use as an observation when movement ceases.
// TODO a better way of determining when this is necessary
float sumQuatVar = P(0,0) + P(1,1) + P(2,2) + P(3,3);
if (sumQuatVar > _params.quat_max_variance) {
fuse_zero_innov = true;
R_YAW = 0.25f;
}
_last_static_yaw = predicted_hdg;
} else {
// Vehicle is at rest so use the last moving prediction as an observation
// to prevent the heading from drifting and to enable yaw gyro bias learning
// before takeoff.
measured_hdg = _last_static_yaw;
}
} else {
_last_static_yaw = predicted_hdg;
}
fuseYaw321(measured_hdg, R_YAW, fuse_zero_innov);
} else {
// pitched more than rolled so use 312 rotation order to calculate the observed yaw angle
predicted_hdg = atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1));
if (_control_status.flags.mag_hdg) {
// Calculate the body to earth frame rotation matrix from the euler angles using a 312 rotation sequence
// with yaw angle set to to zero
Vector3f rotVec312;
rotVec312(0) = 0.0f; // first rotation (yaw) set to zero for alter use when rotating the mag field into earth frame
rotVec312(1) = asinf(_R_to_earth(2, 1)); // second rotation (roll)
rotVec312(2) = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)); // third rotation (pitch)
const Dcmf R_to_earth = taitBryan312ToRotMat(rotVec312);
// rotate the magnetometer measurements into earth frame using a zero yaw angle
if (_control_status.flags.mag_3D) {
// don't apply bias corrections if we are learning them
mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
} else {
mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B);
}
// the angle of the projection onto the horizontal gives the yaw angle
measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
} else if (_control_status.flags.ev_yaw) {
// calculate the yaw angle for a 312 sequence
// Values from yaw_input_312.c file produced by https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw312.m
float Tbn_0_1_neg = 2.0f*(_ev_sample_delayed.quat(0)*_ev_sample_delayed.quat(3)-_ev_sample_delayed.quat(1)*_ev_sample_delayed.quat(2));
float Tbn_1_1 = sq(_ev_sample_delayed.quat(0))-sq(_ev_sample_delayed.quat(1))+sq(_ev_sample_delayed.quat(2))-sq(_ev_sample_delayed.quat(3));
measured_hdg = atan2f(Tbn_0_1_neg,Tbn_1_1);
} else {
measured_hdg = predicted_hdg;
}
// handle special case where yaw measurement is unavailable
bool fuse_zero_innov = false;
if (_yaw_use_inhibit) {
// The yaw measurement cannot be trusted but we need to fuse something to prevent a badly
// conditioned covariance matrix developing over time.
if (!_control_status.flags.vehicle_at_rest) {
// Vehicle is not at rest so fuse a zero innovation if necessary to prevent
// unconstrained quaterniion variance growth and record the predicted heading
// to use as an observation when movement ceases.
// TODO a better way of determining when this is necessary
float sumQuatVar = P(0,0) + P(1,1) + P(2,2) + P(3,3);
if (sumQuatVar > _params.quat_max_variance) {
fuse_zero_innov = true;
R_YAW = 0.25f;
}
_last_static_yaw = predicted_hdg;
} else {
// Vehicle is at rest so use the last moving prediction as an observation
// to prevent the heading from drifting and to enable yaw gyro bias learning
// before takeoff.
measured_hdg = _last_static_yaw;
}
} else {
_last_static_yaw = predicted_hdg;
}
fuseYaw312(measured_hdg, R_YAW, fuse_zero_innov);
}
}
void Ekf::fuseDeclination(float decl_sigma)
{
// assign intermediate state variables

5
EKF/terrain_estimator.cpp
View File

@ -88,6 +88,11 @@ bool Ekf::initHagl()
void Ekf::runTerrainEstimator()
{
// If we are on ground, store the local position and time to use as a reference
if (!_control_status.flags.in_air) {
_last_on_ground_posD = _state.pos(2);
}
// Perform initialisation check and
// on ground, continuously reset the terrain estimator
if (!_terrain_initialised || !_control_status.flags.in_air) {