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

wind_est: correctly include sideslip in initialization model

This commit is contained in:
bresch 2023-10-09 15:37:02 +02:00 committed by Daniel Agar
parent fc32820e19
commit 5e986f6997
2 changed files with 20 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/lib/wind_estimator/python/derivation.py
View File

@ -96,10 +96,10 @@ def init_wind_using_airspeed(
airspeed_var: sf.Scalar,
) -> (sf.V2, sf.M22):
# Initialise wind states assuming zero side slip and horizontal flight
wind = sf.V2(v_local[0] - airspeed * sm.cos(heading), v_local[1] - airspeed * sm.sin(heading))
# Zero sideslip, propagate the sideslip variance using partial derivatives w.r.t heading
J = wind.jacobian([v_local[0], v_local[1], heading, heading, airspeed])
# Initialise wind states assuming horizontal flight
sideslip = sm.Symbol("beta")
wind = sf.V2(v_local[0] - airspeed * sm.cos(heading + sideslip), v_local[1] - airspeed * sm.sin(heading + sideslip))
J = wind.jacobian([v_local[0], v_local[1], heading, sideslip, airspeed])
R = sf.M55()
R[0,0] = v_var
@ -110,6 +110,10 @@ def init_wind_using_airspeed(
P = J * R * J.T
# Assume zero sideslip
P = P.subs({sideslip: 0.0})
wind = wind.subs({sideslip: 0.0})
return (wind, P)
generate_px4_function(fuse_airspeed, output_names=["H", "K", "innov_var", "innov"])

25
src/lib/wind_estimator/python/generated/init_wind_using_airspeed.h
View File

@ -34,21 +34,20 @@ void InitWindUsingAirspeed(const matrix::Matrix<Scalar, 3, 1>& v_local, const Sc
const Scalar sideslip_var, const Scalar airspeed_var,
matrix::Matrix<Scalar, 2, 1>* const wind = nullptr,
matrix::Matrix<Scalar, 2, 2>* const P = nullptr) {
// Total ops: 22
// Unused inputs
(void)heading_var;
// Total ops: 29
// Input arrays
// Intermediate terms (7)
// 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)) * sideslip_var;
const Scalar _tmp4 = std::pow(_tmp0, Scalar(2));
const Scalar _tmp5 = _tmp0 * _tmp1;
const Scalar _tmp6 = -_tmp3 * _tmp5 + _tmp5 * airspeed_var;
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) {
@ -61,10 +60,10 @@ void InitWindUsingAirspeed(const matrix::Matrix<Scalar, 3, 1>& v_local, const Sc
if (P != nullptr) {
matrix::Matrix<Scalar, 2, 2>& _p = (*P);
_p(0, 0) = _tmp2 * _tmp3 + _tmp4 * airspeed_var + v_var;
_p(1, 0) = _tmp6;
_p(0, 1) = _tmp6;
_p(1, 1) = _tmp2 * airspeed_var + _tmp3 * _tmp4 + v_var;
_p(0, 0) = _tmp2 * _tmp4 + _tmp2 * _tmp5 + _tmp6 * airspeed_var + v_var;
_p(1, 0) = _tmp8;
_p(0, 1) = _tmp8;
_p(1, 1) = _tmp2 * airspeed_var + _tmp4 * _tmp6 + _tmp5 * _tmp6 + v_var;
}
} // NOLINT(readability/fn_size)