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

Added support for resetting wind states to external observation (#23277)

Browse Source 
* added support for resetting wind states to external observation

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* moved wind related functions into separate file

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* added VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* correctly compute variances

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* ekf2: implement wind reset

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* only allow VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE during wind dead-reckoning
and increase horizontal velocity variance to allow velocity states to move
towards solution that is aligned with the newly set wind

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* only reset wind on ground

* still use wind reset using airspeed when it wasn't initialized

* exclude func for rover, change reset interface

* handle wind reset in drag-fusion

* replace state reset with variance reset in sideslip/drag fusion

* remove resetWind function

---------

Signed-off-by: RomanBapst <bapstroman@gmail.com>
Co-authored-by: Marco Hauswirth <marco.hauswirth@auterion.com>
This commit is contained in:
Roman Bapst 2024-07-19 14:33:08 +02:00 committed by GitHub
parent ca9948a84d
commit 1b9f1b78e5
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
14 changed files with 231 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/VehicleCommand.msg
View File

@ -102,6 +102,7 @@ uint16 VEHICLE_CMD_FIXED_MAG_CAL_YAW = 42006 # Magnetometer calibrati
uint16 VEHICLE_CMD_DO_WINCH = 42600 # Command to operate winch.
uint16 VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE = 43003 # external reset of estimator global position when deadreckoning
uint16 VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE = 43004
# PX4 vehicle commands (beyond 16 bit mavlink commands)
uint32 VEHICLE_CMD_PX4_INTERNAL_START = 65537 # start of PX4 internal only vehicle commands (> UINT16_MAX)

4
src/modules/ekf2/CMakeLists.txt
View File

@ -214,6 +214,10 @@ if(CONFIG_EKF2_TERRAIN)
list(APPEND EKF_SRCS EKF/terrain_control.cpp)
endif()
if(CONFIG_EKF2_WIND)
list(APPEND EKF_SRCS EKF/wind.cpp)
endif ()
add_subdirectory(EKF)
px4_add_module(

4
src/modules/ekf2/EKF/CMakeLists.txt
View File

@ -138,6 +138,10 @@ if(CONFIG_EKF2_TERRAIN)
list(APPEND EKF_SRCS terrain_control.cpp)
endif()
if(CONFIG_EKF2_WIND)
list(APPEND EKF_SRCS wind.cpp)
endif ()
include_directories(${CMAKE_CURRENT_SOURCE_DIR})
add_library(ecl_EKF
${EKF_SRCS}

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

@ -62,6 +62,10 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
_control_status.flags.wind = false;
}
if (_control_status.flags.wind && _external_wind_init) {
_external_wind_init = false;
}
#if defined(CONFIG_EKF2_GNSS)
// clear yaw estimator airspeed (updated later with true airspeed if airspeed fusion is active)
@ -128,10 +132,11 @@ 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)) {
// 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.
} else if (!_external_wind_init
&& (!_control_status.flags.wind
|| getWindVelocityVariance().longerThan(sq(_params.initial_wind_uncertainty)))) {
resetWindUsingAirspeed(airspeed_sample);
_aid_src_airspeed.time_last_fuse = _time_delayed_us;
}
_control_status.flags.wind = true;

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

@ -48,9 +48,11 @@ void Ekf::controlDragFusion(const imuSample &imu_delayed)
if ((_params.drag_ctrl > 0) && _drag_buffer) {
if (!_control_status.flags.wind && !_control_status.flags.fake_pos && _control_status.flags.in_air) {
// reset the wind states and covariances when starting drag accel fusion
_control_status.flags.wind = true;
resetWindToZero();
if (!_external_wind_init) {
resetWindCov();
}
}
dragSample drag_sample;

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

@ -64,16 +64,12 @@ 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
if (fuseSideslip(_aid_src_sideslip)) {
_control_status.flags.wind = true;
// reset the timeout timers to prevent repeated resets
_aid_src_sideslip.time_last_fuse = imu_delayed.time_us;
resetWindToZero();
}
fuseSideslip(_aid_src_sideslip);
} else if (!_external_wind_init && !_control_status.flags.wind) {
resetWindCov();
}
}
}
}
@ -96,10 +92,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
@ -114,7 +110,7 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
const char *action_string = nullptr;
if (update_wind_only) {
resetWind();
resetWindCov();
action_string = "wind";
} else {
@ -125,7 +121,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;
@ -150,4 +146,6 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
if (is_fused) {
sideslip.time_last_fuse = _time_delayed_us;
}
return is_fused;
}

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

@ -640,6 +640,7 @@ bool yaw_aligned_to_imu_gps :
bool reset_hgt_to_ev : 1; ///< 16 - true when the vertical position state is reset to the ev measurement
bool reset_pos_to_ext_obs :
1; ///< 17 - true when horizontal position was reset to an external observation while deadreckoning
bool reset_wind_to_ext_obs : 1; ///< 18 - true when wind states were reset to an external observation
} flags;
uint32_t value;
};

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

@ -200,15 +200,15 @@ 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));
// wind vel: add process noise
if (!_external_wind_init) {
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;
P(i, i) = fminf(P(i, i) + wind_vel_process_noise, sq(_params.initial_wind_uncertainty));
}
}
@ -348,11 +348,3 @@ void Ekf::resetMagCov()
P.uncorrelateCovarianceSetVariance<State::mag_B.dof>(State::mag_B.idx, sq(_params.mag_noise));
}
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_WIND)
void Ekf::resetWindCov()
{
// start with a small initial uncertainty to improve the initial estimate
P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, sq(_params.initial_wind_uncertainty));
}
#endif // CONFIG_EKF2_WIND

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

@ -526,6 +526,18 @@ public:
bool resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy,
uint64_t timestamp_observation);
/**
* @brief Resets the wind states to an external observation
*
* @param wind_speed The wind speed in m/s
* @param wind_direction The azimuth (from true north) to where the wind is heading in radians
* @param wind_speed_accuracy The 1 sigma accuracy of the wind speed estimate in m/s
* @param wind_direction_accuracy The 1 sigma accuracy of the wind direction estimate in radians
*/
void resetWindToExternalObservation(float wind_speed, float wind_direction, float wind_speed_accuracy,
float wind_direction_accuracy);
bool _external_wind_init{false};
void updateParameters();
friend class AuxGlobalPosition;
@ -782,7 +794,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)
@ -808,6 +820,8 @@ 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);
@ -1095,8 +1109,6 @@ private:
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_WIND)
// perform a reset of the wind states and related covariances
void resetWind();
void resetWindCov();
void resetWindToZero();
#endif // CONFIG_EKF2_WIND

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

@ -864,32 +864,6 @@ void Ekf::updateGroundEffect()
}
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_WIND)
void Ekf::resetWind()
{
#if defined(CONFIG_EKF2_AIRSPEED)
if (_control_status.flags.fuse_aspd && isRecent(_airspeed_sample_delayed.time_us, 1e6)) {
resetWindUsingAirspeed(_airspeed_sample_delayed);
return;
}
#endif // CONFIG_EKF2_AIRSPEED
resetWindToZero();
}
void Ekf::resetWindToZero()
{
ECL_INFO("reset wind to zero");
// If we don't have an airspeed measurement, then assume the wind is zero
_state.wind_vel.setZero();
resetWindCov();
}
#endif // CONFIG_EKF2_WIND
void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
{

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

@ -314,6 +314,20 @@ def compute_wind_init_and_cov_from_airspeed(
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.V2):
wind = sf.V2(wind_speed * sf.cos(wind_direction), wind_speed * sf.sin(wind_direction))
H = wind.jacobian([wind_speed, wind_direction])
R = sf.Matrix.diag([wind_speed_var, wind_direction_var])
P = H * R * H.T
P_diag = sf.V2(P[0,0], P[1,1])
return (wind, P_diag)
def predict_sideslip(
state: State,
epsilon: sf.Scalar
@ -721,6 +735,7 @@ 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"])

61
src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_wind_init_and_cov_from_wind_speed_and_direction.h Normal file
View File

@ -0,0 +1,61 @@
// -----------------------------------------------------------------------------
// 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: Matrix21
*/
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, 1>* const P_wind = nullptr) {
// Total ops: 14
// Input arrays
// Intermediate terms (5)
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));
// 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, 1>& _p_wind = (*P_wind);
_p_wind(0, 0) = _tmp2 * wind_speed_var + _tmp3 * _tmp4;
_p_wind(1, 0) = _tmp2 * _tmp4 + _tmp3 * wind_speed_var;
}
} // NOLINT(readability/fn_size)
// NOLINTNEXTLINE(readability/fn_size)
} // namespace sym

92
src/modules/ekf2/EKF/wind.cpp Normal file
View File

@ -0,0 +1,92 @@
/****************************************************************************
*
* Copyright (c) 2024 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file wind.cpp
* Helper functions for wind states
*/
#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)
{
if (!_control_status.flags.in_air) {
const float wind_speed_constrained = math::max(wind_speed, 0.0f);
const float wind_direction_var = sq(wind_direction_accuracy);
const float wind_speed_var = sq(wind_speed_accuracy);
Vector2f wind;
Vector2f wind_var;
sym::ComputeWindInitAndCovFromWindSpeedAndDirection(wind_speed_constrained, wind_direction, wind_speed_var,
wind_direction_var, &wind, &wind_var);
ECL_INFO("reset wind states to external observation");
_information_events.flags.reset_wind_to_ext_obs = true;
_external_wind_init = true;
resetWindTo(wind, wind_var);
}
}
void Ekf::resetWindTo(const Vector2f &wind, const Vector2f &wind_var)
{
_state.wind_vel = wind;
if (PX4_ISFINITE(wind_var(0))) {
P.uncorrelateCovarianceSetVariance<1>(State::wind_vel.idx,
math::min(sq(_params.initial_wind_uncertainty), wind_var(0)));
}
if (PX4_ISFINITE(wind_var(1))) {
P.uncorrelateCovarianceSetVariance<1>(State::wind_vel.idx + 1,
math::min(sq(_params.initial_wind_uncertainty), wind_var(1)));
}
}
void Ekf::resetWindCov()
{
// start with a small initial uncertainty to improve the initial estimate
P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, sq(_params.initial_wind_uncertainty));
}
void Ekf::resetWindToZero()
{
ECL_INFO("reset wind to zero");
_state.wind_vel.setZero();
resetWindCov();
}

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

@ -561,6 +561,17 @@ void EKF2::Run()
command_ack.timestamp = hrt_absolute_time();
_vehicle_command_ack_pub.publish(command_ack);
}
if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE) {
#if defined(CONFIG_EKF2_WIND)
// wind direction is given as azimuth where wind blows FROM
// PX4 backend expects direction where wind blows TO
const float wind_direction_rad = wrap_pi(math::radians(vehicle_command.param3) + M_PI_F);
const float wind_direction_accuracy_rad = math::radians(vehicle_command.param4);
_ekf.resetWindToExternalObservation(vehicle_command.param1, wind_direction_rad, vehicle_command.param2,
wind_direction_accuracy_rad);
#endif // CONFIG_EKF2_WIND
}
}
}