fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 09:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: estimator aid source status (baro height)

Browse Source
This commit is contained in:
Daniel Agar
2022-05-25 09:25:12 -04:00
parent 4dbe6f0a1c
commit 1e25aee6fa
13 changed files with 218 additions and 156 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.h
+34 -15
View File
@@ -86,9 +86,9 @@ public:
void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const;
void getBaroHgtInnov(float &baro_hgt_innov) const { baro_hgt_innov = _baro_hgt_innov; }
void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _baro_hgt_innov_var; }
void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const { baro_hgt_innov_ratio = _baro_hgt_test_ratio; }
void getBaroHgtInnov(float &baro_hgt_innov) const { baro_hgt_innov = _aid_src_baro_hgt.innovation; }
void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _aid_src_baro_hgt.innovation_variance; }
void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const { baro_hgt_innov_ratio = _aid_src_baro_hgt.test_ratio; }
void getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _rng_hgt_innov; }
void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _rng_hgt_innov_var; }
@@ -340,11 +340,13 @@ public:
const BaroBiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); }
const auto &aid_src_gnss_vel() const { return _aid_src_gnss_vel; }
const auto &aid_src_gnss_pos() const { return _aid_src_gnss_pos; }
const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; }
const auto &aid_src_fake_pos() const { return _aid_src_fake_pos; }
const auto &aid_src_gnss_vel() const { return _aid_src_gnss_vel; }
const auto &aid_src_gnss_pos() const { return _aid_src_gnss_pos; }
private:
// set the internal states and status to their default value
@@ -452,9 +454,6 @@ private:
Vector3f _ev_pos_innov{}; ///< external vision position innovations (m)
Vector3f _ev_pos_innov_var{}; ///< external vision position innovation variances (m**2)
float _baro_hgt_innov{}; ///< baro hgt innovations (m)
float _baro_hgt_innov_var{}; ///< baro hgt innovation variances (m**2)
float _rng_hgt_innov{}; ///< range hgt innovations (m)
float _rng_hgt_innov_var{}; ///< range hgt innovation variances (m**2)
@@ -493,11 +492,13 @@ private:
bool _inhibit_flow_use{false}; ///< true when use of optical flow and range finder is being inhibited
Vector2f _flow_compensated_XY_rad{}; ///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive
estimator_aid_source_3d_s _aid_src_gnss_vel{};
estimator_aid_source_3d_s _aid_src_gnss_pos{};
estimator_aid_source_1d_s _aid_src_baro_hgt{};
estimator_aid_source_2d_s _aid_src_fake_pos{};
estimator_aid_source_3d_s _aid_src_gnss_vel{};
estimator_aid_source_3d_s _aid_src_gnss_pos{};
// output predictor states
Vector3f _delta_angle_corr{}; ///< delta angle correction vector (rad)
Vector3f _vel_err_integ{}; ///< integral of velocity tracking error (m)
@@ -644,10 +645,13 @@ private:
// fuse body frame drag specific forces for multi-rotor wind estimation
void fuseDrag(const dragSample &drag_sample);
void fuseBaroHgt();
void fuseBaroHgt(estimator_aid_source_1d_s &baro_hgt);
void fuseRngHgt();
void fuseEvHgt();
void updateBaroHgt(const baroSample &baro_sample, estimator_aid_source_1d_s &baro_hgt);
// fuse single velocity and position measurement
bool fuseVelPosHeight(const float innov, const float innov_var, const int obs_index);
@@ -1063,7 +1067,7 @@ private:
void resetGpsDriftCheckFilters();
bool resetEstimatorAidStatusFlags(estimator_aid_source_1d_s &status)
bool resetEstimatorAidStatusFlags(estimator_aid_source_1d_s &status) const
{
if (status.timestamp_sample != 0) {
status.timestamp_sample = 0;
@@ -1077,7 +1081,7 @@ private:
return false;
}
void resetEstimatorAidStatus(estimator_aid_source_1d_s &status)
void resetEstimatorAidStatus(estimator_aid_source_1d_s &status) const
{
if (resetEstimatorAidStatusFlags(status)) {
status.observation = 0;
@@ -1090,7 +1094,7 @@ private:
}
template <typename T>
bool resetEstimatorAidStatusFlags(T &status)
bool resetEstimatorAidStatusFlags(T &status) const
{
if (status.timestamp_sample != 0) {
status.timestamp_sample = 0;
@@ -1108,7 +1112,7 @@ private:
}
template <typename T>
void resetEstimatorAidStatus(T &status)
void resetEstimatorAidStatus(T &status) const
{
if (resetEstimatorAidStatusFlags(status)) {
for (size_t i = 0; i < (sizeof(status.fusion_enabled) / sizeof(status.fusion_enabled[0])); i++) {
@@ -1121,6 +1125,21 @@ private:
}
}
}
void setEstimatorAidStatusTestRatio(estimator_aid_source_1d_s &status, float innovation_gate) const
{
status.test_ratio = sq(status.innovation) / (sq(innovation_gate) * status.innovation_variance);
status.innovation_rejected = (status.test_ratio > 1.f);
}
template <typename T>
void setEstimatorAidStatusTestRatio(T &status, float innovation_gate) const
{
for (size_t i = 0; i < (sizeof(status.test_ratio) / sizeof(status.test_ratio[0])); i++) {
status.test_ratio[i] = sq(status.innovation[i]) / (sq(innovation_gate) * status.innovation_variance[i]);
status.innovation_rejected[i] = (status.test_ratio[i] > 1.f);
}
}
};
#endif // !EKF_EKF_H