Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Add control logic to use external yaw sensor

Browse Source
This commit is contained in:
priseborough 2017-07-20 10:40:11 +10:00 committed by Andrew Tridgell
parent 71d358803a
commit 0c4f92d4c1
5 changed files with 78 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -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),

1
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -395,6 +395,7 @@ void NavEKF3_core::checkAttitudeAlignmentStatus()
if (!yawAlignComplete && tiltAlignComplete && use_compass()) {
magYawResetRequest = true;
}
}
// return true if we should use the airspeed sensor

101
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -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;

2
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -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

4
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -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