|
|
|
@ -2051,7 +2051,7 @@ bool AP_AHRS_NavEKF::get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vec
@@ -2051,7 +2051,7 @@ bool AP_AHRS_NavEKF::get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vec
|
|
|
|
|
// indicates prefect consistency between the measurement and the EKF solution and a value of of 1 is the maximum
|
|
|
|
|
// inconsistency that will be accpeted by the filter
|
|
|
|
|
// boolean false is returned if variances are not available
|
|
|
|
|
bool AP_AHRS_NavEKF::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const |
|
|
|
|
bool AP_AHRS_NavEKF::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const |
|
|
|
|
{ |
|
|
|
|
switch (ekf_type()) { |
|
|
|
|
case EKFType::NONE: |
|
|
|
@ -2059,17 +2059,21 @@ bool AP_AHRS_NavEKF::get_variances(float &velVar, float &posVar, float &hgtVar,
@@ -2059,17 +2059,21 @@ bool AP_AHRS_NavEKF::get_variances(float &velVar, float &posVar, float &hgtVar,
|
|
|
|
|
return false; |
|
|
|
|
|
|
|
|
|
#if HAL_NAVEKF2_AVAILABLE |
|
|
|
|
case EKFType::TWO: |
|
|
|
|
case EKFType::TWO: { |
|
|
|
|
// use EKF to get variance
|
|
|
|
|
EKF2.getVariances(-1,velVar, posVar, hgtVar, magVar, tasVar, offset); |
|
|
|
|
Vector2f offset; |
|
|
|
|
EKF2.getVariances(-1, velVar, posVar, hgtVar, magVar, tasVar, offset); |
|
|
|
|
return true; |
|
|
|
|
} |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
#if HAL_NAVEKF3_AVAILABLE |
|
|
|
|
case EKFType::THREE: |
|
|
|
|
case EKFType::THREE: { |
|
|
|
|
// use EKF to get variance
|
|
|
|
|
EKF3.getVariances(-1,velVar, posVar, hgtVar, magVar, tasVar, offset); |
|
|
|
|
Vector2f offset; |
|
|
|
|
EKF3.getVariances(-1, velVar, posVar, hgtVar, magVar, tasVar, offset); |
|
|
|
|
return true; |
|
|
|
|
} |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL |
|
|
|
@ -2079,7 +2083,6 @@ bool AP_AHRS_NavEKF::get_variances(float &velVar, float &posVar, float &hgtVar,
@@ -2079,7 +2083,6 @@ bool AP_AHRS_NavEKF::get_variances(float &velVar, float &posVar, float &hgtVar,
|
|
|
|
|
hgtVar = 0; |
|
|
|
|
magVar.zero(); |
|
|
|
|
tasVar = 0; |
|
|
|
|
offset.zero(); |
|
|
|
|
return true; |
|
|
|
|
#endif |
|
|
|
|
} |
|
|
|
|