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

ekf2: move baro compensation to delayed time horizon and add validity check

Browse Source 
- this removes an unnecessary virtual call and simplifies things a bit

Co-authored-by: bresch <brescianimathieu@gmail.com>
This commit is contained in:
Daniel Agar 2022-10-20 09:17:55 -04:00 committed by GitHub
parent b400b7fcc4
commit fb3adc3faa
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 27 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/ekf2/EKF/baro_height_control.cpp
View File

@ -51,7 +51,7 @@ void Ekf::controlBaroHeightFusion()
if (_baro_buffer && _baro_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &baro_sample)) {
const float measurement = baro_sample.hgt;
const float measurement = compensateBaroForDynamicPressure(baro_sample.hgt);
const float measurement_var = sq(_params.baro_noise);
const float innov_gate = fmaxf(_params.baro_innov_gate, 1.f);
@ -60,10 +60,10 @@ void Ekf::controlBaroHeightFusion()
if (measurement_valid) {
if (_baro_counter == 0) {
_baro_lpf.reset(baro_sample.hgt);
_baro_lpf.reset(measurement);
} else {
_baro_lpf.update(baro_sample.hgt);
_baro_lpf.update(measurement);
}
if (_baro_counter <= _obs_buffer_length) {

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

@ -864,7 +864,7 @@ private:
// and a scalar innovation value
void fuse(const Vector24f &K, float innovation);
float compensateBaroForDynamicPressure(float baro_alt_uncompensated) const override;
float compensateBaroForDynamicPressure(float baro_alt_uncompensated) const;
// calculate the earth rotation vector from a given latitude
Vector3f calcEarthRateNED(float lat_rad) const;

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

@ -511,31 +511,36 @@ void Ekf::constrainStates()
float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const
{
// calculate static pressure error = Pmeas - Ptruth
// model position error sensitivity as a body fixed ellipse with a different scale in the positive and
// negative X and Y directions. Used to correct baro data for positional errors
const matrix::Dcmf R_to_body(_output_new.quat_nominal.inversed());
if (_control_status.flags.wind && local_position_is_valid()) {
// calculate static pressure error = Pmeas - Ptruth
// model position error sensitivity as a body fixed ellipse with a different scale in the positive and
// negative X and Y directions. Used to correct baro data for positional errors
// Calculate airspeed in body frame
const Vector3f velocity_earth = _output_new.vel - _vel_imu_rel_body_ned;
// Calculate airspeed in body frame
const Vector3f vel_imu_rel_body_ned = _R_to_earth * (_ang_rate_delayed_raw % _params.imu_pos_body);
const Vector3f velocity_earth = _state.vel - vel_imu_rel_body_ned;
const Vector3f wind_velocity_earth(_state.wind_vel(0), _state.wind_vel(1), 0.0f);
const Vector3f wind_velocity_earth(_state.wind_vel(0), _state.wind_vel(1), 0.0f);
const Vector3f airspeed_earth = velocity_earth - wind_velocity_earth;
const Vector3f airspeed_earth = velocity_earth - wind_velocity_earth;
const Vector3f airspeed_body = R_to_body * airspeed_earth;
const Vector3f airspeed_body = _state.quat_nominal.rotateVectorInverse(airspeed_earth);
const Vector3f K_pstatic_coef(airspeed_body(0) >= 0.0f ? _params.static_pressure_coef_xp :
_params.static_pressure_coef_xn,
airspeed_body(1) >= 0.0f ? _params.static_pressure_coef_yp : _params.static_pressure_coef_yn,
_params.static_pressure_coef_z);
const Vector3f K_pstatic_coef(
airspeed_body(0) >= 0.f ? _params.static_pressure_coef_xp : _params.static_pressure_coef_xn,
airspeed_body(1) >= 0.f ? _params.static_pressure_coef_yp : _params.static_pressure_coef_yn,
_params.static_pressure_coef_z);
const Vector3f airspeed_squared = matrix::min(airspeed_body.emult(airspeed_body), sq(_params.max_correction_airspeed));
const Vector3f airspeed_squared = matrix::min(airspeed_body.emult(airspeed_body), sq(_params.max_correction_airspeed));
const float pstatic_err = 0.5f * _air_density * (airspeed_squared.dot(K_pstatic_coef));
const float pstatic_err = 0.5f * _air_density * (airspeed_squared.dot(K_pstatic_coef));
// correct baro measurement using pressure error estimate and assuming sea level gravity
return baro_alt_uncompensated + pstatic_err / (_air_density * CONSTANTS_ONE_G);
// correct baro measurement using pressure error estimate and assuming sea level gravity
return baro_alt_uncompensated + pstatic_err / (_air_density * CONSTANTS_ONE_G);
}
// otherwise return the uncorrected baro measurement
return baro_alt_uncompensated;
}
// calculate the earth rotation vector

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

@ -221,7 +221,7 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample)
baroSample baro_sample_new;
baro_sample_new.time_us = time_us;
baro_sample_new.hgt = compensateBaroForDynamicPressure(baro_sample.hgt);
baro_sample_new.hgt = baro_sample.hgt;
_baro_buffer->push(baro_sample_new);
_time_last_baro_buffer_push = _newest_high_rate_imu_sample.time_us;

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

@ -396,8 +396,6 @@ protected:
// this is the previous status of the filter control modes - used to detect mode transitions
filter_control_status_u _control_status_prev{};
virtual float compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const = 0;
// these are used to record single frame events for external monitoring and should NOT be used for
// state logic becasue they will be cleared externally after being read.
warning_event_status_u _warning_events{};