aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: Improve wind estimation without airspeed sensing

Browse Source 
Enable default airspeed variance to be specified externally
Improve control of sideslip fusion
Don't modify non wind states unnecessarily when using synthesised air data measurements
This commit is contained in:
Paul Riseborough 2020-06-07 09:23:52 +10:00 committed by Andrew Tridgell
parent ac00776184
commit 9a5a264aa0
7 changed files with 107 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -416,8 +416,13 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// @Units: m
AP_GROUPINFO("NOAID_M_NSE", 35, NavEKF3, _noaidHorizNoise, 10.0f),
// 36 was LOG_MASK, used for specifying which IMUs/cores to log
// replay data for
// @Param: BETA_MASK
// @DisplayName: Bitmask controlling sidelip angle fusion
// @Description: 1 byte bitmap controlling use of sideslip angle fuson for estimation of non wind states during operation of 'fly forward' vehicle types. Use of sideslip fusion to update non wind states is performed by default when dead reckoning and cannot be didsabled.
// @Bitmask: 0:Always,1:WhenNoYawSensor
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("BETA_MASK", 36, NavEKF3, _betaMask, 0),
// control of magnetic yaw angle fusion
@ -642,6 +647,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// @User: Advanced
// @Bitmask: 0:EnableGPSAffinity,1:EnableBaroAffinity,2:EnableCompassAffinity,3:EnableAirspeedAffinity
// @RebootRequired: True
AP_GROUPINFO("AFFINITY", 62, NavEKF3, _affinity, 0),
AP_SUBGROUPEXTENSION("", 63, NavEKF3, var_info2),
@ -1236,12 +1242,15 @@ void NavEKF3::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainSca
}
// return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis)
void NavEKF3::getWind(int8_t instance, Vector3f &wind) const
// returns true if wind state estimation is active
bool NavEKF3::getWind(int8_t instance, Vector3f &wind) const
{
bool ret = false;
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
core[instance].getWind(wind);
ret = core[instance].getWind(wind);
}
return ret;
}
// return earth magnetic field estimates in measurement units / 1000
@ -1997,14 +2006,14 @@ void NavEKF3::updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_
}
// 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)
// Writes the default equivalent airspeed and 1-sigma uncertainty in m/s to be used in forward flight if a measured airspeed is required and not available.
void NavEKF3::writeDefaultAirSpeed(float airspeed, float uncertainty)
{
AP::dal().log_writeDefaultAirSpeed3(airspeed);
AP::dal().log_writeDefaultAirSpeed3(airspeed, uncertainty);
if (core) {
for (uint8_t i=0; i<num_cores; i++) {
core[i].writeDefaultAirSpeed(airspeed);
core[i].writeDefaultAirSpeed(airspeed, uncertainty);
}
}
}

13
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -117,8 +117,9 @@ public:
void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const;
// return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis)
// An out of range instance (eg -1) returns data for the primary instance
void getWind(int8_t instance, Vector3f &wind) const;
// An out of range instance (eg -1) returns data for the the primary instance
// returns true if wind state estimation is active
bool getWind(int8_t instance, Vector3f &wind) const;
// return earth magnetic field estimates in measurement units / 1000 for the specified instance
// An out of range instance (eg -1) returns data for the primary instance
@ -365,9 +366,9 @@ public:
// check if configured to use GPS for horizontal position estimation
bool configuredToUseGPSForPosXY(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);
// Writes the default equivalent airspeed and 1-sigma uncertainty in m/s to be used in forward flight if a measured airspeed is required and not available.
void writeDefaultAirSpeed(float airspeed, float uncertainty);
// parameter conversion
void convert_parameters();
@ -442,6 +443,7 @@ private:
AP_Float _ballisticCoef_x; // ballistic coefficient measured for flow in X body frame directions
AP_Float _ballisticCoef_y; // ballistic coefficient measured for flow in Y body frame directions
AP_Float _momentumDragCoef; // lift rotor momentum drag coefficient
AP_Int8 _betaMask; // Bitmask controlling when sideslip angle fusion is used to estimate non wind states
// Possible values for _flowUse
#define FLOW_USE_NONE 0
@ -480,6 +482,7 @@ private:
const uint8_t flowIntervalMin_ms = 20; // The minimum allowed time between measurements from optical flow sensors (msec)
const uint8_t extNavIntervalMin_ms = 20; // The minimum allowed time between measurements from external navigation sensors (msec)
const float maxYawEstVelInnov = 2.0f; // Maximum acceptable length of the velocity innovation returned by the EKF-GSF yaw estimator (m/s)
const uint16_t deadReckonDeclare_ms = 1000; // Time without equivalent position or velocity observation to constrain drift beore dead reckoning is declared (msec)
// time at start of current filter update
uint64_t imuSampleTime_us;

108
libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
View File

@ -26,7 +26,6 @@ void NavEKF3_core::FuseAirspeed()
float vwn;
float vwe;
float EAS2TAS = dal.get_EAS2TAS();
const float R_TAS = sq(constrain_float(frontend->_easNoise, 0.5f, 5.0f) * constrain_float(EAS2TAS, 0.9f, 10.0f));
float SH_TAS[3];
float SK_TAS[2];
Vector24 H_TAS = {};
@ -44,6 +43,16 @@ void NavEKF3_core::FuseAirspeed()
// perform fusion of True Airspeed measurement
if (VtasPred > 1.0f)
{
// calculate observation innovation and variance
float specifiedVariance = sq(MAX(frontend->_easNoise, 0.5f));
if (is_positive(defaultAirSpeed)) {
innovVtas = VtasPred - defaultAirSpeed;
specifiedVariance = MAX(specifiedVariance, defaultAirSpeedVariance);
} else {
innovVtas = VtasPred - tasDataDelayed.tas;
}
const float R_TAS = specifiedVariance * sq(constrain_float(EAS2TAS, 0.9f, 10.0f));
// calculate observation jacobians
SH_TAS[0] = 1.0f/VtasPred;
SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2.0f*vwe))*0.5f;
@ -66,18 +75,24 @@ void NavEKF3_core::FuseAirspeed()
return;
}
SK_TAS[1] = SH_TAS[1];
Kfusion[0] = SK_TAS[0]*(P[0][4]*SH_TAS[2] - P[0][22]*SH_TAS[2] + P[0][5]*SK_TAS[1] - P[0][23]*SK_TAS[1] + P[0][6]*vd*SH_TAS[0]);
Kfusion[1] = SK_TAS[0]*(P[1][4]*SH_TAS[2] - P[1][22]*SH_TAS[2] + P[1][5]*SK_TAS[1] - P[1][23]*SK_TAS[1] + P[1][6]*vd*SH_TAS[0]);
Kfusion[2] = SK_TAS[0]*(P[2][4]*SH_TAS[2] - P[2][22]*SH_TAS[2] + P[2][5]*SK_TAS[1] - P[2][23]*SK_TAS[1] + P[2][6]*vd*SH_TAS[0]);
Kfusion[3] = SK_TAS[0]*(P[3][4]*SH_TAS[2] - P[3][22]*SH_TAS[2] + P[3][5]*SK_TAS[1] - P[3][23]*SK_TAS[1] + P[3][6]*vd*SH_TAS[0]);
Kfusion[4] = SK_TAS[0]*(P[4][4]*SH_TAS[2] - P[4][22]*SH_TAS[2] + P[4][5]*SK_TAS[1] - P[4][23]*SK_TAS[1] + P[4][6]*vd*SH_TAS[0]);
Kfusion[5] = SK_TAS[0]*(P[5][4]*SH_TAS[2] - P[5][22]*SH_TAS[2] + P[5][5]*SK_TAS[1] - P[5][23]*SK_TAS[1] + P[5][6]*vd*SH_TAS[0]);
Kfusion[6] = SK_TAS[0]*(P[6][4]*SH_TAS[2] - P[6][22]*SH_TAS[2] + P[6][5]*SK_TAS[1] - P[6][23]*SK_TAS[1] + P[6][6]*vd*SH_TAS[0]);
Kfusion[7] = SK_TAS[0]*(P[7][4]*SH_TAS[2] - P[7][22]*SH_TAS[2] + P[7][5]*SK_TAS[1] - P[7][23]*SK_TAS[1] + P[7][6]*vd*SH_TAS[0]);
Kfusion[8] = SK_TAS[0]*(P[8][4]*SH_TAS[2] - P[8][22]*SH_TAS[2] + P[8][5]*SK_TAS[1] - P[8][23]*SK_TAS[1] + P[8][6]*vd*SH_TAS[0]);
Kfusion[9] = SK_TAS[0]*(P[9][4]*SH_TAS[2] - P[9][22]*SH_TAS[2] + P[9][5]*SK_TAS[1] - P[9][23]*SK_TAS[1] + P[9][6]*vd*SH_TAS[0]);
if (!inhibitDelAngBiasStates) {
if (!airDataFusionWindOnly) {
Kfusion[0] = SK_TAS[0]*(P[0][4]*SH_TAS[2] - P[0][22]*SH_TAS[2] + P[0][5]*SK_TAS[1] - P[0][23]*SK_TAS[1] + P[0][6]*vd*SH_TAS[0]);
Kfusion[1] = SK_TAS[0]*(P[1][4]*SH_TAS[2] - P[1][22]*SH_TAS[2] + P[1][5]*SK_TAS[1] - P[1][23]*SK_TAS[1] + P[1][6]*vd*SH_TAS[0]);
Kfusion[2] = SK_TAS[0]*(P[2][4]*SH_TAS[2] - P[2][22]*SH_TAS[2] + P[2][5]*SK_TAS[1] - P[2][23]*SK_TAS[1] + P[2][6]*vd*SH_TAS[0]);
Kfusion[3] = SK_TAS[0]*(P[3][4]*SH_TAS[2] - P[3][22]*SH_TAS[2] + P[3][5]*SK_TAS[1] - P[3][23]*SK_TAS[1] + P[3][6]*vd*SH_TAS[0]);
Kfusion[4] = SK_TAS[0]*(P[4][4]*SH_TAS[2] - P[4][22]*SH_TAS[2] + P[4][5]*SK_TAS[1] - P[4][23]*SK_TAS[1] + P[4][6]*vd*SH_TAS[0]);
Kfusion[5] = SK_TAS[0]*(P[5][4]*SH_TAS[2] - P[5][22]*SH_TAS[2] + P[5][5]*SK_TAS[1] - P[5][23]*SK_TAS[1] + P[5][6]*vd*SH_TAS[0]);
Kfusion[6] = SK_TAS[0]*(P[6][4]*SH_TAS[2] - P[6][22]*SH_TAS[2] + P[6][5]*SK_TAS[1] - P[6][23]*SK_TAS[1] + P[6][6]*vd*SH_TAS[0]);
Kfusion[7] = SK_TAS[0]*(P[7][4]*SH_TAS[2] - P[7][22]*SH_TAS[2] + P[7][5]*SK_TAS[1] - P[7][23]*SK_TAS[1] + P[7][6]*vd*SH_TAS[0]);
Kfusion[8] = SK_TAS[0]*(P[8][4]*SH_TAS[2] - P[8][22]*SH_TAS[2] + P[8][5]*SK_TAS[1] - P[8][23]*SK_TAS[1] + P[8][6]*vd*SH_TAS[0]);
Kfusion[9] = SK_TAS[0]*(P[9][4]*SH_TAS[2] - P[9][22]*SH_TAS[2] + P[9][5]*SK_TAS[1] - P[9][23]*SK_TAS[1] + P[9][6]*vd*SH_TAS[0]);
} else {
// zero indexes 0 to 9 = 10*4 bytes
memset(&Kfusion[0], 0, 40);
}
if (!inhibitDelAngBiasStates && !airDataFusionWindOnly) {
Kfusion[10] = SK_TAS[0]*(P[10][4]*SH_TAS[2] - P[10][22]*SH_TAS[2] + P[10][5]*SK_TAS[1] - P[10][23]*SK_TAS[1] + P[10][6]*vd*SH_TAS[0]);
Kfusion[11] = SK_TAS[0]*(P[11][4]*SH_TAS[2] - P[11][22]*SH_TAS[2] + P[11][5]*SK_TAS[1] - P[11][23]*SK_TAS[1] + P[11][6]*vd*SH_TAS[0]);
Kfusion[12] = SK_TAS[0]*(P[12][4]*SH_TAS[2] - P[12][22]*SH_TAS[2] + P[12][5]*SK_TAS[1] - P[12][23]*SK_TAS[1] + P[12][6]*vd*SH_TAS[0]);
@ -86,7 +101,7 @@ void NavEKF3_core::FuseAirspeed()
memset(&Kfusion[10], 0, 12);
}
if (!inhibitDelVelBiasStates) {
if (!inhibitDelVelBiasStates && !airDataFusionWindOnly) {
Kfusion[13] = SK_TAS[0]*(P[13][4]*SH_TAS[2] - P[13][22]*SH_TAS[2] + P[13][5]*SK_TAS[1] - P[13][23]*SK_TAS[1] + P[13][6]*vd*SH_TAS[0]);
Kfusion[14] = SK_TAS[0]*(P[14][4]*SH_TAS[2] - P[14][22]*SH_TAS[2] + P[14][5]*SK_TAS[1] - P[14][23]*SK_TAS[1] + P[14][6]*vd*SH_TAS[0]);
Kfusion[15] = SK_TAS[0]*(P[15][4]*SH_TAS[2] - P[15][22]*SH_TAS[2] + P[15][5]*SK_TAS[1] - P[15][23]*SK_TAS[1] + P[15][6]*vd*SH_TAS[0]);
@ -96,7 +111,7 @@ void NavEKF3_core::FuseAirspeed()
}
// zero Kalman gains to inhibit magnetic field state estimation
if (!inhibitMagStates) {
if (!inhibitMagStates && !airDataFusionWindOnly) {
Kfusion[16] = SK_TAS[0]*(P[16][4]*SH_TAS[2] - P[16][22]*SH_TAS[2] + P[16][5]*SK_TAS[1] - P[16][23]*SK_TAS[1] + P[16][6]*vd*SH_TAS[0]);
Kfusion[17] = SK_TAS[0]*(P[17][4]*SH_TAS[2] - P[17][22]*SH_TAS[2] + P[17][5]*SK_TAS[1] - P[17][23]*SK_TAS[1] + P[17][6]*vd*SH_TAS[0]);
Kfusion[18] = SK_TAS[0]*(P[18][4]*SH_TAS[2] - P[18][22]*SH_TAS[2] + P[18][5]*SK_TAS[1] - P[18][23]*SK_TAS[1] + P[18][6]*vd*SH_TAS[0]);
@ -119,9 +134,6 @@ void NavEKF3_core::FuseAirspeed()
// calculate measurement innovation variance
varInnovVtas = 1.0f/SK_TAS[0];
// calculate measurement innovation
innovVtas = VtasPred - tasDataDelayed.tas;
// calculate the innovation consistency test ratio
tasTestRatio = sq(innovVtas) / (sq(MAX(0.01f * (float)frontend->_tasInnovGate, 1.0f)) * varInnovVtas);
@ -198,11 +210,6 @@ void NavEKF3_core::SelectTasFusion()
// get true airspeed measurement
readAirSpdData();
// If we haven't received airspeed data for a while, then declare the airspeed data as being timed out
if (imuSampleTime_ms - tasDataNew.time_ms > frontend->tasRetryTime_ms) {
tasTimeout = true;
}
// if the filter is initialised, wind states are not inhibited and we have data to fuse, then perform TAS fusion
if (tasDataToFuse && statesInitialised && !inhibitWindStates) {
FuseAirspeed();
@ -229,12 +236,30 @@ void NavEKF3_core::SelectBetaDragFusion()
// set true when the fusion time interval has triggered
bool f_timeTrigger = ((imuSampleTime_ms - prevBetaDragStep_ms) >= frontend->betaAvg_ms);
// set true when use of synthetic sideslip fusion is necessary because we have limited sensor data or are dead reckoning position
bool f_beta_required = !(use_compass() && useAirspeed() && ((imuSampleTime_ms - lastPosPassTime_ms) < frontend->posRetryTimeNoVel_ms));
// use of air data to constrain drift is necessary if we have limited sensor data or are doing inertial dead reckoning
bool is_dead_reckoning = ((imuSampleTime_ms - lastPosPassTime_ms) > frontend->deadReckonDeclare_ms) && ((imuSampleTime_ms - lastVelPassTime_ms) > frontend->deadReckonDeclare_ms);
const bool noYawSensor = !use_compass() && !using_external_yaw();
const bool f_required = (noYawSensor && (frontend->_betaMask & (1<<1))) || is_dead_reckoning;
// set true when sideslip fusion is feasible (requires zero sideslip assumption to be valid and use of wind states)
bool f_beta_feasible = (assume_zero_sideslip() && !inhibitWindStates);
const bool f_beta_feasible = (assume_zero_sideslip() && !inhibitWindStates);
// use synthetic sideslip fusion if feasible, required and enough time has lapsed since the last fusion
if (f_beta_feasible && f_beta_required && f_timeTrigger) {
if (f_beta_feasible && f_timeTrigger) {
// unless air data is required to constrain drift, it is only used to update wind state estimates
if (f_required || (frontend->_betaMask & (1<<0))) {
// we are required to correct all states
airDataFusionWindOnly = false;
} else {
// we are required to corrrect only wind states
airDataFusionWindOnly = true;
}
// Fuse estimated airspeed to aid wind estimation
if (is_positive(defaultAirSpeed)) {
frontend->_easNoise = 0.33f * defaultAirSpeed;
FuseAirspeed();
}
FuseSideslip();
prevBetaDragStep_ms = imuSampleTime_ms;
}
@ -346,18 +371,23 @@ void NavEKF3_core::FuseSideslip()
SK_BETA[6] = SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10];
SK_BETA[7] = SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8];
Kfusion[0] = SK_BETA[0]*(P[0][0]*SK_BETA[5] + P[0][1]*SK_BETA[4] - P[0][4]*SK_BETA[1] + P[0][5]*SK_BETA[2] + P[0][2]*SK_BETA[6] + P[0][6]*SK_BETA[3] - P[0][3]*SK_BETA[7] + P[0][22]*SK_BETA[1] - P[0][23]*SK_BETA[2]);
Kfusion[1] = SK_BETA[0]*(P[1][0]*SK_BETA[5] + P[1][1]*SK_BETA[4] - P[1][4]*SK_BETA[1] + P[1][5]*SK_BETA[2] + P[1][2]*SK_BETA[6] + P[1][6]*SK_BETA[3] - P[1][3]*SK_BETA[7] + P[1][22]*SK_BETA[1] - P[1][23]*SK_BETA[2]);
Kfusion[2] = SK_BETA[0]*(P[2][0]*SK_BETA[5] + P[2][1]*SK_BETA[4] - P[2][4]*SK_BETA[1] + P[2][5]*SK_BETA[2] + P[2][2]*SK_BETA[6] + P[2][6]*SK_BETA[3] - P[2][3]*SK_BETA[7] + P[2][22]*SK_BETA[1] - P[2][23]*SK_BETA[2]);
Kfusion[3] = SK_BETA[0]*(P[3][0]*SK_BETA[5] + P[3][1]*SK_BETA[4] - P[3][4]*SK_BETA[1] + P[3][5]*SK_BETA[2] + P[3][2]*SK_BETA[6] + P[3][6]*SK_BETA[3] - P[3][3]*SK_BETA[7] + P[3][22]*SK_BETA[1] - P[3][23]*SK_BETA[2]);
Kfusion[4] = SK_BETA[0]*(P[4][0]*SK_BETA[5] + P[4][1]*SK_BETA[4] - P[4][4]*SK_BETA[1] + P[4][5]*SK_BETA[2] + P[4][2]*SK_BETA[6] + P[4][6]*SK_BETA[3] - P[4][3]*SK_BETA[7] + P[4][22]*SK_BETA[1] - P[4][23]*SK_BETA[2]);
Kfusion[5] = SK_BETA[0]*(P[5][0]*SK_BETA[5] + P[5][1]*SK_BETA[4] - P[5][4]*SK_BETA[1] + P[5][5]*SK_BETA[2] + P[5][2]*SK_BETA[6] + P[5][6]*SK_BETA[3] - P[5][3]*SK_BETA[7] + P[5][22]*SK_BETA[1] - P[5][23]*SK_BETA[2]);
Kfusion[6] = SK_BETA[0]*(P[6][0]*SK_BETA[5] + P[6][1]*SK_BETA[4] - P[6][4]*SK_BETA[1] + P[6][5]*SK_BETA[2] + P[6][2]*SK_BETA[6] + P[6][6]*SK_BETA[3] - P[6][3]*SK_BETA[7] + P[6][22]*SK_BETA[1] - P[6][23]*SK_BETA[2]);
Kfusion[7] = SK_BETA[0]*(P[7][0]*SK_BETA[5] + P[7][1]*SK_BETA[4] - P[7][4]*SK_BETA[1] + P[7][5]*SK_BETA[2] + P[7][2]*SK_BETA[6] + P[7][6]*SK_BETA[3] - P[7][3]*SK_BETA[7] + P[7][22]*SK_BETA[1] - P[7][23]*SK_BETA[2]);
Kfusion[8] = SK_BETA[0]*(P[8][0]*SK_BETA[5] + P[8][1]*SK_BETA[4] - P[8][4]*SK_BETA[1] + P[8][5]*SK_BETA[2] + P[8][2]*SK_BETA[6] + P[8][6]*SK_BETA[3] - P[8][3]*SK_BETA[7] + P[8][22]*SK_BETA[1] - P[8][23]*SK_BETA[2]);
Kfusion[9] = SK_BETA[0]*(P[9][0]*SK_BETA[5] + P[9][1]*SK_BETA[4] - P[9][4]*SK_BETA[1] + P[9][5]*SK_BETA[2] + P[9][2]*SK_BETA[6] + P[9][6]*SK_BETA[3] - P[9][3]*SK_BETA[7] + P[9][22]*SK_BETA[1] - P[9][23]*SK_BETA[2]);
if (!airDataFusionWindOnly) {
Kfusion[0] = SK_BETA[0]*(P[0][0]*SK_BETA[5] + P[0][1]*SK_BETA[4] - P[0][4]*SK_BETA[1] + P[0][5]*SK_BETA[2] + P[0][2]*SK_BETA[6] + P[0][6]*SK_BETA[3] - P[0][3]*SK_BETA[7] + P[0][22]*SK_BETA[1] - P[0][23]*SK_BETA[2]);
Kfusion[1] = SK_BETA[0]*(P[1][0]*SK_BETA[5] + P[1][1]*SK_BETA[4] - P[1][4]*SK_BETA[1] + P[1][5]*SK_BETA[2] + P[1][2]*SK_BETA[6] + P[1][6]*SK_BETA[3] - P[1][3]*SK_BETA[7] + P[1][22]*SK_BETA[1] - P[1][23]*SK_BETA[2]);
Kfusion[2] = SK_BETA[0]*(P[2][0]*SK_BETA[5] + P[2][1]*SK_BETA[4] - P[2][4]*SK_BETA[1] + P[2][5]*SK_BETA[2] + P[2][2]*SK_BETA[6] + P[2][6]*SK_BETA[3] - P[2][3]*SK_BETA[7] + P[2][22]*SK_BETA[1] - P[2][23]*SK_BETA[2]);
Kfusion[3] = SK_BETA[0]*(P[3][0]*SK_BETA[5] + P[3][1]*SK_BETA[4] - P[3][4]*SK_BETA[1] + P[3][5]*SK_BETA[2] + P[3][2]*SK_BETA[6] + P[3][6]*SK_BETA[3] - P[3][3]*SK_BETA[7] + P[3][22]*SK_BETA[1] - P[3][23]*SK_BETA[2]);
Kfusion[4] = SK_BETA[0]*(P[4][0]*SK_BETA[5] + P[4][1]*SK_BETA[4] - P[4][4]*SK_BETA[1] + P[4][5]*SK_BETA[2] + P[4][2]*SK_BETA[6] + P[4][6]*SK_BETA[3] - P[4][3]*SK_BETA[7] + P[4][22]*SK_BETA[1] - P[4][23]*SK_BETA[2]);
Kfusion[5] = SK_BETA[0]*(P[5][0]*SK_BETA[5] + P[5][1]*SK_BETA[4] - P[5][4]*SK_BETA[1] + P[5][5]*SK_BETA[2] + P[5][2]*SK_BETA[6] + P[5][6]*SK_BETA[3] - P[5][3]*SK_BETA[7] + P[5][22]*SK_BETA[1] - P[5][23]*SK_BETA[2]);
Kfusion[6] = SK_BETA[0]*(P[6][0]*SK_BETA[5] + P[6][1]*SK_BETA[4] - P[6][4]*SK_BETA[1] + P[6][5]*SK_BETA[2] + P[6][2]*SK_BETA[6] + P[6][6]*SK_BETA[3] - P[6][3]*SK_BETA[7] + P[6][22]*SK_BETA[1] - P[6][23]*SK_BETA[2]);
Kfusion[7] = SK_BETA[0]*(P[7][0]*SK_BETA[5] + P[7][1]*SK_BETA[4] - P[7][4]*SK_BETA[1] + P[7][5]*SK_BETA[2] + P[7][2]*SK_BETA[6] + P[7][6]*SK_BETA[3] - P[7][3]*SK_BETA[7] + P[7][22]*SK_BETA[1] - P[7][23]*SK_BETA[2]);
Kfusion[8] = SK_BETA[0]*(P[8][0]*SK_BETA[5] + P[8][1]*SK_BETA[4] - P[8][4]*SK_BETA[1] + P[8][5]*SK_BETA[2] + P[8][2]*SK_BETA[6] + P[8][6]*SK_BETA[3] - P[8][3]*SK_BETA[7] + P[8][22]*SK_BETA[1] - P[8][23]*SK_BETA[2]);
Kfusion[9] = SK_BETA[0]*(P[9][0]*SK_BETA[5] + P[9][1]*SK_BETA[4] - P[9][4]*SK_BETA[1] + P[9][5]*SK_BETA[2] + P[9][2]*SK_BETA[6] + P[9][6]*SK_BETA[3] - P[9][3]*SK_BETA[7] + P[9][22]*SK_BETA[1] - P[9][23]*SK_BETA[2]);
} else {
// zero indexes 0 to 9 = 10*4 bytes
memset(&Kfusion[0], 0, 40);
}
if (!inhibitDelAngBiasStates) {
if (!inhibitDelAngBiasStates && !airDataFusionWindOnly) {
Kfusion[10] = SK_BETA[0]*(P[10][0]*SK_BETA[5] + P[10][1]*SK_BETA[4] - P[10][4]*SK_BETA[1] + P[10][5]*SK_BETA[2] + P[10][2]*SK_BETA[6] + P[10][6]*SK_BETA[3] - P[10][3]*SK_BETA[7] + P[10][22]*SK_BETA[1] - P[10][23]*SK_BETA[2]);
Kfusion[11] = SK_BETA[0]*(P[11][0]*SK_BETA[5] + P[11][1]*SK_BETA[4] - P[11][4]*SK_BETA[1] + P[11][5]*SK_BETA[2] + P[11][2]*SK_BETA[6] + P[11][6]*SK_BETA[3] - P[11][3]*SK_BETA[7] + P[11][22]*SK_BETA[1] - P[11][23]*SK_BETA[2]);
Kfusion[12] = SK_BETA[0]*(P[12][0]*SK_BETA[5] + P[12][1]*SK_BETA[4] - P[12][4]*SK_BETA[1] + P[12][5]*SK_BETA[2] + P[12][2]*SK_BETA[6] + P[12][6]*SK_BETA[3] - P[12][3]*SK_BETA[7] + P[12][22]*SK_BETA[1] - P[12][23]*SK_BETA[2]);
@ -366,7 +396,7 @@ void NavEKF3_core::FuseSideslip()
memset(&Kfusion[10], 0, 12);
}
if (!inhibitDelVelBiasStates) {
if (!inhibitDelVelBiasStates && !airDataFusionWindOnly) {
Kfusion[13] = SK_BETA[0]*(P[13][0]*SK_BETA[5] + P[13][1]*SK_BETA[4] - P[13][4]*SK_BETA[1] + P[13][5]*SK_BETA[2] + P[13][2]*SK_BETA[6] + P[13][6]*SK_BETA[3] - P[13][3]*SK_BETA[7] + P[13][22]*SK_BETA[1] - P[13][23]*SK_BETA[2]);
Kfusion[14] = SK_BETA[0]*(P[14][0]*SK_BETA[5] + P[14][1]*SK_BETA[4] - P[14][4]*SK_BETA[1] + P[14][5]*SK_BETA[2] + P[14][2]*SK_BETA[6] + P[14][6]*SK_BETA[3] - P[14][3]*SK_BETA[7] + P[14][22]*SK_BETA[1] - P[14][23]*SK_BETA[2]);
Kfusion[15] = SK_BETA[0]*(P[15][0]*SK_BETA[5] + P[15][1]*SK_BETA[4] - P[15][4]*SK_BETA[1] + P[15][5]*SK_BETA[2] + P[15][2]*SK_BETA[6] + P[15][6]*SK_BETA[3] - P[15][3]*SK_BETA[7] + P[15][22]*SK_BETA[1] - P[15][23]*SK_BETA[2]);
@ -376,7 +406,7 @@ void NavEKF3_core::FuseSideslip()
}
// zero Kalman gains to inhibit magnetic field state estimation
if (!inhibitMagStates) {
if (!inhibitMagStates && !airDataFusionWindOnly) {
Kfusion[16] = SK_BETA[0]*(P[16][0]*SK_BETA[5] + P[16][1]*SK_BETA[4] - P[16][4]*SK_BETA[1] + P[16][5]*SK_BETA[2] + P[16][2]*SK_BETA[6] + P[16][6]*SK_BETA[3] - P[16][3]*SK_BETA[7] + P[16][22]*SK_BETA[1] - P[16][23]*SK_BETA[2]);
Kfusion[17] = SK_BETA[0]*(P[17][0]*SK_BETA[5] + P[17][1]*SK_BETA[4] - P[17][4]*SK_BETA[1] + P[17][5]*SK_BETA[2] + P[17][2]*SK_BETA[6] + P[17][6]*SK_BETA[3] - P[17][3]*SK_BETA[7] + P[17][22]*SK_BETA[1] - P[17][23]*SK_BETA[2]);
Kfusion[18] = SK_BETA[0]*(P[18][0]*SK_BETA[5] + P[18][1]*SK_BETA[4] - P[18][4]*SK_BETA[1] + P[18][5]*SK_BETA[2] + P[18][2]*SK_BETA[6] + P[18][6]*SK_BETA[3] - P[18][3]*SK_BETA[7] + P[18][22]*SK_BETA[1] - P[18][23]*SK_BETA[2]);

5
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -978,10 +978,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)
// Writes the default equivalent airspeed and 1-sigma uncertainty in m/s to be used in forward flight if a measured airspeed is required and not available.
void NavEKF3_core::writeDefaultAirSpeed(float airspeed, float uncertainty)
{
defaultAirSpeed = airspeed;
defaultAirSpeedVariance = sq(uncertainty);
}
/********************************************************

5
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -171,14 +171,15 @@ uint32_t NavEKF3_core::getLastVelNorthEastReset(Vector2f &vel) const
}
// return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis)
void NavEKF3_core::getWind(Vector3f &wind) const
// returns true if wind state estimation is active
bool NavEKF3_core::getWind(Vector3f &wind) const
{
wind.x = stateStruct.wind_vel.x;
wind.y = stateStruct.wind_vel.y;
wind.z = 0.0f; // currently don't estimate this
return !inhibitWindStates;
}
// return the NED velocity of the body frame origin in m/s
//
void NavEKF3_core::getVelNED(Vector3f &vel) const

1
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -310,6 +310,7 @@ void NavEKF3_core::InitialiseVariables()
flowFusionActive = false;
airSpdFusionDelayed = false;
sideSlipFusionDelayed = false;
airDataFusionWindOnly = false;
posResetNE.zero();
velResetNE.zero();
posResetD = 0.0f;

9
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -147,7 +147,8 @@ public:
void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const;
// return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis)
void getWind(Vector3f &wind) const;
// returns true if wind state estimation is active
bool getWind(Vector3f &wind) const;
// return earth magnetic field estimates in measurement units / 1000
void getMagNED(Vector3f &magNED) const;
@ -381,8 +382,8 @@ public:
// 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);
// Writes the default equivalent airspeed and 1-sigma uncertainty in m/s to be used in forward flight if a measured airspeed is required and not available.
void writeDefaultAirSpeed(float airspeed, float uncertainty);
// request a reset the yaw to the EKF-GSF value
void EKFGSF_requestYawReset();
@ -990,6 +991,7 @@ private:
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.
float defaultAirSpeedVariance; // default equivalent airspeed variance in (m/s)**2 to be used when defaultAirSpeed is specified.
bool magFusePerformed; // boolean set to true when magnetometer fusion has been perfomred in that time step
MagCal effectiveMagCal; // the actual mag calibration being used as the default
uint32_t prevTasStep_ms; // time stamp of last TAS fusion step
@ -1074,6 +1076,7 @@ private:
bool optFlowFusionDelayed; // true when the optical flow fusion has been delayed
bool airSpdFusionDelayed; // true when the air speed fusion has been delayed
bool sideSlipFusionDelayed; // true when the sideslip fusion has been delayed
bool airDataFusionWindOnly; // true when sideslip and airspeed fusion is only allowed to modify the wind states
Vector3f lastMagOffsets; // Last magnetometer offsets from COMPASS_ parameters. Used to detect parameter changes.
bool lastMagOffsetsValid; // True when lastMagOffsets has been initialized
Vector2f posResetNE; // Change in North/East position due to last in-flight reset in metres. Returned by getLastPosNorthEastReset