dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: fully disable yaw estimator EKFGSF_yaw with CONFIG_EKF2_GNSS

This commit is contained in:
Daniel Agar 2023-10-17 14:08:24 -04:00
parent 755b45441f
commit fe3314e33e
7 changed files with 94 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/ekf2/EKF/CMakeLists.txt
View File

@ -38,7 +38,6 @@ list(APPEND EKF_SRCS
covariance.cpp
ekf.cpp
ekf_helper.cpp
EKFGSF_yaw.cpp
estimator_interface.cpp
fake_height_control.cpp
fake_pos_control.cpp
@ -85,6 +84,7 @@ if(CONFIG_EKF2_GNSS)
gnss_height_control.cpp
gps_checks.cpp
gps_control.cpp
EKFGSF_yaw.cpp
)
endif()

9
src/modules/ekf2/EKF/airspeed_fusion.cpp
View File

@ -62,6 +62,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
_control_status.flags.wind = false;
}
#if defined(CONFIG_EKF2_GNSS)
// clear yaw estimator airspeed (updated later with true airspeed if airspeed fusion is active)
if (_control_status.flags.fixed_wing) {
if (_control_status.flags.in_air && !_control_status.flags.vehicle_at_rest) {
@ -73,6 +74,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
_yawEstimator.setTrueAirspeed(0.f);
}
}
#endif // CONFIG_EKF2_GNSS
if (_params.arsp_thr <= 0.f) {
stopAirspeedFusion();
@ -99,7 +101,9 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
fuseAirspeed(airspeed_sample, _aid_src_airspeed);
}
#if defined(CONFIG_EKF2_GNSS)
_yawEstimator.setTrueAirspeed(airspeed_sample.true_airspeed);
#endif // CONFIG_EKF2_GNSS
const bool is_fusion_failing = isTimedOut(_aid_src_airspeed.time_last_fuse, (uint64_t)10e6);
@ -219,8 +223,11 @@ void Ekf::stopAirspeedFusion()
if (_control_status.flags.fuse_aspd) {
ECL_INFO("stopping airspeed fusion");
resetEstimatorAidStatus(_aid_src_airspeed);
_yawEstimator.setTrueAirspeed(NAN);
_control_status.flags.fuse_aspd = false;
#if defined(CONFIG_EKF2_GNSS)
_yawEstimator.setTrueAirspeed(NAN);
#endif // CONFIG_EKF2_GNSS
}
}

44
src/modules/ekf2/EKF/ekf.h
View File

@ -45,7 +45,10 @@
#include "estimator_interface.h"
#include "EKFGSF_yaw.h"
#if defined(CONFIG_EKF2_GNSS)
# include "EKFGSF_yaw.h"
#endif // CONFIG_EKF2_GNSS
#include "bias_estimator.hpp"
#include "height_bias_estimator.hpp"
#include "position_bias_estimator.hpp"
@ -492,9 +495,6 @@ public:
Vector3f calcRotVecVariances() const;
float getYawVar() const;
// Returns true if the output of the yaw emergency estimator can be used for a reset
bool isYawEmergencyEstimateAvailable() const;
uint8_t getHeightSensorRef() const { return _height_sensor_ref; }
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
@ -537,11 +537,6 @@ public:
bool gps_checks_passed() const { return _gps_checks_passed; };
// get solution data from the EKF-GSF emergency yaw estimator
// returns false when data is not available
bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]);
const BiasEstimator::status &getGpsHgtBiasEstimatorStatus() const { return _gps_hgt_b_est.getStatus(); }
const auto &aid_src_gnss_hgt() const { return _aid_src_gnss_hgt; }
@ -551,6 +546,15 @@ public:
# if defined(CONFIG_EKF2_GNSS_YAW)
const auto &aid_src_gnss_yaw() const { return _aid_src_gnss_yaw; }
# endif // CONFIG_EKF2_GNSS_YAW
// Returns true if the output of the yaw emergency estimator can be used for a reset
bool isYawEmergencyEstimateAvailable() const;
// get solution data from the EKF-GSF emergency yaw estimator
// returns false when data is not available
bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]);
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_MAGNETOMETER)
@ -1090,7 +1094,6 @@ private:
void stopGpsFusion();
bool shouldResetGpsFusion() const;
bool isYawFailure() const;
// return true id the GPS quality is good enough to set an origin and start aiding
bool gps_is_good(const gpsMessage &gps);
@ -1098,11 +1101,6 @@ private:
void controlGnssHeightFusion(const gpsSample &gps_sample);
void stopGpsHgtFusion();
// Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator
// Resets the horizontal velocity and position to the default navigation sensor
// Returns true if the reset was successful
bool resetYawToEKFGSF();
void resetGpsDriftCheckFilters();
# if defined(CONFIG_EKF2_GNSS_YAW)
@ -1119,6 +1117,17 @@ private:
void updateGpsYaw(const gpsSample &gps_sample);
# endif // CONFIG_EKF2_GNSS_YAW
// Declarations used to control use of the EKF-GSF yaw estimator
bool isYawFailure() const;
// Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator
// Returns true if the reset was successful
bool resetYawToEKFGSF();
// yaw estimator instance
EKFGSF_yaw _yawEstimator{};
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_MAGNETOMETER)
@ -1237,11 +1246,6 @@ private:
// yaw_variance : yaw error variance (rad^2)
void resetQuatStateYaw(float yaw, float yaw_variance);
// Declarations used to control use of the EKF-GSF yaw estimator
// yaw estimator instance
EKFGSF_yaw _yawEstimator{};
uint8_t _height_sensor_ref{HeightSensor::UNKNOWN};
uint8_t _position_sensor_ref{static_cast<uint8_t>(PositionSensor::GNSS)};

50
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -1028,56 +1028,6 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
_time_last_heading_fuse = _time_delayed_us;
}
#if defined(CONFIG_EKF2_GNSS)
bool Ekf::resetYawToEKFGSF()
{
if (!isYawEmergencyEstimateAvailable()) {
return false;
}
// don't allow reset if there's just been a yaw reset
const bool yaw_alignment_changed = (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
const bool quat_reset = (_state_reset_status.reset_count.quat != _state_reset_count_prev.quat);
if (yaw_alignment_changed || quat_reset) {
return false;
}
ECL_INFO("yaw estimator reset heading %.3f -> %.3f rad",
(double)getEulerYaw(_R_to_earth), (double)_yawEstimator.getYaw());
resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());
_control_status.flags.yaw_align = true;
_information_events.flags.yaw_aligned_to_imu_gps = true;
return true;
}
#endif // CONFIG_EKF2_GNSS
bool Ekf::isYawEmergencyEstimateAvailable() const
{
#if defined(CONFIG_EKF2_GNSS)
// don't allow reet using the EKF-GSF estimate until the filter has started fusing velocity
// data and the yaw estimate has converged
if (!_yawEstimator.isActive()) {
return false;
}
return _yawEstimator.getYawVar() < sq(_params.EKFGSF_yaw_err_max);
#else
return false;
#endif
}
#if defined(CONFIG_EKF2_GNSS)
bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF])
{
return _yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight);
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_WIND)
void Ekf::resetWind()
{

66
src/modules/ekf2/EKF/gps_control.cpp
View File

@ -302,18 +302,6 @@ bool Ekf::shouldResetGpsFusion() const
return (is_reset_required || is_inflight_nav_failure);
}
bool Ekf::isYawFailure() const
{
if (!isYawEmergencyEstimateAvailable()) {
return false;
}
const float euler_yaw = getEulerYaw(_R_to_earth);
const float yaw_error = wrap_pi(euler_yaw - _yawEstimator.getYaw());
return fabsf(yaw_error) > math::radians(25.f);
}
#if defined(CONFIG_EKF2_GNSS_YAW)
void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing)
{
@ -436,3 +424,57 @@ void Ekf::stopGpsFusion()
stopGpsYawFusion();
#endif // CONFIG_EKF2_GNSS_YAW
}
bool Ekf::isYawEmergencyEstimateAvailable() const
{
// don't allow reet using the EKF-GSF estimate until the filter has started fusing velocity
// data and the yaw estimate has converged
if (!_yawEstimator.isActive()) {
return false;
}
return _yawEstimator.getYawVar() < sq(_params.EKFGSF_yaw_err_max);
}
bool Ekf::isYawFailure() const
{
if (!isYawEmergencyEstimateAvailable()) {
return false;
}
const float euler_yaw = getEulerYaw(_R_to_earth);
const float yaw_error = wrap_pi(euler_yaw - _yawEstimator.getYaw());
return fabsf(yaw_error) > math::radians(25.f);
}
bool Ekf::resetYawToEKFGSF()
{
if (!isYawEmergencyEstimateAvailable()) {
return false;
}
// don't allow reset if there's just been a yaw reset
const bool yaw_alignment_changed = (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
const bool quat_reset = (_state_reset_status.reset_count.quat != _state_reset_count_prev.quat);
if (yaw_alignment_changed || quat_reset) {
return false;
}
ECL_INFO("yaw estimator reset heading %.3f -> %.3f rad",
(double)getEulerYaw(_R_to_earth), (double)_yawEstimator.getYaw());
resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());
_control_status.flags.yaw_align = true;
_information_events.flags.yaw_aligned_to_imu_gps = true;
return true;
}
bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF])
{
return _yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight);
}

7
src/modules/ekf2/EKF/mag_control.cpp
View File

@ -155,7 +155,9 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading)
* Vector3f(_mag_strength_gps, 0, 0);
// mag_B: reset
#if defined(CONFIG_EKF2_GNSS)
if (isYawEmergencyEstimateAvailable()) {
const Dcmf R_to_earth = updateYawInRotMat(_yawEstimator.getYaw(), _R_to_earth);
const Dcmf R_to_body = R_to_earth.transpose();
@ -163,8 +165,9 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading)
_state.mag_B = mag - (R_to_body * mag_earth_pred);
ECL_INFO("resetMagStates using yaw estimator");
} else if (!reset_heading && _control_status.flags.yaw_align) {
} else
#endif // CONFIG_EKF2_GNSS
if (!reset_heading && _control_status.flags.yaw_align) {
// mag_B: reset using WMM
const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal);
_state.mag_B = mag - (R_to_body * mag_earth_pred);

2
src/modules/ekf2/EKF2.hpp
View File

@ -507,7 +507,9 @@ private:
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_gnss_hgt_pub{ORB_ID(estimator_aid_src_gnss_hgt)};
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)};
uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)};
uORB::PublicationMulti<yaw_estimator_status_s> _yaw_est_pub{ORB_ID(yaw_estimator_status)};
# if defined(CONFIG_EKF2_GNSS_YAW)
hrt_abstime _status_gnss_yaw_pub_last {0};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_gnss_yaw_pub {ORB_ID(estimator_aid_src_gnss_yaw)};