AP_NavEKF3: Enable use of EKF-GSF yaw estimate
AP_NavEKF3: Add emergency yaw reset using a Gussian Sum Filter estimate AP_NavEKF3: Reduce default minimum GPS velocity noise for Copters Enables fail-safe to be set with more sensitivity and improves tracking accuracy. Origin values were set using typical GPS performance for receivers on sale 6 years ago. Receiver performance has improved since then. AP_NavEKF3: Prevent constant mag anomaly yaw resets Prevents constant magnetic anomaly induced resets that can be triggered when flying with vehicle generated magnetic interference. Allows for two resets per takeoff. Allowance for two resets is required, becasue a large ground based magnetic yaw anomaly can cause a sufficiently large yaw innovation that two resets in close succession are required. AP_NavEKF3: Add option to fly without magnetometer AP_NavEKF3: Rework emergency yaw reset logic Use a separate external accessor function to request the yaw reset. Allow reset requests to remain active for a defined period of time. Tidy up reset function to split out accuracy check. AP_NavEKF3: Fix vulnerability to lane switch race condition Prevents the situation where a lane switch results in a lane being selected that does not have the correct yaw. This can occur if the primary lane becomes unhealthy before the external failsafe monitor has time to react. AP_NavEKF3: Fix EKF_MAG_CAL = 6 behaviours Fix bug causing the yaw alignment to be performed at startup before the GSF had a valid estimate. Fix bug causing emergency yaw message to be output for a normal reset. Fix vulnerability to reported negative yaw variance. Remove duplicate timer checks. AP_NavEKF3: Update EK3_MAG_CAL documentation AP_NavEKF3: Relax yaw gyro bias convergence check when not using mag AP_NavEKF3: Reduce yaw drift in hover with no yaw sensor Uses the GSF yaw estimate if available which is better than the EKF's own yaw when no yaw sensor is available.
This commit is contained in:
Paul Riseborough
Andrew Tridgell
parent
6bb7c55bba
commit
410b5825fb
@ -14,8 +14,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 0.5f
|
||||
#define ALT_M_NSE_DEFAULT 2.0f
|
||||
#define MAG_M_NSE_DEFAULT 0.05f
|
||||
@ -604,6 +604,41 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
|
||||
// @Units: mGauss
|
||||
AP_GROUPINFO("MAG_EF_LIM", 56, NavEKF3, _mag_ef_limit, 50),
|
||||
|
||||
// @Param: GSF_RUN
|
||||
// @DisplayName: Bitmask of which EKF-GSF yaw estimators run
|
||||
// @Description: 1 byte bitmap of which EKF3 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 EKF3 instance will be logged as GSF0 and GSF1 messages. Use of the yaw estimate generated by this algorithm is controlled by the EK3_GSF_USE, EK3_GSF_DELAY and EK3_GSF_MAXCOUNT parameters. To run the EKF-GSF yaw estimator in ride-along and logging only, set EK3_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", 57, NavEKF3, _gsfRunMask, 3),
|
||||
|
||||
// @Param: GSF_USE
|
||||
// @DisplayName: Bitmask of which EKF-GSF yaw estimators are used
|
||||
// @Description: 1 byte bitmap of which EKF3 instances will use the output from the EKF-GSF yaw estimator that has been turned on by the EK3_GSF_RUN parameter. If the inertial navigation calculation stops following the GPS, then the vehicle code can request EKF3 to attempt to resolve the issue, either by performing a yaw reset if enabled by this parameter by switching to another EKF3 instance. Additionally the EKF3 will initiate a reset internally if navigation is lost for more than EK3_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", 58, NavEKF3, _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 EK3_GSF_DELAY milli-seconds, then the EKF3 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 EK3_GSF_USE parameter.
|
||||
// @Range: 500 5000
|
||||
// @Increment: 100
|
||||
// @Units: ms
|
||||
// @User: Advanced
|
||||
// @RebootRequired: True
|
||||
AP_GROUPINFO("GSF_DELAY", 59, NavEKF3, _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 EKF3 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 EK3_GSF_USE parameter.
|
||||
// @Range: 1 10
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
// @RebootRequired: True
|
||||
AP_GROUPINFO("GSF_MAXCOUNT", 60, NavEKF3, _gsfResetMaxCount, 2),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
@ -837,6 +872,7 @@ void NavEKF3::checkLaneSwitch(void)
|
||||
// don't switch twice in 5 seconds
|
||||
return;
|
||||
}
|
||||
|
||||
float primaryErrorScore = core[primary].errorScore();
|
||||
float lowestErrorScore = primaryErrorScore;
|
||||
uint8_t newPrimaryIndex = primary;
|
||||
@ -863,6 +899,13 @@ void NavEKF3::checkLaneSwitch(void)
|
||||
}
|
||||
}
|
||||
|
||||
void NavEKF3::requestYawReset(void)
|
||||
{
|
||||
for (uint8_t i = 0; i < num_cores; i++) {
|
||||
core[primary].EKFGSF_requestYawReset();
|
||||
}
|
||||
}
|
||||
|
||||
// Check basic filter health metrics and return a consolidated health status
|
||||
bool NavEKF3::healthy(void) const
|
||||
{
|
||||
@ -1333,6 +1376,19 @@ uint32_t NavEKF3::getBodyFrameOdomDebug(int8_t instance, Vector3f &velInnov, Vec
|
||||
return ret;
|
||||
}
|
||||
|
||||
// return data for debugging EKF-GSF yaw estimator
|
||||
// return false if data not available
|
||||
bool NavEKF3::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;
|
||||
}
|
||||
|
||||
// return data for debugging range beacon fusion
|
||||
bool NavEKF3::getRangeBeaconDebug(int8_t instance, uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED,
|
||||
float &offsetHigh, float &offsetLow, Vector3f &posNED) const
|
||||
@ -1699,3 +1755,12 @@ void NavEKF3::getTimingStatistics(int8_t instance, struct ekf_timing &timing) co
|
||||
}
|
||||
}
|
||||
|
||||
// Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available.
|
||||
void NavEKF3::writeDefaultAirSpeed(float airspeed)
|
||||
{
|
||||
if (core) {
|
||||
for (uint8_t i=0; i<num_cores; i++) {
|
||||
core[i].writeDefaultAirSpeed(airspeed);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -381,12 +381,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();
|
||||
|
||||
// are we using an external yaw source? This is needed by AHRS attitudes_consistent check
|
||||
bool using_external_yaw(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
|
||||
@ -451,6 +464,10 @@ private:
|
||||
AP_Int8 _flowUse; // Controls if the optical flow data is fused into the main navigation estimator and/or the terrain estimator.
|
||||
AP_Float _hrt_filt_freq; // frequency of output observer height rate complementary filter in Hz
|
||||
AP_Int16 _mag_ef_limit; // limit on difference between WMM tables and learned earth field.
|
||||
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
|
||||
@ -556,4 +573,6 @@ private:
|
||||
void Log_Write_Beacon(uint64_t time_us) const;
|
||||
void Log_Write_BodyOdom(uint64_t time_us) const;
|
||||
void Log_Write_State_Variances(uint64_t time_us) const;
|
||||
void Log_Write_GSF(uint8_t core, uint64_t time_us) const;
|
||||
|
||||
};
|
||||
|
||||
@ -273,6 +273,7 @@ void NavEKF3_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) {
|
||||
@ -282,6 +283,8 @@ void NavEKF3_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) {
|
||||
@ -298,6 +301,10 @@ void NavEKF3_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;
|
||||
}
|
||||
@ -524,9 +531,18 @@ void NavEKF3_core::checkGyroCalStatus(void)
|
||||
{
|
||||
// check delta angle bias variances
|
||||
const float delAngBiasVarMax = sq(radians(0.15f * dtEkfAvg));
|
||||
delAngBiasLearned = (P[10][10] <= delAngBiasVarMax) &&
|
||||
(P[11][11] <= delAngBiasVarMax) &&
|
||||
(P[12][12] <= delAngBiasVarMax);
|
||||
if (frontend->_magCal == 6) {
|
||||
// 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[10][10],P[11][11],P[12][12]);
|
||||
Vector3f temp = prevTnb * delAngBiasVarVec;
|
||||
delAngBiasLearned = (fabsf(temp.x) < delAngBiasVarMax) &&
|
||||
(fabsf(temp.y) < delAngBiasVarMax);
|
||||
} else {
|
||||
delAngBiasLearned = (P[10][10] <= delAngBiasVarMax) &&
|
||||
(P[11][11] <= delAngBiasVarMax) &&
|
||||
(P[12][12] <= delAngBiasVarMax);
|
||||
}
|
||||
}
|
||||
|
||||
// Commands the EKF to not use GPS.
|
||||
@ -579,3 +595,39 @@ void NavEKF3_core::updateFilterStatus(void)
|
||||
filterStatus.flags.initalized = filterStatus.flags.initalized || healthy();
|
||||
}
|
||||
|
||||
void NavEKF3_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 NavEKF3_core::EKFGSF_requestYawReset()
|
||||
{
|
||||
EKFGSF_yaw_reset_request_ms = imuSampleTime_ms;
|
||||
}
|
||||
|
||||
@ -317,6 +317,7 @@ void NavEKF3::Log_Write()
|
||||
Log_Write_XKF3(i, time_us);
|
||||
Log_Write_XKF4(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
|
||||
@ -340,3 +341,52 @@ void NavEKF3::Log_Write()
|
||||
}
|
||||
}
|
||||
|
||||
void NavEKF3::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]);
|
||||
}
|
||||
}
|
||||
@ -48,6 +48,11 @@ void NavEKF3_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;
|
||||
@ -70,7 +75,12 @@ void NavEKF3_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
|
||||
@ -83,66 +93,83 @@ void NavEKF3_core::controlMagYawReset()
|
||||
finalResetRequest; // the final reset when we have achieved enough height to be in stable magnetic field environment
|
||||
|
||||
// Perform a reset of magnetic field states and reset yaw to corrected magnetic heading
|
||||
if (magYawResetRequest || magStateResetRequest) {
|
||||
if (magYawResetRequest) {
|
||||
// update rotation matrix from body to NED frame
|
||||
stateStruct.quat.inverse().rotation_matrix(prevTnb);
|
||||
|
||||
// get the euler angles from the current state estimate
|
||||
Vector3f eulerAngles;
|
||||
stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
|
||||
// read the magnetometer data
|
||||
readMagData();
|
||||
|
||||
// Use the Euler angles and magnetometer measurement to update the magnetic field states
|
||||
// and get an updated quaternion
|
||||
Quaternion newQuat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
|
||||
// rotate the magnetic field into NED axes
|
||||
Vector3f initMagNED = prevTnb * magDataDelayed.mag;
|
||||
|
||||
// if a yaw reset has been requested, apply the updated quaternion to the current state
|
||||
if (magYawResetRequest) {
|
||||
// previous value used to calculate a reset delta
|
||||
Quaternion prevQuat = stateStruct.quat;
|
||||
// calculate heading of mag field rel to body heading
|
||||
float magHeading = atan2f(initMagNED.y, initMagNED.x);
|
||||
|
||||
// calculate the variance for the rotation estimate expressed as a rotation vector
|
||||
// this will be used later to reset the quaternion state covariances
|
||||
Vector3f angleErrVarVec = calcRotVecVariances();
|
||||
// get the magnetic declination
|
||||
float magDecAng = MagDeclination();
|
||||
|
||||
// update the quaternion states using the new yaw angle
|
||||
stateStruct.quat = newQuat;
|
||||
// calculate yaw angle delta
|
||||
float yaw_delta = magDecAng - magHeading;
|
||||
|
||||
// update the yaw angle variance using the variance of the measurement
|
||||
angleErrVarVec.z = sq(MAX(frontend->_yawNoise, 1.0e-2f));
|
||||
// update quaternion states and covariances
|
||||
resetQuatStateYawOnly(yaw_delta, sq(MAX(frontend->_yawNoise, 1.0e-2f)), true);
|
||||
|
||||
// reset the quaternion covariances using the rotation vector variances
|
||||
initialiseQuatCovariances(angleErrVarVec);
|
||||
|
||||
// calculate the change in the quaternion state and apply it to the output history buffer
|
||||
prevQuat = stateStruct.quat/prevQuat;
|
||||
StoreQuatRotate(prevQuat);
|
||||
|
||||
// send initial alignment status to console
|
||||
if (!yawAlignComplete) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u initial yaw alignment complete",(unsigned)imu_index);
|
||||
}
|
||||
|
||||
// send in-flight yaw alignment status to console
|
||||
if (finalResetRequest) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u in-flight yaw alignment complete",(unsigned)imu_index);
|
||||
} else if (interimResetRequest) {
|
||||
gcs().send_text(MAV_SEVERITY_WARNING, "EKF3 IMU%u ground mag anomaly, yaw re-aligned",(unsigned)imu_index);
|
||||
}
|
||||
|
||||
// prevent reset of variances in ConstrainVariances()
|
||||
inhibitMagStates = false;
|
||||
|
||||
// update the yaw reset completed status
|
||||
recordYawReset();
|
||||
|
||||
// clear the yaw reset request flag
|
||||
magYawResetRequest = false;
|
||||
|
||||
// clear the complete flags if an interim reset has been performed to allow subsequent
|
||||
// and final reset to occur
|
||||
if (interimResetRequest) {
|
||||
finalInflightYawInit = false;
|
||||
finalInflightMagInit = false;
|
||||
}
|
||||
// send initial alignment status to console
|
||||
if (!yawAlignComplete) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u initial yaw alignment complete",(unsigned)imu_index);
|
||||
}
|
||||
|
||||
// send in-flight yaw alignment status to console
|
||||
if (finalResetRequest) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u in-flight yaw alignment complete",(unsigned)imu_index);
|
||||
} else if (interimResetRequest) {
|
||||
magYawAnomallyCount++;
|
||||
gcs().send_text(MAV_SEVERITY_WARNING, "EKF3 IMU%u ground mag anomaly, yaw re-aligned",(unsigned)imu_index);
|
||||
}
|
||||
|
||||
// clear the complete flags if an interim reset has been performed to allow subsequent
|
||||
// and final reset to occur
|
||||
if (interimResetRequest) {
|
||||
finalInflightYawInit = false;
|
||||
finalInflightMagInit = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (magStateResetRequest) {
|
||||
|
||||
// Rotate Mag measurements into NED to set initial NED magnetic field states
|
||||
// Don't do this if the earth field has already been learned
|
||||
if (!magFieldLearned) {
|
||||
if (have_table_earth_field && frontend->_mag_ef_limit > 0) {
|
||||
stateStruct.earth_magfield = table_earth_field_ga;
|
||||
} else {
|
||||
stateStruct.earth_magfield = prevTnb.transposed() * magDataDelayed.mag;
|
||||
}
|
||||
|
||||
// set the NE earth magnetic field states using the published declination
|
||||
// and set the corresponding variances and covariances
|
||||
alignMagStateDeclination();
|
||||
|
||||
// set the remaining variances and covariances
|
||||
zeroRows(P,18,21);
|
||||
zeroCols(P,18,21);
|
||||
P[18][18] = sq(frontend->_magNoise);
|
||||
P[19][19] = P[18][18];
|
||||
P[20][20] = P[18][18];
|
||||
P[21][21] = P[18][18];
|
||||
|
||||
}
|
||||
|
||||
// record the fact we have initialised the magnetic field states
|
||||
recordMagReset();
|
||||
|
||||
// prevent reset of variances in ConstrainVariances()
|
||||
inhibitMagStates = false;
|
||||
|
||||
// clear mag state reset request
|
||||
magStateResetRequest = false;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@ -169,13 +196,13 @@ void NavEKF3_core::realignYawGPS()
|
||||
|
||||
// correct yaw angle using GPS ground course if compass yaw bad
|
||||
if (badMagYaw) {
|
||||
// attempt to use EKF-GSF estimate if available as it is more robust to GPS glitches
|
||||
if (EKFGSF_resetMainFilterYaw()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// calculate the variance for the rotation estimate expressed as a rotation vector
|
||||
// this will be used later to reset the quaternion state covariances
|
||||
Vector3f angleErrVarVec = calcRotVecVariances();
|
||||
|
||||
// calculate new filter quaternion states from Euler angles
|
||||
stateStruct.quat.from_euler(eulerAngles.x, eulerAngles.y, gpsYaw);
|
||||
// keep roll and pitch and reset yaw
|
||||
resetQuatStateYawOnly(gpsYaw, sq(radians(45.0f)), false);
|
||||
|
||||
// reset the velocity and position states as they will be inaccurate due to bad yaw
|
||||
velResetSource = GPS;
|
||||
@ -183,25 +210,9 @@ void NavEKF3_core::realignYawGPS()
|
||||
posResetSource = GPS;
|
||||
ResetPosition();
|
||||
|
||||
// set the yaw angle variance to a larger value to reflect the uncertainty in yaw
|
||||
angleErrVarVec.z = sq(radians(45.0f));
|
||||
|
||||
// reset the quaternion covariances using the rotation vector variances
|
||||
zeroRows(P,0,3);
|
||||
zeroCols(P,0,3);
|
||||
initialiseQuatCovariances(angleErrVarVec);
|
||||
|
||||
// send yaw alignment information to console
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u yaw aligned to GPS velocity",(unsigned)imu_index);
|
||||
|
||||
|
||||
// record the yaw reset event
|
||||
recordYawReset();
|
||||
|
||||
// clear all pending yaw reset requests
|
||||
gpsYawResetRequest = false;
|
||||
magYawResetRequest = false;
|
||||
|
||||
if (use_compass()) {
|
||||
// request a mag field reset which may enable us to use the magnetometer if the previous fault was due to bad initialisation
|
||||
magStateResetRequest = true;
|
||||
@ -238,7 +249,6 @@ void NavEKF3_core::alignYawAngle()
|
||||
// send yaw alignment information to console
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u yaw aligned",(unsigned)imu_index);
|
||||
|
||||
|
||||
// record the yaw reset event
|
||||
recordYawReset();
|
||||
|
||||
@ -246,7 +256,6 @@ void NavEKF3_core::alignYawAngle()
|
||||
gpsYawResetRequest = false;
|
||||
magYawResetRequest = false;
|
||||
|
||||
|
||||
}
|
||||
|
||||
/********************************************************
|
||||
@ -347,6 +356,28 @@ void NavEKF3_core::SelectMagFusion()
|
||||
// above
|
||||
readMagData();
|
||||
}
|
||||
// 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 (frontend->_magCal == 6 || !use_compass()) {
|
||||
if (yawAlignComplete) {
|
||||
// Fuse a synthetic heading measurement at 7Hz to prevent the filter covariances from
|
||||
// becoming badly conditioned when static for long periods. For planes we only do this on-ground
|
||||
// because they can align the yaw from GPS when airborne. For other platform types we do this
|
||||
// all the time.
|
||||
if ((onGround || !assume_zero_sideslip()) && (imuSampleTime_ms - lastSynthYawTime_ms > 140)) {
|
||||
fuseEulerYaw(true, false);
|
||||
magTestRatio.zero();
|
||||
yawTestRatio = 0.0f;
|
||||
lastSynthYawTime_ms = imuSampleTime_ms;
|
||||
}
|
||||
return;
|
||||
}
|
||||
yawAlignComplete = EKFGSF_resetMainFilterYaw();
|
||||
return;
|
||||
}
|
||||
|
||||
// check for and read new magnetometer measurements
|
||||
readMagData();
|
||||
|
||||
// If we are using the compass and the magnetometer has been unhealthy for too long we declare a timeout
|
||||
if (magHealth) {
|
||||
@ -888,7 +919,8 @@ void NavEKF3_core::FuseMagnetometer()
|
||||
* The following booleans are passed to the function to control the fusion process:
|
||||
*
|
||||
* usePredictedYaw - Set this to true if no valid yaw measurement will be available for an extended periods.
|
||||
* This uses an innovation set to zero which prevents uncontrolled quaternion covaraince growth.
|
||||
* This uses an innovation set to zero which prevents uncontrolled quaternion covariance
|
||||
* growth or if available, a yaw estimate from the Gaussian Sum Filter.
|
||||
* UseExternalYawSensor - Set this to true if yaw data from an external yaw sensor is being used instead of the magnetometer.
|
||||
*/
|
||||
void NavEKF3_core::fuseEulerYaw(bool usePredictedYaw, bool useExternalYawSensor)
|
||||
@ -898,9 +930,21 @@ void NavEKF3_core::fuseEulerYaw(bool usePredictedYaw, bool useExternalYawSensor)
|
||||
float q2 = stateStruct.quat[2];
|
||||
float q3 = stateStruct.quat[3];
|
||||
|
||||
// external yaw available check
|
||||
bool canUseGsfYaw = false;
|
||||
float gsfYaw = 0.0f;
|
||||
float gsfYawVariance = 0.0f;
|
||||
if (usePredictedYaw && yawEstimator != nullptr) {
|
||||
canUseGsfYaw = yawEstimator->getYawData(&gsfYaw, &gsfYawVariance)
|
||||
&& is_positive(gsfYawVariance)
|
||||
&& gsfYawVariance < sq(radians(15.0f));
|
||||
}
|
||||
|
||||
// yaw measurement error variance (rad^2)
|
||||
float R_YAW;
|
||||
if (!useExternalYawSensor) {
|
||||
if (canUseGsfYaw) {
|
||||
R_YAW = gsfYawVariance;
|
||||
} else if (!useExternalYawSensor) {
|
||||
R_YAW = sq(frontend->_yawNoise);
|
||||
} else {
|
||||
R_YAW = sq(yawAngDataDelayed.yawAngErr);
|
||||
@ -1019,9 +1063,13 @@ void NavEKF3_core::fuseEulerYaw(bool usePredictedYaw, bool useExternalYawSensor)
|
||||
// use the external yaw sensor data
|
||||
innovation = wrap_PI(yawAngPredicted - yawAngDataDelayed.yawAng);
|
||||
}
|
||||
} else if (canUseGsfYaw) {
|
||||
// The GSF yaw esitimator can provide a better estimate than the main nav filter can when no yaw
|
||||
// sensor is available
|
||||
innovation = wrap_PI(yawAngPredicted - gsfYaw);
|
||||
} else {
|
||||
// setting the innovation to zero enables quaternion covariance growth to be constrained when there is no
|
||||
// method of observing yaw
|
||||
// setting the innovation to zero enables quaternion covariance growth to be constrained when there
|
||||
// is no method of observing yaw
|
||||
innovation = 0.0f;
|
||||
}
|
||||
|
||||
@ -1429,3 +1477,99 @@ bool NavEKF3_core::learnMagBiasFromGPS(void)
|
||||
}
|
||||
return ok;
|
||||
}
|
||||
// Reset states using yaw from EKF-GSF and velocity and position from GPS
|
||||
bool NavEKF3_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 (frontend->_magCal == 6) {
|
||||
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 NavEKF3_core::resetQuatStateYawOnly(float yaw, float yawVariance, bool isDeltaYaw)
|
||||
{
|
||||
Quaternion quatBeforeReset = stateStruct.quat;
|
||||
Vector3f angleErrVarVec = calcRotVecVariances();
|
||||
|
||||
// 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);
|
||||
|
||||
// update the yaw angle variance using the variance of the EKF-GSF estimate
|
||||
angleErrVarVec.z = yawVariance;
|
||||
initialiseQuatCovariances(angleErrVarVec);
|
||||
|
||||
// record the yaw reset event
|
||||
yawResetAngle += deltaYaw;
|
||||
lastYawReset_ms = imuSampleTime_ms;
|
||||
|
||||
// record the yaw reset event
|
||||
recordYawReset();
|
||||
|
||||
// clear all pending yaw reset requests
|
||||
gpsYawResetRequest = false;
|
||||
magYawResetRequest = false;
|
||||
|
||||
}
|
||||
|
||||
@ -664,6 +664,10 @@ void NavEKF3_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
|
||||
@ -930,7 +934,11 @@ void NavEKF3_core::writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_
|
||||
yawMeasTime_ms = timeStamp_ms;
|
||||
}
|
||||
|
||||
|
||||
// Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available.
|
||||
void NavEKF3_core::writeDefaultAirSpeed(float airspeed)
|
||||
{
|
||||
defaultAirSpeed = airspeed;
|
||||
}
|
||||
|
||||
/*
|
||||
update timing statistics structure
|
||||
|
||||
@ -637,3 +637,10 @@ void NavEKF3_core::getOutputTrackingError(Vector3f &error) const
|
||||
error = outputTrackError;
|
||||
}
|
||||
|
||||
bool NavEKF3_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;
|
||||
}
|
||||
|
||||
@ -284,9 +284,6 @@ void NavEKF3_core::SelectVelPosFusion()
|
||||
posVelFusionDelayed = false;
|
||||
}
|
||||
|
||||
// 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) {
|
||||
|
||||
|
||||
@ -393,10 +393,6 @@ void NavEKF3_core::detectFlight()
|
||||
|
||||
}
|
||||
|
||||
// store current on-ground and in-air status for next time
|
||||
prevOnGround = onGround;
|
||||
prevInFlight = inFlight;
|
||||
|
||||
// Store vehicle height and range prior to takeoff for use in post takeoff checks
|
||||
if (onGround) {
|
||||
// store vertical position at start of flight to use as a reference for ground relative checks
|
||||
@ -412,6 +408,22 @@ void NavEKF3_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;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -152,6 +152,22 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
|
||||
(unsigned)obs_buffer_length,
|
||||
(unsigned)flow_buffer_length,
|
||||
(double)dtEkfAvg);
|
||||
|
||||
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, "EKF3 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, "EKF3 IMU%uGSF: allocation failed",(unsigned)imu_index);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -389,6 +405,12 @@ void NavEKF3_core::InitialiseVariables()
|
||||
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF3 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;
|
||||
}
|
||||
|
||||
|
||||
@ -419,8 +441,13 @@ void NavEKF3_core::InitialiseVariablesMag()
|
||||
storedYawAng.reset();
|
||||
}
|
||||
|
||||
// Initialise the states from accelerometer and magnetometer data (if present)
|
||||
// This method can only be used when the vehicle is static
|
||||
/*
|
||||
Initialise the states from accelerometer data. This assumes measured acceleration
|
||||
is dominated by gravity. If this assumption is not true then the EKF will require
|
||||
timee to reduce the resulting tilt error. Yaw alignment is not performed by this
|
||||
function, but is perfomred later and initiated the SelectMagFusion() function
|
||||
after the tilt has stabilised.
|
||||
*/
|
||||
bool NavEKF3_core::InitialiseFilterBootstrap(void)
|
||||
{
|
||||
// If we are a plane and don't have GPS lock then don't initialise
|
||||
@ -602,6 +629,14 @@ void NavEKF3_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 or external yaw sensor data
|
||||
SelectMagFusion();
|
||||
|
||||
@ -1599,89 +1634,60 @@ void NavEKF3_core::calcEarthRateNED(Vector3f &omega, int32_t latitude) const
|
||||
omega.z = -earthRate*sinf(lat_rad);
|
||||
}
|
||||
|
||||
// initialise the earth magnetic field states using declination, supplied roll/pitch
|
||||
// and magnetometer measurements and return initial attitude quaternion
|
||||
Quaternion NavEKF3_core::calcQuatAndFieldStates(float roll, float pitch)
|
||||
// update quaternion, mag field states and associated variances using magnetomer and declination data
|
||||
void NavEKF3_core::calcQuatAndFieldStates()
|
||||
{
|
||||
// declare local variables required to calculate initial orientation and magnetic field
|
||||
float yaw;
|
||||
Matrix3f Tbn;
|
||||
Vector3f initMagNED;
|
||||
Quaternion initQuat;
|
||||
|
||||
if (use_compass()) {
|
||||
// calculate rotation matrix from body to NED frame
|
||||
Tbn.from_euler(roll, pitch, 0.0f);
|
||||
|
||||
// read the magnetometer data
|
||||
readMagData();
|
||||
|
||||
// rotate the magnetic field into NED axes
|
||||
initMagNED = Tbn * magDataDelayed.mag;
|
||||
|
||||
// calculate heading of mag field rel to body heading
|
||||
float magHeading = atan2f(initMagNED.y, initMagNED.x);
|
||||
|
||||
// get the magnetic declination
|
||||
float magDecAng = MagDeclination();
|
||||
|
||||
// calculate yaw angle rel to true north
|
||||
yaw = magDecAng - magHeading;
|
||||
|
||||
// calculate initial filter quaternion states using yaw from magnetometer
|
||||
// store the yaw change so that it can be retrieved externally for use by the control loops to prevent yaw disturbances following a reset
|
||||
Vector3f tempEuler;
|
||||
stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z);
|
||||
// this check ensures we accumulate the resets that occur within a single iteration of the EKF
|
||||
if (imuSampleTime_ms != lastYawReset_ms) {
|
||||
yawResetAngle = 0.0f;
|
||||
}
|
||||
yawResetAngle += wrap_PI(yaw - tempEuler.z);
|
||||
lastYawReset_ms = imuSampleTime_ms;
|
||||
// calculate an initial quaternion using the new yaw value
|
||||
initQuat.from_euler(roll, pitch, yaw);
|
||||
// zero the attitude covariances because the correlations will now be invalid
|
||||
zeroAttCovOnly();
|
||||
|
||||
// calculate initial Tbn matrix and rotate Mag measurements into NED
|
||||
// to set initial NED magnetic field states
|
||||
// don't do this if the earth field has already been learned
|
||||
if (!magFieldLearned) {
|
||||
initQuat.rotation_matrix(Tbn);
|
||||
if (have_table_earth_field && frontend->_mag_ef_limit > 0) {
|
||||
stateStruct.earth_magfield = table_earth_field_ga;
|
||||
} else {
|
||||
stateStruct.earth_magfield = Tbn * magDataDelayed.mag;
|
||||
}
|
||||
|
||||
// set the NE earth magnetic field states using the published declination
|
||||
// and set the corresponding variances and covariances
|
||||
alignMagStateDeclination();
|
||||
|
||||
// set the remaining variances and covariances
|
||||
zeroRows(P,18,21);
|
||||
zeroCols(P,18,21);
|
||||
P[18][18] = sq(frontend->_magNoise);
|
||||
P[19][19] = P[18][18];
|
||||
P[20][20] = P[18][18];
|
||||
P[21][21] = P[18][18];
|
||||
|
||||
}
|
||||
|
||||
// record the fact we have initialised the magnetic field states
|
||||
recordMagReset();
|
||||
|
||||
// clear mag state reset request
|
||||
magStateResetRequest = false;
|
||||
|
||||
} else {
|
||||
// this function should not be called if there is no compass data but if it is, return the
|
||||
// current attitude
|
||||
initQuat = stateStruct.quat;
|
||||
if (!use_compass()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// return attitude quaternion
|
||||
return initQuat;
|
||||
// update rotation matrix from body to NED frame
|
||||
stateStruct.quat.inverse().rotation_matrix(prevTnb);
|
||||
|
||||
// read the magnetometer data
|
||||
readMagData();
|
||||
|
||||
// rotate the magnetic field into NED axes
|
||||
Vector3f initMagNED = prevTnb * magDataDelayed.mag;
|
||||
|
||||
// calculate heading of mag field rel to body heading
|
||||
float magHeading = atan2f(initMagNED.y, initMagNED.x);
|
||||
|
||||
// get the magnetic declination
|
||||
float magDecAng = MagDeclination();
|
||||
|
||||
// calculate yaw angle delta
|
||||
float yaw_delta = magDecAng - magHeading;
|
||||
|
||||
// update quaternion states and covariances
|
||||
resetQuatStateYawOnly(yaw_delta, sq(MAX(frontend->_yawNoise, 1.0e-2f)), true);
|
||||
|
||||
// Rotate Mag measurements into NED to set initial NED magnetic field states
|
||||
// Don't do this if the earth field has already been learned
|
||||
if (!magFieldLearned) {
|
||||
if (have_table_earth_field && frontend->_mag_ef_limit > 0) {
|
||||
stateStruct.earth_magfield = table_earth_field_ga;
|
||||
} else {
|
||||
stateStruct.earth_magfield = prevTnb.transposed() * magDataDelayed.mag;
|
||||
}
|
||||
|
||||
// set the NE earth magnetic field states using the published declination
|
||||
// and set the corresponding variances and covariances
|
||||
alignMagStateDeclination();
|
||||
|
||||
// set the remaining variances and covariances
|
||||
zeroRows(P,18,21);
|
||||
zeroCols(P,18,21);
|
||||
P[18][18] = sq(frontend->_magNoise);
|
||||
P[19][19] = P[18][18];
|
||||
P[20][20] = P[18][18];
|
||||
P[21][21] = P[18][18];
|
||||
|
||||
}
|
||||
|
||||
// record the fact we have initialised the magnetic field states
|
||||
recordMagReset();
|
||||
|
||||
}
|
||||
|
||||
// zero the attitude covariances, but preserve the variances
|
||||
|
||||
@ -30,6 +30,7 @@
|
||||
#include <AP_NavEKF/AP_NavEKF_core_common.h>
|
||||
#include <AP_NavEKF3/AP_NavEKF3_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)
|
||||
@ -67,6 +68,11 @@
|
||||
|
||||
// learning limit for mag biases when using GPS yaw (Gauss)
|
||||
#define EK3_GPS_MAG_LEARN_LIMIT 0.02f
|
||||
// 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;
|
||||
|
||||
@ -387,7 +393,19 @@ public:
|
||||
// are we using an external yaw source? This is needed by AHRS attitudes_consistent check
|
||||
bool using_external_yaw(void) const;
|
||||
|
||||
// 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
|
||||
NavEKF3 *frontend;
|
||||
uint8_t imu_index; // preferred IMU index
|
||||
@ -715,8 +733,8 @@ private:
|
||||
// align the yaw angle for the quaternion states using the external yaw sensor
|
||||
void alignYawAngle();
|
||||
|
||||
// and return attitude quaternion
|
||||
Quaternion calcQuatAndFieldStates(float roll, float pitch);
|
||||
// update quaternion, mag field states and associated variances using magnetomer and declination data
|
||||
void calcQuatAndFieldStates();
|
||||
|
||||
// zero stored variables
|
||||
void InitialiseVariables();
|
||||
@ -867,6 +885,22 @@ private:
|
||||
// correct GPS data for antenna position
|
||||
void CorrectGPSForAntennaOffset(gps_elements &gps_data) 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();
|
||||
|
||||
// Variables
|
||||
bool statesInitialised; // boolean true when filter states have been initialised
|
||||
bool velHealth; // boolean true if velocity measurements have passed innovation consistency check
|
||||
@ -914,6 +948,7 @@ private:
|
||||
Vector3f varInnovMag; // innovation variance output from fusion of X,Y,Z compass measurements
|
||||
ftype innovVtas; // innovation output from fusion of airspeed measurements
|
||||
ftype varInnovVtas; // innovation variance output from fusion of airspeed measurements
|
||||
float defaultAirSpeed; // default equivalent airspeed in m/s to be used if the measurement is unavailable. Do not use if not positive.
|
||||
bool magFusePerformed; // boolean set to true when magnetometer fusion has been perfomred in that time step
|
||||
bool magFuseRequired; // boolean set to true when magnetometer fusion will be perfomred in the next time step
|
||||
uint32_t prevTasStep_ms; // time stamp of last TAS fusion step
|
||||
@ -1231,6 +1266,7 @@ private:
|
||||
|
||||
// control of post takeoff magentic field and heading resets
|
||||
bool finalInflightYawInit; // true when the final post takeoff initialisation of yaw angle has been performed
|
||||
uint8_t magYawAnomallyCount; // Number of times the yaw has been reset due to a magnetic anomaly during initial ascent
|
||||
bool finalInflightMagInit; // true when the final post takeoff initialisation of magnetic field states been performed
|
||||
bool magStateResetRequest; // true if magnetic field states need to be reset using the magnetomter measurements
|
||||
bool magYawResetRequest; // true if the vehicle yaw and magnetic field states need to be reset using the magnetometer measurements
|
||||
@ -1354,4 +1390,9 @@ private:
|
||||
*/
|
||||
void updateMovementCheck(void);
|
||||
|
||||
// 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