forked from Archive/PX4-Autopilot
ekf2-drag: do not generate Kalman gain to save flash
This commit is contained in:
bresch
Mathieu Bresciani
parent
1efb08375a
commit
17d55dddd6
|
|
@ -37,8 +37,8 @@
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "ekf.h"
|
#include "ekf.h"
|
||||||
#include <ekf_derivation/generated/compute_drag_x_innov_var_and_k.h>
|
#include <ekf_derivation/generated/compute_drag_x_innov_var_and_h.h>
|
||||||
#include <ekf_derivation/generated/compute_drag_y_innov_var_and_k.h>
|
#include <ekf_derivation/generated/compute_drag_y_innov_var_and_h.h>
|
||||||
|
|
||||||
#include <mathlib/mathlib.h>
|
#include <mathlib/mathlib.h>
|
||||||
#include <lib/atmosphere/atmosphere.h>
|
#include <lib/atmosphere/atmosphere.h>
|
||||||
|
|
@ -110,7 +110,7 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
|
||||||
|
|
||||||
bool fused[] {false, false};
|
bool fused[] {false, false};
|
||||||
|
|
||||||
VectorState Kfusion;
|
VectorState H;
|
||||||
|
|
||||||
// perform sequential fusion of XY specific forces
|
// perform sequential fusion of XY specific forces
|
||||||
for (uint8_t axis_index = 0; axis_index < 2; axis_index++) {
|
for (uint8_t axis_index = 0; axis_index < 2; axis_index++) {
|
||||||
|
|
@ -128,16 +128,16 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
|
||||||
_aid_src_drag.innovation_variance[axis_index] = NAN; // reset
|
_aid_src_drag.innovation_variance[axis_index] = NAN; // reset
|
||||||
|
|
||||||
if (axis_index == 0) {
|
if (axis_index == 0) {
|
||||||
sym::ComputeDragXInnovVarAndK(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON,
|
sym::ComputeDragXInnovVarAndH(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON,
|
||||||
&_aid_src_drag.innovation_variance[axis_index], &Kfusion);
|
&_aid_src_drag.innovation_variance[axis_index], &H);
|
||||||
|
|
||||||
if (!using_bcoef_x && !using_mcoef) {
|
if (!using_bcoef_x && !using_mcoef) {
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
} else if (axis_index == 1) {
|
} else if (axis_index == 1) {
|
||||||
sym::ComputeDragYInnovVarAndK(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON,
|
sym::ComputeDragYInnovVarAndH(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON,
|
||||||
&_aid_src_drag.innovation_variance[axis_index], &Kfusion);
|
&_aid_src_drag.innovation_variance[axis_index], &H);
|
||||||
|
|
||||||
if (!using_bcoef_y && !using_mcoef) {
|
if (!using_bcoef_y && !using_mcoef) {
|
||||||
continue;
|
continue;
|
||||||
|
|
@ -157,7 +157,10 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
|
||||||
&& PX4_ISFINITE(_aid_src_drag.innovation_variance[axis_index]) && PX4_ISFINITE(_aid_src_drag.innovation[axis_index])
|
&& PX4_ISFINITE(_aid_src_drag.innovation_variance[axis_index]) && PX4_ISFINITE(_aid_src_drag.innovation[axis_index])
|
||||||
&& (_aid_src_drag.test_ratio[axis_index] < 1.f)
|
&& (_aid_src_drag.test_ratio[axis_index] < 1.f)
|
||||||
) {
|
) {
|
||||||
if (measurementUpdate(Kfusion, _aid_src_drag.innovation_variance[axis_index], _aid_src_drag.innovation[axis_index])) {
|
|
||||||
|
VectorState K = P * H / _aid_src_drag.innovation_variance[axis_index];
|
||||||
|
|
||||||
|
if (measurementUpdate(K, _aid_src_drag.innovation_variance[axis_index], _aid_src_drag.innovation[axis_index])) {
|
||||||
fused[axis_index] = true;
|
fused[axis_index] = true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -567,7 +567,7 @@ def predict_drag(
|
||||||
return bluff_body_drag + momentum_drag
|
return bluff_body_drag + momentum_drag
|
||||||
|
|
||||||
|
|
||||||
def compute_drag_x_innov_var_and_k(
|
def compute_drag_x_innov_var_and_h(
|
||||||
state: VState,
|
state: VState,
|
||||||
P: MTangent,
|
P: MTangent,
|
||||||
rho: sf.Scalar,
|
rho: sf.Scalar,
|
||||||
|
|
@ -581,11 +581,10 @@ def compute_drag_x_innov_var_and_k(
|
||||||
meas_pred = predict_drag(state, rho, cd, cm, epsilon)
|
meas_pred = predict_drag(state, rho, cd, cm, epsilon)
|
||||||
Hx = sf.V1(meas_pred[0]).jacobian(state)
|
Hx = sf.V1(meas_pred[0]).jacobian(state)
|
||||||
innov_var = (Hx * P * Hx.T + R)[0,0]
|
innov_var = (Hx * P * Hx.T + R)[0,0]
|
||||||
K = P * Hx.T / sf.Max(innov_var, epsilon)
|
|
||||||
|
|
||||||
return (innov_var, K)
|
return (innov_var, Hx.T)
|
||||||
|
|
||||||
def compute_drag_y_innov_var_and_k(
|
def compute_drag_y_innov_var_and_h(
|
||||||
state: VState,
|
state: VState,
|
||||||
P: MTangent,
|
P: MTangent,
|
||||||
rho: sf.Scalar,
|
rho: sf.Scalar,
|
||||||
|
|
@ -599,9 +598,8 @@ def compute_drag_y_innov_var_and_k(
|
||||||
meas_pred = predict_drag(state, rho, cd, cm, epsilon)
|
meas_pred = predict_drag(state, rho, cd, cm, epsilon)
|
||||||
Hy = sf.V1(meas_pred[1]).jacobian(state)
|
Hy = sf.V1(meas_pred[1]).jacobian(state)
|
||||||
innov_var = (Hy * P * Hy.T + R)[0,0]
|
innov_var = (Hy * P * Hy.T + R)[0,0]
|
||||||
K = P * Hy.T / sf.Max(innov_var, epsilon)
|
|
||||||
|
|
||||||
return (innov_var, K)
|
return (innov_var, Hy.T)
|
||||||
|
|
||||||
def predict_gravity_direction(state: State):
|
def predict_gravity_direction(state: State):
|
||||||
# get transform from earth to body frame
|
# get transform from earth to body frame
|
||||||
|
|
@ -674,8 +672,8 @@ if not args.disable_mag:
|
||||||
if not args.disable_wind:
|
if not args.disable_wind:
|
||||||
generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"])
|
generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"])
|
||||||
generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"])
|
generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"])
|
||||||
generate_px4_function(compute_drag_x_innov_var_and_k, output_names=["innov_var", "K"])
|
generate_px4_function(compute_drag_x_innov_var_and_h, output_names=["innov_var", "Hx"])
|
||||||
generate_px4_function(compute_drag_y_innov_var_and_k, output_names=["innov_var", "K"])
|
generate_px4_function(compute_drag_y_innov_var_and_h, output_names=["innov_var", "Hy"])
|
||||||
generate_px4_function(compute_sideslip_h_and_k, output_names=["H", "K"])
|
generate_px4_function(compute_sideslip_h_and_k, output_names=["H", "K"])
|
||||||
generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"])
|
generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"])
|
||||||
generate_px4_function(compute_wind_init_and_cov_from_airspeed, output_names=["wind", "P_wind"])
|
generate_px4_function(compute_wind_init_and_cov_from_airspeed, output_names=["wind", "P_wind"])
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,168 @@
|
||||||
|
// -----------------------------------------------------------------------------
|
||||||
|
// This file was autogenerated by symforce from template:
|
||||||
|
// function/FUNCTION.h.jinja
|
||||||
|
// Do NOT modify by hand.
|
||||||
|
// -----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <matrix/math.hpp>
|
||||||
|
|
||||||
|
namespace sym {
|
||||||
|
|
||||||
|
/**
|
||||||
|
* This function was autogenerated from a symbolic function. Do not modify by hand.
|
||||||
|
*
|
||||||
|
* Symbolic function: compute_drag_x_innov_var_and_h
|
||||||
|
*
|
||||||
|
* Args:
|
||||||
|
* state: Matrix24_1
|
||||||
|
* P: Matrix23_23
|
||||||
|
* rho: Scalar
|
||||||
|
* cd: Scalar
|
||||||
|
* cm: Scalar
|
||||||
|
* R: Scalar
|
||||||
|
* epsilon: Scalar
|
||||||
|
*
|
||||||
|
* Outputs:
|
||||||
|
* innov_var: Scalar
|
||||||
|
* Hx: Matrix23_1
|
||||||
|
*/
|
||||||
|
template <typename Scalar>
|
||||||
|
void ComputeDragXInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||||
|
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar rho,
|
||||||
|
const Scalar cd, const Scalar cm, const Scalar R,
|
||||||
|
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||||
|
matrix::Matrix<Scalar, 23, 1>* const Hx = nullptr) {
|
||||||
|
// Total ops: 317
|
||||||
|
|
||||||
|
// Input arrays
|
||||||
|
|
||||||
|
// Intermediate terms (73)
|
||||||
|
const Scalar _tmp0 = 2 * state(3, 0);
|
||||||
|
const Scalar _tmp1 = _tmp0 * state(0, 0);
|
||||||
|
const Scalar _tmp2 = 2 * state(2, 0);
|
||||||
|
const Scalar _tmp3 = _tmp2 * state(1, 0);
|
||||||
|
const Scalar _tmp4 = _tmp1 + _tmp3;
|
||||||
|
const Scalar _tmp5 = _tmp4 * cm;
|
||||||
|
const Scalar _tmp6 = std::pow(state(3, 0), Scalar(2));
|
||||||
|
const Scalar _tmp7 = -2 * _tmp6;
|
||||||
|
const Scalar _tmp8 = std::pow(state(2, 0), Scalar(2));
|
||||||
|
const Scalar _tmp9 = -2 * _tmp8;
|
||||||
|
const Scalar _tmp10 = _tmp7 + _tmp9 + 1;
|
||||||
|
const Scalar _tmp11 = -state(22, 0) + state(4, 0);
|
||||||
|
const Scalar _tmp12 = -state(23, 0) + state(5, 0);
|
||||||
|
const Scalar _tmp13 = _tmp2 * state(0, 0);
|
||||||
|
const Scalar _tmp14 = -_tmp13;
|
||||||
|
const Scalar _tmp15 = _tmp0 * state(1, 0);
|
||||||
|
const Scalar _tmp16 = _tmp14 + _tmp15;
|
||||||
|
const Scalar _tmp17 = _tmp12 * _tmp4 + _tmp16 * state(6, 0);
|
||||||
|
const Scalar _tmp18 = _tmp10 * _tmp11 + _tmp17;
|
||||||
|
const Scalar _tmp19 = 2 * _tmp18;
|
||||||
|
const Scalar _tmp20 = _tmp19 * _tmp4;
|
||||||
|
const Scalar _tmp21 = _tmp0 * state(2, 0);
|
||||||
|
const Scalar _tmp22 = 2 * state(0, 0) * state(1, 0);
|
||||||
|
const Scalar _tmp23 = -_tmp22;
|
||||||
|
const Scalar _tmp24 = _tmp21 + _tmp23;
|
||||||
|
const Scalar _tmp25 = std::pow(state(1, 0), Scalar(2));
|
||||||
|
const Scalar _tmp26 = 1 - 2 * _tmp25;
|
||||||
|
const Scalar _tmp27 = _tmp26 + _tmp9;
|
||||||
|
const Scalar _tmp28 = _tmp13 + _tmp15;
|
||||||
|
const Scalar _tmp29 = _tmp11 * _tmp28 + _tmp12 * _tmp24;
|
||||||
|
const Scalar _tmp30 = _tmp27 * state(6, 0) + _tmp29;
|
||||||
|
const Scalar _tmp31 = 2 * _tmp30;
|
||||||
|
const Scalar _tmp32 = _tmp24 * _tmp31;
|
||||||
|
const Scalar _tmp33 = _tmp26 + _tmp7;
|
||||||
|
const Scalar _tmp34 = -_tmp1;
|
||||||
|
const Scalar _tmp35 = _tmp3 + _tmp34;
|
||||||
|
const Scalar _tmp36 = _tmp21 + _tmp22;
|
||||||
|
const Scalar _tmp37 = _tmp11 * _tmp35 + _tmp36 * state(6, 0);
|
||||||
|
const Scalar _tmp38 = _tmp12 * _tmp33 + _tmp37;
|
||||||
|
const Scalar _tmp39 = 2 * _tmp38;
|
||||||
|
const Scalar _tmp40 = _tmp33 * _tmp39;
|
||||||
|
const Scalar _tmp41 = std::sqrt(Scalar(std::pow(_tmp18, Scalar(2)) + std::pow(_tmp30, Scalar(2)) +
|
||||||
|
std::pow(_tmp38, Scalar(2)) + epsilon));
|
||||||
|
const Scalar _tmp42 = cd * rho;
|
||||||
|
const Scalar _tmp43 = Scalar(0.25) * _tmp18 * _tmp42 / _tmp41;
|
||||||
|
const Scalar _tmp44 = Scalar(0.5) * _tmp41 * _tmp42;
|
||||||
|
const Scalar _tmp45 = _tmp4 * _tmp44;
|
||||||
|
const Scalar _tmp46 = -_tmp43 * (_tmp20 + _tmp32 + _tmp40) - _tmp45 - _tmp5;
|
||||||
|
const Scalar _tmp47 = -_tmp25;
|
||||||
|
const Scalar _tmp48 = _tmp47 + _tmp6;
|
||||||
|
const Scalar _tmp49 = std::pow(state(0, 0), Scalar(2));
|
||||||
|
const Scalar _tmp50 = -_tmp49;
|
||||||
|
const Scalar _tmp51 = _tmp50 + _tmp8;
|
||||||
|
const Scalar _tmp52 = -_tmp3;
|
||||||
|
const Scalar _tmp53 = -_tmp15;
|
||||||
|
const Scalar _tmp54 = -_tmp6;
|
||||||
|
const Scalar _tmp55 = _tmp12 * (_tmp47 + _tmp49 + _tmp54 + _tmp8) + _tmp37;
|
||||||
|
const Scalar _tmp56 = -_tmp43 * (_tmp19 * _tmp55 + _tmp39 * (_tmp11 * (_tmp48 + _tmp51) +
|
||||||
|
_tmp12 * (_tmp34 + _tmp52) +
|
||||||
|
state(6, 0) * (_tmp13 + _tmp53))) -
|
||||||
|
_tmp44 * _tmp55 - _tmp55 * cm;
|
||||||
|
const Scalar _tmp57 = -_tmp43 * (-_tmp20 - _tmp32 - _tmp40) + _tmp45 + _tmp5;
|
||||||
|
const Scalar _tmp58 = _tmp10 * cm;
|
||||||
|
const Scalar _tmp59 = _tmp10 * _tmp19;
|
||||||
|
const Scalar _tmp60 = _tmp28 * _tmp31;
|
||||||
|
const Scalar _tmp61 = _tmp35 * _tmp39;
|
||||||
|
const Scalar _tmp62 = _tmp10 * _tmp44;
|
||||||
|
const Scalar _tmp63 = -_tmp43 * (-_tmp59 - _tmp60 - _tmp61) + _tmp58 + _tmp62;
|
||||||
|
const Scalar _tmp64 = -_tmp8;
|
||||||
|
const Scalar _tmp65 = -_tmp21;
|
||||||
|
const Scalar _tmp66 = _tmp49 + _tmp64;
|
||||||
|
const Scalar _tmp67 =
|
||||||
|
_tmp43 * (_tmp31 * (_tmp11 * (_tmp1 + _tmp52) + _tmp12 * (_tmp25 + _tmp50 + _tmp6 + _tmp64) +
|
||||||
|
state(6, 0) * (_tmp23 + _tmp65)) +
|
||||||
|
_tmp39 * (_tmp29 + state(6, 0) * (_tmp48 + _tmp66)));
|
||||||
|
const Scalar _tmp68 = _tmp25 + _tmp54;
|
||||||
|
const Scalar _tmp69 =
|
||||||
|
_tmp11 * (_tmp14 + _tmp53) + _tmp12 * (_tmp22 + _tmp65) + state(6, 0) * (_tmp51 + _tmp68);
|
||||||
|
const Scalar _tmp70 =
|
||||||
|
-_tmp43 * (_tmp19 * _tmp69 + _tmp31 * (_tmp11 * (_tmp66 + _tmp68) + _tmp17)) -
|
||||||
|
_tmp44 * _tmp69 - _tmp69 * cm;
|
||||||
|
const Scalar _tmp71 = -_tmp43 * (_tmp59 + _tmp60 + _tmp61) - _tmp58 - _tmp62;
|
||||||
|
const Scalar _tmp72 = -_tmp16 * _tmp44 - _tmp16 * cm -
|
||||||
|
_tmp43 * (_tmp16 * _tmp19 + _tmp27 * _tmp31 + _tmp36 * _tmp39);
|
||||||
|
|
||||||
|
// Output terms (2)
|
||||||
|
if (innov_var != nullptr) {
|
||||||
|
Scalar& _innov_var = (*innov_var);
|
||||||
|
|
||||||
|
_innov_var =
|
||||||
|
R +
|
||||||
|
_tmp46 * (-P(0, 4) * _tmp67 + P(1, 4) * _tmp70 + P(2, 4) * _tmp56 + P(21, 4) * _tmp63 +
|
||||||
|
P(22, 4) * _tmp57 + P(3, 4) * _tmp71 + P(4, 4) * _tmp46 + P(5, 4) * _tmp72) +
|
||||||
|
_tmp56 * (-P(0, 2) * _tmp67 + P(1, 2) * _tmp70 + P(2, 2) * _tmp56 + P(21, 2) * _tmp63 +
|
||||||
|
P(22, 2) * _tmp57 + P(3, 2) * _tmp71 + P(4, 2) * _tmp46 + P(5, 2) * _tmp72) +
|
||||||
|
_tmp57 * (-P(0, 22) * _tmp67 + P(1, 22) * _tmp70 + P(2, 22) * _tmp56 + P(21, 22) * _tmp63 +
|
||||||
|
P(22, 22) * _tmp57 + P(3, 22) * _tmp71 + P(4, 22) * _tmp46 + P(5, 22) * _tmp72) +
|
||||||
|
_tmp63 * (-P(0, 21) * _tmp67 + P(1, 21) * _tmp70 + P(2, 21) * _tmp56 + P(21, 21) * _tmp63 +
|
||||||
|
P(22, 21) * _tmp57 + P(3, 21) * _tmp71 + P(4, 21) * _tmp46 + P(5, 21) * _tmp72) -
|
||||||
|
_tmp67 * (-P(0, 0) * _tmp67 + P(1, 0) * _tmp70 + P(2, 0) * _tmp56 + P(21, 0) * _tmp63 +
|
||||||
|
P(22, 0) * _tmp57 + P(3, 0) * _tmp71 + P(4, 0) * _tmp46 + P(5, 0) * _tmp72) +
|
||||||
|
_tmp70 * (-P(0, 1) * _tmp67 + P(1, 1) * _tmp70 + P(2, 1) * _tmp56 + P(21, 1) * _tmp63 +
|
||||||
|
P(22, 1) * _tmp57 + P(3, 1) * _tmp71 + P(4, 1) * _tmp46 + P(5, 1) * _tmp72) +
|
||||||
|
_tmp71 * (-P(0, 3) * _tmp67 + P(1, 3) * _tmp70 + P(2, 3) * _tmp56 + P(21, 3) * _tmp63 +
|
||||||
|
P(22, 3) * _tmp57 + P(3, 3) * _tmp71 + P(4, 3) * _tmp46 + P(5, 3) * _tmp72) +
|
||||||
|
_tmp72 * (-P(0, 5) * _tmp67 + P(1, 5) * _tmp70 + P(2, 5) * _tmp56 + P(21, 5) * _tmp63 +
|
||||||
|
P(22, 5) * _tmp57 + P(3, 5) * _tmp71 + P(4, 5) * _tmp46 + P(5, 5) * _tmp72);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (Hx != nullptr) {
|
||||||
|
matrix::Matrix<Scalar, 23, 1>& _hx = (*Hx);
|
||||||
|
|
||||||
|
_hx.setZero();
|
||||||
|
|
||||||
|
_hx(0, 0) = -_tmp67;
|
||||||
|
_hx(1, 0) = _tmp70;
|
||||||
|
_hx(2, 0) = _tmp56;
|
||||||
|
_hx(3, 0) = _tmp71;
|
||||||
|
_hx(4, 0) = _tmp46;
|
||||||
|
_hx(5, 0) = _tmp72;
|
||||||
|
_hx(21, 0) = _tmp63;
|
||||||
|
_hx(22, 0) = _tmp57;
|
||||||
|
}
|
||||||
|
} // NOLINT(readability/fn_size)
|
||||||
|
|
||||||
|
// NOLINTNEXTLINE(readability/fn_size)
|
||||||
|
} // namespace sym
|
||||||
|
|
@ -1,237 +0,0 @@
|
||||||
// -----------------------------------------------------------------------------
|
|
||||||
// This file was autogenerated by symforce from template:
|
|
||||||
// function/FUNCTION.h.jinja
|
|
||||||
// Do NOT modify by hand.
|
|
||||||
// -----------------------------------------------------------------------------
|
|
||||||
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <matrix/math.hpp>
|
|
||||||
|
|
||||||
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: Matrix23_23
|
|
||||||
* rho: Scalar
|
|
||||||
* cd: Scalar
|
|
||||||
* cm: Scalar
|
|
||||||
* R: Scalar
|
|
||||||
* epsilon: Scalar
|
|
||||||
*
|
|
||||||
* Outputs:
|
|
||||||
* innov_var: Scalar
|
|
||||||
* K: Matrix23_1
|
|
||||||
*/
|
|
||||||
template <typename Scalar>
|
|
||||||
void ComputeDragXInnovVarAndK(const matrix::Matrix<Scalar, 24, 1>& state,
|
|
||||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar rho,
|
|
||||||
const Scalar cd, const Scalar cm, const Scalar R,
|
|
||||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
|
||||||
matrix::Matrix<Scalar, 23, 1>* const K = nullptr) {
|
|
||||||
// Total ops: 679
|
|
||||||
|
|
||||||
// Input arrays
|
|
||||||
|
|
||||||
// Intermediate terms (83)
|
|
||||||
const Scalar _tmp0 = 2 * state(0, 0);
|
|
||||||
const Scalar _tmp1 = _tmp0 * state(3, 0);
|
|
||||||
const Scalar _tmp2 = 2 * state(1, 0);
|
|
||||||
const Scalar _tmp3 = _tmp2 * state(2, 0);
|
|
||||||
const Scalar _tmp4 = _tmp1 + _tmp3;
|
|
||||||
const Scalar _tmp5 = _tmp4 * cm;
|
|
||||||
const Scalar _tmp6 = std::pow(state(3, 0), Scalar(2));
|
|
||||||
const Scalar _tmp7 = -2 * _tmp6;
|
|
||||||
const Scalar _tmp8 = std::pow(state(2, 0), Scalar(2));
|
|
||||||
const Scalar _tmp9 = -2 * _tmp8;
|
|
||||||
const Scalar _tmp10 = _tmp7 + _tmp9 + 1;
|
|
||||||
const Scalar _tmp11 = -state(22, 0) + state(4, 0);
|
|
||||||
const Scalar _tmp12 = -state(23, 0) + state(5, 0);
|
|
||||||
const Scalar _tmp13 = _tmp0 * state(2, 0);
|
|
||||||
const Scalar _tmp14 = -_tmp13;
|
|
||||||
const Scalar _tmp15 = _tmp2 * state(3, 0);
|
|
||||||
const Scalar _tmp16 = _tmp14 + _tmp15;
|
|
||||||
const Scalar _tmp17 = _tmp12 * _tmp4 + _tmp16 * state(6, 0);
|
|
||||||
const Scalar _tmp18 = _tmp10 * _tmp11 + _tmp17;
|
|
||||||
const Scalar _tmp19 = 2 * _tmp18;
|
|
||||||
const Scalar _tmp20 = _tmp19 * _tmp4;
|
|
||||||
const Scalar _tmp21 = 2 * state(2, 0) * state(3, 0);
|
|
||||||
const Scalar _tmp22 = _tmp2 * state(0, 0);
|
|
||||||
const Scalar _tmp23 = -_tmp22;
|
|
||||||
const Scalar _tmp24 = _tmp21 + _tmp23;
|
|
||||||
const Scalar _tmp25 = std::pow(state(1, 0), Scalar(2));
|
|
||||||
const Scalar _tmp26 = 1 - 2 * _tmp25;
|
|
||||||
const Scalar _tmp27 = _tmp26 + _tmp9;
|
|
||||||
const Scalar _tmp28 = _tmp13 + _tmp15;
|
|
||||||
const Scalar _tmp29 = _tmp11 * _tmp28 + _tmp12 * _tmp24;
|
|
||||||
const Scalar _tmp30 = _tmp27 * state(6, 0) + _tmp29;
|
|
||||||
const Scalar _tmp31 = 2 * _tmp30;
|
|
||||||
const Scalar _tmp32 = _tmp24 * _tmp31;
|
|
||||||
const Scalar _tmp33 = _tmp26 + _tmp7;
|
|
||||||
const Scalar _tmp34 = -_tmp1;
|
|
||||||
const Scalar _tmp35 = _tmp3 + _tmp34;
|
|
||||||
const Scalar _tmp36 = _tmp21 + _tmp22;
|
|
||||||
const Scalar _tmp37 = _tmp11 * _tmp35 + _tmp36 * state(6, 0);
|
|
||||||
const Scalar _tmp38 = _tmp12 * _tmp33 + _tmp37;
|
|
||||||
const Scalar _tmp39 = 2 * _tmp38;
|
|
||||||
const Scalar _tmp40 = _tmp33 * _tmp39;
|
|
||||||
const Scalar _tmp41 = std::sqrt(Scalar(std::pow(_tmp18, Scalar(2)) + std::pow(_tmp30, Scalar(2)) +
|
|
||||||
std::pow(_tmp38, Scalar(2)) + epsilon));
|
|
||||||
const Scalar _tmp42 = cd * rho;
|
|
||||||
const Scalar _tmp43 = Scalar(0.25) * _tmp18 * _tmp42 / _tmp41;
|
|
||||||
const Scalar _tmp44 = Scalar(0.5) * _tmp41 * _tmp42;
|
|
||||||
const Scalar _tmp45 = _tmp4 * _tmp44;
|
|
||||||
const Scalar _tmp46 = -_tmp43 * (_tmp20 + _tmp32 + _tmp40) - _tmp45 - _tmp5;
|
|
||||||
const Scalar _tmp47 = -_tmp25;
|
|
||||||
const Scalar _tmp48 = _tmp47 + _tmp8;
|
|
||||||
const Scalar _tmp49 = std::pow(state(0, 0), Scalar(2));
|
|
||||||
const Scalar _tmp50 = -_tmp49;
|
|
||||||
const Scalar _tmp51 = _tmp50 + _tmp6;
|
|
||||||
const Scalar _tmp52 = -_tmp3;
|
|
||||||
const Scalar _tmp53 = -_tmp15;
|
|
||||||
const Scalar _tmp54 = -_tmp6;
|
|
||||||
const Scalar _tmp55 = _tmp49 + _tmp54;
|
|
||||||
const Scalar _tmp56 = _tmp12 * (_tmp48 + _tmp55) + _tmp37;
|
|
||||||
const Scalar _tmp57 = -_tmp43 * (_tmp19 * _tmp56 + _tmp39 * (_tmp11 * (_tmp48 + _tmp51) +
|
|
||||||
_tmp12 * (_tmp34 + _tmp52) +
|
|
||||||
state(6, 0) * (_tmp13 + _tmp53))) -
|
|
||||||
_tmp44 * _tmp56 - _tmp56 * cm;
|
|
||||||
const Scalar _tmp58 = -_tmp43 * (-_tmp20 - _tmp32 - _tmp40) + _tmp45 + _tmp5;
|
|
||||||
const Scalar _tmp59 = P(4, 4) * _tmp46;
|
|
||||||
const Scalar _tmp60 = _tmp10 * cm;
|
|
||||||
const Scalar _tmp61 = _tmp10 * _tmp19;
|
|
||||||
const Scalar _tmp62 = _tmp28 * _tmp31;
|
|
||||||
const Scalar _tmp63 = _tmp35 * _tmp39;
|
|
||||||
const Scalar _tmp64 = _tmp10 * _tmp44;
|
|
||||||
const Scalar _tmp65 = -_tmp43 * (-_tmp61 - _tmp62 - _tmp63) + _tmp60 + _tmp64;
|
|
||||||
const Scalar _tmp66 = -_tmp8;
|
|
||||||
const Scalar _tmp67 = _tmp25 + _tmp66;
|
|
||||||
const Scalar _tmp68 = -_tmp21;
|
|
||||||
const Scalar _tmp69 =
|
|
||||||
_tmp43 * (_tmp31 * (_tmp11 * (_tmp1 + _tmp52) + _tmp12 * (_tmp51 + _tmp67) +
|
|
||||||
state(6, 0) * (_tmp23 + _tmp68)) +
|
|
||||||
_tmp39 * (_tmp29 + state(6, 0) * (_tmp47 + _tmp49 + _tmp6 + _tmp66)));
|
|
||||||
const Scalar _tmp70 = _tmp11 * (_tmp14 + _tmp53) + _tmp12 * (_tmp22 + _tmp68) +
|
|
||||||
state(6, 0) * (_tmp25 + _tmp50 + _tmp54 + _tmp8);
|
|
||||||
const Scalar _tmp71 =
|
|
||||||
-_tmp43 * (_tmp19 * _tmp70 + _tmp31 * (_tmp11 * (_tmp55 + _tmp67) + _tmp17)) -
|
|
||||||
_tmp44 * _tmp70 - _tmp70 * cm;
|
|
||||||
const Scalar _tmp72 = -_tmp43 * (_tmp61 + _tmp62 + _tmp63) - _tmp60 - _tmp64;
|
|
||||||
const Scalar _tmp73 = -_tmp16 * _tmp44 - _tmp16 * cm -
|
|
||||||
_tmp43 * (_tmp16 * _tmp19 + _tmp27 * _tmp31 + _tmp36 * _tmp39);
|
|
||||||
const Scalar _tmp74 = -P(0, 0) * _tmp69;
|
|
||||||
const Scalar _tmp75 = P(21, 21) * _tmp65;
|
|
||||||
const Scalar _tmp76 = P(2, 2) * _tmp57;
|
|
||||||
const Scalar _tmp77 = P(1, 1) * _tmp71;
|
|
||||||
const Scalar _tmp78 = P(5, 5) * _tmp73;
|
|
||||||
const Scalar _tmp79 = P(3, 3) * _tmp72;
|
|
||||||
const Scalar _tmp80 = P(22, 22) * _tmp58;
|
|
||||||
const Scalar _tmp81 =
|
|
||||||
R +
|
|
||||||
_tmp46 * (-P(0, 4) * _tmp69 + P(1, 4) * _tmp71 + P(2, 4) * _tmp57 + P(21, 4) * _tmp65 +
|
|
||||||
P(22, 4) * _tmp58 + P(3, 4) * _tmp72 + P(5, 4) * _tmp73 + _tmp59) +
|
|
||||||
_tmp57 * (-P(0, 2) * _tmp69 + P(1, 2) * _tmp71 + P(21, 2) * _tmp65 + P(22, 2) * _tmp58 +
|
|
||||||
P(3, 2) * _tmp72 + P(4, 2) * _tmp46 + P(5, 2) * _tmp73 + _tmp76) +
|
|
||||||
_tmp58 * (-P(0, 22) * _tmp69 + P(1, 22) * _tmp71 + P(2, 22) * _tmp57 + P(21, 22) * _tmp65 +
|
|
||||||
P(3, 22) * _tmp72 + P(4, 22) * _tmp46 + P(5, 22) * _tmp73 + _tmp80) +
|
|
||||||
_tmp65 * (-P(0, 21) * _tmp69 + P(1, 21) * _tmp71 + P(2, 21) * _tmp57 + P(22, 21) * _tmp58 +
|
|
||||||
P(3, 21) * _tmp72 + P(4, 21) * _tmp46 + P(5, 21) * _tmp73 + _tmp75) -
|
|
||||||
_tmp69 * (P(1, 0) * _tmp71 + P(2, 0) * _tmp57 + P(21, 0) * _tmp65 + P(22, 0) * _tmp58 +
|
|
||||||
P(3, 0) * _tmp72 + P(4, 0) * _tmp46 + P(5, 0) * _tmp73 + _tmp74) +
|
|
||||||
_tmp71 * (-P(0, 1) * _tmp69 + P(2, 1) * _tmp57 + P(21, 1) * _tmp65 + P(22, 1) * _tmp58 +
|
|
||||||
P(3, 1) * _tmp72 + P(4, 1) * _tmp46 + P(5, 1) * _tmp73 + _tmp77) +
|
|
||||||
_tmp72 * (-P(0, 3) * _tmp69 + P(1, 3) * _tmp71 + P(2, 3) * _tmp57 + P(21, 3) * _tmp65 +
|
|
||||||
P(22, 3) * _tmp58 + P(4, 3) * _tmp46 + P(5, 3) * _tmp73 + _tmp79) +
|
|
||||||
_tmp73 * (-P(0, 5) * _tmp69 + P(1, 5) * _tmp71 + P(2, 5) * _tmp57 + P(21, 5) * _tmp65 +
|
|
||||||
P(22, 5) * _tmp58 + P(3, 5) * _tmp72 + P(4, 5) * _tmp46 + _tmp78);
|
|
||||||
const Scalar _tmp82 = Scalar(1.0) / (math::max<Scalar>(_tmp81, epsilon));
|
|
||||||
|
|
||||||
// Output terms (2)
|
|
||||||
if (innov_var != nullptr) {
|
|
||||||
Scalar& _innov_var = (*innov_var);
|
|
||||||
|
|
||||||
_innov_var = _tmp81;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (K != nullptr) {
|
|
||||||
matrix::Matrix<Scalar, 23, 1>& _k = (*K);
|
|
||||||
|
|
||||||
_k(0, 0) =
|
|
||||||
_tmp82 * (P(0, 1) * _tmp71 + P(0, 2) * _tmp57 + P(0, 21) * _tmp65 + P(0, 22) * _tmp58 +
|
|
||||||
P(0, 3) * _tmp72 + P(0, 4) * _tmp46 + P(0, 5) * _tmp73 + _tmp74);
|
|
||||||
_k(1, 0) =
|
|
||||||
_tmp82 * (-P(1, 0) * _tmp69 + P(1, 2) * _tmp57 + P(1, 21) * _tmp65 + P(1, 22) * _tmp58 +
|
|
||||||
P(1, 3) * _tmp72 + P(1, 4) * _tmp46 + P(1, 5) * _tmp73 + _tmp77);
|
|
||||||
_k(2, 0) =
|
|
||||||
_tmp82 * (-P(2, 0) * _tmp69 + P(2, 1) * _tmp71 + P(2, 21) * _tmp65 + P(2, 22) * _tmp58 +
|
|
||||||
P(2, 3) * _tmp72 + P(2, 4) * _tmp46 + P(2, 5) * _tmp73 + _tmp76);
|
|
||||||
_k(3, 0) =
|
|
||||||
_tmp82 * (-P(3, 0) * _tmp69 + P(3, 1) * _tmp71 + P(3, 2) * _tmp57 + P(3, 21) * _tmp65 +
|
|
||||||
P(3, 22) * _tmp58 + P(3, 4) * _tmp46 + P(3, 5) * _tmp73 + _tmp79);
|
|
||||||
_k(4, 0) =
|
|
||||||
_tmp82 * (-P(4, 0) * _tmp69 + P(4, 1) * _tmp71 + P(4, 2) * _tmp57 + P(4, 21) * _tmp65 +
|
|
||||||
P(4, 22) * _tmp58 + P(4, 3) * _tmp72 + P(4, 5) * _tmp73 + _tmp59);
|
|
||||||
_k(5, 0) =
|
|
||||||
_tmp82 * (-P(5, 0) * _tmp69 + P(5, 1) * _tmp71 + P(5, 2) * _tmp57 + P(5, 21) * _tmp65 +
|
|
||||||
P(5, 22) * _tmp58 + P(5, 3) * _tmp72 + P(5, 4) * _tmp46 + _tmp78);
|
|
||||||
_k(6, 0) =
|
|
||||||
_tmp82 * (-P(6, 0) * _tmp69 + P(6, 1) * _tmp71 + P(6, 2) * _tmp57 + P(6, 21) * _tmp65 +
|
|
||||||
P(6, 22) * _tmp58 + P(6, 3) * _tmp72 + P(6, 4) * _tmp46 + P(6, 5) * _tmp73);
|
|
||||||
_k(7, 0) =
|
|
||||||
_tmp82 * (-P(7, 0) * _tmp69 + P(7, 1) * _tmp71 + P(7, 2) * _tmp57 + P(7, 21) * _tmp65 +
|
|
||||||
P(7, 22) * _tmp58 + P(7, 3) * _tmp72 + P(7, 4) * _tmp46 + P(7, 5) * _tmp73);
|
|
||||||
_k(8, 0) =
|
|
||||||
_tmp82 * (-P(8, 0) * _tmp69 + P(8, 1) * _tmp71 + P(8, 2) * _tmp57 + P(8, 21) * _tmp65 +
|
|
||||||
P(8, 22) * _tmp58 + P(8, 3) * _tmp72 + P(8, 4) * _tmp46 + P(8, 5) * _tmp73);
|
|
||||||
_k(9, 0) =
|
|
||||||
_tmp82 * (-P(9, 0) * _tmp69 + P(9, 1) * _tmp71 + P(9, 2) * _tmp57 + P(9, 21) * _tmp65 +
|
|
||||||
P(9, 22) * _tmp58 + P(9, 3) * _tmp72 + P(9, 4) * _tmp46 + P(9, 5) * _tmp73);
|
|
||||||
_k(10, 0) =
|
|
||||||
_tmp82 * (-P(10, 0) * _tmp69 + P(10, 1) * _tmp71 + P(10, 2) * _tmp57 + P(10, 21) * _tmp65 +
|
|
||||||
P(10, 22) * _tmp58 + P(10, 3) * _tmp72 + P(10, 4) * _tmp46 + P(10, 5) * _tmp73);
|
|
||||||
_k(11, 0) =
|
|
||||||
_tmp82 * (-P(11, 0) * _tmp69 + P(11, 1) * _tmp71 + P(11, 2) * _tmp57 + P(11, 21) * _tmp65 +
|
|
||||||
P(11, 22) * _tmp58 + P(11, 3) * _tmp72 + P(11, 4) * _tmp46 + P(11, 5) * _tmp73);
|
|
||||||
_k(12, 0) =
|
|
||||||
_tmp82 * (-P(12, 0) * _tmp69 + P(12, 1) * _tmp71 + P(12, 2) * _tmp57 + P(12, 21) * _tmp65 +
|
|
||||||
P(12, 22) * _tmp58 + P(12, 3) * _tmp72 + P(12, 4) * _tmp46 + P(12, 5) * _tmp73);
|
|
||||||
_k(13, 0) =
|
|
||||||
_tmp82 * (-P(13, 0) * _tmp69 + P(13, 1) * _tmp71 + P(13, 2) * _tmp57 + P(13, 21) * _tmp65 +
|
|
||||||
P(13, 22) * _tmp58 + P(13, 3) * _tmp72 + P(13, 4) * _tmp46 + P(13, 5) * _tmp73);
|
|
||||||
_k(14, 0) =
|
|
||||||
_tmp82 * (-P(14, 0) * _tmp69 + P(14, 1) * _tmp71 + P(14, 2) * _tmp57 + P(14, 21) * _tmp65 +
|
|
||||||
P(14, 22) * _tmp58 + P(14, 3) * _tmp72 + P(14, 4) * _tmp46 + P(14, 5) * _tmp73);
|
|
||||||
_k(15, 0) =
|
|
||||||
_tmp82 * (-P(15, 0) * _tmp69 + P(15, 1) * _tmp71 + P(15, 2) * _tmp57 + P(15, 21) * _tmp65 +
|
|
||||||
P(15, 22) * _tmp58 + P(15, 3) * _tmp72 + P(15, 4) * _tmp46 + P(15, 5) * _tmp73);
|
|
||||||
_k(16, 0) =
|
|
||||||
_tmp82 * (-P(16, 0) * _tmp69 + P(16, 1) * _tmp71 + P(16, 2) * _tmp57 + P(16, 21) * _tmp65 +
|
|
||||||
P(16, 22) * _tmp58 + P(16, 3) * _tmp72 + P(16, 4) * _tmp46 + P(16, 5) * _tmp73);
|
|
||||||
_k(17, 0) =
|
|
||||||
_tmp82 * (-P(17, 0) * _tmp69 + P(17, 1) * _tmp71 + P(17, 2) * _tmp57 + P(17, 21) * _tmp65 +
|
|
||||||
P(17, 22) * _tmp58 + P(17, 3) * _tmp72 + P(17, 4) * _tmp46 + P(17, 5) * _tmp73);
|
|
||||||
_k(18, 0) =
|
|
||||||
_tmp82 * (-P(18, 0) * _tmp69 + P(18, 1) * _tmp71 + P(18, 2) * _tmp57 + P(18, 21) * _tmp65 +
|
|
||||||
P(18, 22) * _tmp58 + P(18, 3) * _tmp72 + P(18, 4) * _tmp46 + P(18, 5) * _tmp73);
|
|
||||||
_k(19, 0) =
|
|
||||||
_tmp82 * (-P(19, 0) * _tmp69 + P(19, 1) * _tmp71 + P(19, 2) * _tmp57 + P(19, 21) * _tmp65 +
|
|
||||||
P(19, 22) * _tmp58 + P(19, 3) * _tmp72 + P(19, 4) * _tmp46 + P(19, 5) * _tmp73);
|
|
||||||
_k(20, 0) =
|
|
||||||
_tmp82 * (-P(20, 0) * _tmp69 + P(20, 1) * _tmp71 + P(20, 2) * _tmp57 + P(20, 21) * _tmp65 +
|
|
||||||
P(20, 22) * _tmp58 + P(20, 3) * _tmp72 + P(20, 4) * _tmp46 + P(20, 5) * _tmp73);
|
|
||||||
_k(21, 0) =
|
|
||||||
_tmp82 * (-P(21, 0) * _tmp69 + P(21, 1) * _tmp71 + P(21, 2) * _tmp57 + P(21, 22) * _tmp58 +
|
|
||||||
P(21, 3) * _tmp72 + P(21, 4) * _tmp46 + P(21, 5) * _tmp73 + _tmp75);
|
|
||||||
_k(22, 0) =
|
|
||||||
_tmp82 * (-P(22, 0) * _tmp69 + P(22, 1) * _tmp71 + P(22, 2) * _tmp57 + P(22, 21) * _tmp65 +
|
|
||||||
P(22, 3) * _tmp72 + P(22, 4) * _tmp46 + P(22, 5) * _tmp73 + _tmp80);
|
|
||||||
}
|
|
||||||
} // NOLINT(readability/fn_size)
|
|
||||||
|
|
||||||
// NOLINTNEXTLINE(readability/fn_size)
|
|
||||||
} // namespace sym
|
|
||||||
|
|
@ -0,0 +1,170 @@
|
||||||
|
// -----------------------------------------------------------------------------
|
||||||
|
// This file was autogenerated by symforce from template:
|
||||||
|
// function/FUNCTION.h.jinja
|
||||||
|
// Do NOT modify by hand.
|
||||||
|
// -----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <matrix/math.hpp>
|
||||||
|
|
||||||
|
namespace sym {
|
||||||
|
|
||||||
|
/**
|
||||||
|
* This function was autogenerated from a symbolic function. Do not modify by hand.
|
||||||
|
*
|
||||||
|
* Symbolic function: compute_drag_y_innov_var_and_h
|
||||||
|
*
|
||||||
|
* Args:
|
||||||
|
* state: Matrix24_1
|
||||||
|
* P: Matrix23_23
|
||||||
|
* rho: Scalar
|
||||||
|
* cd: Scalar
|
||||||
|
* cm: Scalar
|
||||||
|
* R: Scalar
|
||||||
|
* epsilon: Scalar
|
||||||
|
*
|
||||||
|
* Outputs:
|
||||||
|
* innov_var: Scalar
|
||||||
|
* Hy: Matrix23_1
|
||||||
|
*/
|
||||||
|
template <typename Scalar>
|
||||||
|
void ComputeDragYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||||
|
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar rho,
|
||||||
|
const Scalar cd, const Scalar cm, const Scalar R,
|
||||||
|
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||||
|
matrix::Matrix<Scalar, 23, 1>* const Hy = nullptr) {
|
||||||
|
// Total ops: 317
|
||||||
|
|
||||||
|
// Input arrays
|
||||||
|
|
||||||
|
// Intermediate terms (73)
|
||||||
|
const Scalar _tmp0 = 2 * state(2, 0) * state(3, 0);
|
||||||
|
const Scalar _tmp1 = 2 * state(1, 0);
|
||||||
|
const Scalar _tmp2 = _tmp1 * state(0, 0);
|
||||||
|
const Scalar _tmp3 = _tmp0 + _tmp2;
|
||||||
|
const Scalar _tmp4 = std::pow(state(3, 0), Scalar(2));
|
||||||
|
const Scalar _tmp5 = -2 * _tmp4;
|
||||||
|
const Scalar _tmp6 = std::pow(state(2, 0), Scalar(2));
|
||||||
|
const Scalar _tmp7 = -2 * _tmp6;
|
||||||
|
const Scalar _tmp8 = _tmp5 + _tmp7 + 1;
|
||||||
|
const Scalar _tmp9 = -state(22, 0) + state(4, 0);
|
||||||
|
const Scalar _tmp10 = 2 * state(0, 0);
|
||||||
|
const Scalar _tmp11 = _tmp10 * state(3, 0);
|
||||||
|
const Scalar _tmp12 = _tmp1 * state(2, 0);
|
||||||
|
const Scalar _tmp13 = _tmp11 + _tmp12;
|
||||||
|
const Scalar _tmp14 = -state(23, 0) + state(5, 0);
|
||||||
|
const Scalar _tmp15 = _tmp10 * state(2, 0);
|
||||||
|
const Scalar _tmp16 = -_tmp15;
|
||||||
|
const Scalar _tmp17 = _tmp1 * state(3, 0);
|
||||||
|
const Scalar _tmp18 = _tmp16 + _tmp17;
|
||||||
|
const Scalar _tmp19 = _tmp13 * _tmp14 + _tmp18 * state(6, 0);
|
||||||
|
const Scalar _tmp20 = _tmp19 + _tmp8 * _tmp9;
|
||||||
|
const Scalar _tmp21 = std::pow(state(1, 0), Scalar(2));
|
||||||
|
const Scalar _tmp22 = 1 - 2 * _tmp21;
|
||||||
|
const Scalar _tmp23 = _tmp22 + _tmp7;
|
||||||
|
const Scalar _tmp24 = -_tmp2;
|
||||||
|
const Scalar _tmp25 = _tmp0 + _tmp24;
|
||||||
|
const Scalar _tmp26 = _tmp15 + _tmp17;
|
||||||
|
const Scalar _tmp27 = _tmp14 * _tmp25 + _tmp26 * _tmp9;
|
||||||
|
const Scalar _tmp28 = _tmp23 * state(6, 0) + _tmp27;
|
||||||
|
const Scalar _tmp29 = _tmp22 + _tmp5;
|
||||||
|
const Scalar _tmp30 = -_tmp11;
|
||||||
|
const Scalar _tmp31 = _tmp12 + _tmp30;
|
||||||
|
const Scalar _tmp32 = _tmp3 * state(6, 0) + _tmp31 * _tmp9;
|
||||||
|
const Scalar _tmp33 = _tmp14 * _tmp29 + _tmp32;
|
||||||
|
const Scalar _tmp34 = std::sqrt(Scalar(std::pow(_tmp20, Scalar(2)) + std::pow(_tmp28, Scalar(2)) +
|
||||||
|
std::pow(_tmp33, Scalar(2)) + epsilon));
|
||||||
|
const Scalar _tmp35 = cd * rho;
|
||||||
|
const Scalar _tmp36 = Scalar(0.5) * _tmp34 * _tmp35;
|
||||||
|
const Scalar _tmp37 = 2 * _tmp20;
|
||||||
|
const Scalar _tmp38 = 2 * _tmp28;
|
||||||
|
const Scalar _tmp39 = 2 * _tmp33;
|
||||||
|
const Scalar _tmp40 = Scalar(0.25) * _tmp33 * _tmp35 / _tmp34;
|
||||||
|
const Scalar _tmp41 =
|
||||||
|
-_tmp3 * _tmp36 - _tmp3 * cm - _tmp40 * (_tmp18 * _tmp37 + _tmp23 * _tmp38 + _tmp3 * _tmp39);
|
||||||
|
const Scalar _tmp42 = std::pow(state(0, 0), Scalar(2));
|
||||||
|
const Scalar _tmp43 = -_tmp42;
|
||||||
|
const Scalar _tmp44 = -_tmp6;
|
||||||
|
const Scalar _tmp45 = -_tmp12;
|
||||||
|
const Scalar _tmp46 = -_tmp0;
|
||||||
|
const Scalar _tmp47 = -_tmp21;
|
||||||
|
const Scalar _tmp48 = _tmp4 + _tmp47;
|
||||||
|
const Scalar _tmp49 = _tmp42 + _tmp44;
|
||||||
|
const Scalar _tmp50 = _tmp27 + state(6, 0) * (_tmp48 + _tmp49);
|
||||||
|
const Scalar _tmp51 =
|
||||||
|
-_tmp36 * _tmp50 -
|
||||||
|
_tmp40 * (_tmp38 * (_tmp14 * (_tmp21 + _tmp4 + _tmp43 + _tmp44) + _tmp9 * (_tmp11 + _tmp45) +
|
||||||
|
state(6, 0) * (_tmp24 + _tmp46)) +
|
||||||
|
_tmp39 * _tmp50) -
|
||||||
|
_tmp50 * cm;
|
||||||
|
const Scalar _tmp52 = _tmp43 + _tmp6;
|
||||||
|
const Scalar _tmp53 = -_tmp17;
|
||||||
|
const Scalar _tmp54 =
|
||||||
|
_tmp14 * (_tmp30 + _tmp45) + _tmp9 * (_tmp48 + _tmp52) + state(6, 0) * (_tmp15 + _tmp53);
|
||||||
|
const Scalar _tmp55 = -_tmp4;
|
||||||
|
const Scalar _tmp56 =
|
||||||
|
-_tmp36 * _tmp54 -
|
||||||
|
_tmp40 * (_tmp37 * (_tmp14 * (_tmp42 + _tmp47 + _tmp55 + _tmp6) + _tmp32) + _tmp39 * _tmp54) -
|
||||||
|
_tmp54 * cm;
|
||||||
|
const Scalar _tmp57 = _tmp29 * cm;
|
||||||
|
const Scalar _tmp58 = _tmp13 * _tmp37;
|
||||||
|
const Scalar _tmp59 = _tmp25 * _tmp38;
|
||||||
|
const Scalar _tmp60 = _tmp29 * _tmp39;
|
||||||
|
const Scalar _tmp61 = _tmp29 * _tmp36;
|
||||||
|
const Scalar _tmp62 = -_tmp40 * (-_tmp58 - _tmp59 - _tmp60) + _tmp57 + _tmp61;
|
||||||
|
const Scalar _tmp63 = _tmp21 + _tmp55;
|
||||||
|
const Scalar _tmp64 = _tmp40 * (_tmp37 * (_tmp14 * (_tmp2 + _tmp46) + _tmp9 * (_tmp16 + _tmp53) +
|
||||||
|
state(6, 0) * (_tmp52 + _tmp63)) +
|
||||||
|
_tmp38 * (_tmp19 + _tmp9 * (_tmp49 + _tmp63)));
|
||||||
|
const Scalar _tmp65 = -_tmp40 * (_tmp58 + _tmp59 + _tmp60) - _tmp57 - _tmp61;
|
||||||
|
const Scalar _tmp66 = _tmp31 * cm;
|
||||||
|
const Scalar _tmp67 = _tmp31 * _tmp36;
|
||||||
|
const Scalar _tmp68 = _tmp37 * _tmp8;
|
||||||
|
const Scalar _tmp69 = _tmp26 * _tmp38;
|
||||||
|
const Scalar _tmp70 = _tmp31 * _tmp39;
|
||||||
|
const Scalar _tmp71 = -_tmp40 * (_tmp68 + _tmp69 + _tmp70) - _tmp66 - _tmp67;
|
||||||
|
const Scalar _tmp72 = -_tmp40 * (-_tmp68 - _tmp69 - _tmp70) + _tmp66 + _tmp67;
|
||||||
|
|
||||||
|
// Output terms (2)
|
||||||
|
if (innov_var != nullptr) {
|
||||||
|
Scalar& _innov_var = (*innov_var);
|
||||||
|
|
||||||
|
_innov_var =
|
||||||
|
R +
|
||||||
|
_tmp41 * (P(0, 5) * _tmp51 - P(1, 5) * _tmp64 + P(2, 5) * _tmp56 + P(21, 5) * _tmp72 +
|
||||||
|
P(22, 5) * _tmp62 + P(3, 5) * _tmp71 + P(4, 5) * _tmp65 + P(5, 5) * _tmp41) +
|
||||||
|
_tmp51 * (P(0, 0) * _tmp51 - P(1, 0) * _tmp64 + P(2, 0) * _tmp56 + P(21, 0) * _tmp72 +
|
||||||
|
P(22, 0) * _tmp62 + P(3, 0) * _tmp71 + P(4, 0) * _tmp65 + P(5, 0) * _tmp41) +
|
||||||
|
_tmp56 * (P(0, 2) * _tmp51 - P(1, 2) * _tmp64 + P(2, 2) * _tmp56 + P(21, 2) * _tmp72 +
|
||||||
|
P(22, 2) * _tmp62 + P(3, 2) * _tmp71 + P(4, 2) * _tmp65 + P(5, 2) * _tmp41) +
|
||||||
|
_tmp62 * (P(0, 22) * _tmp51 - P(1, 22) * _tmp64 + P(2, 22) * _tmp56 + P(21, 22) * _tmp72 +
|
||||||
|
P(22, 22) * _tmp62 + P(3, 22) * _tmp71 + P(4, 22) * _tmp65 + P(5, 22) * _tmp41) -
|
||||||
|
_tmp64 * (P(0, 1) * _tmp51 - P(1, 1) * _tmp64 + P(2, 1) * _tmp56 + P(21, 1) * _tmp72 +
|
||||||
|
P(22, 1) * _tmp62 + P(3, 1) * _tmp71 + P(4, 1) * _tmp65 + P(5, 1) * _tmp41) +
|
||||||
|
_tmp65 * (P(0, 4) * _tmp51 - P(1, 4) * _tmp64 + P(2, 4) * _tmp56 + P(21, 4) * _tmp72 +
|
||||||
|
P(22, 4) * _tmp62 + P(3, 4) * _tmp71 + P(4, 4) * _tmp65 + P(5, 4) * _tmp41) +
|
||||||
|
_tmp71 * (P(0, 3) * _tmp51 - P(1, 3) * _tmp64 + P(2, 3) * _tmp56 + P(21, 3) * _tmp72 +
|
||||||
|
P(22, 3) * _tmp62 + P(3, 3) * _tmp71 + P(4, 3) * _tmp65 + P(5, 3) * _tmp41) +
|
||||||
|
_tmp72 * (P(0, 21) * _tmp51 - P(1, 21) * _tmp64 + P(2, 21) * _tmp56 + P(21, 21) * _tmp72 +
|
||||||
|
P(22, 21) * _tmp62 + P(3, 21) * _tmp71 + P(4, 21) * _tmp65 + P(5, 21) * _tmp41);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (Hy != nullptr) {
|
||||||
|
matrix::Matrix<Scalar, 23, 1>& _hy = (*Hy);
|
||||||
|
|
||||||
|
_hy.setZero();
|
||||||
|
|
||||||
|
_hy(0, 0) = _tmp51;
|
||||||
|
_hy(1, 0) = -_tmp64;
|
||||||
|
_hy(2, 0) = _tmp56;
|
||||||
|
_hy(3, 0) = _tmp71;
|
||||||
|
_hy(4, 0) = _tmp65;
|
||||||
|
_hy(5, 0) = _tmp41;
|
||||||
|
_hy(21, 0) = _tmp72;
|
||||||
|
_hy(22, 0) = _tmp62;
|
||||||
|
}
|
||||||
|
} // NOLINT(readability/fn_size)
|
||||||
|
|
||||||
|
// NOLINTNEXTLINE(readability/fn_size)
|
||||||
|
} // namespace sym
|
||||||
|
|
@ -1,239 +0,0 @@
|
||||||
// -----------------------------------------------------------------------------
|
|
||||||
// This file was autogenerated by symforce from template:
|
|
||||||
// function/FUNCTION.h.jinja
|
|
||||||
// Do NOT modify by hand.
|
|
||||||
// -----------------------------------------------------------------------------
|
|
||||||
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <matrix/math.hpp>
|
|
||||||
|
|
||||||
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: Matrix23_23
|
|
||||||
* rho: Scalar
|
|
||||||
* cd: Scalar
|
|
||||||
* cm: Scalar
|
|
||||||
* R: Scalar
|
|
||||||
* epsilon: Scalar
|
|
||||||
*
|
|
||||||
* Outputs:
|
|
||||||
* innov_var: Scalar
|
|
||||||
* K: Matrix23_1
|
|
||||||
*/
|
|
||||||
template <typename Scalar>
|
|
||||||
void ComputeDragYInnovVarAndK(const matrix::Matrix<Scalar, 24, 1>& state,
|
|
||||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar rho,
|
|
||||||
const Scalar cd, const Scalar cm, const Scalar R,
|
|
||||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
|
||||||
matrix::Matrix<Scalar, 23, 1>* const K = nullptr) {
|
|
||||||
// Total ops: 679
|
|
||||||
|
|
||||||
// Input arrays
|
|
||||||
|
|
||||||
// Intermediate terms (83)
|
|
||||||
const Scalar _tmp0 = 2 * state(3, 0);
|
|
||||||
const Scalar _tmp1 = _tmp0 * state(2, 0);
|
|
||||||
const Scalar _tmp2 = 2 * state(1, 0);
|
|
||||||
const Scalar _tmp3 = _tmp2 * state(0, 0);
|
|
||||||
const Scalar _tmp4 = _tmp1 + _tmp3;
|
|
||||||
const Scalar _tmp5 = std::pow(state(2, 0), Scalar(2));
|
|
||||||
const Scalar _tmp6 = -2 * _tmp5;
|
|
||||||
const Scalar _tmp7 = std::pow(state(3, 0), Scalar(2));
|
|
||||||
const Scalar _tmp8 = 1 - 2 * _tmp7;
|
|
||||||
const Scalar _tmp9 = _tmp6 + _tmp8;
|
|
||||||
const Scalar _tmp10 = -state(22, 0) + state(4, 0);
|
|
||||||
const Scalar _tmp11 = _tmp0 * state(0, 0);
|
|
||||||
const Scalar _tmp12 = _tmp2 * state(2, 0);
|
|
||||||
const Scalar _tmp13 = _tmp11 + _tmp12;
|
|
||||||
const Scalar _tmp14 = -state(23, 0) + state(5, 0);
|
|
||||||
const Scalar _tmp15 = 2 * state(0, 0) * state(2, 0);
|
|
||||||
const Scalar _tmp16 = -_tmp15;
|
|
||||||
const Scalar _tmp17 = _tmp2 * state(3, 0);
|
|
||||||
const Scalar _tmp18 = _tmp16 + _tmp17;
|
|
||||||
const Scalar _tmp19 = _tmp13 * _tmp14 + _tmp18 * state(6, 0);
|
|
||||||
const Scalar _tmp20 = _tmp10 * _tmp9 + _tmp19;
|
|
||||||
const Scalar _tmp21 = std::pow(state(1, 0), Scalar(2));
|
|
||||||
const Scalar _tmp22 = -2 * _tmp21;
|
|
||||||
const Scalar _tmp23 = _tmp22 + _tmp6 + 1;
|
|
||||||
const Scalar _tmp24 = -_tmp3;
|
|
||||||
const Scalar _tmp25 = _tmp1 + _tmp24;
|
|
||||||
const Scalar _tmp26 = _tmp15 + _tmp17;
|
|
||||||
const Scalar _tmp27 = _tmp10 * _tmp26 + _tmp14 * _tmp25;
|
|
||||||
const Scalar _tmp28 = _tmp23 * state(6, 0) + _tmp27;
|
|
||||||
const Scalar _tmp29 = _tmp22 + _tmp8;
|
|
||||||
const Scalar _tmp30 = -_tmp11;
|
|
||||||
const Scalar _tmp31 = _tmp12 + _tmp30;
|
|
||||||
const Scalar _tmp32 = _tmp10 * _tmp31 + _tmp4 * state(6, 0);
|
|
||||||
const Scalar _tmp33 = _tmp14 * _tmp29 + _tmp32;
|
|
||||||
const Scalar _tmp34 = std::sqrt(Scalar(std::pow(_tmp20, Scalar(2)) + std::pow(_tmp28, Scalar(2)) +
|
|
||||||
std::pow(_tmp33, Scalar(2)) + epsilon));
|
|
||||||
const Scalar _tmp35 = cd * rho;
|
|
||||||
const Scalar _tmp36 = Scalar(0.5) * _tmp34 * _tmp35;
|
|
||||||
const Scalar _tmp37 = 2 * _tmp20;
|
|
||||||
const Scalar _tmp38 = 2 * _tmp28;
|
|
||||||
const Scalar _tmp39 = 2 * _tmp33;
|
|
||||||
const Scalar _tmp40 = Scalar(0.25) * _tmp33 * _tmp35 / _tmp34;
|
|
||||||
const Scalar _tmp41 =
|
|
||||||
-_tmp36 * _tmp4 - _tmp4 * cm - _tmp40 * (_tmp18 * _tmp37 + _tmp23 * _tmp38 + _tmp39 * _tmp4);
|
|
||||||
const Scalar _tmp42 = std::pow(state(0, 0), Scalar(2));
|
|
||||||
const Scalar _tmp43 = -_tmp42;
|
|
||||||
const Scalar _tmp44 = -_tmp5;
|
|
||||||
const Scalar _tmp45 = -_tmp12;
|
|
||||||
const Scalar _tmp46 = -_tmp1;
|
|
||||||
const Scalar _tmp47 = -_tmp21;
|
|
||||||
const Scalar _tmp48 = _tmp47 + _tmp7;
|
|
||||||
const Scalar _tmp49 = _tmp42 + _tmp44;
|
|
||||||
const Scalar _tmp50 = _tmp27 + state(6, 0) * (_tmp48 + _tmp49);
|
|
||||||
const Scalar _tmp51 =
|
|
||||||
-_tmp36 * _tmp50 -
|
|
||||||
_tmp40 * (_tmp38 * (_tmp10 * (_tmp11 + _tmp45) + _tmp14 * (_tmp21 + _tmp43 + _tmp44 + _tmp7) +
|
|
||||||
state(6, 0) * (_tmp24 + _tmp46)) +
|
|
||||||
_tmp39 * _tmp50) -
|
|
||||||
_tmp50 * cm;
|
|
||||||
const Scalar _tmp52 = _tmp43 + _tmp5;
|
|
||||||
const Scalar _tmp53 = -_tmp17;
|
|
||||||
const Scalar _tmp54 =
|
|
||||||
_tmp10 * (_tmp48 + _tmp52) + _tmp14 * (_tmp30 + _tmp45) + state(6, 0) * (_tmp15 + _tmp53);
|
|
||||||
const Scalar _tmp55 = -_tmp7;
|
|
||||||
const Scalar _tmp56 =
|
|
||||||
-_tmp36 * _tmp54 -
|
|
||||||
_tmp40 * (_tmp37 * (_tmp14 * (_tmp42 + _tmp47 + _tmp5 + _tmp55) + _tmp32) + _tmp39 * _tmp54) -
|
|
||||||
_tmp54 * cm;
|
|
||||||
const Scalar _tmp57 = _tmp29 * cm;
|
|
||||||
const Scalar _tmp58 = _tmp13 * _tmp37;
|
|
||||||
const Scalar _tmp59 = _tmp25 * _tmp38;
|
|
||||||
const Scalar _tmp60 = _tmp29 * _tmp39;
|
|
||||||
const Scalar _tmp61 = _tmp29 * _tmp36;
|
|
||||||
const Scalar _tmp62 = -_tmp40 * (-_tmp58 - _tmp59 - _tmp60) + _tmp57 + _tmp61;
|
|
||||||
const Scalar _tmp63 = _tmp21 + _tmp55;
|
|
||||||
const Scalar _tmp64 = _tmp40 * (_tmp37 * (_tmp10 * (_tmp16 + _tmp53) + _tmp14 * (_tmp3 + _tmp46) +
|
|
||||||
state(6, 0) * (_tmp52 + _tmp63)) +
|
|
||||||
_tmp38 * (_tmp10 * (_tmp49 + _tmp63) + _tmp19));
|
|
||||||
const Scalar _tmp65 = -_tmp40 * (_tmp58 + _tmp59 + _tmp60) - _tmp57 - _tmp61;
|
|
||||||
const Scalar _tmp66 = P(4, 4) * _tmp65;
|
|
||||||
const Scalar _tmp67 = _tmp31 * cm;
|
|
||||||
const Scalar _tmp68 = _tmp31 * _tmp36;
|
|
||||||
const Scalar _tmp69 = _tmp37 * _tmp9;
|
|
||||||
const Scalar _tmp70 = _tmp26 * _tmp38;
|
|
||||||
const Scalar _tmp71 = _tmp31 * _tmp39;
|
|
||||||
const Scalar _tmp72 = -_tmp40 * (_tmp69 + _tmp70 + _tmp71) - _tmp67 - _tmp68;
|
|
||||||
const Scalar _tmp73 = -_tmp40 * (-_tmp69 - _tmp70 - _tmp71) + _tmp67 + _tmp68;
|
|
||||||
const Scalar _tmp74 = -P(1, 1) * _tmp64;
|
|
||||||
const Scalar _tmp75 = P(0, 0) * _tmp51;
|
|
||||||
const Scalar _tmp76 = P(22, 22) * _tmp62;
|
|
||||||
const Scalar _tmp77 = P(21, 21) * _tmp73;
|
|
||||||
const Scalar _tmp78 = P(2, 2) * _tmp56;
|
|
||||||
const Scalar _tmp79 = P(3, 3) * _tmp72;
|
|
||||||
const Scalar _tmp80 = P(5, 5) * _tmp41;
|
|
||||||
const Scalar _tmp81 =
|
|
||||||
R +
|
|
||||||
_tmp41 * (P(0, 5) * _tmp51 - P(1, 5) * _tmp64 + P(2, 5) * _tmp56 + P(21, 5) * _tmp73 +
|
|
||||||
P(22, 5) * _tmp62 + P(3, 5) * _tmp72 + P(4, 5) * _tmp65 + _tmp80) +
|
|
||||||
_tmp51 * (-P(1, 0) * _tmp64 + P(2, 0) * _tmp56 + P(21, 0) * _tmp73 + P(22, 0) * _tmp62 +
|
|
||||||
P(3, 0) * _tmp72 + P(4, 0) * _tmp65 + P(5, 0) * _tmp41 + _tmp75) +
|
|
||||||
_tmp56 * (P(0, 2) * _tmp51 - P(1, 2) * _tmp64 + P(21, 2) * _tmp73 + P(22, 2) * _tmp62 +
|
|
||||||
P(3, 2) * _tmp72 + P(4, 2) * _tmp65 + P(5, 2) * _tmp41 + _tmp78) +
|
|
||||||
_tmp62 * (P(0, 22) * _tmp51 - P(1, 22) * _tmp64 + P(2, 22) * _tmp56 + P(21, 22) * _tmp73 +
|
|
||||||
P(3, 22) * _tmp72 + P(4, 22) * _tmp65 + P(5, 22) * _tmp41 + _tmp76) -
|
|
||||||
_tmp64 * (P(0, 1) * _tmp51 + P(2, 1) * _tmp56 + P(21, 1) * _tmp73 + P(22, 1) * _tmp62 +
|
|
||||||
P(3, 1) * _tmp72 + P(4, 1) * _tmp65 + P(5, 1) * _tmp41 + _tmp74) +
|
|
||||||
_tmp65 * (P(0, 4) * _tmp51 - P(1, 4) * _tmp64 + P(2, 4) * _tmp56 + P(21, 4) * _tmp73 +
|
|
||||||
P(22, 4) * _tmp62 + P(3, 4) * _tmp72 + P(5, 4) * _tmp41 + _tmp66) +
|
|
||||||
_tmp72 * (P(0, 3) * _tmp51 - P(1, 3) * _tmp64 + P(2, 3) * _tmp56 + P(21, 3) * _tmp73 +
|
|
||||||
P(22, 3) * _tmp62 + P(4, 3) * _tmp65 + P(5, 3) * _tmp41 + _tmp79) +
|
|
||||||
_tmp73 * (P(0, 21) * _tmp51 - P(1, 21) * _tmp64 + P(2, 21) * _tmp56 + P(22, 21) * _tmp62 +
|
|
||||||
P(3, 21) * _tmp72 + P(4, 21) * _tmp65 + P(5, 21) * _tmp41 + _tmp77);
|
|
||||||
const Scalar _tmp82 = Scalar(1.0) / (math::max<Scalar>(_tmp81, epsilon));
|
|
||||||
|
|
||||||
// Output terms (2)
|
|
||||||
if (innov_var != nullptr) {
|
|
||||||
Scalar& _innov_var = (*innov_var);
|
|
||||||
|
|
||||||
_innov_var = _tmp81;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (K != nullptr) {
|
|
||||||
matrix::Matrix<Scalar, 23, 1>& _k = (*K);
|
|
||||||
|
|
||||||
_k(0, 0) =
|
|
||||||
_tmp82 * (-P(0, 1) * _tmp64 + P(0, 2) * _tmp56 + P(0, 21) * _tmp73 + P(0, 22) * _tmp62 +
|
|
||||||
P(0, 3) * _tmp72 + P(0, 4) * _tmp65 + P(0, 5) * _tmp41 + _tmp75);
|
|
||||||
_k(1, 0) =
|
|
||||||
_tmp82 * (P(1, 0) * _tmp51 + P(1, 2) * _tmp56 + P(1, 21) * _tmp73 + P(1, 22) * _tmp62 +
|
|
||||||
P(1, 3) * _tmp72 + P(1, 4) * _tmp65 + P(1, 5) * _tmp41 + _tmp74);
|
|
||||||
_k(2, 0) =
|
|
||||||
_tmp82 * (P(2, 0) * _tmp51 - P(2, 1) * _tmp64 + P(2, 21) * _tmp73 + P(2, 22) * _tmp62 +
|
|
||||||
P(2, 3) * _tmp72 + P(2, 4) * _tmp65 + P(2, 5) * _tmp41 + _tmp78);
|
|
||||||
_k(3, 0) =
|
|
||||||
_tmp82 * (P(3, 0) * _tmp51 - P(3, 1) * _tmp64 + P(3, 2) * _tmp56 + P(3, 21) * _tmp73 +
|
|
||||||
P(3, 22) * _tmp62 + P(3, 4) * _tmp65 + P(3, 5) * _tmp41 + _tmp79);
|
|
||||||
_k(4, 0) =
|
|
||||||
_tmp82 * (P(4, 0) * _tmp51 - P(4, 1) * _tmp64 + P(4, 2) * _tmp56 + P(4, 21) * _tmp73 +
|
|
||||||
P(4, 22) * _tmp62 + P(4, 3) * _tmp72 + P(4, 5) * _tmp41 + _tmp66);
|
|
||||||
_k(5, 0) =
|
|
||||||
_tmp82 * (P(5, 0) * _tmp51 - P(5, 1) * _tmp64 + P(5, 2) * _tmp56 + P(5, 21) * _tmp73 +
|
|
||||||
P(5, 22) * _tmp62 + P(5, 3) * _tmp72 + P(5, 4) * _tmp65 + _tmp80);
|
|
||||||
_k(6, 0) =
|
|
||||||
_tmp82 * (P(6, 0) * _tmp51 - P(6, 1) * _tmp64 + P(6, 2) * _tmp56 + P(6, 21) * _tmp73 +
|
|
||||||
P(6, 22) * _tmp62 + P(6, 3) * _tmp72 + P(6, 4) * _tmp65 + P(6, 5) * _tmp41);
|
|
||||||
_k(7, 0) =
|
|
||||||
_tmp82 * (P(7, 0) * _tmp51 - P(7, 1) * _tmp64 + P(7, 2) * _tmp56 + P(7, 21) * _tmp73 +
|
|
||||||
P(7, 22) * _tmp62 + P(7, 3) * _tmp72 + P(7, 4) * _tmp65 + P(7, 5) * _tmp41);
|
|
||||||
_k(8, 0) =
|
|
||||||
_tmp82 * (P(8, 0) * _tmp51 - P(8, 1) * _tmp64 + P(8, 2) * _tmp56 + P(8, 21) * _tmp73 +
|
|
||||||
P(8, 22) * _tmp62 + P(8, 3) * _tmp72 + P(8, 4) * _tmp65 + P(8, 5) * _tmp41);
|
|
||||||
_k(9, 0) =
|
|
||||||
_tmp82 * (P(9, 0) * _tmp51 - P(9, 1) * _tmp64 + P(9, 2) * _tmp56 + P(9, 21) * _tmp73 +
|
|
||||||
P(9, 22) * _tmp62 + P(9, 3) * _tmp72 + P(9, 4) * _tmp65 + P(9, 5) * _tmp41);
|
|
||||||
_k(10, 0) =
|
|
||||||
_tmp82 * (P(10, 0) * _tmp51 - P(10, 1) * _tmp64 + P(10, 2) * _tmp56 + P(10, 21) * _tmp73 +
|
|
||||||
P(10, 22) * _tmp62 + P(10, 3) * _tmp72 + P(10, 4) * _tmp65 + P(10, 5) * _tmp41);
|
|
||||||
_k(11, 0) =
|
|
||||||
_tmp82 * (P(11, 0) * _tmp51 - P(11, 1) * _tmp64 + P(11, 2) * _tmp56 + P(11, 21) * _tmp73 +
|
|
||||||
P(11, 22) * _tmp62 + P(11, 3) * _tmp72 + P(11, 4) * _tmp65 + P(11, 5) * _tmp41);
|
|
||||||
_k(12, 0) =
|
|
||||||
_tmp82 * (P(12, 0) * _tmp51 - P(12, 1) * _tmp64 + P(12, 2) * _tmp56 + P(12, 21) * _tmp73 +
|
|
||||||
P(12, 22) * _tmp62 + P(12, 3) * _tmp72 + P(12, 4) * _tmp65 + P(12, 5) * _tmp41);
|
|
||||||
_k(13, 0) =
|
|
||||||
_tmp82 * (P(13, 0) * _tmp51 - P(13, 1) * _tmp64 + P(13, 2) * _tmp56 + P(13, 21) * _tmp73 +
|
|
||||||
P(13, 22) * _tmp62 + P(13, 3) * _tmp72 + P(13, 4) * _tmp65 + P(13, 5) * _tmp41);
|
|
||||||
_k(14, 0) =
|
|
||||||
_tmp82 * (P(14, 0) * _tmp51 - P(14, 1) * _tmp64 + P(14, 2) * _tmp56 + P(14, 21) * _tmp73 +
|
|
||||||
P(14, 22) * _tmp62 + P(14, 3) * _tmp72 + P(14, 4) * _tmp65 + P(14, 5) * _tmp41);
|
|
||||||
_k(15, 0) =
|
|
||||||
_tmp82 * (P(15, 0) * _tmp51 - P(15, 1) * _tmp64 + P(15, 2) * _tmp56 + P(15, 21) * _tmp73 +
|
|
||||||
P(15, 22) * _tmp62 + P(15, 3) * _tmp72 + P(15, 4) * _tmp65 + P(15, 5) * _tmp41);
|
|
||||||
_k(16, 0) =
|
|
||||||
_tmp82 * (P(16, 0) * _tmp51 - P(16, 1) * _tmp64 + P(16, 2) * _tmp56 + P(16, 21) * _tmp73 +
|
|
||||||
P(16, 22) * _tmp62 + P(16, 3) * _tmp72 + P(16, 4) * _tmp65 + P(16, 5) * _tmp41);
|
|
||||||
_k(17, 0) =
|
|
||||||
_tmp82 * (P(17, 0) * _tmp51 - P(17, 1) * _tmp64 + P(17, 2) * _tmp56 + P(17, 21) * _tmp73 +
|
|
||||||
P(17, 22) * _tmp62 + P(17, 3) * _tmp72 + P(17, 4) * _tmp65 + P(17, 5) * _tmp41);
|
|
||||||
_k(18, 0) =
|
|
||||||
_tmp82 * (P(18, 0) * _tmp51 - P(18, 1) * _tmp64 + P(18, 2) * _tmp56 + P(18, 21) * _tmp73 +
|
|
||||||
P(18, 22) * _tmp62 + P(18, 3) * _tmp72 + P(18, 4) * _tmp65 + P(18, 5) * _tmp41);
|
|
||||||
_k(19, 0) =
|
|
||||||
_tmp82 * (P(19, 0) * _tmp51 - P(19, 1) * _tmp64 + P(19, 2) * _tmp56 + P(19, 21) * _tmp73 +
|
|
||||||
P(19, 22) * _tmp62 + P(19, 3) * _tmp72 + P(19, 4) * _tmp65 + P(19, 5) * _tmp41);
|
|
||||||
_k(20, 0) =
|
|
||||||
_tmp82 * (P(20, 0) * _tmp51 - P(20, 1) * _tmp64 + P(20, 2) * _tmp56 + P(20, 21) * _tmp73 +
|
|
||||||
P(20, 22) * _tmp62 + P(20, 3) * _tmp72 + P(20, 4) * _tmp65 + P(20, 5) * _tmp41);
|
|
||||||
_k(21, 0) =
|
|
||||||
_tmp82 * (P(21, 0) * _tmp51 - P(21, 1) * _tmp64 + P(21, 2) * _tmp56 + P(21, 22) * _tmp62 +
|
|
||||||
P(21, 3) * _tmp72 + P(21, 4) * _tmp65 + P(21, 5) * _tmp41 + _tmp77);
|
|
||||||
_k(22, 0) =
|
|
||||||
_tmp82 * (P(22, 0) * _tmp51 - P(22, 1) * _tmp64 + P(22, 2) * _tmp56 + P(22, 21) * _tmp73 +
|
|
||||||
P(22, 3) * _tmp72 + P(22, 4) * _tmp65 + P(22, 5) * _tmp41 + _tmp76);
|
|
||||||
}
|
|
||||||
} // NOLINT(readability/fn_size)
|
|
||||||
|
|
||||||
// NOLINTNEXTLINE(readability/fn_size)
|
|
||||||
} // namespace sym
|
|
||||||
Loading…
Reference in New Issue