diff --git a/src/modules/ekf2/EKF/drag_fusion.cpp b/src/modules/ekf2/EKF/drag_fusion.cpp index 9ab27510bd..48ff631526 100644 --- a/src/modules/ekf2/EKF/drag_fusion.cpp +++ b/src/modules/ekf2/EKF/drag_fusion.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 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. * @@ -34,27 +34,22 @@ /** * @file drag_fusion.cpp * Body frame drag fusion methods used for multi-rotor wind estimation. - * equations generated using EKF/python/ekf_derivation/main.py - * - * @author Paul Riseborough - * */ #include "ekf.h" +#include "python/ekf_derivation/generated/compute_drag_x_innov_var_and_k.h" +#include "python/ekf_derivation/generated/compute_drag_y_innov_var_and_k.h" #include void Ekf::fuseDrag(const dragSample &drag_sample) { - SparseVector24f<0,1,2,3,4,5,6,22,23> Hfusion; // Observation Jacobians - Vector24f Kfusion; // Kalman gain vector - const float R_ACC = fmaxf(_params.drag_noise, 0.5f); // observation noise variance in specific force drag (m/sec**2)**2 const float rho = fmaxf(_air_density, 0.1f); // air density (kg/m**3) // correct rotor momentum drag for increase in required rotor mass flow with altitude // obtained from momentum disc theory - const float mcoef_corrrected = _params.mcoef * sqrtf(rho / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + const float mcoef_corrrected = fmaxf(_params.mcoef * sqrtf(rho / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C), 0.f); // drag model parameters const bool using_bcoef_x = _params.bcoef_x > 1.0f; @@ -65,26 +60,16 @@ void Ekf::fuseDrag(const dragSample &drag_sample) return; } - // get latest estimated orientation - const float q0 = _state.quat_nominal(0); - const float q1 = _state.quat_nominal(1); - const float q2 = _state.quat_nominal(2); - const float q3 = _state.quat_nominal(3); - - // get latest velocity in earth frame - const float vn = _state.vel(0); - const float ve = _state.vel(1); - const float vd = _state.vel(2); - - // get latest wind velocity in earth frame - const float vwn = _state.wind_vel(0); - const float vwe = _state.wind_vel(1); - // predicted specific forces // calculate relative wind velocity in earth frame and rotate into body frame - const Vector3f rel_wind_earth(vn - vwn, ve - vwe, vd); + const Vector3f rel_wind_earth(_state.vel(0) - _state.wind_vel(0), + _state.vel(1) - _state.wind_vel(1), + _state.vel(2)); const Vector3f rel_wind_body = _state.quat_nominal.rotateVectorInverse(rel_wind_earth); const float rel_wind_speed = rel_wind_body.norm(); + const Vector24f state_vector_prev = getStateAtFusionHorizonAsVector(); + + Vector24f Kfusion; // perform sequential fusion of XY specific forces for (uint8_t axis_index = 0; axis_index < 2; axis_index++) { @@ -94,236 +79,30 @@ void Ekf::fuseDrag(const dragSample &drag_sample) // Drag is modelled as an arbitrary combination of bluff body drag that proportional to // equivalent airspeed squared, and rotor momentum drag that is proportional to true airspeed // parallel to the rotor disc and mass flow through the rotor disc. - float pred_acc = 0.0f; // predicted drag acceleration + float bcoef_inv = 0.f; if (axis_index == 0) { - float Kacc; // Derivative of specific force wrt airspeed - - if (using_mcoef && using_bcoef_x) { - // Use a combination of bluff body and propeller momentum drag - const float bcoef_inv = 1.0f / _params.bcoef_x; - // The airspeed used for linearisation is calculated from the measured acceleration by solving the following quadratic - // mea_acc = 0.5 * rho * bcoef_inv * airspeed**2 + mcoef_corrrected * airspeed - // TODO: this should be modified as well and in the derivation: mea_acc = 0.5 * rho * bcoef_inv * airspeed * airspeed_norm + mcoef_corrrected * airspeed - const float airspeed = (_params.bcoef_x / rho) * (- mcoef_corrrected + sqrtf(sq(mcoef_corrrected) + 2.0f * rho * bcoef_inv * fabsf(mea_acc))); - Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed + mcoef_corrrected); - pred_acc = -0.5f * bcoef_inv * rho * rel_wind_body(0) * rel_wind_speed - rel_wind_body(0) * mcoef_corrrected; - - } else if (using_mcoef) { - // Use propeller momentum drag only - Kacc = fmaxf(1e-1f, mcoef_corrrected); - pred_acc = -rel_wind_body(0) * mcoef_corrrected; - - } else if (using_bcoef_x) { - // Use bluff body drag only - // The airspeed used for linearisation is calculated from the measured acceleration by solving the following quadratic - // mea_acc = (0.5 * rho / _params.bcoef_x) * airspeed**2 - const float airspeed = sqrtf((2.0f * _params.bcoef_x * fabsf(mea_acc)) / rho); - const float bcoef_inv = 1.0f / _params.bcoef_x; - Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed); - pred_acc = -0.5f * bcoef_inv * rho * rel_wind_body(0) * rel_wind_speed; - - } else { - // skip this axis - continue; + if (using_bcoef_x) { + bcoef_inv = 1.0f / _params.bcoef_x; } - // intermediate variables - const float HK0 = vn - vwn; - const float HK1 = ve - vwe; - const float HK2 = HK0*q0 + HK1*q3 - q2*vd; - const float HK3 = 2*Kacc; - const float HK4 = HK0*q1 + HK1*q2 + q3*vd; - const float HK5 = HK0*q2 - HK1*q1 + q0*vd; - const float HK6 = -HK0*q3 + HK1*q0 + q1*vd; - const float HK7 = ecl::powf(q0, 2) + ecl::powf(q1, 2) - ecl::powf(q2, 2) - ecl::powf(q3, 2); - const float HK8 = HK7*Kacc; - const float HK9 = q0*q3 + q1*q2; - const float HK10 = HK3*HK9; - const float HK11 = q0*q2 - q1*q3; - const float HK12 = 2*HK9; - const float HK13 = 2*HK11; - const float HK14 = 2*HK4; - const float HK15 = 2*HK2; - const float HK16 = 2*HK5; - const float HK17 = 2*HK6; - const float HK18 = -HK12*P(0,23) + HK12*P(0,5) - HK13*P(0,6) + HK14*P(0,1) + HK15*P(0,0) - HK16*P(0,2) + HK17*P(0,3) - HK7*P(0,22) + HK7*P(0,4); - const float HK19 = HK12*P(5,23); - const float HK20 = -HK12*P(23,23) - HK13*P(6,23) + HK14*P(1,23) + HK15*P(0,23) - HK16*P(2,23) + HK17*P(3,23) + HK19 - HK7*P(22,23) + HK7*P(4,23); - const float HK21 = ecl::powf(Kacc, 2); - const float HK22 = HK12*HK21; - const float HK23 = HK12*P(5,5) - HK13*P(5,6) + HK14*P(1,5) + HK15*P(0,5) - HK16*P(2,5) + HK17*P(3,5) - HK19 + HK7*P(4,5) - HK7*P(5,22); - const float HK24 = HK12*P(5,6) - HK12*P(6,23) - HK13*P(6,6) + HK14*P(1,6) + HK15*P(0,6) - HK16*P(2,6) + HK17*P(3,6) + HK7*P(4,6) - HK7*P(6,22); - const float HK25 = HK7*P(4,22); - const float HK26 = -HK12*P(4,23) + HK12*P(4,5) - HK13*P(4,6) + HK14*P(1,4) + HK15*P(0,4) - HK16*P(2,4) + HK17*P(3,4) - HK25 + HK7*P(4,4); - const float HK27 = HK21*HK7; - const float HK28 = -HK12*P(22,23) + HK12*P(5,22) - HK13*P(6,22) + HK14*P(1,22) + HK15*P(0,22) - HK16*P(2,22) + HK17*P(3,22) + HK25 - HK7*P(22,22); - const float HK29 = -HK12*P(1,23) + HK12*P(1,5) - HK13*P(1,6) + HK14*P(1,1) + HK15*P(0,1) - HK16*P(1,2) + HK17*P(1,3) - HK7*P(1,22) + HK7*P(1,4); - const float HK30 = -HK12*P(2,23) + HK12*P(2,5) - HK13*P(2,6) + HK14*P(1,2) + HK15*P(0,2) - HK16*P(2,2) + HK17*P(2,3) - HK7*P(2,22) + HK7*P(2,4); - const float HK31 = -HK12*P(3,23) + HK12*P(3,5) - HK13*P(3,6) + HK14*P(1,3) + HK15*P(0,3) - HK16*P(2,3) + HK17*P(3,3) - HK7*P(3,22) + HK7*P(3,4); - //const float HK32 = Kacc/(-HK13*HK21*HK24 + HK14*HK21*HK29 + HK15*HK18*HK21 - HK16*HK21*HK30 + HK17*HK21*HK31 - HK20*HK22 + HK22*HK23 + HK26*HK27 - HK27*HK28 + R_ACC); - - // calculate innovation variance and exit if badly conditioned - _drag_innov_var(0) = (-HK13*HK21*HK24 + HK14*HK21*HK29 + HK15*HK18*HK21 - HK16*HK21*HK30 + HK17*HK21*HK31 - HK20*HK22 + HK22*HK23 + HK26*HK27 - HK27*HK28 + R_ACC); - if (_drag_innov_var(0) < R_ACC) { - return; - } - - const float HK32 = Kacc / _drag_innov_var(0); - - // Observation Jacobians - Hfusion.at<0>() = -HK2*HK3; - Hfusion.at<1>() = -HK3*HK4; - Hfusion.at<2>() = HK3*HK5; - Hfusion.at<3>() = -HK3*HK6; - Hfusion.at<4>() = -HK8; - Hfusion.at<5>() = -HK10; - Hfusion.at<6>() = HK11*HK3; - Hfusion.at<22>() = HK8; - Hfusion.at<23>() = HK10; - - // Kalman gains - // Don't allow modification of any states other than wind velocity at this stage of development - we only need a wind estimate. - // Kfusion(0) = -HK18*HK32; - // Kfusion(1) = -HK29*HK32; - // Kfusion(2) = -HK30*HK32; - // Kfusion(3) = -HK31*HK32; - // Kfusion(4) = -HK26*HK32; - // Kfusion(5) = -HK23*HK32; - // Kfusion(6) = -HK24*HK32; - // Kfusion(7) = -HK32*(HK12*P(5,7) - HK12*P(7,23) - HK13*P(6,7) + HK14*P(1,7) + HK15*P(0,7) - HK16*P(2,7) + HK17*P(3,7) + HK7*P(4,7) - HK7*P(7,22)); - // Kfusion(8) = -HK32*(HK12*P(5,8) - HK12*P(8,23) - HK13*P(6,8) + HK14*P(1,8) + HK15*P(0,8) - HK16*P(2,8) + HK17*P(3,8) + HK7*P(4,8) - HK7*P(8,22)); - // Kfusion(9) = -HK32*(HK12*P(5,9) - HK12*P(9,23) - HK13*P(6,9) + HK14*P(1,9) + HK15*P(0,9) - HK16*P(2,9) + HK17*P(3,9) + HK7*P(4,9) - HK7*P(9,22)); - // Kfusion(10) = -HK32*(-HK12*P(10,23) + HK12*P(5,10) - HK13*P(6,10) + HK14*P(1,10) + HK15*P(0,10) - HK16*P(2,10) + HK17*P(3,10) - HK7*P(10,22) + HK7*P(4,10)); - // Kfusion(11) = -HK32*(-HK12*P(11,23) + HK12*P(5,11) - HK13*P(6,11) + HK14*P(1,11) + HK15*P(0,11) - HK16*P(2,11) + HK17*P(3,11) - HK7*P(11,22) + HK7*P(4,11)); - // Kfusion(12) = -HK32*(-HK12*P(12,23) + HK12*P(5,12) - HK13*P(6,12) + HK14*P(1,12) + HK15*P(0,12) - HK16*P(2,12) + HK17*P(3,12) - HK7*P(12,22) + HK7*P(4,12)); - // Kfusion(13) = -HK32*(-HK12*P(13,23) + HK12*P(5,13) - HK13*P(6,13) + HK14*P(1,13) + HK15*P(0,13) - HK16*P(2,13) + HK17*P(3,13) - HK7*P(13,22) + HK7*P(4,13)); - // Kfusion(14) = -HK32*(-HK12*P(14,23) + HK12*P(5,14) - HK13*P(6,14) + HK14*P(1,14) + HK15*P(0,14) - HK16*P(2,14) + HK17*P(3,14) - HK7*P(14,22) + HK7*P(4,14)); - // Kfusion(15) = -HK32*(-HK12*P(15,23) + HK12*P(5,15) - HK13*P(6,15) + HK14*P(1,15) + HK15*P(0,15) - HK16*P(2,15) + HK17*P(3,15) - HK7*P(15,22) + HK7*P(4,15)); - // Kfusion(16) = -HK32*(-HK12*P(16,23) + HK12*P(5,16) - HK13*P(6,16) + HK14*P(1,16) + HK15*P(0,16) - HK16*P(2,16) + HK17*P(3,16) - HK7*P(16,22) + HK7*P(4,16)); - // Kfusion(17) = -HK32*(-HK12*P(17,23) + HK12*P(5,17) - HK13*P(6,17) + HK14*P(1,17) + HK15*P(0,17) - HK16*P(2,17) + HK17*P(3,17) - HK7*P(17,22) + HK7*P(4,17)); - // Kfusion(18) = -HK32*(-HK12*P(18,23) + HK12*P(5,18) - HK13*P(6,18) + HK14*P(1,18) + HK15*P(0,18) - HK16*P(2,18) + HK17*P(3,18) - HK7*P(18,22) + HK7*P(4,18)); - // Kfusion(19) = -HK32*(-HK12*P(19,23) + HK12*P(5,19) - HK13*P(6,19) + HK14*P(1,19) + HK15*P(0,19) - HK16*P(2,19) + HK17*P(3,19) - HK7*P(19,22) + HK7*P(4,19)); - // Kfusion(20) = -HK32*(-HK12*P(20,23) + HK12*P(5,20) - HK13*P(6,20) + HK14*P(1,20) + HK15*P(0,20) - HK16*P(2,20) + HK17*P(3,20) - HK7*P(20,22) + HK7*P(4,20)); - // Kfusion(21) = -HK32*(-HK12*P(21,23) + HK12*P(5,21) - HK13*P(6,21) + HK14*P(1,21) + HK15*P(0,21) - HK16*P(2,21) + HK17*P(3,21) - HK7*P(21,22) + HK7*P(4,21)); - Kfusion(22) = -HK28*HK32; - Kfusion(23) = -HK20*HK32; - + sym::ComputeDragXInnovVarAndK(state_vector_prev, P, rho, bcoef_inv, mcoef_corrrected, R_ACC, FLT_EPSILON, &_drag_innov_var(0), &Kfusion); } else if (axis_index == 1) { - float Kacc; // Derivative of specific force wrt airspeed - - if (using_mcoef && using_bcoef_y) { - // Use a combination of bluff body and propeller momentum drag - const float bcoef_inv = 1.0f / _params.bcoef_y; - // The airspeed used for linearisation is calculated from the measured acceleration by solving the following quadratic - // mea_acc = 0.5 * rho * bcoef_inv * airspeed**2 + mcoef_corrrected * airspeed - const float airspeed = (_params.bcoef_y / rho) * (- mcoef_corrrected + sqrtf(sq(mcoef_corrrected) + 2.0f * rho * bcoef_inv * fabsf(mea_acc))); - Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed + mcoef_corrrected); - pred_acc = -0.5f * bcoef_inv * rho * rel_wind_body(1) * rel_wind_speed - rel_wind_body(1) * mcoef_corrrected; - - } else if (using_mcoef) { - // Use propeller momentum drag only - Kacc = fmaxf(1e-1f, mcoef_corrrected); - pred_acc = -rel_wind_body(1) * mcoef_corrrected; - - } else if (using_bcoef_y) { - // Use bluff body drag only - // The airspeed used for linearisation is calculated from the measured acceleration by solving the following quadratic - // mea_acc = (0.5 * rho / _params.bcoef_y) * airspeed**2 - const float airspeed = sqrtf((2.0f * _params.bcoef_y * fabsf(mea_acc)) / rho); - const float bcoef_inv = 1.0f / _params.bcoef_y; - Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed); - pred_acc = -0.5f * bcoef_inv * rho * rel_wind_body(1) * rel_wind_speed; - - } else { - // nothing more to do - return; + if (using_bcoef_y) { + bcoef_inv = 1.0f / _params.bcoef_y; } - // intermediate variables - const float HK0 = ve - vwe; - const float HK1 = vn - vwn; - const float HK2 = HK0*q0 - HK1*q3 + q1*vd; - const float HK3 = 2*Kacc; - const float HK4 = -HK0*q1 + HK1*q2 + q0*vd; - const float HK5 = HK0*q2 + HK1*q1 + q3*vd; - const float HK6 = HK0*q3 + HK1*q0 - q2*vd; - const float HK7 = q0*q3 - q1*q2; - const float HK8 = HK3*HK7; - const float HK9 = ecl::powf(q0, 2) - ecl::powf(q1, 2) + ecl::powf(q2, 2) - ecl::powf(q3, 2); - const float HK10 = HK9*Kacc; - const float HK11 = q0*q1 + q2*q3; - const float HK12 = 2*HK11; - const float HK13 = 2*HK7; - const float HK14 = 2*HK5; - const float HK15 = 2*HK2; - const float HK16 = 2*HK4; - const float HK17 = 2*HK6; - const float HK18 = HK12*P(0,6) + HK13*P(0,22) - HK13*P(0,4) + HK14*P(0,2) + HK15*P(0,0) + HK16*P(0,1) - HK17*P(0,3) - HK9*P(0,23) + HK9*P(0,5); - const float HK19 = ecl::powf(Kacc, 2); - const float HK20 = HK12*P(6,6) - HK13*P(4,6) + HK13*P(6,22) + HK14*P(2,6) + HK15*P(0,6) + HK16*P(1,6) - HK17*P(3,6) + HK9*P(5,6) - HK9*P(6,23); - const float HK21 = HK13*P(4,22); - const float HK22 = HK12*P(6,22) + HK13*P(22,22) + HK14*P(2,22) + HK15*P(0,22) + HK16*P(1,22) - HK17*P(3,22) - HK21 - HK9*P(22,23) + HK9*P(5,22); - const float HK23 = HK13*HK19; - const float HK24 = HK12*P(4,6) - HK13*P(4,4) + HK14*P(2,4) + HK15*P(0,4) + HK16*P(1,4) - HK17*P(3,4) + HK21 - HK9*P(4,23) + HK9*P(4,5); - const float HK25 = HK9*P(5,23); - const float HK26 = HK12*P(5,6) - HK13*P(4,5) + HK13*P(5,22) + HK14*P(2,5) + HK15*P(0,5) + HK16*P(1,5) - HK17*P(3,5) - HK25 + HK9*P(5,5); - const float HK27 = HK19*HK9; - const float HK28 = HK12*P(6,23) + HK13*P(22,23) - HK13*P(4,23) + HK14*P(2,23) + HK15*P(0,23) + HK16*P(1,23) - HK17*P(3,23) + HK25 - HK9*P(23,23); - const float HK29 = HK12*P(2,6) + HK13*P(2,22) - HK13*P(2,4) + HK14*P(2,2) + HK15*P(0,2) + HK16*P(1,2) - HK17*P(2,3) - HK9*P(2,23) + HK9*P(2,5); - const float HK30 = HK12*P(1,6) + HK13*P(1,22) - HK13*P(1,4) + HK14*P(1,2) + HK15*P(0,1) + HK16*P(1,1) - HK17*P(1,3) - HK9*P(1,23) + HK9*P(1,5); - const float HK31 = HK12*P(3,6) + HK13*P(3,22) - HK13*P(3,4) + HK14*P(2,3) + HK15*P(0,3) + HK16*P(1,3) - HK17*P(3,3) - HK9*P(3,23) + HK9*P(3,5); - // const float HK32 = Kacc/(HK12*HK19*HK20 + HK14*HK19*HK29 + HK15*HK18*HK19 + HK16*HK19*HK30 - HK17*HK19*HK31 + HK22*HK23 - HK23*HK24 + HK26*HK27 - HK27*HK28 + R_ACC); - - _drag_innov_var(1) = (HK12*HK19*HK20 + HK14*HK19*HK29 + HK15*HK18*HK19 + HK16*HK19*HK30 - HK17*HK19*HK31 + HK22*HK23 - HK23*HK24 + HK26*HK27 - HK27*HK28 + R_ACC); - if (_drag_innov_var(1) < R_ACC) { - // calculation is badly conditioned - return; - } - - const float HK32 = Kacc / _drag_innov_var(1); - - // Observation Jacobians - Hfusion.at<0>() = -HK2*HK3; - Hfusion.at<1>() = -HK3*HK4; - Hfusion.at<2>() = -HK3*HK5; - Hfusion.at<3>() = HK3*HK6; - Hfusion.at<4>() = HK8; - Hfusion.at<5>() = -HK10; - Hfusion.at<6>() = -HK11*HK3; - Hfusion.at<22>() = -HK8; - Hfusion.at<23>() = HK10; - - // Kalman gains - // Don't allow modification of any states other than wind velocity at this stage of development - we only need a wind estimate. - // Kfusion(0) = -HK18*HK32; - // Kfusion(1) = -HK30*HK32; - // Kfusion(2) = -HK29*HK32; - // Kfusion(3) = -HK31*HK32; - // Kfusion(4) = -HK24*HK32; - // Kfusion(5) = -HK26*HK32; - // Kfusion(6) = -HK20*HK32; - // Kfusion(7) = -HK32*(HK12*P(6,7) - HK13*P(4,7) + HK13*P(7,22) + HK14*P(2,7) + HK15*P(0,7) + HK16*P(1,7) - HK17*P(3,7) + HK9*P(5,7) - HK9*P(7,23)); - // Kfusion(8) = -HK32*(HK12*P(6,8) - HK13*P(4,8) + HK13*P(8,22) + HK14*P(2,8) + HK15*P(0,8) + HK16*P(1,8) - HK17*P(3,8) + HK9*P(5,8) - HK9*P(8,23)); - // Kfusion(9) = -HK32*(HK12*P(6,9) - HK13*P(4,9) + HK13*P(9,22) + HK14*P(2,9) + HK15*P(0,9) + HK16*P(1,9) - HK17*P(3,9) + HK9*P(5,9) - HK9*P(9,23)); - // Kfusion(10) = -HK32*(HK12*P(6,10) + HK13*P(10,22) - HK13*P(4,10) + HK14*P(2,10) + HK15*P(0,10) + HK16*P(1,10) - HK17*P(3,10) - HK9*P(10,23) + HK9*P(5,10)); - // Kfusion(11) = -HK32*(HK12*P(6,11) + HK13*P(11,22) - HK13*P(4,11) + HK14*P(2,11) + HK15*P(0,11) + HK16*P(1,11) - HK17*P(3,11) - HK9*P(11,23) + HK9*P(5,11)); - // Kfusion(12) = -HK32*(HK12*P(6,12) + HK13*P(12,22) - HK13*P(4,12) + HK14*P(2,12) + HK15*P(0,12) + HK16*P(1,12) - HK17*P(3,12) - HK9*P(12,23) + HK9*P(5,12)); - // Kfusion(13) = -HK32*(HK12*P(6,13) + HK13*P(13,22) - HK13*P(4,13) + HK14*P(2,13) + HK15*P(0,13) + HK16*P(1,13) - HK17*P(3,13) - HK9*P(13,23) + HK9*P(5,13)); - // Kfusion(14) = -HK32*(HK12*P(6,14) + HK13*P(14,22) - HK13*P(4,14) + HK14*P(2,14) + HK15*P(0,14) + HK16*P(1,14) - HK17*P(3,14) - HK9*P(14,23) + HK9*P(5,14)); - // Kfusion(15) = -HK32*(HK12*P(6,15) + HK13*P(15,22) - HK13*P(4,15) + HK14*P(2,15) + HK15*P(0,15) + HK16*P(1,15) - HK17*P(3,15) - HK9*P(15,23) + HK9*P(5,15)); - // Kfusion(16) = -HK32*(HK12*P(6,16) + HK13*P(16,22) - HK13*P(4,16) + HK14*P(2,16) + HK15*P(0,16) + HK16*P(1,16) - HK17*P(3,16) - HK9*P(16,23) + HK9*P(5,16)); - // Kfusion(17) = -HK32*(HK12*P(6,17) + HK13*P(17,22) - HK13*P(4,17) + HK14*P(2,17) + HK15*P(0,17) + HK16*P(1,17) - HK17*P(3,17) - HK9*P(17,23) + HK9*P(5,17)); - // Kfusion(18) = -HK32*(HK12*P(6,18) + HK13*P(18,22) - HK13*P(4,18) + HK14*P(2,18) + HK15*P(0,18) + HK16*P(1,18) - HK17*P(3,18) - HK9*P(18,23) + HK9*P(5,18)); - // Kfusion(19) = -HK32*(HK12*P(6,19) + HK13*P(19,22) - HK13*P(4,19) + HK14*P(2,19) + HK15*P(0,19) + HK16*P(1,19) - HK17*P(3,19) - HK9*P(19,23) + HK9*P(5,19)); - // Kfusion(20) = -HK32*(HK12*P(6,20) + HK13*P(20,22) - HK13*P(4,20) + HK14*P(2,20) + HK15*P(0,20) + HK16*P(1,20) - HK17*P(3,20) - HK9*P(20,23) + HK9*P(5,20)); - // Kfusion(21) = -HK32*(HK12*P(6,21) + HK13*P(21,22) - HK13*P(4,21) + HK14*P(2,21) + HK15*P(0,21) + HK16*P(1,21) - HK17*P(3,21) - HK9*P(21,23) + HK9*P(5,21)); - Kfusion(22) = -HK22*HK32; - Kfusion(23) = -HK28*HK32; - + sym::ComputeDragYInnovVarAndK(state_vector_prev, P, rho, bcoef_inv, mcoef_corrrected, R_ACC, FLT_EPSILON, &_drag_innov_var(1), &Kfusion); } + if (_drag_innov_var(axis_index) < R_ACC) { + // calculation is badly conditioned + return; + } + + const float pred_acc = -0.5f * bcoef_inv * rho * rel_wind_body(axis_index) * rel_wind_speed - rel_wind_body(axis_index) * mcoef_corrrected; + // Apply an innovation consistency check with a 5 Sigma threshold _drag_innov(axis_index) = pred_acc - mea_acc; _drag_test_ratio(axis_index) = sq(_drag_innov(axis_index)) / (sq(5.0f) * _drag_innov_var(axis_index)); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 3a09abaaa6..ed8cf5e99f 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -415,6 +415,68 @@ def compute_gnss_yaw_innon_innov_var_and_h( return (innov, innov_var, H.T) +def predict_drag( + state: VState, + rho: sf.Scalar, + cd: sf.Scalar, + cm: sf.Scalar, + epsilon: sf.Scalar + ): + q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz]) + R_to_earth = quat_to_rot(q_att) + R_to_body = R_to_earth.T + + vel_rel = sf.V3(state[State.vx] - state[State.wx], + state[State.vy] - state[State.wy], + state[State.vz]) + vel_rel_body = R_to_body * vel_rel + + bluff_body_drag = -0.5 * rho * cd * sf.V2(vel_rel_body) * vel_rel_body.norm(epsilon=epsilon) + momentum_drag = -cm * sf.V2(vel_rel_body) + + return bluff_body_drag + momentum_drag + + +def compute_drag_x_innov_var_and_k( + state: VState, + P: MState, + rho: sf.Scalar, + cd: sf.Scalar, + cm: sf.Scalar, + R: sf.Scalar, + epsilon: sf.Scalar +) -> (sf.Scalar, sf.Scalar, VState): + + meas_pred = predict_drag(state, rho, cd, cm, epsilon) + Hx = sf.V1(meas_pred[0]).jacobian(state) + innov_var = (Hx * P * Hx.T + R)[0,0] + Ktotal = P * Hx.T / sf.Max(innov_var, epsilon) + K = VState() + K[State.wx] = Ktotal[State.wx] + K[State.wy] = Ktotal[State.wy] + + return (innov_var, K) + +def compute_drag_y_innov_var_and_k( + state: VState, + P: MState, + rho: sf.Scalar, + cd: sf.Scalar, + cm: sf.Scalar, + R: sf.Scalar, + epsilon: sf.Scalar +) -> (sf.Scalar, sf.Scalar, VState): + + meas_pred = predict_drag(state, rho, cd, cm, epsilon) + Hy = sf.V1(meas_pred[1]).jacobian(state) + innov_var = (Hy * P * Hy.T + R)[0,0] + Ktotal = P * Hy.T / sf.Max(innov_var, epsilon) + K = VState() + K[State.wx] = Ktotal[State.wx] + K[State.wy] = Ktotal[State.wy] + + return (innov_var, K) + print("Derive EKF2 equations...") 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"]) @@ -433,3 +495,5 @@ generate_px4_function(compute_mag_declination_innov_innov_var_and_h, output_name generate_px4_function(compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"]) generate_px4_function(compute_flow_y_innov_var_and_h, output_names=["innov_var", "H"]) generate_px4_function(compute_gnss_yaw_innon_innov_var_and_h, output_names=["innov", "innov_var", "H"]) +generate_px4_function(compute_drag_x_innov_var_and_k, output_names=["innov_var", "K"]) +generate_px4_function(compute_drag_y_innov_var_and_k, output_names=["innov_var", "K"]) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_x_innov_var_and_k.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_x_innov_var_and_k.h new file mode 100644 index 0000000000..bdff5c8fd9 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_x_innov_var_and_k.h @@ -0,0 +1,184 @@ +// ----------------------------------------------------------------------------- +// 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_drag_x_innov_var_and_k + * + * Args: + * state: Matrix24_1 + * P: Matrix24_24 + * rho: Scalar + * cd: Scalar + * cm: Scalar + * R: Scalar + * epsilon: Scalar + * + * Outputs: + * innov_var: Scalar + * K: Matrix24_1 + */ +template +void ComputeDragXInnovVarAndK(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar rho, + const Scalar cd, const Scalar cm, const Scalar R, + const Scalar epsilon, Scalar* const innov_var = nullptr, + matrix::Matrix* const K = nullptr) { + // Total ops: 407 + + // Input arrays + + // Intermediate terms (76) + const Scalar _tmp0 = -state(23, 0) + state(5, 0); + const Scalar _tmp1 = 2 * _tmp0; + const Scalar _tmp2 = _tmp1 * state(1, 0); + const Scalar _tmp3 = -state(22, 0) + state(4, 0); + const Scalar _tmp4 = 2 * _tmp3; + const Scalar _tmp5 = _tmp4 * state(2, 0); + const Scalar _tmp6 = 2 * state(6, 0); + const Scalar _tmp7 = _tmp6 * state(0, 0); + const Scalar _tmp8 = _tmp2 - _tmp5 - _tmp7; + const Scalar _tmp9 = std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp10 = -_tmp9; + const Scalar _tmp11 = std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp12 = std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp13 = std::pow(state(0, 0), Scalar(2)); + const Scalar _tmp14 = -_tmp12 + _tmp13; + const Scalar _tmp15 = _tmp10 + _tmp11 + _tmp14; + const Scalar _tmp16 = state(0, 0) * state(3, 0); + const Scalar _tmp17 = state(1, 0) * state(2, 0); + const Scalar _tmp18 = _tmp16 + _tmp17; + const Scalar _tmp19 = state(0, 0) * state(2, 0); + const Scalar _tmp20 = state(1, 0) * state(3, 0); + const Scalar _tmp21 = _tmp1 * _tmp18 + _tmp15 * _tmp3 + _tmp6 * (-_tmp19 + _tmp20); + const Scalar _tmp22 = state(2, 0) * state(3, 0); + const Scalar _tmp23 = state(0, 0) * state(1, 0); + const Scalar _tmp24 = _tmp22 - _tmp23; + const Scalar _tmp25 = _tmp19 + _tmp20; + const Scalar _tmp26 = -_tmp11; + const Scalar _tmp27 = _tmp10 + _tmp12 + _tmp13 + _tmp26; + const Scalar _tmp28 = _tmp1 * _tmp24 + _tmp25 * _tmp4 + _tmp27 * state(6, 0); + const Scalar _tmp29 = _tmp14 + _tmp26 + _tmp9; + const Scalar _tmp30 = -_tmp16 + _tmp17; + const Scalar _tmp31 = _tmp0 * _tmp29 + _tmp30 * _tmp4 + _tmp6 * (_tmp22 + _tmp23); + const Scalar _tmp32 = std::sqrt(Scalar(std::pow(_tmp21, Scalar(2)) + std::pow(_tmp28, Scalar(2)) + + std::pow(_tmp31, Scalar(2)) + epsilon)); + const Scalar _tmp33 = cd * rho; + const Scalar _tmp34 = _tmp32 * _tmp33; + const Scalar _tmp35 = Scalar(0.5) * _tmp34; + const Scalar _tmp36 = _tmp4 * state(0, 0); + const Scalar _tmp37 = _tmp1 * state(3, 0); + const Scalar _tmp38 = _tmp6 * state(2, 0); + const Scalar _tmp39 = _tmp36 + _tmp37 - _tmp38; + const Scalar _tmp40 = 2 * _tmp28; + const Scalar _tmp41 = _tmp1 * state(2, 0) + _tmp4 * state(1, 0) + _tmp6 * state(3, 0); + const Scalar _tmp42 = 2 * _tmp31; + const Scalar _tmp43 = 2 * _tmp21; + const Scalar _tmp44 = Scalar(0.25) * _tmp21 * _tmp33 / _tmp32; + const Scalar _tmp45 = + -_tmp35 * _tmp8 - _tmp44 * (_tmp39 * _tmp40 + _tmp41 * _tmp42 + _tmp43 * _tmp8) - _tmp8 * cm; + const Scalar _tmp46 = 2 * _tmp19; + const Scalar _tmp47 = 2 * _tmp20; + const Scalar _tmp48 = -_tmp46 + _tmp47; + const Scalar _tmp49 = 2 * _tmp22; + const Scalar _tmp50 = 2 * _tmp23; + const Scalar _tmp51 = -_tmp35 * _tmp48 - + _tmp44 * (_tmp27 * _tmp40 + _tmp42 * (_tmp49 + _tmp50) + _tmp43 * _tmp48) - + _tmp48 * cm; + const Scalar _tmp52 = -_tmp2 + _tmp5 + _tmp7; + const Scalar _tmp53 = _tmp4 * state(3, 0); + const Scalar _tmp54 = _tmp1 * state(0, 0); + const Scalar _tmp55 = _tmp6 * state(1, 0); + const Scalar _tmp56 = -_tmp53 + _tmp54 + _tmp55; + const Scalar _tmp57 = -_tmp35 * _tmp39 - _tmp39 * cm - + _tmp44 * (_tmp39 * _tmp43 + _tmp40 * _tmp52 + _tmp42 * _tmp56); + const Scalar _tmp58 = _tmp15 * cm; + const Scalar _tmp59 = _tmp15 * _tmp35; + const Scalar _tmp60 = _tmp15 * _tmp43; + const Scalar _tmp61 = 2 * _tmp16; + const Scalar _tmp62 = 2 * _tmp17; + const Scalar _tmp63 = + -_tmp44 * (_tmp40 * (_tmp46 + _tmp47) + _tmp42 * (-_tmp61 + _tmp62) + _tmp60) - _tmp58 - + _tmp59; + const Scalar _tmp64 = 4 * _tmp28; + const Scalar _tmp65 = _tmp29 * _tmp42; + const Scalar _tmp66 = Scalar(1.0) * _tmp18 * _tmp34 + 2 * _tmp18 * cm - + _tmp44 * (-4 * _tmp18 * _tmp21 - _tmp24 * _tmp64 - _tmp65); + const Scalar _tmp67 = + -_tmp35 * _tmp41 - _tmp41 * cm - + _tmp44 * (_tmp40 * (_tmp53 - _tmp54 - _tmp55) + _tmp41 * _tmp43 + _tmp42 * _tmp52); + const Scalar _tmp68 = _tmp61 + _tmp62; + const Scalar _tmp69 = -_tmp35 * _tmp68 - + _tmp44 * (_tmp40 * (_tmp49 - _tmp50) + _tmp43 * _tmp68 + _tmp65) - + _tmp68 * cm; + const Scalar _tmp70 = + -_tmp35 * _tmp56 - + _tmp44 * (_tmp40 * _tmp41 + _tmp42 * (-_tmp36 - _tmp37 + _tmp38) + _tmp43 * _tmp56) - + _tmp56 * cm; + const Scalar _tmp71 = + -_tmp44 * (-_tmp25 * _tmp64 - 4 * _tmp30 * _tmp31 - _tmp60) + _tmp58 + _tmp59; + const Scalar _tmp72 = P(23, 23) * _tmp66; + const Scalar _tmp73 = P(22, 22) * _tmp71; + const Scalar _tmp74 = R + + _tmp45 * (P(0, 2) * _tmp57 + P(1, 2) * _tmp67 + P(2, 2) * _tmp45 + + P(22, 2) * _tmp71 + P(23, 2) * _tmp66 + P(3, 2) * _tmp70 + + P(4, 2) * _tmp63 + P(5, 2) * _tmp69 + P(6, 2) * _tmp51) + + _tmp51 * (P(0, 6) * _tmp57 + P(1, 6) * _tmp67 + P(2, 6) * _tmp45 + + P(22, 6) * _tmp71 + P(23, 6) * _tmp66 + P(3, 6) * _tmp70 + + P(4, 6) * _tmp63 + P(5, 6) * _tmp69 + P(6, 6) * _tmp51) + + _tmp57 * (P(0, 0) * _tmp57 + P(1, 0) * _tmp67 + P(2, 0) * _tmp45 + + P(22, 0) * _tmp71 + P(23, 0) * _tmp66 + P(3, 0) * _tmp70 + + P(4, 0) * _tmp63 + P(5, 0) * _tmp69 + P(6, 0) * _tmp51) + + _tmp63 * (P(0, 4) * _tmp57 + P(1, 4) * _tmp67 + P(2, 4) * _tmp45 + + P(22, 4) * _tmp71 + P(23, 4) * _tmp66 + P(3, 4) * _tmp70 + + P(4, 4) * _tmp63 + P(5, 4) * _tmp69 + P(6, 4) * _tmp51) + + _tmp66 * (P(0, 23) * _tmp57 + P(1, 23) * _tmp67 + P(2, 23) * _tmp45 + + P(22, 23) * _tmp71 + P(3, 23) * _tmp70 + P(4, 23) * _tmp63 + + P(5, 23) * _tmp69 + P(6, 23) * _tmp51 + _tmp72) + + _tmp67 * (P(0, 1) * _tmp57 + P(1, 1) * _tmp67 + P(2, 1) * _tmp45 + + P(22, 1) * _tmp71 + P(23, 1) * _tmp66 + P(3, 1) * _tmp70 + + P(4, 1) * _tmp63 + P(5, 1) * _tmp69 + P(6, 1) * _tmp51) + + _tmp69 * (P(0, 5) * _tmp57 + P(1, 5) * _tmp67 + P(2, 5) * _tmp45 + + P(22, 5) * _tmp71 + P(23, 5) * _tmp66 + P(3, 5) * _tmp70 + + P(4, 5) * _tmp63 + P(5, 5) * _tmp69 + P(6, 5) * _tmp51) + + _tmp70 * (P(0, 3) * _tmp57 + P(1, 3) * _tmp67 + P(2, 3) * _tmp45 + + P(22, 3) * _tmp71 + P(23, 3) * _tmp66 + P(3, 3) * _tmp70 + + P(4, 3) * _tmp63 + P(5, 3) * _tmp69 + P(6, 3) * _tmp51) + + _tmp71 * (P(0, 22) * _tmp57 + P(1, 22) * _tmp67 + P(2, 22) * _tmp45 + + P(23, 22) * _tmp66 + P(3, 22) * _tmp70 + P(4, 22) * _tmp63 + + P(5, 22) * _tmp69 + P(6, 22) * _tmp51 + _tmp73); + const Scalar _tmp75 = Scalar(1.0) / (math::max(_tmp74, epsilon)); + + // Output terms (2) + if (innov_var != nullptr) { + Scalar& _innov_var = (*innov_var); + + _innov_var = _tmp74; + } + + if (K != nullptr) { + matrix::Matrix& _K = (*K); + + _K.setZero(); + + _K(22, 0) = _tmp75 * (P(22, 0) * _tmp57 + P(22, 1) * _tmp67 + P(22, 2) * _tmp45 + + P(22, 23) * _tmp66 + P(22, 3) * _tmp70 + P(22, 4) * _tmp63 + + P(22, 5) * _tmp69 + P(22, 6) * _tmp51 + _tmp73); + _K(23, 0) = _tmp75 * (P(23, 0) * _tmp57 + P(23, 1) * _tmp67 + P(23, 2) * _tmp45 + + P(23, 22) * _tmp71 + P(23, 3) * _tmp70 + P(23, 4) * _tmp63 + + P(23, 5) * _tmp69 + P(23, 6) * _tmp51 + _tmp72); + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_y_innov_var_and_k.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_y_innov_var_and_k.h new file mode 100644 index 0000000000..c8407d00e1 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_y_innov_var_and_k.h @@ -0,0 +1,185 @@ +// ----------------------------------------------------------------------------- +// 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_drag_y_innov_var_and_k + * + * Args: + * state: Matrix24_1 + * P: Matrix24_24 + * rho: Scalar + * cd: Scalar + * cm: Scalar + * R: Scalar + * epsilon: Scalar + * + * Outputs: + * innov_var: Scalar + * K: Matrix24_1 + */ +template +void ComputeDragYInnovVarAndK(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar rho, + const Scalar cd, const Scalar cm, const Scalar R, + const Scalar epsilon, Scalar* const innov_var = nullptr, + matrix::Matrix* const K = nullptr) { + // Total ops: 407 + + // Input arrays + + // Intermediate terms (76) + const Scalar _tmp0 = -state(23, 0) + state(5, 0); + const Scalar _tmp1 = 2 * _tmp0; + const Scalar _tmp2 = _tmp1 * state(1, 0); + const Scalar _tmp3 = -state(22, 0) + state(4, 0); + const Scalar _tmp4 = 2 * _tmp3; + const Scalar _tmp5 = _tmp4 * state(2, 0); + const Scalar _tmp6 = 2 * state(6, 0); + const Scalar _tmp7 = _tmp6 * state(0, 0); + const Scalar _tmp8 = -_tmp2 + _tmp5 + _tmp7; + const Scalar _tmp9 = std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp10 = -_tmp9; + const Scalar _tmp11 = std::pow(state(0, 0), Scalar(2)); + const Scalar _tmp12 = std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp13 = -_tmp12; + const Scalar _tmp14 = std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp15 = _tmp10 + _tmp11 + _tmp13 + _tmp14; + const Scalar _tmp16 = state(0, 0) * state(3, 0); + const Scalar _tmp17 = state(1, 0) * state(2, 0); + const Scalar _tmp18 = _tmp16 + _tmp17; + const Scalar _tmp19 = state(0, 0) * state(2, 0); + const Scalar _tmp20 = state(1, 0) * state(3, 0); + const Scalar _tmp21 = _tmp1 * _tmp18 + _tmp15 * _tmp3 + _tmp6 * (-_tmp19 + _tmp20); + const Scalar _tmp22 = state(2, 0) * state(3, 0); + const Scalar _tmp23 = state(0, 0) * state(1, 0); + const Scalar _tmp24 = _tmp22 - _tmp23; + const Scalar _tmp25 = _tmp19 + _tmp20; + const Scalar _tmp26 = _tmp11 - _tmp14; + const Scalar _tmp27 = _tmp13 + _tmp26 + _tmp9; + const Scalar _tmp28 = _tmp1 * _tmp24 + _tmp25 * _tmp4 + _tmp27 * state(6, 0); + const Scalar _tmp29 = _tmp10 + _tmp12 + _tmp26; + const Scalar _tmp30 = -_tmp16 + _tmp17; + const Scalar _tmp31 = _tmp0 * _tmp29 + _tmp30 * _tmp4 + _tmp6 * (_tmp22 + _tmp23); + const Scalar _tmp32 = std::sqrt(Scalar(std::pow(_tmp21, Scalar(2)) + std::pow(_tmp28, Scalar(2)) + + std::pow(_tmp31, Scalar(2)) + epsilon)); + const Scalar _tmp33 = cd * rho; + const Scalar _tmp34 = _tmp32 * _tmp33; + const Scalar _tmp35 = Scalar(0.5) * _tmp34; + const Scalar _tmp36 = _tmp4 * state(3, 0); + const Scalar _tmp37 = _tmp1 * state(0, 0); + const Scalar _tmp38 = _tmp6 * state(1, 0); + const Scalar _tmp39 = 2 * _tmp28; + const Scalar _tmp40 = 2 * _tmp31; + const Scalar _tmp41 = _tmp1 * state(2, 0) + _tmp4 * state(1, 0) + _tmp6 * state(3, 0); + const Scalar _tmp42 = 2 * _tmp21; + const Scalar _tmp43 = Scalar(0.25) * _tmp31 * _tmp33 / _tmp32; + const Scalar _tmp44 = + -_tmp35 * _tmp8 - + _tmp43 * (_tmp39 * (_tmp36 - _tmp37 - _tmp38) + _tmp40 * _tmp8 + _tmp41 * _tmp42) - + _tmp8 * cm; + const Scalar _tmp45 = -_tmp36 + _tmp37 + _tmp38; + const Scalar _tmp46 = _tmp4 * state(0, 0); + const Scalar _tmp47 = _tmp1 * state(3, 0); + const Scalar _tmp48 = _tmp6 * state(2, 0); + const Scalar _tmp49 = _tmp46 + _tmp47 - _tmp48; + const Scalar _tmp50 = -_tmp35 * _tmp45 - + _tmp43 * (_tmp39 * _tmp8 + _tmp40 * _tmp45 + _tmp42 * _tmp49) - _tmp45 * cm; + const Scalar _tmp51 = + -_tmp35 * _tmp41 - _tmp41 * cm - + _tmp43 * (_tmp39 * _tmp49 + _tmp40 * _tmp41 + _tmp42 * (_tmp2 - _tmp5 - _tmp7)); + const Scalar _tmp52 = -_tmp46 - _tmp47 + _tmp48; + const Scalar _tmp53 = -_tmp35 * _tmp52 - + _tmp43 * (_tmp39 * _tmp41 + _tmp40 * _tmp52 + _tmp42 * _tmp45) - + _tmp52 * cm; + const Scalar _tmp54 = _tmp29 * cm; + const Scalar _tmp55 = _tmp29 * _tmp35; + const Scalar _tmp56 = 4 * _tmp28; + const Scalar _tmp57 = _tmp29 * _tmp40; + const Scalar _tmp58 = + -_tmp43 * (-4 * _tmp18 * _tmp21 - _tmp24 * _tmp56 - _tmp57) + _tmp54 + _tmp55; + const Scalar _tmp59 = 2 * _tmp16; + const Scalar _tmp60 = 2 * _tmp17; + const Scalar _tmp61 = -_tmp59 + _tmp60; + const Scalar _tmp62 = _tmp15 * _tmp42; + const Scalar _tmp63 = 2 * _tmp19; + const Scalar _tmp64 = 2 * _tmp20; + const Scalar _tmp65 = -_tmp35 * _tmp61 - + _tmp43 * (_tmp39 * (_tmp63 + _tmp64) + _tmp40 * _tmp61 + _tmp62) - + _tmp61 * cm; + const Scalar _tmp66 = Scalar(1.0) * _tmp30 * _tmp34 + 2 * _tmp30 * cm - + _tmp43 * (-_tmp25 * _tmp56 - 4 * _tmp30 * _tmp31 - _tmp62); + const Scalar _tmp67 = 2 * _tmp22; + const Scalar _tmp68 = 2 * _tmp23; + const Scalar _tmp69 = _tmp67 + _tmp68; + const Scalar _tmp70 = -_tmp35 * _tmp69 - + _tmp43 * (_tmp27 * _tmp39 + _tmp40 * _tmp69 + _tmp42 * (-_tmp63 + _tmp64)) - + _tmp69 * cm; + const Scalar _tmp71 = + -_tmp43 * (_tmp39 * (_tmp67 - _tmp68) + _tmp42 * (_tmp59 + _tmp60) + _tmp57) - _tmp54 - + _tmp55; + const Scalar _tmp72 = P(22, 22) * _tmp66; + const Scalar _tmp73 = P(23, 23) * _tmp58; + const Scalar _tmp74 = R + + _tmp44 * (P(0, 1) * _tmp50 + P(1, 1) * _tmp44 + P(2, 1) * _tmp51 + + P(22, 1) * _tmp66 + P(23, 1) * _tmp58 + P(3, 1) * _tmp53 + + P(4, 1) * _tmp65 + P(5, 1) * _tmp71 + P(6, 1) * _tmp70) + + _tmp50 * (P(0, 0) * _tmp50 + P(1, 0) * _tmp44 + P(2, 0) * _tmp51 + + P(22, 0) * _tmp66 + P(23, 0) * _tmp58 + P(3, 0) * _tmp53 + + P(4, 0) * _tmp65 + P(5, 0) * _tmp71 + P(6, 0) * _tmp70) + + _tmp51 * (P(0, 2) * _tmp50 + P(1, 2) * _tmp44 + P(2, 2) * _tmp51 + + P(22, 2) * _tmp66 + P(23, 2) * _tmp58 + P(3, 2) * _tmp53 + + P(4, 2) * _tmp65 + P(5, 2) * _tmp71 + P(6, 2) * _tmp70) + + _tmp53 * (P(0, 3) * _tmp50 + P(1, 3) * _tmp44 + P(2, 3) * _tmp51 + + P(22, 3) * _tmp66 + P(23, 3) * _tmp58 + P(3, 3) * _tmp53 + + P(4, 3) * _tmp65 + P(5, 3) * _tmp71 + P(6, 3) * _tmp70) + + _tmp58 * (P(0, 23) * _tmp50 + P(1, 23) * _tmp44 + P(2, 23) * _tmp51 + + P(22, 23) * _tmp66 + P(3, 23) * _tmp53 + P(4, 23) * _tmp65 + + P(5, 23) * _tmp71 + P(6, 23) * _tmp70 + _tmp73) + + _tmp65 * (P(0, 4) * _tmp50 + P(1, 4) * _tmp44 + P(2, 4) * _tmp51 + + P(22, 4) * _tmp66 + P(23, 4) * _tmp58 + P(3, 4) * _tmp53 + + P(4, 4) * _tmp65 + P(5, 4) * _tmp71 + P(6, 4) * _tmp70) + + _tmp66 * (P(0, 22) * _tmp50 + P(1, 22) * _tmp44 + P(2, 22) * _tmp51 + + P(23, 22) * _tmp58 + P(3, 22) * _tmp53 + P(4, 22) * _tmp65 + + P(5, 22) * _tmp71 + P(6, 22) * _tmp70 + _tmp72) + + _tmp70 * (P(0, 6) * _tmp50 + P(1, 6) * _tmp44 + P(2, 6) * _tmp51 + + P(22, 6) * _tmp66 + P(23, 6) * _tmp58 + P(3, 6) * _tmp53 + + P(4, 6) * _tmp65 + P(5, 6) * _tmp71 + P(6, 6) * _tmp70) + + _tmp71 * (P(0, 5) * _tmp50 + P(1, 5) * _tmp44 + P(2, 5) * _tmp51 + + P(22, 5) * _tmp66 + P(23, 5) * _tmp58 + P(3, 5) * _tmp53 + + P(4, 5) * _tmp65 + P(5, 5) * _tmp71 + P(6, 5) * _tmp70); + const Scalar _tmp75 = Scalar(1.0) / (math::max(_tmp74, epsilon)); + + // Output terms (2) + if (innov_var != nullptr) { + Scalar& _innov_var = (*innov_var); + + _innov_var = _tmp74; + } + + if (K != nullptr) { + matrix::Matrix& _K = (*K); + + _K.setZero(); + + _K(22, 0) = _tmp75 * (P(22, 0) * _tmp50 + P(22, 1) * _tmp44 + P(22, 2) * _tmp51 + + P(22, 23) * _tmp58 + P(22, 3) * _tmp53 + P(22, 4) * _tmp65 + + P(22, 5) * _tmp71 + P(22, 6) * _tmp70 + _tmp72); + _K(23, 0) = _tmp75 * (P(23, 0) * _tmp50 + P(23, 1) * _tmp44 + P(23, 2) * _tmp51 + + P(23, 22) * _tmp66 + P(23, 3) * _tmp53 + P(23, 4) * _tmp65 + + P(23, 5) * _tmp71 + P(23, 6) * _tmp70 + _tmp73); + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym