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

ekf2: estimator aid source status (range height)

This commit is contained in:
Daniel Agar 2022-05-22 14:14:50 -04:00
parent 1e25aee6fa
commit 58ea6235fe
9 changed files with 63 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/estimator_aid_source_1d.msg
View File

@ -19,4 +19,4 @@ bool innovation_rejected # true if the observation has been rejected
bool fused # true if the sample was successfully fused
# TOPICS estimator_aid_source_1d
# TOPICS estimator_aid_src_baro_hgt
# TOPICS estimator_aid_src_baro_hgt estimator_aid_src_rng_hgt

14
src/modules/ekf2/EKF/control.cpp
View File

@ -125,8 +125,8 @@ void Ekf::controlFusionModes()
if (_range_buffer) {
// Get range data from buffer and check validity
bool is_rng_data_ready = _range_buffer->pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress());
_range_sensor.setDataReadiness(is_rng_data_ready);
_rng_data_ready = _range_buffer->pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress());
_range_sensor.setDataReadiness(_rng_data_ready);
// update range sensor angle parameters in case they have changed
_range_sensor.setPitchOffset(_params.rng_sens_pitch);
@ -959,13 +959,15 @@ void Ekf::controlHeightFusion()
}
}
if (_control_status.flags.rng_hgt) {
if (_rng_data_ready) {
updateRngHgt(_aid_src_rng_hgt);
if (_range_sensor.isDataHealthy()) {
fuseRngHgt();
if (_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) {
fuseRngHgt(_aid_src_rng_hgt);
}
}
} else if (_control_status.flags.ev_hgt) {
if (_control_status.flags.ev_hgt) {
if (_control_status.flags.ev_hgt && _ev_data_ready) {
fuseEvHgt();

2
src/modules/ekf2/EKF/covariance.cpp
View File

@ -993,7 +993,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
if (bad_vz_gps || bad_vz_ev) {
bool bad_z_baro = _control_status.flags.baro_hgt && (down_dvel_bias * _aid_src_baro_hgt.innovation < 0.0f);
bool bad_z_gps = _control_status.flags.gps_hgt && (down_dvel_bias * _aid_src_gnss_pos.innovation[2] < 0.0f);
bool bad_z_rng = _control_status.flags.rng_hgt && (down_dvel_bias * _rng_hgt_innov < 0.0f);
bool bad_z_rng = _control_status.flags.rng_hgt && (down_dvel_bias * _aid_src_rng_hgt.innovation < 0.0f);
bool bad_z_ev = _control_status.flags.ev_hgt && (down_dvel_bias * _ev_pos_innov(2) < 0.0f);

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

@ -90,9 +90,9 @@ public:
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; }
void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _rng_hgt_test_ratio; }
void getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _aid_src_rng_hgt.innovation; }
void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _aid_src_rng_hgt.innovation_variance; }
void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _aid_src_rng_hgt.test_ratio; }
void getAuxVelInnov(float aux_vel_innov[2]) const;
void getAuxVelInnovVar(float aux_vel_innov[2]) const;
@ -341,6 +341,7 @@ public:
const BaroBiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); }
const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; }
const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; }
const auto &aid_src_fake_pos() const { return _aid_src_fake_pos; }
@ -388,6 +389,7 @@ private:
// booleans true when fresh sensor data is available at the fusion time horizon
bool _gps_data_ready{false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
bool _baro_data_ready{false}; ///< true when new baro height data has fallen behind the fusion time horizon and is available to be fused
bool _rng_data_ready{false};
bool _flow_data_ready{false}; ///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon
bool _ev_data_ready{false}; ///< true when new external vision system data has fallen behind the fusion time horizon and is available to be fused
bool _tas_data_ready{false}; ///< true when new true airspeed data has fallen behind the fusion time horizon and is available to be fused
@ -454,9 +456,6 @@ private:
Vector3f _ev_pos_innov{}; ///< external vision position innovations (m)
Vector3f _ev_pos_innov_var{}; ///< external vision position innovation variances (m**2)
float _rng_hgt_innov{}; ///< range hgt innovations (m)
float _rng_hgt_innov_var{}; ///< range hgt innovation variances (m**2)
Vector3f _aux_vel_innov{}; ///< horizontal auxiliary velocity innovations: (m/sec)
Vector3f _aux_vel_innov_var{}; ///< horizontal auxiliary velocity innovation variances: ((m/sec)**2)
@ -493,6 +492,7 @@ private:
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_1d_s _aid_src_baro_hgt{};
estimator_aid_source_1d_s _aid_src_rng_hgt{};
estimator_aid_source_2d_s _aid_src_fake_pos{};
@ -646,11 +646,11 @@ private:
void fuseDrag(const dragSample &drag_sample);
void fuseBaroHgt(estimator_aid_source_1d_s &baro_hgt);
void fuseRngHgt();
void fuseRngHgt(estimator_aid_source_1d_s &range_hgt);
void fuseEvHgt();
void updateBaroHgt(const baroSample &baro_sample, estimator_aid_source_1d_s &baro_hgt);
void updateRngHgt(estimator_aid_source_1d_s &rng_hgt);
// fuse single velocity and position measurement
bool fuseVelPosHeight(const float innov, const float innov_var, const int obs_index);

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

@ -947,7 +947,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
hgt = math::max(sqrtf(_aid_src_gnss_pos.test_ratio[2]), FLT_MIN);
} else if (_control_status.flags.rng_hgt) {
hgt = math::max(sqrtf(_rng_hgt_test_ratio), FLT_MIN);
hgt = math::max(sqrtf(_aid_src_rng_hgt.test_ratio), FLT_MIN);
} else if (_control_status.flags.ev_hgt) {
hgt = math::max(sqrtf(_ev_pos_test_ratio(1)), FLT_MIN);

1
src/modules/ekf2/EKF/estimator_interface.h
View File

@ -334,7 +334,6 @@ protected:
Vector2f _ev_vel_test_ratio{}; // EV velocity innovation consistency check ratios
Vector2f _ev_pos_test_ratio{}; // EV position innovation consistency check ratios
Vector2f _aux_vel_test_ratio{}; // Auxiliary horizontal velocity innovation consistency check ratio
float _rng_hgt_test_ratio{}; // range finder height innovation consistency check ratios
float _optflow_test_ratio{}; // Optical flow innovation consistency check ratio
float _tas_test_ratio{}; // tas innovation consistency check ratio
float _hagl_test_ratio{}; // height above terrain measurement innovation consistency check ratio

46
src/modules/ekf2/EKF/height_fusion.cpp
View File

@ -99,20 +99,48 @@ void Ekf::fuseBaroHgt(estimator_aid_source_1d_s &baro_hgt)
}
}
void Ekf::fuseRngHgt()
void Ekf::updateRngHgt(estimator_aid_source_1d_s &rng_hgt)
{
// use range finder with tilt correction
_rng_hgt_innov = _state.pos(2) - (-math::max(_range_sensor.getDistBottom(),
_params.rng_gnd_clearance)) - _rng_hgt_offset;
// innovation gate size
float innov_gate = fmaxf(_params.range_innov_gate, 1.f);
// reset flags
resetEstimatorAidStatusFlags(rng_hgt);
// observation variance - user parameter defined
float obs_var = fmaxf(sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f);
fuseVerticalPosition(_rng_hgt_innov, innov_gate, obs_var,
_rng_hgt_innov_var, _rng_hgt_test_ratio);
// innovation gate size
float innov_gate = fmaxf(_params.range_innov_gate, 1.f);
// vertical position innovation, use range finder with tilt correction
rng_hgt.observation = (-math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance)) + _rng_hgt_offset;
rng_hgt.observation_variance = obs_var;
rng_hgt.innovation = _state.pos(2) - rng_hgt.observation;
rng_hgt.innovation_variance = P(9, 9) + obs_var;
setEstimatorAidStatusTestRatio(rng_hgt, innov_gate);
// special case if there is bad vertical acceleration data, then don't reject measurement,
// but limit innovation to prevent spikes that could destabilise the filter
if (_fault_status.flags.bad_acc_vertical && rng_hgt.innovation_rejected) {
const float innov_limit = innov_gate * sqrtf(rng_hgt.innovation_variance);
rng_hgt.innovation = math::constrain(rng_hgt.innovation, -innov_limit, innov_limit);
rng_hgt.innovation_rejected = false;
}
rng_hgt.fusion_enabled = _control_status.flags.rng_hgt;
rng_hgt.timestamp_sample = _range_sensor.getSampleAddress()->time_us;
}
void Ekf::fuseRngHgt(estimator_aid_source_1d_s &rng_hgt)
{
if (rng_hgt.fusion_enabled
&& !rng_hgt.innovation_rejected
&& fuseVelPosHeight(rng_hgt.innovation, rng_hgt.innovation_variance, 5)) {
rng_hgt.fused = true;
rng_hgt.time_last_fuse = _time_last_imu;
}
}
void Ekf::fuseEvHgt()

3
src/modules/ekf2/EKF2.cpp
View File

@ -639,6 +639,9 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
// baro height
PublishAidSourceStatus(_ekf.aid_src_baro_hgt(), _status_baro_hgt_pub_last, _estimator_aid_src_baro_hgt_pub);
// RNG height
PublishAidSourceStatus(_ekf.aid_src_rng_hgt(), _status_rng_hgt_pub_last, _estimator_aid_src_rng_hgt_pub);
// fake position
PublishAidSourceStatus(_ekf.aid_src_fake_pos(), _status_fake_pos_pub_last, _estimator_aid_src_fake_pos_pub);

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

@ -256,6 +256,7 @@ private:
hrt_abstime _last_gps_status_published{0};
hrt_abstime _status_baro_hgt_pub_last{0};
hrt_abstime _status_rng_hgt_pub_last{0};
hrt_abstime _status_fake_pos_pub_last{0};
@ -322,7 +323,10 @@ private:
uORB::PublicationMulti<yaw_estimator_status_s> _yaw_est_pub{ORB_ID(yaw_estimator_status)};
uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_baro_hgt_pub{ORB_ID(estimator_aid_src_baro_hgt)};
uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_rng_hgt_pub{ORB_ID(estimator_aid_src_rng_hgt)};
uORB::PublicationMulti<estimator_aid_source_2d_s> _estimator_aid_src_fake_pos_pub{ORB_ID(estimator_aid_src_fake_pos)};
uORB::PublicationMulti<estimator_aid_source_3d_s> _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)};
uORB::PublicationMulti<estimator_aid_source_3d_s> _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)};