fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 06:00:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: migrate sideslip fusion to SymForce

Browse Source 
- split fusion into update (compute innov and innov_var) and actual fusion to the state vector
- use estimator_aid_source_1d struct to group the data
This commit is contained in:
bresch
2022-10-05 15:23:03 +02:00
committed by Daniel Agar
parent c09be0f0ac
commit 5f54f6fcda
13 changed files with 441 additions and 163 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/python/ekf_derivation/derivation.py
+50 -1
View File
@@ -98,9 +98,58 @@ def compute_airspeed_h_and_k(
airspeed_pred = vel_rel.norm(epsilon=epsilon)
H = sf.V1(airspeed_pred).jacobian(state)
K = P * H.T / sm.Max(innov_var, epsilon)
K = P * H.T / sf.Max(innov_var, epsilon)
return (H.T, K)
def predict_sideslip(
state: VState,
epsilon: sf.Scalar
) -> (sf.Scalar):
vel_rel = sf.V3(state[State.vx] - state[State.wx], state[State.vy] - state[State.wy], state[State.vz])
q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
relative_wind_body = quat_to_rot(q_att).T * vel_rel
# Small angle approximation of side slip model
# Protect division by zero using epsilon
sideslip_pred = add_epsilon_sign(relative_wind_body[1] / relative_wind_body[0], relative_wind_body[0], epsilon)
return sideslip_pred
def compute_sideslip_innov_and_innov_var(
state: VState,
P: MState,
R: sf.Scalar,
epsilon: sf.Scalar
) -> (sf.Scalar, sf.Scalar, sf.Scalar):
sideslip_pred = predict_sideslip(state, epsilon);
innov = 0.0 - sideslip_pred
H = sf.V1(sideslip_pred).jacobian(state)
innov_var = (H * P * H.T + R)[0,0]
return (innov, innov_var)
def compute_sideslip_h_and_k(
state: VState,
P: MState,
innov_var: sf.Scalar,
epsilon: sf.Scalar
) -> (VState, VState):
sideslip_pred = predict_sideslip(state, epsilon);
H = sf.V1(sideslip_pred).jacobian(state)
K = P * H.T / sf.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"])
generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"])
generate_px4_function(compute_sideslip_h_and_k, output_names=["H", "K"])
src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h
+181
View File
@@ -0,0 +1,181 @@
// -----------------------------------------------------------------------------
// This file was autogenerated by symforce from template:
// backends/cpp/templates/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_sideslip_h_and_k
*
* Args:
* state: Matrix24_1
* P: Matrix24_24
* innov_var: Scalar
* epsilon: Scalar
*
* Outputs:
* H: Matrix24_1
* K: Matrix24_1
*/
template <typename Scalar>
void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar innov_var,
const Scalar epsilon, matrix::Matrix<Scalar, 24, 1>* const H = nullptr,
matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
// Total ops: 539
// Input arrays
// Intermediate terms (44)
const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2));
const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2));
const Scalar _tmp2 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2));
const Scalar _tmp3 = -_tmp0 + _tmp1 + _tmp2;
const Scalar _tmp4 = -state(22, 0) + state(4, 0);
const Scalar _tmp5 = state(0, 0) * state(3, 0);
const Scalar _tmp6 = state(1, 0) * state(2, 0);
const Scalar _tmp7 = _tmp5 + _tmp6;
const Scalar _tmp8 = -state(23, 0) + state(5, 0);
const Scalar _tmp9 = 2 * _tmp8;
const Scalar _tmp10 = state(0, 0) * state(2, 0);
const Scalar _tmp11 = state(1, 0) * state(3, 0);
const Scalar _tmp12 = 2 * state(6, 0);
const Scalar _tmp13 = _tmp12 * (-_tmp10 + _tmp11) + _tmp3 * _tmp4 + _tmp7 * _tmp9;
const Scalar _tmp14 =
_tmp13 + epsilon * (2 * math::min<Scalar>(0, (((_tmp13) > 0) - ((_tmp13) < 0))) + 1);
const Scalar _tmp15 = Scalar(1.0) / (_tmp14);
const Scalar _tmp16 = 2 * _tmp4;
const Scalar _tmp17 = _tmp12 * state(1, 0) - _tmp16 * state(3, 0) + _tmp9 * state(0, 0);
const Scalar _tmp18 = _tmp16 * state(0, 0);
const Scalar _tmp19 = _tmp9 * state(3, 0);
const Scalar _tmp20 = _tmp12 * state(2, 0);
const Scalar _tmp21 = _tmp0 - _tmp1 + _tmp2;
const Scalar _tmp22 = -2 * _tmp5 + 2 * _tmp6;
const Scalar _tmp23 = state(2, 0) * state(3, 0);
const Scalar _tmp24 = state(0, 0) * state(1, 0);
const Scalar _tmp25 =
(_tmp12 * (_tmp23 + _tmp24) + _tmp21 * _tmp8 + _tmp22 * _tmp4) / std::pow(_tmp14, Scalar(2));
const Scalar _tmp26 = _tmp15 * _tmp17 - _tmp25 * (_tmp18 + _tmp19 - _tmp20);
const Scalar _tmp27 = _tmp9 * state(1, 0);
const Scalar _tmp28 = _tmp16 * state(2, 0);
const Scalar _tmp29 = _tmp12 * state(0, 0);
const Scalar _tmp30 = _tmp12 * state(3, 0) + _tmp16 * state(1, 0) + _tmp9 * state(2, 0);
const Scalar _tmp31 = _tmp15 * (-_tmp27 + _tmp28 + _tmp29) - _tmp25 * _tmp30;
const Scalar _tmp32 = _tmp15 * _tmp30 - _tmp25 * (_tmp27 - _tmp28 - _tmp29);
const Scalar _tmp33 = _tmp15 * (-_tmp18 - _tmp19 + _tmp20) - _tmp17 * _tmp25;
const Scalar _tmp34 = _tmp25 * _tmp3;
const Scalar _tmp35 = 2 * _tmp5;
const Scalar _tmp36 = 2 * _tmp6;
const Scalar _tmp37 = _tmp15 * (-_tmp35 + _tmp36) - _tmp34;
const Scalar _tmp38 = _tmp15 * _tmp21;
const Scalar _tmp39 = -_tmp25 * (_tmp35 + _tmp36) + _tmp38;
const Scalar _tmp40 = _tmp15 * (2 * _tmp23 + 2 * _tmp24) - _tmp25 * (-2 * _tmp10 + 2 * _tmp11);
const Scalar _tmp41 = -_tmp15 * _tmp22 + _tmp34;
const Scalar _tmp42 = 2 * _tmp25 * _tmp7 - _tmp38;
const Scalar _tmp43 = Scalar(1.0) / (math::max<Scalar>(epsilon, innov_var));
// Output terms (2)
if (H != nullptr) {
matrix::Matrix<Scalar, 24, 1>& _H = (*H);
_H.setZero();
_H(0, 0) = _tmp26;
_H(1, 0) = _tmp31;
_H(2, 0) = _tmp32;
_H(3, 0) = _tmp33;
_H(4, 0) = _tmp37;
_H(5, 0) = _tmp39;
_H(6, 0) = _tmp40;
_H(22, 0) = _tmp41;
_H(23, 0) = _tmp42;
}
if (K != nullptr) {
matrix::Matrix<Scalar, 24, 1>& _K = (*K);
_K(0, 0) = _tmp43 * (P(0, 0) * _tmp26 + P(0, 1) * _tmp31 + P(0, 2) * _tmp32 +
P(0, 22) * _tmp41 + P(0, 23) * _tmp42 + P(0, 3) * _tmp33 +
P(0, 4) * _tmp37 + P(0, 5) * _tmp39 + P(0, 6) * _tmp40);
_K(1, 0) = _tmp43 * (P(1, 0) * _tmp26 + P(1, 1) * _tmp31 + P(1, 2) * _tmp32 +
P(1, 22) * _tmp41 + P(1, 23) * _tmp42 + P(1, 3) * _tmp33 +
P(1, 4) * _tmp37 + P(1, 5) * _tmp39 + P(1, 6) * _tmp40);
_K(2, 0) = _tmp43 * (P(2, 0) * _tmp26 + P(2, 1) * _tmp31 + P(2, 2) * _tmp32 +
P(2, 22) * _tmp41 + P(2, 23) * _tmp42 + P(2, 3) * _tmp33 +
P(2, 4) * _tmp37 + P(2, 5) * _tmp39 + P(2, 6) * _tmp40);
_K(3, 0) = _tmp43 * (P(3, 0) * _tmp26 + P(3, 1) * _tmp31 + P(3, 2) * _tmp32 +
P(3, 22) * _tmp41 + P(3, 23) * _tmp42 + P(3, 3) * _tmp33 +
P(3, 4) * _tmp37 + P(3, 5) * _tmp39 + P(3, 6) * _tmp40);
_K(4, 0) = _tmp43 * (P(4, 0) * _tmp26 + P(4, 1) * _tmp31 + P(4, 2) * _tmp32 +
P(4, 22) * _tmp41 + P(4, 23) * _tmp42 + P(4, 3) * _tmp33 +
P(4, 4) * _tmp37 + P(4, 5) * _tmp39 + P(4, 6) * _tmp40);
_K(5, 0) = _tmp43 * (P(5, 0) * _tmp26 + P(5, 1) * _tmp31 + P(5, 2) * _tmp32 +
P(5, 22) * _tmp41 + P(5, 23) * _tmp42 + P(5, 3) * _tmp33 +
P(5, 4) * _tmp37 + P(5, 5) * _tmp39 + P(5, 6) * _tmp40);
_K(6, 0) = _tmp43 * (P(6, 0) * _tmp26 + P(6, 1) * _tmp31 + P(6, 2) * _tmp32 +
P(6, 22) * _tmp41 + P(6, 23) * _tmp42 + P(6, 3) * _tmp33 +
P(6, 4) * _tmp37 + P(6, 5) * _tmp39 + P(6, 6) * _tmp40);
_K(7, 0) = _tmp43 * (P(7, 0) * _tmp26 + P(7, 1) * _tmp31 + P(7, 2) * _tmp32 +
P(7, 22) * _tmp41 + P(7, 23) * _tmp42 + P(7, 3) * _tmp33 +
P(7, 4) * _tmp37 + P(7, 5) * _tmp39 + P(7, 6) * _tmp40);
_K(8, 0) = _tmp43 * (P(8, 0) * _tmp26 + P(8, 1) * _tmp31 + P(8, 2) * _tmp32 +
P(8, 22) * _tmp41 + P(8, 23) * _tmp42 + P(8, 3) * _tmp33 +
P(8, 4) * _tmp37 + P(8, 5) * _tmp39 + P(8, 6) * _tmp40);
_K(9, 0) = _tmp43 * (P(9, 0) * _tmp26 + P(9, 1) * _tmp31 + P(9, 2) * _tmp32 +
P(9, 22) * _tmp41 + P(9, 23) * _tmp42 + P(9, 3) * _tmp33 +
P(9, 4) * _tmp37 + P(9, 5) * _tmp39 + P(9, 6) * _tmp40);
_K(10, 0) = _tmp43 * (P(10, 0) * _tmp26 + P(10, 1) * _tmp31 + P(10, 2) * _tmp32 +
P(10, 22) * _tmp41 + P(10, 23) * _tmp42 + P(10, 3) * _tmp33 +
P(10, 4) * _tmp37 + P(10, 5) * _tmp39 + P(10, 6) * _tmp40);
_K(11, 0) = _tmp43 * (P(11, 0) * _tmp26 + P(11, 1) * _tmp31 + P(11, 2) * _tmp32 +
P(11, 22) * _tmp41 + P(11, 23) * _tmp42 + P(11, 3) * _tmp33 +
P(11, 4) * _tmp37 + P(11, 5) * _tmp39 + P(11, 6) * _tmp40);
_K(12, 0) = _tmp43 * (P(12, 0) * _tmp26 + P(12, 1) * _tmp31 + P(12, 2) * _tmp32 +
P(12, 22) * _tmp41 + P(12, 23) * _tmp42 + P(12, 3) * _tmp33 +
P(12, 4) * _tmp37 + P(12, 5) * _tmp39 + P(12, 6) * _tmp40);
_K(13, 0) = _tmp43 * (P(13, 0) * _tmp26 + P(13, 1) * _tmp31 + P(13, 2) * _tmp32 +
P(13, 22) * _tmp41 + P(13, 23) * _tmp42 + P(13, 3) * _tmp33 +
P(13, 4) * _tmp37 + P(13, 5) * _tmp39 + P(13, 6) * _tmp40);
_K(14, 0) = _tmp43 * (P(14, 0) * _tmp26 + P(14, 1) * _tmp31 + P(14, 2) * _tmp32 +
P(14, 22) * _tmp41 + P(14, 23) * _tmp42 + P(14, 3) * _tmp33 +
P(14, 4) * _tmp37 + P(14, 5) * _tmp39 + P(14, 6) * _tmp40);
_K(15, 0) = _tmp43 * (P(15, 0) * _tmp26 + P(15, 1) * _tmp31 + P(15, 2) * _tmp32 +
P(15, 22) * _tmp41 + P(15, 23) * _tmp42 + P(15, 3) * _tmp33 +
P(15, 4) * _tmp37 + P(15, 5) * _tmp39 + P(15, 6) * _tmp40);
_K(16, 0) = _tmp43 * (P(16, 0) * _tmp26 + P(16, 1) * _tmp31 + P(16, 2) * _tmp32 +
P(16, 22) * _tmp41 + P(16, 23) * _tmp42 + P(16, 3) * _tmp33 +
P(16, 4) * _tmp37 + P(16, 5) * _tmp39 + P(16, 6) * _tmp40);
_K(17, 0) = _tmp43 * (P(17, 0) * _tmp26 + P(17, 1) * _tmp31 + P(17, 2) * _tmp32 +
P(17, 22) * _tmp41 + P(17, 23) * _tmp42 + P(17, 3) * _tmp33 +
P(17, 4) * _tmp37 + P(17, 5) * _tmp39 + P(17, 6) * _tmp40);
_K(18, 0) = _tmp43 * (P(18, 0) * _tmp26 + P(18, 1) * _tmp31 + P(18, 2) * _tmp32 +
P(18, 22) * _tmp41 + P(18, 23) * _tmp42 + P(18, 3) * _tmp33 +
P(18, 4) * _tmp37 + P(18, 5) * _tmp39 + P(18, 6) * _tmp40);
_K(19, 0) = _tmp43 * (P(19, 0) * _tmp26 + P(19, 1) * _tmp31 + P(19, 2) * _tmp32 +
P(19, 22) * _tmp41 + P(19, 23) * _tmp42 + P(19, 3) * _tmp33 +
P(19, 4) * _tmp37 + P(19, 5) * _tmp39 + P(19, 6) * _tmp40);
_K(20, 0) = _tmp43 * (P(20, 0) * _tmp26 + P(20, 1) * _tmp31 + P(20, 2) * _tmp32 +
P(20, 22) * _tmp41 + P(20, 23) * _tmp42 + P(20, 3) * _tmp33 +
P(20, 4) * _tmp37 + P(20, 5) * _tmp39 + P(20, 6) * _tmp40);
_K(21, 0) = _tmp43 * (P(21, 0) * _tmp26 + P(21, 1) * _tmp31 + P(21, 2) * _tmp32 +
P(21, 22) * _tmp41 + P(21, 23) * _tmp42 + P(21, 3) * _tmp33 +
P(21, 4) * _tmp37 + P(21, 5) * _tmp39 + P(21, 6) * _tmp40);
_K(22, 0) = _tmp43 * (P(22, 0) * _tmp26 + P(22, 1) * _tmp31 + P(22, 2) * _tmp32 +
P(22, 22) * _tmp41 + P(22, 23) * _tmp42 + P(22, 3) * _tmp33 +
P(22, 4) * _tmp37 + P(22, 5) * _tmp39 + P(22, 6) * _tmp40);
_K(23, 0) = _tmp43 * (P(23, 0) * _tmp26 + P(23, 1) * _tmp31 + P(23, 2) * _tmp32 +
P(23, 22) * _tmp41 + P(23, 23) * _tmp42 + P(23, 3) * _tmp33 +
P(23, 4) * _tmp37 + P(23, 5) * _tmp39 + P(23, 6) * _tmp40);
}
} // NOLINT(readability/fn_size)
// NOLINTNEXTLINE(readability/fn_size)
} // namespace sym
src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h
+126
View File
@@ -0,0 +1,126 @@
// -----------------------------------------------------------------------------
// This file was autogenerated by symforce from template:
// backends/cpp/templates/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_sideslip_innov_and_innov_var
*
* Args:
* state: Matrix24_1
* P: Matrix24_24
* R: Scalar
* epsilon: Scalar
*
* Outputs:
* innov: Scalar
* innov_var: Scalar
*/
template <typename Scalar>
void ComputeSideslipInnovAndInnovVar(const matrix::Matrix<Scalar, 24, 1>& state,
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
const Scalar epsilon, Scalar* const innov = nullptr,
Scalar* const innov_var = nullptr) {
// Total ops: 277
// Input arrays
// Intermediate terms (44)
const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2));
const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2));
const Scalar _tmp2 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2));
const Scalar _tmp3 = -_tmp0 + _tmp1 + _tmp2;
const Scalar _tmp4 = -state(22, 0) + state(4, 0);
const Scalar _tmp5 = state(0, 0) * state(3, 0);
const Scalar _tmp6 = state(1, 0) * state(2, 0);
const Scalar _tmp7 = _tmp5 + _tmp6;
const Scalar _tmp8 = -state(23, 0) + state(5, 0);
const Scalar _tmp9 = 2 * _tmp8;
const Scalar _tmp10 = state(0, 0) * state(2, 0);
const Scalar _tmp11 = state(1, 0) * state(3, 0);
const Scalar _tmp12 = 2 * state(6, 0);
const Scalar _tmp13 = _tmp12 * (-_tmp10 + _tmp11) + _tmp3 * _tmp4 + _tmp7 * _tmp9;
const Scalar _tmp14 =
_tmp13 + epsilon * (2 * math::min<Scalar>(0, (((_tmp13) > 0) - ((_tmp13) < 0))) + 1);
const Scalar _tmp15 = Scalar(1.0) / (_tmp14);
const Scalar _tmp16 = _tmp0 - _tmp1 + _tmp2;
const Scalar _tmp17 = -_tmp5 + _tmp6;
const Scalar _tmp18 = 2 * _tmp4;
const Scalar _tmp19 = state(2, 0) * state(3, 0);
const Scalar _tmp20 = state(0, 0) * state(1, 0);
const Scalar _tmp21 = _tmp12 * (_tmp19 + _tmp20) + _tmp16 * _tmp8 + _tmp17 * _tmp18;
const Scalar _tmp22 = _tmp9 * state(1, 0);
const Scalar _tmp23 = _tmp18 * state(2, 0);
const Scalar _tmp24 = _tmp12 * state(0, 0);
const Scalar _tmp25 = _tmp12 * state(3, 0) + _tmp18 * state(1, 0) + _tmp9 * state(2, 0);
const Scalar _tmp26 = _tmp21 / std::pow(_tmp14, Scalar(2));
const Scalar _tmp27 = _tmp15 * (-_tmp22 + _tmp23 + _tmp24) - _tmp25 * _tmp26;
const Scalar _tmp28 = _tmp15 * _tmp16;
const Scalar _tmp29 = 2 * _tmp5;
const Scalar _tmp30 = 2 * _tmp6;
const Scalar _tmp31 = -_tmp26 * (_tmp29 + _tmp30) + _tmp28;
const Scalar _tmp32 = _tmp12 * state(1, 0) - _tmp18 * state(3, 0) + _tmp9 * state(0, 0);
const Scalar _tmp33 = _tmp18 * state(0, 0);
const Scalar _tmp34 = _tmp9 * state(3, 0);
const Scalar _tmp35 = _tmp12 * state(2, 0);
const Scalar _tmp36 = _tmp15 * _tmp32 - _tmp26 * (_tmp33 + _tmp34 - _tmp35);
const Scalar _tmp37 = 2 * _tmp26 * _tmp7 - _tmp28;
const Scalar _tmp38 = _tmp26 * _tmp3;
const Scalar _tmp39 = -2 * _tmp15 * _tmp17 + _tmp38;
const Scalar _tmp40 = _tmp15 * _tmp25 - _tmp26 * (_tmp22 - _tmp23 - _tmp24);
const Scalar _tmp41 = _tmp15 * (-_tmp33 - _tmp34 + _tmp35) - _tmp26 * _tmp32;
const Scalar _tmp42 = _tmp15 * (2 * _tmp19 + 2 * _tmp20) - _tmp26 * (-2 * _tmp10 + 2 * _tmp11);
const Scalar _tmp43 = _tmp15 * (-_tmp29 + _tmp30) - _tmp38;
// Output terms (2)
if (innov != nullptr) {
Scalar& _innov = (*innov);
_innov = -_tmp15 * _tmp21;
}
if (innov_var != nullptr) {
Scalar& _innov_var = (*innov_var);
_innov_var = R +
_tmp27 * (P(0, 1) * _tmp36 + P(1, 1) * _tmp27 + P(2, 1) * _tmp40 +
P(22, 1) * _tmp39 + P(23, 1) * _tmp37 + P(3, 1) * _tmp41 +
P(4, 1) * _tmp43 + P(5, 1) * _tmp31 + P(6, 1) * _tmp42) +
_tmp31 * (P(0, 5) * _tmp36 + P(1, 5) * _tmp27 + P(2, 5) * _tmp40 +
P(22, 5) * _tmp39 + P(23, 5) * _tmp37 + P(3, 5) * _tmp41 +
P(4, 5) * _tmp43 + P(5, 5) * _tmp31 + P(6, 5) * _tmp42) +
_tmp36 * (P(0, 0) * _tmp36 + P(1, 0) * _tmp27 + P(2, 0) * _tmp40 +
P(22, 0) * _tmp39 + P(23, 0) * _tmp37 + P(3, 0) * _tmp41 +
P(4, 0) * _tmp43 + P(5, 0) * _tmp31 + P(6, 0) * _tmp42) +
_tmp37 * (P(0, 23) * _tmp36 + P(1, 23) * _tmp27 + P(2, 23) * _tmp40 +
P(22, 23) * _tmp39 + P(23, 23) * _tmp37 + P(3, 23) * _tmp41 +
P(4, 23) * _tmp43 + P(5, 23) * _tmp31 + P(6, 23) * _tmp42) +
_tmp39 * (P(0, 22) * _tmp36 + P(1, 22) * _tmp27 + P(2, 22) * _tmp40 +
P(22, 22) * _tmp39 + P(23, 22) * _tmp37 + P(3, 22) * _tmp41 +
P(4, 22) * _tmp43 + P(5, 22) * _tmp31 + P(6, 22) * _tmp42) +
_tmp40 * (P(0, 2) * _tmp36 + P(1, 2) * _tmp27 + P(2, 2) * _tmp40 +
P(22, 2) * _tmp39 + P(23, 2) * _tmp37 + P(3, 2) * _tmp41 +
P(4, 2) * _tmp43 + P(5, 2) * _tmp31 + P(6, 2) * _tmp42) +
_tmp41 * (P(0, 3) * _tmp36 + P(1, 3) * _tmp27 + P(2, 3) * _tmp40 +
P(22, 3) * _tmp39 + P(23, 3) * _tmp37 + P(3, 3) * _tmp41 +
P(4, 3) * _tmp43 + P(5, 3) * _tmp31 + P(6, 3) * _tmp42) +
_tmp42 * (P(0, 6) * _tmp36 + P(1, 6) * _tmp27 + P(2, 6) * _tmp40 +
P(22, 6) * _tmp39 + P(23, 6) * _tmp37 + P(3, 6) * _tmp41 +
P(4, 6) * _tmp43 + P(5, 6) * _tmp31 + P(6, 6) * _tmp42) +
_tmp43 * (P(0, 4) * _tmp36 + P(1, 4) * _tmp27 + P(2, 4) * _tmp40 +
P(22, 4) * _tmp39 + P(23, 4) * _tmp37 + P(3, 4) * _tmp41 +
P(4, 4) * _tmp43 + P(5, 4) * _tmp31 + P(6, 4) * _tmp42);
}
} // NOLINT(readability/fn_size)
// NOLINTNEXTLINE(readability/fn_size)
} // namespace sym