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

AP_NavEKF3: Split GSF yaw estimator processing

Browse Source 
Separate prediction and correction steps are required to provide an up to
This commit is contained in:
Paul Riseborough 2020-04-23 16:31:12 +10:00 committed by Andrew Tridgell
parent abc821ad36
commit 7ba39c844c
3 changed files with 22 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -597,7 +597,7 @@ void NavEKF3_core::updateFilterStatus(void)
filterStatus.flags.initalized = filterStatus.flags.initalized || healthy();
}
void NavEKF3_core::runYawEstimator()
void NavEKF3_core::runYawEstimatorPrediction()
{
if (yawEstimator != nullptr && frontend->_fusionModeGPS <= 1) {
float trueAirspeed;
@ -612,7 +612,12 @@ void NavEKF3_core::runYawEstimator()
}
yawEstimator->update(imuDataDelayed.delAng, imuDataDelayed.delVel, imuDataDelayed.delAngDT, imuDataDelayed.delVelDT, EKFGSF_run_filterbank, trueAirspeed);
}
}
void NavEKF3_core::runYawEstimatorCorrection()
{
if (yawEstimator != nullptr && frontend->_fusionModeGPS <= 1) {
if (gpsDataToFuse) {
Vector2f gpsVelNE = Vector2f(gpsDataDelayed.vel.x, gpsDataDelayed.vel.y);
float gpsVelAcc = fmaxf(gpsSpdAccuracy, frontend->_gpsHorizVelNoise);
@ -624,7 +629,6 @@ void NavEKF3_core::runYawEstimator()
EKFGSF_resetMainFilterYaw();
}
}
}
// request a reset the yaw to the GSF estimate

17
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -631,20 +631,21 @@ void NavEKF3_core::UpdateFilter(bool predict)
// Predict the covariance growth
CovariancePrediction();
// Run the IMU prediction step for the GSF yaw estimator algorithm
// using IMU and optionally true airspeed data.
// Must be run before SelectMagFusion() to provide an up to date yaw estimate
runYawEstimatorPrediction();
// Update states using magnetometer or external yaw sensor data
SelectMagFusion();
// Update states using GPS and altimeter data
SelectVelPosFusion();
// Generate an alternative yaw estimate used for inflight
// recovery from bad compass data requires horizontal GPS
// velocity. We only do this when posVelFusionDelayed is false
// as otherwise there will be no new GPS data, as the
// sttoredGPS recall will have been skipped
if (!posVelFusionDelayed) {
runYawEstimator();
}
// Run the GPS velocity correction step for the GSF yaw estimator algorithm
// and use the yaw estimate to reset the main EKF yaw if requested
// Muat be run after SelectVelPosFusion() so that fresh GPS data is available
runYawEstimatorCorrection();
// Update states using range beacon data
SelectRngBcnFusion();

10
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -887,9 +887,13 @@ 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);
// Runs the IMU prediction step for an independent GSF yaw estimator algorithm
// that uses IMU, GPS horizontal velocity and optionally true airspeed data.
void runYawEstimatorPrediction(void);
// Run the GPS velocity correction step for the GSF yaw estimator and use the
// yaw estimate to reset the main EKF yaw if requested
void runYawEstimatorCorrection(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