Skip to content

Commit

Permalink
ekf2: move height rate lpf update where used
Browse files Browse the repository at this point in the history
This also saves a bit of flash for board not using the WIND config
  • Loading branch information
bresch committed Dec 17, 2024
1 parent 3ea4e50 commit 5c39bea
Show file tree
Hide file tree
Showing 3 changed files with 5 additions and 5 deletions.
6 changes: 3 additions & 3 deletions src/modules/ekf2/EKF/covariance.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -202,9 +202,9 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
#if defined(CONFIG_EKF2_WIND)

// wind vel: add process noise
float wind_vel_nsd_scaled = _params.wind_vel_nsd * (1.f + _params.wind_vel_nsd_scaler * fabsf(
_height_rate_lpf.getState()));
float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt;
const float height_rate = _height_rate_lpf.update(_state.vel(2), imu_delayed.delta_vel_dt);
const float wind_vel_nsd_scaled = _params.wind_vel_nsd * (1.f + _params.wind_vel_nsd_scaler * fabsf(height_rate));
const float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt;

for (unsigned index = 0; index < State::wind_vel.dof; index++) {
const unsigned i = State::wind_vel.idx + index;
Expand Down
2 changes: 0 additions & 2 deletions src/modules/ekf2/EKF/ekf.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -274,8 +274,6 @@ void Ekf::predictState(const imuSample &imu_delayed)

// calculate a filtered horizontal acceleration this are used for manoeuvre detection elsewhere
_accel_horiz_lpf.update(corrected_delta_vel_ef.xy() / imu_delayed.delta_vel_dt, imu_delayed.delta_vel_dt);

_height_rate_lpf.update(_state.vel(2), imu_delayed.delta_vel_dt);
}

bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const double longitude, const float altitude,
Expand Down
2 changes: 2 additions & 0 deletions src/modules/ekf2/EKF/ekf.h
Original file line number Diff line number Diff line change
Expand Up @@ -493,8 +493,10 @@ class Ekf final : public EstimatorInterface
static constexpr float _accel_horiz_lpf_time_constant = 1.f;
AlphaFilter<Vector2f> _accel_horiz_lpf{_accel_horiz_lpf_time_constant}; ///< Low pass filtered horizontal earth frame acceleration (m/sec**2)

#if defined(CONFIG_EKF2_WIND)
static constexpr float _height_rate_lpf_time_constant = 10.f;
AlphaFilter<float> _height_rate_lpf{_height_rate_lpf_time_constant};
#endif // CONFIG_EKF2_WIND

SquareMatrixState P{}; ///< state covariance matrix

Expand Down

0 comments on commit 5c39bea

Please sign in to comment.