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

adjustments for wind fusion

Browse Source
This commit is contained in:
Marco Hauswirth 2024-07-11 09:44:19 +02:00
parent 16cf77da38
commit 503dc210ee
9 changed files with 32 additions and 128 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp
View File

@ -127,7 +127,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
if (_control_status.flags.inertial_dead_reckoning && !is_airspeed_consistent) {
resetVelUsingAirspeed(airspeed_sample);
} else if (!_control_status.flags.wind || getWindVelocityVariance().longerThan(_params.initial_wind_uncertainty)) {
} else if (!_control_status.flags.wind) {
// If starting wind state estimation, reset the wind states and covariances before fusing any data
// Also catch the case where sideslip fusion enabled wind estimation recently and didn't converge yet.
resetWindUsingAirspeed(airspeed_sample);

4
src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp
View File

@ -57,6 +57,10 @@ void Ekf::controlDragFusion(const imuSample &imu_delayed)
if (_drag_buffer->pop_first_older_than(imu_delayed.time_us, &drag_sample)) {
fuseDrag(drag_sample);
if (!_aid_src_drag.fused && !_control_status.flags.fuse_aspd && !_control_status.flags.fuse_beta) {
resetWind();
}
}
}
}

17
src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp
View File

@ -64,16 +64,10 @@ void Ekf::controlBetaFusion(const imuSample &imu_delayed)
updateSideslip(_aid_src_sideslip);
_innov_check_fail_status.flags.reject_sideslip = _aid_src_sideslip.innovation_rejected;
// If starting wind state estimation, reset the wind states and covariances before fusing any data
if (!_control_status.flags.wind) {
// activate the wind states
_control_status.flags.wind = true;
// reset the timeout timers to prevent repeated resets
_aid_src_sideslip.time_last_fuse = imu_delayed.time_us;
if (!fuseSideslip(_aid_src_sideslip) && !_control_status.flags.wind) {
resetWindToZero();
}
fuseSideslip(_aid_src_sideslip);
_control_status.flags.wind = true;
}
}
}
@ -96,10 +90,10 @@ void Ekf::updateSideslip(estimator_aid_source1d_s &aid_src) const
math::max(_params.beta_innov_gate, 1.f)); // innovation gate
}
void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
bool Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
{
if (sideslip.innovation_rejected) {
return;
return false;
}
// determine if we need the sideslip fusion to correct states other than wind
@ -125,7 +119,7 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
ECL_ERR("sideslip badly conditioned - %s covariance reset", action_string);
return;
return false;
}
_fault_status.flags.bad_sideslip = false;
@ -149,4 +143,5 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
if (is_fused) {
sideslip.time_last_fuse = _time_delayed_us;
}
return is_fused;
}

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

@ -195,15 +195,14 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
#if defined(CONFIG_EKF2_WIND)
if (_control_status.flags.wind) {
// wind vel: add process noise
float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f, 1.f) * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf));
float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt;
// wind vel: add process noise
float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f,
1.f) * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf));
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;
P(i, i) += wind_vel_process_noise;
}
for (unsigned index = 0; index < State::wind_vel.dof; index++) {
const unsigned i = State::wind_vel.idx + index;
P(i, i) = fminf(P(i, i) + wind_vel_process_noise, _params.initial_wind_uncertainty);
}
#endif // CONFIG_EKF2_WIND

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

@ -772,7 +772,7 @@ private:
// fuse synthetic zero sideslip measurement
void updateSideslip(estimator_aid_source1d_s &_aid_src_sideslip) const;
void fuseSideslip(estimator_aid_source1d_s &_aid_src_sideslip);
bool fuseSideslip(estimator_aid_source1d_s &_aid_src_sideslip);
#endif // CONFIG_EKF2_SIDESLIP
#if defined(CONFIG_EKF2_DRAG_FUSION)
@ -798,8 +798,6 @@ private:
void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var);
void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const float pos_var = NAN) { resetHorizontalPositionTo(new_horz_pos, Vector2f(pos_var, pos_var)); }
void resetWindTo(const Vector2f &wind, const Vector2f &wind_var);
bool isHeightResetRequired() const;
void resetVerticalPositionTo(float new_vert_pos, float new_vert_pos_var = NAN);

30
src/modules/ekf2/EKF/python/ekf_derivation/derivation.py
View File

@ -298,38 +298,15 @@ def compute_wind_init_and_cov_from_airspeed(
# Initialise wind states assuming horizontal flight
sideslip = sf.Symbol("beta")
wind = sf.V2(v_local[0] - airspeed * sf.cos(heading + sideslip), v_local[1] - airspeed * sf.sin(heading + sideslip))
J = wind.jacobian([v_local[0], v_local[1], heading, sideslip, airspeed])
H = wind.jacobian([v_local[0], v_local[1], heading, sideslip, airspeed])
R = sf.M55()
R[0,0] = v_var[0]
R[1,1] = v_var[1]
R[2,2] = heading_var
R[3,3] = sideslip_var
R[4,4] = airspeed_var
P = J * R * J.T
P = sf.Matrix.diag([v_var[0], v_var[1], heading_var, sideslip_var, airspeed_var])
P = H * P * H.T
# Assume zero sideslip
P = P.subs({sideslip: 0.0})
wind = wind.subs({sideslip: 0.0})
return (wind, P)
def compute_wind_init_and_cov_from_wind_speed_and_direction(
wind_speed: sf.Scalar,
wind_direction: sf.Scalar,
wind_speed_var: sf.Scalar,
wind_direction_var: sf.Scalar
)-> (sf.V2, sf.M22):
wind = sf.V2(wind_speed * sf.cos(wind_direction), wind_speed * sf.sin(wind_direction))
J = wind.jacobian([wind_speed, wind_direction])
R = sf.M22()
R[0,0] = wind_speed_var
R[1,1] = wind_direction_var
P = J * R * J.T
return (wind, P)
def predict_sideslip(
state: State,
@ -704,7 +681,6 @@ if not args.disable_wind:
generate_px4_function(compute_sideslip_h_and_k, output_names=["H", "K"])
generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"])
generate_px4_function(compute_wind_init_and_cov_from_airspeed, output_names=["wind", "P_wind"])
generate_px4_function(compute_wind_init_and_cov_from_wind_speed_and_direction, output_names=["wind", "P_wind"])
generate_px4_function(compute_yaw_innov_var_and_h, output_names=["innov_var", "H"])
generate_px4_function(compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"])

65
src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_wind_init_and_cov_from_wind_speed_and_direction.h
View File

@ -1,65 +0,0 @@
// -----------------------------------------------------------------------------
// This file was autogenerated by symforce from template:
// function/FUNCTION.h.jinja
// Do NOT modify by hand.
// -----------------------------------------------------------------------------
#pragma once
#include <matrix/math.hpp>
namespace sym {
/**
* This function was autogenerated from a symbolic function. Do not modify by hand.
*
* Symbolic function: compute_wind_init_and_cov_from_wind_speed_and_direction
*
* Args:
* wind_speed: Scalar
* wind_direction: Scalar
* wind_speed_var: Scalar
* wind_direction_var: Scalar
*
* Outputs:
* wind: Matrix21
* P_wind: Matrix22
*/
template <typename Scalar>
void ComputeWindInitAndCovFromWindSpeedAndDirection(
const Scalar wind_speed, const Scalar wind_direction, const Scalar wind_speed_var,
const Scalar wind_direction_var, matrix::Matrix<Scalar, 2, 1>* const wind = nullptr,
matrix::Matrix<Scalar, 2, 2>* const P_wind = nullptr) {
// Total ops: 18
// Input arrays
// Intermediate terms (7)
const Scalar _tmp0 = std::cos(wind_direction);
const Scalar _tmp1 = std::sin(wind_direction);
const Scalar _tmp2 = std::pow(_tmp0, Scalar(2));
const Scalar _tmp3 = std::pow(_tmp1, Scalar(2));
const Scalar _tmp4 = wind_direction_var * std::pow(wind_speed, Scalar(2));
const Scalar _tmp5 = _tmp0 * _tmp1;
const Scalar _tmp6 = -_tmp4 * _tmp5 + _tmp5 * wind_speed_var;
// Output terms (2)
if (wind != nullptr) {
matrix::Matrix<Scalar, 2, 1>& _wind = (*wind);
_wind(0, 0) = _tmp0 * wind_speed;
_wind(1, 0) = _tmp1 * wind_speed;
}
if (P_wind != nullptr) {
matrix::Matrix<Scalar, 2, 2>& _p_wind = (*P_wind);
_p_wind(0, 0) = _tmp2 * wind_speed_var + _tmp3 * _tmp4;
_p_wind(1, 0) = _tmp6;
_p_wind(0, 1) = _tmp6;
_p_wind(1, 1) = _tmp2 * _tmp4 + _tmp3 * wind_speed_var;
}
} // NOLINT(readability/fn_size)
// NOLINTNEXTLINE(readability/fn_size)
} // namespace sym

21
src/modules/ekf2/EKF/wind.cpp
View File

@ -37,7 +37,6 @@
*/
#include "ekf.h"
#include <ekf_derivation/generated/compute_wind_init_and_cov_from_wind_speed_and_direction.h>
void Ekf::resetWindToExternalObservation(float wind_speed, float wind_direction, float wind_speed_accuracy, float wind_direction_accuracy)
{
@ -45,21 +44,19 @@ void Ekf::resetWindToExternalObservation(float wind_speed, float wind_direction,
// wind direction is given as azimuth where wind blows FROM, we need direction where wind blows TO
const float wind_direction_rad = wrap_pi(math::radians(wind_direction) + M_PI_F);
const float wind_direction_var = sq(math::radians(wind_direction_accuracy));
const float wind_speed_var = sq(wind_speed_accuracy);
matrix::SquareMatrix<float, 2> P_wind;
Vector2f wind;
sym::ComputeWindInitAndCovFromWindSpeedAndDirection(wind_speed_constrained, wind_direction_rad, wind_speed_var, wind_direction_var, &wind, &P_wind);
const Vector2f wind_var = P_wind.diag();
Vector2f wind = wind_speed_constrained * Vector2f(cosf(wind_direction_rad), sinf(wind_direction_rad));
ECL_INFO("reset wind states to external observation");
_information_events.flags.reset_wind_to_ext_obs = true;
resetWindTo(wind, wind_var);
Vector2f innov = _state.wind_vel - wind;
float R = sq(0.1f);
Vector2f innov_var{P(State::wind_vel.idx, State::wind_vel.idx) + R, P(State::wind_vel.idx + 1, State::wind_vel.idx + 1) + R};
const bool control_status_wind_prev = _control_status.flags.wind;
_control_status.flags.wind = true;
fuseDirectStateMeasurement(innov(0), innov_var(0), R, State::wind_vel.idx);
fuseDirectStateMeasurement(innov(1), innov_var(1), R, State::wind_vel.idx + 1);
_control_status.flags.wind = control_status_wind_prev;
// reset the horizontal velocity variances to allow the velocity states to be pulled towards
// a solution that is aligned with the newly set wind estimates

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

@ -536,7 +536,7 @@ void EKF2::Run()
}
if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE) {
if (_ekf.control_status_flags().wind_dead_reckoning) {
if (_ekf.control_status_flags().wind_dead_reckoning || !_ekf.control_status_flags().in_air) {
_ekf.resetWindToExternalObservation(vehicle_command.param1, vehicle_command.param3, vehicle_command.param2,
vehicle_command.param4);
}