diff --git a/Tools/setup/requirements.txt b/Tools/setup/requirements.txt index e84c676d7e..61a0e970a9 100644 --- a/Tools/setup/requirements.txt +++ b/Tools/setup/requirements.txt @@ -26,3 +26,5 @@ requests setuptools>=39.2.0 six>=1.12.0 toml>=0.9 +symforce>=0.5.0 +sympy>=1.10.1 diff --git a/src/lib/wind_estimator/WindEstimator.cpp b/src/lib/wind_estimator/WindEstimator.cpp index 9891233b77..1118cb0d34 100644 --- a/src/lib/wind_estimator/WindEstimator.cpp +++ b/src/lib/wind_estimator/WindEstimator.cpp @@ -141,37 +141,11 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const _time_last_airspeed_fuse = time_now; - // assign helper variables - const float v_n = velI(0); - const float v_e = velI(1); - const float v_d = velI(2); - - // calculate airspeed from ground speed and wind states (without scale) - const float airspeed_predicted_raw = sqrtf((v_n - _state(INDEX_W_N)) * (v_n - _state(INDEX_W_N)) + - (v_e - _state(INDEX_W_E)) * (v_e - _state(INDEX_W_E)) + v_d * v_d); - - // compute state observation matrix H - const float HH0 = airspeed_predicted_raw; - const float HH1 = _state(INDEX_TAS_SCALE) / math::max(HH0, 0.1f); - matrix::Matrix H_tas; - H_tas(0, 0) = HH1 * (-v_n + _state(INDEX_W_N)); - H_tas(0, 1) = HH1 * (-v_e + _state(INDEX_W_E)); - H_tas(0, 2) = HH0; + matrix::Matrix K; - // compute innovation covariance S - const matrix::Matrix S = H_tas * _P * H_tas.transpose() + _tas_var; - - // compute Kalman gain - matrix::Matrix K = _P * H_tas.transpose(); - K /= S(0, 0); - - // compute innovation - const float airspeed_pred = _state(INDEX_TAS_SCALE) * airspeed_predicted_raw; - _tas_innov = true_airspeed - airspeed_pred; - - // innovation variance - _tas_innov_var = S(0, 0); + sym::FuseAirspeed(velI, _state, _P, true_airspeed, _tas_var, FLT_EPSILON, + &H_tas, &K, &_tas_innov_var, &_tas_innov); bool reinit_filter = false; bool meas_is_rejected = false; diff --git a/src/lib/wind_estimator/WindEstimator.hpp b/src/lib/wind_estimator/WindEstimator.hpp index 434d8404a6..b1ba8b1ea7 100644 --- a/src/lib/wind_estimator/WindEstimator.hpp +++ b/src/lib/wind_estimator/WindEstimator.hpp @@ -41,6 +41,8 @@ #include #include +#include "python/generated/fuse_airspeed.h" + using namespace time_literals; class WindEstimator diff --git a/src/lib/wind_estimator/python/derivation.py b/src/lib/wind_estimator/python/derivation.py new file mode 100644 index 0000000000..643cf2a1a0 --- /dev/null +++ b/src/lib/wind_estimator/python/derivation.py @@ -0,0 +1,50 @@ +import os +from symforce import symbolic as sm +from symforce import geo +from symforce import typing as T + +def fuse_airspeed( + v_local: geo.V3, + state: geo.V3, + P: geo.M33, + airspeed: T.Scalar, + R: T.Scalar, + epsilon: T.Scalar +) -> geo.V3: + + vel_rel = geo.V3(v_local[0] - state[0], v_local[1] - state[1], v_local[2]) + airspeed_pred = vel_rel.norm(epsilon=epsilon) * state[2] + + innov = airspeed - airspeed_pred + + H = geo.V1(airspeed_pred).jacobian(state) + innov_var = (H * P * H.transpose() + R)[0,0] + + K = P * H.transpose() / sm.Max(innov_var, epsilon) + + return (geo.V3(H), K, innov_var, innov) + +from symforce.codegen import Codegen, CppConfig + +codegen = Codegen.function( + fuse_airspeed, + output_names=["H", "K", "innov_var", "innov"], + config=CppConfig()) +metadata = codegen.generate_function( + output_dir="generated", + skip_directory_nesting=True) + +print("Files generated in {}:\n".format(metadata.output_dir)) +for f in metadata.generated_files: + print(" |- {}".format(os.path.relpath(f, metadata.output_dir))) + +# Replace cstdlib and Eigen functions by PX4 equivalents +import fileinput + +with fileinput.FileInput(os.path.abspath(metadata.generated_files[0]), inplace=True, backup='.bak') as file: + for line in file: + line = line.replace("std::max", "math::max") + line = line.replace("std", "matrix") + line = line.replace("Eigen", "matrix") + line = line.replace("matrix/Dense", "matrix/math.hpp") + print(line, end='') diff --git a/src/lib/wind_estimator/python/generated/fuse_airspeed.h b/src/lib/wind_estimator/python/generated/fuse_airspeed.h new file mode 100644 index 0000000000..89babbe870 --- /dev/null +++ b/src/lib/wind_estimator/python/generated/fuse_airspeed.h @@ -0,0 +1,90 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// backends/cpp/templates/function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: fuse_airspeed + * + * Args: + * v_local: Matrix31 + * state: Matrix31 + * P: Matrix33 + * airspeed: Scalar + * R: Scalar + * epsilon: Scalar + * + * Outputs: + * H: Matrix13 + * K: Matrix31 + * innov_var: Scalar + * innov: Scalar + */ +template +void FuseAirspeed(const matrix::Matrix& v_local, + const matrix::Matrix& state, const matrix::Matrix& P, + const Scalar airspeed, const Scalar R, const Scalar epsilon, + matrix::Matrix* const H = nullptr, + matrix::Matrix* const K = nullptr, Scalar* const innov_var = nullptr, + Scalar* const innov = nullptr) { + // Total ops: 56 + + // Input arrays + + // Intermediate terms (11) + const Scalar _tmp0 = -state(0, 0) + v_local(0, 0); + const Scalar _tmp1 = -state(1, 0) + v_local(1, 0); + const Scalar _tmp2 = matrix::sqrt(Scalar(matrix::pow(_tmp0, Scalar(2)) + matrix::pow(_tmp1, Scalar(2)) + + epsilon + matrix::pow(v_local(2, 0), Scalar(2)))); + const Scalar _tmp3 = state(2, 0) / _tmp2; + const Scalar _tmp4 = _tmp0 * _tmp3; + const Scalar _tmp5 = _tmp1 * _tmp3; + const Scalar _tmp6 = -P(0, 0) * _tmp4; + const Scalar _tmp7 = -P(1, 1) * _tmp5; + const Scalar _tmp8 = P(2, 2) * _tmp2; + const Scalar _tmp9 = R + _tmp2 * (-P(0, 2) * _tmp4 - P(1, 2) * _tmp5 + _tmp8) - + _tmp4 * (-P(1, 0) * _tmp5 + P(2, 0) * _tmp2 + _tmp6) - + _tmp5 * (-P(0, 1) * _tmp4 + P(2, 1) * _tmp2 + _tmp7); + const Scalar _tmp10 = Scalar(1.0) / (math::max(_tmp9, epsilon)); + + // Output terms (4) + if (H != nullptr) { + matrix::Matrix& _H = (*H); + + _H(0, 0) = -_tmp4; + _H(0, 1) = -_tmp5; + _H(0, 2) = _tmp2; + } + + if (K != nullptr) { + matrix::Matrix& _K = (*K); + + _K(0, 0) = _tmp10 * (-P(0, 1) * _tmp5 + P(0, 2) * _tmp2 + _tmp6); + _K(1, 0) = _tmp10 * (-P(1, 0) * _tmp4 + P(1, 2) * _tmp2 + _tmp7); + _K(2, 0) = _tmp10 * (-P(2, 0) * _tmp4 - P(2, 1) * _tmp5 + _tmp8); + } + + if (innov_var != nullptr) { + Scalar& _innov_var = (*innov_var); + + _innov_var = _tmp9; + } + + if (innov != nullptr) { + Scalar& _innov = (*innov); + + _innov = -_tmp2 * state(2, 0) + airspeed; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym