From 0c4f92d4c18c87a2745695abefac41ccaf5b3dde Mon Sep 17 00:00:00 2001 From: priseborough Date: Thu, 20 Jul 2017 10:40:11 +1000 Subject: [PATCH] AP_NavEKF3: Add control logic to use external yaw sensor --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 4 +- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 1 + libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 101 +++++++++++++----- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 4 +- 5 files changed, 78 insertions(+), 34 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index bc6008181d..8b893aeb09 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -243,8 +243,8 @@ const AP_Param::GroupInfo NavEKF3::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. 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 : 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 EK3_MAG_MASK parameter. - // @Values: 0:When flying,1:When manoeuvring,2:Never,3:After first climb yaw reset,4:Always + // @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. 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. EK3_MAG_CAL = 5 uses an external yaw sensor with simple heading fusion. 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 EK3_MAG_MASK parameter. + // @Values: 0:When flying,1:When manoeuvring,2:Never,3:After first climb yaw reset,4:Always,5:Use external yaw sensor // @User: Advanced // @RebootRequired: True AP_GROUPINFO("MAG_CAL", 14, NavEKF3, _magCal, MAG_CAL_DEFAULT), diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 4cba774796..980d657695 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -395,6 +395,7 @@ void NavEKF3_core::checkAttitudeAlignmentStatus() if (!yawAlignComplete && tiltAlignComplete && use_compass()) { magYawResetRequest = true; } + } // return true if we should use the airspeed sensor diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index a721d3d506..637a072273 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -260,6 +260,20 @@ void NavEKF3_core::SelectMagFusion() // used for load levelling magFusePerformed = false; + // Handle case where we are using an external yaw sensor instead of a magnetomer + if ((frontend->_magCal == 5) && !use_compass()) { + if (storedYawAng.recall(yawAngDataDelayed,imuDataDelayed.time_ms)) { + if (tiltAlignComplete && !yawAlignComplete) { + alignYawAngle(); + } else if (tiltAlignComplete && yawAlignComplete) { + fuseEulerYaw(false, true); + } else { + fuseEulerYaw(true, true); + } + } + return; + } + // check for and read new magnetometer measurements readMagData(); @@ -274,13 +288,9 @@ void NavEKF3_core::SelectMagFusion() // check for availability of magnetometer or other yaw data to fuse magDataToFuse = storedMag.recall(magDataDelayed,imuDataDelayed.time_ms); - bool yawDataToFuse = storedYawAng.recall(yawAngDataDelayed,imuDataDelayed.time_ms); - // Control reset of yaw and magnetic field states if we are using compass data - if (use_compass() && magDataToFuse) { + if (magDataToFuse) { controlMagYawReset(); - } else if (!use_compass() && yawDataToFuse) { - } // determine if conditions are right to start a new fusion cycle @@ -289,9 +299,11 @@ void NavEKF3_core::SelectMagFusion() if (dataReady) { // use the simple method of declination to maintain heading if we cannot use the magnetic field states if(inhibitMagStates || magStateResetRequest || !magStateInitComplete) { - fuseEulerYaw(); + fuseEulerYaw(false, false); + // zero the test ratio output from the inactive 3-axis magnetometer fusion magTestRatio.zero(); + } else { // if we are not doing aiding with earth relative observations (eg GPS) then the declination is // maintained by fusing declination as a synthesised observation @@ -319,7 +331,7 @@ void NavEKF3_core::SelectMagFusion() // airborne. For other platform types we do this all the time. if (!use_compass()) { if ((onGround || !assume_zero_sideslip()) && (imuSampleTime_ms - lastSynthYawTime_ms > 140)) { - fuseEulerYaw(); + fuseEulerYaw(true, false); magTestRatio.zero(); yawTestRatio = 0.0f; lastSynthYawTime_ms = imuSampleTime_ms; @@ -787,23 +799,51 @@ void NavEKF3_core::FuseMagnetometer() * It is suitable for use when the external magnetic field environment is disturbed (eg close to metal structures, on ground). * It is not as robust to magnetometer failures. * It is not suitable for operation where the horizontal magnetic field strength is weak (within 30 degrees latitude of the magnetic poles) + * + * 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. + * UseExternalYawSensor - Set this to true if yaw data from an external yaw sensor is being used instead of the magnetometer. */ -void NavEKF3_core::fuseEulerYaw() +void NavEKF3_core::fuseEulerYaw(bool usePredictedYaw, bool useExternalYawSensor) { float q0 = stateStruct.quat[0]; float q1 = stateStruct.quat[1]; float q2 = stateStruct.quat[2]; float q3 = stateStruct.quat[3]; - // compass measurement error variance (rad^2) - const float R_YAW = sq(frontend->_yawNoise); + // yaw measurement error variance (rad^2) + float R_YAW; + if (!useExternalYawSensor) { + R_YAW = sq(frontend->_yawNoise); + } else { + R_YAW = sq(yawAngDataDelayed.yawAngErr); + } + + // determine if a 321 or 312 Euler sequence is best + bool useEuler321 = true; + if (useExternalYawSensor) { + // If using an external sensor, the definition of yaw is specified through the sensor interface + if (yawAngDataDelayed.type == 2) { + useEuler321 = true; + } else if (yawAngDataDelayed.type == 1) { + useEuler321 = false; + } else { + // invalid selection + return; + } + } else { + // if using the magnetometer, it is determined automatically + useEuler321 = (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])); + } // calculate observation jacobian, predicted yaw and zero yaw body to earth rotation matrix - // determine if a 321 or 312 Euler sequence is best - float predicted_yaw; + float yawAngPredicted; float H_YAW[4]; Matrix3f Tbn_zeroYaw; - if (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])) { + + if (useEuler321) { // calculate observation jacobian when we are observing the first rotation in a 321 sequence float t9 = q0*q3; float t10 = q1*q2; @@ -837,7 +877,7 @@ void NavEKF3_core::fuseEulerYaw() // Get the 321 euler angles Vector3f euler321; stateStruct.quat.to_euler(euler321.x, euler321.y, euler321.z); - predicted_yaw = euler321.z; + yawAngPredicted = euler321.z; // set the yaw to zero and calculate the zero yaw rotation from body to earth frame Tbn_zeroYaw.from_euler(euler321.x, euler321.y, 0.0f); @@ -875,27 +915,30 @@ void NavEKF3_core::fuseEulerYaw() // Get the 321 euler angles Vector3f euler312 = stateStruct.quat.to_vector312(); - predicted_yaw = euler312.z; + yawAngPredicted = euler312.z; // set the yaw to zero and calculate the zero yaw rotation from body to earth frame Tbn_zeroYaw.from_euler312(euler312.x, euler312.y, 0.0f); } - // rotate measured mag components into earth frame - Vector3f magMeasNED = Tbn_zeroYaw*magDataDelayed.mag; - - // Use the difference between the horizontal projection and declination to give the measured yaw - // If we can't use compass data, set the measurement to the predicted - // to prevent uncontrolled variance growth whilst on ground without magnetometer - float measured_yaw; - if (use_compass() && yawAlignComplete) { - measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + _ahrs->get_compass()->get_declination()); - } else { - measured_yaw = predicted_yaw; - } - // Calculate the innovation - float innovation = wrap_PI(predicted_yaw - measured_yaw); + float innovation; + if (!usePredictedYaw) { + if (!useExternalYawSensor) { + // Use the difference between the horizontal projection and declination to give the measured yaw + // rotate measured mag components into earth frame + Vector3f magMeasNED = Tbn_zeroYaw*magDataDelayed.mag; + float yawAngMeasured = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + _ahrs->get_compass()->get_declination()); + innovation = wrap_PI(yawAngPredicted - yawAngMeasured); + } else { + // use the external yaw sensor data + innovation = wrap_PI(yawAngPredicted - yawAngDataDelayed.yawAng); + } + } else { + // setting the innovation to zero enables quaternion covariance growth to be constrained when there is no + // method of observing yaw + innovation = 0.0f; + } // Copy raw value to output variable used for data logging innovYaw = innovation; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 8145fe5460..69064ad075 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -597,7 +597,7 @@ void NavEKF3_core::UpdateFilter(bool predict) // Predict the covariance growth CovariancePrediction(); - // Update states using magnetometer data + // Update states using magnetometer or external yaw sensor data SelectMagFusion(); // Update states using GPS and altimeter data diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 289e979516..06aeb8d639 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -690,7 +690,7 @@ private: // initialise the earth magnetic field states using declination and current attitude and magnetometer measurements - // align the yaw angle using the data from the yaw sensor buffer + // align the yaw angle for the quaternion states using the external yaw sensor void alignYawAngle(); // and return attitude quaternion @@ -796,7 +796,7 @@ private: void alignMagStateDeclination(); // Fuse compass measurements using a simple declination observation (doesn't require magnetic field states) - void fuseEulerYaw(); + void fuseEulerYaw(bool usePredictedYaw, bool useExternalYawSensor); // Fuse declination angle to keep earth field declination from changing when we don't have earth relative observations. // Input is 1-sigma uncertainty in published declination