From 9ab6b5dffd293b29bb503102fba0e31e6ef2ac86 Mon Sep 17 00:00:00 2001 From: bradley-solliday-skydio Date: Mon, 19 Dec 2022 14:26:06 -0800 Subject: [PATCH] [ImuFactor] tangent space Imu Factor Don't bother reviewing this yet At the moment, most of the work was done here by Hayk Martiros. However, while moving changes around, it proved practical to lose track of the original commit. Topic: tangent_space_imu_factor Relative: on_manifold_imu_factor_test Labels: draft --- dev_requirements.txt | 7 +- .../sym/factors/imu_preintegration_update.h | 1188 +++++++++++ .../sym/factors/imu_preintegration_update.py | 1896 +++++++++++++++++ setup.cfg | 3 + setup.py | 1 + symforce/slam/imu_preintegration/generate.py | 30 + .../imu_preintegration/preintegrated_imu.py | 105 + symforce/slam/imu_preintegration/symbolic.py | 87 + symforce/slam/imu_preintegration/test_util.py | 34 + test/symforce_gen_codegen_test.py | 11 + test/symforce_preintegrated_imu_test.py | 86 + 11 files changed, 3447 insertions(+), 1 deletion(-) create mode 100644 gen/cpp/sym/factors/imu_preintegration_update.h create mode 100644 gen/python/sym/factors/imu_preintegration_update.py create mode 100644 symforce/slam/imu_preintegration/preintegrated_imu.py create mode 100644 symforce/slam/imu_preintegration/symbolic.py create mode 100644 symforce/slam/imu_preintegration/test_util.py create mode 100644 test/symforce_preintegrated_imu_test.py diff --git a/dev_requirements.txt b/dev_requirements.txt index 186f4ce39..43a1d91f1 100644 --- a/dev_requirements.txt +++ b/dev_requirements.txt @@ -82,6 +82,8 @@ fonttools==4.38.0 # via matplotlib graphviz==0.20.1 # via symforce (setup.py) +gtsam==4.1.1 ; python_version < "3.10" + # via symforce (setup.py) idna==3.4 # via requests imagesize==1.4.1 @@ -192,6 +194,7 @@ numba==0.56.4 ; python_version < "3.11" numpy==1.23.5 # via # contourpy + # gtsam # matplotlib # numba # pandas @@ -253,7 +256,9 @@ pygments==2.13.0 pylint==2.15.9 # via symforce (setup.py) pyparsing==3.0.9 - # via matplotlib + # via + # gtsam + # matplotlib pyrsistent==0.19.2 # via jsonschema python-dateutil==2.8.2 diff --git a/gen/cpp/sym/factors/imu_preintegration_update.h b/gen/cpp/sym/factors/imu_preintegration_update.h new file mode 100644 index 000000000..3159f2219 --- /dev/null +++ b/gen/cpp/sym/factors/imu_preintegration_update.h @@ -0,0 +1,1188 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * Symbolic function to perform the full update of incorporating a new IMU measurement + * into the state of a preintegrated IMU factor. + */ +template +void ImuPreintegrationUpdate( + const Eigen::Matrix& state, const Eigen::Matrix& state_cov, + const Eigen::Matrix& state_D_accel_bias, + const Eigen::Matrix& state_D_gyro_bias, + const Eigen::Matrix& accel_bias, const Eigen::Matrix& gyro_bias, + const Eigen::Matrix& accel_cov, const Eigen::Matrix& gyro_cov, + const Eigen::Matrix& accel, const Eigen::Matrix& gyro, + const Scalar dt, const Scalar epsilon, Eigen::Matrix* const new_state = nullptr, + Eigen::Matrix* const new_state_cov = nullptr, + Eigen::Matrix* const new_state_D_accel_bias = nullptr, + Eigen::Matrix* const new_state_D_gyro_bias = nullptr) { + // Total ops: 2652 + + // Input arrays + + // Intermediate terms (736) + const Scalar _tmp0 = gyro(0, 0) - gyro_bias(0, 0); + const Scalar _tmp1 = std::pow(dt, Scalar(2)); + const Scalar _tmp2 = gyro(1, 0) - gyro_bias(1, 0); + const Scalar _tmp3 = std::pow(_tmp2, Scalar(2)); + const Scalar _tmp4 = std::pow(_tmp0, Scalar(2)); + const Scalar _tmp5 = gyro(2, 0) - gyro_bias(2, 0); + const Scalar _tmp6 = std::pow(_tmp5, Scalar(2)); + const Scalar _tmp7 = std::pow(epsilon, Scalar(2)); + const Scalar _tmp8 = _tmp1 * _tmp3 + _tmp1 * _tmp4 + _tmp1 * _tmp6 + _tmp7; + const Scalar _tmp9 = std::sqrt(_tmp8); + const Scalar _tmp10 = Scalar(1.0) / (_tmp9); + const Scalar _tmp11 = std::pow(state(0, 0), Scalar(2)); + const Scalar _tmp12 = std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp13 = std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp14 = _tmp11 + _tmp12 + _tmp13 + _tmp7; + const Scalar _tmp15 = std::sqrt(_tmp14); + const Scalar _tmp16 = (Scalar(1) / Scalar(2)) * _tmp15; + const Scalar _tmp17 = std::cos(_tmp16); + const Scalar _tmp18 = (Scalar(1) / Scalar(2)) * _tmp9; + const Scalar _tmp19 = std::sin(_tmp18); + const Scalar _tmp20 = _tmp17 * _tmp19; + const Scalar _tmp21 = _tmp10 * _tmp20; + const Scalar _tmp22 = _tmp21 * dt; + const Scalar _tmp23 = _tmp0 * _tmp22; + const Scalar _tmp24 = Scalar(1.0) / (_tmp15); + const Scalar _tmp25 = std::sin(_tmp16); + const Scalar _tmp26 = _tmp24 * _tmp25; + const Scalar _tmp27 = _tmp19 * _tmp26; + const Scalar _tmp28 = _tmp10 * dt; + const Scalar _tmp29 = _tmp27 * _tmp28; + const Scalar _tmp30 = _tmp29 * state(2, 0); + const Scalar _tmp31 = _tmp2 * _tmp30; + const Scalar _tmp32 = std::cos(_tmp18); + const Scalar _tmp33 = _tmp25 * _tmp32; + const Scalar _tmp34 = _tmp24 * _tmp33; + const Scalar _tmp35 = _tmp34 * state(0, 0); + const Scalar _tmp36 = _tmp29 * state(1, 0); + const Scalar _tmp37 = _tmp36 * _tmp5; + const Scalar _tmp38 = _tmp23 - _tmp31 + _tmp35 + _tmp37; + const Scalar _tmp39 = _tmp17 * _tmp32; + const Scalar _tmp40 = _tmp30 * _tmp5; + const Scalar _tmp41 = _tmp27 * state(0, 0); + const Scalar _tmp42 = _tmp28 * _tmp41; + const Scalar _tmp43 = _tmp0 * _tmp42; + const Scalar _tmp44 = _tmp2 * _tmp36; + const Scalar _tmp45 = _tmp40 + _tmp43 + _tmp44; + const Scalar _tmp46 = _tmp39 - _tmp45; + const Scalar _tmp47 = std::fabs(_tmp46); + const Scalar _tmp48 = 1 - epsilon; + const Scalar _tmp49 = std::min(_tmp47, _tmp48); + const Scalar _tmp50 = 1 - std::pow(_tmp49, Scalar(2)); + const Scalar _tmp51 = std::acos(_tmp49); + const Scalar _tmp52 = (((_tmp46) > 0) - ((_tmp46) < 0)); + const Scalar _tmp53 = 2 * std::min(0, _tmp52) + 1; + const Scalar _tmp54 = 2 * _tmp53; + const Scalar _tmp55 = _tmp51 * _tmp54 / std::sqrt(_tmp50); + const Scalar _tmp56 = _tmp42 * _tmp5; + const Scalar _tmp57 = _tmp2 * _tmp22; + const Scalar _tmp58 = _tmp0 * _tmp30; + const Scalar _tmp59 = _tmp34 * state(1, 0); + const Scalar _tmp60 = -_tmp56 + _tmp57 + _tmp58 + _tmp59; + const Scalar _tmp61 = _tmp22 * _tmp5; + const Scalar _tmp62 = _tmp2 * _tmp42; + const Scalar _tmp63 = _tmp34 * state(2, 0); + const Scalar _tmp64 = _tmp0 * _tmp36; + const Scalar _tmp65 = _tmp61 + _tmp62 + _tmp63 - _tmp64; + const Scalar _tmp66 = Scalar(1.0) / (_tmp14); + const Scalar _tmp67 = std::pow(_tmp25, Scalar(2)); + const Scalar _tmp68 = _tmp66 * _tmp67; + const Scalar _tmp69 = _tmp13 * _tmp68; + const Scalar _tmp70 = -2 * _tmp69; + const Scalar _tmp71 = _tmp12 * _tmp68; + const Scalar _tmp72 = -2 * _tmp71; + const Scalar _tmp73 = _tmp70 + _tmp72 + 1; + const Scalar _tmp74 = accel(0, 0) - accel_bias(0, 0); + const Scalar _tmp75 = state(0, 0) * state(1, 0); + const Scalar _tmp76 = _tmp68 * _tmp75; + const Scalar _tmp77 = 2 * _tmp76; + const Scalar _tmp78 = 2 * _tmp17; + const Scalar _tmp79 = _tmp26 * _tmp78; + const Scalar _tmp80 = _tmp79 * state(2, 0); + const Scalar _tmp81 = _tmp77 - _tmp80; + const Scalar _tmp82 = accel(1, 0) - accel_bias(1, 0); + const Scalar _tmp83 = state(0, 0) * state(2, 0); + const Scalar _tmp84 = _tmp68 * _tmp83; + const Scalar _tmp85 = 2 * _tmp84; + const Scalar _tmp86 = _tmp79 * state(1, 0); + const Scalar _tmp87 = _tmp85 + _tmp86; + const Scalar _tmp88 = accel(2, 0) - accel_bias(2, 0); + const Scalar _tmp89 = dt * (_tmp73 * _tmp74 + _tmp81 * _tmp82 + _tmp87 * _tmp88); + const Scalar _tmp90 = Scalar(0.5) * std::fabs(dt); + const Scalar _tmp91 = _tmp11 * _tmp68; + const Scalar _tmp92 = 1 - 2 * _tmp91; + const Scalar _tmp93 = _tmp72 + _tmp92; + const Scalar _tmp94 = _tmp77 + _tmp80; + const Scalar _tmp95 = state(1, 0) * state(2, 0); + const Scalar _tmp96 = _tmp68 * _tmp95; + const Scalar _tmp97 = 2 * _tmp96; + const Scalar _tmp98 = _tmp79 * state(0, 0); + const Scalar _tmp99 = _tmp97 - _tmp98; + const Scalar _tmp100 = dt * (_tmp74 * _tmp94 + _tmp82 * _tmp93 + _tmp88 * _tmp99); + const Scalar _tmp101 = _tmp70 + _tmp92; + const Scalar _tmp102 = _tmp85 - _tmp86; + const Scalar _tmp103 = _tmp97 + _tmp98; + const Scalar _tmp104 = dt * (_tmp101 * _tmp88 + _tmp102 * _tmp74 + _tmp103 * _tmp82); + const Scalar _tmp105 = _tmp2 * _tmp5; + const Scalar _tmp106 = [&]() { + const Scalar base = dt; + return base * base * base; + }(); + const Scalar _tmp107 = std::pow(_tmp8, Scalar(Scalar(-3) / Scalar(2))); + const Scalar _tmp108 = _tmp106 * _tmp107; + const Scalar _tmp109 = _tmp27 * state(2, 0); + const Scalar _tmp110 = _tmp108 * _tmp109; + const Scalar _tmp111 = _tmp105 * _tmp110; + const Scalar _tmp112 = Scalar(1.0) / (_tmp8); + const Scalar _tmp113 = (Scalar(1) / Scalar(2)) * _tmp63; + const Scalar _tmp114 = _tmp112 * _tmp113; + const Scalar _tmp115 = _tmp105 * _tmp106; + const Scalar _tmp116 = _tmp114 * _tmp115; + const Scalar _tmp117 = (Scalar(1) / Scalar(2)) * _tmp1; + const Scalar _tmp118 = _tmp117 * _tmp21; + const Scalar _tmp119 = _tmp27 * state(1, 0); + const Scalar _tmp120 = _tmp108 * _tmp119; + const Scalar _tmp121 = _tmp106 * _tmp3; + const Scalar _tmp122 = (Scalar(1) / Scalar(2)) * _tmp59; + const Scalar _tmp123 = _tmp112 * _tmp122; + const Scalar _tmp124 = (Scalar(1) / Scalar(2)) * _tmp35; + const Scalar _tmp125 = _tmp112 * _tmp124; + const Scalar _tmp126 = _tmp0 * _tmp106; + const Scalar _tmp127 = _tmp126 * _tmp2; + const Scalar _tmp128 = _tmp108 * _tmp41; + const Scalar _tmp129 = _tmp0 * _tmp128; + const Scalar _tmp130 = _tmp125 * _tmp127 - _tmp129 * _tmp2; + const Scalar _tmp131 = + -_tmp111 + _tmp116 + _tmp118 * _tmp2 - _tmp120 * _tmp3 + _tmp121 * _tmp123 + _tmp130 + _tmp36; + const Scalar _tmp132 = _tmp38 * _tmp53; + const Scalar _tmp133 = -_tmp39 + _tmp45; + const Scalar _tmp134 = std::fabs(_tmp133); + const Scalar _tmp135 = std::min(_tmp134, _tmp48); + const Scalar _tmp136 = std::acos(_tmp135); + const Scalar _tmp137 = 1 - std::pow(_tmp135, Scalar(2)); + const Scalar _tmp138 = ((((-_tmp134 + _tmp48) > 0) - ((-_tmp134 + _tmp48) < 0)) + 1) * + (((_tmp133) > 0) - ((_tmp133) < 0)); + const Scalar _tmp139 = _tmp135 * _tmp136 * _tmp138 / (_tmp137 * std::sqrt(_tmp137)); + const Scalar _tmp140 = _tmp132 * _tmp139; + const Scalar _tmp141 = _tmp10 * _tmp117; + const Scalar _tmp142 = _tmp141 * _tmp41; + const Scalar _tmp143 = -_tmp105 * _tmp120 + _tmp115 * _tmp123; + const Scalar _tmp144 = _tmp107 * _tmp20; + const Scalar _tmp145 = _tmp106 * _tmp144; + const Scalar _tmp146 = _tmp0 * _tmp145; + const Scalar _tmp147 = (Scalar(1) / Scalar(2)) * _tmp39; + const Scalar _tmp148 = _tmp112 * _tmp147; + const Scalar _tmp149 = _tmp106 * _tmp148; + const Scalar _tmp150 = _tmp0 * _tmp149; + const Scalar _tmp151 = -_tmp146 * _tmp2 + _tmp150 * _tmp2; + const Scalar _tmp152 = _tmp136 * _tmp54 / std::sqrt(_tmp137); + const Scalar _tmp153 = _tmp138 / _tmp137; + const Scalar _tmp154 = _tmp132 * _tmp153; + const Scalar _tmp155 = _tmp131 * _tmp140 - _tmp131 * _tmp154 + + _tmp152 * (_tmp110 * _tmp3 - _tmp114 * _tmp121 - _tmp142 * _tmp2 + + _tmp143 + _tmp151 - _tmp30); + const Scalar _tmp156 = _tmp106 * _tmp6; + const Scalar _tmp157 = _tmp129 * _tmp5; + const Scalar _tmp158 = _tmp126 * _tmp5; + const Scalar _tmp159 = _tmp125 * _tmp158; + const Scalar _tmp160 = _tmp107 * _tmp156; + const Scalar _tmp161 = -_tmp109 * _tmp160 + _tmp114 * _tmp156 + _tmp118 * _tmp5 + _tmp143 - + _tmp157 + _tmp159 + _tmp30; + const Scalar _tmp162 = _tmp139 * _tmp161; + const Scalar _tmp163 = -_tmp146 * _tmp5 + _tmp150 * _tmp5; + const Scalar _tmp164 = _tmp132 * _tmp162 + + _tmp152 * (_tmp111 - _tmp116 - _tmp119 * _tmp160 + _tmp123 * _tmp156 - + _tmp142 * _tmp5 + _tmp163 + _tmp36) - + _tmp154 * _tmp161; + const Scalar _tmp165 = Scalar(1.0) / (dt); + const Scalar _tmp166 = _tmp164 * _tmp165; + const Scalar _tmp167 = _tmp0 * _tmp120; + const Scalar _tmp168 = _tmp167 * _tmp2; + const Scalar _tmp169 = _tmp123 * _tmp127; + const Scalar _tmp170 = _tmp106 * _tmp4; + const Scalar _tmp171 = _tmp114 * _tmp126; + const Scalar _tmp172 = _tmp0 * _tmp110; + const Scalar _tmp173 = _tmp171 * _tmp5 - _tmp172 * _tmp5; + const Scalar _tmp174 = + _tmp0 * _tmp118 + _tmp125 * _tmp170 - _tmp128 * _tmp4 - _tmp168 + _tmp169 + _tmp173 + _tmp42; + const Scalar _tmp175 = _tmp171 * _tmp2; + const Scalar _tmp176 = _tmp123 * _tmp158; + const Scalar _tmp177 = _tmp167 * _tmp5; + const Scalar _tmp178 = _tmp172 * _tmp2; + const Scalar _tmp179 = _tmp140 * _tmp174 + + _tmp152 * (-_tmp0 * _tmp142 - _tmp145 * _tmp4 + _tmp149 * _tmp4 - _tmp175 + + _tmp176 - _tmp177 + _tmp178 + _tmp22) - + _tmp154 * _tmp174; + const Scalar _tmp180 = _tmp165 * _tmp179; + const Scalar _tmp181 = _tmp155 * _tmp165; + const Scalar _tmp182 = + _tmp166 * gyro_cov(2, 1) + _tmp180 * gyro_cov(0, 1) + _tmp181 * gyro_cov(1, 1); + const Scalar _tmp183 = + _tmp166 * gyro_cov(2, 2) + _tmp180 * gyro_cov(0, 2) + _tmp181 * gyro_cov(1, 2); + const Scalar _tmp184 = _tmp165 * gyro_cov(0, 0); + const Scalar _tmp185 = _tmp166 * gyro_cov(2, 0) + _tmp179 * _tmp184 + _tmp181 * gyro_cov(1, 0); + const Scalar _tmp186 = std::pow(_tmp14, Scalar(Scalar(-3) / Scalar(2))); + const Scalar _tmp187 = _tmp186 * _tmp25; + const Scalar _tmp188 = _tmp187 * _tmp19 * _tmp28; + const Scalar _tmp189 = _tmp0 * _tmp188; + const Scalar _tmp190 = _tmp188 * _tmp2; + const Scalar _tmp191 = _tmp190 * _tmp75; + const Scalar _tmp192 = (Scalar(1) / Scalar(2)) * _tmp66; + const Scalar _tmp193 = _tmp192 * _tmp75; + const Scalar _tmp194 = _tmp193 * _tmp57; + const Scalar _tmp195 = _tmp11 * _tmp192; + const Scalar _tmp196 = _tmp0 * _tmp29; + const Scalar _tmp197 = -_tmp196; + const Scalar _tmp198 = _tmp192 * _tmp61; + const Scalar _tmp199 = _tmp188 * _tmp5; + const Scalar _tmp200 = -_tmp198 * _tmp83 + _tmp199 * _tmp83; + const Scalar _tmp201 = + _tmp11 * _tmp189 - _tmp124 + _tmp191 - _tmp194 - _tmp195 * _tmp23 + _tmp197 + _tmp200; + const Scalar _tmp202 = _tmp52 * ((((-_tmp47 + _tmp48) > 0) - ((-_tmp47 + _tmp48) < 0)) + 1); + const Scalar _tmp203 = _tmp202 * _tmp49 * _tmp51 / (_tmp50 * std::sqrt(_tmp50)); + const Scalar _tmp204 = _tmp201 * _tmp203; + const Scalar _tmp205 = _tmp190 * _tmp83; + const Scalar _tmp206 = _tmp198 * _tmp75; + const Scalar _tmp207 = _tmp199 * _tmp75; + const Scalar _tmp208 = _tmp147 * _tmp66; + const Scalar _tmp209 = _tmp192 * _tmp83; + const Scalar _tmp210 = _tmp209 * _tmp57; + const Scalar _tmp211 = _tmp186 * _tmp33; + const Scalar _tmp212 = _tmp202 / _tmp50; + const Scalar _tmp213 = _tmp201 * _tmp212; + const Scalar _tmp214 = _tmp132 * _tmp204 - _tmp132 * _tmp213 + + _tmp55 * (_tmp11 * _tmp208 - _tmp11 * _tmp211 + _tmp205 + _tmp206 - + _tmp207 - _tmp210 + _tmp34 - Scalar(1) / Scalar(2) * _tmp43); + const Scalar _tmp215 = _tmp29 * _tmp5; + const Scalar _tmp216 = _tmp13 * _tmp192; + const Scalar _tmp217 = _tmp192 * _tmp95; + const Scalar _tmp218 = _tmp190 * _tmp95 - _tmp217 * _tmp57; + const Scalar _tmp219 = _tmp208 * _tmp75 - _tmp211 * _tmp75; + const Scalar _tmp220 = _tmp2 * _tmp29; + const Scalar _tmp221 = -_tmp220; + const Scalar _tmp222 = _tmp199 * _tmp95; + const Scalar _tmp223 = _tmp198 * _tmp95; + const Scalar _tmp224 = _tmp189 * _tmp75 - _tmp193 * _tmp23; + const Scalar _tmp225 = + -_tmp122 + _tmp13 * _tmp190 - _tmp216 * _tmp57 + _tmp221 + _tmp222 - _tmp223 + _tmp224; + const Scalar _tmp226 = _tmp132 * _tmp203; + const Scalar _tmp227 = _tmp212 * _tmp225; + const Scalar _tmp228 = -_tmp132 * _tmp227 + _tmp225 * _tmp226 + + _tmp55 * (-_tmp13 * _tmp199 + _tmp215 + _tmp216 * _tmp61 + _tmp218 + + _tmp219 - Scalar(1) / Scalar(2) * _tmp64); + const Scalar _tmp229 = _tmp189 * _tmp83; + const Scalar _tmp230 = _tmp12 * _tmp188; + const Scalar _tmp231 = _tmp209 * _tmp23; + const Scalar _tmp232 = -_tmp215; + const Scalar _tmp233 = _tmp12 * _tmp192; + const Scalar _tmp234 = + -_tmp113 + _tmp218 + _tmp229 + _tmp230 * _tmp5 - _tmp231 + _tmp232 - _tmp233 * _tmp61; + const Scalar _tmp235 = _tmp212 * _tmp234; + const Scalar _tmp236 = _tmp208 * _tmp83 - _tmp211 * _tmp83; + const Scalar _tmp237 = -_tmp132 * _tmp235 + _tmp226 * _tmp234 + + _tmp55 * (_tmp2 * _tmp230 + _tmp221 - _tmp222 + _tmp223 - + _tmp233 * _tmp57 + _tmp236 - Scalar(1) / Scalar(2) * _tmp58); + const Scalar _tmp238 = + _tmp214 * state_cov(0, 0) + _tmp228 * state_cov(1, 0) + _tmp237 * state_cov(2, 0); + const Scalar _tmp239 = + _tmp214 * state_cov(0, 1) + _tmp228 * state_cov(1, 1) + _tmp237 * state_cov(2, 1); + const Scalar _tmp240 = + _tmp214 * state_cov(0, 2) + _tmp228 * state_cov(1, 2) + _tmp237 * state_cov(2, 2); + const Scalar _tmp241 = _tmp53 * _tmp60; + const Scalar _tmp242 = _tmp139 * _tmp174; + const Scalar _tmp243 = _tmp153 * _tmp241; + const Scalar _tmp244 = _tmp119 * _tmp141; + const Scalar _tmp245 = _tmp152 * (-_tmp0 * _tmp244 - _tmp110 * _tmp4 + _tmp114 * _tmp170 + + _tmp151 + _tmp157 - _tmp159 + _tmp30) - + _tmp174 * _tmp243 + _tmp241 * _tmp242; + const Scalar _tmp246 = -_tmp105 * _tmp145 + _tmp105 * _tmp149; + const Scalar _tmp247 = _tmp152 * (-_tmp125 * _tmp156 + _tmp160 * _tmp41 + _tmp173 - + _tmp244 * _tmp5 + _tmp246 - _tmp42) - + _tmp161 * _tmp243 + _tmp162 * _tmp241; + const Scalar _tmp248 = _tmp165 * _tmp247; + const Scalar _tmp249 = _tmp105 * _tmp128; + const Scalar _tmp250 = _tmp115 * _tmp125; + const Scalar _tmp251 = _tmp131 * _tmp153; + const Scalar _tmp252 = _tmp131 * _tmp139; + const Scalar _tmp253 = _tmp152 * (-_tmp145 * _tmp3 + _tmp149 * _tmp3 + _tmp175 - _tmp178 - + _tmp2 * _tmp244 + _tmp22 + _tmp249 - _tmp250) - + _tmp241 * _tmp251 + _tmp241 * _tmp252; + const Scalar _tmp254 = _tmp165 * _tmp253; + const Scalar _tmp255 = _tmp184 * _tmp245 + _tmp248 * gyro_cov(2, 0) + _tmp254 * gyro_cov(1, 0); + const Scalar _tmp256 = _tmp165 * gyro_cov(0, 1); + const Scalar _tmp257 = _tmp245 * _tmp256 + _tmp248 * gyro_cov(2, 1) + _tmp254 * gyro_cov(1, 1); + const Scalar _tmp258 = _tmp208 * _tmp95 - _tmp211 * _tmp95; + const Scalar _tmp259 = _tmp203 * _tmp241; + const Scalar _tmp260 = _tmp234 * _tmp259 - _tmp235 * _tmp241 + + _tmp55 * (-_tmp12 * _tmp189 + _tmp196 + _tmp200 + _tmp23 * _tmp233 + + _tmp258 - Scalar(1) / Scalar(2) * _tmp31); + const Scalar _tmp261 = _tmp204 * _tmp241 - _tmp213 * _tmp241 + + _tmp55 * (_tmp11 * _tmp199 - _tmp195 * _tmp61 + _tmp219 - _tmp229 + + _tmp231 + _tmp232 - Scalar(1) / Scalar(2) * _tmp62); + const Scalar _tmp262 = _tmp189 * _tmp95; + const Scalar _tmp263 = _tmp217 * _tmp23; + const Scalar _tmp264 = _tmp225 * _tmp259 - _tmp227 * _tmp241 + + _tmp55 * (_tmp13 * _tmp208 - _tmp13 * _tmp211 - _tmp206 + _tmp207 - + _tmp262 + _tmp263 + _tmp34 - Scalar(1) / Scalar(2) * _tmp44); + const Scalar _tmp265 = + _tmp260 * state_cov(2, 1) + _tmp261 * state_cov(0, 1) + _tmp264 * state_cov(1, 1); + const Scalar _tmp266 = _tmp165 * gyro_cov(0, 2); + const Scalar _tmp267 = _tmp245 * _tmp266 + _tmp248 * gyro_cov(2, 2) + _tmp254 * gyro_cov(1, 2); + const Scalar _tmp268 = + _tmp260 * state_cov(2, 0) + _tmp261 * state_cov(0, 0) + _tmp264 * state_cov(1, 0); + const Scalar _tmp269 = + _tmp260 * state_cov(2, 2) + _tmp261 * state_cov(0, 2) + _tmp264 * state_cov(1, 2); + const Scalar _tmp270 = _tmp109 * _tmp141; + const Scalar _tmp271 = _tmp53 * _tmp65; + const Scalar _tmp272 = _tmp153 * _tmp271; + const Scalar _tmp273 = _tmp152 * (-_tmp144 * _tmp156 + _tmp148 * _tmp156 - _tmp176 + _tmp177 + + _tmp22 - _tmp249 + _tmp250 - _tmp270 * _tmp5) - + _tmp161 * _tmp272 + _tmp162 * _tmp271; + const Scalar _tmp274 = _tmp165 * _tmp273; + const Scalar _tmp275 = _tmp152 * (-_tmp0 * _tmp270 + _tmp120 * _tmp4 - _tmp123 * _tmp170 + + _tmp130 + _tmp163 - _tmp36) - + _tmp174 * _tmp272 + _tmp242 * _tmp271; + const Scalar _tmp276 = _tmp152 * (_tmp121 * _tmp125 - _tmp128 * _tmp3 + _tmp168 - _tmp169 - + _tmp2 * _tmp270 + _tmp246 + _tmp42) - + _tmp251 * _tmp271 + _tmp252 * _tmp271; + const Scalar _tmp277 = _tmp165 * _tmp276; + const Scalar _tmp278 = _tmp256 * _tmp275 + _tmp274 * gyro_cov(2, 1) + _tmp277 * gyro_cov(1, 1); + const Scalar _tmp279 = _tmp266 * _tmp275 + _tmp274 * gyro_cov(2, 2) + _tmp277 * gyro_cov(1, 2); + const Scalar _tmp280 = _tmp203 * _tmp271; + const Scalar _tmp281 = _tmp225 * _tmp280 - _tmp227 * _tmp271 + + _tmp55 * (_tmp13 * _tmp189 - _tmp191 + _tmp194 + _tmp197 - + _tmp216 * _tmp23 + _tmp258 - Scalar(1) / Scalar(2) * _tmp37); + const Scalar _tmp282 = _tmp234 * _tmp280 - _tmp235 * _tmp271 + + _tmp55 * (_tmp12 * _tmp208 - _tmp12 * _tmp211 - _tmp205 + _tmp210 + + _tmp262 - _tmp263 + _tmp34 - Scalar(1) / Scalar(2) * _tmp40); + const Scalar _tmp283 = _tmp204 * _tmp271 - _tmp213 * _tmp271 + + _tmp55 * (-_tmp11 * _tmp190 + _tmp195 * _tmp57 + _tmp220 + _tmp224 + + _tmp236 - Scalar(1) / Scalar(2) * _tmp56); + const Scalar _tmp284 = + _tmp281 * state_cov(1, 0) + _tmp282 * state_cov(2, 0) + _tmp283 * state_cov(0, 0); + const Scalar _tmp285 = + _tmp281 * state_cov(1, 2) + _tmp282 * state_cov(2, 2) + _tmp283 * state_cov(0, 2); + const Scalar _tmp286 = + _tmp281 * state_cov(1, 1) + _tmp282 * state_cov(2, 1) + _tmp283 * state_cov(0, 1); + const Scalar _tmp287 = _tmp184 * _tmp275 + _tmp274 * gyro_cov(2, 0) + _tmp277 * gyro_cov(1, 0); + const Scalar _tmp288 = _tmp187 * _tmp78; + const Scalar _tmp289 = _tmp13 * _tmp288; + const Scalar _tmp290 = _tmp289 * state(2, 0); + const Scalar _tmp291 = 4 * _tmp67 / std::pow(_tmp14, Scalar(2)); + const Scalar _tmp292 = _tmp13 * _tmp291; + const Scalar _tmp293 = _tmp292 * state(2, 0); + const Scalar _tmp294 = -_tmp290 + _tmp293; + const Scalar _tmp295 = 4 * _tmp68; + const Scalar _tmp296 = [&]() { + const Scalar base = state(2, 0); + return base * base * base; + }(); + const Scalar _tmp297 = -_tmp288 * _tmp296 + _tmp291 * _tmp296 - _tmp295 * state(2, 0); + const Scalar _tmp298 = _tmp288 * _tmp95; + const Scalar _tmp299 = std::pow(_tmp17, Scalar(2)) * _tmp66; + const Scalar _tmp300 = _tmp299 * _tmp95; + const Scalar _tmp301 = -_tmp298 + _tmp300 - _tmp96; + const Scalar _tmp302 = 2 * _tmp68; + const Scalar _tmp303 = _tmp302 * state(0, 0); + const Scalar _tmp304 = _tmp12 * _tmp291 * state(0, 0); + const Scalar _tmp305 = _tmp12 * _tmp288; + const Scalar _tmp306 = _tmp305 * state(0, 0); + const Scalar _tmp307 = _tmp303 - _tmp304 + _tmp306; + const Scalar _tmp308 = _tmp12 * _tmp299; + const Scalar _tmp309 = _tmp291 * state(2, 0); + const Scalar _tmp310 = _tmp288 * _tmp75; + const Scalar _tmp311 = -_tmp309 * _tmp75 + _tmp310 * state(2, 0); + const Scalar _tmp312 = _tmp311 - _tmp79; + const Scalar _tmp313 = + dt * (_tmp74 * (_tmp294 + _tmp297) + _tmp82 * (_tmp305 - _tmp308 + _tmp312 + _tmp71) + + _tmp88 * (_tmp301 + _tmp307)); + const Scalar _tmp314 = _tmp313 * state_cov(2, 1); + const Scalar _tmp315 = _tmp289 * state(0, 0); + const Scalar _tmp316 = _tmp292 * state(0, 0); + const Scalar _tmp317 = _tmp303 + _tmp315 - _tmp316; + const Scalar _tmp318 = _tmp298 - _tmp300 + _tmp96; + const Scalar _tmp319 = _tmp305 * state(1, 0); + const Scalar _tmp320 = _tmp291 * state(1, 0); + const Scalar _tmp321 = _tmp12 * _tmp320; + const Scalar _tmp322 = -_tmp319 + _tmp321; + const Scalar _tmp323 = [&]() { + const Scalar base = state(1, 0); + return base * base * base; + }(); + const Scalar _tmp324 = -_tmp288 * _tmp323 + _tmp291 * _tmp323 - _tmp295 * state(1, 0); + const Scalar _tmp325 = _tmp13 * _tmp299; + const Scalar _tmp326 = _tmp311 + _tmp79; + const Scalar _tmp327 = _tmp74 * (_tmp322 + _tmp324) + _tmp82 * (_tmp317 + _tmp318) + + _tmp88 * (-_tmp289 + _tmp325 + _tmp326 - _tmp69); + const Scalar _tmp328 = _tmp327 * dt; + const Scalar _tmp329 = _tmp328 * state_cov(1, 1); + const Scalar _tmp330 = _tmp304 - _tmp306; + const Scalar _tmp331 = -_tmp315 + _tmp316; + const Scalar _tmp332 = _tmp299 * _tmp75; + const Scalar _tmp333 = -_tmp310 + _tmp332 - _tmp76; + const Scalar _tmp334 = _tmp302 * state(2, 0); + const Scalar _tmp335 = _tmp11 * _tmp309; + const Scalar _tmp336 = _tmp11 * _tmp288; + const Scalar _tmp337 = _tmp336 * state(2, 0); + const Scalar _tmp338 = _tmp334 - _tmp335 + _tmp337; + const Scalar _tmp339 = _tmp299 * _tmp83; + const Scalar _tmp340 = _tmp288 * _tmp83; + const Scalar _tmp341 = -_tmp339 + _tmp340 + _tmp84; + const Scalar _tmp342 = _tmp302 * state(1, 0); + const Scalar _tmp343 = _tmp336 * state(1, 0); + const Scalar _tmp344 = _tmp11 * _tmp320; + const Scalar _tmp345 = _tmp342 + _tmp343 - _tmp344; + const Scalar _tmp346 = + _tmp74 * (_tmp330 + _tmp331) + _tmp82 * (_tmp341 + _tmp345) + _tmp88 * (_tmp333 + _tmp338); + const Scalar _tmp347 = _tmp346 * dt; + const Scalar _tmp348 = _tmp347 * state_cov(0, 1); + const Scalar _tmp349 = _tmp314 * _tmp90 + _tmp329 * _tmp90 + _tmp348 * _tmp90 + + dt * state_cov(6, 1) + state_cov(3, 1); + const Scalar _tmp350 = _tmp313 * state_cov(2, 0); + const Scalar _tmp351 = _tmp328 * state_cov(1, 0); + const Scalar _tmp352 = _tmp347 * state_cov(0, 0); + const Scalar _tmp353 = _tmp350 * _tmp90 + _tmp351 * _tmp90 + _tmp352 * _tmp90 + + dt * state_cov(6, 0) + state_cov(3, 0); + const Scalar _tmp354 = _tmp313 * state_cov(2, 2); + const Scalar _tmp355 = _tmp328 * state_cov(1, 2); + const Scalar _tmp356 = _tmp347 * state_cov(0, 2); + const Scalar _tmp357 = _tmp354 * _tmp90 + _tmp355 * _tmp90 + _tmp356 * _tmp90 + + dt * state_cov(6, 2) + state_cov(3, 2); + const Scalar _tmp358 = -_tmp343 + _tmp344; + const Scalar _tmp359 = _tmp290 - _tmp293 + _tmp334; + const Scalar _tmp360 = _tmp310 - _tmp332 + _tmp76; + const Scalar _tmp361 = + _tmp74 * (_tmp301 + _tmp317) + _tmp82 * (_tmp322 + _tmp358) + _tmp88 * (_tmp359 + _tmp360); + const Scalar _tmp362 = _tmp361 * dt; + const Scalar _tmp363 = _tmp362 * state_cov(1, 0); + const Scalar _tmp364 = [&]() { + const Scalar base = state(0, 0); + return base * base * base; + }(); + const Scalar _tmp365 = -_tmp288 * _tmp364 + _tmp291 * _tmp364 - _tmp295 * state(0, 0); + const Scalar _tmp366 = _tmp339 - _tmp340 - _tmp84; + const Scalar _tmp367 = _tmp11 * _tmp299; + const Scalar _tmp368 = _tmp74 * (_tmp345 + _tmp366) + _tmp82 * (_tmp330 + _tmp365) + + _tmp88 * (_tmp312 + _tmp336 - _tmp367 + _tmp91); + const Scalar _tmp369 = _tmp368 * dt; + const Scalar _tmp370 = _tmp369 * state_cov(0, 0); + const Scalar _tmp371 = _tmp319 - _tmp321 + _tmp342; + const Scalar _tmp372 = _tmp335 - _tmp337; + const Scalar _tmp373 = dt * (_tmp74 * (-_tmp305 + _tmp308 + _tmp326 - _tmp71) + + _tmp82 * (_tmp297 + _tmp372) + _tmp88 * (_tmp341 + _tmp371)); + const Scalar _tmp374 = _tmp373 * state_cov(2, 0); + const Scalar _tmp375 = _tmp363 * _tmp90 + _tmp370 * _tmp90 + _tmp374 * _tmp90 + + dt * state_cov(7, 0) + state_cov(4, 0); + const Scalar _tmp376 = _tmp362 * state_cov(1, 1); + const Scalar _tmp377 = _tmp369 * state_cov(0, 1); + const Scalar _tmp378 = _tmp373 * state_cov(2, 1); + const Scalar _tmp379 = _tmp376 * _tmp90 + _tmp377 * _tmp90 + _tmp378 * _tmp90 + + dt * state_cov(7, 1) + state_cov(4, 1); + const Scalar _tmp380 = _tmp362 * state_cov(1, 2); + const Scalar _tmp381 = _tmp369 * state_cov(0, 2); + const Scalar _tmp382 = _tmp373 * state_cov(2, 2); + const Scalar _tmp383 = _tmp380 * _tmp90 + _tmp381 * _tmp90 + _tmp382 * _tmp90 + + dt * state_cov(7, 2) + state_cov(4, 2); + const Scalar _tmp384 = _tmp74 * (_tmp338 + _tmp360) + + _tmp82 * (_tmp326 - _tmp336 + _tmp367 - _tmp91) + + _tmp88 * (_tmp331 + _tmp365); + const Scalar _tmp385 = _tmp384 * dt; + const Scalar _tmp386 = _tmp385 * state_cov(0, 0); + const Scalar _tmp387 = _tmp74 * (_tmp289 + _tmp312 - _tmp325 + _tmp69) + + _tmp82 * (_tmp333 + _tmp359) + _tmp88 * (_tmp324 + _tmp358); + const Scalar _tmp388 = _tmp387 * dt; + const Scalar _tmp389 = _tmp388 * state_cov(1, 0); + const Scalar _tmp390 = dt * (_tmp74 * (_tmp307 + _tmp318) + _tmp82 * (_tmp366 + _tmp371) + + _tmp88 * (_tmp294 + _tmp372)); + const Scalar _tmp391 = _tmp390 * state_cov(2, 0); + const Scalar _tmp392 = _tmp386 * _tmp90 + _tmp389 * _tmp90 + _tmp391 * _tmp90 + + dt * state_cov(8, 0) + state_cov(5, 0); + const Scalar _tmp393 = _tmp385 * state_cov(0, 1); + const Scalar _tmp394 = _tmp388 * state_cov(1, 1); + const Scalar _tmp395 = _tmp390 * state_cov(2, 1); + const Scalar _tmp396 = _tmp393 * _tmp90 + _tmp394 * _tmp90 + _tmp395 * _tmp90 + + dt * state_cov(8, 1) + state_cov(5, 1); + const Scalar _tmp397 = _tmp385 * state_cov(0, 2); + const Scalar _tmp398 = _tmp388 * state_cov(1, 2); + const Scalar _tmp399 = _tmp390 * state_cov(2, 2); + const Scalar _tmp400 = _tmp397 * _tmp90 + _tmp398 * _tmp90 + _tmp399 * _tmp90 + + dt * state_cov(8, 2) + state_cov(5, 2); + const Scalar _tmp401 = _tmp314 + _tmp329 + _tmp348 + state_cov(6, 1); + const Scalar _tmp402 = _tmp354 + _tmp355 + _tmp356 + state_cov(6, 2); + const Scalar _tmp403 = _tmp350 + _tmp351 + _tmp352 + state_cov(6, 0); + const Scalar _tmp404 = _tmp363 + _tmp370 + _tmp374 + state_cov(7, 0); + const Scalar _tmp405 = _tmp380 + _tmp381 + _tmp382 + state_cov(7, 2); + const Scalar _tmp406 = _tmp376 + _tmp377 + _tmp378 + state_cov(7, 1); + const Scalar _tmp407 = _tmp393 + _tmp394 + _tmp395 + state_cov(8, 1); + const Scalar _tmp408 = _tmp397 + _tmp398 + _tmp399 + state_cov(8, 2); + const Scalar _tmp409 = _tmp386 + _tmp389 + _tmp391 + state_cov(8, 0); + const Scalar _tmp410 = + _tmp214 * state_cov(0, 6) + _tmp228 * state_cov(1, 6) + _tmp237 * state_cov(2, 6); + const Scalar _tmp411 = _tmp238 * _tmp347; + const Scalar _tmp412 = _tmp239 * _tmp328; + const Scalar _tmp413 = _tmp240 * _tmp313; + const Scalar _tmp414 = + _tmp260 * state_cov(2, 6) + _tmp261 * state_cov(0, 6) + _tmp264 * state_cov(1, 6); + const Scalar _tmp415 = _tmp265 * _tmp328; + const Scalar _tmp416 = _tmp269 * _tmp313; + const Scalar _tmp417 = _tmp268 * _tmp347; + const Scalar _tmp418 = _tmp284 * dt; + const Scalar _tmp419 = _tmp346 * _tmp418; + const Scalar _tmp420 = _tmp285 * _tmp313; + const Scalar _tmp421 = _tmp286 * dt; + const Scalar _tmp422 = _tmp327 * _tmp421; + const Scalar _tmp423 = + _tmp281 * state_cov(1, 6) + _tmp282 * state_cov(2, 6) + _tmp283 * state_cov(0, 6); + const Scalar _tmp424 = _tmp313 * _tmp357; + const Scalar _tmp425 = _tmp87 * accel_cov(2, 2); + const Scalar _tmp426 = _tmp81 * accel_cov(1, 2); + const Scalar _tmp427 = _tmp73 * accel_cov(0, 2); + const Scalar _tmp428 = _tmp425 * _tmp90 + _tmp426 * _tmp90 + _tmp427 * _tmp90; + const Scalar _tmp429 = _tmp87 * dt; + const Scalar _tmp430 = _tmp428 * _tmp429; + const Scalar _tmp431 = _tmp313 * state_cov(2, 3); + const Scalar _tmp432 = _tmp87 * accel_cov(2, 1); + const Scalar _tmp433 = _tmp81 * accel_cov(1, 1); + const Scalar _tmp434 = _tmp73 * accel_cov(0, 1); + const Scalar _tmp435 = _tmp432 * _tmp90 + _tmp433 * _tmp90 + _tmp434 * _tmp90; + const Scalar _tmp436 = _tmp81 * dt; + const Scalar _tmp437 = _tmp435 * _tmp436; + const Scalar _tmp438 = _tmp87 * accel_cov(2, 0); + const Scalar _tmp439 = _tmp81 * accel_cov(1, 0); + const Scalar _tmp440 = _tmp73 * accel_cov(0, 0); + const Scalar _tmp441 = _tmp438 * _tmp90 + _tmp439 * _tmp90 + _tmp440 * _tmp90; + const Scalar _tmp442 = _tmp73 * dt; + const Scalar _tmp443 = _tmp441 * _tmp442; + const Scalar _tmp444 = _tmp328 * state_cov(1, 3); + const Scalar _tmp445 = _tmp347 * _tmp353; + const Scalar _tmp446 = _tmp328 * _tmp349; + const Scalar _tmp447 = _tmp313 * state_cov(2, 6); + const Scalar _tmp448 = _tmp328 * state_cov(1, 6); + const Scalar _tmp449 = _tmp347 * state_cov(0, 6); + const Scalar _tmp450 = _tmp447 * _tmp90 + _tmp448 * _tmp90 + _tmp449 * _tmp90 + + dt * state_cov(6, 6) + state_cov(3, 6); + const Scalar _tmp451 = _tmp347 * state_cov(0, 3); + const Scalar _tmp452 = _tmp347 * _tmp375; + const Scalar _tmp453 = _tmp362 * state_cov(1, 3); + const Scalar _tmp454 = _tmp328 * _tmp379; + const Scalar _tmp455 = _tmp313 * _tmp383; + const Scalar _tmp456 = _tmp93 * accel_cov(1, 2); + const Scalar _tmp457 = _tmp94 * accel_cov(0, 2); + const Scalar _tmp458 = _tmp99 * accel_cov(2, 2); + const Scalar _tmp459 = _tmp456 * _tmp90 + _tmp457 * _tmp90 + _tmp458 * _tmp90; + const Scalar _tmp460 = _tmp429 * _tmp459; + const Scalar _tmp461 = _tmp369 * state_cov(0, 3); + const Scalar _tmp462 = _tmp93 * accel_cov(1, 0); + const Scalar _tmp463 = _tmp94 * accel_cov(0, 0); + const Scalar _tmp464 = _tmp99 * accel_cov(2, 0); + const Scalar _tmp465 = _tmp462 * _tmp90 + _tmp463 * _tmp90 + _tmp464 * _tmp90; + const Scalar _tmp466 = _tmp442 * _tmp465; + const Scalar _tmp467 = _tmp373 * state_cov(2, 3); + const Scalar _tmp468 = _tmp93 * accel_cov(1, 1); + const Scalar _tmp469 = _tmp94 * accel_cov(0, 1); + const Scalar _tmp470 = _tmp99 * accel_cov(2, 1); + const Scalar _tmp471 = _tmp468 * _tmp90 + _tmp469 * _tmp90 + _tmp470 * _tmp90; + const Scalar _tmp472 = _tmp436 * _tmp471; + const Scalar _tmp473 = _tmp362 * state_cov(1, 6); + const Scalar _tmp474 = _tmp369 * state_cov(0, 6); + const Scalar _tmp475 = _tmp373 * state_cov(2, 6); + const Scalar _tmp476 = _tmp473 * _tmp90 + _tmp474 * _tmp90 + _tmp475 * _tmp90 + + dt * state_cov(7, 6) + state_cov(4, 6); + const Scalar _tmp477 = _tmp347 * _tmp392; + const Scalar _tmp478 = _tmp328 * _tmp396; + const Scalar _tmp479 = _tmp385 * state_cov(0, 3); + const Scalar _tmp480 = _tmp102 * accel_cov(0, 2); + const Scalar _tmp481 = _tmp101 * accel_cov(2, 2); + const Scalar _tmp482 = _tmp103 * accel_cov(1, 2); + const Scalar _tmp483 = _tmp480 * _tmp90 + _tmp481 * _tmp90 + _tmp482 * _tmp90; + const Scalar _tmp484 = _tmp429 * _tmp483; + const Scalar _tmp485 = _tmp102 * accel_cov(0, 1); + const Scalar _tmp486 = _tmp101 * accel_cov(2, 1); + const Scalar _tmp487 = _tmp103 * accel_cov(1, 1); + const Scalar _tmp488 = _tmp485 * _tmp90 + _tmp486 * _tmp90 + _tmp487 * _tmp90; + const Scalar _tmp489 = _tmp436 * _tmp488; + const Scalar _tmp490 = _tmp388 * state_cov(1, 3); + const Scalar _tmp491 = _tmp313 * _tmp400; + const Scalar _tmp492 = _tmp390 * state_cov(2, 3); + const Scalar _tmp493 = _tmp385 * state_cov(0, 6); + const Scalar _tmp494 = _tmp388 * state_cov(1, 6); + const Scalar _tmp495 = _tmp390 * state_cov(2, 6); + const Scalar _tmp496 = _tmp493 * _tmp90 + _tmp494 * _tmp90 + _tmp495 * _tmp90 + + dt * state_cov(8, 6) + state_cov(5, 6); + const Scalar _tmp497 = _tmp102 * accel_cov(0, 0); + const Scalar _tmp498 = _tmp101 * accel_cov(2, 0); + const Scalar _tmp499 = _tmp103 * accel_cov(1, 0); + const Scalar _tmp500 = _tmp497 * _tmp90 + _tmp498 * _tmp90 + _tmp499 * _tmp90; + const Scalar _tmp501 = _tmp442 * _tmp500; + const Scalar _tmp502 = _tmp447 + _tmp448 + _tmp449 + state_cov(6, 6); + const Scalar _tmp503 = _tmp425 + _tmp426 + _tmp427; + const Scalar _tmp504 = _tmp429 * _tmp503; + const Scalar _tmp505 = _tmp313 * _tmp402; + const Scalar _tmp506 = _tmp328 * _tmp401; + const Scalar _tmp507 = _tmp347 * _tmp403; + const Scalar _tmp508 = _tmp432 + _tmp433 + _tmp434; + const Scalar _tmp509 = _tmp436 * _tmp508; + const Scalar _tmp510 = _tmp438 + _tmp439 + _tmp440; + const Scalar _tmp511 = _tmp442 * _tmp510; + const Scalar _tmp512 = _tmp328 * _tmp406; + const Scalar _tmp513 = _tmp347 * _tmp404; + const Scalar _tmp514 = _tmp468 + _tmp469 + _tmp470; + const Scalar _tmp515 = _tmp436 * _tmp514; + const Scalar _tmp516 = _tmp456 + _tmp457 + _tmp458; + const Scalar _tmp517 = _tmp429 * _tmp516; + const Scalar _tmp518 = _tmp462 + _tmp463 + _tmp464; + const Scalar _tmp519 = _tmp442 * _tmp518; + const Scalar _tmp520 = _tmp473 + _tmp474 + _tmp475 + state_cov(7, 6); + const Scalar _tmp521 = _tmp313 * _tmp405; + const Scalar _tmp522 = _tmp328 * _tmp407; + const Scalar _tmp523 = _tmp493 + _tmp494 + _tmp495 + state_cov(8, 6); + const Scalar _tmp524 = _tmp347 * _tmp409; + const Scalar _tmp525 = _tmp497 + _tmp498 + _tmp499; + const Scalar _tmp526 = _tmp442 * _tmp525; + const Scalar _tmp527 = _tmp313 * _tmp408; + const Scalar _tmp528 = _tmp480 + _tmp481 + _tmp482; + const Scalar _tmp529 = _tmp429 * _tmp528; + const Scalar _tmp530 = _tmp485 + _tmp486 + _tmp487; + const Scalar _tmp531 = _tmp436 * _tmp530; + const Scalar _tmp532 = + _tmp214 * state_cov(0, 7) + _tmp228 * state_cov(1, 7) + _tmp237 * state_cov(2, 7); + const Scalar _tmp533 = _tmp239 * _tmp362; + const Scalar _tmp534 = _tmp240 * _tmp373; + const Scalar _tmp535 = _tmp238 * _tmp369; + const Scalar _tmp536 = + _tmp260 * state_cov(2, 7) + _tmp261 * state_cov(0, 7) + _tmp264 * state_cov(1, 7); + const Scalar _tmp537 = _tmp268 * _tmp369; + const Scalar _tmp538 = _tmp265 * _tmp362; + const Scalar _tmp539 = _tmp269 * _tmp373; + const Scalar _tmp540 = _tmp285 * _tmp373; + const Scalar _tmp541 = _tmp361 * _tmp421; + const Scalar _tmp542 = + _tmp281 * state_cov(1, 7) + _tmp282 * state_cov(2, 7) + _tmp283 * state_cov(0, 7); + const Scalar _tmp543 = _tmp368 * _tmp418; + const Scalar _tmp544 = _tmp357 * _tmp373; + const Scalar _tmp545 = _tmp93 * dt; + const Scalar _tmp546 = _tmp435 * _tmp545; + const Scalar _tmp547 = _tmp349 * _tmp362; + const Scalar _tmp548 = _tmp313 * state_cov(2, 4); + const Scalar _tmp549 = _tmp94 * dt; + const Scalar _tmp550 = _tmp441 * _tmp549; + const Scalar _tmp551 = _tmp328 * state_cov(1, 4); + const Scalar _tmp552 = _tmp353 * _tmp369; + const Scalar _tmp553 = _tmp99 * dt; + const Scalar _tmp554 = _tmp428 * _tmp553; + const Scalar _tmp555 = _tmp313 * state_cov(2, 7); + const Scalar _tmp556 = _tmp328 * state_cov(1, 7); + const Scalar _tmp557 = _tmp347 * state_cov(0, 7); + const Scalar _tmp558 = _tmp555 * _tmp90 + _tmp556 * _tmp90 + _tmp557 * _tmp90 + + dt * state_cov(6, 7) + state_cov(3, 7); + const Scalar _tmp559 = _tmp347 * state_cov(0, 4); + const Scalar _tmp560 = _tmp362 * state_cov(1, 4); + const Scalar _tmp561 = _tmp369 * state_cov(0, 4); + const Scalar _tmp562 = _tmp373 * _tmp383; + const Scalar _tmp563 = _tmp373 * state_cov(2, 4); + const Scalar _tmp564 = _tmp459 * _tmp553; + const Scalar _tmp565 = _tmp465 * _tmp549; + const Scalar _tmp566 = _tmp362 * state_cov(1, 7); + const Scalar _tmp567 = _tmp369 * state_cov(0, 7); + const Scalar _tmp568 = _tmp373 * state_cov(2, 7); + const Scalar _tmp569 = _tmp566 * _tmp90 + _tmp567 * _tmp90 + _tmp568 * _tmp90 + + dt * state_cov(7, 7) + state_cov(4, 7); + const Scalar _tmp570 = _tmp362 * _tmp379; + const Scalar _tmp571 = _tmp471 * _tmp545; + const Scalar _tmp572 = _tmp369 * _tmp375; + const Scalar _tmp573 = _tmp385 * state_cov(0, 4); + const Scalar _tmp574 = _tmp362 * _tmp396; + const Scalar _tmp575 = _tmp488 * _tmp545; + const Scalar _tmp576 = _tmp500 * _tmp549; + const Scalar _tmp577 = _tmp373 * _tmp400; + const Scalar _tmp578 = _tmp369 * _tmp392; + const Scalar _tmp579 = _tmp388 * state_cov(1, 4); + const Scalar _tmp580 = _tmp483 * _tmp553; + const Scalar _tmp581 = _tmp390 * state_cov(2, 4); + const Scalar _tmp582 = _tmp385 * state_cov(0, 7); + const Scalar _tmp583 = _tmp388 * state_cov(1, 7); + const Scalar _tmp584 = _tmp390 * state_cov(2, 7); + const Scalar _tmp585 = _tmp582 * _tmp90 + _tmp583 * _tmp90 + _tmp584 * _tmp90 + + dt * state_cov(8, 7) + state_cov(5, 7); + const Scalar _tmp586 = _tmp510 * _tmp549; + const Scalar _tmp587 = _tmp508 * _tmp545; + const Scalar _tmp588 = _tmp555 + _tmp556 + _tmp557 + state_cov(6, 7); + const Scalar _tmp589 = _tmp369 * _tmp403; + const Scalar _tmp590 = _tmp362 * _tmp401; + const Scalar _tmp591 = _tmp503 * _tmp553; + const Scalar _tmp592 = _tmp373 * _tmp402; + const Scalar _tmp593 = _tmp373 * _tmp405; + const Scalar _tmp594 = _tmp362 * _tmp406; + const Scalar _tmp595 = _tmp518 * _tmp549; + const Scalar _tmp596 = _tmp369 * _tmp404; + const Scalar _tmp597 = _tmp516 * _tmp553; + const Scalar _tmp598 = _tmp514 * _tmp545; + const Scalar _tmp599 = _tmp566 + _tmp567 + _tmp568 + state_cov(7, 7); + const Scalar _tmp600 = _tmp373 * _tmp408; + const Scalar _tmp601 = _tmp528 * _tmp553; + const Scalar _tmp602 = _tmp530 * _tmp545; + const Scalar _tmp603 = _tmp582 + _tmp583 + _tmp584 + state_cov(8, 7); + const Scalar _tmp604 = _tmp369 * _tmp409; + const Scalar _tmp605 = _tmp525 * _tmp549; + const Scalar _tmp606 = _tmp362 * _tmp407; + const Scalar _tmp607 = _tmp238 * _tmp385; + const Scalar _tmp608 = + _tmp214 * state_cov(0, 8) + _tmp228 * state_cov(1, 8) + _tmp237 * state_cov(2, 8); + const Scalar _tmp609 = _tmp240 * _tmp390; + const Scalar _tmp610 = _tmp239 * _tmp388; + const Scalar _tmp611 = + _tmp260 * state_cov(2, 8) + _tmp261 * state_cov(0, 8) + _tmp264 * state_cov(1, 8); + const Scalar _tmp612 = _tmp265 * _tmp388; + const Scalar _tmp613 = _tmp269 * _tmp390; + const Scalar _tmp614 = _tmp268 * _tmp385; + const Scalar _tmp615 = _tmp387 * _tmp421; + const Scalar _tmp616 = + _tmp281 * state_cov(1, 8) + _tmp282 * state_cov(2, 8) + _tmp283 * state_cov(0, 8); + const Scalar _tmp617 = _tmp285 * _tmp390; + const Scalar _tmp618 = _tmp384 * _tmp418; + const Scalar _tmp619 = _tmp101 * dt; + const Scalar _tmp620 = _tmp428 * _tmp619; + const Scalar _tmp621 = _tmp357 * _tmp390; + const Scalar _tmp622 = _tmp349 * _tmp388; + const Scalar _tmp623 = _tmp102 * dt; + const Scalar _tmp624 = _tmp441 * _tmp623; + const Scalar _tmp625 = _tmp313 * state_cov(2, 5); + const Scalar _tmp626 = _tmp353 * _tmp385; + const Scalar _tmp627 = _tmp328 * state_cov(1, 5); + const Scalar _tmp628 = _tmp103 * dt; + const Scalar _tmp629 = _tmp435 * _tmp628; + const Scalar _tmp630 = _tmp313 * state_cov(2, 8); + const Scalar _tmp631 = _tmp328 * state_cov(1, 8); + const Scalar _tmp632 = _tmp347 * state_cov(0, 8); + const Scalar _tmp633 = _tmp630 * _tmp90 + _tmp631 * _tmp90 + _tmp632 * _tmp90 + + dt * state_cov(6, 8) + state_cov(3, 8); + const Scalar _tmp634 = _tmp347 * state_cov(0, 5); + const Scalar _tmp635 = _tmp379 * _tmp388; + const Scalar _tmp636 = _tmp362 * state_cov(1, 5); + const Scalar _tmp637 = _tmp465 * _tmp623; + const Scalar _tmp638 = _tmp369 * state_cov(0, 5); + const Scalar _tmp639 = _tmp383 * _tmp390; + const Scalar _tmp640 = _tmp471 * _tmp628; + const Scalar _tmp641 = _tmp373 * state_cov(2, 5); + const Scalar _tmp642 = _tmp375 * _tmp385; + const Scalar _tmp643 = _tmp459 * _tmp619; + const Scalar _tmp644 = _tmp362 * state_cov(1, 8); + const Scalar _tmp645 = _tmp369 * state_cov(0, 8); + const Scalar _tmp646 = _tmp373 * state_cov(2, 8); + const Scalar _tmp647 = _tmp644 * _tmp90 + _tmp645 * _tmp90 + _tmp646 * _tmp90 + + dt * state_cov(7, 8) + state_cov(4, 8); + const Scalar _tmp648 = _tmp385 * state_cov(0, 5); + const Scalar _tmp649 = _tmp500 * _tmp623; + const Scalar _tmp650 = _tmp483 * _tmp619; + const Scalar _tmp651 = _tmp390 * _tmp400; + const Scalar _tmp652 = _tmp388 * _tmp396; + const Scalar _tmp653 = _tmp388 * state_cov(1, 5); + const Scalar _tmp654 = _tmp488 * _tmp628; + const Scalar _tmp655 = _tmp390 * state_cov(2, 5); + const Scalar _tmp656 = _tmp385 * state_cov(0, 8); + const Scalar _tmp657 = _tmp388 * state_cov(1, 8); + const Scalar _tmp658 = _tmp390 * state_cov(2, 8); + const Scalar _tmp659 = _tmp656 * _tmp90 + _tmp657 * _tmp90 + _tmp658 * _tmp90 + + dt * state_cov(8, 8) + state_cov(5, 8); + const Scalar _tmp660 = _tmp385 * _tmp392; + const Scalar _tmp661 = _tmp510 * _tmp623; + const Scalar _tmp662 = _tmp390 * _tmp402; + const Scalar _tmp663 = _tmp630 + _tmp631 + _tmp632 + state_cov(6, 8); + const Scalar _tmp664 = _tmp503 * _tmp619; + const Scalar _tmp665 = _tmp508 * _tmp628; + const Scalar _tmp666 = _tmp385 * _tmp403; + const Scalar _tmp667 = _tmp388 * _tmp401; + const Scalar _tmp668 = _tmp390 * _tmp405; + const Scalar _tmp669 = _tmp388 * _tmp406; + const Scalar _tmp670 = _tmp385 * _tmp404; + const Scalar _tmp671 = _tmp516 * _tmp619; + const Scalar _tmp672 = _tmp518 * _tmp623; + const Scalar _tmp673 = _tmp514 * _tmp628; + const Scalar _tmp674 = _tmp644 + _tmp645 + _tmp646 + state_cov(7, 8); + const Scalar _tmp675 = _tmp388 * _tmp407; + const Scalar _tmp676 = _tmp530 * _tmp628; + const Scalar _tmp677 = _tmp528 * _tmp619; + const Scalar _tmp678 = _tmp656 + _tmp657 + _tmp658 + state_cov(8, 8); + const Scalar _tmp679 = _tmp525 * _tmp623; + const Scalar _tmp680 = _tmp385 * _tmp409; + const Scalar _tmp681 = _tmp390 * _tmp408; + const Scalar _tmp682 = _tmp347 * state_D_accel_bias(0, 0); + const Scalar _tmp683 = _tmp328 * state_D_accel_bias(1, 0); + const Scalar _tmp684 = _tmp313 * state_D_accel_bias(2, 0); + const Scalar _tmp685 = _tmp362 * state_D_accel_bias(1, 0); + const Scalar _tmp686 = _tmp373 * state_D_accel_bias(2, 0); + const Scalar _tmp687 = _tmp369 * state_D_accel_bias(0, 0); + const Scalar _tmp688 = _tmp388 * state_D_accel_bias(1, 0); + const Scalar _tmp689 = _tmp390 * state_D_accel_bias(2, 0); + const Scalar _tmp690 = _tmp385 * state_D_accel_bias(0, 0); + const Scalar _tmp691 = _tmp347 * state_D_accel_bias(0, 1); + const Scalar _tmp692 = _tmp328 * state_D_accel_bias(1, 1); + const Scalar _tmp693 = _tmp313 * state_D_accel_bias(2, 1); + const Scalar _tmp694 = _tmp362 * state_D_accel_bias(1, 1); + const Scalar _tmp695 = _tmp373 * state_D_accel_bias(2, 1); + const Scalar _tmp696 = _tmp369 * state_D_accel_bias(0, 1); + const Scalar _tmp697 = _tmp388 * state_D_accel_bias(1, 1); + const Scalar _tmp698 = _tmp390 * state_D_accel_bias(2, 1); + const Scalar _tmp699 = _tmp385 * state_D_accel_bias(0, 1); + const Scalar _tmp700 = _tmp347 * state_D_accel_bias(0, 2); + const Scalar _tmp701 = _tmp328 * state_D_accel_bias(1, 2); + const Scalar _tmp702 = _tmp313 * state_D_accel_bias(2, 2); + const Scalar _tmp703 = _tmp362 * state_D_accel_bias(1, 2); + const Scalar _tmp704 = _tmp373 * state_D_accel_bias(2, 2); + const Scalar _tmp705 = _tmp369 * state_D_accel_bias(0, 2); + const Scalar _tmp706 = _tmp388 * state_D_accel_bias(1, 2); + const Scalar _tmp707 = _tmp390 * state_D_accel_bias(2, 2); + const Scalar _tmp708 = _tmp385 * state_D_accel_bias(0, 2); + const Scalar _tmp709 = _tmp328 * state_D_gyro_bias(1, 0); + const Scalar _tmp710 = _tmp313 * state_D_gyro_bias(2, 0); + const Scalar _tmp711 = _tmp347 * state_D_gyro_bias(0, 0); + const Scalar _tmp712 = _tmp369 * state_D_gyro_bias(0, 0); + const Scalar _tmp713 = _tmp362 * state_D_gyro_bias(1, 0); + const Scalar _tmp714 = _tmp373 * state_D_gyro_bias(2, 0); + const Scalar _tmp715 = _tmp385 * state_D_gyro_bias(0, 0); + const Scalar _tmp716 = _tmp390 * state_D_gyro_bias(2, 0); + const Scalar _tmp717 = _tmp388 * state_D_gyro_bias(1, 0); + const Scalar _tmp718 = _tmp328 * state_D_gyro_bias(1, 1); + const Scalar _tmp719 = _tmp313 * state_D_gyro_bias(2, 1); + const Scalar _tmp720 = _tmp347 * state_D_gyro_bias(0, 1); + const Scalar _tmp721 = _tmp369 * state_D_gyro_bias(0, 1); + const Scalar _tmp722 = _tmp362 * state_D_gyro_bias(1, 1); + const Scalar _tmp723 = _tmp373 * state_D_gyro_bias(2, 1); + const Scalar _tmp724 = _tmp385 * state_D_gyro_bias(0, 1); + const Scalar _tmp725 = _tmp390 * state_D_gyro_bias(2, 1); + const Scalar _tmp726 = _tmp388 * state_D_gyro_bias(1, 1); + const Scalar _tmp727 = _tmp328 * state_D_gyro_bias(1, 2); + const Scalar _tmp728 = _tmp313 * state_D_gyro_bias(2, 2); + const Scalar _tmp729 = _tmp347 * state_D_gyro_bias(0, 2); + const Scalar _tmp730 = _tmp369 * state_D_gyro_bias(0, 2); + const Scalar _tmp731 = _tmp362 * state_D_gyro_bias(1, 2); + const Scalar _tmp732 = _tmp373 * state_D_gyro_bias(2, 2); + const Scalar _tmp733 = _tmp385 * state_D_gyro_bias(0, 2); + const Scalar _tmp734 = _tmp390 * state_D_gyro_bias(2, 2); + const Scalar _tmp735 = _tmp388 * state_D_gyro_bias(1, 2); + + // Output terms (4) + if (new_state != nullptr) { + Eigen::Matrix& _new_state = (*new_state); + + _new_state(0, 0) = _tmp38 * _tmp55; + _new_state(1, 0) = _tmp55 * _tmp60; + _new_state(2, 0) = _tmp55 * _tmp65; + _new_state(3, 0) = _tmp89 * _tmp90 + dt * state(6, 0) + state(3, 0); + _new_state(4, 0) = _tmp100 * _tmp90 + dt * state(7, 0) + state(4, 0); + _new_state(5, 0) = _tmp104 * _tmp90 + dt * state(8, 0) + state(5, 0); + _new_state(6, 0) = _tmp89 + state(6, 0); + _new_state(7, 0) = _tmp100 + state(7, 0); + _new_state(8, 0) = _tmp104 + state(8, 0); + } + + if (new_state_cov != nullptr) { + Eigen::Matrix& _new_state_cov = (*new_state_cov); + + _new_state_cov(0, 0) = _tmp155 * _tmp182 + _tmp164 * _tmp183 + _tmp179 * _tmp185 + + _tmp214 * _tmp238 + _tmp228 * _tmp239 + _tmp237 * _tmp240; + _new_state_cov(1, 0) = _tmp155 * _tmp257 + _tmp164 * _tmp267 + _tmp179 * _tmp255 + + _tmp214 * _tmp268 + _tmp228 * _tmp265 + _tmp237 * _tmp269; + _new_state_cov(2, 0) = _tmp155 * _tmp278 + _tmp164 * _tmp279 + _tmp179 * _tmp287 + + _tmp214 * _tmp284 + _tmp228 * _tmp286 + _tmp237 * _tmp285; + _new_state_cov(3, 0) = _tmp214 * _tmp353 + _tmp228 * _tmp349 + _tmp237 * _tmp357; + _new_state_cov(4, 0) = _tmp214 * _tmp375 + _tmp228 * _tmp379 + _tmp237 * _tmp383; + _new_state_cov(5, 0) = _tmp214 * _tmp392 + _tmp228 * _tmp396 + _tmp237 * _tmp400; + _new_state_cov(6, 0) = _tmp214 * _tmp403 + _tmp228 * _tmp401 + _tmp237 * _tmp402; + _new_state_cov(7, 0) = _tmp214 * _tmp404 + _tmp228 * _tmp406 + _tmp237 * _tmp405; + _new_state_cov(8, 0) = _tmp214 * _tmp409 + _tmp228 * _tmp407 + _tmp237 * _tmp408; + _new_state_cov(0, 1) = _tmp182 * _tmp253 + _tmp183 * _tmp247 + _tmp185 * _tmp245 + + _tmp238 * _tmp261 + _tmp239 * _tmp264 + _tmp240 * _tmp260; + _new_state_cov(1, 1) = _tmp245 * _tmp255 + _tmp247 * _tmp267 + _tmp253 * _tmp257 + + _tmp260 * _tmp269 + _tmp261 * _tmp268 + _tmp264 * _tmp265; + _new_state_cov(2, 1) = _tmp245 * _tmp287 + _tmp247 * _tmp279 + _tmp253 * _tmp278 + + _tmp260 * _tmp285 + _tmp261 * _tmp284 + _tmp264 * _tmp286; + _new_state_cov(3, 1) = _tmp260 * _tmp357 + _tmp261 * _tmp353 + _tmp264 * _tmp349; + _new_state_cov(4, 1) = _tmp260 * _tmp383 + _tmp261 * _tmp375 + _tmp264 * _tmp379; + _new_state_cov(5, 1) = _tmp260 * _tmp400 + _tmp261 * _tmp392 + _tmp264 * _tmp396; + _new_state_cov(6, 1) = _tmp260 * _tmp402 + _tmp261 * _tmp403 + _tmp264 * _tmp401; + _new_state_cov(7, 1) = _tmp260 * _tmp405 + _tmp261 * _tmp404 + _tmp264 * _tmp406; + _new_state_cov(8, 1) = _tmp260 * _tmp408 + _tmp261 * _tmp409 + _tmp264 * _tmp407; + _new_state_cov(0, 2) = _tmp182 * _tmp276 + _tmp183 * _tmp273 + _tmp185 * _tmp275 + + _tmp238 * _tmp283 + _tmp239 * _tmp281 + _tmp240 * _tmp282; + _new_state_cov(1, 2) = _tmp255 * _tmp275 + _tmp257 * _tmp276 + _tmp265 * _tmp281 + + _tmp267 * _tmp273 + _tmp268 * _tmp283 + _tmp269 * _tmp282; + _new_state_cov(2, 2) = _tmp273 * _tmp279 + _tmp275 * _tmp287 + _tmp276 * _tmp278 + + _tmp281 * _tmp286 + _tmp282 * _tmp285 + _tmp283 * _tmp284; + _new_state_cov(3, 2) = _tmp281 * _tmp349 + _tmp282 * _tmp357 + _tmp283 * _tmp353; + _new_state_cov(4, 2) = _tmp281 * _tmp379 + _tmp282 * _tmp383 + _tmp283 * _tmp375; + _new_state_cov(5, 2) = _tmp281 * _tmp396 + _tmp282 * _tmp400 + _tmp283 * _tmp392; + _new_state_cov(6, 2) = _tmp281 * _tmp401 + _tmp282 * _tmp402 + _tmp283 * _tmp403; + _new_state_cov(7, 2) = _tmp281 * _tmp406 + _tmp282 * _tmp405 + _tmp283 * _tmp404; + _new_state_cov(8, 2) = _tmp281 * _tmp407 + _tmp282 * _tmp408 + _tmp283 * _tmp409; + _new_state_cov(0, 3) = _tmp214 * state_cov(0, 3) + _tmp228 * state_cov(1, 3) + + _tmp237 * state_cov(2, 3) + _tmp410 * dt + _tmp411 * _tmp90 + + _tmp412 * _tmp90 + _tmp413 * _tmp90; + _new_state_cov(1, 3) = _tmp260 * state_cov(2, 3) + _tmp261 * state_cov(0, 3) + + _tmp264 * state_cov(1, 3) + _tmp414 * dt + _tmp415 * _tmp90 + + _tmp416 * _tmp90 + _tmp417 * _tmp90; + _new_state_cov(2, 3) = _tmp281 * state_cov(1, 3) + _tmp282 * state_cov(2, 3) + + _tmp283 * state_cov(0, 3) + _tmp419 * _tmp90 + _tmp420 * _tmp90 + + _tmp422 * _tmp90 + _tmp423 * dt; + _new_state_cov(3, 3) = _tmp424 * _tmp90 + _tmp430 * _tmp90 + _tmp431 * _tmp90 + + _tmp437 * _tmp90 + _tmp443 * _tmp90 + _tmp444 * _tmp90 + + _tmp445 * _tmp90 + _tmp446 * _tmp90 + _tmp450 * dt + _tmp451 * _tmp90 + + dt * state_cov(6, 3) + state_cov(3, 3); + _new_state_cov(4, 3) = _tmp452 * _tmp90 + _tmp453 * _tmp90 + _tmp454 * _tmp90 + + _tmp455 * _tmp90 + _tmp460 * _tmp90 + _tmp461 * _tmp90 + + _tmp466 * _tmp90 + _tmp467 * _tmp90 + _tmp472 * _tmp90 + _tmp476 * dt + + dt * state_cov(7, 3) + state_cov(4, 3); + _new_state_cov(5, 3) = _tmp477 * _tmp90 + _tmp478 * _tmp90 + _tmp479 * _tmp90 + + _tmp484 * _tmp90 + _tmp489 * _tmp90 + _tmp490 * _tmp90 + + _tmp491 * _tmp90 + _tmp492 * _tmp90 + _tmp496 * dt + _tmp501 * _tmp90 + + dt * state_cov(8, 3) + state_cov(5, 3); + _new_state_cov(6, 3) = _tmp431 + _tmp444 + _tmp451 + _tmp502 * dt + _tmp504 * _tmp90 + + _tmp505 * _tmp90 + _tmp506 * _tmp90 + _tmp507 * _tmp90 + + _tmp509 * _tmp90 + _tmp511 * _tmp90 + state_cov(6, 3); + _new_state_cov(7, 3) = _tmp453 + _tmp461 + _tmp467 + _tmp512 * _tmp90 + _tmp513 * _tmp90 + + _tmp515 * _tmp90 + _tmp517 * _tmp90 + _tmp519 * _tmp90 + _tmp520 * dt + + _tmp521 * _tmp90 + state_cov(7, 3); + _new_state_cov(8, 3) = _tmp479 + _tmp490 + _tmp492 + _tmp522 * _tmp90 + _tmp523 * dt + + _tmp524 * _tmp90 + _tmp526 * _tmp90 + _tmp527 * _tmp90 + + _tmp529 * _tmp90 + _tmp531 * _tmp90 + state_cov(8, 3); + _new_state_cov(0, 4) = _tmp214 * state_cov(0, 4) + _tmp228 * state_cov(1, 4) + + _tmp237 * state_cov(2, 4) + _tmp532 * dt + _tmp533 * _tmp90 + + _tmp534 * _tmp90 + _tmp535 * _tmp90; + _new_state_cov(1, 4) = _tmp260 * state_cov(2, 4) + _tmp261 * state_cov(0, 4) + + _tmp264 * state_cov(1, 4) + _tmp536 * dt + _tmp537 * _tmp90 + + _tmp538 * _tmp90 + _tmp539 * _tmp90; + _new_state_cov(2, 4) = _tmp281 * state_cov(1, 4) + _tmp282 * state_cov(2, 4) + + _tmp283 * state_cov(0, 4) + _tmp540 * _tmp90 + _tmp541 * _tmp90 + + _tmp542 * dt + _tmp543 * _tmp90; + _new_state_cov(3, 4) = _tmp544 * _tmp90 + _tmp546 * _tmp90 + _tmp547 * _tmp90 + + _tmp548 * _tmp90 + _tmp550 * _tmp90 + _tmp551 * _tmp90 + + _tmp552 * _tmp90 + _tmp554 * _tmp90 + _tmp558 * dt + _tmp559 * _tmp90 + + dt * state_cov(6, 4) + state_cov(3, 4); + _new_state_cov(4, 4) = _tmp560 * _tmp90 + _tmp561 * _tmp90 + _tmp562 * _tmp90 + + _tmp563 * _tmp90 + _tmp564 * _tmp90 + _tmp565 * _tmp90 + _tmp569 * dt + + _tmp570 * _tmp90 + _tmp571 * _tmp90 + _tmp572 * _tmp90 + + dt * state_cov(7, 4) + state_cov(4, 4); + _new_state_cov(5, 4) = _tmp573 * _tmp90 + _tmp574 * _tmp90 + _tmp575 * _tmp90 + + _tmp576 * _tmp90 + _tmp577 * _tmp90 + _tmp578 * _tmp90 + + _tmp579 * _tmp90 + _tmp580 * _tmp90 + _tmp581 * _tmp90 + _tmp585 * dt + + dt * state_cov(8, 4) + state_cov(5, 4); + _new_state_cov(6, 4) = _tmp548 + _tmp551 + _tmp559 + _tmp586 * _tmp90 + _tmp587 * _tmp90 + + _tmp588 * dt + _tmp589 * _tmp90 + _tmp590 * _tmp90 + _tmp591 * _tmp90 + + _tmp592 * _tmp90 + state_cov(6, 4); + _new_state_cov(7, 4) = _tmp560 + _tmp561 + _tmp563 + _tmp593 * _tmp90 + _tmp594 * _tmp90 + + _tmp595 * _tmp90 + _tmp596 * _tmp90 + _tmp597 * _tmp90 + + _tmp598 * _tmp90 + _tmp599 * dt + state_cov(7, 4); + _new_state_cov(8, 4) = _tmp573 + _tmp579 + _tmp581 + _tmp600 * _tmp90 + _tmp601 * _tmp90 + + _tmp602 * _tmp90 + _tmp603 * dt + _tmp604 * _tmp90 + _tmp605 * _tmp90 + + _tmp606 * _tmp90 + state_cov(8, 4); + _new_state_cov(0, 5) = _tmp214 * state_cov(0, 5) + _tmp228 * state_cov(1, 5) + + _tmp237 * state_cov(2, 5) + _tmp607 * _tmp90 + _tmp608 * dt + + _tmp609 * _tmp90 + _tmp610 * _tmp90; + _new_state_cov(1, 5) = _tmp260 * state_cov(2, 5) + _tmp261 * state_cov(0, 5) + + _tmp264 * state_cov(1, 5) + _tmp611 * dt + _tmp612 * _tmp90 + + _tmp613 * _tmp90 + _tmp614 * _tmp90; + _new_state_cov(2, 5) = _tmp281 * state_cov(1, 5) + _tmp282 * state_cov(2, 5) + + _tmp283 * state_cov(0, 5) + _tmp615 * _tmp90 + _tmp616 * dt + + _tmp617 * _tmp90 + _tmp618 * _tmp90; + _new_state_cov(3, 5) = _tmp620 * _tmp90 + _tmp621 * _tmp90 + _tmp622 * _tmp90 + + _tmp624 * _tmp90 + _tmp625 * _tmp90 + _tmp626 * _tmp90 + + _tmp627 * _tmp90 + _tmp629 * _tmp90 + _tmp633 * dt + _tmp634 * _tmp90 + + dt * state_cov(6, 5) + state_cov(3, 5); + _new_state_cov(4, 5) = _tmp635 * _tmp90 + _tmp636 * _tmp90 + _tmp637 * _tmp90 + + _tmp638 * _tmp90 + _tmp639 * _tmp90 + _tmp640 * _tmp90 + + _tmp641 * _tmp90 + _tmp642 * _tmp90 + _tmp643 * _tmp90 + _tmp647 * dt + + dt * state_cov(7, 5) + state_cov(4, 5); + _new_state_cov(5, 5) = _tmp648 * _tmp90 + _tmp649 * _tmp90 + _tmp650 * _tmp90 + + _tmp651 * _tmp90 + _tmp652 * _tmp90 + _tmp653 * _tmp90 + + _tmp654 * _tmp90 + _tmp655 * _tmp90 + _tmp659 * dt + _tmp660 * _tmp90 + + dt * state_cov(8, 5) + state_cov(5, 5); + _new_state_cov(6, 5) = _tmp625 + _tmp627 + _tmp634 + _tmp661 * _tmp90 + _tmp662 * _tmp90 + + _tmp663 * dt + _tmp664 * _tmp90 + _tmp665 * _tmp90 + _tmp666 * _tmp90 + + _tmp667 * _tmp90 + state_cov(6, 5); + _new_state_cov(7, 5) = _tmp636 + _tmp638 + _tmp641 + _tmp668 * _tmp90 + _tmp669 * _tmp90 + + _tmp670 * _tmp90 + _tmp671 * _tmp90 + _tmp672 * _tmp90 + + _tmp673 * _tmp90 + _tmp674 * dt + state_cov(7, 5); + _new_state_cov(8, 5) = _tmp648 + _tmp653 + _tmp655 + _tmp675 * _tmp90 + _tmp676 * _tmp90 + + _tmp677 * _tmp90 + _tmp678 * dt + _tmp679 * _tmp90 + _tmp680 * _tmp90 + + _tmp681 * _tmp90 + state_cov(8, 5); + _new_state_cov(0, 6) = _tmp410 + _tmp411 + _tmp412 + _tmp413; + _new_state_cov(1, 6) = _tmp414 + _tmp415 + _tmp416 + _tmp417; + _new_state_cov(2, 6) = _tmp419 + _tmp420 + _tmp422 + _tmp423; + _new_state_cov(3, 6) = _tmp424 + _tmp430 + _tmp437 + _tmp443 + _tmp445 + _tmp446 + _tmp450; + _new_state_cov(4, 6) = _tmp452 + _tmp454 + _tmp455 + _tmp460 + _tmp466 + _tmp472 + _tmp476; + _new_state_cov(5, 6) = _tmp477 + _tmp478 + _tmp484 + _tmp489 + _tmp491 + _tmp496 + _tmp501; + _new_state_cov(6, 6) = _tmp502 + _tmp504 + _tmp505 + _tmp506 + _tmp507 + _tmp509 + _tmp511; + _new_state_cov(7, 6) = _tmp512 + _tmp513 + _tmp515 + _tmp517 + _tmp519 + _tmp520 + _tmp521; + _new_state_cov(8, 6) = _tmp522 + _tmp523 + _tmp524 + _tmp526 + _tmp527 + _tmp529 + _tmp531; + _new_state_cov(0, 7) = _tmp532 + _tmp533 + _tmp534 + _tmp535; + _new_state_cov(1, 7) = _tmp536 + _tmp537 + _tmp538 + _tmp539; + _new_state_cov(2, 7) = _tmp540 + _tmp541 + _tmp542 + _tmp543; + _new_state_cov(3, 7) = _tmp544 + _tmp546 + _tmp547 + _tmp550 + _tmp552 + _tmp554 + _tmp558; + _new_state_cov(4, 7) = _tmp562 + _tmp564 + _tmp565 + _tmp569 + _tmp570 + _tmp571 + _tmp572; + _new_state_cov(5, 7) = _tmp574 + _tmp575 + _tmp576 + _tmp577 + _tmp578 + _tmp580 + _tmp585; + _new_state_cov(6, 7) = _tmp586 + _tmp587 + _tmp588 + _tmp589 + _tmp590 + _tmp591 + _tmp592; + _new_state_cov(7, 7) = _tmp593 + _tmp594 + _tmp595 + _tmp596 + _tmp597 + _tmp598 + _tmp599; + _new_state_cov(8, 7) = _tmp600 + _tmp601 + _tmp602 + _tmp603 + _tmp604 + _tmp605 + _tmp606; + _new_state_cov(0, 8) = _tmp607 + _tmp608 + _tmp609 + _tmp610; + _new_state_cov(1, 8) = _tmp611 + _tmp612 + _tmp613 + _tmp614; + _new_state_cov(2, 8) = _tmp615 + _tmp616 + _tmp617 + _tmp618; + _new_state_cov(3, 8) = _tmp620 + _tmp621 + _tmp622 + _tmp624 + _tmp626 + _tmp629 + _tmp633; + _new_state_cov(4, 8) = _tmp635 + _tmp637 + _tmp639 + _tmp640 + _tmp642 + _tmp643 + _tmp647; + _new_state_cov(5, 8) = _tmp649 + _tmp650 + _tmp651 + _tmp652 + _tmp654 + _tmp659 + _tmp660; + _new_state_cov(6, 8) = _tmp661 + _tmp662 + _tmp663 + _tmp664 + _tmp665 + _tmp666 + _tmp667; + _new_state_cov(7, 8) = _tmp668 + _tmp669 + _tmp670 + _tmp671 + _tmp672 + _tmp673 + _tmp674; + _new_state_cov(8, 8) = _tmp675 + _tmp676 + _tmp677 + _tmp678 + _tmp679 + _tmp680 + _tmp681; + } + + if (new_state_D_accel_bias != nullptr) { + Eigen::Matrix& _new_state_D_accel_bias = (*new_state_D_accel_bias); + + _new_state_D_accel_bias(0, 0) = _tmp214 * state_D_accel_bias(0, 0) + + _tmp228 * state_D_accel_bias(1, 0) + + _tmp237 * state_D_accel_bias(2, 0); + _new_state_D_accel_bias(1, 0) = _tmp260 * state_D_accel_bias(2, 0) + + _tmp261 * state_D_accel_bias(0, 0) + + _tmp264 * state_D_accel_bias(1, 0); + _new_state_D_accel_bias(2, 0) = _tmp281 * state_D_accel_bias(1, 0) + + _tmp282 * state_D_accel_bias(2, 0) + + _tmp283 * state_D_accel_bias(0, 0); + _new_state_D_accel_bias(3, 0) = -_tmp442 * _tmp90 + _tmp682 * _tmp90 + _tmp683 * _tmp90 + + _tmp684 * _tmp90 + dt * state_D_accel_bias(6, 0) + + state_D_accel_bias(3, 0); + _new_state_D_accel_bias(4, 0) = -_tmp549 * _tmp90 + _tmp685 * _tmp90 + _tmp686 * _tmp90 + + _tmp687 * _tmp90 + dt * state_D_accel_bias(7, 0) + + state_D_accel_bias(4, 0); + _new_state_D_accel_bias(5, 0) = -_tmp623 * _tmp90 + _tmp688 * _tmp90 + _tmp689 * _tmp90 + + _tmp690 * _tmp90 + dt * state_D_accel_bias(8, 0) + + state_D_accel_bias(5, 0); + _new_state_D_accel_bias(6, 0) = + -_tmp442 + _tmp682 + _tmp683 + _tmp684 + state_D_accel_bias(6, 0); + _new_state_D_accel_bias(7, 0) = + -_tmp549 + _tmp685 + _tmp686 + _tmp687 + state_D_accel_bias(7, 0); + _new_state_D_accel_bias(8, 0) = + -_tmp623 + _tmp688 + _tmp689 + _tmp690 + state_D_accel_bias(8, 0); + _new_state_D_accel_bias(0, 1) = _tmp214 * state_D_accel_bias(0, 1) + + _tmp228 * state_D_accel_bias(1, 1) + + _tmp237 * state_D_accel_bias(2, 1); + _new_state_D_accel_bias(1, 1) = _tmp260 * state_D_accel_bias(2, 1) + + _tmp261 * state_D_accel_bias(0, 1) + + _tmp264 * state_D_accel_bias(1, 1); + _new_state_D_accel_bias(2, 1) = _tmp281 * state_D_accel_bias(1, 1) + + _tmp282 * state_D_accel_bias(2, 1) + + _tmp283 * state_D_accel_bias(0, 1); + _new_state_D_accel_bias(3, 1) = -_tmp436 * _tmp90 + _tmp691 * _tmp90 + _tmp692 * _tmp90 + + _tmp693 * _tmp90 + dt * state_D_accel_bias(6, 1) + + state_D_accel_bias(3, 1); + _new_state_D_accel_bias(4, 1) = -_tmp545 * _tmp90 + _tmp694 * _tmp90 + _tmp695 * _tmp90 + + _tmp696 * _tmp90 + dt * state_D_accel_bias(7, 1) + + state_D_accel_bias(4, 1); + _new_state_D_accel_bias(5, 1) = -_tmp628 * _tmp90 + _tmp697 * _tmp90 + _tmp698 * _tmp90 + + _tmp699 * _tmp90 + dt * state_D_accel_bias(8, 1) + + state_D_accel_bias(5, 1); + _new_state_D_accel_bias(6, 1) = + -_tmp436 + _tmp691 + _tmp692 + _tmp693 + state_D_accel_bias(6, 1); + _new_state_D_accel_bias(7, 1) = + -_tmp545 + _tmp694 + _tmp695 + _tmp696 + state_D_accel_bias(7, 1); + _new_state_D_accel_bias(8, 1) = + -_tmp628 + _tmp697 + _tmp698 + _tmp699 + state_D_accel_bias(8, 1); + _new_state_D_accel_bias(0, 2) = _tmp214 * state_D_accel_bias(0, 2) + + _tmp228 * state_D_accel_bias(1, 2) + + _tmp237 * state_D_accel_bias(2, 2); + _new_state_D_accel_bias(1, 2) = _tmp260 * state_D_accel_bias(2, 2) + + _tmp261 * state_D_accel_bias(0, 2) + + _tmp264 * state_D_accel_bias(1, 2); + _new_state_D_accel_bias(2, 2) = _tmp281 * state_D_accel_bias(1, 2) + + _tmp282 * state_D_accel_bias(2, 2) + + _tmp283 * state_D_accel_bias(0, 2); + _new_state_D_accel_bias(3, 2) = -_tmp429 * _tmp90 + _tmp700 * _tmp90 + _tmp701 * _tmp90 + + _tmp702 * _tmp90 + dt * state_D_accel_bias(6, 2) + + state_D_accel_bias(3, 2); + _new_state_D_accel_bias(4, 2) = -_tmp553 * _tmp90 + _tmp703 * _tmp90 + _tmp704 * _tmp90 + + _tmp705 * _tmp90 + dt * state_D_accel_bias(7, 2) + + state_D_accel_bias(4, 2); + _new_state_D_accel_bias(5, 2) = -_tmp619 * _tmp90 + _tmp706 * _tmp90 + _tmp707 * _tmp90 + + _tmp708 * _tmp90 + dt * state_D_accel_bias(8, 2) + + state_D_accel_bias(5, 2); + _new_state_D_accel_bias(6, 2) = + -_tmp429 + _tmp700 + _tmp701 + _tmp702 + state_D_accel_bias(6, 2); + _new_state_D_accel_bias(7, 2) = + -_tmp553 + _tmp703 + _tmp704 + _tmp705 + state_D_accel_bias(7, 2); + _new_state_D_accel_bias(8, 2) = + -_tmp619 + _tmp706 + _tmp707 + _tmp708 + state_D_accel_bias(8, 2); + } + + if (new_state_D_gyro_bias != nullptr) { + Eigen::Matrix& _new_state_D_gyro_bias = (*new_state_D_gyro_bias); + + _new_state_D_gyro_bias(0, 0) = -_tmp179 + _tmp214 * state_D_gyro_bias(0, 0) + + _tmp228 * state_D_gyro_bias(1, 0) + + _tmp237 * state_D_gyro_bias(2, 0); + _new_state_D_gyro_bias(1, 0) = -_tmp245 + _tmp260 * state_D_gyro_bias(2, 0) + + _tmp261 * state_D_gyro_bias(0, 0) + + _tmp264 * state_D_gyro_bias(1, 0); + _new_state_D_gyro_bias(2, 0) = -_tmp275 + _tmp281 * state_D_gyro_bias(1, 0) + + _tmp282 * state_D_gyro_bias(2, 0) + + _tmp283 * state_D_gyro_bias(0, 0); + _new_state_D_gyro_bias(3, 0) = _tmp709 * _tmp90 + _tmp710 * _tmp90 + _tmp711 * _tmp90 + + dt * state_D_gyro_bias(6, 0) + state_D_gyro_bias(3, 0); + _new_state_D_gyro_bias(4, 0) = _tmp712 * _tmp90 + _tmp713 * _tmp90 + _tmp714 * _tmp90 + + dt * state_D_gyro_bias(7, 0) + state_D_gyro_bias(4, 0); + _new_state_D_gyro_bias(5, 0) = _tmp715 * _tmp90 + _tmp716 * _tmp90 + _tmp717 * _tmp90 + + dt * state_D_gyro_bias(8, 0) + state_D_gyro_bias(5, 0); + _new_state_D_gyro_bias(6, 0) = _tmp709 + _tmp710 + _tmp711 + state_D_gyro_bias(6, 0); + _new_state_D_gyro_bias(7, 0) = _tmp712 + _tmp713 + _tmp714 + state_D_gyro_bias(7, 0); + _new_state_D_gyro_bias(8, 0) = _tmp715 + _tmp716 + _tmp717 + state_D_gyro_bias(8, 0); + _new_state_D_gyro_bias(0, 1) = -_tmp155 + _tmp214 * state_D_gyro_bias(0, 1) + + _tmp228 * state_D_gyro_bias(1, 1) + + _tmp237 * state_D_gyro_bias(2, 1); + _new_state_D_gyro_bias(1, 1) = -_tmp253 + _tmp260 * state_D_gyro_bias(2, 1) + + _tmp261 * state_D_gyro_bias(0, 1) + + _tmp264 * state_D_gyro_bias(1, 1); + _new_state_D_gyro_bias(2, 1) = -_tmp276 + _tmp281 * state_D_gyro_bias(1, 1) + + _tmp282 * state_D_gyro_bias(2, 1) + + _tmp283 * state_D_gyro_bias(0, 1); + _new_state_D_gyro_bias(3, 1) = _tmp718 * _tmp90 + _tmp719 * _tmp90 + _tmp720 * _tmp90 + + dt * state_D_gyro_bias(6, 1) + state_D_gyro_bias(3, 1); + _new_state_D_gyro_bias(4, 1) = _tmp721 * _tmp90 + _tmp722 * _tmp90 + _tmp723 * _tmp90 + + dt * state_D_gyro_bias(7, 1) + state_D_gyro_bias(4, 1); + _new_state_D_gyro_bias(5, 1) = _tmp724 * _tmp90 + _tmp725 * _tmp90 + _tmp726 * _tmp90 + + dt * state_D_gyro_bias(8, 1) + state_D_gyro_bias(5, 1); + _new_state_D_gyro_bias(6, 1) = _tmp718 + _tmp719 + _tmp720 + state_D_gyro_bias(6, 1); + _new_state_D_gyro_bias(7, 1) = _tmp721 + _tmp722 + _tmp723 + state_D_gyro_bias(7, 1); + _new_state_D_gyro_bias(8, 1) = _tmp724 + _tmp725 + _tmp726 + state_D_gyro_bias(8, 1); + _new_state_D_gyro_bias(0, 2) = -_tmp164 + _tmp214 * state_D_gyro_bias(0, 2) + + _tmp228 * state_D_gyro_bias(1, 2) + + _tmp237 * state_D_gyro_bias(2, 2); + _new_state_D_gyro_bias(1, 2) = -_tmp247 + _tmp260 * state_D_gyro_bias(2, 2) + + _tmp261 * state_D_gyro_bias(0, 2) + + _tmp264 * state_D_gyro_bias(1, 2); + _new_state_D_gyro_bias(2, 2) = -_tmp273 + _tmp281 * state_D_gyro_bias(1, 2) + + _tmp282 * state_D_gyro_bias(2, 2) + + _tmp283 * state_D_gyro_bias(0, 2); + _new_state_D_gyro_bias(3, 2) = _tmp727 * _tmp90 + _tmp728 * _tmp90 + _tmp729 * _tmp90 + + dt * state_D_gyro_bias(6, 2) + state_D_gyro_bias(3, 2); + _new_state_D_gyro_bias(4, 2) = _tmp730 * _tmp90 + _tmp731 * _tmp90 + _tmp732 * _tmp90 + + dt * state_D_gyro_bias(7, 2) + state_D_gyro_bias(4, 2); + _new_state_D_gyro_bias(5, 2) = _tmp733 * _tmp90 + _tmp734 * _tmp90 + _tmp735 * _tmp90 + + dt * state_D_gyro_bias(8, 2) + state_D_gyro_bias(5, 2); + _new_state_D_gyro_bias(6, 2) = _tmp727 + _tmp728 + _tmp729 + state_D_gyro_bias(6, 2); + _new_state_D_gyro_bias(7, 2) = _tmp730 + _tmp731 + _tmp732 + state_D_gyro_bias(7, 2); + _new_state_D_gyro_bias(8, 2) = _tmp733 + _tmp734 + _tmp735 + state_D_gyro_bias(8, 2); + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/gen/python/sym/factors/imu_preintegration_update.py b/gen/python/sym/factors/imu_preintegration_update.py new file mode 100644 index 000000000..66e94d0a6 --- /dev/null +++ b/gen/python/sym/factors/imu_preintegration_update.py @@ -0,0 +1,1896 @@ +# ----------------------------------------------------------------------------- +# This file was autogenerated by symforce from template: +# function/FUNCTION.py.jinja +# Do NOT modify by hand. +# ----------------------------------------------------------------------------- + +# pylint: disable=too-many-locals,too-many-lines,too-many-statements,unused-argument,unused-import + +import math +import typing as T + +import numpy + +import sym + + +def imu_preintegration_update( + state, + state_cov, + state_D_accel_bias, + state_D_gyro_bias, + accel_bias, + gyro_bias, + accel_cov, + gyro_cov, + accel, + gyro, + dt, + epsilon, +): + # type: (numpy.ndarray, numpy.ndarray, numpy.ndarray, numpy.ndarray, numpy.ndarray, numpy.ndarray, numpy.ndarray, numpy.ndarray, numpy.ndarray, numpy.ndarray, float, float) -> T.Tuple[numpy.ndarray, numpy.ndarray, numpy.ndarray, numpy.ndarray] + """ + Symbolic function to perform the full update of incorporating a new IMU measurement + into the state of a preintegrated IMU factor. + """ + + # Total ops: 2652 + + # Input arrays + if state.shape == (9,): + state = state.reshape((9, 1)) + elif state.shape != (9, 1): + raise IndexError( + "state is expected to have shape (9, 1) or (9,); instead had shape {}".format( + state.shape + ) + ) + + if accel_bias.shape == (3,): + accel_bias = accel_bias.reshape((3, 1)) + elif accel_bias.shape != (3, 1): + raise IndexError( + "accel_bias is expected to have shape (3, 1) or (3,); instead had shape {}".format( + accel_bias.shape + ) + ) + + if gyro_bias.shape == (3,): + gyro_bias = gyro_bias.reshape((3, 1)) + elif gyro_bias.shape != (3, 1): + raise IndexError( + "gyro_bias is expected to have shape (3, 1) or (3,); instead had shape {}".format( + gyro_bias.shape + ) + ) + + if accel.shape == (3,): + accel = accel.reshape((3, 1)) + elif accel.shape != (3, 1): + raise IndexError( + "accel is expected to have shape (3, 1) or (3,); instead had shape {}".format( + accel.shape + ) + ) + + if gyro.shape == (3,): + gyro = gyro.reshape((3, 1)) + elif gyro.shape != (3, 1): + raise IndexError( + "gyro is expected to have shape (3, 1) or (3,); instead had shape {}".format(gyro.shape) + ) + + # Intermediate terms (736) + _tmp0 = gyro[0, 0] - gyro_bias[0, 0] + _tmp1 = dt ** 2 + _tmp2 = gyro[1, 0] - gyro_bias[1, 0] + _tmp3 = _tmp2 ** 2 + _tmp4 = _tmp0 ** 2 + _tmp5 = gyro[2, 0] - gyro_bias[2, 0] + _tmp6 = _tmp5 ** 2 + _tmp7 = epsilon ** 2 + _tmp8 = _tmp1 * _tmp3 + _tmp1 * _tmp4 + _tmp1 * _tmp6 + _tmp7 + _tmp9 = math.sqrt(_tmp8) + _tmp10 = 1 / _tmp9 + _tmp11 = state[0, 0] ** 2 + _tmp12 = state[2, 0] ** 2 + _tmp13 = state[1, 0] ** 2 + _tmp14 = _tmp11 + _tmp12 + _tmp13 + _tmp7 + _tmp15 = math.sqrt(_tmp14) + _tmp16 = (1.0 / 2.0) * _tmp15 + _tmp17 = math.cos(_tmp16) + _tmp18 = (1.0 / 2.0) * _tmp9 + _tmp19 = math.sin(_tmp18) + _tmp20 = _tmp17 * _tmp19 + _tmp21 = _tmp10 * _tmp20 + _tmp22 = _tmp21 * dt + _tmp23 = _tmp0 * _tmp22 + _tmp24 = 1 / _tmp15 + _tmp25 = math.sin(_tmp16) + _tmp26 = _tmp24 * _tmp25 + _tmp27 = _tmp19 * _tmp26 + _tmp28 = _tmp10 * dt + _tmp29 = _tmp27 * _tmp28 + _tmp30 = _tmp29 * state[2, 0] + _tmp31 = _tmp2 * _tmp30 + _tmp32 = math.cos(_tmp18) + _tmp33 = _tmp25 * _tmp32 + _tmp34 = _tmp24 * _tmp33 + _tmp35 = _tmp34 * state[0, 0] + _tmp36 = _tmp29 * state[1, 0] + _tmp37 = _tmp36 * _tmp5 + _tmp38 = _tmp23 - _tmp31 + _tmp35 + _tmp37 + _tmp39 = _tmp17 * _tmp32 + _tmp40 = _tmp30 * _tmp5 + _tmp41 = _tmp27 * state[0, 0] + _tmp42 = _tmp28 * _tmp41 + _tmp43 = _tmp0 * _tmp42 + _tmp44 = _tmp2 * _tmp36 + _tmp45 = _tmp40 + _tmp43 + _tmp44 + _tmp46 = _tmp39 - _tmp45 + _tmp47 = abs(_tmp46) + _tmp48 = 1 - epsilon + _tmp49 = min(_tmp47, _tmp48) + _tmp50 = 1 - _tmp49 ** 2 + _tmp51 = math.acos(_tmp49) + _tmp52 = 0.0 if _tmp46 == 0 else math.copysign(1, _tmp46) + _tmp53 = 2 * min(0, _tmp52) + 1 + _tmp54 = 2 * _tmp53 + _tmp55 = _tmp51 * _tmp54 / math.sqrt(_tmp50) + _tmp56 = _tmp42 * _tmp5 + _tmp57 = _tmp2 * _tmp22 + _tmp58 = _tmp0 * _tmp30 + _tmp59 = _tmp34 * state[1, 0] + _tmp60 = -_tmp56 + _tmp57 + _tmp58 + _tmp59 + _tmp61 = _tmp22 * _tmp5 + _tmp62 = _tmp2 * _tmp42 + _tmp63 = _tmp34 * state[2, 0] + _tmp64 = _tmp0 * _tmp36 + _tmp65 = _tmp61 + _tmp62 + _tmp63 - _tmp64 + _tmp66 = 1 / _tmp14 + _tmp67 = _tmp25 ** 2 + _tmp68 = _tmp66 * _tmp67 + _tmp69 = _tmp13 * _tmp68 + _tmp70 = -2 * _tmp69 + _tmp71 = _tmp12 * _tmp68 + _tmp72 = -2 * _tmp71 + _tmp73 = _tmp70 + _tmp72 + 1 + _tmp74 = accel[0, 0] - accel_bias[0, 0] + _tmp75 = state[0, 0] * state[1, 0] + _tmp76 = _tmp68 * _tmp75 + _tmp77 = 2 * _tmp76 + _tmp78 = 2 * _tmp17 + _tmp79 = _tmp26 * _tmp78 + _tmp80 = _tmp79 * state[2, 0] + _tmp81 = _tmp77 - _tmp80 + _tmp82 = accel[1, 0] - accel_bias[1, 0] + _tmp83 = state[0, 0] * state[2, 0] + _tmp84 = _tmp68 * _tmp83 + _tmp85 = 2 * _tmp84 + _tmp86 = _tmp79 * state[1, 0] + _tmp87 = _tmp85 + _tmp86 + _tmp88 = accel[2, 0] - accel_bias[2, 0] + _tmp89 = dt * (_tmp73 * _tmp74 + _tmp81 * _tmp82 + _tmp87 * _tmp88) + _tmp90 = 0.5 * abs(dt) + _tmp91 = _tmp11 * _tmp68 + _tmp92 = 1 - 2 * _tmp91 + _tmp93 = _tmp72 + _tmp92 + _tmp94 = _tmp77 + _tmp80 + _tmp95 = state[1, 0] * state[2, 0] + _tmp96 = _tmp68 * _tmp95 + _tmp97 = 2 * _tmp96 + _tmp98 = _tmp79 * state[0, 0] + _tmp99 = _tmp97 - _tmp98 + _tmp100 = dt * (_tmp74 * _tmp94 + _tmp82 * _tmp93 + _tmp88 * _tmp99) + _tmp101 = _tmp70 + _tmp92 + _tmp102 = _tmp85 - _tmp86 + _tmp103 = _tmp97 + _tmp98 + _tmp104 = dt * (_tmp101 * _tmp88 + _tmp102 * _tmp74 + _tmp103 * _tmp82) + _tmp105 = _tmp2 * _tmp5 + _tmp106 = dt ** 3 + _tmp107 = _tmp8 ** (-3.0 / 2.0) + _tmp108 = _tmp106 * _tmp107 + _tmp109 = _tmp27 * state[2, 0] + _tmp110 = _tmp108 * _tmp109 + _tmp111 = _tmp105 * _tmp110 + _tmp112 = 1 / _tmp8 + _tmp113 = (1.0 / 2.0) * _tmp63 + _tmp114 = _tmp112 * _tmp113 + _tmp115 = _tmp105 * _tmp106 + _tmp116 = _tmp114 * _tmp115 + _tmp117 = (1.0 / 2.0) * _tmp1 + _tmp118 = _tmp117 * _tmp21 + _tmp119 = _tmp27 * state[1, 0] + _tmp120 = _tmp108 * _tmp119 + _tmp121 = _tmp106 * _tmp3 + _tmp122 = (1.0 / 2.0) * _tmp59 + _tmp123 = _tmp112 * _tmp122 + _tmp124 = (1.0 / 2.0) * _tmp35 + _tmp125 = _tmp112 * _tmp124 + _tmp126 = _tmp0 * _tmp106 + _tmp127 = _tmp126 * _tmp2 + _tmp128 = _tmp108 * _tmp41 + _tmp129 = _tmp0 * _tmp128 + _tmp130 = _tmp125 * _tmp127 - _tmp129 * _tmp2 + _tmp131 = ( + -_tmp111 + + _tmp116 + + _tmp118 * _tmp2 + - _tmp120 * _tmp3 + + _tmp121 * _tmp123 + + _tmp130 + + _tmp36 + ) + _tmp132 = _tmp38 * _tmp53 + _tmp133 = -_tmp39 + _tmp45 + _tmp134 = abs(_tmp133) + _tmp135 = min(_tmp134, _tmp48) + _tmp136 = math.acos(_tmp135) + _tmp137 = 1 - _tmp135 ** 2 + _tmp138 = ((0.0 if -_tmp134 + _tmp48 == 0 else math.copysign(1, -_tmp134 + _tmp48)) + 1) * ( + 0.0 if _tmp133 == 0 else math.copysign(1, _tmp133) + ) + _tmp139 = _tmp135 * _tmp136 * _tmp138 / _tmp137 ** (3.0 / 2.0) + _tmp140 = _tmp132 * _tmp139 + _tmp141 = _tmp10 * _tmp117 + _tmp142 = _tmp141 * _tmp41 + _tmp143 = -_tmp105 * _tmp120 + _tmp115 * _tmp123 + _tmp144 = _tmp107 * _tmp20 + _tmp145 = _tmp106 * _tmp144 + _tmp146 = _tmp0 * _tmp145 + _tmp147 = (1.0 / 2.0) * _tmp39 + _tmp148 = _tmp112 * _tmp147 + _tmp149 = _tmp106 * _tmp148 + _tmp150 = _tmp0 * _tmp149 + _tmp151 = -_tmp146 * _tmp2 + _tmp150 * _tmp2 + _tmp152 = _tmp136 * _tmp54 / math.sqrt(_tmp137) + _tmp153 = _tmp138 / _tmp137 + _tmp154 = _tmp132 * _tmp153 + _tmp155 = ( + _tmp131 * _tmp140 + - _tmp131 * _tmp154 + + _tmp152 + * (_tmp110 * _tmp3 - _tmp114 * _tmp121 - _tmp142 * _tmp2 + _tmp143 + _tmp151 - _tmp30) + ) + _tmp156 = _tmp106 * _tmp6 + _tmp157 = _tmp129 * _tmp5 + _tmp158 = _tmp126 * _tmp5 + _tmp159 = _tmp125 * _tmp158 + _tmp160 = _tmp107 * _tmp156 + _tmp161 = ( + -_tmp109 * _tmp160 + + _tmp114 * _tmp156 + + _tmp118 * _tmp5 + + _tmp143 + - _tmp157 + + _tmp159 + + _tmp30 + ) + _tmp162 = _tmp139 * _tmp161 + _tmp163 = -_tmp146 * _tmp5 + _tmp150 * _tmp5 + _tmp164 = ( + _tmp132 * _tmp162 + + _tmp152 + * ( + _tmp111 + - _tmp116 + - _tmp119 * _tmp160 + + _tmp123 * _tmp156 + - _tmp142 * _tmp5 + + _tmp163 + + _tmp36 + ) + - _tmp154 * _tmp161 + ) + _tmp165 = 1 / dt + _tmp166 = _tmp164 * _tmp165 + _tmp167 = _tmp0 * _tmp120 + _tmp168 = _tmp167 * _tmp2 + _tmp169 = _tmp123 * _tmp127 + _tmp170 = _tmp106 * _tmp4 + _tmp171 = _tmp114 * _tmp126 + _tmp172 = _tmp0 * _tmp110 + _tmp173 = _tmp171 * _tmp5 - _tmp172 * _tmp5 + _tmp174 = ( + _tmp0 * _tmp118 + _tmp125 * _tmp170 - _tmp128 * _tmp4 - _tmp168 + _tmp169 + _tmp173 + _tmp42 + ) + _tmp175 = _tmp171 * _tmp2 + _tmp176 = _tmp123 * _tmp158 + _tmp177 = _tmp167 * _tmp5 + _tmp178 = _tmp172 * _tmp2 + _tmp179 = ( + _tmp140 * _tmp174 + + _tmp152 + * ( + -_tmp0 * _tmp142 + - _tmp145 * _tmp4 + + _tmp149 * _tmp4 + - _tmp175 + + _tmp176 + - _tmp177 + + _tmp178 + + _tmp22 + ) + - _tmp154 * _tmp174 + ) + _tmp180 = _tmp165 * _tmp179 + _tmp181 = _tmp155 * _tmp165 + _tmp182 = _tmp166 * gyro_cov[2, 1] + _tmp180 * gyro_cov[0, 1] + _tmp181 * gyro_cov[1, 1] + _tmp183 = _tmp166 * gyro_cov[2, 2] + _tmp180 * gyro_cov[0, 2] + _tmp181 * gyro_cov[1, 2] + _tmp184 = _tmp165 * gyro_cov[0, 0] + _tmp185 = _tmp166 * gyro_cov[2, 0] + _tmp179 * _tmp184 + _tmp181 * gyro_cov[1, 0] + _tmp186 = _tmp14 ** (-3.0 / 2.0) + _tmp187 = _tmp186 * _tmp25 + _tmp188 = _tmp187 * _tmp19 * _tmp28 + _tmp189 = _tmp0 * _tmp188 + _tmp190 = _tmp188 * _tmp2 + _tmp191 = _tmp190 * _tmp75 + _tmp192 = (1.0 / 2.0) * _tmp66 + _tmp193 = _tmp192 * _tmp75 + _tmp194 = _tmp193 * _tmp57 + _tmp195 = _tmp11 * _tmp192 + _tmp196 = _tmp0 * _tmp29 + _tmp197 = -_tmp196 + _tmp198 = _tmp192 * _tmp61 + _tmp199 = _tmp188 * _tmp5 + _tmp200 = -_tmp198 * _tmp83 + _tmp199 * _tmp83 + _tmp201 = _tmp11 * _tmp189 - _tmp124 + _tmp191 - _tmp194 - _tmp195 * _tmp23 + _tmp197 + _tmp200 + _tmp202 = _tmp52 * ((0.0 if -_tmp47 + _tmp48 == 0 else math.copysign(1, -_tmp47 + _tmp48)) + 1) + _tmp203 = _tmp202 * _tmp49 * _tmp51 / _tmp50 ** (3.0 / 2.0) + _tmp204 = _tmp201 * _tmp203 + _tmp205 = _tmp190 * _tmp83 + _tmp206 = _tmp198 * _tmp75 + _tmp207 = _tmp199 * _tmp75 + _tmp208 = _tmp147 * _tmp66 + _tmp209 = _tmp192 * _tmp83 + _tmp210 = _tmp209 * _tmp57 + _tmp211 = _tmp186 * _tmp33 + _tmp212 = _tmp202 / _tmp50 + _tmp213 = _tmp201 * _tmp212 + _tmp214 = ( + _tmp132 * _tmp204 + - _tmp132 * _tmp213 + + _tmp55 + * ( + _tmp11 * _tmp208 + - _tmp11 * _tmp211 + + _tmp205 + + _tmp206 + - _tmp207 + - _tmp210 + + _tmp34 + - 1.0 / 2.0 * _tmp43 + ) + ) + _tmp215 = _tmp29 * _tmp5 + _tmp216 = _tmp13 * _tmp192 + _tmp217 = _tmp192 * _tmp95 + _tmp218 = _tmp190 * _tmp95 - _tmp217 * _tmp57 + _tmp219 = _tmp208 * _tmp75 - _tmp211 * _tmp75 + _tmp220 = _tmp2 * _tmp29 + _tmp221 = -_tmp220 + _tmp222 = _tmp199 * _tmp95 + _tmp223 = _tmp198 * _tmp95 + _tmp224 = _tmp189 * _tmp75 - _tmp193 * _tmp23 + _tmp225 = -_tmp122 + _tmp13 * _tmp190 - _tmp216 * _tmp57 + _tmp221 + _tmp222 - _tmp223 + _tmp224 + _tmp226 = _tmp132 * _tmp203 + _tmp227 = _tmp212 * _tmp225 + _tmp228 = ( + -_tmp132 * _tmp227 + + _tmp225 * _tmp226 + + _tmp55 + * (-_tmp13 * _tmp199 + _tmp215 + _tmp216 * _tmp61 + _tmp218 + _tmp219 - 1.0 / 2.0 * _tmp64) + ) + _tmp229 = _tmp189 * _tmp83 + _tmp230 = _tmp12 * _tmp188 + _tmp231 = _tmp209 * _tmp23 + _tmp232 = -_tmp215 + _tmp233 = _tmp12 * _tmp192 + _tmp234 = -_tmp113 + _tmp218 + _tmp229 + _tmp230 * _tmp5 - _tmp231 + _tmp232 - _tmp233 * _tmp61 + _tmp235 = _tmp212 * _tmp234 + _tmp236 = _tmp208 * _tmp83 - _tmp211 * _tmp83 + _tmp237 = ( + -_tmp132 * _tmp235 + + _tmp226 * _tmp234 + + _tmp55 + * ( + _tmp2 * _tmp230 + + _tmp221 + - _tmp222 + + _tmp223 + - _tmp233 * _tmp57 + + _tmp236 + - 1.0 / 2.0 * _tmp58 + ) + ) + _tmp238 = _tmp214 * state_cov[0, 0] + _tmp228 * state_cov[1, 0] + _tmp237 * state_cov[2, 0] + _tmp239 = _tmp214 * state_cov[0, 1] + _tmp228 * state_cov[1, 1] + _tmp237 * state_cov[2, 1] + _tmp240 = _tmp214 * state_cov[0, 2] + _tmp228 * state_cov[1, 2] + _tmp237 * state_cov[2, 2] + _tmp241 = _tmp53 * _tmp60 + _tmp242 = _tmp139 * _tmp174 + _tmp243 = _tmp153 * _tmp241 + _tmp244 = _tmp119 * _tmp141 + _tmp245 = ( + _tmp152 + * ( + -_tmp0 * _tmp244 + - _tmp110 * _tmp4 + + _tmp114 * _tmp170 + + _tmp151 + + _tmp157 + - _tmp159 + + _tmp30 + ) + - _tmp174 * _tmp243 + + _tmp241 * _tmp242 + ) + _tmp246 = -_tmp105 * _tmp145 + _tmp105 * _tmp149 + _tmp247 = ( + _tmp152 + * (-_tmp125 * _tmp156 + _tmp160 * _tmp41 + _tmp173 - _tmp244 * _tmp5 + _tmp246 - _tmp42) + - _tmp161 * _tmp243 + + _tmp162 * _tmp241 + ) + _tmp248 = _tmp165 * _tmp247 + _tmp249 = _tmp105 * _tmp128 + _tmp250 = _tmp115 * _tmp125 + _tmp251 = _tmp131 * _tmp153 + _tmp252 = _tmp131 * _tmp139 + _tmp253 = ( + _tmp152 + * ( + -_tmp145 * _tmp3 + + _tmp149 * _tmp3 + + _tmp175 + - _tmp178 + - _tmp2 * _tmp244 + + _tmp22 + + _tmp249 + - _tmp250 + ) + - _tmp241 * _tmp251 + + _tmp241 * _tmp252 + ) + _tmp254 = _tmp165 * _tmp253 + _tmp255 = _tmp184 * _tmp245 + _tmp248 * gyro_cov[2, 0] + _tmp254 * gyro_cov[1, 0] + _tmp256 = _tmp165 * gyro_cov[0, 1] + _tmp257 = _tmp245 * _tmp256 + _tmp248 * gyro_cov[2, 1] + _tmp254 * gyro_cov[1, 1] + _tmp258 = _tmp208 * _tmp95 - _tmp211 * _tmp95 + _tmp259 = _tmp203 * _tmp241 + _tmp260 = ( + _tmp234 * _tmp259 + - _tmp235 * _tmp241 + + _tmp55 + * (-_tmp12 * _tmp189 + _tmp196 + _tmp200 + _tmp23 * _tmp233 + _tmp258 - 1.0 / 2.0 * _tmp31) + ) + _tmp261 = ( + _tmp204 * _tmp241 + - _tmp213 * _tmp241 + + _tmp55 + * ( + _tmp11 * _tmp199 + - _tmp195 * _tmp61 + + _tmp219 + - _tmp229 + + _tmp231 + + _tmp232 + - 1.0 / 2.0 * _tmp62 + ) + ) + _tmp262 = _tmp189 * _tmp95 + _tmp263 = _tmp217 * _tmp23 + _tmp264 = ( + _tmp225 * _tmp259 + - _tmp227 * _tmp241 + + _tmp55 + * ( + _tmp13 * _tmp208 + - _tmp13 * _tmp211 + - _tmp206 + + _tmp207 + - _tmp262 + + _tmp263 + + _tmp34 + - 1.0 / 2.0 * _tmp44 + ) + ) + _tmp265 = _tmp260 * state_cov[2, 1] + _tmp261 * state_cov[0, 1] + _tmp264 * state_cov[1, 1] + _tmp266 = _tmp165 * gyro_cov[0, 2] + _tmp267 = _tmp245 * _tmp266 + _tmp248 * gyro_cov[2, 2] + _tmp254 * gyro_cov[1, 2] + _tmp268 = _tmp260 * state_cov[2, 0] + _tmp261 * state_cov[0, 0] + _tmp264 * state_cov[1, 0] + _tmp269 = _tmp260 * state_cov[2, 2] + _tmp261 * state_cov[0, 2] + _tmp264 * state_cov[1, 2] + _tmp270 = _tmp109 * _tmp141 + _tmp271 = _tmp53 * _tmp65 + _tmp272 = _tmp153 * _tmp271 + _tmp273 = ( + _tmp152 + * ( + -_tmp144 * _tmp156 + + _tmp148 * _tmp156 + - _tmp176 + + _tmp177 + + _tmp22 + - _tmp249 + + _tmp250 + - _tmp270 * _tmp5 + ) + - _tmp161 * _tmp272 + + _tmp162 * _tmp271 + ) + _tmp274 = _tmp165 * _tmp273 + _tmp275 = ( + _tmp152 + * (-_tmp0 * _tmp270 + _tmp120 * _tmp4 - _tmp123 * _tmp170 + _tmp130 + _tmp163 - _tmp36) + - _tmp174 * _tmp272 + + _tmp242 * _tmp271 + ) + _tmp276 = ( + _tmp152 + * ( + _tmp121 * _tmp125 + - _tmp128 * _tmp3 + + _tmp168 + - _tmp169 + - _tmp2 * _tmp270 + + _tmp246 + + _tmp42 + ) + - _tmp251 * _tmp271 + + _tmp252 * _tmp271 + ) + _tmp277 = _tmp165 * _tmp276 + _tmp278 = _tmp256 * _tmp275 + _tmp274 * gyro_cov[2, 1] + _tmp277 * gyro_cov[1, 1] + _tmp279 = _tmp266 * _tmp275 + _tmp274 * gyro_cov[2, 2] + _tmp277 * gyro_cov[1, 2] + _tmp280 = _tmp203 * _tmp271 + _tmp281 = ( + _tmp225 * _tmp280 + - _tmp227 * _tmp271 + + _tmp55 + * ( + _tmp13 * _tmp189 + - _tmp191 + + _tmp194 + + _tmp197 + - _tmp216 * _tmp23 + + _tmp258 + - 1.0 / 2.0 * _tmp37 + ) + ) + _tmp282 = ( + _tmp234 * _tmp280 + - _tmp235 * _tmp271 + + _tmp55 + * ( + _tmp12 * _tmp208 + - _tmp12 * _tmp211 + - _tmp205 + + _tmp210 + + _tmp262 + - _tmp263 + + _tmp34 + - 1.0 / 2.0 * _tmp40 + ) + ) + _tmp283 = ( + _tmp204 * _tmp271 + - _tmp213 * _tmp271 + + _tmp55 + * (-_tmp11 * _tmp190 + _tmp195 * _tmp57 + _tmp220 + _tmp224 + _tmp236 - 1.0 / 2.0 * _tmp56) + ) + _tmp284 = _tmp281 * state_cov[1, 0] + _tmp282 * state_cov[2, 0] + _tmp283 * state_cov[0, 0] + _tmp285 = _tmp281 * state_cov[1, 2] + _tmp282 * state_cov[2, 2] + _tmp283 * state_cov[0, 2] + _tmp286 = _tmp281 * state_cov[1, 1] + _tmp282 * state_cov[2, 1] + _tmp283 * state_cov[0, 1] + _tmp287 = _tmp184 * _tmp275 + _tmp274 * gyro_cov[2, 0] + _tmp277 * gyro_cov[1, 0] + _tmp288 = _tmp187 * _tmp78 + _tmp289 = _tmp13 * _tmp288 + _tmp290 = _tmp289 * state[2, 0] + _tmp291 = 4 * _tmp67 / _tmp14 ** 2 + _tmp292 = _tmp13 * _tmp291 + _tmp293 = _tmp292 * state[2, 0] + _tmp294 = -_tmp290 + _tmp293 + _tmp295 = 4 * _tmp68 + _tmp296 = state[2, 0] ** 3 + _tmp297 = -_tmp288 * _tmp296 + _tmp291 * _tmp296 - _tmp295 * state[2, 0] + _tmp298 = _tmp288 * _tmp95 + _tmp299 = _tmp17 ** 2 * _tmp66 + _tmp300 = _tmp299 * _tmp95 + _tmp301 = -_tmp298 + _tmp300 - _tmp96 + _tmp302 = 2 * _tmp68 + _tmp303 = _tmp302 * state[0, 0] + _tmp304 = _tmp12 * _tmp291 * state[0, 0] + _tmp305 = _tmp12 * _tmp288 + _tmp306 = _tmp305 * state[0, 0] + _tmp307 = _tmp303 - _tmp304 + _tmp306 + _tmp308 = _tmp12 * _tmp299 + _tmp309 = _tmp291 * state[2, 0] + _tmp310 = _tmp288 * _tmp75 + _tmp311 = -_tmp309 * _tmp75 + _tmp310 * state[2, 0] + _tmp312 = _tmp311 - _tmp79 + _tmp313 = dt * ( + _tmp74 * (_tmp294 + _tmp297) + + _tmp82 * (_tmp305 - _tmp308 + _tmp312 + _tmp71) + + _tmp88 * (_tmp301 + _tmp307) + ) + _tmp314 = _tmp313 * state_cov[2, 1] + _tmp315 = _tmp289 * state[0, 0] + _tmp316 = _tmp292 * state[0, 0] + _tmp317 = _tmp303 + _tmp315 - _tmp316 + _tmp318 = _tmp298 - _tmp300 + _tmp96 + _tmp319 = _tmp305 * state[1, 0] + _tmp320 = _tmp291 * state[1, 0] + _tmp321 = _tmp12 * _tmp320 + _tmp322 = -_tmp319 + _tmp321 + _tmp323 = state[1, 0] ** 3 + _tmp324 = -_tmp288 * _tmp323 + _tmp291 * _tmp323 - _tmp295 * state[1, 0] + _tmp325 = _tmp13 * _tmp299 + _tmp326 = _tmp311 + _tmp79 + _tmp327 = ( + _tmp74 * (_tmp322 + _tmp324) + + _tmp82 * (_tmp317 + _tmp318) + + _tmp88 * (-_tmp289 + _tmp325 + _tmp326 - _tmp69) + ) + _tmp328 = _tmp327 * dt + _tmp329 = _tmp328 * state_cov[1, 1] + _tmp330 = _tmp304 - _tmp306 + _tmp331 = -_tmp315 + _tmp316 + _tmp332 = _tmp299 * _tmp75 + _tmp333 = -_tmp310 + _tmp332 - _tmp76 + _tmp334 = _tmp302 * state[2, 0] + _tmp335 = _tmp11 * _tmp309 + _tmp336 = _tmp11 * _tmp288 + _tmp337 = _tmp336 * state[2, 0] + _tmp338 = _tmp334 - _tmp335 + _tmp337 + _tmp339 = _tmp299 * _tmp83 + _tmp340 = _tmp288 * _tmp83 + _tmp341 = -_tmp339 + _tmp340 + _tmp84 + _tmp342 = _tmp302 * state[1, 0] + _tmp343 = _tmp336 * state[1, 0] + _tmp344 = _tmp11 * _tmp320 + _tmp345 = _tmp342 + _tmp343 - _tmp344 + _tmp346 = ( + _tmp74 * (_tmp330 + _tmp331) + _tmp82 * (_tmp341 + _tmp345) + _tmp88 * (_tmp333 + _tmp338) + ) + _tmp347 = _tmp346 * dt + _tmp348 = _tmp347 * state_cov[0, 1] + _tmp349 = ( + _tmp314 * _tmp90 + + _tmp329 * _tmp90 + + _tmp348 * _tmp90 + + dt * state_cov[6, 1] + + state_cov[3, 1] + ) + _tmp350 = _tmp313 * state_cov[2, 0] + _tmp351 = _tmp328 * state_cov[1, 0] + _tmp352 = _tmp347 * state_cov[0, 0] + _tmp353 = ( + _tmp350 * _tmp90 + + _tmp351 * _tmp90 + + _tmp352 * _tmp90 + + dt * state_cov[6, 0] + + state_cov[3, 0] + ) + _tmp354 = _tmp313 * state_cov[2, 2] + _tmp355 = _tmp328 * state_cov[1, 2] + _tmp356 = _tmp347 * state_cov[0, 2] + _tmp357 = ( + _tmp354 * _tmp90 + + _tmp355 * _tmp90 + + _tmp356 * _tmp90 + + dt * state_cov[6, 2] + + state_cov[3, 2] + ) + _tmp358 = -_tmp343 + _tmp344 + _tmp359 = _tmp290 - _tmp293 + _tmp334 + _tmp360 = _tmp310 - _tmp332 + _tmp76 + _tmp361 = ( + _tmp74 * (_tmp301 + _tmp317) + _tmp82 * (_tmp322 + _tmp358) + _tmp88 * (_tmp359 + _tmp360) + ) + _tmp362 = _tmp361 * dt + _tmp363 = _tmp362 * state_cov[1, 0] + _tmp364 = state[0, 0] ** 3 + _tmp365 = -_tmp288 * _tmp364 + _tmp291 * _tmp364 - _tmp295 * state[0, 0] + _tmp366 = _tmp339 - _tmp340 - _tmp84 + _tmp367 = _tmp11 * _tmp299 + _tmp368 = ( + _tmp74 * (_tmp345 + _tmp366) + + _tmp82 * (_tmp330 + _tmp365) + + _tmp88 * (_tmp312 + _tmp336 - _tmp367 + _tmp91) + ) + _tmp369 = _tmp368 * dt + _tmp370 = _tmp369 * state_cov[0, 0] + _tmp371 = _tmp319 - _tmp321 + _tmp342 + _tmp372 = _tmp335 - _tmp337 + _tmp373 = dt * ( + _tmp74 * (-_tmp305 + _tmp308 + _tmp326 - _tmp71) + + _tmp82 * (_tmp297 + _tmp372) + + _tmp88 * (_tmp341 + _tmp371) + ) + _tmp374 = _tmp373 * state_cov[2, 0] + _tmp375 = ( + _tmp363 * _tmp90 + + _tmp370 * _tmp90 + + _tmp374 * _tmp90 + + dt * state_cov[7, 0] + + state_cov[4, 0] + ) + _tmp376 = _tmp362 * state_cov[1, 1] + _tmp377 = _tmp369 * state_cov[0, 1] + _tmp378 = _tmp373 * state_cov[2, 1] + _tmp379 = ( + _tmp376 * _tmp90 + + _tmp377 * _tmp90 + + _tmp378 * _tmp90 + + dt * state_cov[7, 1] + + state_cov[4, 1] + ) + _tmp380 = _tmp362 * state_cov[1, 2] + _tmp381 = _tmp369 * state_cov[0, 2] + _tmp382 = _tmp373 * state_cov[2, 2] + _tmp383 = ( + _tmp380 * _tmp90 + + _tmp381 * _tmp90 + + _tmp382 * _tmp90 + + dt * state_cov[7, 2] + + state_cov[4, 2] + ) + _tmp384 = ( + _tmp74 * (_tmp338 + _tmp360) + + _tmp82 * (_tmp326 - _tmp336 + _tmp367 - _tmp91) + + _tmp88 * (_tmp331 + _tmp365) + ) + _tmp385 = _tmp384 * dt + _tmp386 = _tmp385 * state_cov[0, 0] + _tmp387 = ( + _tmp74 * (_tmp289 + _tmp312 - _tmp325 + _tmp69) + + _tmp82 * (_tmp333 + _tmp359) + + _tmp88 * (_tmp324 + _tmp358) + ) + _tmp388 = _tmp387 * dt + _tmp389 = _tmp388 * state_cov[1, 0] + _tmp390 = dt * ( + _tmp74 * (_tmp307 + _tmp318) + _tmp82 * (_tmp366 + _tmp371) + _tmp88 * (_tmp294 + _tmp372) + ) + _tmp391 = _tmp390 * state_cov[2, 0] + _tmp392 = ( + _tmp386 * _tmp90 + + _tmp389 * _tmp90 + + _tmp391 * _tmp90 + + dt * state_cov[8, 0] + + state_cov[5, 0] + ) + _tmp393 = _tmp385 * state_cov[0, 1] + _tmp394 = _tmp388 * state_cov[1, 1] + _tmp395 = _tmp390 * state_cov[2, 1] + _tmp396 = ( + _tmp393 * _tmp90 + + _tmp394 * _tmp90 + + _tmp395 * _tmp90 + + dt * state_cov[8, 1] + + state_cov[5, 1] + ) + _tmp397 = _tmp385 * state_cov[0, 2] + _tmp398 = _tmp388 * state_cov[1, 2] + _tmp399 = _tmp390 * state_cov[2, 2] + _tmp400 = ( + _tmp397 * _tmp90 + + _tmp398 * _tmp90 + + _tmp399 * _tmp90 + + dt * state_cov[8, 2] + + state_cov[5, 2] + ) + _tmp401 = _tmp314 + _tmp329 + _tmp348 + state_cov[6, 1] + _tmp402 = _tmp354 + _tmp355 + _tmp356 + state_cov[6, 2] + _tmp403 = _tmp350 + _tmp351 + _tmp352 + state_cov[6, 0] + _tmp404 = _tmp363 + _tmp370 + _tmp374 + state_cov[7, 0] + _tmp405 = _tmp380 + _tmp381 + _tmp382 + state_cov[7, 2] + _tmp406 = _tmp376 + _tmp377 + _tmp378 + state_cov[7, 1] + _tmp407 = _tmp393 + _tmp394 + _tmp395 + state_cov[8, 1] + _tmp408 = _tmp397 + _tmp398 + _tmp399 + state_cov[8, 2] + _tmp409 = _tmp386 + _tmp389 + _tmp391 + state_cov[8, 0] + _tmp410 = _tmp214 * state_cov[0, 6] + _tmp228 * state_cov[1, 6] + _tmp237 * state_cov[2, 6] + _tmp411 = _tmp238 * _tmp347 + _tmp412 = _tmp239 * _tmp328 + _tmp413 = _tmp240 * _tmp313 + _tmp414 = _tmp260 * state_cov[2, 6] + _tmp261 * state_cov[0, 6] + _tmp264 * state_cov[1, 6] + _tmp415 = _tmp265 * _tmp328 + _tmp416 = _tmp269 * _tmp313 + _tmp417 = _tmp268 * _tmp347 + _tmp418 = _tmp284 * dt + _tmp419 = _tmp346 * _tmp418 + _tmp420 = _tmp285 * _tmp313 + _tmp421 = _tmp286 * dt + _tmp422 = _tmp327 * _tmp421 + _tmp423 = _tmp281 * state_cov[1, 6] + _tmp282 * state_cov[2, 6] + _tmp283 * state_cov[0, 6] + _tmp424 = _tmp313 * _tmp357 + _tmp425 = _tmp87 * accel_cov[2, 2] + _tmp426 = _tmp81 * accel_cov[1, 2] + _tmp427 = _tmp73 * accel_cov[0, 2] + _tmp428 = _tmp425 * _tmp90 + _tmp426 * _tmp90 + _tmp427 * _tmp90 + _tmp429 = _tmp87 * dt + _tmp430 = _tmp428 * _tmp429 + _tmp431 = _tmp313 * state_cov[2, 3] + _tmp432 = _tmp87 * accel_cov[2, 1] + _tmp433 = _tmp81 * accel_cov[1, 1] + _tmp434 = _tmp73 * accel_cov[0, 1] + _tmp435 = _tmp432 * _tmp90 + _tmp433 * _tmp90 + _tmp434 * _tmp90 + _tmp436 = _tmp81 * dt + _tmp437 = _tmp435 * _tmp436 + _tmp438 = _tmp87 * accel_cov[2, 0] + _tmp439 = _tmp81 * accel_cov[1, 0] + _tmp440 = _tmp73 * accel_cov[0, 0] + _tmp441 = _tmp438 * _tmp90 + _tmp439 * _tmp90 + _tmp440 * _tmp90 + _tmp442 = _tmp73 * dt + _tmp443 = _tmp441 * _tmp442 + _tmp444 = _tmp328 * state_cov[1, 3] + _tmp445 = _tmp347 * _tmp353 + _tmp446 = _tmp328 * _tmp349 + _tmp447 = _tmp313 * state_cov[2, 6] + _tmp448 = _tmp328 * state_cov[1, 6] + _tmp449 = _tmp347 * state_cov[0, 6] + _tmp450 = ( + _tmp447 * _tmp90 + + _tmp448 * _tmp90 + + _tmp449 * _tmp90 + + dt * state_cov[6, 6] + + state_cov[3, 6] + ) + _tmp451 = _tmp347 * state_cov[0, 3] + _tmp452 = _tmp347 * _tmp375 + _tmp453 = _tmp362 * state_cov[1, 3] + _tmp454 = _tmp328 * _tmp379 + _tmp455 = _tmp313 * _tmp383 + _tmp456 = _tmp93 * accel_cov[1, 2] + _tmp457 = _tmp94 * accel_cov[0, 2] + _tmp458 = _tmp99 * accel_cov[2, 2] + _tmp459 = _tmp456 * _tmp90 + _tmp457 * _tmp90 + _tmp458 * _tmp90 + _tmp460 = _tmp429 * _tmp459 + _tmp461 = _tmp369 * state_cov[0, 3] + _tmp462 = _tmp93 * accel_cov[1, 0] + _tmp463 = _tmp94 * accel_cov[0, 0] + _tmp464 = _tmp99 * accel_cov[2, 0] + _tmp465 = _tmp462 * _tmp90 + _tmp463 * _tmp90 + _tmp464 * _tmp90 + _tmp466 = _tmp442 * _tmp465 + _tmp467 = _tmp373 * state_cov[2, 3] + _tmp468 = _tmp93 * accel_cov[1, 1] + _tmp469 = _tmp94 * accel_cov[0, 1] + _tmp470 = _tmp99 * accel_cov[2, 1] + _tmp471 = _tmp468 * _tmp90 + _tmp469 * _tmp90 + _tmp470 * _tmp90 + _tmp472 = _tmp436 * _tmp471 + _tmp473 = _tmp362 * state_cov[1, 6] + _tmp474 = _tmp369 * state_cov[0, 6] + _tmp475 = _tmp373 * state_cov[2, 6] + _tmp476 = ( + _tmp473 * _tmp90 + + _tmp474 * _tmp90 + + _tmp475 * _tmp90 + + dt * state_cov[7, 6] + + state_cov[4, 6] + ) + _tmp477 = _tmp347 * _tmp392 + _tmp478 = _tmp328 * _tmp396 + _tmp479 = _tmp385 * state_cov[0, 3] + _tmp480 = _tmp102 * accel_cov[0, 2] + _tmp481 = _tmp101 * accel_cov[2, 2] + _tmp482 = _tmp103 * accel_cov[1, 2] + _tmp483 = _tmp480 * _tmp90 + _tmp481 * _tmp90 + _tmp482 * _tmp90 + _tmp484 = _tmp429 * _tmp483 + _tmp485 = _tmp102 * accel_cov[0, 1] + _tmp486 = _tmp101 * accel_cov[2, 1] + _tmp487 = _tmp103 * accel_cov[1, 1] + _tmp488 = _tmp485 * _tmp90 + _tmp486 * _tmp90 + _tmp487 * _tmp90 + _tmp489 = _tmp436 * _tmp488 + _tmp490 = _tmp388 * state_cov[1, 3] + _tmp491 = _tmp313 * _tmp400 + _tmp492 = _tmp390 * state_cov[2, 3] + _tmp493 = _tmp385 * state_cov[0, 6] + _tmp494 = _tmp388 * state_cov[1, 6] + _tmp495 = _tmp390 * state_cov[2, 6] + _tmp496 = ( + _tmp493 * _tmp90 + + _tmp494 * _tmp90 + + _tmp495 * _tmp90 + + dt * state_cov[8, 6] + + state_cov[5, 6] + ) + _tmp497 = _tmp102 * accel_cov[0, 0] + _tmp498 = _tmp101 * accel_cov[2, 0] + _tmp499 = _tmp103 * accel_cov[1, 0] + _tmp500 = _tmp497 * _tmp90 + _tmp498 * _tmp90 + _tmp499 * _tmp90 + _tmp501 = _tmp442 * _tmp500 + _tmp502 = _tmp447 + _tmp448 + _tmp449 + state_cov[6, 6] + _tmp503 = _tmp425 + _tmp426 + _tmp427 + _tmp504 = _tmp429 * _tmp503 + _tmp505 = _tmp313 * _tmp402 + _tmp506 = _tmp328 * _tmp401 + _tmp507 = _tmp347 * _tmp403 + _tmp508 = _tmp432 + _tmp433 + _tmp434 + _tmp509 = _tmp436 * _tmp508 + _tmp510 = _tmp438 + _tmp439 + _tmp440 + _tmp511 = _tmp442 * _tmp510 + _tmp512 = _tmp328 * _tmp406 + _tmp513 = _tmp347 * _tmp404 + _tmp514 = _tmp468 + _tmp469 + _tmp470 + _tmp515 = _tmp436 * _tmp514 + _tmp516 = _tmp456 + _tmp457 + _tmp458 + _tmp517 = _tmp429 * _tmp516 + _tmp518 = _tmp462 + _tmp463 + _tmp464 + _tmp519 = _tmp442 * _tmp518 + _tmp520 = _tmp473 + _tmp474 + _tmp475 + state_cov[7, 6] + _tmp521 = _tmp313 * _tmp405 + _tmp522 = _tmp328 * _tmp407 + _tmp523 = _tmp493 + _tmp494 + _tmp495 + state_cov[8, 6] + _tmp524 = _tmp347 * _tmp409 + _tmp525 = _tmp497 + _tmp498 + _tmp499 + _tmp526 = _tmp442 * _tmp525 + _tmp527 = _tmp313 * _tmp408 + _tmp528 = _tmp480 + _tmp481 + _tmp482 + _tmp529 = _tmp429 * _tmp528 + _tmp530 = _tmp485 + _tmp486 + _tmp487 + _tmp531 = _tmp436 * _tmp530 + _tmp532 = _tmp214 * state_cov[0, 7] + _tmp228 * state_cov[1, 7] + _tmp237 * state_cov[2, 7] + _tmp533 = _tmp239 * _tmp362 + _tmp534 = _tmp240 * _tmp373 + _tmp535 = _tmp238 * _tmp369 + _tmp536 = _tmp260 * state_cov[2, 7] + _tmp261 * state_cov[0, 7] + _tmp264 * state_cov[1, 7] + _tmp537 = _tmp268 * _tmp369 + _tmp538 = _tmp265 * _tmp362 + _tmp539 = _tmp269 * _tmp373 + _tmp540 = _tmp285 * _tmp373 + _tmp541 = _tmp361 * _tmp421 + _tmp542 = _tmp281 * state_cov[1, 7] + _tmp282 * state_cov[2, 7] + _tmp283 * state_cov[0, 7] + _tmp543 = _tmp368 * _tmp418 + _tmp544 = _tmp357 * _tmp373 + _tmp545 = _tmp93 * dt + _tmp546 = _tmp435 * _tmp545 + _tmp547 = _tmp349 * _tmp362 + _tmp548 = _tmp313 * state_cov[2, 4] + _tmp549 = _tmp94 * dt + _tmp550 = _tmp441 * _tmp549 + _tmp551 = _tmp328 * state_cov[1, 4] + _tmp552 = _tmp353 * _tmp369 + _tmp553 = _tmp99 * dt + _tmp554 = _tmp428 * _tmp553 + _tmp555 = _tmp313 * state_cov[2, 7] + _tmp556 = _tmp328 * state_cov[1, 7] + _tmp557 = _tmp347 * state_cov[0, 7] + _tmp558 = ( + _tmp555 * _tmp90 + + _tmp556 * _tmp90 + + _tmp557 * _tmp90 + + dt * state_cov[6, 7] + + state_cov[3, 7] + ) + _tmp559 = _tmp347 * state_cov[0, 4] + _tmp560 = _tmp362 * state_cov[1, 4] + _tmp561 = _tmp369 * state_cov[0, 4] + _tmp562 = _tmp373 * _tmp383 + _tmp563 = _tmp373 * state_cov[2, 4] + _tmp564 = _tmp459 * _tmp553 + _tmp565 = _tmp465 * _tmp549 + _tmp566 = _tmp362 * state_cov[1, 7] + _tmp567 = _tmp369 * state_cov[0, 7] + _tmp568 = _tmp373 * state_cov[2, 7] + _tmp569 = ( + _tmp566 * _tmp90 + + _tmp567 * _tmp90 + + _tmp568 * _tmp90 + + dt * state_cov[7, 7] + + state_cov[4, 7] + ) + _tmp570 = _tmp362 * _tmp379 + _tmp571 = _tmp471 * _tmp545 + _tmp572 = _tmp369 * _tmp375 + _tmp573 = _tmp385 * state_cov[0, 4] + _tmp574 = _tmp362 * _tmp396 + _tmp575 = _tmp488 * _tmp545 + _tmp576 = _tmp500 * _tmp549 + _tmp577 = _tmp373 * _tmp400 + _tmp578 = _tmp369 * _tmp392 + _tmp579 = _tmp388 * state_cov[1, 4] + _tmp580 = _tmp483 * _tmp553 + _tmp581 = _tmp390 * state_cov[2, 4] + _tmp582 = _tmp385 * state_cov[0, 7] + _tmp583 = _tmp388 * state_cov[1, 7] + _tmp584 = _tmp390 * state_cov[2, 7] + _tmp585 = ( + _tmp582 * _tmp90 + + _tmp583 * _tmp90 + + _tmp584 * _tmp90 + + dt * state_cov[8, 7] + + state_cov[5, 7] + ) + _tmp586 = _tmp510 * _tmp549 + _tmp587 = _tmp508 * _tmp545 + _tmp588 = _tmp555 + _tmp556 + _tmp557 + state_cov[6, 7] + _tmp589 = _tmp369 * _tmp403 + _tmp590 = _tmp362 * _tmp401 + _tmp591 = _tmp503 * _tmp553 + _tmp592 = _tmp373 * _tmp402 + _tmp593 = _tmp373 * _tmp405 + _tmp594 = _tmp362 * _tmp406 + _tmp595 = _tmp518 * _tmp549 + _tmp596 = _tmp369 * _tmp404 + _tmp597 = _tmp516 * _tmp553 + _tmp598 = _tmp514 * _tmp545 + _tmp599 = _tmp566 + _tmp567 + _tmp568 + state_cov[7, 7] + _tmp600 = _tmp373 * _tmp408 + _tmp601 = _tmp528 * _tmp553 + _tmp602 = _tmp530 * _tmp545 + _tmp603 = _tmp582 + _tmp583 + _tmp584 + state_cov[8, 7] + _tmp604 = _tmp369 * _tmp409 + _tmp605 = _tmp525 * _tmp549 + _tmp606 = _tmp362 * _tmp407 + _tmp607 = _tmp238 * _tmp385 + _tmp608 = _tmp214 * state_cov[0, 8] + _tmp228 * state_cov[1, 8] + _tmp237 * state_cov[2, 8] + _tmp609 = _tmp240 * _tmp390 + _tmp610 = _tmp239 * _tmp388 + _tmp611 = _tmp260 * state_cov[2, 8] + _tmp261 * state_cov[0, 8] + _tmp264 * state_cov[1, 8] + _tmp612 = _tmp265 * _tmp388 + _tmp613 = _tmp269 * _tmp390 + _tmp614 = _tmp268 * _tmp385 + _tmp615 = _tmp387 * _tmp421 + _tmp616 = _tmp281 * state_cov[1, 8] + _tmp282 * state_cov[2, 8] + _tmp283 * state_cov[0, 8] + _tmp617 = _tmp285 * _tmp390 + _tmp618 = _tmp384 * _tmp418 + _tmp619 = _tmp101 * dt + _tmp620 = _tmp428 * _tmp619 + _tmp621 = _tmp357 * _tmp390 + _tmp622 = _tmp349 * _tmp388 + _tmp623 = _tmp102 * dt + _tmp624 = _tmp441 * _tmp623 + _tmp625 = _tmp313 * state_cov[2, 5] + _tmp626 = _tmp353 * _tmp385 + _tmp627 = _tmp328 * state_cov[1, 5] + _tmp628 = _tmp103 * dt + _tmp629 = _tmp435 * _tmp628 + _tmp630 = _tmp313 * state_cov[2, 8] + _tmp631 = _tmp328 * state_cov[1, 8] + _tmp632 = _tmp347 * state_cov[0, 8] + _tmp633 = ( + _tmp630 * _tmp90 + + _tmp631 * _tmp90 + + _tmp632 * _tmp90 + + dt * state_cov[6, 8] + + state_cov[3, 8] + ) + _tmp634 = _tmp347 * state_cov[0, 5] + _tmp635 = _tmp379 * _tmp388 + _tmp636 = _tmp362 * state_cov[1, 5] + _tmp637 = _tmp465 * _tmp623 + _tmp638 = _tmp369 * state_cov[0, 5] + _tmp639 = _tmp383 * _tmp390 + _tmp640 = _tmp471 * _tmp628 + _tmp641 = _tmp373 * state_cov[2, 5] + _tmp642 = _tmp375 * _tmp385 + _tmp643 = _tmp459 * _tmp619 + _tmp644 = _tmp362 * state_cov[1, 8] + _tmp645 = _tmp369 * state_cov[0, 8] + _tmp646 = _tmp373 * state_cov[2, 8] + _tmp647 = ( + _tmp644 * _tmp90 + + _tmp645 * _tmp90 + + _tmp646 * _tmp90 + + dt * state_cov[7, 8] + + state_cov[4, 8] + ) + _tmp648 = _tmp385 * state_cov[0, 5] + _tmp649 = _tmp500 * _tmp623 + _tmp650 = _tmp483 * _tmp619 + _tmp651 = _tmp390 * _tmp400 + _tmp652 = _tmp388 * _tmp396 + _tmp653 = _tmp388 * state_cov[1, 5] + _tmp654 = _tmp488 * _tmp628 + _tmp655 = _tmp390 * state_cov[2, 5] + _tmp656 = _tmp385 * state_cov[0, 8] + _tmp657 = _tmp388 * state_cov[1, 8] + _tmp658 = _tmp390 * state_cov[2, 8] + _tmp659 = ( + _tmp656 * _tmp90 + + _tmp657 * _tmp90 + + _tmp658 * _tmp90 + + dt * state_cov[8, 8] + + state_cov[5, 8] + ) + _tmp660 = _tmp385 * _tmp392 + _tmp661 = _tmp510 * _tmp623 + _tmp662 = _tmp390 * _tmp402 + _tmp663 = _tmp630 + _tmp631 + _tmp632 + state_cov[6, 8] + _tmp664 = _tmp503 * _tmp619 + _tmp665 = _tmp508 * _tmp628 + _tmp666 = _tmp385 * _tmp403 + _tmp667 = _tmp388 * _tmp401 + _tmp668 = _tmp390 * _tmp405 + _tmp669 = _tmp388 * _tmp406 + _tmp670 = _tmp385 * _tmp404 + _tmp671 = _tmp516 * _tmp619 + _tmp672 = _tmp518 * _tmp623 + _tmp673 = _tmp514 * _tmp628 + _tmp674 = _tmp644 + _tmp645 + _tmp646 + state_cov[7, 8] + _tmp675 = _tmp388 * _tmp407 + _tmp676 = _tmp530 * _tmp628 + _tmp677 = _tmp528 * _tmp619 + _tmp678 = _tmp656 + _tmp657 + _tmp658 + state_cov[8, 8] + _tmp679 = _tmp525 * _tmp623 + _tmp680 = _tmp385 * _tmp409 + _tmp681 = _tmp390 * _tmp408 + _tmp682 = _tmp347 * state_D_accel_bias[0, 0] + _tmp683 = _tmp328 * state_D_accel_bias[1, 0] + _tmp684 = _tmp313 * state_D_accel_bias[2, 0] + _tmp685 = _tmp362 * state_D_accel_bias[1, 0] + _tmp686 = _tmp373 * state_D_accel_bias[2, 0] + _tmp687 = _tmp369 * state_D_accel_bias[0, 0] + _tmp688 = _tmp388 * state_D_accel_bias[1, 0] + _tmp689 = _tmp390 * state_D_accel_bias[2, 0] + _tmp690 = _tmp385 * state_D_accel_bias[0, 0] + _tmp691 = _tmp347 * state_D_accel_bias[0, 1] + _tmp692 = _tmp328 * state_D_accel_bias[1, 1] + _tmp693 = _tmp313 * state_D_accel_bias[2, 1] + _tmp694 = _tmp362 * state_D_accel_bias[1, 1] + _tmp695 = _tmp373 * state_D_accel_bias[2, 1] + _tmp696 = _tmp369 * state_D_accel_bias[0, 1] + _tmp697 = _tmp388 * state_D_accel_bias[1, 1] + _tmp698 = _tmp390 * state_D_accel_bias[2, 1] + _tmp699 = _tmp385 * state_D_accel_bias[0, 1] + _tmp700 = _tmp347 * state_D_accel_bias[0, 2] + _tmp701 = _tmp328 * state_D_accel_bias[1, 2] + _tmp702 = _tmp313 * state_D_accel_bias[2, 2] + _tmp703 = _tmp362 * state_D_accel_bias[1, 2] + _tmp704 = _tmp373 * state_D_accel_bias[2, 2] + _tmp705 = _tmp369 * state_D_accel_bias[0, 2] + _tmp706 = _tmp388 * state_D_accel_bias[1, 2] + _tmp707 = _tmp390 * state_D_accel_bias[2, 2] + _tmp708 = _tmp385 * state_D_accel_bias[0, 2] + _tmp709 = _tmp328 * state_D_gyro_bias[1, 0] + _tmp710 = _tmp313 * state_D_gyro_bias[2, 0] + _tmp711 = _tmp347 * state_D_gyro_bias[0, 0] + _tmp712 = _tmp369 * state_D_gyro_bias[0, 0] + _tmp713 = _tmp362 * state_D_gyro_bias[1, 0] + _tmp714 = _tmp373 * state_D_gyro_bias[2, 0] + _tmp715 = _tmp385 * state_D_gyro_bias[0, 0] + _tmp716 = _tmp390 * state_D_gyro_bias[2, 0] + _tmp717 = _tmp388 * state_D_gyro_bias[1, 0] + _tmp718 = _tmp328 * state_D_gyro_bias[1, 1] + _tmp719 = _tmp313 * state_D_gyro_bias[2, 1] + _tmp720 = _tmp347 * state_D_gyro_bias[0, 1] + _tmp721 = _tmp369 * state_D_gyro_bias[0, 1] + _tmp722 = _tmp362 * state_D_gyro_bias[1, 1] + _tmp723 = _tmp373 * state_D_gyro_bias[2, 1] + _tmp724 = _tmp385 * state_D_gyro_bias[0, 1] + _tmp725 = _tmp390 * state_D_gyro_bias[2, 1] + _tmp726 = _tmp388 * state_D_gyro_bias[1, 1] + _tmp727 = _tmp328 * state_D_gyro_bias[1, 2] + _tmp728 = _tmp313 * state_D_gyro_bias[2, 2] + _tmp729 = _tmp347 * state_D_gyro_bias[0, 2] + _tmp730 = _tmp369 * state_D_gyro_bias[0, 2] + _tmp731 = _tmp362 * state_D_gyro_bias[1, 2] + _tmp732 = _tmp373 * state_D_gyro_bias[2, 2] + _tmp733 = _tmp385 * state_D_gyro_bias[0, 2] + _tmp734 = _tmp390 * state_D_gyro_bias[2, 2] + _tmp735 = _tmp388 * state_D_gyro_bias[1, 2] + + # Output terms + _new_state = numpy.zeros((9, 1)) + _new_state[0, 0] = _tmp38 * _tmp55 + _new_state[1, 0] = _tmp55 * _tmp60 + _new_state[2, 0] = _tmp55 * _tmp65 + _new_state[3, 0] = _tmp89 * _tmp90 + dt * state[6, 0] + state[3, 0] + _new_state[4, 0] = _tmp100 * _tmp90 + dt * state[7, 0] + state[4, 0] + _new_state[5, 0] = _tmp104 * _tmp90 + dt * state[8, 0] + state[5, 0] + _new_state[6, 0] = _tmp89 + state[6, 0] + _new_state[7, 0] = _tmp100 + state[7, 0] + _new_state[8, 0] = _tmp104 + state[8, 0] + _new_state_cov = numpy.zeros((9, 9)) + _new_state_cov[0, 0] = ( + _tmp155 * _tmp182 + + _tmp164 * _tmp183 + + _tmp179 * _tmp185 + + _tmp214 * _tmp238 + + _tmp228 * _tmp239 + + _tmp237 * _tmp240 + ) + _new_state_cov[1, 0] = ( + _tmp155 * _tmp257 + + _tmp164 * _tmp267 + + _tmp179 * _tmp255 + + _tmp214 * _tmp268 + + _tmp228 * _tmp265 + + _tmp237 * _tmp269 + ) + _new_state_cov[2, 0] = ( + _tmp155 * _tmp278 + + _tmp164 * _tmp279 + + _tmp179 * _tmp287 + + _tmp214 * _tmp284 + + _tmp228 * _tmp286 + + _tmp237 * _tmp285 + ) + _new_state_cov[3, 0] = _tmp214 * _tmp353 + _tmp228 * _tmp349 + _tmp237 * _tmp357 + _new_state_cov[4, 0] = _tmp214 * _tmp375 + _tmp228 * _tmp379 + _tmp237 * _tmp383 + _new_state_cov[5, 0] = _tmp214 * _tmp392 + _tmp228 * _tmp396 + _tmp237 * _tmp400 + _new_state_cov[6, 0] = _tmp214 * _tmp403 + _tmp228 * _tmp401 + _tmp237 * _tmp402 + _new_state_cov[7, 0] = _tmp214 * _tmp404 + _tmp228 * _tmp406 + _tmp237 * _tmp405 + _new_state_cov[8, 0] = _tmp214 * _tmp409 + _tmp228 * _tmp407 + _tmp237 * _tmp408 + _new_state_cov[0, 1] = ( + _tmp182 * _tmp253 + + _tmp183 * _tmp247 + + _tmp185 * _tmp245 + + _tmp238 * _tmp261 + + _tmp239 * _tmp264 + + _tmp240 * _tmp260 + ) + _new_state_cov[1, 1] = ( + _tmp245 * _tmp255 + + _tmp247 * _tmp267 + + _tmp253 * _tmp257 + + _tmp260 * _tmp269 + + _tmp261 * _tmp268 + + _tmp264 * _tmp265 + ) + _new_state_cov[2, 1] = ( + _tmp245 * _tmp287 + + _tmp247 * _tmp279 + + _tmp253 * _tmp278 + + _tmp260 * _tmp285 + + _tmp261 * _tmp284 + + _tmp264 * _tmp286 + ) + _new_state_cov[3, 1] = _tmp260 * _tmp357 + _tmp261 * _tmp353 + _tmp264 * _tmp349 + _new_state_cov[4, 1] = _tmp260 * _tmp383 + _tmp261 * _tmp375 + _tmp264 * _tmp379 + _new_state_cov[5, 1] = _tmp260 * _tmp400 + _tmp261 * _tmp392 + _tmp264 * _tmp396 + _new_state_cov[6, 1] = _tmp260 * _tmp402 + _tmp261 * _tmp403 + _tmp264 * _tmp401 + _new_state_cov[7, 1] = _tmp260 * _tmp405 + _tmp261 * _tmp404 + _tmp264 * _tmp406 + _new_state_cov[8, 1] = _tmp260 * _tmp408 + _tmp261 * _tmp409 + _tmp264 * _tmp407 + _new_state_cov[0, 2] = ( + _tmp182 * _tmp276 + + _tmp183 * _tmp273 + + _tmp185 * _tmp275 + + _tmp238 * _tmp283 + + _tmp239 * _tmp281 + + _tmp240 * _tmp282 + ) + _new_state_cov[1, 2] = ( + _tmp255 * _tmp275 + + _tmp257 * _tmp276 + + _tmp265 * _tmp281 + + _tmp267 * _tmp273 + + _tmp268 * _tmp283 + + _tmp269 * _tmp282 + ) + _new_state_cov[2, 2] = ( + _tmp273 * _tmp279 + + _tmp275 * _tmp287 + + _tmp276 * _tmp278 + + _tmp281 * _tmp286 + + _tmp282 * _tmp285 + + _tmp283 * _tmp284 + ) + _new_state_cov[3, 2] = _tmp281 * _tmp349 + _tmp282 * _tmp357 + _tmp283 * _tmp353 + _new_state_cov[4, 2] = _tmp281 * _tmp379 + _tmp282 * _tmp383 + _tmp283 * _tmp375 + _new_state_cov[5, 2] = _tmp281 * _tmp396 + _tmp282 * _tmp400 + _tmp283 * _tmp392 + _new_state_cov[6, 2] = _tmp281 * _tmp401 + _tmp282 * _tmp402 + _tmp283 * _tmp403 + _new_state_cov[7, 2] = _tmp281 * _tmp406 + _tmp282 * _tmp405 + _tmp283 * _tmp404 + _new_state_cov[8, 2] = _tmp281 * _tmp407 + _tmp282 * _tmp408 + _tmp283 * _tmp409 + _new_state_cov[0, 3] = ( + _tmp214 * state_cov[0, 3] + + _tmp228 * state_cov[1, 3] + + _tmp237 * state_cov[2, 3] + + _tmp410 * dt + + _tmp411 * _tmp90 + + _tmp412 * _tmp90 + + _tmp413 * _tmp90 + ) + _new_state_cov[1, 3] = ( + _tmp260 * state_cov[2, 3] + + _tmp261 * state_cov[0, 3] + + _tmp264 * state_cov[1, 3] + + _tmp414 * dt + + _tmp415 * _tmp90 + + _tmp416 * _tmp90 + + _tmp417 * _tmp90 + ) + _new_state_cov[2, 3] = ( + _tmp281 * state_cov[1, 3] + + _tmp282 * state_cov[2, 3] + + _tmp283 * state_cov[0, 3] + + _tmp419 * _tmp90 + + _tmp420 * _tmp90 + + _tmp422 * _tmp90 + + _tmp423 * dt + ) + _new_state_cov[3, 3] = ( + _tmp424 * _tmp90 + + _tmp430 * _tmp90 + + _tmp431 * _tmp90 + + _tmp437 * _tmp90 + + _tmp443 * _tmp90 + + _tmp444 * _tmp90 + + _tmp445 * _tmp90 + + _tmp446 * _tmp90 + + _tmp450 * dt + + _tmp451 * _tmp90 + + dt * state_cov[6, 3] + + state_cov[3, 3] + ) + _new_state_cov[4, 3] = ( + _tmp452 * _tmp90 + + _tmp453 * _tmp90 + + _tmp454 * _tmp90 + + _tmp455 * _tmp90 + + _tmp460 * _tmp90 + + _tmp461 * _tmp90 + + _tmp466 * _tmp90 + + _tmp467 * _tmp90 + + _tmp472 * _tmp90 + + _tmp476 * dt + + dt * state_cov[7, 3] + + state_cov[4, 3] + ) + _new_state_cov[5, 3] = ( + _tmp477 * _tmp90 + + _tmp478 * _tmp90 + + _tmp479 * _tmp90 + + _tmp484 * _tmp90 + + _tmp489 * _tmp90 + + _tmp490 * _tmp90 + + _tmp491 * _tmp90 + + _tmp492 * _tmp90 + + _tmp496 * dt + + _tmp501 * _tmp90 + + dt * state_cov[8, 3] + + state_cov[5, 3] + ) + _new_state_cov[6, 3] = ( + _tmp431 + + _tmp444 + + _tmp451 + + _tmp502 * dt + + _tmp504 * _tmp90 + + _tmp505 * _tmp90 + + _tmp506 * _tmp90 + + _tmp507 * _tmp90 + + _tmp509 * _tmp90 + + _tmp511 * _tmp90 + + state_cov[6, 3] + ) + _new_state_cov[7, 3] = ( + _tmp453 + + _tmp461 + + _tmp467 + + _tmp512 * _tmp90 + + _tmp513 * _tmp90 + + _tmp515 * _tmp90 + + _tmp517 * _tmp90 + + _tmp519 * _tmp90 + + _tmp520 * dt + + _tmp521 * _tmp90 + + state_cov[7, 3] + ) + _new_state_cov[8, 3] = ( + _tmp479 + + _tmp490 + + _tmp492 + + _tmp522 * _tmp90 + + _tmp523 * dt + + _tmp524 * _tmp90 + + _tmp526 * _tmp90 + + _tmp527 * _tmp90 + + _tmp529 * _tmp90 + + _tmp531 * _tmp90 + + state_cov[8, 3] + ) + _new_state_cov[0, 4] = ( + _tmp214 * state_cov[0, 4] + + _tmp228 * state_cov[1, 4] + + _tmp237 * state_cov[2, 4] + + _tmp532 * dt + + _tmp533 * _tmp90 + + _tmp534 * _tmp90 + + _tmp535 * _tmp90 + ) + _new_state_cov[1, 4] = ( + _tmp260 * state_cov[2, 4] + + _tmp261 * state_cov[0, 4] + + _tmp264 * state_cov[1, 4] + + _tmp536 * dt + + _tmp537 * _tmp90 + + _tmp538 * _tmp90 + + _tmp539 * _tmp90 + ) + _new_state_cov[2, 4] = ( + _tmp281 * state_cov[1, 4] + + _tmp282 * state_cov[2, 4] + + _tmp283 * state_cov[0, 4] + + _tmp540 * _tmp90 + + _tmp541 * _tmp90 + + _tmp542 * dt + + _tmp543 * _tmp90 + ) + _new_state_cov[3, 4] = ( + _tmp544 * _tmp90 + + _tmp546 * _tmp90 + + _tmp547 * _tmp90 + + _tmp548 * _tmp90 + + _tmp550 * _tmp90 + + _tmp551 * _tmp90 + + _tmp552 * _tmp90 + + _tmp554 * _tmp90 + + _tmp558 * dt + + _tmp559 * _tmp90 + + dt * state_cov[6, 4] + + state_cov[3, 4] + ) + _new_state_cov[4, 4] = ( + _tmp560 * _tmp90 + + _tmp561 * _tmp90 + + _tmp562 * _tmp90 + + _tmp563 * _tmp90 + + _tmp564 * _tmp90 + + _tmp565 * _tmp90 + + _tmp569 * dt + + _tmp570 * _tmp90 + + _tmp571 * _tmp90 + + _tmp572 * _tmp90 + + dt * state_cov[7, 4] + + state_cov[4, 4] + ) + _new_state_cov[5, 4] = ( + _tmp573 * _tmp90 + + _tmp574 * _tmp90 + + _tmp575 * _tmp90 + + _tmp576 * _tmp90 + + _tmp577 * _tmp90 + + _tmp578 * _tmp90 + + _tmp579 * _tmp90 + + _tmp580 * _tmp90 + + _tmp581 * _tmp90 + + _tmp585 * dt + + dt * state_cov[8, 4] + + state_cov[5, 4] + ) + _new_state_cov[6, 4] = ( + _tmp548 + + _tmp551 + + _tmp559 + + _tmp586 * _tmp90 + + _tmp587 * _tmp90 + + _tmp588 * dt + + _tmp589 * _tmp90 + + _tmp590 * _tmp90 + + _tmp591 * _tmp90 + + _tmp592 * _tmp90 + + state_cov[6, 4] + ) + _new_state_cov[7, 4] = ( + _tmp560 + + _tmp561 + + _tmp563 + + _tmp593 * _tmp90 + + _tmp594 * _tmp90 + + _tmp595 * _tmp90 + + _tmp596 * _tmp90 + + _tmp597 * _tmp90 + + _tmp598 * _tmp90 + + _tmp599 * dt + + state_cov[7, 4] + ) + _new_state_cov[8, 4] = ( + _tmp573 + + _tmp579 + + _tmp581 + + _tmp600 * _tmp90 + + _tmp601 * _tmp90 + + _tmp602 * _tmp90 + + _tmp603 * dt + + _tmp604 * _tmp90 + + _tmp605 * _tmp90 + + _tmp606 * _tmp90 + + state_cov[8, 4] + ) + _new_state_cov[0, 5] = ( + _tmp214 * state_cov[0, 5] + + _tmp228 * state_cov[1, 5] + + _tmp237 * state_cov[2, 5] + + _tmp607 * _tmp90 + + _tmp608 * dt + + _tmp609 * _tmp90 + + _tmp610 * _tmp90 + ) + _new_state_cov[1, 5] = ( + _tmp260 * state_cov[2, 5] + + _tmp261 * state_cov[0, 5] + + _tmp264 * state_cov[1, 5] + + _tmp611 * dt + + _tmp612 * _tmp90 + + _tmp613 * _tmp90 + + _tmp614 * _tmp90 + ) + _new_state_cov[2, 5] = ( + _tmp281 * state_cov[1, 5] + + _tmp282 * state_cov[2, 5] + + _tmp283 * state_cov[0, 5] + + _tmp615 * _tmp90 + + _tmp616 * dt + + _tmp617 * _tmp90 + + _tmp618 * _tmp90 + ) + _new_state_cov[3, 5] = ( + _tmp620 * _tmp90 + + _tmp621 * _tmp90 + + _tmp622 * _tmp90 + + _tmp624 * _tmp90 + + _tmp625 * _tmp90 + + _tmp626 * _tmp90 + + _tmp627 * _tmp90 + + _tmp629 * _tmp90 + + _tmp633 * dt + + _tmp634 * _tmp90 + + dt * state_cov[6, 5] + + state_cov[3, 5] + ) + _new_state_cov[4, 5] = ( + _tmp635 * _tmp90 + + _tmp636 * _tmp90 + + _tmp637 * _tmp90 + + _tmp638 * _tmp90 + + _tmp639 * _tmp90 + + _tmp640 * _tmp90 + + _tmp641 * _tmp90 + + _tmp642 * _tmp90 + + _tmp643 * _tmp90 + + _tmp647 * dt + + dt * state_cov[7, 5] + + state_cov[4, 5] + ) + _new_state_cov[5, 5] = ( + _tmp648 * _tmp90 + + _tmp649 * _tmp90 + + _tmp650 * _tmp90 + + _tmp651 * _tmp90 + + _tmp652 * _tmp90 + + _tmp653 * _tmp90 + + _tmp654 * _tmp90 + + _tmp655 * _tmp90 + + _tmp659 * dt + + _tmp660 * _tmp90 + + dt * state_cov[8, 5] + + state_cov[5, 5] + ) + _new_state_cov[6, 5] = ( + _tmp625 + + _tmp627 + + _tmp634 + + _tmp661 * _tmp90 + + _tmp662 * _tmp90 + + _tmp663 * dt + + _tmp664 * _tmp90 + + _tmp665 * _tmp90 + + _tmp666 * _tmp90 + + _tmp667 * _tmp90 + + state_cov[6, 5] + ) + _new_state_cov[7, 5] = ( + _tmp636 + + _tmp638 + + _tmp641 + + _tmp668 * _tmp90 + + _tmp669 * _tmp90 + + _tmp670 * _tmp90 + + _tmp671 * _tmp90 + + _tmp672 * _tmp90 + + _tmp673 * _tmp90 + + _tmp674 * dt + + state_cov[7, 5] + ) + _new_state_cov[8, 5] = ( + _tmp648 + + _tmp653 + + _tmp655 + + _tmp675 * _tmp90 + + _tmp676 * _tmp90 + + _tmp677 * _tmp90 + + _tmp678 * dt + + _tmp679 * _tmp90 + + _tmp680 * _tmp90 + + _tmp681 * _tmp90 + + state_cov[8, 5] + ) + _new_state_cov[0, 6] = _tmp410 + _tmp411 + _tmp412 + _tmp413 + _new_state_cov[1, 6] = _tmp414 + _tmp415 + _tmp416 + _tmp417 + _new_state_cov[2, 6] = _tmp419 + _tmp420 + _tmp422 + _tmp423 + _new_state_cov[3, 6] = _tmp424 + _tmp430 + _tmp437 + _tmp443 + _tmp445 + _tmp446 + _tmp450 + _new_state_cov[4, 6] = _tmp452 + _tmp454 + _tmp455 + _tmp460 + _tmp466 + _tmp472 + _tmp476 + _new_state_cov[5, 6] = _tmp477 + _tmp478 + _tmp484 + _tmp489 + _tmp491 + _tmp496 + _tmp501 + _new_state_cov[6, 6] = _tmp502 + _tmp504 + _tmp505 + _tmp506 + _tmp507 + _tmp509 + _tmp511 + _new_state_cov[7, 6] = _tmp512 + _tmp513 + _tmp515 + _tmp517 + _tmp519 + _tmp520 + _tmp521 + _new_state_cov[8, 6] = _tmp522 + _tmp523 + _tmp524 + _tmp526 + _tmp527 + _tmp529 + _tmp531 + _new_state_cov[0, 7] = _tmp532 + _tmp533 + _tmp534 + _tmp535 + _new_state_cov[1, 7] = _tmp536 + _tmp537 + _tmp538 + _tmp539 + _new_state_cov[2, 7] = _tmp540 + _tmp541 + _tmp542 + _tmp543 + _new_state_cov[3, 7] = _tmp544 + _tmp546 + _tmp547 + _tmp550 + _tmp552 + _tmp554 + _tmp558 + _new_state_cov[4, 7] = _tmp562 + _tmp564 + _tmp565 + _tmp569 + _tmp570 + _tmp571 + _tmp572 + _new_state_cov[5, 7] = _tmp574 + _tmp575 + _tmp576 + _tmp577 + _tmp578 + _tmp580 + _tmp585 + _new_state_cov[6, 7] = _tmp586 + _tmp587 + _tmp588 + _tmp589 + _tmp590 + _tmp591 + _tmp592 + _new_state_cov[7, 7] = _tmp593 + _tmp594 + _tmp595 + _tmp596 + _tmp597 + _tmp598 + _tmp599 + _new_state_cov[8, 7] = _tmp600 + _tmp601 + _tmp602 + _tmp603 + _tmp604 + _tmp605 + _tmp606 + _new_state_cov[0, 8] = _tmp607 + _tmp608 + _tmp609 + _tmp610 + _new_state_cov[1, 8] = _tmp611 + _tmp612 + _tmp613 + _tmp614 + _new_state_cov[2, 8] = _tmp615 + _tmp616 + _tmp617 + _tmp618 + _new_state_cov[3, 8] = _tmp620 + _tmp621 + _tmp622 + _tmp624 + _tmp626 + _tmp629 + _tmp633 + _new_state_cov[4, 8] = _tmp635 + _tmp637 + _tmp639 + _tmp640 + _tmp642 + _tmp643 + _tmp647 + _new_state_cov[5, 8] = _tmp649 + _tmp650 + _tmp651 + _tmp652 + _tmp654 + _tmp659 + _tmp660 + _new_state_cov[6, 8] = _tmp661 + _tmp662 + _tmp663 + _tmp664 + _tmp665 + _tmp666 + _tmp667 + _new_state_cov[7, 8] = _tmp668 + _tmp669 + _tmp670 + _tmp671 + _tmp672 + _tmp673 + _tmp674 + _new_state_cov[8, 8] = _tmp675 + _tmp676 + _tmp677 + _tmp678 + _tmp679 + _tmp680 + _tmp681 + _new_state_D_accel_bias = numpy.zeros((9, 3)) + _new_state_D_accel_bias[0, 0] = ( + _tmp214 * state_D_accel_bias[0, 0] + + _tmp228 * state_D_accel_bias[1, 0] + + _tmp237 * state_D_accel_bias[2, 0] + ) + _new_state_D_accel_bias[1, 0] = ( + _tmp260 * state_D_accel_bias[2, 0] + + _tmp261 * state_D_accel_bias[0, 0] + + _tmp264 * state_D_accel_bias[1, 0] + ) + _new_state_D_accel_bias[2, 0] = ( + _tmp281 * state_D_accel_bias[1, 0] + + _tmp282 * state_D_accel_bias[2, 0] + + _tmp283 * state_D_accel_bias[0, 0] + ) + _new_state_D_accel_bias[3, 0] = ( + -_tmp442 * _tmp90 + + _tmp682 * _tmp90 + + _tmp683 * _tmp90 + + _tmp684 * _tmp90 + + dt * state_D_accel_bias[6, 0] + + state_D_accel_bias[3, 0] + ) + _new_state_D_accel_bias[4, 0] = ( + -_tmp549 * _tmp90 + + _tmp685 * _tmp90 + + _tmp686 * _tmp90 + + _tmp687 * _tmp90 + + dt * state_D_accel_bias[7, 0] + + state_D_accel_bias[4, 0] + ) + _new_state_D_accel_bias[5, 0] = ( + -_tmp623 * _tmp90 + + _tmp688 * _tmp90 + + _tmp689 * _tmp90 + + _tmp690 * _tmp90 + + dt * state_D_accel_bias[8, 0] + + state_D_accel_bias[5, 0] + ) + _new_state_D_accel_bias[6, 0] = ( + -_tmp442 + _tmp682 + _tmp683 + _tmp684 + state_D_accel_bias[6, 0] + ) + _new_state_D_accel_bias[7, 0] = ( + -_tmp549 + _tmp685 + _tmp686 + _tmp687 + state_D_accel_bias[7, 0] + ) + _new_state_D_accel_bias[8, 0] = ( + -_tmp623 + _tmp688 + _tmp689 + _tmp690 + state_D_accel_bias[8, 0] + ) + _new_state_D_accel_bias[0, 1] = ( + _tmp214 * state_D_accel_bias[0, 1] + + _tmp228 * state_D_accel_bias[1, 1] + + _tmp237 * state_D_accel_bias[2, 1] + ) + _new_state_D_accel_bias[1, 1] = ( + _tmp260 * state_D_accel_bias[2, 1] + + _tmp261 * state_D_accel_bias[0, 1] + + _tmp264 * state_D_accel_bias[1, 1] + ) + _new_state_D_accel_bias[2, 1] = ( + _tmp281 * state_D_accel_bias[1, 1] + + _tmp282 * state_D_accel_bias[2, 1] + + _tmp283 * state_D_accel_bias[0, 1] + ) + _new_state_D_accel_bias[3, 1] = ( + -_tmp436 * _tmp90 + + _tmp691 * _tmp90 + + _tmp692 * _tmp90 + + _tmp693 * _tmp90 + + dt * state_D_accel_bias[6, 1] + + state_D_accel_bias[3, 1] + ) + _new_state_D_accel_bias[4, 1] = ( + -_tmp545 * _tmp90 + + _tmp694 * _tmp90 + + _tmp695 * _tmp90 + + _tmp696 * _tmp90 + + dt * state_D_accel_bias[7, 1] + + state_D_accel_bias[4, 1] + ) + _new_state_D_accel_bias[5, 1] = ( + -_tmp628 * _tmp90 + + _tmp697 * _tmp90 + + _tmp698 * _tmp90 + + _tmp699 * _tmp90 + + dt * state_D_accel_bias[8, 1] + + state_D_accel_bias[5, 1] + ) + _new_state_D_accel_bias[6, 1] = ( + -_tmp436 + _tmp691 + _tmp692 + _tmp693 + state_D_accel_bias[6, 1] + ) + _new_state_D_accel_bias[7, 1] = ( + -_tmp545 + _tmp694 + _tmp695 + _tmp696 + state_D_accel_bias[7, 1] + ) + _new_state_D_accel_bias[8, 1] = ( + -_tmp628 + _tmp697 + _tmp698 + _tmp699 + state_D_accel_bias[8, 1] + ) + _new_state_D_accel_bias[0, 2] = ( + _tmp214 * state_D_accel_bias[0, 2] + + _tmp228 * state_D_accel_bias[1, 2] + + _tmp237 * state_D_accel_bias[2, 2] + ) + _new_state_D_accel_bias[1, 2] = ( + _tmp260 * state_D_accel_bias[2, 2] + + _tmp261 * state_D_accel_bias[0, 2] + + _tmp264 * state_D_accel_bias[1, 2] + ) + _new_state_D_accel_bias[2, 2] = ( + _tmp281 * state_D_accel_bias[1, 2] + + _tmp282 * state_D_accel_bias[2, 2] + + _tmp283 * state_D_accel_bias[0, 2] + ) + _new_state_D_accel_bias[3, 2] = ( + -_tmp429 * _tmp90 + + _tmp700 * _tmp90 + + _tmp701 * _tmp90 + + _tmp702 * _tmp90 + + dt * state_D_accel_bias[6, 2] + + state_D_accel_bias[3, 2] + ) + _new_state_D_accel_bias[4, 2] = ( + -_tmp553 * _tmp90 + + _tmp703 * _tmp90 + + _tmp704 * _tmp90 + + _tmp705 * _tmp90 + + dt * state_D_accel_bias[7, 2] + + state_D_accel_bias[4, 2] + ) + _new_state_D_accel_bias[5, 2] = ( + -_tmp619 * _tmp90 + + _tmp706 * _tmp90 + + _tmp707 * _tmp90 + + _tmp708 * _tmp90 + + dt * state_D_accel_bias[8, 2] + + state_D_accel_bias[5, 2] + ) + _new_state_D_accel_bias[6, 2] = ( + -_tmp429 + _tmp700 + _tmp701 + _tmp702 + state_D_accel_bias[6, 2] + ) + _new_state_D_accel_bias[7, 2] = ( + -_tmp553 + _tmp703 + _tmp704 + _tmp705 + state_D_accel_bias[7, 2] + ) + _new_state_D_accel_bias[8, 2] = ( + -_tmp619 + _tmp706 + _tmp707 + _tmp708 + state_D_accel_bias[8, 2] + ) + _new_state_D_gyro_bias = numpy.zeros((9, 3)) + _new_state_D_gyro_bias[0, 0] = ( + -_tmp179 + + _tmp214 * state_D_gyro_bias[0, 0] + + _tmp228 * state_D_gyro_bias[1, 0] + + _tmp237 * state_D_gyro_bias[2, 0] + ) + _new_state_D_gyro_bias[1, 0] = ( + -_tmp245 + + _tmp260 * state_D_gyro_bias[2, 0] + + _tmp261 * state_D_gyro_bias[0, 0] + + _tmp264 * state_D_gyro_bias[1, 0] + ) + _new_state_D_gyro_bias[2, 0] = ( + -_tmp275 + + _tmp281 * state_D_gyro_bias[1, 0] + + _tmp282 * state_D_gyro_bias[2, 0] + + _tmp283 * state_D_gyro_bias[0, 0] + ) + _new_state_D_gyro_bias[3, 0] = ( + _tmp709 * _tmp90 + + _tmp710 * _tmp90 + + _tmp711 * _tmp90 + + dt * state_D_gyro_bias[6, 0] + + state_D_gyro_bias[3, 0] + ) + _new_state_D_gyro_bias[4, 0] = ( + _tmp712 * _tmp90 + + _tmp713 * _tmp90 + + _tmp714 * _tmp90 + + dt * state_D_gyro_bias[7, 0] + + state_D_gyro_bias[4, 0] + ) + _new_state_D_gyro_bias[5, 0] = ( + _tmp715 * _tmp90 + + _tmp716 * _tmp90 + + _tmp717 * _tmp90 + + dt * state_D_gyro_bias[8, 0] + + state_D_gyro_bias[5, 0] + ) + _new_state_D_gyro_bias[6, 0] = _tmp709 + _tmp710 + _tmp711 + state_D_gyro_bias[6, 0] + _new_state_D_gyro_bias[7, 0] = _tmp712 + _tmp713 + _tmp714 + state_D_gyro_bias[7, 0] + _new_state_D_gyro_bias[8, 0] = _tmp715 + _tmp716 + _tmp717 + state_D_gyro_bias[8, 0] + _new_state_D_gyro_bias[0, 1] = ( + -_tmp155 + + _tmp214 * state_D_gyro_bias[0, 1] + + _tmp228 * state_D_gyro_bias[1, 1] + + _tmp237 * state_D_gyro_bias[2, 1] + ) + _new_state_D_gyro_bias[1, 1] = ( + -_tmp253 + + _tmp260 * state_D_gyro_bias[2, 1] + + _tmp261 * state_D_gyro_bias[0, 1] + + _tmp264 * state_D_gyro_bias[1, 1] + ) + _new_state_D_gyro_bias[2, 1] = ( + -_tmp276 + + _tmp281 * state_D_gyro_bias[1, 1] + + _tmp282 * state_D_gyro_bias[2, 1] + + _tmp283 * state_D_gyro_bias[0, 1] + ) + _new_state_D_gyro_bias[3, 1] = ( + _tmp718 * _tmp90 + + _tmp719 * _tmp90 + + _tmp720 * _tmp90 + + dt * state_D_gyro_bias[6, 1] + + state_D_gyro_bias[3, 1] + ) + _new_state_D_gyro_bias[4, 1] = ( + _tmp721 * _tmp90 + + _tmp722 * _tmp90 + + _tmp723 * _tmp90 + + dt * state_D_gyro_bias[7, 1] + + state_D_gyro_bias[4, 1] + ) + _new_state_D_gyro_bias[5, 1] = ( + _tmp724 * _tmp90 + + _tmp725 * _tmp90 + + _tmp726 * _tmp90 + + dt * state_D_gyro_bias[8, 1] + + state_D_gyro_bias[5, 1] + ) + _new_state_D_gyro_bias[6, 1] = _tmp718 + _tmp719 + _tmp720 + state_D_gyro_bias[6, 1] + _new_state_D_gyro_bias[7, 1] = _tmp721 + _tmp722 + _tmp723 + state_D_gyro_bias[7, 1] + _new_state_D_gyro_bias[8, 1] = _tmp724 + _tmp725 + _tmp726 + state_D_gyro_bias[8, 1] + _new_state_D_gyro_bias[0, 2] = ( + -_tmp164 + + _tmp214 * state_D_gyro_bias[0, 2] + + _tmp228 * state_D_gyro_bias[1, 2] + + _tmp237 * state_D_gyro_bias[2, 2] + ) + _new_state_D_gyro_bias[1, 2] = ( + -_tmp247 + + _tmp260 * state_D_gyro_bias[2, 2] + + _tmp261 * state_D_gyro_bias[0, 2] + + _tmp264 * state_D_gyro_bias[1, 2] + ) + _new_state_D_gyro_bias[2, 2] = ( + -_tmp273 + + _tmp281 * state_D_gyro_bias[1, 2] + + _tmp282 * state_D_gyro_bias[2, 2] + + _tmp283 * state_D_gyro_bias[0, 2] + ) + _new_state_D_gyro_bias[3, 2] = ( + _tmp727 * _tmp90 + + _tmp728 * _tmp90 + + _tmp729 * _tmp90 + + dt * state_D_gyro_bias[6, 2] + + state_D_gyro_bias[3, 2] + ) + _new_state_D_gyro_bias[4, 2] = ( + _tmp730 * _tmp90 + + _tmp731 * _tmp90 + + _tmp732 * _tmp90 + + dt * state_D_gyro_bias[7, 2] + + state_D_gyro_bias[4, 2] + ) + _new_state_D_gyro_bias[5, 2] = ( + _tmp733 * _tmp90 + + _tmp734 * _tmp90 + + _tmp735 * _tmp90 + + dt * state_D_gyro_bias[8, 2] + + state_D_gyro_bias[5, 2] + ) + _new_state_D_gyro_bias[6, 2] = _tmp727 + _tmp728 + _tmp729 + state_D_gyro_bias[6, 2] + _new_state_D_gyro_bias[7, 2] = _tmp730 + _tmp731 + _tmp732 + state_D_gyro_bias[7, 2] + _new_state_D_gyro_bias[8, 2] = _tmp733 + _tmp734 + _tmp735 + state_D_gyro_bias[8, 2] + return _new_state, _new_state_cov, _new_state_D_accel_bias, _new_state_D_gyro_bias diff --git a/setup.cfg b/setup.cfg index 36cc10576..82b1b8319 100644 --- a/setup.cfg +++ b/setup.cfg @@ -17,6 +17,9 @@ ignore_missing_imports = True [mypy-graphviz.*] ignore_missing_imports = True +[mypy-gtsam.*] +ignore_missing_imports = True + [mypy-IPython.*] ignore_missing_imports = True diff --git a/setup.py b/setup.py index 0bffe4d23..af2309c3f 100644 --- a/setup.py +++ b/setup.py @@ -479,6 +479,7 @@ def fixed_readme() -> str: "argh", "black[jupyter]==21.12b0", "coverage", + "gtsam ; python_version<'3.10'", "isort", "jinja2~=3.0.3", "mypy==0.910", diff --git a/symforce/slam/imu_preintegration/generate.py b/symforce/slam/imu_preintegration/generate.py index 3d8b7fe24..e3699d157 100644 --- a/symforce/slam/imu_preintegration/generate.py +++ b/symforce/slam/imu_preintegration/generate.py @@ -4,11 +4,14 @@ # ---------------------------------------------------------------------------- import functools +import tempfile from symforce import codegen +from symforce import logger from symforce import typing as T from symforce.slam.imu_preintegration.manifold_symbolic import imu_manifold_preintegration_update from symforce.slam.imu_preintegration.manifold_symbolic import internal_imu_residual +from symforce.slam.imu_preintegration.symbolic import imu_preintegration_update def generate_manifold_imu_preintegration( @@ -71,3 +74,30 @@ def generate_manifold_imu_preintegration( ] ) codegen_residual.generate_function(output_dir=output_dir, skip_directory_nesting=True) + + +def generate_imu_preintegration( + config: codegen.CodegenConfig, output_dir: T.Openable = None +) -> None: + """ + Generate the IMU preintegration update function. + """ + # Create output directory if needed + if output_dir is None: + output_dir = tempfile.mkdtemp( + prefix=f"sf_codegen_{type(config).__name__.lower()}_", dir="/tmp" + ) + logger.debug(f"Creating temp directory: {output_dir}") + + cg = codegen.Codegen.function( + imu_preintegration_update, + config=config, + output_names=[ + "new_state", + "new_state_cov", + "new_state_D_accel_bias", + "new_state_D_gyro_bias", + ], + ) + + cg.generate_function(output_dir=output_dir, skip_directory_nesting=True) diff --git a/symforce/slam/imu_preintegration/preintegrated_imu.py b/symforce/slam/imu_preintegration/preintegrated_imu.py new file mode 100644 index 000000000..739a3aaf4 --- /dev/null +++ b/symforce/slam/imu_preintegration/preintegrated_imu.py @@ -0,0 +1,105 @@ +# ---------------------------------------------------------------------------- +# SymForce - Copyright 2022, Skydio, Inc. +# This source code is under the Apache 2.0 license found in the LICENSE file. +# ---------------------------------------------------------------------------- + +import numpy as np + +from sym.factors.imu_preintegration_update import imu_preintegration_update + + +class PreintegratedImu: + """ + Numerical Python class for IMU preintegration. + + Usage example: + pim = PreintegratedImu(...) + + for meas in imu_measurements: + pim.integrate_measurement(meas.accel, meas.gyro, meas.dt, epsilon=epsilon) + """ + + def __init__( + self, + accel_cov: np.ndarray, + gyro_cov: np.ndarray, + accel_bias: np.ndarray, + gyro_bias: np.ndarray, + ): + """ + Args: + accel_cov: + gyro_cov: + accel_bias: + gyro_bias: + """ + # IMU measurement white noise covariance + assert accel_cov.shape == (3, 3) + self.accel_cov = accel_cov + assert gyro_cov.shape == (3, 3) + self.gyro_cov = gyro_cov + + # IMU bias + assert accel_bias.shape == (3,) + self.accel_bias = accel_bias.reshape(3, 1) + assert gyro_bias.shape == (3,) + self.gyro_bias = gyro_bias.reshape(3, 1) + + # Total time from t(i) to t(j). + self.delta_t_ij = 0.0 + + # Preintegrated state as a 9D vector on tangent space at frame i + # Order is: theta, position, velocity + self.state = np.zeros((9, 1)) + + # Measurement covariance of the preintegrated state + self.state_cov = np.zeros((9, 9)) + + # Integrated measurements wrt accel and gyro biases. + self.state_D_accel_bias = np.zeros((9, 3)) + self.state_D_gyro_bias = np.zeros((9, 3)) + + def integrate_measurement( + self, accel: np.ndarray, gyro: np.ndarray, dt: float, epsilon: float + ) -> None: + """ + Integrate forward for a single IMU measurement, updating uncertainty and derivatives. + + Args: + accel: + gyro: + dt: + epsilon: + """ + assert accel.shape == (3,) + assert gyro.shape == (3,) + + # Update time + self.delta_t_ij += dt + + # Update everything else + ( + self.state, + self.state_cov, + self.state_D_accel_bias, + self.state_D_gyro_bias, + ) = imu_preintegration_update( + # State + state=self.state, + state_cov=self.state_cov, + state_D_accel_bias=self.state_D_accel_bias, + state_D_gyro_bias=self.state_D_gyro_bias, + # Biases and noise model + accel_bias=self.accel_bias, + gyro_bias=self.gyro_bias, + accel_cov=self.accel_cov, + gyro_cov=self.gyro_cov, + # Measurement + accel=accel.reshape(3, 1), + gyro=gyro.reshape(3, 1), + dt=dt, + # Singularity handling + epsilon=epsilon, + ) + + self.state = np.array(self.state).reshape(9, 1) diff --git a/symforce/slam/imu_preintegration/symbolic.py b/symforce/slam/imu_preintegration/symbolic.py new file mode 100644 index 000000000..ff11df9ae --- /dev/null +++ b/symforce/slam/imu_preintegration/symbolic.py @@ -0,0 +1,87 @@ +# ---------------------------------------------------------------------------- +# SymForce - Copyright 2022, Skydio, Inc. +# This source code is under the Apache 2.0 license found in the LICENSE file. +# ---------------------------------------------------------------------------- + +import symforce.symbolic as sf +from symforce import typing as T + + +def imu_preintegration_update( + # Initial state + state: sf.V9, + state_cov: sf.M99, + state_D_accel_bias: sf.M93, + state_D_gyro_bias: sf.M93, + # Biases and noise model + accel_bias: sf.V3, + gyro_bias: sf.V3, + accel_cov: sf.M33, + gyro_cov: sf.M33, + # Measurement + accel: sf.V3, + gyro: sf.V3, + dt: T.Scalar, + # Singularity handling + epsilon: T.Scalar, +) -> T.Tuple[sf.V9, sf.Matrix, sf.Matrix, sf.Matrix]: + """ + Symbolic function to perform the full update of incorporating a new IMU measurement + into the state of a preintegrated IMU factor. + """ + # Correct for IMU bias + corrected_accel = accel - accel_bias + corrected_gyro = gyro - gyro_bias + + # Integrate the state + new_state = integrate_state( + state, accel=corrected_accel, gyro=corrected_gyro, dt=dt, epsilon=epsilon + ) + + # Compute jacobians + new_D_old = new_state.jacobian(state) + new_D_accel = new_state.jacobian(corrected_accel) + new_D_gyro = new_state.jacobian(corrected_gyro) + accel_D_bias = corrected_accel.jacobian(accel_bias) + gyro_D_bias = corrected_gyro.jacobian(gyro_bias) + + # Update the state covariance + new_state_cov = new_D_old * state_cov * new_D_old.T + new_state_cov += new_D_accel * (accel_cov / dt) * new_D_accel.T + new_state_cov += new_D_gyro * (gyro_cov / dt) * new_D_gyro.T + + # Update the bias derivatives + new_state_D_accel_bias = new_D_old * state_D_accel_bias + new_D_accel * accel_D_bias + new_state_D_gyro_bias = new_D_old * state_D_gyro_bias + new_D_gyro * gyro_D_bias + + return new_state, new_state_cov, new_state_D_accel_bias, new_state_D_gyro_bias + + +def integrate_state( + state: sf.V9, accel: sf.V3, gyro: sf.V3, dt: T.Scalar, epsilon: T.Scalar +) -> sf.V9: + """ + Update the given preintegrated state with the given IMU measurement in body frame. + """ + w_ij, p_ij, v_ij = state[0:3, 0], state[3:6, 0], state[6:9, 0] + dt22 = 0.5 * dt * sf.Abs(dt) # Question by, Brad: why is is this dt*|dt|? and not dt^2? + + R_ij = sf.Rot3.from_tangent(w_ij, epsilon=epsilon) + R_ik = R_ij.retract(gyro * dt, epsilon=epsilon) + + # Convert acceleration to frame (i) + # Average over the interval [j, k] for increased accuracy. This measurably reduces + # integration error over longer intervals, but does not match the GTSAM implementation. + average_accel = False + if average_accel: + a_i = (R_ij * accel + R_ik * accel) / 2 + else: + a_i = R_ij * accel + + # Create the integrated result + new_state = sf.Vector9.zero() + new_state[0:3] = sf.M(R_ik.to_tangent(epsilon=epsilon)) + new_state[3:6] = p_ij + (v_ij * dt) + (a_i * dt22) + new_state[6:9] = v_ij + (a_i * dt) + + return new_state diff --git a/symforce/slam/imu_preintegration/test_util.py b/symforce/slam/imu_preintegration/test_util.py new file mode 100644 index 000000000..ef5dd819d --- /dev/null +++ b/symforce/slam/imu_preintegration/test_util.py @@ -0,0 +1,34 @@ +# ---------------------------------------------------------------------------- +# SymForce - Copyright 2022, Skydio, Inc. +# This source code is under the Apache 2.0 license found in the LICENSE file. +# ---------------------------------------------------------------------------- + +import typing as T +from dataclasses import dataclass + +import numpy as np + + +@dataclass +class ImuMeas: + accel: np.ndarray + gyro: np.ndarray + dt: float + + +def generate_random_imu_measurements( + num: int, + accel_sigma: float = 1.0, + gyro_sigma: float = 0.1, + min_dt: float = 0.01, + max_dt: float = 0.25, +) -> T.Iterator[ImuMeas]: + """ + Generate random IMU measurements from a reasonable distribution. + """ + for _ in range(num): + yield ImuMeas( + accel=np.random.normal(loc=np.array([0, 0, -9.81]), scale=accel_sigma, size=(3,)), + gyro=np.random.normal(loc=np.array([0, 0, 0]), scale=gyro_sigma, size=(3,)), + dt=np.random.uniform(low=min_dt, high=max_dt), + ) diff --git a/test/symforce_gen_codegen_test.py b/test/symforce_gen_codegen_test.py index 9d9bda525..46dad25ac 100644 --- a/test/symforce_gen_codegen_test.py +++ b/test/symforce_gen_codegen_test.py @@ -26,6 +26,7 @@ from symforce.codegen import slam_factors_codegen from symforce.codegen import sym_util_package_codegen from symforce.codegen import template_util +from symforce.slam.imu_preintegration.generate import generate_imu_preintegration from symforce.slam.imu_preintegration.generate import generate_manifold_imu_preintegration from symforce.test_util import TestCase from symforce.test_util import symengine_only @@ -92,6 +93,12 @@ def test_gen_package_codegen_python(self) -> None: config = codegen.PythonConfig() cam_package_codegen.generate(config=config, output_dir=output_dir) + # TODO(brad): See if I can get this working on sympy + if symforce.get_symbolic_api() == "symengine": + generate_imu_preintegration( + config=config, + output_dir=Path(output_dir) / "sym" / "factors", + ) template_util.render_template( template_dir=config.template_dir(), template_path="setup.py.jinja", @@ -151,6 +158,10 @@ def test_gen_package_codegen_cpp(self) -> None: # Prior factors, between factors, and SLAM factors for C++. geo_factors_codegen.generate(output_dir / "sym") slam_factors_codegen.generate(output_dir / "sym") + generate_imu_preintegration( + config=config, + output_dir=output_dir / "sym" / "factors", + ) generate_manifold_imu_preintegration( config=config, output_dir=output_dir / "sym" / "factors" / "internal", diff --git a/test/symforce_preintegrated_imu_test.py b/test/symforce_preintegrated_imu_test.py new file mode 100644 index 000000000..065bac546 --- /dev/null +++ b/test/symforce_preintegrated_imu_test.py @@ -0,0 +1,86 @@ +# ---------------------------------------------------------------------------- +# SymForce - Copyright 2022, Skydio, Inc. +# This source code is under the Apache 2.0 license found in the LICENSE file. +# ---------------------------------------------------------------------------- + +import numpy as np + +import symforce + +symforce.set_epsilon_to_symbol() +from symforce import logger +from symforce.slam.imu_preintegration.preintegrated_imu import PreintegratedImu +from symforce.slam.imu_preintegration.test_util import generate_random_imu_measurements +from symforce.test_util import TestCase + + +class SymforcePreintegratedImuTest(TestCase): + """ + Test IMU preintegration. + """ + + def test_python_numerical(self) -> None: + pim = PreintegratedImu( + accel_cov=np.eye(3, 3), + gyro_cov=np.eye(3, 3), + accel_bias=np.zeros(3), + gyro_bias=np.zeros(3), + ) + + imu_measurements = list(generate_random_imu_measurements(num=10)) + for meas in imu_measurements: + pim.integrate_measurement(accel=meas.accel, gyro=meas.gyro, dt=meas.dt, epsilon=1e-12) + + self.assertEqual(sum(m.dt for m in imu_measurements), pim.delta_t_ij) + + print(f"state=\n{pim.state}") + print(f"state_cov=\n{pim.state_cov}") + print(f"state_D_accel_bias=\n{pim.state_D_accel_bias}") + print(f"state_D_gyro_bias=\n{pim.state_D_gyro_bias}") + + def test_python_numerical_against_gtsam(self) -> None: + try: + import gtsam + except ImportError: + logger.info("Skipping test, GTSAM not installed.") + return + + pim_sym = PreintegratedImu( + accel_cov=np.eye(3, 3), + gyro_cov=np.eye(3, 3), + accel_bias=np.zeros(3), + gyro_bias=np.zeros(3), + ) + + pim_gtsam = gtsam.PreintegratedImuMeasurements( + params=gtsam.PreintegrationParams( + n_gravity=np.array([0, 0, -9.81]), + ), + bias=gtsam.gtsam.imuBias.ConstantBias( + biasAcc=pim_sym.accel_bias, + biasGyro=pim_sym.gyro_bias, + ), + ) + + imu_measurements = list(generate_random_imu_measurements(num=10)) + for i, meas in enumerate(imu_measurements): + pim_sym.integrate_measurement( + accel=meas.accel, gyro=meas.gyro, dt=meas.dt, epsilon=1e-12 + ) + + pim_gtsam.integrateMeasurement( + measuredAcc=meas.accel, + measuredOmega=meas.gyro, + deltaT=meas.dt, + ) + + state_sym = pim_sym.state.T.squeeze() + state_gtsam = pim_gtsam.preintegrated() + + state_diff_rel = np.linalg.norm(state_sym - state_gtsam) / np.linalg.norm(state_gtsam) + logger.debug(f"{i}: {state_diff_rel=}") + self.assertLess(state_diff_rel, 1e-6) + + +if __name__ == "__main__": + TestCase.main()