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

ekf2: move height fusion code to separate source file

Browse Source
This commit is contained in:
bresch
2021-12-06 15:26:58 +01:00
committed by Mathieu Bresciani
parent 452a7199a6
commit d583ef6b79
5 changed files with 129 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/control.cpp
+4 -64
View File
@@ -955,76 +955,16 @@ void Ekf::controlHeightFusion()
if (fuse_height) {
if (_control_status.flags.baro_hgt) {
Vector2f baro_hgt_innov_gate;
Vector3f baro_hgt_obs_var;
// vertical position innovation - baro measurement has opposite sign to earth z axis
const float unbiased_baro = _baro_sample_delayed.hgt - _baro_b_est.getBias();
_baro_hgt_innov(2) = _state.pos(2) + unbiased_baro - _baro_hgt_offset;
// observation variance - user parameter defined
baro_hgt_obs_var(2) = sq(fmaxf(_params.baro_noise, 0.01f));
// innovation gate size
baro_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f);
// Compensate for positive static pressure transients (negative vertical position innovations)
// caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
const float deadzone_start = 0.0f;
const float deadzone_end = deadzone_start + _params.gnd_effect_deadzone;
if (_control_status.flags.gnd_effect) {
if (_baro_hgt_innov(2) < -deadzone_start) {
if (_baro_hgt_innov(2) <= -deadzone_end) {
_baro_hgt_innov(2) += deadzone_end;
} else {
_baro_hgt_innov(2) = -deadzone_start;
}
}
}
// fuse height information
fuseVerticalPosition(_baro_hgt_innov, baro_hgt_innov_gate,
baro_hgt_obs_var, _baro_hgt_innov_var, _baro_hgt_test_ratio);
fuseBaroHgt();
} else if (_control_status.flags.gps_hgt) {
Vector2f gps_hgt_innov_gate;
Vector3f gps_hgt_obs_var;
// vertical position innovation - gps measurement has opposite sign to earth z axis
_gps_pos_innov(2) = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset;
gps_hgt_obs_var(2) = getGpsHeightVariance();
// innovation gate size
gps_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f);
// fuse height information
fuseVerticalPosition(_gps_pos_innov, gps_hgt_innov_gate,
gps_hgt_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio);
fuseGpsHgt();
} else if (_control_status.flags.rng_hgt) {
Vector2f rng_hgt_innov_gate;
Vector3f rng_hgt_obs_var;
// use range finder with tilt correction
_rng_hgt_innov(2) = _state.pos(2) - (-math::max(_range_sensor.getDistBottom(),
_params.rng_gnd_clearance)) - _hgt_sensor_offset;
// observation variance - user parameter defined
rng_hgt_obs_var(2) = fmaxf(sq(_params.range_noise)
+ sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f);
// innovation gate size
rng_hgt_innov_gate(1) = fmaxf(_params.range_innov_gate, 1.0f);
// fuse height information
fuseVerticalPosition(_rng_hgt_innov, rng_hgt_innov_gate,
rng_hgt_obs_var, _rng_hgt_innov_var, _rng_hgt_test_ratio);
fuseRngHgt();
} else if (_control_status.flags.ev_hgt) {
Vector2f ev_hgt_innov_gate;
Vector3f ev_hgt_obs_var;
// calculate the innovation assuming the external vision observation is in local NED frame
_ev_pos_innov(2) = _state.pos(2) - _ev_sample_delayed.pos(2);
// observation variance - defined externally
ev_hgt_obs_var(2) = fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f));
// innovation gate size
ev_hgt_innov_gate(1) = fmaxf(_params.ev_pos_innov_gate, 1.0f);
// fuse height information
fuseVerticalPosition(_ev_pos_innov, ev_hgt_innov_gate,
ev_hgt_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio);
fuseEvHgt();
}
}
}