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 <Eigen/Dense>
+
+namespace sym {
+
+/**
+ * Symbolic function to perform the full update of incorporating a new IMU measurement
+ * into the state of a preintegrated IMU factor.
+ */
+template <typename Scalar>
+void ImuPreintegrationUpdate(
+    const Eigen::Matrix<Scalar, 9, 1>& state, const Eigen::Matrix<Scalar, 9, 9>& state_cov,
+    const Eigen::Matrix<Scalar, 9, 3>& state_D_accel_bias,
+    const Eigen::Matrix<Scalar, 9, 3>& state_D_gyro_bias,
+    const Eigen::Matrix<Scalar, 3, 1>& accel_bias, const Eigen::Matrix<Scalar, 3, 1>& gyro_bias,
+    const Eigen::Matrix<Scalar, 3, 3>& accel_cov, const Eigen::Matrix<Scalar, 3, 3>& gyro_cov,
+    const Eigen::Matrix<Scalar, 3, 1>& accel, const Eigen::Matrix<Scalar, 3, 1>& gyro,
+    const Scalar dt, const Scalar epsilon, Eigen::Matrix<Scalar, 9, 1>* const new_state = nullptr,
+    Eigen::Matrix<Scalar, 9, 9>* const new_state_cov = nullptr,
+    Eigen::Matrix<Scalar, 9, 3>* const new_state_D_accel_bias = nullptr,
+    Eigen::Matrix<Scalar, 9, 3>* 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<Scalar>(_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<Scalar>(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<Scalar>(_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<Scalar, 9, 1>& _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<Scalar, 9, 9>& _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<Scalar, 9, 3>& _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<Scalar, 9, 3>& _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()