dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: initialize wind covariance using symforce

This commit is contained in:
bresch 2023-10-09 13:53:20 +02:00 committed by Daniel Agar
parent 28d58a947f
commit fc32820e19
4 changed files with 112 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
src/modules/ekf2/EKF/airspeed_fusion.cpp
View File

@ -46,6 +46,7 @@
#include "python/ekf_derivation/generated/compute_airspeed_h_and_k.h"
#include "python/ekf_derivation/generated/compute_airspeed_innov_and_innov_var.h"
#include "python/ekf_derivation/generated/compute_wind_init_and_cov_from_airspeed.h"
#include <mathlib/mathlib.h>
@ -225,46 +226,17 @@ void Ekf::stopAirspeedFusion()
void Ekf::resetWindUsingAirspeed(const airspeedSample &airspeed_sample)
{
const float euler_yaw = getEulerYaw(_R_to_earth);
constexpr float sideslip_var = sq(math::radians(15.0f));
// estimate wind using zero sideslip assumption and airspeed measurement if airspeed available
_state.wind_vel(0) = _state.vel(0) - airspeed_sample.true_airspeed * cosf(euler_yaw);
_state.wind_vel(1) = _state.vel(1) - airspeed_sample.true_airspeed * sinf(euler_yaw);
const float euler_yaw = getEulerYaw(_R_to_earth);
const float airspeed_var = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f));
matrix::SquareMatrix<float, State::wind_vel.dof> P_wind;
sym::ComputeWindInitAndCovFromAirspeed(_state.vel, euler_yaw, airspeed_sample.true_airspeed, getVelocityVariance(), getYawVar(), sideslip_var, airspeed_var, &_state.wind_vel, &P_wind);
resetStateCovariance<State::wind_vel>(P_wind);
ECL_INFO("reset wind using airspeed to (%.3f, %.3f)", (double)_state.wind_vel(0), (double)_state.wind_vel(1));
resetWindCovarianceUsingAirspeed(airspeed_sample);
_aid_src_airspeed.time_last_fuse = _time_delayed_us;
}
void Ekf::resetWindCovarianceUsingAirspeed(const airspeedSample &airspeed_sample)
{
// Derived using EKF/matlab/scripts/Inertial Nav EKF/wind_cov.py
// TODO: explicitly include the sideslip angle in the derivation
const float euler_yaw = getEulerYaw(_R_to_earth);
const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f));
constexpr float initial_sideslip_uncertainty = math::radians(15.0f);
const float initial_wind_var_body_y = sq(airspeed_sample.true_airspeed * sinf(initial_sideslip_uncertainty));
constexpr float R_yaw = sq(math::radians(10.0f));
const float cos_yaw = cosf(euler_yaw);
const float sin_yaw = sinf(euler_yaw);
// rotate wind velocity into earth frame aligned with vehicle yaw
const float Wx = _state.wind_vel(0) * cos_yaw + _state.wind_vel(1) * sin_yaw;
const float Wy = -_state.wind_vel(0) * sin_yaw + _state.wind_vel(1) * cos_yaw;
// it is safer to remove all existing correlations to other states at this time
P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, 0.0f);
P(22, 22) = R_TAS * sq(cos_yaw) + R_yaw * sq(-Wx * sin_yaw - Wy * cos_yaw) + initial_wind_var_body_y * sq(sin_yaw);
P(22, 23) = R_TAS * sin_yaw * cos_yaw + R_yaw * (-Wx * sin_yaw - Wy * cos_yaw) * (Wx * cos_yaw - Wy * sin_yaw) -
initial_wind_var_body_y * sin_yaw * cos_yaw;
P(23, 22) = P(22, 23);
P(23, 23) = R_TAS * sq(sin_yaw) + R_yaw * sq(Wx * cos_yaw - Wy * sin_yaw) + initial_wind_var_body_y * sq(cos_yaw);
// Now add the variance due to uncertainty in vehicle velocity that was used to calculate the initial wind speed
P(22, 22) += P(4, 4);
P(23, 23) += P(5, 5);
}

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

@ -865,9 +865,6 @@ private:
// Reset the wind states using the current airspeed measurement, ground relative nav velocity, yaw angle and assumption of zero sideslip
void resetWindUsingAirspeed(const airspeedSample &airspeed_sample);
// perform a limited reset of the wind state covariances
void resetWindCovarianceUsingAirspeed(const airspeedSample &airspeed_sample);
#endif // CONFIG_EKF2_AIRSPEED
#if defined(CONFIG_EKF2_SIDESLIP)

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

@ -175,6 +175,35 @@ def compute_airspeed_h_and_k(
return (H.T, K)
def compute_wind_init_and_cov_from_airspeed(
v_local: sf.V3,
heading: sf.Scalar,
airspeed: sf.Scalar,
v_var: sf.V3,
heading_var: sf.Scalar,
sideslip_var: sf.Scalar,
airspeed_var: sf.Scalar,
) -> (sf.V2, sf.M22):
# 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])
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
# Assume zero sideslip
P = P.subs({sideslip: 0.0})
wind = wind.subs({sideslip: 0.0})
return (wind, P)
def predict_sideslip(
state: State,
epsilon: sf.Scalar
@ -568,6 +597,7 @@ print("Derive EKF2 equations...")
generate_px4_function(predict_covariance, output_names=["P_new"])
generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"])
generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"])
generate_px4_function(compute_wind_init_and_cov_from_airspeed, output_names=["wind", "P_wind"])
generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"])
generate_px4_function(compute_sideslip_h_and_k, output_names=["H", "K"])
generate_px4_function(compute_mag_innov_innov_var_and_hx, output_names=["innov", "innov_var", "Hx"])

73
src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_wind_init_and_cov_from_airspeed.h Normal file
View File

@ -0,0 +1,73 @@
// -----------------------------------------------------------------------------
// 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_airspeed
*
* Args:
* v_local: Matrix31
* heading: Scalar
* airspeed: Scalar
* v_var: Matrix31
* heading_var: Scalar
* sideslip_var: Scalar
* airspeed_var: Scalar
*
* Outputs:
* wind: Matrix21
* P_wind: Matrix22
*/
template <typename Scalar>
void ComputeWindInitAndCovFromAirspeed(const matrix::Matrix<Scalar, 3, 1>& v_local,
const Scalar heading, const Scalar airspeed,
const matrix::Matrix<Scalar, 3, 1>& v_var,
const Scalar heading_var, const Scalar sideslip_var,
const Scalar airspeed_var,
matrix::Matrix<Scalar, 2, 1>* const wind = nullptr,
matrix::Matrix<Scalar, 2, 2>* const P_wind = nullptr) {
// Total ops: 29
// Input arrays
// Intermediate terms (9)
const Scalar _tmp0 = std::cos(heading);
const Scalar _tmp1 = std::sin(heading);
const Scalar _tmp2 = std::pow(_tmp1, Scalar(2));
const Scalar _tmp3 = std::pow(airspeed, Scalar(2));
const Scalar _tmp4 = _tmp3 * sideslip_var;
const Scalar _tmp5 = _tmp3 * heading_var;
const Scalar _tmp6 = std::pow(_tmp0, Scalar(2));
const Scalar _tmp7 = _tmp0 * _tmp1;
const Scalar _tmp8 = -_tmp4 * _tmp7 - _tmp5 * _tmp7 + _tmp7 * airspeed_var;
// Output terms (2)
if (wind != nullptr) {
matrix::Matrix<Scalar, 2, 1>& _wind = (*wind);
_wind(0, 0) = -_tmp0 * airspeed + v_local(0, 0);
_wind(1, 0) = -_tmp1 * airspeed + v_local(1, 0);
}
if (P_wind != nullptr) {
matrix::Matrix<Scalar, 2, 2>& _p_wind = (*P_wind);
_p_wind(0, 0) = _tmp2 * _tmp4 + _tmp2 * _tmp5 + _tmp6 * airspeed_var + v_var(0, 0);
_p_wind(1, 0) = _tmp8;
_p_wind(0, 1) = _tmp8;
_p_wind(1, 1) = _tmp2 * airspeed_var + _tmp4 * _tmp6 + _tmp5 * _tmp6 + v_var(1, 0);
}
} // NOLINT(readability/fn_size)
// NOLINTNEXTLINE(readability/fn_size)
} // namespace sym