From 29ebef1f74df8234922680d300a0481aa83498ad Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 30 Sep 2022 13:20:10 +0200 Subject: [PATCH] ekf2: migrate fuse_airspeed to SymForce --- src/modules/ekf2/EKF/airspeed_fusion.cpp | 89 +++----------- .../EKF/python/ekf_derivation/derivation.py | 106 ++++++++++++++++ .../python/ekf_derivation/derivation_utils.py | 101 +++++++++++++++ .../generated/compute_airspeed_h_and_k.h | 116 ++++++++++++++++++ .../compute_airspeed_innov_and_innov_var.h | 74 +++++++++++ 5 files changed, 416 insertions(+), 70 deletions(-) create mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/derivation.py create mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py create mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h create mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_innov_and_innov_var.h diff --git a/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/airspeed_fusion.cpp index 414ce4cf11..1101b9801c 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/airspeed_fusion.cpp @@ -43,6 +43,10 @@ */ #include "ekf.h" + +#include "python/ekf_derivation/generated/compute_airspeed_h_and_k.h" +#include "python/ekf_derivation/generated/compute_airspeed_innov_and_innov_var.h" + #include void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source_1d_s &airspeed) const @@ -50,38 +54,17 @@ void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_so // reset flags resetEstimatorAidStatusFlags(airspeed); - const float vn = _state.vel(0); // Velocity in north direction - const float ve = _state.vel(1); // Velocity in east direction - const float vd = _state.vel(2); // Velocity in downwards direction - const float vwn = _state.wind_vel(0); // Wind speed in north direction - const float vwe = _state.wind_vel(1); // Wind speed in east direction - // Variance for true airspeed measurement - (m/sec)^2 - const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * - math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f)); + const float R = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * + math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f)); - // Intermediate variables - const float IV0 = ve - vwe; - const float IV1 = vn - vwn; - const float IV2 = (IV0)*(IV0) + (IV1)*(IV1) + (vd)*(vd); - - const float predicted_airspeed = sqrtf(IV2); - - if (fabsf(predicted_airspeed) < FLT_EPSILON) { - return; - } - - const float IV3 = 1.0F/(IV2); - const float IV4 = IV0*P(5,23); - const float IV5 = IV0*IV3; - const float IV6 = IV1*P(4,22); - const float IV7 = IV1*IV3; - - const float innov_var = IV3*vd*(IV0*P(5,6) - IV0*P(6,23) + IV1*P(4,6) - IV1*P(6,22) + P(6,6)*vd) - IV5*(-IV0*P(23,23) - IV1*P(22,23) + IV1*P(4,23) + IV4 + P(6,23)*vd) + IV5*(IV0*P(5,5) + IV1*P(4,5) - IV1*P(5,22) - IV4 + P(5,6)*vd) - IV7*(-IV0*P(22,23) + IV0*P(5,22) - IV1*P(22,22) + IV6 + P(6,22)*vd) + IV7*(-IV0*P(4,23) + IV0*P(4,5) + IV1*P(4,4) - IV6 + P(4,6)*vd) + R_TAS; + float innov = 0.f; + float innov_var = 0.f; + sym::ComputeAirspeedInnovAndInnovVar(getStateAtFusionHorizonAsVector(), P, airspeed_sample.true_airspeed, R, FLT_EPSILON, &innov, &innov_var); airspeed.observation = airspeed_sample.true_airspeed; - airspeed.observation_variance = R_TAS; - airspeed.innovation = predicted_airspeed - airspeed.observation; + airspeed.observation_variance = R; + airspeed.innovation = innov; airspeed.innovation_variance = innov_var; airspeed.fusion_enabled = _control_status.flags.fuse_aspd; @@ -98,34 +81,9 @@ void Ekf::fuseAirspeed(estimator_aid_source_1d_s &airspeed) return; } - const float vn = _state.vel(0); // Velocity in north direction - const float ve = _state.vel(1); // Velocity in east direction - const float vd = _state.vel(2); // Velocity in downwards direction - const float vwn = _state.wind_vel(0); // Wind speed in north direction - const float vwe = _state.wind_vel(1); // Wind speed in east direction - // determine if we need the airspeed fusion to correct states other than wind const bool update_wind_only = !_control_status.flags.wind_dead_reckoning; - // Intermediate variables - const float HK0 = vn - vwn; - const float HK1 = ve - vwe; - const float HK2 = sqrtf((HK0)*(HK0) + (HK1)*(HK1) + (vd)*(vd)); - - const float predicted_airspeed = HK2; - - if (predicted_airspeed < 1.0f) { - // calculation can be badly conditioned for very low airspeed values so don't fuse this time - return; - } - - const float HK3 = 1.0F/(HK2); - const float HK4 = HK0*HK3; - const float HK5 = HK1*HK3; - const float HK6 = HK3*vd; - const float HK7 = -HK0*HK3; - const float HK8 = -HK1*HK3; - const float innov_var = airspeed.innovation_variance; if (innov_var < airspeed.observation_variance || innov_var < FLT_EPSILON) { @@ -150,31 +108,22 @@ void Ekf::fuseAirspeed(estimator_aid_source_1d_s &airspeed) return; } - const float HK9 = 1.0F/(innov_var); - _fault_status.flags.bad_airspeed = false; - // Observation Jacobians - SparseVector24f<4,5,6,22,23> Hfusion; - Hfusion.at<4>() = HK4; - Hfusion.at<5>() = HK5; - Hfusion.at<6>() = HK6; - Hfusion.at<22>() = HK7; - Hfusion.at<23>() = HK8; + Vector24f H; // Observation jacobian + Vector24f K; // Kalman gain vector - Vector24f Kfusion; // Kalman gain vector + sym::ComputeAirspeedHAndK(getStateAtFusionHorizonAsVector(), P, innov_var, FLT_EPSILON, &H, &K); - if (!update_wind_only) { - // we have no other source of aiding, so use airspeed measurements to correct states + SparseVector24f<4,5,6,22,23> H_sparse(H); + + if (update_wind_only) { for (unsigned row = 0; row <= 21; row++) { - Kfusion(row) = HK9*(HK4*P(row,4) + HK5*P(row,5) + HK6*P(row,6) + HK7*P(row,22) + HK8*P(row,23)); + K(row) = 0.f; } } - Kfusion(22) = HK9*(HK4*P(4,22) + HK5*P(5,22) + HK6*P(6,22) + HK7*P(22,22) + HK8*P(22,23)); - Kfusion(23) = HK9*(HK4*P(4,23) + HK5*P(5,23) + HK6*P(6,23) + HK7*P(22,23) + HK8*P(23,23)); - - const bool is_fused = measurementUpdate(Kfusion, Hfusion, airspeed.innovation); + const bool is_fused = measurementUpdate(K, H_sparse, airspeed.innovation); airspeed.fused = is_fused; _fault_status.flags.bad_airspeed = !is_fused; diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py new file mode 100644 index 0000000000..66bf54f980 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -0,0 +1,106 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" + Copyright (c) 2022 PX4 Development Team + 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: derivation.py +Description: +""" + +import symforce.symbolic as sf +from derivation_utils import * + +class State: + qw = 0 + qx = 1 + qy = 2 + qz = 3 + vx = 4 + vy = 5 + vz = 6 + px = 7 + py = 8 + pz = 9 + d_ang_bx = 10 + d_ang_by = 11 + d_ang_bz = 12 + d_vel_bx = 13 + d_vel_by = 14 + d_vel_bz = 15 + ix = 16 + iy = 17 + iz = 18 + ibx = 19 + iby = 20 + ibz = 21 + wx = 22 + wy = 23 + n_states = 24 + +class VState(sf.Matrix): + SHAPE = (State.n_states, 1) + +class MState(sf.Matrix): + SHAPE = (State.n_states, State.n_states) + +def compute_airspeed_innov_and_innov_var( + state: VState, + P: MState, + airspeed: sf.Scalar, + R: sf.Scalar, + epsilon: sf.Scalar +) -> (sf.Scalar, sf.Scalar): + + vel_rel = sf.V3(state[State.vx] - state[State.wx], state[State.vy] - state[State.wy], state[State.vz]) + airspeed_pred = vel_rel.norm(epsilon=epsilon) + + innov = airspeed_pred - airspeed + + H = sf.V1(airspeed_pred).jacobian(state) + innov_var = (H * P * H.T + R)[0,0] + + return (innov, innov_var) + +def compute_airspeed_h_and_k( + state: VState, + P: MState, + innov_var: sf.Scalar, + epsilon: sf.Scalar +) -> (VState, VState): + + vel_rel = sf.V3(state[State.vx] - state[State.wx], state[State.vy] - state[State.wy], state[State.vz]) + airspeed_pred = vel_rel.norm(epsilon=epsilon) + H = sf.V1(airspeed_pred).jacobian(state) + + K = P * H.T / sm.Max(innov_var, epsilon) + + return (H.T, K) + +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"]) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py new file mode 100644 index 0000000000..68ee60931e --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py @@ -0,0 +1,101 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" + Copyright (c) 2022 PX4 Development Team + 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: derivation_utils.py +Description: + Common functions used for the derivation of most estimators +""" + +from symforce import symbolic as sm +from symforce import geo +from symforce import typing as T + +# q: quaternion describing rotation from frame 1 to frame 2 +# returns a rotation matrix derived form q which describes the same +# rotation +def quat_to_rot(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): + # Avoids a singularity at 0 while keeping the derivative correct + return expr.subs(var, var + eps * sign_no_zero(var)) + +def generate_px4_function(function_name, output_names): + from symforce.codegen import Codegen, CppConfig + import os + import fileinput + + codegen = Codegen.function( + function_name, + output_names=output_names, + 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 + 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='') + +def generate_python_function(function_name, output_names): + from symforce.codegen import Codegen, PythonConfig + codegen = Codegen.function( + function_name, + output_names=output_names, + config=PythonConfig()) + + metadata = codegen.generate_function( + output_dir="generated", + skip_directory_nesting=True) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h new file mode 100644 index 0000000000..0bf15dd0cc --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h @@ -0,0 +1,116 @@ +// ----------------------------------------------------------------------------- +// 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: compute_airspeed_h_and_k + * + * Args: + * state: Matrix24_1 + * P: Matrix24_24 + * innov_var: Scalar + * epsilon: Scalar + * + * Outputs: + * H: Matrix24_1 + * K: Matrix24_1 + */ +template +void ComputeAirspeedHAndK(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar innov_var, + const Scalar epsilon, matrix::Matrix* const H = nullptr, + matrix::Matrix* const K = nullptr) { + // Total ops: 256 + + // Input arrays + + // Intermediate terms (7) + const Scalar _tmp0 = -state(23, 0) + state(5, 0); + const Scalar _tmp1 = -state(22, 0) + state(4, 0); + const Scalar _tmp2 = std::pow(Scalar(std::pow(_tmp0, Scalar(2)) + std::pow(_tmp1, Scalar(2)) + + epsilon + std::pow(state(6, 0), Scalar(2))), + Scalar(Scalar(-1) / Scalar(2))); + const Scalar _tmp3 = _tmp1 * _tmp2; + const Scalar _tmp4 = _tmp0 * _tmp2; + const Scalar _tmp5 = _tmp2 * state(6, 0); + const Scalar _tmp6 = Scalar(1.0) / (math::max(epsilon, innov_var)); + + // Output terms (2) + if (H != nullptr) { + matrix::Matrix& _H = (*H); + + _H.setZero(); + + _H(4, 0) = _tmp3; + _H(5, 0) = _tmp4; + _H(6, 0) = _tmp5; + _H(22, 0) = -_tmp3; + _H(23, 0) = -_tmp4; + } + + if (K != nullptr) { + matrix::Matrix& _K = (*K); + + _K(0, 0) = _tmp6 * (-P(0, 22) * _tmp3 - P(0, 23) * _tmp4 + P(0, 4) * _tmp3 + P(0, 5) * _tmp4 + + P(0, 6) * _tmp5); + _K(1, 0) = _tmp6 * (-P(1, 22) * _tmp3 - P(1, 23) * _tmp4 + P(1, 4) * _tmp3 + P(1, 5) * _tmp4 + + P(1, 6) * _tmp5); + _K(2, 0) = _tmp6 * (-P(2, 22) * _tmp3 - P(2, 23) * _tmp4 + P(2, 4) * _tmp3 + P(2, 5) * _tmp4 + + P(2, 6) * _tmp5); + _K(3, 0) = _tmp6 * (-P(3, 22) * _tmp3 - P(3, 23) * _tmp4 + P(3, 4) * _tmp3 + P(3, 5) * _tmp4 + + P(3, 6) * _tmp5); + _K(4, 0) = _tmp6 * (-P(4, 22) * _tmp3 - P(4, 23) * _tmp4 + P(4, 4) * _tmp3 + P(4, 5) * _tmp4 + + P(4, 6) * _tmp5); + _K(5, 0) = _tmp6 * (-P(5, 22) * _tmp3 - P(5, 23) * _tmp4 + P(5, 4) * _tmp3 + P(5, 5) * _tmp4 + + P(5, 6) * _tmp5); + _K(6, 0) = _tmp6 * (-P(6, 22) * _tmp3 - P(6, 23) * _tmp4 + P(6, 4) * _tmp3 + P(6, 5) * _tmp4 + + P(6, 6) * _tmp5); + _K(7, 0) = _tmp6 * (-P(7, 22) * _tmp3 - P(7, 23) * _tmp4 + P(7, 4) * _tmp3 + P(7, 5) * _tmp4 + + P(7, 6) * _tmp5); + _K(8, 0) = _tmp6 * (-P(8, 22) * _tmp3 - P(8, 23) * _tmp4 + P(8, 4) * _tmp3 + P(8, 5) * _tmp4 + + P(8, 6) * _tmp5); + _K(9, 0) = _tmp6 * (-P(9, 22) * _tmp3 - P(9, 23) * _tmp4 + P(9, 4) * _tmp3 + P(9, 5) * _tmp4 + + P(9, 6) * _tmp5); + _K(10, 0) = _tmp6 * (-P(10, 22) * _tmp3 - P(10, 23) * _tmp4 + P(10, 4) * _tmp3 + + P(10, 5) * _tmp4 + P(10, 6) * _tmp5); + _K(11, 0) = _tmp6 * (-P(11, 22) * _tmp3 - P(11, 23) * _tmp4 + P(11, 4) * _tmp3 + + P(11, 5) * _tmp4 + P(11, 6) * _tmp5); + _K(12, 0) = _tmp6 * (-P(12, 22) * _tmp3 - P(12, 23) * _tmp4 + P(12, 4) * _tmp3 + + P(12, 5) * _tmp4 + P(12, 6) * _tmp5); + _K(13, 0) = _tmp6 * (-P(13, 22) * _tmp3 - P(13, 23) * _tmp4 + P(13, 4) * _tmp3 + + P(13, 5) * _tmp4 + P(13, 6) * _tmp5); + _K(14, 0) = _tmp6 * (-P(14, 22) * _tmp3 - P(14, 23) * _tmp4 + P(14, 4) * _tmp3 + + P(14, 5) * _tmp4 + P(14, 6) * _tmp5); + _K(15, 0) = _tmp6 * (-P(15, 22) * _tmp3 - P(15, 23) * _tmp4 + P(15, 4) * _tmp3 + + P(15, 5) * _tmp4 + P(15, 6) * _tmp5); + _K(16, 0) = _tmp6 * (-P(16, 22) * _tmp3 - P(16, 23) * _tmp4 + P(16, 4) * _tmp3 + + P(16, 5) * _tmp4 + P(16, 6) * _tmp5); + _K(17, 0) = _tmp6 * (-P(17, 22) * _tmp3 - P(17, 23) * _tmp4 + P(17, 4) * _tmp3 + + P(17, 5) * _tmp4 + P(17, 6) * _tmp5); + _K(18, 0) = _tmp6 * (-P(18, 22) * _tmp3 - P(18, 23) * _tmp4 + P(18, 4) * _tmp3 + + P(18, 5) * _tmp4 + P(18, 6) * _tmp5); + _K(19, 0) = _tmp6 * (-P(19, 22) * _tmp3 - P(19, 23) * _tmp4 + P(19, 4) * _tmp3 + + P(19, 5) * _tmp4 + P(19, 6) * _tmp5); + _K(20, 0) = _tmp6 * (-P(20, 22) * _tmp3 - P(20, 23) * _tmp4 + P(20, 4) * _tmp3 + + P(20, 5) * _tmp4 + P(20, 6) * _tmp5); + _K(21, 0) = _tmp6 * (-P(21, 22) * _tmp3 - P(21, 23) * _tmp4 + P(21, 4) * _tmp3 + + P(21, 5) * _tmp4 + P(21, 6) * _tmp5); + _K(22, 0) = _tmp6 * (-P(22, 22) * _tmp3 - P(22, 23) * _tmp4 + P(22, 4) * _tmp3 + + P(22, 5) * _tmp4 + P(22, 6) * _tmp5); + _K(23, 0) = _tmp6 * (-P(23, 22) * _tmp3 - P(23, 23) * _tmp4 + P(23, 4) * _tmp3 + + P(23, 5) * _tmp4 + P(23, 6) * _tmp5); + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_innov_and_innov_var.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_innov_and_innov_var.h new file mode 100644 index 0000000000..4add44e02c --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_innov_and_innov_var.h @@ -0,0 +1,74 @@ +// ----------------------------------------------------------------------------- +// 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: compute_airspeed_innov_and_innov_var + * + * Args: + * state: Matrix24_1 + * P: Matrix24_24 + * airspeed: Scalar + * R: Scalar + * epsilon: Scalar + * + * Outputs: + * innov: Scalar + * innov_var: Scalar + */ +template +void ComputeAirspeedInnovAndInnovVar(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar airspeed, + const Scalar R, const Scalar epsilon, + Scalar* const innov = nullptr, + Scalar* const innov_var = nullptr) { + // Total ops: 69 + + // Input arrays + + // Intermediate terms (7) + const Scalar _tmp0 = -state(23, 0) + state(5, 0); + const Scalar _tmp1 = -state(22, 0) + state(4, 0); + const Scalar _tmp2 = std::sqrt(Scalar(std::pow(_tmp0, Scalar(2)) + std::pow(_tmp1, Scalar(2)) + + epsilon + std::pow(state(6, 0), Scalar(2)))); + const Scalar _tmp3 = Scalar(1.0) / (_tmp2); + const Scalar _tmp4 = _tmp3 * state(6, 0); + const Scalar _tmp5 = _tmp1 * _tmp3; + const Scalar _tmp6 = _tmp0 * _tmp3; + + // Output terms (2) + if (innov != nullptr) { + Scalar& _innov = (*innov); + + _innov = _tmp2 - airspeed; + } + + if (innov_var != nullptr) { + Scalar& _innov_var = (*innov_var); + + _innov_var = R + + _tmp4 * (-P(22, 6) * _tmp5 - P(23, 6) * _tmp6 + P(4, 6) * _tmp5 + P(5, 6) * _tmp6 + + P(6, 6) * _tmp4) - + _tmp5 * (-P(22, 22) * _tmp5 - P(23, 22) * _tmp6 + P(4, 22) * _tmp5 + + P(5, 22) * _tmp6 + P(6, 22) * _tmp4) + + _tmp5 * (-P(22, 4) * _tmp5 - P(23, 4) * _tmp6 + P(4, 4) * _tmp5 + P(5, 4) * _tmp6 + + P(6, 4) * _tmp4) - + _tmp6 * (-P(22, 23) * _tmp5 - P(23, 23) * _tmp6 + P(4, 23) * _tmp5 + + P(5, 23) * _tmp6 + P(6, 23) * _tmp4) + + _tmp6 * (-P(22, 5) * _tmp5 - P(23, 5) * _tmp6 + P(4, 5) * _tmp5 + P(5, 5) * _tmp6 + + P(6, 5) * _tmp4); + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym