diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/computy_gravity_innov_var_and_k_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/computy_gravity_innov_var_and_k_and_h.h new file mode 100644 index 0000000000..9a01adc00a --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/computy_gravity_innov_var_and_k_and_h.h @@ -0,0 +1,116 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// 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: computy_gravity_innov_var_and_k_and_h + * + * Args: + * state: Matrix24_1 + * P: Matrix24_24 + * meas: Matrix31 + * R: Scalar + * epsilon: Scalar + * + * Outputs: + * innov: Matrix31 + * innov_var: Matrix31 + * Kx: Matrix24_1 + * Ky: Matrix24_1 + * Kz: Matrix24_1 + * Hx: Matrix1_24 + * Hy: Matrix1_24 + * Hz: Matrix1_24 + */ +template +void ComputyGravityInnovVarAndKAndH(const matrix::Matrix& state, + const matrix::Matrix& P, + const matrix::Matrix& meas, const Scalar R, + const Scalar epsilon, + matrix::Matrix* const innov = nullptr, + matrix::Matrix* const innov_var = nullptr, + matrix::Matrix* const Kx = nullptr, + matrix::Matrix* const Ky = nullptr, + matrix::Matrix* const Kz = nullptr, + matrix::Matrix* const Hx = nullptr, + matrix::Matrix* const Hy = nullptr, + matrix::Matrix* const Hz = nullptr) { + // Total ops: 10 + + // Unused inputs + (void)state; + (void)P; + + // Input arrays + + // Intermediate terms (1) + const Scalar _tmp0 = + -std::sqrt(Scalar(epsilon + std::pow(meas(0, 0), Scalar(2)) + + std::pow(meas(1, 0), Scalar(2)) + std::pow(meas(2, 0), Scalar(2)))); + + // Output terms (8) + if (innov != nullptr) { + matrix::Matrix& _innov = (*innov); + + _innov(0, 0) = _tmp0; + _innov(1, 0) = _tmp0; + _innov(2, 0) = -Scalar(9.8066499999999994) * R + _tmp0; + } + + if (innov_var != nullptr) { + matrix::Matrix& _innov_var = (*innov_var); + + _innov_var(0, 0) = R; + _innov_var(1, 0) = R; + _innov_var(2, 0) = R; + } + + if (Kx != nullptr) { + matrix::Matrix& _kx = (*Kx); + + _kx.setZero(); + } + + if (Ky != nullptr) { + matrix::Matrix& _ky = (*Ky); + + _ky.setZero(); + } + + if (Kz != nullptr) { + matrix::Matrix& _kz = (*Kz); + + _kz.setZero(); + } + + if (Hx != nullptr) { + matrix::Matrix& _hx = (*Hx); + + _hx.setZero(); + } + + if (Hy != nullptr) { + matrix::Matrix& _hy = (*Hy); + + _hy.setZero(); + } + + if (Hz != nullptr) { + matrix::Matrix& _hz = (*Hz); + + _hz.setZero(); + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym