diff --git a/src/lib/wind_estimator/WindEstimator.cpp b/src/lib/wind_estimator/WindEstimator.cpp index f1ec03947d..df9c2bb7ad 100644 --- a/src/lib/wind_estimator/WindEstimator.cpp +++ b/src/lib/wind_estimator/WindEstimator.cpp @@ -187,56 +187,12 @@ WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const _time_last_beta_fuse = time_now; - const float v_n = velI(0); - const float v_e = velI(1); - const float v_d = velI(2); - - // compute sideslip observation vector - float HB0 = 2.0f * q_att(0); - float HB1 = HB0 * q_att(3); - float HB2 = 2.0f * q_att(1); - float HB3 = HB2 * q_att(2); - float HB4 = v_e - _state(INDEX_W_E); - float HB5 = HB1 + HB3; - float HB6 = v_n - _state(INDEX_W_N); - float HB7 = q_att(0) * q_att(0); - float HB8 = q_att(3) * q_att(3); - float HB9 = HB7 - HB8; - float HB10 = q_att(1) * q_att(1); - float HB11 = q_att(2) * q_att(2); - float HB12 = HB10 - HB11; - float HB13 = HB12 + HB9; - float HB14 = HB13 * HB6 + HB4 * HB5 + v_d * (-HB0 * q_att(2) + HB2 * q_att(3)); - float HB15 = 1.0f / HB14; - float HB16 = (HB4 * (-HB10 + HB11 + HB9) + HB6 * (-HB1 + HB3) + v_d * (HB0 * q_att(1) + 2.0f * q_att(2) * q_att(3))) / - (HB14 * HB14); matrix::Matrix H_beta; - H_beta(0, 0) = HB13 * HB16 + HB15 * (HB1 - HB3); - H_beta(0, 1) = HB15 * (HB12 - HB7 + HB8) + HB16 * HB5; - H_beta(0, 2) = 0; + matrix::Matrix K; - // compute innovation covariance S - const matrix::Matrix S = H_beta * _P * H_beta.transpose() + _beta_var; - - // compute Kalman gain - matrix::Matrix K = _P * H_beta.transpose(); - K /= S(0, 0); - - // compute predicted side slip angle - matrix::Vector3f rel_wind(velI(0) - _state(INDEX_W_N), velI(1) - _state(INDEX_W_E), velI(2)); - matrix::Dcmf R_body_to_earth(q_att); - rel_wind = R_body_to_earth.transpose() * rel_wind; - - if (fabsf(rel_wind(0)) < 0.1f) { - return; - } - - // use small angle approximation, sin(x) = x for small x - const float beta_pred = rel_wind(1) / rel_wind(0); - - _beta_innov = 0.0f - beta_pred; - _beta_innov_var = S(0, 0); + sym::FuseBeta(velI, _state, _P, q_att, _beta_var, FLT_EPSILON, + &H_beta, &K, &_beta_innov_var, &_beta_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 f168ecbe3f..98e41194ec 100644 --- a/src/lib/wind_estimator/WindEstimator.hpp +++ b/src/lib/wind_estimator/WindEstimator.hpp @@ -44,6 +44,7 @@ #include #include "python/generated/fuse_airspeed.h" +#include "python/generated/fuse_beta.h" using namespace time_literals; diff --git a/src/lib/wind_estimator/python/derivation.py b/src/lib/wind_estimator/python/derivation.py index 4eeea47bdb..cd76a06c17 100644 --- a/src/lib/wind_estimator/python/derivation.py +++ b/src/lib/wind_estimator/python/derivation.py @@ -24,6 +24,56 @@ def fuse_airspeed( return (geo.V3(H), K, innov_var, innov) +# q: quaternion describing rotation from frame 1 to frame 2 +# returns a rotation matrix derived form q which describes the same +# rotation +def quat2Rot(q): + q0 = q[0] + q1 = q[1] + q2 = q[2] + q3 = q[3] + + Rot = geo.M33([[q0**2 + q1**2 - q2**2 - q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)], + [2*(q1*q2 + q0*q3), q0**2 - q1**2 + q2**2 - q3**2, 2*(q2*q3 - q0*q1)], + [2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0**2 - q1**2 - q2**2 + q3**2]]) + + return Rot + +def sign_no_zero(x) -> T.Scalar: + """ + Returns -1 if x is negative, 1 if x is positive, and 1 if x is zero + """ + return 2 * sm.Min(sm.sign(x), 0) + 1 + +def add_epsilon_sign(expr, var, eps): + return expr.subs(var, var + eps * sign_no_zero(var)) + +def fuse_beta( + v_local: geo.V3, + state: geo.V3, + P: geo.M33, + q_att: geo.V4, + R: T.Scalar, + epsilon: T.Scalar +) -> (geo.V3, geo.V3, T.Scalar, T.Scalar): + + vel_rel = geo.V3(v_local[0] - state[0], v_local[1] - state[1], v_local[2]) + relative_wind_body = quat2Rot(q_att).T * vel_rel + + # small angle approximation of side slip model + # protect division by zero using epsilon + beta_pred = add_epsilon_sign(relative_wind_body[1] / relative_wind_body[0], relative_wind_body[0], epsilon) + + innov = 0.0 - beta_pred + + H = geo.V1(beta_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) + + def generatePx4Function(function_name, output_names): from symforce.codegen import Codegen, CppConfig codegen = Codegen.function( @@ -44,6 +94,7 @@ def generatePx4Function(function_name, output_names): 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::min", "math::min") line = line.replace("Eigen", "matrix") line = line.replace("matrix/Dense", "matrix/math.hpp") print(line, end='') @@ -61,3 +112,4 @@ def generatePythonFunction(function_name, output_names): generatePx4Function(fuse_airspeed, output_names=["H", "K", "innov_var", "innov"]) generatePythonFunction(fuse_airspeed, output_names=["H", "K", "innov_var", "innov"]) +generatePx4Function(fuse_beta, output_names=["H", "K", "innov_var", "innov"]) diff --git a/src/lib/wind_estimator/python/generated/fuse_beta.h b/src/lib/wind_estimator/python/generated/fuse_beta.h new file mode 100644 index 0000000000..33fd7a0126 --- /dev/null +++ b/src/lib/wind_estimator/python/generated/fuse_beta.h @@ -0,0 +1,102 @@ +// ----------------------------------------------------------------------------- +// 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_beta + * + * Args: + * v_local: Matrix31 + * state: Matrix31 + * P: Matrix33 + * q_att: Matrix41 + * R: Scalar + * epsilon: Scalar + * + * Outputs: + * H: Matrix13 + * K: Matrix31 + * innov_var: Scalar + * innov: Scalar + */ +template +void FuseBeta(const matrix::Matrix& v_local, const matrix::Matrix& state, + const matrix::Matrix& P, const matrix::Matrix& q_att, + 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: 75 + + // Input arrays + + // Intermediate terms (23) + const Scalar _tmp0 = 2 * v_local(2, 0); + const Scalar _tmp1 = std::pow(q_att(1, 0), Scalar(2)); + const Scalar _tmp2 = std::pow(q_att(2, 0), Scalar(2)); + const Scalar _tmp3 = std::pow(q_att(0, 0), Scalar(2)) - std::pow(q_att(3, 0), Scalar(2)); + const Scalar _tmp4 = _tmp1 - _tmp2 + _tmp3; + const Scalar _tmp5 = -state(0, 0) + v_local(0, 0); + const Scalar _tmp6 = -state(1, 0) + v_local(1, 0); + const Scalar _tmp7 = q_att(0, 0) * q_att(3, 0); + const Scalar _tmp8 = q_att(1, 0) * q_att(2, 0); + const Scalar _tmp9 = 2 * _tmp7 + 2 * _tmp8; + const Scalar _tmp10 = _tmp0 * (-q_att(0, 0) * q_att(2, 0) + q_att(1, 0) * q_att(3, 0)) + + _tmp4 * _tmp5 + _tmp6 * _tmp9; + const Scalar _tmp11 = + _tmp10 + epsilon * (2 * math::min(0, (((_tmp10) > 0) - ((_tmp10) < 0))) + 1); + const Scalar _tmp12 = Scalar(1.0) / (_tmp11); + const Scalar _tmp13 = -2 * _tmp7 + 2 * _tmp8; + const Scalar _tmp14 = -_tmp1 + _tmp2 + _tmp3; + const Scalar _tmp15 = _tmp0 * (q_att(0, 0) * q_att(1, 0) + q_att(2, 0) * q_att(3, 0)) + + _tmp13 * _tmp5 + _tmp14 * _tmp6; + const Scalar _tmp16 = _tmp15 / std::pow(_tmp11, Scalar(2)); + const Scalar _tmp17 = -_tmp12 * _tmp13 + _tmp16 * _tmp4; + const Scalar _tmp18 = -_tmp12 * _tmp14 + _tmp16 * _tmp9; + const Scalar _tmp19 = P(0, 0) * _tmp17; + const Scalar _tmp20 = P(1, 1) * _tmp18; + const Scalar _tmp21 = + R + _tmp17 * (P(1, 0) * _tmp18 + _tmp19) + _tmp18 * (P(0, 1) * _tmp17 + _tmp20); + const Scalar _tmp22 = Scalar(1.0) / (math::max(_tmp21, epsilon)); + + // Output terms (4) + if (H != nullptr) { + matrix::Matrix& _H = (*H); + + _H(0, 0) = _tmp17; + _H(0, 1) = _tmp18; + _H(0, 2) = 0; + } + + if (K != nullptr) { + matrix::Matrix& _K = (*K); + + _K(0, 0) = _tmp22 * (P(0, 1) * _tmp18 + _tmp19); + _K(1, 0) = _tmp22 * (P(1, 0) * _tmp17 + _tmp20); + _K(2, 0) = _tmp22 * (P(2, 0) * _tmp17 + P(2, 1) * _tmp18); + } + + if (innov_var != nullptr) { + Scalar& _innov_var = (*innov_var); + + _innov_var = _tmp21; + } + + if (innov != nullptr) { + Scalar& _innov = (*innov); + + _innov = -_tmp12 * _tmp15; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym