From 3a50fbbdfd8bde141cc10906a69888bc80dd301e Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Wed, 1 Feb 2023 10:00:30 -0500 Subject: [PATCH] EKF2: Corrected a number of mistakes, including a silly typo. Signed-off-by: Daniel M. Sahu --- .../EKF/python/ekf_derivation/derivation.py | 23 +- .../compute_gravity_innov_var_and_k_and_h.h | 276 ++++++++++++++++++ .../computy_gravity_innov_var_and_k_and_h.h | 246 +++++++++++++--- 3 files changed, 492 insertions(+), 53 deletions(-) create mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index ea92f3bf56..74037adfb5 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -477,32 +477,35 @@ def compute_drag_y_innov_var_and_k( return (innov_var, K) -def computy_gravity_innov_var_and_k_and_h( +def compute_gravity_innov_var_and_k_and_h( state: VState, P: MState, meas: sf.V3, R: sf.Scalar, epsilon: sf.Scalar -) -> (sf.V3, sf.V3, VState, VState, VState, VState, VState, VState): +) -> (sf.V3, sf.V3, VState, VState, VState): + + # get transform from earth to body frame + q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz]) + R_to_body = quat_to_rot(q_att).T # the innovation is the error between measured acceleration # and predicted (body frame), assuming no body acceleration - meas_pred = R * sf.Matrix([0,0,-9.80665]) - innov = meas_pred - meas.norm(epsilon=epsilon) + meas_pred = R_to_body * sf.Matrix([0,0,-9.80665]) + innov = meas_pred - 9.80665 * meas.norm(epsilon=epsilon) # initialize outputs innov_var = sf.V3() - H = [None] * 3 K = [None] * 3 # calculate observation jacobian (H), kalman gain (K), and innovation variance (S) # for each axis for i in range(3): - H[i] = sf.V1(meas_pred[i]).jacobian(state) - innov_var[i] = (H[i] * P * H[i].T + R)[0,0] - K[i] = P * H[i].T / innov_var[i] + H = sf.V1(meas_pred[i]).jacobian(state) + innov_var[i] = (H * P * H.T + R)[0,0] + K[i] = P * H.T / innov_var[i] - return (innov, innov_var, K[0], K[1], K[2], H[0], H[1], H[2]) + return (innov, innov_var, K[0], K[1], K[2]) print("Derive EKF2 equations...") generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"]) @@ -524,4 +527,4 @@ generate_px4_function(compute_flow_y_innov_var_and_h, output_names=["innov_var", 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"]) -generate_px4_function(computy_gravity_innov_var_and_k_and_h, output_names=["innov", "innov_var", "Kx", "Ky", "Kz", "Hx", "Hy", "Hz"]) +generate_px4_function(compute_gravity_innov_var_and_k_and_h, output_names=["innov", "innov_var", "Kx", "Ky", "Kz"]) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h new file mode 100644 index 0000000000..61a191994d --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h @@ -0,0 +1,276 @@ +// ----------------------------------------------------------------------------- +// 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: compute_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 + */ +template +void ComputeGravityInnovVarAndKAndH(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) { + // Total ops: 734 + + // Input arrays + + // Intermediate terms (55) + const Scalar _tmp0 = + -Scalar(9.8066499999999994) * + 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)))); + const Scalar _tmp1 = Scalar(19.613299999999999) * state(1, 0); + const Scalar _tmp2 = -P(3, 0) * _tmp1; + const Scalar _tmp3 = Scalar(19.613299999999999) * state(2, 0); + const Scalar _tmp4 = P(0, 0) * _tmp3; + const Scalar _tmp5 = Scalar(19.613299999999999) * state(0, 0); + const Scalar _tmp6 = P(2, 0) * _tmp5; + const Scalar _tmp7 = Scalar(19.613299999999999) * state(3, 0); + const Scalar _tmp8 = P(3, 1) * _tmp1; + const Scalar _tmp9 = P(2, 1) * _tmp5; + const Scalar _tmp10 = -P(1, 1) * _tmp7; + const Scalar _tmp11 = P(0, 2) * _tmp3; + const Scalar _tmp12 = P(2, 2) * _tmp5; + const Scalar _tmp13 = -P(1, 2) * _tmp7; + const Scalar _tmp14 = -P(3, 3) * _tmp1; + const Scalar _tmp15 = P(0, 3) * _tmp3; + const Scalar _tmp16 = -P(1, 3) * _tmp7; + const Scalar _tmp17 = R - _tmp1 * (P(2, 3) * _tmp5 + _tmp14 + _tmp15 + _tmp16) + + _tmp3 * (-P(1, 0) * _tmp7 + _tmp2 + _tmp4 + _tmp6) + + _tmp5 * (-P(3, 2) * _tmp1 + _tmp11 + _tmp12 + _tmp13) - + _tmp7 * (P(0, 1) * _tmp3 + _tmp10 - _tmp8 + _tmp9); + const Scalar _tmp18 = P(3, 0) * _tmp3; + const Scalar _tmp19 = -P(0, 0) * _tmp1; + const Scalar _tmp20 = -P(1, 0) * _tmp5; + const Scalar _tmp21 = P(3, 2) * _tmp3; + const Scalar _tmp22 = -P(2, 2) * _tmp7; + const Scalar _tmp23 = P(1, 2) * _tmp5; + const Scalar _tmp24 = P(0, 1) * _tmp1; + const Scalar _tmp25 = -P(2, 1) * _tmp7; + const Scalar _tmp26 = -P(1, 1) * _tmp5; + const Scalar _tmp27 = -P(3, 3) * _tmp3; + const Scalar _tmp28 = -P(0, 3) * _tmp1; + const Scalar _tmp29 = -P(2, 3) * _tmp7; + const Scalar _tmp30 = R - _tmp1 * (-P(2, 0) * _tmp7 - _tmp18 + _tmp19 + _tmp20) - + _tmp3 * (-P(1, 3) * _tmp5 + _tmp27 + _tmp28 + _tmp29) - + _tmp5 * (-P(3, 1) * _tmp3 - _tmp24 + _tmp25 + _tmp26) - + _tmp7 * (-P(0, 2) * _tmp1 - _tmp21 + _tmp22 - _tmp23); + const Scalar _tmp31 = -P(0, 0) * _tmp5; + const Scalar _tmp32 = P(2, 0) * _tmp3; + const Scalar _tmp33 = P(1, 0) * _tmp1; + const Scalar _tmp34 = -P(3, 2) * _tmp7; + const Scalar _tmp35 = P(0, 2) * _tmp5; + const Scalar _tmp36 = P(2, 2) * _tmp3; + const Scalar _tmp37 = -P(3, 1) * _tmp7; + const Scalar _tmp38 = -P(0, 1) * _tmp5; + const Scalar _tmp39 = P(1, 1) * _tmp1; + const Scalar _tmp40 = -P(3, 3) * _tmp7; + const Scalar _tmp41 = P(2, 3) * _tmp3; + const Scalar _tmp42 = P(1, 3) * _tmp1; + const Scalar _tmp43 = R + _tmp1 * (P(2, 1) * _tmp3 + _tmp37 + _tmp38 + _tmp39) + + _tmp3 * (P(1, 2) * _tmp1 + _tmp34 - _tmp35 + _tmp36) - + _tmp5 * (-P(3, 0) * _tmp7 + _tmp31 + _tmp32 + _tmp33) - + _tmp7 * (-P(0, 3) * _tmp5 + _tmp40 + _tmp41 + _tmp42); + const Scalar _tmp44 = Scalar(1.0) / (_tmp17); + const Scalar _tmp45 = Scalar(19.613299999999999) * P(4, 0); + const Scalar _tmp46 = Scalar(19.613299999999999) * P(4, 2); + const Scalar _tmp47 = Scalar(19.613299999999999) * P(8, 3); + const Scalar _tmp48 = Scalar(19.613299999999999) * P(8, 0); + const Scalar _tmp49 = Scalar(19.613299999999999) * P(8, 1); + const Scalar _tmp50 = Scalar(19.613299999999999) * P(8, 2); + const Scalar _tmp51 = Scalar(19.613299999999999) * P(9, 2); + const Scalar _tmp52 = Scalar(19.613299999999999) * P(9, 0); + const Scalar _tmp53 = Scalar(1.0) / (_tmp30); + const Scalar _tmp54 = Scalar(1.0) / (_tmp43); + + // Output terms (5) + if (innov != nullptr) { + matrix::Matrix& _innov = (*innov); + + _innov(0, 0) = _tmp0 + Scalar(19.613299999999999) * state(0, 0) * state(2, 0) - + Scalar(19.613299999999999) * state(1, 0) * state(3, 0); + _innov(1, 0) = _tmp0 - Scalar(19.613299999999999) * state(0, 0) * state(1, 0) - + Scalar(19.613299999999999) * state(2, 0) * state(3, 0); + _innov(2, 0) = _tmp0 - Scalar(9.8066499999999994) * std::pow(state(0, 0), Scalar(2)) + + Scalar(9.8066499999999994) * std::pow(state(1, 0), Scalar(2)) + + Scalar(9.8066499999999994) * std::pow(state(2, 0), Scalar(2)) - + Scalar(9.8066499999999994) * std::pow(state(3, 0), Scalar(2)); + } + + if (innov_var != nullptr) { + matrix::Matrix& _innov_var = (*innov_var); + + _innov_var(0, 0) = _tmp17; + _innov_var(1, 0) = _tmp30; + _innov_var(2, 0) = _tmp43; + } + + if (Kx != nullptr) { + matrix::Matrix& _kx = (*Kx); + + _kx(0, 0) = _tmp44 * (-P(0, 1) * _tmp7 + _tmp28 + _tmp35 + _tmp4); + _kx(1, 0) = _tmp44 * (P(1, 0) * _tmp3 + _tmp10 + _tmp23 - _tmp42); + _kx(2, 0) = _tmp44 * (-P(2, 3) * _tmp1 + _tmp12 + _tmp25 + _tmp32); + _kx(3, 0) = _tmp44 * (P(3, 2) * _tmp5 + _tmp14 + _tmp18 + _tmp37); + _kx(4, 0) = + _tmp44 * (-P(4, 1) * _tmp7 - P(4, 3) * _tmp1 + _tmp45 * state(2, 0) + _tmp46 * state(0, 0)); + _kx(5, 0) = _tmp44 * (P(5, 0) * _tmp3 - P(5, 1) * _tmp7 + P(5, 2) * _tmp5 - P(5, 3) * _tmp1); + _kx(6, 0) = _tmp44 * (P(6, 0) * _tmp3 - P(6, 1) * _tmp7 + P(6, 2) * _tmp5 - P(6, 3) * _tmp1); + _kx(7, 0) = _tmp44 * (P(7, 0) * _tmp3 - P(7, 1) * _tmp7 + P(7, 2) * _tmp5 - P(7, 3) * _tmp1); + _kx(8, 0) = _tmp44 * (-_tmp47 * state(1, 0) + _tmp48 * state(2, 0) - _tmp49 * state(3, 0) + + _tmp50 * state(0, 0)); + _kx(9, 0) = + _tmp44 * (-P(9, 1) * _tmp7 - P(9, 3) * _tmp1 + _tmp51 * state(0, 0) + _tmp52 * state(2, 0)); + _kx(10, 0) = + _tmp44 * (P(10, 0) * _tmp3 - P(10, 1) * _tmp7 + P(10, 2) * _tmp5 - P(10, 3) * _tmp1); + _kx(11, 0) = + _tmp44 * (P(11, 0) * _tmp3 - P(11, 1) * _tmp7 + P(11, 2) * _tmp5 - P(11, 3) * _tmp1); + _kx(12, 0) = + _tmp44 * (P(12, 0) * _tmp3 - P(12, 1) * _tmp7 + P(12, 2) * _tmp5 - P(12, 3) * _tmp1); + _kx(13, 0) = + _tmp44 * (P(13, 0) * _tmp3 - P(13, 1) * _tmp7 + P(13, 2) * _tmp5 - P(13, 3) * _tmp1); + _kx(14, 0) = + _tmp44 * (P(14, 0) * _tmp3 - P(14, 1) * _tmp7 + P(14, 2) * _tmp5 - P(14, 3) * _tmp1); + _kx(15, 0) = + _tmp44 * (P(15, 0) * _tmp3 - P(15, 1) * _tmp7 + P(15, 2) * _tmp5 - P(15, 3) * _tmp1); + _kx(16, 0) = + _tmp44 * (P(16, 0) * _tmp3 - P(16, 1) * _tmp7 + P(16, 2) * _tmp5 - P(16, 3) * _tmp1); + _kx(17, 0) = + _tmp44 * (P(17, 0) * _tmp3 - P(17, 1) * _tmp7 + P(17, 2) * _tmp5 - P(17, 3) * _tmp1); + _kx(18, 0) = + _tmp44 * (P(18, 0) * _tmp3 - P(18, 1) * _tmp7 + P(18, 2) * _tmp5 - P(18, 3) * _tmp1); + _kx(19, 0) = + _tmp44 * (P(19, 0) * _tmp3 - P(19, 1) * _tmp7 + P(19, 2) * _tmp5 - P(19, 3) * _tmp1); + _kx(20, 0) = + _tmp44 * (P(20, 0) * _tmp3 - P(20, 1) * _tmp7 + P(20, 2) * _tmp5 - P(20, 3) * _tmp1); + _kx(21, 0) = + _tmp44 * (P(21, 0) * _tmp3 - P(21, 1) * _tmp7 + P(21, 2) * _tmp5 - P(21, 3) * _tmp1); + _kx(22, 0) = + _tmp44 * (P(22, 0) * _tmp3 - P(22, 1) * _tmp7 + P(22, 2) * _tmp5 - P(22, 3) * _tmp1); + _kx(23, 0) = + _tmp44 * (P(23, 0) * _tmp3 - P(23, 1) * _tmp7 + P(23, 2) * _tmp5 - P(23, 3) * _tmp1); + } + + if (Ky != nullptr) { + matrix::Matrix& _ky = (*Ky); + + _ky(0, 0) = _tmp53 * (-P(0, 2) * _tmp7 - _tmp15 + _tmp19 + _tmp38); + _ky(1, 0) = _tmp53 * (-P(1, 3) * _tmp3 + _tmp13 + _tmp26 - _tmp33); + _ky(2, 0) = _tmp53 * (-P(2, 0) * _tmp1 + _tmp22 - _tmp41 - _tmp9); + _ky(3, 0) = _tmp53 * (-P(3, 1) * _tmp5 + _tmp2 + _tmp27 + _tmp34); + _ky(4, 0) = _tmp53 * (-P(4, 0) * _tmp1 - P(4, 1) * _tmp5 - P(4, 2) * _tmp7 - P(4, 3) * _tmp3); + _ky(5, 0) = _tmp53 * (-P(5, 0) * _tmp1 - P(5, 1) * _tmp5 - P(5, 2) * _tmp7 - P(5, 3) * _tmp3); + _ky(6, 0) = _tmp53 * (-P(6, 0) * _tmp1 - P(6, 1) * _tmp5 - P(6, 2) * _tmp7 - P(6, 3) * _tmp3); + _ky(7, 0) = _tmp53 * (-P(7, 0) * _tmp1 - P(7, 1) * _tmp5 - P(7, 2) * _tmp7 - P(7, 3) * _tmp3); + _ky(8, 0) = _tmp53 * (-P(8, 2) * _tmp7 - _tmp47 * state(2, 0) - _tmp48 * state(1, 0) - + _tmp49 * state(0, 0)); + _ky(9, 0) = + _tmp53 * (-P(9, 1) * _tmp5 - P(9, 2) * _tmp7 - P(9, 3) * _tmp3 - _tmp52 * state(1, 0)); + _ky(10, 0) = + _tmp53 * (-P(10, 0) * _tmp1 - P(10, 1) * _tmp5 - P(10, 2) * _tmp7 - P(10, 3) * _tmp3); + _ky(11, 0) = + _tmp53 * (-P(11, 0) * _tmp1 - P(11, 1) * _tmp5 - P(11, 2) * _tmp7 - P(11, 3) * _tmp3); + _ky(12, 0) = + _tmp53 * (-P(12, 0) * _tmp1 - P(12, 1) * _tmp5 - P(12, 2) * _tmp7 - P(12, 3) * _tmp3); + _ky(13, 0) = + _tmp53 * (-P(13, 0) * _tmp1 - P(13, 1) * _tmp5 - P(13, 2) * _tmp7 - P(13, 3) * _tmp3); + _ky(14, 0) = + _tmp53 * (-P(14, 0) * _tmp1 - P(14, 1) * _tmp5 - P(14, 2) * _tmp7 - P(14, 3) * _tmp3); + _ky(15, 0) = + _tmp53 * (-P(15, 0) * _tmp1 - P(15, 1) * _tmp5 - P(15, 2) * _tmp7 - P(15, 3) * _tmp3); + _ky(16, 0) = + _tmp53 * (-P(16, 0) * _tmp1 - P(16, 1) * _tmp5 - P(16, 2) * _tmp7 - P(16, 3) * _tmp3); + _ky(17, 0) = + _tmp53 * (-P(17, 0) * _tmp1 - P(17, 1) * _tmp5 - P(17, 2) * _tmp7 - P(17, 3) * _tmp3); + _ky(18, 0) = + _tmp53 * (-P(18, 0) * _tmp1 - P(18, 1) * _tmp5 - P(18, 2) * _tmp7 - P(18, 3) * _tmp3); + _ky(19, 0) = + _tmp53 * (-P(19, 0) * _tmp1 - P(19, 1) * _tmp5 - P(19, 2) * _tmp7 - P(19, 3) * _tmp3); + _ky(20, 0) = + _tmp53 * (-P(20, 0) * _tmp1 - P(20, 1) * _tmp5 - P(20, 2) * _tmp7 - P(20, 3) * _tmp3); + _ky(21, 0) = + _tmp53 * (-P(21, 0) * _tmp1 - P(21, 1) * _tmp5 - P(21, 2) * _tmp7 - P(21, 3) * _tmp3); + _ky(22, 0) = + _tmp53 * (-P(22, 0) * _tmp1 - P(22, 1) * _tmp5 - P(22, 2) * _tmp7 - P(22, 3) * _tmp3); + _ky(23, 0) = + _tmp53 * (-P(23, 0) * _tmp1 - P(23, 1) * _tmp5 - P(23, 2) * _tmp7 - P(23, 3) * _tmp3); + } + + if (Kz != nullptr) { + matrix::Matrix& _kz = (*Kz); + + _kz(0, 0) = _tmp54 * (-P(0, 3) * _tmp7 + _tmp11 + _tmp24 + _tmp31); + _kz(1, 0) = _tmp54 * (P(1, 2) * _tmp3 + _tmp16 + _tmp20 + _tmp39); + _kz(2, 0) = _tmp54 * (P(2, 1) * _tmp1 + _tmp29 + _tmp36 - _tmp6); + _kz(3, 0) = _tmp54 * (-P(3, 0) * _tmp5 + _tmp21 + _tmp40 + _tmp8); + _kz(4, 0) = + _tmp54 * (P(4, 1) * _tmp1 - P(4, 3) * _tmp7 - _tmp45 * state(0, 0) + _tmp46 * state(2, 0)); + _kz(5, 0) = _tmp54 * (-P(5, 0) * _tmp5 + P(5, 1) * _tmp1 + P(5, 2) * _tmp3 - P(5, 3) * _tmp7); + _kz(6, 0) = _tmp54 * (-P(6, 0) * _tmp5 + P(6, 1) * _tmp1 + P(6, 2) * _tmp3 - P(6, 3) * _tmp7); + _kz(7, 0) = _tmp54 * (-P(7, 0) * _tmp5 + P(7, 1) * _tmp1 + P(7, 2) * _tmp3 - P(7, 3) * _tmp7); + _kz(8, 0) = _tmp54 * (-P(8, 3) * _tmp7 - _tmp48 * state(0, 0) + _tmp49 * state(1, 0) + + _tmp50 * state(2, 0)); + _kz(9, 0) = + _tmp54 * (P(9, 1) * _tmp1 - P(9, 3) * _tmp7 + _tmp51 * state(2, 0) - _tmp52 * state(0, 0)); + _kz(10, 0) = + _tmp54 * (-P(10, 0) * _tmp5 + P(10, 1) * _tmp1 + P(10, 2) * _tmp3 - P(10, 3) * _tmp7); + _kz(11, 0) = + _tmp54 * (-P(11, 0) * _tmp5 + P(11, 1) * _tmp1 + P(11, 2) * _tmp3 - P(11, 3) * _tmp7); + _kz(12, 0) = + _tmp54 * (-P(12, 0) * _tmp5 + P(12, 1) * _tmp1 + P(12, 2) * _tmp3 - P(12, 3) * _tmp7); + _kz(13, 0) = + _tmp54 * (-P(13, 0) * _tmp5 + P(13, 1) * _tmp1 + P(13, 2) * _tmp3 - P(13, 3) * _tmp7); + _kz(14, 0) = + _tmp54 * (-P(14, 0) * _tmp5 + P(14, 1) * _tmp1 + P(14, 2) * _tmp3 - P(14, 3) * _tmp7); + _kz(15, 0) = + _tmp54 * (-P(15, 0) * _tmp5 + P(15, 1) * _tmp1 + P(15, 2) * _tmp3 - P(15, 3) * _tmp7); + _kz(16, 0) = + _tmp54 * (-P(16, 0) * _tmp5 + P(16, 1) * _tmp1 + P(16, 2) * _tmp3 - P(16, 3) * _tmp7); + _kz(17, 0) = + _tmp54 * (-P(17, 0) * _tmp5 + P(17, 1) * _tmp1 + P(17, 2) * _tmp3 - P(17, 3) * _tmp7); + _kz(18, 0) = + _tmp54 * (-P(18, 0) * _tmp5 + P(18, 1) * _tmp1 + P(18, 2) * _tmp3 - P(18, 3) * _tmp7); + _kz(19, 0) = + _tmp54 * (-P(19, 0) * _tmp5 + P(19, 1) * _tmp1 + P(19, 2) * _tmp3 - P(19, 3) * _tmp7); + _kz(20, 0) = + _tmp54 * (-P(20, 0) * _tmp5 + P(20, 1) * _tmp1 + P(20, 2) * _tmp3 - P(20, 3) * _tmp7); + _kz(21, 0) = + _tmp54 * (-P(21, 0) * _tmp5 + P(21, 1) * _tmp1 + P(21, 2) * _tmp3 - P(21, 3) * _tmp7); + _kz(22, 0) = + _tmp54 * (-P(22, 0) * _tmp5 + P(22, 1) * _tmp1 + P(22, 2) * _tmp3 - P(22, 3) * _tmp7); + _kz(23, 0) = + _tmp54 * (-P(23, 0) * _tmp5 + P(23, 1) * _tmp1 + P(23, 2) * _tmp3 - P(23, 3) * _tmp7); + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym 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 index 9a01adc00a..e2d4835b25 100644 --- 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 @@ -28,9 +28,6 @@ namespace sym { * 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, @@ -41,74 +38,237 @@ void ComputyGravityInnovVarAndKAndH(const matrix::Matrix& state, 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; + matrix::Matrix* const Kz = nullptr) { + // Total ops: 734 // Input arrays - // Intermediate terms (1) + // Intermediate terms (55) 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)))); + -Scalar(9.8066499999999994) * + 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)))); + const Scalar _tmp1 = Scalar(19.613299999999999) * state(1, 0); + const Scalar _tmp2 = -P(3, 0) * _tmp1; + const Scalar _tmp3 = Scalar(19.613299999999999) * state(2, 0); + const Scalar _tmp4 = P(0, 0) * _tmp3; + const Scalar _tmp5 = Scalar(19.613299999999999) * state(0, 0); + const Scalar _tmp6 = P(2, 0) * _tmp5; + const Scalar _tmp7 = Scalar(19.613299999999999) * state(3, 0); + const Scalar _tmp8 = P(3, 1) * _tmp1; + const Scalar _tmp9 = P(2, 1) * _tmp5; + const Scalar _tmp10 = -P(1, 1) * _tmp7; + const Scalar _tmp11 = P(0, 2) * _tmp3; + const Scalar _tmp12 = P(2, 2) * _tmp5; + const Scalar _tmp13 = -P(1, 2) * _tmp7; + const Scalar _tmp14 = -P(3, 3) * _tmp1; + const Scalar _tmp15 = P(0, 3) * _tmp3; + const Scalar _tmp16 = -P(1, 3) * _tmp7; + const Scalar _tmp17 = R - _tmp1 * (P(2, 3) * _tmp5 + _tmp14 + _tmp15 + _tmp16) + + _tmp3 * (-P(1, 0) * _tmp7 + _tmp2 + _tmp4 + _tmp6) + + _tmp5 * (-P(3, 2) * _tmp1 + _tmp11 + _tmp12 + _tmp13) - + _tmp7 * (P(0, 1) * _tmp3 + _tmp10 - _tmp8 + _tmp9); + const Scalar _tmp18 = P(3, 0) * _tmp3; + const Scalar _tmp19 = -P(0, 0) * _tmp1; + const Scalar _tmp20 = -P(1, 0) * _tmp5; + const Scalar _tmp21 = P(3, 2) * _tmp3; + const Scalar _tmp22 = -P(2, 2) * _tmp7; + const Scalar _tmp23 = P(1, 2) * _tmp5; + const Scalar _tmp24 = P(0, 1) * _tmp1; + const Scalar _tmp25 = -P(2, 1) * _tmp7; + const Scalar _tmp26 = -P(1, 1) * _tmp5; + const Scalar _tmp27 = -P(3, 3) * _tmp3; + const Scalar _tmp28 = -P(0, 3) * _tmp1; + const Scalar _tmp29 = -P(2, 3) * _tmp7; + const Scalar _tmp30 = R - _tmp1 * (-P(2, 0) * _tmp7 - _tmp18 + _tmp19 + _tmp20) - + _tmp3 * (-P(1, 3) * _tmp5 + _tmp27 + _tmp28 + _tmp29) - + _tmp5 * (-P(3, 1) * _tmp3 - _tmp24 + _tmp25 + _tmp26) - + _tmp7 * (-P(0, 2) * _tmp1 - _tmp21 + _tmp22 - _tmp23); + const Scalar _tmp31 = -P(0, 0) * _tmp5; + const Scalar _tmp32 = P(2, 0) * _tmp3; + const Scalar _tmp33 = P(1, 0) * _tmp1; + const Scalar _tmp34 = -P(3, 2) * _tmp7; + const Scalar _tmp35 = P(0, 2) * _tmp5; + const Scalar _tmp36 = P(2, 2) * _tmp3; + const Scalar _tmp37 = -P(3, 1) * _tmp7; + const Scalar _tmp38 = -P(0, 1) * _tmp5; + const Scalar _tmp39 = P(1, 1) * _tmp1; + const Scalar _tmp40 = -P(3, 3) * _tmp7; + const Scalar _tmp41 = P(2, 3) * _tmp3; + const Scalar _tmp42 = P(1, 3) * _tmp1; + const Scalar _tmp43 = R + _tmp1 * (P(2, 1) * _tmp3 + _tmp37 + _tmp38 + _tmp39) + + _tmp3 * (P(1, 2) * _tmp1 + _tmp34 - _tmp35 + _tmp36) - + _tmp5 * (-P(3, 0) * _tmp7 + _tmp31 + _tmp32 + _tmp33) - + _tmp7 * (-P(0, 3) * _tmp5 + _tmp40 + _tmp41 + _tmp42); + const Scalar _tmp44 = Scalar(1.0) / (_tmp17); + const Scalar _tmp45 = Scalar(19.613299999999999) * P(4, 0); + const Scalar _tmp46 = Scalar(19.613299999999999) * P(4, 2); + const Scalar _tmp47 = Scalar(19.613299999999999) * P(8, 3); + const Scalar _tmp48 = Scalar(19.613299999999999) * P(8, 0); + const Scalar _tmp49 = Scalar(19.613299999999999) * P(8, 1); + const Scalar _tmp50 = Scalar(19.613299999999999) * P(8, 2); + const Scalar _tmp51 = Scalar(19.613299999999999) * P(9, 2); + const Scalar _tmp52 = Scalar(19.613299999999999) * P(9, 0); + const Scalar _tmp53 = Scalar(1.0) / (_tmp30); + const Scalar _tmp54 = Scalar(1.0) / (_tmp43); - // Output terms (8) + // Output terms (5) if (innov != nullptr) { matrix::Matrix& _innov = (*innov); - _innov(0, 0) = _tmp0; - _innov(1, 0) = _tmp0; - _innov(2, 0) = -Scalar(9.8066499999999994) * R + _tmp0; + _innov(0, 0) = _tmp0 + Scalar(19.613299999999999) * state(0, 0) * state(2, 0) - + Scalar(19.613299999999999) * state(1, 0) * state(3, 0); + _innov(1, 0) = _tmp0 - Scalar(19.613299999999999) * state(0, 0) * state(1, 0) - + Scalar(19.613299999999999) * state(2, 0) * state(3, 0); + _innov(2, 0) = _tmp0 - Scalar(9.8066499999999994) * std::pow(state(0, 0), Scalar(2)) + + Scalar(9.8066499999999994) * std::pow(state(1, 0), Scalar(2)) + + Scalar(9.8066499999999994) * std::pow(state(2, 0), Scalar(2)) - + Scalar(9.8066499999999994) * std::pow(state(3, 0), Scalar(2)); } 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; + _innov_var(0, 0) = _tmp17; + _innov_var(1, 0) = _tmp30; + _innov_var(2, 0) = _tmp43; } if (Kx != nullptr) { matrix::Matrix& _kx = (*Kx); - _kx.setZero(); + _kx(0, 0) = _tmp44 * (-P(0, 1) * _tmp7 + _tmp28 + _tmp35 + _tmp4); + _kx(1, 0) = _tmp44 * (P(1, 0) * _tmp3 + _tmp10 + _tmp23 - _tmp42); + _kx(2, 0) = _tmp44 * (-P(2, 3) * _tmp1 + _tmp12 + _tmp25 + _tmp32); + _kx(3, 0) = _tmp44 * (P(3, 2) * _tmp5 + _tmp14 + _tmp18 + _tmp37); + _kx(4, 0) = + _tmp44 * (-P(4, 1) * _tmp7 - P(4, 3) * _tmp1 + _tmp45 * state(2, 0) + _tmp46 * state(0, 0)); + _kx(5, 0) = _tmp44 * (P(5, 0) * _tmp3 - P(5, 1) * _tmp7 + P(5, 2) * _tmp5 - P(5, 3) * _tmp1); + _kx(6, 0) = _tmp44 * (P(6, 0) * _tmp3 - P(6, 1) * _tmp7 + P(6, 2) * _tmp5 - P(6, 3) * _tmp1); + _kx(7, 0) = _tmp44 * (P(7, 0) * _tmp3 - P(7, 1) * _tmp7 + P(7, 2) * _tmp5 - P(7, 3) * _tmp1); + _kx(8, 0) = _tmp44 * (-_tmp47 * state(1, 0) + _tmp48 * state(2, 0) - _tmp49 * state(3, 0) + + _tmp50 * state(0, 0)); + _kx(9, 0) = + _tmp44 * (-P(9, 1) * _tmp7 - P(9, 3) * _tmp1 + _tmp51 * state(0, 0) + _tmp52 * state(2, 0)); + _kx(10, 0) = + _tmp44 * (P(10, 0) * _tmp3 - P(10, 1) * _tmp7 + P(10, 2) * _tmp5 - P(10, 3) * _tmp1); + _kx(11, 0) = + _tmp44 * (P(11, 0) * _tmp3 - P(11, 1) * _tmp7 + P(11, 2) * _tmp5 - P(11, 3) * _tmp1); + _kx(12, 0) = + _tmp44 * (P(12, 0) * _tmp3 - P(12, 1) * _tmp7 + P(12, 2) * _tmp5 - P(12, 3) * _tmp1); + _kx(13, 0) = + _tmp44 * (P(13, 0) * _tmp3 - P(13, 1) * _tmp7 + P(13, 2) * _tmp5 - P(13, 3) * _tmp1); + _kx(14, 0) = + _tmp44 * (P(14, 0) * _tmp3 - P(14, 1) * _tmp7 + P(14, 2) * _tmp5 - P(14, 3) * _tmp1); + _kx(15, 0) = + _tmp44 * (P(15, 0) * _tmp3 - P(15, 1) * _tmp7 + P(15, 2) * _tmp5 - P(15, 3) * _tmp1); + _kx(16, 0) = + _tmp44 * (P(16, 0) * _tmp3 - P(16, 1) * _tmp7 + P(16, 2) * _tmp5 - P(16, 3) * _tmp1); + _kx(17, 0) = + _tmp44 * (P(17, 0) * _tmp3 - P(17, 1) * _tmp7 + P(17, 2) * _tmp5 - P(17, 3) * _tmp1); + _kx(18, 0) = + _tmp44 * (P(18, 0) * _tmp3 - P(18, 1) * _tmp7 + P(18, 2) * _tmp5 - P(18, 3) * _tmp1); + _kx(19, 0) = + _tmp44 * (P(19, 0) * _tmp3 - P(19, 1) * _tmp7 + P(19, 2) * _tmp5 - P(19, 3) * _tmp1); + _kx(20, 0) = + _tmp44 * (P(20, 0) * _tmp3 - P(20, 1) * _tmp7 + P(20, 2) * _tmp5 - P(20, 3) * _tmp1); + _kx(21, 0) = + _tmp44 * (P(21, 0) * _tmp3 - P(21, 1) * _tmp7 + P(21, 2) * _tmp5 - P(21, 3) * _tmp1); + _kx(22, 0) = + _tmp44 * (P(22, 0) * _tmp3 - P(22, 1) * _tmp7 + P(22, 2) * _tmp5 - P(22, 3) * _tmp1); + _kx(23, 0) = + _tmp44 * (P(23, 0) * _tmp3 - P(23, 1) * _tmp7 + P(23, 2) * _tmp5 - P(23, 3) * _tmp1); } if (Ky != nullptr) { matrix::Matrix& _ky = (*Ky); - _ky.setZero(); + _ky(0, 0) = _tmp53 * (-P(0, 2) * _tmp7 - _tmp15 + _tmp19 + _tmp38); + _ky(1, 0) = _tmp53 * (-P(1, 3) * _tmp3 + _tmp13 + _tmp26 - _tmp33); + _ky(2, 0) = _tmp53 * (-P(2, 0) * _tmp1 + _tmp22 - _tmp41 - _tmp9); + _ky(3, 0) = _tmp53 * (-P(3, 1) * _tmp5 + _tmp2 + _tmp27 + _tmp34); + _ky(4, 0) = _tmp53 * (-P(4, 0) * _tmp1 - P(4, 1) * _tmp5 - P(4, 2) * _tmp7 - P(4, 3) * _tmp3); + _ky(5, 0) = _tmp53 * (-P(5, 0) * _tmp1 - P(5, 1) * _tmp5 - P(5, 2) * _tmp7 - P(5, 3) * _tmp3); + _ky(6, 0) = _tmp53 * (-P(6, 0) * _tmp1 - P(6, 1) * _tmp5 - P(6, 2) * _tmp7 - P(6, 3) * _tmp3); + _ky(7, 0) = _tmp53 * (-P(7, 0) * _tmp1 - P(7, 1) * _tmp5 - P(7, 2) * _tmp7 - P(7, 3) * _tmp3); + _ky(8, 0) = _tmp53 * (-P(8, 2) * _tmp7 - _tmp47 * state(2, 0) - _tmp48 * state(1, 0) - + _tmp49 * state(0, 0)); + _ky(9, 0) = + _tmp53 * (-P(9, 1) * _tmp5 - P(9, 2) * _tmp7 - P(9, 3) * _tmp3 - _tmp52 * state(1, 0)); + _ky(10, 0) = + _tmp53 * (-P(10, 0) * _tmp1 - P(10, 1) * _tmp5 - P(10, 2) * _tmp7 - P(10, 3) * _tmp3); + _ky(11, 0) = + _tmp53 * (-P(11, 0) * _tmp1 - P(11, 1) * _tmp5 - P(11, 2) * _tmp7 - P(11, 3) * _tmp3); + _ky(12, 0) = + _tmp53 * (-P(12, 0) * _tmp1 - P(12, 1) * _tmp5 - P(12, 2) * _tmp7 - P(12, 3) * _tmp3); + _ky(13, 0) = + _tmp53 * (-P(13, 0) * _tmp1 - P(13, 1) * _tmp5 - P(13, 2) * _tmp7 - P(13, 3) * _tmp3); + _ky(14, 0) = + _tmp53 * (-P(14, 0) * _tmp1 - P(14, 1) * _tmp5 - P(14, 2) * _tmp7 - P(14, 3) * _tmp3); + _ky(15, 0) = + _tmp53 * (-P(15, 0) * _tmp1 - P(15, 1) * _tmp5 - P(15, 2) * _tmp7 - P(15, 3) * _tmp3); + _ky(16, 0) = + _tmp53 * (-P(16, 0) * _tmp1 - P(16, 1) * _tmp5 - P(16, 2) * _tmp7 - P(16, 3) * _tmp3); + _ky(17, 0) = + _tmp53 * (-P(17, 0) * _tmp1 - P(17, 1) * _tmp5 - P(17, 2) * _tmp7 - P(17, 3) * _tmp3); + _ky(18, 0) = + _tmp53 * (-P(18, 0) * _tmp1 - P(18, 1) * _tmp5 - P(18, 2) * _tmp7 - P(18, 3) * _tmp3); + _ky(19, 0) = + _tmp53 * (-P(19, 0) * _tmp1 - P(19, 1) * _tmp5 - P(19, 2) * _tmp7 - P(19, 3) * _tmp3); + _ky(20, 0) = + _tmp53 * (-P(20, 0) * _tmp1 - P(20, 1) * _tmp5 - P(20, 2) * _tmp7 - P(20, 3) * _tmp3); + _ky(21, 0) = + _tmp53 * (-P(21, 0) * _tmp1 - P(21, 1) * _tmp5 - P(21, 2) * _tmp7 - P(21, 3) * _tmp3); + _ky(22, 0) = + _tmp53 * (-P(22, 0) * _tmp1 - P(22, 1) * _tmp5 - P(22, 2) * _tmp7 - P(22, 3) * _tmp3); + _ky(23, 0) = + _tmp53 * (-P(23, 0) * _tmp1 - P(23, 1) * _tmp5 - P(23, 2) * _tmp7 - P(23, 3) * _tmp3); } 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(); + _kz(0, 0) = _tmp54 * (-P(0, 3) * _tmp7 + _tmp11 + _tmp24 + _tmp31); + _kz(1, 0) = _tmp54 * (P(1, 2) * _tmp3 + _tmp16 + _tmp20 + _tmp39); + _kz(2, 0) = _tmp54 * (P(2, 1) * _tmp1 + _tmp29 + _tmp36 - _tmp6); + _kz(3, 0) = _tmp54 * (-P(3, 0) * _tmp5 + _tmp21 + _tmp40 + _tmp8); + _kz(4, 0) = + _tmp54 * (P(4, 1) * _tmp1 - P(4, 3) * _tmp7 - _tmp45 * state(0, 0) + _tmp46 * state(2, 0)); + _kz(5, 0) = _tmp54 * (-P(5, 0) * _tmp5 + P(5, 1) * _tmp1 + P(5, 2) * _tmp3 - P(5, 3) * _tmp7); + _kz(6, 0) = _tmp54 * (-P(6, 0) * _tmp5 + P(6, 1) * _tmp1 + P(6, 2) * _tmp3 - P(6, 3) * _tmp7); + _kz(7, 0) = _tmp54 * (-P(7, 0) * _tmp5 + P(7, 1) * _tmp1 + P(7, 2) * _tmp3 - P(7, 3) * _tmp7); + _kz(8, 0) = _tmp54 * (-P(8, 3) * _tmp7 - _tmp48 * state(0, 0) + _tmp49 * state(1, 0) + + _tmp50 * state(2, 0)); + _kz(9, 0) = + _tmp54 * (P(9, 1) * _tmp1 - P(9, 3) * _tmp7 + _tmp51 * state(2, 0) - _tmp52 * state(0, 0)); + _kz(10, 0) = + _tmp54 * (-P(10, 0) * _tmp5 + P(10, 1) * _tmp1 + P(10, 2) * _tmp3 - P(10, 3) * _tmp7); + _kz(11, 0) = + _tmp54 * (-P(11, 0) * _tmp5 + P(11, 1) * _tmp1 + P(11, 2) * _tmp3 - P(11, 3) * _tmp7); + _kz(12, 0) = + _tmp54 * (-P(12, 0) * _tmp5 + P(12, 1) * _tmp1 + P(12, 2) * _tmp3 - P(12, 3) * _tmp7); + _kz(13, 0) = + _tmp54 * (-P(13, 0) * _tmp5 + P(13, 1) * _tmp1 + P(13, 2) * _tmp3 - P(13, 3) * _tmp7); + _kz(14, 0) = + _tmp54 * (-P(14, 0) * _tmp5 + P(14, 1) * _tmp1 + P(14, 2) * _tmp3 - P(14, 3) * _tmp7); + _kz(15, 0) = + _tmp54 * (-P(15, 0) * _tmp5 + P(15, 1) * _tmp1 + P(15, 2) * _tmp3 - P(15, 3) * _tmp7); + _kz(16, 0) = + _tmp54 * (-P(16, 0) * _tmp5 + P(16, 1) * _tmp1 + P(16, 2) * _tmp3 - P(16, 3) * _tmp7); + _kz(17, 0) = + _tmp54 * (-P(17, 0) * _tmp5 + P(17, 1) * _tmp1 + P(17, 2) * _tmp3 - P(17, 3) * _tmp7); + _kz(18, 0) = + _tmp54 * (-P(18, 0) * _tmp5 + P(18, 1) * _tmp1 + P(18, 2) * _tmp3 - P(18, 3) * _tmp7); + _kz(19, 0) = + _tmp54 * (-P(19, 0) * _tmp5 + P(19, 1) * _tmp1 + P(19, 2) * _tmp3 - P(19, 3) * _tmp7); + _kz(20, 0) = + _tmp54 * (-P(20, 0) * _tmp5 + P(20, 1) * _tmp1 + P(20, 2) * _tmp3 - P(20, 3) * _tmp7); + _kz(21, 0) = + _tmp54 * (-P(21, 0) * _tmp5 + P(21, 1) * _tmp1 + P(21, 2) * _tmp3 - P(21, 3) * _tmp7); + _kz(22, 0) = + _tmp54 * (-P(22, 0) * _tmp5 + P(22, 1) * _tmp1 + P(22, 2) * _tmp3 - P(22, 3) * _tmp7); + _kz(23, 0) = + _tmp54 * (-P(23, 0) * _tmp5 + P(23, 1) * _tmp1 + P(23, 2) * _tmp3 - P(23, 3) * _tmp7); } } // NOLINT(readability/fn_size)