mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-23 09:08:30 -04:00
AP_NavEKF2: Enable use of EKF-GSF yaw estimate
Enables the yaw to be reset in flight to a value estimated from a specialised yaw estimator. This allows faster recovery if taking off with a bad magnetometer and also allows yaw alignment and GPS use to commence in-air when operating without any yaw sensing. AP_NavEKF2: Add missing accesor functions for default airspeed
This commit is contained in:
Paul Riseborough
Andrew Tridgell
parent
863f989130
commit
de0040ad69
@ -15,8 +15,8 @@
|
||||
*/
|
||||
#if APM_BUILD_TYPE(APM_BUILD_ArduCopter) || APM_BUILD_TYPE(APM_BUILD_Replay)
|
||||
// copter defaults
|
||||
#define VELNE_M_NSE_DEFAULT 0.5f
|
||||
#define VELD_M_NSE_DEFAULT 0.7f
|
||||
#define VELNE_M_NSE_DEFAULT 0.3f
|
||||
#define VELD_M_NSE_DEFAULT 0.5f
|
||||
#define POSNE_M_NSE_DEFAULT 1.0f
|
||||
#define ALT_M_NSE_DEFAULT 3.0f
|
||||
#define MAG_M_NSE_DEFAULT 0.05f
|
||||
@ -245,7 +245,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
|
||||
|
||||
// @Param: MAG_CAL
|
||||
// @DisplayName: Magnetometer default fusion mode
|
||||
// @Description: This determines when the filter will use the 3-axis magnetometer fusion model that estimates both earth and body fixed magnetic field states and when it will use a simpler magnetic heading fusion model that does not use magnetic field states. The 3-axis magnetometer fusion is only suitable for use when the external magnetic field environment is stable. EK2_MAG_CAL = 0 uses heading fusion on ground, 3-axis fusion in-flight, and is the default setting for Plane users. EK2_MAG_CAL = 1 uses 3-axis fusion only when manoeuvring. EK2_MAG_CAL = 2 uses heading fusion at all times, is recommended if the external magnetic field is varying and is the default for rovers. EK2_MAG_CAL = 3 uses heading fusion on the ground and 3-axis fusion after the first in-air field and yaw reset has completed, and is the default for copters. EK2_MAG_CAL = 4 uses 3-axis fusion at all times. NOTE : Use of simple heading magnetometer fusion makes vehicle compass calibration and alignment errors harder for the EKF to detect which reduces the sensitivity of the Copter EKF failsafe algorithm. NOTE: The fusion mode can be forced to 2 for specific EKF cores using the EK2_MAG_MASK parameter.
|
||||
// @Description: This determines when the filter will use the 3-axis magnetometer fusion model that estimates both earth and body fixed magnetic field states, when it will use a simpler magnetic heading fusion model that does not use magnetic field states and when it will use an alternative method of yaw determination to the magnetometer. The 3-axis magnetometer fusion is only suitable for use when the external magnetic field environment is stable. EK3_MAG_CAL = 0 uses heading fusion on ground, 3-axis fusion in-flight, and is the default setting for Plane users. EK3_MAG_CAL = 1 uses 3-axis fusion only when manoeuvring. EK3_MAG_CAL = 2 uses heading fusion at all times, is recommended if the external magnetic field is varying and is the default for rovers. EK3_MAG_CAL = 3 uses heading fusion on the ground and 3-axis fusion after the first in-air field and yaw reset has completed, and is the default for copters. EK3_MAG_CAL = 4 uses 3-axis fusion at all times. NOTE: The fusion mode can be forced to 2 for specific EKF cores using the EK2_MAG_MASK parameter. NOTE: limited operation without a magnetometer or any other yaw sensor is possible by setting the COMPASS_USE parameters to 0. If this is done, the EK2_GSF_RUN and EK2_GSF_USE masks must be set to the same as EK3_IMU_MASK.
|
||||
// @Values: 0:When flying,1:When manoeuvring,2:Never,3:After first climb yaw reset,4:Always
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("MAG_CAL", 14, NavEKF2, _magCal, MAG_CAL_DEFAULT),
|
||||
@ -583,6 +583,41 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
|
||||
// @RebootRequired: False
|
||||
AP_GROUPINFO("HRT_FILT", 53, NavEKF2, _hrt_filt_freq, 2.0f),
|
||||
|
||||
// @Param: GSF_RUN
|
||||
// @DisplayName: Bitmask of which EKF-GSF yaw estimators run
|
||||
// @Description: 1 byte bitmap of which EKF2 instances run an independant EKF-GSF yaw estimator to provide a backup yaw estimate that doesn't rely on magnetometer data. This estimator uses IMU, GPS and, if available, airspeed data. EKF-GSF yaw estimator data for the primary EKF2 instance will be logged as GSF0 and GSF1 messages. Use of the yaw estimate generated by this algorithm is controlled by the EK2_GSF_USE, EK2_GSF_DELAY and EK2_GSF_MAXCOUNT parameters. To run the EKF-GSF yaw estimator in ride-along and logging only, set EK2_GSF_USE to 0.
|
||||
// @Bitmask: 0:FirstEKF,1:SecondEKF,2:ThirdEKF,3:FourthEKF,4:FifthEKF,5:SixthEKF
|
||||
// @User: Advanced
|
||||
// @RebootRequired: True
|
||||
AP_GROUPINFO("GSF_RUN", 54, NavEKF2, _gsfRunMask, 3),
|
||||
|
||||
// @Param: GSF_USE
|
||||
// @DisplayName: Bitmask of which EKF-GSF yaw estimators are used
|
||||
// @Description: 1 byte bitmap of which EKF2 instances will use the output from the EKF-GSF yaw estimator that has been turned on by the EK2_GSF_RUN parameter. If the inertial navigation calculation stops following the GPS, then the vehicle code can request EKF2 to attempt to resolve the issue, either by performing a yaw reset if enabled by this parameter by switching to another EKF2 instance. Additionally the EKF2 will initiate a reset internally if navigation is lost for more than EK2_GSF_DELAY milli seconds.
|
||||
// @Bitmask: 0:FirstEKF,1:SecondEKF,2:ThirdEKF,3:FourthEKF,4:FifthEKF,5:SixthEKF
|
||||
// @User: Advanced
|
||||
// @RebootRequired: True
|
||||
AP_GROUPINFO("GSF_USE", 55, NavEKF2, _gsfUseMask, 3),
|
||||
|
||||
// @Param: GSF_DELAY
|
||||
// @DisplayName: Delay from loss of navigation to yaw reset
|
||||
// @Description: If the inertial navigation calculation stops following the GPS and other positioning sensors for longer than EK2_GSF_DELAY milli-seconds, then the EKF2 code will generate a reset request internally and reset the yaw to the estimate from the EKF-GSF filter and reset the horizontal velocity and position to the GPS. This reset will not be performed unless the use of the EKF-GSF yaw estimate is enabled via the EK2_GSF_USE parameter.
|
||||
// @Range: 500 5000
|
||||
// @Increment: 100
|
||||
// @Units: ms
|
||||
// @User: Advanced
|
||||
// @RebootRequired: True
|
||||
AP_GROUPINFO("GSF_DELAY", 56, NavEKF2, _gsfResetDelay, 1000),
|
||||
|
||||
// @Param: GSF_MAXCOUNT
|
||||
// @DisplayName: Maximum number of resets to the EKF-GSF yaw estimate allowed
|
||||
// @Description: Sets the maximum number of times the EKF2 will be allowed to reset it's yaw to the estimate from the EKF-GSF yaw estimator. No resets will be allowed unless the use of the EKF-GSF yaw estimate is enabled via the EK2_GSF_USE parameter.
|
||||
// @Range: 1 10
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
// @RebootRequired: True
|
||||
AP_GROUPINFO("GSF_MAXCOUNT", 57, NavEKF2, _gsfResetMaxCount, 2),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
@ -1649,3 +1684,32 @@ bool NavEKF2::isExtNavUsedForYaw() const
|
||||
return false;
|
||||
}
|
||||
|
||||
void NavEKF2::requestYawReset(void)
|
||||
{
|
||||
for (uint8_t i = 0; i < num_cores; i++) {
|
||||
core[primary].EKFGSF_requestYawReset();
|
||||
}
|
||||
}
|
||||
|
||||
// return data for debugging EKF-GSF yaw estimator
|
||||
// return false if data not available
|
||||
bool NavEKF2::getDataEKFGSF(int8_t instance, float *yaw_composite, float *yaw_composite_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) const
|
||||
{
|
||||
if (instance < 0 || instance >= num_cores) instance = primary;
|
||||
if (core) {
|
||||
if (core[instance].getDataEKFGSF(yaw_composite, yaw_composite_variance, yaw, innov_VN, innov_VE, weight)) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available.
|
||||
void NavEKF2::writeDefaultAirSpeed(float airspeed)
|
||||
{
|
||||
if (core) {
|
||||
for (uint8_t i=0; i<num_cores; i++) {
|
||||
core[i].writeDefaultAirSpeed(airspeed);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -350,12 +350,25 @@ public:
|
||||
*/
|
||||
void checkLaneSwitch(void);
|
||||
|
||||
/*
|
||||
Request a reset of the EKF yaw. This is called when the vehicle code is about to
|
||||
trigger an EKF failsafe, and it would like to avoid that.
|
||||
*/
|
||||
void requestYawReset(void);
|
||||
|
||||
// write EKF information to on-board logs
|
||||
void Log_Write();
|
||||
|
||||
// check if external navigation is being used for yaw observation
|
||||
bool isExtNavUsedForYaw(void) const;
|
||||
|
||||
// Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available.
|
||||
void writeDefaultAirSpeed(float airspeed);
|
||||
|
||||
// log debug data for yaw estimator
|
||||
// return false if data not available
|
||||
bool getDataEKFGSF(int8_t instance, float *yaw_composite, float *yaw_composite_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) const;
|
||||
|
||||
private:
|
||||
uint8_t num_cores; // number of allocated cores
|
||||
uint8_t primary; // current primary core
|
||||
@ -419,6 +432,10 @@ private:
|
||||
AP_Int8 _flowUse; // Controls if the optical flow data is fused into the main navigation estimator and/or the terrain estimator.
|
||||
AP_Int16 _mag_ef_limit; // limit on difference between WMM tables and learned earth field.
|
||||
AP_Float _hrt_filt_freq; // frequency of output observer height rate complementary filter in Hz
|
||||
AP_Int8 _gsfRunMask; // mask controlling which EKF3 instances run a separate EKF-GSF yaw estimator
|
||||
AP_Int8 _gsfUseMask; // mask controlling which EKF3 instances will use EKF-GSF yaw estimator data to assit with yaw resets
|
||||
AP_Int16 _gsfResetDelay; // number of mSec from loss of navigation to requesting a reset using EKF-GSF yaw estimator data
|
||||
AP_Int8 _gsfResetMaxCount; // maximum number of times the EKF3 is allowed to reset it's yaw to the EKF-GSF estimate
|
||||
|
||||
// Possible values for _flowUse
|
||||
#define FLOW_USE_NONE 0
|
||||
@ -537,4 +554,5 @@ private:
|
||||
void Log_Write_NKF5(uint64_t time_us) const;
|
||||
void Log_Write_Quaternion(uint8_t core, uint64_t time_us) const;
|
||||
void Log_Write_Beacon(uint64_t time_us) const;
|
||||
void Log_Write_GSF(uint8_t core, uint64_t time_us) const;
|
||||
};
|
||||
|
||||
@ -230,6 +230,7 @@ void NavEKF2_core::setAidingMode()
|
||||
|
||||
// Check if the loss of position accuracy has become critical
|
||||
bool posAidLossCritical = false;
|
||||
bool posAidLossPending = false;
|
||||
if (!posAiding ) {
|
||||
uint16_t maxLossTime_ms;
|
||||
if (!velAiding) {
|
||||
@ -239,6 +240,8 @@ void NavEKF2_core::setAidingMode()
|
||||
}
|
||||
posAidLossCritical = (imuSampleTime_ms - lastRngBcnPassTime_ms > maxLossTime_ms) &&
|
||||
(imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms);
|
||||
posAidLossPending = (imuSampleTime_ms > lastRngBcnPassTime_ms + frontend->_gsfResetDelay) &&
|
||||
(imuSampleTime_ms > lastPosPassTime_ms + frontend->_gsfResetDelay);
|
||||
}
|
||||
|
||||
if (attAidLossCritical) {
|
||||
@ -258,6 +261,9 @@ void NavEKF2_core::setAidingMode()
|
||||
velTimeout = true;
|
||||
rngBcnTimeout = true;
|
||||
gpsNotAvailable = true;
|
||||
} else if (posAidLossPending) {
|
||||
// attempt to reset the yaw to the estimate from the EKF-GSF algorithm
|
||||
EKFGSF_yaw_reset_request_ms = imuSampleTime_ms;
|
||||
}
|
||||
break;
|
||||
}
|
||||
@ -465,9 +471,18 @@ bool NavEKF2_core::checkGyroCalStatus(void)
|
||||
{
|
||||
// check delta angle bias variances
|
||||
const float delAngBiasVarMax = sq(radians(0.15f * dtEkfAvg));
|
||||
delAngBiasLearned = (P[9][9] <= delAngBiasVarMax) &&
|
||||
(P[10][10] <= delAngBiasVarMax) &&
|
||||
(P[11][11] <= delAngBiasVarMax);
|
||||
if (!use_compass()) {
|
||||
// rotate the variances into earth frame and evaluate horizontal terms only as yaw component is poorly observable without a compass
|
||||
// which can make this check fail
|
||||
Vector3f delAngBiasVarVec = Vector3f(P[9][9],P[10][10],P[11][11]);
|
||||
Vector3f temp = prevTnb * delAngBiasVarVec;
|
||||
delAngBiasLearned = (fabsf(temp.x) < delAngBiasVarMax) &&
|
||||
(fabsf(temp.y) < delAngBiasVarMax);
|
||||
} else {
|
||||
delAngBiasLearned = (P[9][9] <= delAngBiasVarMax) &&
|
||||
(P[10][10] <= delAngBiasVarMax) &&
|
||||
(P[11][11] <= delAngBiasVarMax);
|
||||
}
|
||||
return delAngBiasLearned;
|
||||
}
|
||||
|
||||
@ -524,3 +539,39 @@ void NavEKF2_core::updateFilterStatus(void)
|
||||
filterStatus.flags.initalized = filterStatus.flags.initalized || healthy();
|
||||
}
|
||||
|
||||
void NavEKF2_core::runYawEstimator()
|
||||
{
|
||||
if (yawEstimator != nullptr && frontend->_fusionModeGPS <= 1) {
|
||||
float trueAirspeed;
|
||||
if (is_positive(defaultAirSpeed) && assume_zero_sideslip()) {
|
||||
if (imuDataDelayed.time_ms < (tasDataDelayed.time_ms + 5000)) {
|
||||
trueAirspeed = tasDataDelayed.tas;
|
||||
} else {
|
||||
trueAirspeed = defaultAirSpeed * AP::ahrs().get_EAS2TAS();
|
||||
}
|
||||
} else {
|
||||
trueAirspeed = 0.0f;
|
||||
}
|
||||
|
||||
yawEstimator->update(imuDataDelayed.delAng, imuDataDelayed.delVel, imuDataDelayed.delAngDT, imuDataDelayed.delVelDT, EKFGSF_run_filterbank, trueAirspeed);
|
||||
|
||||
if (gpsDataToFuse) {
|
||||
Vector2f gpsVelNE = Vector2f(gpsDataDelayed.vel.x, gpsDataDelayed.vel.y);
|
||||
float gpsVelAcc = fmaxf(gpsSpdAccuracy, frontend->_gpsHorizVelNoise);
|
||||
yawEstimator->pushVelData(gpsVelNE, gpsVelAcc);
|
||||
}
|
||||
|
||||
// action an external reset request
|
||||
if (EKFGSF_yaw_reset_request_ms > 0 && imuSampleTime_ms < (EKFGSF_yaw_reset_request_ms + YAW_RESET_TO_GSF_TIMEOUT_MS)) {
|
||||
EKFGSF_resetMainFilterYaw();
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// request a reset the yaw to the GSF estimate
|
||||
// request times out after YAW_RESET_TO_GSF_TIMEOUT_MS if it cannot be actioned
|
||||
void NavEKF2_core::EKFGSF_requestYawReset()
|
||||
{
|
||||
EKFGSF_yaw_reset_request_ms = imuSampleTime_ms;
|
||||
}
|
||||
@ -253,6 +253,7 @@ void NavEKF2::Log_Write()
|
||||
Log_Write_NKF3(i, time_us);
|
||||
Log_Write_NKF4(i, time_us);
|
||||
Log_Write_Quaternion(i, time_us);
|
||||
Log_Write_GSF(i, time_us);
|
||||
}
|
||||
|
||||
// write range beacon fusion debug packet if the range value is non-zero
|
||||
@ -269,3 +270,53 @@ void NavEKF2::Log_Write()
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void NavEKF2::Log_Write_GSF(uint8_t _core, uint64_t time_us) const
|
||||
{
|
||||
float yaw_composite;
|
||||
float yaw_composite_variance;
|
||||
float yaw[N_MODELS_EKFGSF];
|
||||
float ivn[N_MODELS_EKFGSF];
|
||||
float ive[N_MODELS_EKFGSF];
|
||||
float wgt[N_MODELS_EKFGSF];
|
||||
|
||||
if (getDataEKFGSF(_core, &yaw_composite, &yaw_composite_variance, yaw, ivn, ive, wgt)) {
|
||||
AP::logger().Write("GSF0",
|
||||
"TimeUS,C,YC,YCS,Y0,Y1,Y2,Y3,Y4,W0,W1,W2,W3,W4",
|
||||
"s#rrrrrrr-----",
|
||||
"F-000000000000",
|
||||
"QBffffffffffff",
|
||||
time_us,
|
||||
_core,
|
||||
yaw_composite,
|
||||
sqrtf(MAX(yaw_composite_variance, 0.0f)),
|
||||
yaw[0],
|
||||
yaw[1],
|
||||
yaw[2],
|
||||
yaw[3],
|
||||
yaw[4],
|
||||
wgt[0],
|
||||
wgt[1],
|
||||
wgt[2],
|
||||
wgt[3],
|
||||
wgt[4]);
|
||||
|
||||
AP::logger().Write("GSF1",
|
||||
"TimeUS,C,IVN0,IVN1,IVN2,IVN3,IVN4,IVE0,IVE1,IVE2,IVE3,IVE4",
|
||||
"s#nnnnnnnnnn",
|
||||
"F-0000000000",
|
||||
"QBffffffffff",
|
||||
time_us,
|
||||
_core,
|
||||
ivn[0],
|
||||
ivn[1],
|
||||
ivn[2],
|
||||
ivn[3],
|
||||
ivn[4],
|
||||
ive[0],
|
||||
ive[1],
|
||||
ive[2],
|
||||
ive[3],
|
||||
ive[4]);
|
||||
}
|
||||
}
|
||||
@ -50,6 +50,11 @@ void NavEKF2_core::controlMagYawReset()
|
||||
|
||||
}
|
||||
|
||||
// reset the limit on the number of magnetic anomaly resets for each takeoff
|
||||
if (onGround) {
|
||||
magYawAnomallyCount = 0;
|
||||
}
|
||||
|
||||
// Check if conditions for a interim or final yaw/mag reset are met
|
||||
bool finalResetRequest = false;
|
||||
bool interimResetRequest = false;
|
||||
@ -72,7 +77,12 @@ void NavEKF2_core::controlMagYawReset()
|
||||
|
||||
// if yaw innovations and height have increased and we haven't rotated much
|
||||
// then we are climbing away from a ground based magnetic anomaly and need to reset
|
||||
interimResetRequest = hgtIncreasing && yawInnovIncreasing && !largeAngleChange;
|
||||
interimResetRequest = !finalInflightYawInit
|
||||
&& !finalResetRequest
|
||||
&& (magYawAnomallyCount < MAG_ANOMALY_RESET_MAX)
|
||||
&& hgtIncreasing
|
||||
&& yawInnovIncreasing
|
||||
&& !largeAngleChange;
|
||||
}
|
||||
|
||||
// an initial reset is required if we have not yet aligned the yaw angle
|
||||
@ -236,6 +246,19 @@ void NavEKF2_core::SelectMagFusion()
|
||||
// used for load levelling
|
||||
magFusePerformed = false;
|
||||
|
||||
// Handle case where we are not using a yaw sensor of any type and and attempt to reset the yaw in
|
||||
// flight using the output from the GSF yaw estimator.
|
||||
if (!use_compass() && tiltAlignComplete) {
|
||||
if ((onGround || !assume_zero_sideslip()) && (imuSampleTime_ms - lastYawTime_ms > 140)) {
|
||||
fuseEulerYaw();
|
||||
}
|
||||
if (yawAlignComplete) {
|
||||
return;
|
||||
}
|
||||
yawAlignComplete = EKFGSF_resetMainFilterYaw();
|
||||
return;
|
||||
}
|
||||
|
||||
// check for and read new magnetometer measurements
|
||||
readMagData();
|
||||
|
||||
@ -287,16 +310,6 @@ void NavEKF2_core::SelectMagFusion()
|
||||
}
|
||||
}
|
||||
|
||||
// If we have no magnetometer and are on the ground, fuse in a synthetic heading measurement to prevent the
|
||||
// filter covariances from becoming badly conditioned
|
||||
if (!use_compass()) {
|
||||
if (onGround && (imuSampleTime_ms - lastYawTime_ms > 1000)) {
|
||||
fuseEulerYaw();
|
||||
magTestRatio.zero();
|
||||
yawTestRatio = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
// If the final yaw reset has been performed and the state variances are sufficiently low
|
||||
// record that the earth field has been learned.
|
||||
if (!magFieldLearned && finalInflightMagInit) {
|
||||
@ -771,8 +784,8 @@ void NavEKF2_core::fuseEulerYaw()
|
||||
float q2 = stateStruct.quat[2];
|
||||
float q3 = stateStruct.quat[3];
|
||||
|
||||
// compass measurement error variance (rad^2)
|
||||
const float R_YAW = sq(frontend->_yawNoise);
|
||||
// compass measurement error variance (rad^2) set to parameter value as a default
|
||||
float R_YAW = sq(frontend->_yawNoise);
|
||||
|
||||
// calculate observation jacobian, predicted yaw and zero yaw body to earth rotation matrix
|
||||
// determine if a 321 or 312 Euler sequence is best
|
||||
@ -825,8 +838,21 @@ void NavEKF2_core::fuseEulerYaw()
|
||||
extNavDataDelayed.quat.to_euler(euler321.x, euler321.y, euler321.z);
|
||||
measured_yaw = euler321.z;
|
||||
} else {
|
||||
// no data so use predicted to prevent unconstrained variance growth
|
||||
measured_yaw = predicted_yaw;
|
||||
if (imuSampleTime_ms > prevBetaStep_ms + 1000 && yawEstimator != nullptr) {
|
||||
float gsfYaw, gsfYawVariance;
|
||||
if (yawEstimator->getYawData(&gsfYaw, &gsfYawVariance) &&
|
||||
is_positive(gsfYawVariance) &&
|
||||
gsfYawVariance < sq(radians(15.0f))) {
|
||||
measured_yaw = gsfYaw;
|
||||
R_YAW = gsfYawVariance;
|
||||
} else {
|
||||
// use predicted to prevent unconstrained variance growth
|
||||
measured_yaw = predicted_yaw;
|
||||
}
|
||||
} else {
|
||||
// use predicted to prevent unconstrained variance growth
|
||||
measured_yaw = predicted_yaw;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// calculate observation jacobian when we are observing a rotation in a 312 sequence
|
||||
@ -871,8 +897,21 @@ void NavEKF2_core::fuseEulerYaw()
|
||||
euler312 = extNavDataDelayed.quat.to_vector312();
|
||||
measured_yaw = euler312.z;
|
||||
} else {
|
||||
// no data so use predicted to prevent unconstrained variance growth
|
||||
measured_yaw = predicted_yaw;
|
||||
if (imuSampleTime_ms > prevBetaStep_ms + 1000 && yawEstimator != nullptr) {
|
||||
float gsfYaw, gsfYawVariance;
|
||||
if (yawEstimator->getYawData(&gsfYaw, &gsfYawVariance) &&
|
||||
is_positive(gsfYawVariance) &&
|
||||
gsfYawVariance < sq(radians(15.0f))) {
|
||||
measured_yaw = gsfYaw;
|
||||
R_YAW = gsfYawVariance;
|
||||
} else {
|
||||
// use predicted to prevent unconstrained variance growth
|
||||
measured_yaw = predicted_yaw;
|
||||
}
|
||||
} else {
|
||||
// use predicted to prevent unconstrained variance growth
|
||||
measured_yaw = predicted_yaw;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -1174,3 +1213,111 @@ void NavEKF2_core::recordMagReset()
|
||||
yawInnovAtLastMagReset = innovYaw;
|
||||
}
|
||||
|
||||
// Reset states using yaw from EKF-GSF and velocity and position from GPS
|
||||
bool NavEKF2_core::EKFGSF_resetMainFilterYaw()
|
||||
{
|
||||
// Don't do a reset unless permitted by the EK3_GSF_USE and EKF3_GSF_RUN parameter masks
|
||||
if ((yawEstimator == nullptr)
|
||||
|| !(frontend->_gsfUseMask & (1U<<core_index))
|
||||
|| EKFGSF_yaw_reset_count >= frontend->_gsfResetMaxCount) {
|
||||
return false;
|
||||
};
|
||||
|
||||
float yawEKFGSF, yawVarianceEKFGSF;
|
||||
if (yawEstimator->getYawData(&yawEKFGSF, &yawVarianceEKFGSF) && is_positive(yawVarianceEKFGSF) && yawVarianceEKFGSF < sq(radians(15.0f))) {
|
||||
|
||||
// keep roll and pitch and reset yaw
|
||||
resetQuatStateYawOnly(yawEKFGSF, yawVarianceEKFGSF, false);
|
||||
|
||||
// record the emergency reset event
|
||||
EKFGSF_yaw_reset_request_ms = 0;
|
||||
EKFGSF_yaw_reset_ms = imuSampleTime_ms;
|
||||
EKFGSF_yaw_reset_count++;
|
||||
|
||||
if (!use_compass()) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u yaw aligned using GPS",(unsigned)imu_index);
|
||||
} else {
|
||||
gcs().send_text(MAV_SEVERITY_WARNING, "EKF3 IMU%u emergency yaw reset - mag sensor stopped",(unsigned)imu_index);
|
||||
}
|
||||
|
||||
// Fail the magnetomer so it doesn't get used and pull the yaw away from the correct value
|
||||
allMagSensorsFailed = true;
|
||||
|
||||
// reset velocity and position states to GPS - if yaw is fixed then the filter should start to operate correctly
|
||||
ResetVelocity();
|
||||
ResetPosition();
|
||||
|
||||
// reset test ratios that are reported to prevent a race condition with the external state machine requesting the reset
|
||||
velTestRatio = 0.0f;
|
||||
posTestRatio = 0.0f;
|
||||
|
||||
return true;
|
||||
|
||||
}
|
||||
|
||||
return false;
|
||||
|
||||
}
|
||||
|
||||
void NavEKF2_core::resetQuatStateYawOnly(float yaw, float yawVariance, bool isDeltaYaw)
|
||||
{
|
||||
Quaternion quatBeforeReset = stateStruct.quat;
|
||||
|
||||
// check if we should use a 321 or 312 Rotation sequence and update the quaternion
|
||||
// states using the preferred yaw definition
|
||||
stateStruct.quat.inverse().rotation_matrix(prevTnb);
|
||||
Vector3f eulerAngles;
|
||||
if (fabsf(prevTnb[2][0]) < fabsf(prevTnb[2][1])) {
|
||||
// rolled more than pitched so use 321 rotation order
|
||||
stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
|
||||
if (isDeltaYaw) {
|
||||
yaw = wrap_PI(yaw + eulerAngles.z);
|
||||
}
|
||||
stateStruct.quat.from_euler(eulerAngles.x, eulerAngles.y, yaw);
|
||||
} else {
|
||||
// pitched more than rolled so use 312 rotation order
|
||||
eulerAngles = stateStruct.quat.to_vector312();
|
||||
if (isDeltaYaw) {
|
||||
yaw = wrap_PI(yaw + eulerAngles.z);
|
||||
}
|
||||
stateStruct.quat.from_vector312(eulerAngles.x, eulerAngles.y, yaw);
|
||||
}
|
||||
|
||||
// Update the rotation matrix
|
||||
stateStruct.quat.inverse().rotation_matrix(prevTnb);
|
||||
|
||||
float deltaYaw = wrap_PI(yaw - eulerAngles.z);
|
||||
|
||||
// calculate the change in the quaternion state and apply it to the output history buffer
|
||||
Quaternion quat_delta = stateStruct.quat / quatBeforeReset;
|
||||
StoreQuatRotate(quat_delta);
|
||||
|
||||
// rotate attitude error variances into earth frame
|
||||
Vector3f bf_variances = Vector3f(P[0][0], P[1][1], P[2][2]);
|
||||
Vector3f ef_variances = prevTnb.transposed() * bf_variances;
|
||||
|
||||
// reset vertical component to supplied value
|
||||
ef_variances.z = yawVariance;
|
||||
|
||||
// rotate back into body frame
|
||||
bf_variances = prevTnb * ef_variances;
|
||||
|
||||
// Reset all attitude error state covariances
|
||||
zeroRows(P, 0, 2);
|
||||
zeroCols(P, 0, 2);
|
||||
|
||||
// Initialise variances
|
||||
P[0][0] = bf_variances.x;
|
||||
P[1][1] = bf_variances.y;
|
||||
P[2][2] = bf_variances.z;
|
||||
|
||||
// record the yaw reset event
|
||||
yawResetAngle += deltaYaw;
|
||||
lastYawReset_ms = imuSampleTime_ms;
|
||||
recordYawReset();
|
||||
|
||||
// clear all pending yaw reset requests
|
||||
gpsYawResetRequest = false;
|
||||
magYawResetRequest = false;
|
||||
|
||||
}
|
||||
@ -619,6 +619,10 @@ void NavEKF2_core::readGpsData()
|
||||
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "Waiting for 3D fix");
|
||||
}
|
||||
}
|
||||
|
||||
// get data that has now fallen behind the fusion time horizon
|
||||
gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms);
|
||||
|
||||
}
|
||||
|
||||
// read the delta angle and corresponding time interval from the IMU
|
||||
@ -1013,3 +1017,8 @@ void NavEKF2_core::learnInactiveBiases(void)
|
||||
}
|
||||
}
|
||||
|
||||
// Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available.
|
||||
void NavEKF2_core::writeDefaultAirSpeed(float airspeed)
|
||||
{
|
||||
defaultAirSpeed = airspeed;
|
||||
}
|
||||
|
||||
@ -633,3 +633,10 @@ bool NavEKF2_core::isExtNavUsedForYaw()
|
||||
return extNavUsedForYaw;
|
||||
}
|
||||
|
||||
bool NavEKF2_core::getDataEKFGSF(float *yaw_composite, float *yaw_composite_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF])
|
||||
{
|
||||
if (yawEstimator != nullptr) {
|
||||
return yawEstimator->getLogData(yaw_composite, yaw_composite_variance, yaw, innov_VN, innov_VE, weight);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
@ -315,9 +315,6 @@ void NavEKF2_core::SelectVelPosFusion()
|
||||
// Check for data at the fusion time horizon
|
||||
extNavDataToFuse = storedExtNav.recall(extNavDataDelayed, imuDataDelayed.time_ms);
|
||||
|
||||
// read GPS data from the sensor and check for new data in the buffer
|
||||
readGpsData();
|
||||
gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms);
|
||||
// Determine if we need to fuse position and velocity data on this time step
|
||||
if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE) {
|
||||
// set fusion request flags
|
||||
|
||||
@ -396,6 +396,18 @@ void NavEKF2_core::detectFlight()
|
||||
|
||||
}
|
||||
|
||||
// handle reset of counters used to control how many times we will try to reset the yaw to the EKF-GSF value per flight
|
||||
if (!prevOnGround && onGround) {
|
||||
// landed so disable filter bank
|
||||
EKFGSF_run_filterbank = false;
|
||||
} else if (!prevInFlight && inFlight) {
|
||||
// started flying so reset counters and enable filter bank
|
||||
EKFGSF_yaw_reset_ms = 0;
|
||||
EKFGSF_yaw_reset_request_ms = 0;
|
||||
EKFGSF_yaw_reset_count = 0;
|
||||
EKFGSF_run_filterbank = true;
|
||||
}
|
||||
|
||||
// store current on-ground and in-air status for next time
|
||||
prevOnGround = onGround;
|
||||
prevInFlight = inFlight;
|
||||
|
||||
@ -111,6 +111,21 @@ bool NavEKF2_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
|
||||
return false;
|
||||
}
|
||||
|
||||
if ((yawEstimator == nullptr) && (frontend->_gsfRunMask & (1U<<core_index))) {
|
||||
// check if there is enough memory to create the EKF-GSF object
|
||||
if (hal.util->available_memory() < sizeof(EKFGSF_yaw) + 1024) {
|
||||
gcs().send_text(MAV_SEVERITY_CRITICAL, "EKF2 IMU%u GSF: not enough memory",(unsigned)imu_index);
|
||||
return false;
|
||||
}
|
||||
|
||||
// try to instantiate
|
||||
yawEstimator = new EKFGSF_yaw();
|
||||
if (yawEstimator == nullptr) {
|
||||
gcs().send_text(MAV_SEVERITY_CRITICAL, "EKF2 IMU%uGSF: allocation failed",(unsigned)imu_index);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -340,6 +355,12 @@ void NavEKF2_core::InitialiseVariables()
|
||||
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF2 still initialising");
|
||||
|
||||
InitialiseVariablesMag();
|
||||
|
||||
// emergency reset of yaw to EKFGSF estimate
|
||||
EKFGSF_yaw_reset_ms = 0;
|
||||
EKFGSF_yaw_reset_request_ms = 0;
|
||||
EKFGSF_yaw_reset_count = 0;
|
||||
EKFGSF_run_filterbank = false;
|
||||
}
|
||||
|
||||
|
||||
@ -567,6 +588,14 @@ void NavEKF2_core::UpdateFilter(bool predict)
|
||||
// Predict the covariance growth
|
||||
CovariancePrediction();
|
||||
|
||||
// Read GPS data from the sensor
|
||||
// This is required by multiple processes so needs to be done before other fusion steps
|
||||
readGpsData();
|
||||
|
||||
// Generate an alternative yaw estimate used for inflight recovery from bad compass data
|
||||
// requires horizontal GPS velocity
|
||||
runYawEstimator();
|
||||
|
||||
// Update states using magnetometer data
|
||||
SelectMagFusion();
|
||||
|
||||
|
||||
@ -33,6 +33,7 @@
|
||||
#include <AP_NavEKF/AP_NavEKF_core_common.h>
|
||||
#include <AP_NavEKF2/AP_NavEKF2_Buffer.h>
|
||||
#include <AP_InertialSensor/AP_InertialSensor.h>
|
||||
#include "AP_NavEKF/EKFGSF_yaw.h"
|
||||
|
||||
// GPS pre-flight check bit locations
|
||||
#define MASK_GPS_NSATS (1<<0)
|
||||
@ -57,6 +58,12 @@
|
||||
// mag fusion final reset altitude
|
||||
#define EKF2_MAG_FINAL_RESET_ALT 2.5f
|
||||
|
||||
// maximum number of yaw resets due to detected magnetic anomaly allowed per flight
|
||||
#define MAG_ANOMALY_RESET_MAX 2
|
||||
|
||||
// number of seconds a request to reset the yaw to the GSF estimate is active before it times out
|
||||
#define YAW_RESET_TO_GSF_TIMEOUT_MS 5000
|
||||
|
||||
class AP_AHRS;
|
||||
|
||||
class NavEKF2_core : public NavEKF_core_common
|
||||
@ -329,7 +336,19 @@ public:
|
||||
// return true when external nav data is also being used as a yaw observation
|
||||
bool isExtNavUsedForYaw(void);
|
||||
|
||||
// get solution data for the EKF-GSF emergency yaw estimator
|
||||
// return false if data not available
|
||||
bool getDataEKFGSF(float *yaw_composite, float *yaw_composite_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]);
|
||||
|
||||
// Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available.
|
||||
void writeDefaultAirSpeed(float airspeed);
|
||||
|
||||
// request a reset the yaw to the EKF-GSF value
|
||||
void EKFGSF_requestYawReset();
|
||||
|
||||
private:
|
||||
EKFGSF_yaw *yawEstimator;
|
||||
|
||||
// Reference to the global EKF frontend for parameters
|
||||
NavEKF2 *frontend;
|
||||
uint8_t imu_index; // preferred IMU index
|
||||
@ -781,6 +800,22 @@ private:
|
||||
|
||||
// correct external navigation earth-frame position using sensor body-frame offset
|
||||
void CorrectExtNavForSensorOffset(Vector3f &ext_position) const;
|
||||
|
||||
// Run an independent yaw estimator algorithm that uses IMU, GPs horizontal velocity
|
||||
// and optionally true airspeed data.
|
||||
void runYawEstimator(void);
|
||||
|
||||
// reset the quaternion states using the supplied yaw angle, maintaining the previous roll and pitch
|
||||
// also reset the body to nav frame rotation matrix
|
||||
// reset the quaternion state covariances using the supplied yaw variance
|
||||
// yaw : new yaw angle (rad)
|
||||
// yaw_variance : variance of new yaw angle (rad^2)
|
||||
// isDeltaYaw : true when the yaw should be added to the existing yaw angle
|
||||
void resetQuatStateYawOnly(float yaw, float yawVariance, bool isDeltaYaw);
|
||||
|
||||
// attempt to reset the yaw to the EKF-GSF value
|
||||
// returns false if unsuccessful
|
||||
bool EKFGSF_resetMainFilterYaw();
|
||||
|
||||
// Length of FIFO buffers used for non-IMU sensor data.
|
||||
// Must be larger than the time period defined by IMU_BUFFER_LENGTH
|
||||
@ -864,6 +899,7 @@ private:
|
||||
float hgtTestRatio; // sum of squares of baro height innovation divided by fail threshold
|
||||
Vector3f magTestRatio; // sum of squares of magnetometer innovations divided by fail threshold
|
||||
float tasTestRatio; // sum of squares of true airspeed innovation divided by fail threshold
|
||||
float defaultAirSpeed; // default equivalent airspeed in m/s to be used if the measurement is unavailable. Do not use if not positive.
|
||||
bool inhibitWindStates; // true when wind states and covariances are to remain constant
|
||||
bool inhibitMagStates; // true when magnetic field states and covariances are to remain constant
|
||||
bool gpsNotAvailable; // bool true when valid GPS data is not available
|
||||
@ -1114,6 +1150,7 @@ private:
|
||||
float posDownAtLastMagReset; // vertical position last time the mag states were reset (m)
|
||||
float yawInnovAtLastMagReset; // magnetic yaw innovation last time the yaw and mag field states were reset (rad)
|
||||
Quaternion quatAtLastMagReset; // quaternion states last time the mag states were reset
|
||||
uint8_t magYawAnomallyCount; // Number of times the yaw has been reset due to a magnetic anomaly during initial ascent
|
||||
|
||||
// external navigation fusion
|
||||
obs_ring_buffer_t<ext_nav_elements> storedExtNav; // external navigation data buffer
|
||||
@ -1212,4 +1249,10 @@ private:
|
||||
|
||||
// vehicle specific initial gyro bias uncertainty
|
||||
float InitialGyroBiasUncertainty(void) const;
|
||||
|
||||
// The following declarations are used to control when the main navigation filter resets it's yaw to the estimate provided by the GSF
|
||||
uint64_t EKFGSF_yaw_reset_ms; // timestamp of last emergency yaw reset (uSec)
|
||||
uint64_t EKFGSF_yaw_reset_request_ms; // timestamp of last emergency yaw reset request (uSec)
|
||||
uint8_t EKFGSF_yaw_reset_count; // number of emergency yaw resets performed
|
||||
bool EKFGSF_run_filterbank; // true when the filter bank is active
|
||||
};
|
||||
|
||||
Loading…
Reference in New Issue
Block a user