diff --git a/CHANGELOGS_c2.md b/CHANGELOGS_c2.md
index 9d7500037..bb957c37a 100644
--- a/CHANGELOGS_c2.md
+++ b/CHANGELOGS_c2.md
@@ -18,7 +18,10 @@ dragonpilot [Latest] - EON/C2 Release
* TWEAK: DM reverted to 0.8.13 model and algorithm.
* TWEAK: E2E Initial speed matches ACC initial speed. (105 -> 40)
* FIXED: Fixed Reverse message not showing issue.
- * FIXED: Honda ALKA not working. @loveloveses
+ * FIXED: Honda - ALKA not working. @loveloveses
+ * FIXED: Display Mode.
+ * FIXED: Toyota - Speed Override.
+ * FIXED: Re-include CJK system font for EON/C2.
dragonpilot 2023.02.08 - EON/C2 Release
========================
diff --git a/cereal/visionipc/visionipc_pyx.so b/cereal/visionipc/visionipc_pyx.so
index f79928f49..b50a37759 100755
Binary files a/cereal/visionipc/visionipc_pyx.so and b/cereal/visionipc/visionipc_pyx.so differ
diff --git a/common/params_pyx.so b/common/params_pyx.so
index 9e3befd84..068dae9a3 100755
Binary files a/common/params_pyx.so and b/common/params_pyx.so differ
diff --git a/common/version.h b/common/version.h
index ff4a14ea0..0e6db0690 100644
--- a/common/version.h
+++ b/common/version.h
@@ -1 +1 @@
-#define COMMA_VERSION "2023.04.01"
+#define COMMA_VERSION "2023.04.13"
diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh
index 68b3650f2..9aededc11 100755
--- a/launch_chffrplus.sh
+++ b/launch_chffrplus.sh
@@ -9,6 +9,20 @@ source "$BASEDIR/launch_env.sh"
DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )"
function two_init {
+ if [ -f /EON ]; then
+ if [ ! -f /system/fonts/NotoSansCJKtc-Regular.otf ]; then
+ mount -o remount,rw /system
+ rm -fr /system/fonts/NotoSansTC*.otf
+ rm -fr /system/fonts/NotoSansSC*.otf
+ rm -fr /system/fonts/NotoSansKR*.otf
+ rm -fr /system/fonts/NotoSansJP*.otf
+ cp -rf /data/openpilot/selfdrive/assets/fonts/NotoSansCJKtc-* /system/fonts/
+ cp -rf /data/openpilot/selfdrive/assets/fonts/fonts.xml /system/etc/fonts.xml
+ chmod 644 /system/etc/fonts.xml
+ chmod 644 /system/fonts/NotoSansCJKtc-*
+ mount -o remount,r /system
+ fi
+ fi
dt=$(date +%s)
diff --git a/opendbc/can/libdbc.so b/opendbc/can/libdbc.so
index 7d5371089..9fd1cbbe7 100755
Binary files a/opendbc/can/libdbc.so and b/opendbc/can/libdbc.so differ
diff --git a/panda/board/obj/bootstub.panda.bin b/panda/board/obj/bootstub.panda.bin
index 9808906ab..44002b85c 100755
Binary files a/panda/board/obj/bootstub.panda.bin and b/panda/board/obj/bootstub.panda.bin differ
diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed
index 7fbb95986..ece3707d7 100644
Binary files a/panda/board/obj/panda.bin.signed and b/panda/board/obj/panda.bin.signed differ
diff --git a/rednose/helpers/ekf_sym_pyx.so b/rednose/helpers/ekf_sym_pyx.so
index 4ea0f1716..bdc8e8383 100755
Binary files a/rednose/helpers/ekf_sym_pyx.so and b/rednose/helpers/ekf_sym_pyx.so differ
diff --git a/selfdrive/assets/fonts/fonts.xml b/selfdrive/assets/fonts/fonts.xml
new file mode 100644
index 000000000..de40a7624
--- /dev/null
+++ b/selfdrive/assets/fonts/fonts.xml
@@ -0,0 +1,372 @@
+
+
+
+
+
+ Roboto-Thin.ttf
+ Roboto-ThinItalic.ttf
+ Roboto-Light.ttf
+ Roboto-LightItalic.ttf
+ Roboto-Regular.ttf
+ Roboto-Italic.ttf
+ Roboto-Medium.ttf
+ Roboto-MediumItalic.ttf
+ Roboto-Black.ttf
+ Roboto-BlackItalic.ttf
+ Roboto-Bold.ttf
+ Roboto-BoldItalic.ttf
+
+
+
+
+
+
+
+
+
+
+
+
+
+ RobotoCondensed-Light.ttf
+ RobotoCondensed-LightItalic.ttf
+ RobotoCondensed-Regular.ttf
+ RobotoCondensed-Italic.ttf
+ RobotoCondensed-Bold.ttf
+ RobotoCondensed-BoldItalic.ttf
+
+
+
+
+ NotoSerif-Regular.ttf
+ NotoSerif-Bold.ttf
+ NotoSerif-Italic.ttf
+ NotoSerif-BoldItalic.ttf
+
+
+
+
+
+
+
+
+
+
+
+ DroidSansMono.ttf
+
+
+
+
+
+ CutiveMono.ttf
+
+
+
+
+
+ ComingSoon.ttf
+
+
+
+ DancingScript-Regular.ttf
+ DancingScript-Bold.ttf
+
+
+
+ CarroisGothicSC-Regular.ttf
+
+
+
+
+ NotoNaskhArabic-Regular.ttf
+ NotoNaskhArabic-Bold.ttf
+
+
+ NotoNaskhArabicUI-Regular.ttf
+ NotoNaskhArabicUI-Bold.ttf
+
+
+ NotoSansEthiopic-Regular.ttf
+ NotoSansEthiopic-Bold.ttf
+
+
+ NotoSansHebrew-Regular.ttf
+ NotoSansHebrew-Bold.ttf
+
+
+ NotoSansThai-Regular.ttf
+ NotoSansThai-Bold.ttf
+
+
+ NotoSansThaiUI-Regular.ttf
+ NotoSansThaiUI-Bold.ttf
+
+
+ NotoSansArmenian-Regular.ttf
+ NotoSansArmenian-Bold.ttf
+
+
+ NotoSansGeorgian-Regular.ttf
+ NotoSansGeorgian-Bold.ttf
+
+
+ NotoSansDevanagari-Regular.ttf
+ NotoSansDevanagari-Bold.ttf
+
+
+ NotoSansDevanagariUI-Regular.ttf
+ NotoSansDevanagariUI-Bold.ttf
+
+
+
+ NotoSansGujarati-Regular.ttf
+ NotoSansGujarati-Bold.ttf
+
+
+ NotoSansGujaratiUI-Regular.ttf
+ NotoSansGujaratiUI-Bold.ttf
+
+
+
+ NotoSansGurmukhi-Regular.ttf
+ NotoSansGurmukhi-Bold.ttf
+
+
+ NotoSansGurmukhiUI-Regular.ttf
+ NotoSansGurmukhiUI-Bold.ttf
+
+
+ NotoSansTamil-Regular.ttf
+ NotoSansTamil-Bold.ttf
+
+
+ NotoSansTamilUI-Regular.ttf
+ NotoSansTamilUI-Bold.ttf
+
+
+ NotoSansMalayalam-Regular.ttf
+ NotoSansMalayalam-Bold.ttf
+
+
+ NotoSansMalayalamUI-Regular.ttf
+ NotoSansMalayalamUI-Bold.ttf
+
+
+ NotoSansBengali-Regular.ttf
+ NotoSansBengali-Bold.ttf
+
+
+ NotoSansBengaliUI-Regular.ttf
+ NotoSansBengaliUI-Bold.ttf
+
+
+ NotoSansTelugu-Regular.ttf
+ NotoSansTelugu-Bold.ttf
+
+
+ NotoSansTeluguUI-Regular.ttf
+ NotoSansTeluguUI-Bold.ttf
+
+
+ NotoSansKannada-Regular.ttf
+ NotoSansKannada-Bold.ttf
+
+
+ NotoSansKannadaUI-Regular.ttf
+ NotoSansKannadaUI-Bold.ttf
+
+
+ NotoSansOriya-Regular.ttf
+ NotoSansOriya-Bold.ttf
+
+
+ NotoSansOriyaUI-Regular.ttf
+ NotoSansOriyaUI-Bold.ttf
+
+
+ NotoSansSinhala-Regular.ttf
+ NotoSansSinhala-Bold.ttf
+
+
+ NotoSansKhmer-Regular.ttf
+ NotoSansKhmer-Bold.ttf
+
+
+ NotoSansKhmerUI-Regular.ttf
+ NotoSansKhmerUI-Bold.ttf
+
+
+ NotoSansLao-Regular.ttf
+ NotoSansLao-Bold.ttf
+
+
+ NotoSansLaoUI-Regular.ttf
+ NotoSansLaoUI-Bold.ttf
+
+
+ NotoSansMyanmar-Regular.ttf
+ NotoSansMyanmar-Bold.ttf
+
+
+ NotoSansMyanmarUI-Regular.ttf
+ NotoSansMyanmarUI-Bold.ttf
+
+
+ NotoSansThaana-Regular.ttf
+ NotoSansThaana-Bold.ttf
+
+
+ NotoSansCham-Regular.ttf
+ NotoSansCham-Bold.ttf
+
+
+ NotoSansBalinese-Regular.ttf
+
+
+ NotoSansBamum-Regular.ttf
+
+
+ NotoSansBatak-Regular.ttf
+
+
+ NotoSansBuginese-Regular.ttf
+
+
+ NotoSansBuhid-Regular.ttf
+
+
+ NotoSansCanadianAboriginal-Regular.ttf
+
+
+ NotoSansCherokee-Regular.ttf
+
+
+ NotoSansCoptic-Regular.ttf
+
+
+ NotoSansGlagolitic-Regular.ttf
+
+
+ NotoSansHanunoo-Regular.ttf
+
+
+ NotoSansJavanese-Regular.ttf
+
+
+ NotoSansKayahLi-Regular.ttf
+
+
+ NotoSansLepcha-Regular.ttf
+
+
+ NotoSansLimbu-Regular.ttf
+
+
+ NotoSansLisu-Regular.ttf
+
+
+ NotoSansMandaic-Regular.ttf
+
+
+ NotoSansMeeteiMayek-Regular.ttf
+
+
+ NotoSansNewTaiLue-Regular.ttf
+
+
+ NotoSansNKo-Regular.ttf
+
+
+ NotoSansOlChiki-Regular.ttf
+
+
+ NotoSansRejang-Regular.ttf
+
+
+ NotoSansSaurashtra-Regular.ttf
+
+
+ NotoSansSundanese-Regular.ttf
+
+
+ NotoSansSylotiNagri-Regular.ttf
+
+
+ NotoSansSyriacEstrangela-Regular.ttf
+
+
+ NotoSansTagbanwa-Regular.ttf
+
+
+ NotoSansTaiTham-Regular.ttf
+
+
+ NotoSansTaiViet-Regular.ttf
+
+
+ NotoSansTibetan-Regular.ttf
+
+
+ NotoSansTifinagh-Regular.ttf
+
+
+ NotoSansVai-Regular.ttf
+
+
+ NotoSansYi-Regular.ttf
+
+
+ NotoSansSymbols-Regular-Subsetted.ttf
+
+
+ NotoSansJP-Regular.otf
+
+
+ NotoSansKR-Regular.otf
+
+
+ NanumGothic.ttf
+
+
+ NotoColorEmoji.ttf
+
+
+ NotoSansCJKtc-Regular.otf
+ NotoSansCJKtc-Medium.otf
+ NotoSansCJKtc-Bold.otf
+
+
+ NotoSansCJKtc-Regular.otf
+ NotoSansCJKtc-Medium.otf
+ NotoSansCJKtc-Bold.otf
+
+
+ MTLmr3m.ttf
+
+
+
+ NotoSansTaiLe-Regular.ttf
+
+
+ NotoSansMongolian-Regular.ttf
+
+
diff --git a/selfdrive/boardd/boardd b/selfdrive/boardd/boardd
index 8be24c0cf..b101c5574 100755
Binary files a/selfdrive/boardd/boardd and b/selfdrive/boardd/boardd differ
diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index bb9a94e9c..c75c05543 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -3,24 +3,15 @@ from cereal import car
from common.conversions import Conversions as CV
from panda import Panda
from selfdrive.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \
- MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, UNSUPPORTED_DSU_CAR, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR
+ MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, UNSUPPORTED_DSU_CAR, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR
from selfdrive.car import STD_CARGO_KG, scale_tire_stiffness, get_safety_config
from selfdrive.car.interfaces import CarInterfaceBase
from common.params import Params
EventName = car.CarEvent.EventName
-CRUISE_OVERRIDE_SPEED_MIN = 5 * CV.KPH_TO_MS
-
class CarInterface(CarInterfaceBase):
- def __init__(self, CP, CarController, CarState):
- super().__init__(CP, CarController, CarState)
-
- self.dp_cruise_speed = 0. # km/h
- self.dp_override_speed_last = 0. # km/h
- self.dp_override_speed = 0. # m/s
-
@staticmethod
def get_pid_accel_limits(CP, current_speed, cruise_speed):
return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX
@@ -278,19 +269,6 @@ class CarInterface(CarInterfaceBase):
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam)
- # low speed re-write
- if self.dragonconf.dpToyotaCruiseOverride:
- if self.dragonconf.dpToyotaCruiseOverrideSpeed != self.dp_override_speed_last:
- self.dp_override_speed = self.dragonconf.dpToyotaCruiseOverrideSpeed * CV.KPH_TO_MS
- self.dp_override_speed_last = self.dragonconf.dpToyotaCruiseOverrideSpeed
- if self.CP.openpilotLongitudinalControl and ret.cruiseState.speed <= self.dp_override_speed:
- if self.dp_cruise_speed == 0.:
- self.dp_cruise_speed = self.dp_cruise_speed = max(CRUISE_OVERRIDE_SPEED_MIN, ret.vEgo)
- else:
- ret.cruiseState.speed = self.dp_cruise_speed
- else:
- self.dp_cruise_speed = 0.
-
# events
events = self.create_common_events(ret)
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 8847eb013..885bba5fa 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -563,7 +563,10 @@ class Controls:
def state_transition(self, CS):
"""Compute conditional state transitions and execute actions on state transitions"""
- self.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric)
+ # dp - toyota speed override here
+ # dp - @todo may apply to other makes in the future?
+ dp_override_speed = self.sm['dragonConf'].dpToyotaCruiseOverrideSpeed if self.sm['dragonConf'].dpToyotaCruiseOverride else False
+ self.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric, dp_override_speed)
# decrement the soft disable timer at every step, as it's reset on
# entrance in SOFT_DISABLING state
diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py
index 9edd630dc..a0a8f1fb7 100644
--- a/selfdrive/controls/lib/drive_helpers.py
+++ b/selfdrive/controls/lib/drive_helpers.py
@@ -62,12 +62,14 @@ class VCruiseHelper:
self.v_cruise_kph_last = 0
self.button_timers = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0}
self.button_change_states = {btn: {"standstill": False, "enabled": False} for btn in self.button_timers}
+ self.dp_override_v_cruise_kph = V_CRUISE_UNSET
+ self.dp_override_cruise_speed_last = V_CRUISE_UNSET
@property
def v_cruise_initialized(self):
return self.v_cruise_kph != V_CRUISE_UNSET
- def update_v_cruise(self, CS, enabled, is_metric):
+ def update_v_cruise(self, CS, enabled, is_metric, dp_override_speed):
self.v_cruise_kph_last = self.v_cruise_kph
if CS.cruiseState.available:
@@ -77,11 +79,28 @@ class VCruiseHelper:
self.v_cruise_cluster_kph = self.v_cruise_kph
self.update_button_timers(CS, enabled)
else:
- self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
- self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH
- else:
- self.v_cruise_kph = V_CRUISE_UNSET
- self.v_cruise_cluster_kph = V_CRUISE_UNSET
+ if dp_override_speed:
+ # when set speed changed, reset override_speed to unset
+ if CS.cruiseState.speed != self.dp_override_cruise_speed_last:
+ self.dp_override_v_cruise_kph = V_CRUISE_UNSET
+
+ # when override_speed is unset, use current speed as set speed
+ if CS.cruiseState.speed * CV.MS_TO_KPH < dp_override_speed:
+ self.dp_override_v_cruise_kph = clip(CS.vEgo * CV.MS_TO_KPH, V_CRUISE_MIN, V_CRUISE_MAX)
+
+ # when we have an override_speed, use it
+ if self.dp_override_v_cruise_kph != V_CRUISE_UNSET:
+ self.v_cruise_kph = self.dp_override_v_cruise_kph
+ self.v_cruise_cluster_kph = self.dp_override_v_cruise_kph
+ else:
+ self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
+ self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH
+
+ self.dp_override_cruise_speed_last = CS.cruiseState.speed
+ else:
+ self.dp_override_v_cruise_kph = V_CRUISE_UNSET
+ self.v_cruise_kph = V_CRUISE_UNSET
+ self.v_cruise_cluster_kph = V_CRUISE_UNSET
def _update_v_cruise_non_pcm(self, CS, enabled, is_metric):
# handle button presses. TODO: this should be in state_control, but a decelCruise press
@@ -158,7 +177,6 @@ class VCruiseHelper:
self.v_cruise_cluster_kph = self.v_cruise_kph
-
def apply_deadzone(error, deadzone):
if error > deadzone:
error -= deadzone
diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py
index 53f104bf7..2b1369cc9 100644
--- a/selfdrive/controls/lib/lateral_planner.py
+++ b/selfdrive/controls/lib/lateral_planner.py
@@ -35,7 +35,6 @@ class LateralPlanner:
self.LP = LanePlanner(Params().get_bool('WideCameraOnly'))
self.dp_lanelines_enable = False
self.dp_lanelines_active = False
- self.dp_lanelines_active_tmp = False
self.dp_camera_offset = 4 if TICI else -6
self.dp_path_offset = 4 if TICI else 0
@@ -84,6 +83,7 @@ class LateralPlanner:
# dp - when laneline mode enabled, we use old logic (including lane changing)
d_path_xyz = self.lanelines_mode(md, sm['carState'], sm['carControl'].latActive, sm['dragonConf'])
else:
+ self.dp_lanelines_active = False
# dp -- tab spacing begin (stock logic) --
# Lane change logic
desire_state = md.meta.desireState
@@ -152,7 +152,7 @@ class LateralPlanner:
lateralPlan.solverExecutionTime = self.lat_mpc.solve_time
lateralPlan.desire = self.DH.desire
- lateralPlan.useLaneLines = self.dp_lanelines_enable and self.dp_lanelines_active
+ lateralPlan.useLaneLines = self.dp_lanelines_active
lateralPlan.laneChangeState = self.DH.lane_change_state
lateralPlan.laneChangeDirection = self.DH.lane_change_direction
@@ -177,8 +177,7 @@ class LateralPlanner:
self.LP.rll_prob *= self.DH.lane_change_ll_prob
# dynamic laneline/laneless logic
- self.dp_lanelines_active_tmp = get_lane_laneless_mode(self.LP.lll_prob, self.LP.rll_prob, self.dp_lanelines_active_tmp)
- self.dp_lanelines_active = self.dp_lanelines_active_tmp
+ self.dp_lanelines_active = get_lane_laneless_mode(self.LP.lll_prob, self.LP.rll_prob, self.dp_lanelines_active)
# Calculate final driving path and set MPC costs
if self.dp_lanelines_active:
diff --git a/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/acados_ocp_lat.json b/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/acados_ocp_lat.json
new file mode 100644
index 000000000..1cbe32253
--- /dev/null
+++ b/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/acados_ocp_lat.json
@@ -0,0 +1,450 @@
+{
+ "acados_include_path": "/data/openpilot/third_party/acados/include/acados/include",
+ "acados_lib_path": "/data/openpilot/third_party/acados/include/acados/lib",
+ "code_export_directory": "/data/openpilot/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code",
+ "constraints": {
+ "C": [],
+ "C_e": [],
+ "D": [],
+ "constr_type": "BGH",
+ "constr_type_e": "BGH",
+ "idxbu": [],
+ "idxbx": [
+ 2,
+ 3
+ ],
+ "idxbx_0": [
+ 0,
+ 1,
+ 2,
+ 3
+ ],
+ "idxbx_e": [],
+ "idxbxe_0": [
+ 0,
+ 1,
+ 2,
+ 3
+ ],
+ "idxsbu": [],
+ "idxsbx": [],
+ "idxsbx_e": [],
+ "idxsg": [],
+ "idxsg_e": [],
+ "idxsh": [],
+ "idxsh_e": [],
+ "idxsphi": [],
+ "idxsphi_e": [],
+ "lbu": [],
+ "lbx": [
+ -1.5707963267948966,
+ -0.8726646259971648
+ ],
+ "lbx_0": [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ "lbx_e": [],
+ "lg": [],
+ "lg_e": [],
+ "lh": [],
+ "lh_e": [],
+ "lphi": [],
+ "lphi_e": [],
+ "lsbu": [],
+ "lsbx": [],
+ "lsbx_e": [],
+ "lsg": [],
+ "lsg_e": [],
+ "lsh": [],
+ "lsh_e": [],
+ "lsphi": [],
+ "lsphi_e": [],
+ "ubu": [],
+ "ubx": [
+ 1.5707963267948966,
+ 0.8726646259971648
+ ],
+ "ubx_0": [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ "ubx_e": [],
+ "ug": [],
+ "ug_e": [],
+ "uh": [],
+ "uh_e": [],
+ "uphi": [],
+ "uphi_e": [],
+ "usbu": [],
+ "usbx": [],
+ "usbx_e": [],
+ "usg": [],
+ "usg_e": [],
+ "ush": [],
+ "ush_e": [],
+ "usphi": [],
+ "usphi_e": []
+ },
+ "cost": {
+ "Vu": [
+ [
+ 0.0
+ ],
+ [
+ 0.0
+ ],
+ [
+ 0.0
+ ]
+ ],
+ "Vu_0": [
+ [
+ 0.0
+ ],
+ [
+ 0.0
+ ],
+ [
+ 0.0
+ ]
+ ],
+ "Vx": [
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ]
+ ],
+ "Vx_0": [
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ]
+ ],
+ "Vx_e": [
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ]
+ ],
+ "Vz": [],
+ "Vz_0": [],
+ "W": [
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ]
+ ],
+ "W_0": [
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ]
+ ],
+ "W_e": [
+ [
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0
+ ]
+ ],
+ "Zl": [],
+ "Zl_e": [],
+ "Zu": [],
+ "Zu_e": [],
+ "cost_ext_fun_type": "casadi",
+ "cost_ext_fun_type_0": "casadi",
+ "cost_ext_fun_type_e": "casadi",
+ "cost_type": "NONLINEAR_LS",
+ "cost_type_0": "NONLINEAR_LS",
+ "cost_type_e": "NONLINEAR_LS",
+ "yref": [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ "yref_0": [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ "yref_e": [
+ 0.0,
+ 0.0
+ ],
+ "zl": [],
+ "zl_e": [],
+ "zu": [],
+ "zu_e": []
+ },
+ "cython_include_dirs": "/data/data/com.termux/files/usr/lib/python3.8/site-packages/numpy/core/include",
+ "dims": {
+ "N": 16,
+ "nbu": 0,
+ "nbx": 2,
+ "nbx_0": 4,
+ "nbx_e": 0,
+ "nbxe_0": 4,
+ "ng": 0,
+ "ng_e": 0,
+ "nh": 0,
+ "nh_e": 0,
+ "np": 2,
+ "nphi": 0,
+ "nphi_e": 0,
+ "nr": 0,
+ "nr_e": 0,
+ "ns": 0,
+ "ns_e": 0,
+ "nsbu": 0,
+ "nsbx": 0,
+ "nsbx_e": 0,
+ "nsg": 0,
+ "nsg_e": 0,
+ "nsh": 0,
+ "nsh_e": 0,
+ "nsphi": 0,
+ "nsphi_e": 0,
+ "nu": 1,
+ "nx": 4,
+ "ny": 3,
+ "ny_0": 3,
+ "ny_e": 2,
+ "nz": 0
+ },
+ "model": {
+ "dyn_disc_fun": null,
+ "dyn_disc_fun_jac": null,
+ "dyn_disc_fun_jac_hess": null,
+ "dyn_ext_fun_type": "casadi",
+ "dyn_source_discrete": null,
+ "gnsf": {
+ "nontrivial_f_LO": 1,
+ "purely_linear": 0
+ },
+ "name": "lat"
+ },
+ "parameter_values": [
+ 0.0,
+ 0.0
+ ],
+ "problem_class": "OCP",
+ "simulink_opts": {
+ "inputs": {
+ "cost_W": 0,
+ "cost_W_0": 0,
+ "cost_W_e": 0,
+ "lbu": 1,
+ "lbx": 1,
+ "lbx_0": 1,
+ "lbx_e": 1,
+ "lg": 1,
+ "lh": 1,
+ "parameter_traj": 1,
+ "reset_solver": 0,
+ "u_init": 0,
+ "ubu": 1,
+ "ubx": 1,
+ "ubx_0": 1,
+ "ubx_e": 1,
+ "ug": 1,
+ "uh": 1,
+ "x_init": 0,
+ "y_ref": 1,
+ "y_ref_0": 1,
+ "y_ref_e": 1
+ },
+ "outputs": {
+ "CPU_time": 1,
+ "CPU_time_lin": 0,
+ "CPU_time_qp": 0,
+ "CPU_time_sim": 0,
+ "KKT_residual": 1,
+ "solver_status": 1,
+ "sqp_iter": 1,
+ "u0": 1,
+ "utraj": 0,
+ "x1": 1,
+ "xtraj": 0
+ },
+ "samplingtime": "t0"
+ },
+ "solver_options": {
+ "Tsim": 0.009765625,
+ "alpha_min": 0.05,
+ "alpha_reduction": 0.7,
+ "collocation_type": "GAUSS_LEGENDRE",
+ "eps_sufficient_descent": 0.0001,
+ "exact_hess_constr": 1,
+ "exact_hess_cost": 1,
+ "exact_hess_dyn": 1,
+ "ext_cost_num_hess": 0,
+ "full_step_dual": 0,
+ "globalization": "FIXED_STEP",
+ "globalization_use_SOC": 0,
+ "hessian_approx": "GAUSS_NEWTON",
+ "hpipm_mode": "BALANCE",
+ "initialize_t_slacks": 0,
+ "integrator_type": "ERK",
+ "levenberg_marquardt": 0.0,
+ "line_search_use_sufficient_descent": 0,
+ "model_external_shared_lib_dir": null,
+ "model_external_shared_lib_name": null,
+ "nlp_solver_max_iter": 100,
+ "nlp_solver_step_length": 1.0,
+ "nlp_solver_tol_comp": 1e-06,
+ "nlp_solver_tol_eq": 1e-06,
+ "nlp_solver_tol_ineq": 1e-06,
+ "nlp_solver_tol_stat": 1e-06,
+ "nlp_solver_type": "SQP_RTI",
+ "print_level": 0,
+ "qp_solver": "PARTIAL_CONDENSING_HPIPM",
+ "qp_solver_cond_N": 1,
+ "qp_solver_iter_max": 1,
+ "qp_solver_tol_comp": null,
+ "qp_solver_tol_eq": null,
+ "qp_solver_tol_ineq": null,
+ "qp_solver_tol_stat": null,
+ "qp_solver_warm_start": 0,
+ "regularize_method": null,
+ "sim_method_jac_reuse": [
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0
+ ],
+ "sim_method_newton_iter": 3,
+ "sim_method_num_stages": [
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4
+ ],
+ "sim_method_num_steps": [
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1
+ ],
+ "tf": 2.5,
+ "time_steps": [
+ 0.009765625,
+ 0.029296875,
+ 0.048828125,
+ 0.068359375,
+ 0.087890625,
+ 0.107421875,
+ 0.126953125,
+ 0.146484375,
+ 0.166015625,
+ 0.185546875,
+ 0.205078125,
+ 0.224609375,
+ 0.244140625,
+ 0.263671875,
+ 0.283203125,
+ 0.302734375
+ ]
+ }
+}
\ No newline at end of file
diff --git a/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c b/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c
new file mode 100644
index 000000000..d0c541d3b
--- /dev/null
+++ b/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c
@@ -0,0 +1,278 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+// standard
+#include
+#include
+
+// acados
+#include "acados_c/external_function_interface.h"
+#include "acados_c/sim_interface.h"
+#include "acados_c/external_function_interface.h"
+
+#include "acados/sim/sim_common.h"
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/print.h"
+
+
+// example specific
+#include "lat_model/lat_model.h"
+#include "acados_sim_solver_lat.h"
+
+
+// ** solver data **
+
+sim_solver_capsule * lat_acados_sim_solver_create_capsule()
+{
+ void* capsule_mem = malloc(sizeof(sim_solver_capsule));
+ sim_solver_capsule *capsule = (sim_solver_capsule *) capsule_mem;
+
+ return capsule;
+}
+
+
+int lat_acados_sim_solver_free_capsule(sim_solver_capsule * capsule)
+{
+ free(capsule);
+ return 0;
+}
+
+
+int lat_acados_sim_create(sim_solver_capsule * capsule)
+{
+ // initialize
+ const int nx = LAT_NX;
+ const int nu = LAT_NU;
+ const int nz = LAT_NZ;
+ const int np = LAT_NP;
+ bool tmp_bool;
+
+
+ double Tsim = 0.009765625;
+
+
+ // explicit ode
+ capsule->sim_forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+ capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+
+ capsule->sim_forw_vde_casadi->casadi_fun = &lat_expl_vde_forw;
+ capsule->sim_forw_vde_casadi->casadi_n_in = &lat_expl_vde_forw_n_in;
+ capsule->sim_forw_vde_casadi->casadi_n_out = &lat_expl_vde_forw_n_out;
+ capsule->sim_forw_vde_casadi->casadi_sparsity_in = &lat_expl_vde_forw_sparsity_in;
+ capsule->sim_forw_vde_casadi->casadi_sparsity_out = &lat_expl_vde_forw_sparsity_out;
+ capsule->sim_forw_vde_casadi->casadi_work = &lat_expl_vde_forw_work;
+ external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np);
+
+ capsule->sim_expl_ode_fun_casadi->casadi_fun = &lat_expl_ode_fun;
+ capsule->sim_expl_ode_fun_casadi->casadi_n_in = &lat_expl_ode_fun_n_in;
+ capsule->sim_expl_ode_fun_casadi->casadi_n_out = &lat_expl_ode_fun_n_out;
+ capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &lat_expl_ode_fun_sparsity_in;
+ capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &lat_expl_ode_fun_sparsity_out;
+ capsule->sim_expl_ode_fun_casadi->casadi_work = &lat_expl_ode_fun_work;
+ external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, np);
+
+
+
+ // sim plan & config
+ sim_solver_plan_t plan;
+ plan.sim_solver = ERK;
+
+ // create correct config based on plan
+ sim_config * lat_sim_config = sim_config_create(plan);
+ capsule->acados_sim_config = lat_sim_config;
+
+ // sim dims
+ void *lat_sim_dims = sim_dims_create(lat_sim_config);
+ capsule->acados_sim_dims = lat_sim_dims;
+ sim_dims_set(lat_sim_config, lat_sim_dims, "nx", &nx);
+ sim_dims_set(lat_sim_config, lat_sim_dims, "nu", &nu);
+ sim_dims_set(lat_sim_config, lat_sim_dims, "nz", &nz);
+
+
+ // sim opts
+ sim_opts *lat_sim_opts = sim_opts_create(lat_sim_config, lat_sim_dims);
+ capsule->acados_sim_opts = lat_sim_opts;
+ int tmp_int = 3;
+ sim_opts_set(lat_sim_config, lat_sim_opts, "newton_iter", &tmp_int);
+ sim_collocation_type collocation_type = GAUSS_LEGENDRE;
+ sim_opts_set(lat_sim_config, lat_sim_opts, "collocation_type", &collocation_type);
+
+
+ tmp_int = 4;
+ sim_opts_set(lat_sim_config, lat_sim_opts, "num_stages", &tmp_int);
+ tmp_int = 1;
+ sim_opts_set(lat_sim_config, lat_sim_opts, "num_steps", &tmp_int);
+ tmp_bool = 0;
+ sim_opts_set(lat_sim_config, lat_sim_opts, "jac_reuse", &tmp_bool);
+
+
+ // sim in / out
+ sim_in *lat_sim_in = sim_in_create(lat_sim_config, lat_sim_dims);
+ capsule->acados_sim_in = lat_sim_in;
+ sim_out *lat_sim_out = sim_out_create(lat_sim_config, lat_sim_dims);
+ capsule->acados_sim_out = lat_sim_out;
+
+ sim_in_set(lat_sim_config, lat_sim_dims,
+ lat_sim_in, "T", &Tsim);
+
+ // model functions
+ lat_sim_config->model_set(lat_sim_in->model,
+ "expl_vde_for", capsule->sim_forw_vde_casadi);
+ lat_sim_config->model_set(lat_sim_in->model,
+ "expl_ode_fun", capsule->sim_expl_ode_fun_casadi);
+
+ // sim solver
+ sim_solver *lat_sim_solver = sim_solver_create(lat_sim_config,
+ lat_sim_dims, lat_sim_opts);
+ capsule->acados_sim_solver = lat_sim_solver;
+
+
+ /* initialize parameter values */
+ double* p = calloc(np, sizeof(double));
+
+
+ lat_acados_sim_update_params(capsule, p, np);
+ free(p);
+
+
+ /* initialize input */
+ // x
+ double x0[4];
+ for (int ii = 0; ii < 4; ii++)
+ x0[ii] = 0.0;
+
+ sim_in_set(lat_sim_config, lat_sim_dims,
+ lat_sim_in, "x", x0);
+
+
+ // u
+ double u0[1];
+ for (int ii = 0; ii < 1; ii++)
+ u0[ii] = 0.0;
+
+ sim_in_set(lat_sim_config, lat_sim_dims,
+ lat_sim_in, "u", u0);
+
+ // S_forw
+ double S_forw[20];
+ for (int ii = 0; ii < 20; ii++)
+ S_forw[ii] = 0.0;
+ for (int ii = 0; ii < 4; ii++)
+ S_forw[ii + ii * 4 ] = 1.0;
+
+
+ sim_in_set(lat_sim_config, lat_sim_dims,
+ lat_sim_in, "S_forw", S_forw);
+
+ int status = sim_precompute(lat_sim_solver, lat_sim_in, lat_sim_out);
+
+ return status;
+}
+
+
+int lat_acados_sim_solve(sim_solver_capsule *capsule)
+{
+ // integrate dynamics using acados sim_solver
+ int status = sim_solve(capsule->acados_sim_solver,
+ capsule->acados_sim_in, capsule->acados_sim_out);
+ if (status != 0)
+ printf("error in lat_acados_sim_solve()! Exiting.\n");
+
+ return status;
+}
+
+
+int lat_acados_sim_free(sim_solver_capsule *capsule)
+{
+ // free memory
+ sim_solver_destroy(capsule->acados_sim_solver);
+ sim_in_destroy(capsule->acados_sim_in);
+ sim_out_destroy(capsule->acados_sim_out);
+ sim_opts_destroy(capsule->acados_sim_opts);
+ sim_dims_destroy(capsule->acados_sim_dims);
+ sim_config_destroy(capsule->acados_sim_config);
+
+ // free external function
+ external_function_param_casadi_free(capsule->sim_forw_vde_casadi);
+ external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi);
+
+ return 0;
+}
+
+
+int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np)
+{
+ int status = 0;
+ int casadi_np = LAT_NP;
+
+ if (casadi_np != np) {
+ printf("lat_acados_sim_update_params: trying to set %i parameters for external functions."
+ " External function has %i parameters. Exiting.\n", np, casadi_np);
+ exit(1);
+ }
+ capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p);
+ capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p);
+
+ return status;
+}
+
+/* getters pointers to C objects*/
+sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule)
+{
+ return capsule->acados_sim_config;
+};
+
+sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule)
+{
+ return capsule->acados_sim_in;
+};
+
+sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule)
+{
+ return capsule->acados_sim_out;
+};
+
+void * lat_acados_get_sim_dims(sim_solver_capsule *capsule)
+{
+ return capsule->acados_sim_dims;
+};
+
+sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule)
+{
+ return capsule->acados_sim_opts;
+};
+
+sim_solver * lat_acados_get_sim_solver(sim_solver_capsule *capsule)
+{
+ return capsule->acados_sim_solver;
+};
+
diff --git a/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h b/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h
new file mode 100644
index 000000000..86b9c84c9
--- /dev/null
+++ b/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h
@@ -0,0 +1,103 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+#ifndef ACADOS_SIM_lat_H_
+#define ACADOS_SIM_lat_H_
+
+#include "acados_c/sim_interface.h"
+#include "acados_c/external_function_interface.h"
+
+#define LAT_NX 4
+#define LAT_NZ 0
+#define LAT_NU 1
+#define LAT_NP 2
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+// ** capsule for solver data **
+typedef struct sim_solver_capsule
+{
+ // acados objects
+ sim_in *acados_sim_in;
+ sim_out *acados_sim_out;
+ sim_solver *acados_sim_solver;
+ sim_opts *acados_sim_opts;
+ sim_config *acados_sim_config;
+ void *acados_sim_dims;
+
+ /* external functions */
+ // ERK
+ external_function_param_casadi * sim_forw_vde_casadi;
+ external_function_param_casadi * sim_expl_ode_fun_casadi;
+ external_function_param_casadi * sim_expl_ode_hess;
+
+ // IRK
+ external_function_param_casadi * sim_impl_dae_fun;
+ external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z;
+ external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z;
+ external_function_param_casadi * sim_impl_dae_hess;
+
+ // GNSF
+ external_function_param_casadi * sim_gnsf_phi_fun;
+ external_function_param_casadi * sim_gnsf_phi_fun_jac_y;
+ external_function_param_casadi * sim_gnsf_phi_jac_y_uhat;
+ external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z;
+ external_function_param_casadi * sim_gnsf_get_matrices_fun;
+
+} sim_solver_capsule;
+
+
+ACADOS_SYMBOL_EXPORT int lat_acados_sim_create(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT int lat_acados_sim_solve(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT int lat_acados_sim_free(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *value, int np);
+
+ACADOS_SYMBOL_EXPORT sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT void * lat_acados_get_sim_dims(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT sim_solver * lat_acados_get_sim_solver(sim_solver_capsule *capsule);
+
+
+ACADOS_SYMBOL_EXPORT sim_solver_capsule * lat_acados_sim_solver_create_capsule(void);
+ACADOS_SYMBOL_EXPORT int lat_acados_sim_solver_free_capsule(sim_solver_capsule *capsule);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // ACADOS_SIM_lat_H_
diff --git a/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c b/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c
new file mode 100644
index 000000000..827b6c7f0
--- /dev/null
+++ b/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c
@@ -0,0 +1,264 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+#define S_FUNCTION_NAME acados_solver_sfunction_lat
+#define S_FUNCTION_LEVEL 2
+
+#define MDL_START
+
+// acados
+// #include "acados/utils/print.h"
+#include "acados_c/sim_interface.h"
+#include "acados_c/external_function_interface.h"
+
+// example specific
+#include "lat_model/lat_model.h"
+#include "acados_solver_lat.h"
+
+#include "simstruc.h"
+
+#define SAMPLINGTIME 0.009765625
+
+static void mdlInitializeSizes (SimStruct *S)
+{
+ // specify the number of continuous and discrete states
+ ssSetNumContStates(S, 0);
+ ssSetNumDiscStates(S, 0);// specify the number of input ports
+ if ( !ssSetNumInputPorts(S, 8) )
+ return;
+
+ // specify the number of output ports
+ if ( !ssSetNumOutputPorts(S, 6) )
+ return;
+
+ // specify dimension information for the input ports
+ // lbx_0
+ ssSetInputPortVectorDimension(S, 0, 4);
+ // ubx_0
+ ssSetInputPortVectorDimension(S, 1, 4);
+ // parameters
+ ssSetInputPortVectorDimension(S, 2, (16+1) * 2);
+ // y_ref_0
+ ssSetInputPortVectorDimension(S, 3, 3);
+ // y_ref
+ ssSetInputPortVectorDimension(S, 4, 45);
+ // y_ref_e
+ ssSetInputPortVectorDimension(S, 5, 2);
+ // lbx
+ ssSetInputPortVectorDimension(S, 6, 30);
+ // ubx
+ ssSetInputPortVectorDimension(S, 7, 30);/* specify dimension information for the OUTPUT ports */
+ ssSetOutputPortVectorDimension(S, 0, 1 );
+ ssSetOutputPortVectorDimension(S, 1, 1 );
+ ssSetOutputPortVectorDimension(S, 2, 1 );
+ ssSetOutputPortVectorDimension(S, 3, 4 ); // state at shooting node 1
+ ssSetOutputPortVectorDimension(S, 4, 1);
+ ssSetOutputPortVectorDimension(S, 5, 1 );
+
+ // specify the direct feedthrough status
+ // should be set to 1 for all inputs used in mdlOutputs
+ ssSetInputPortDirectFeedThrough(S, 0, 1);
+ ssSetInputPortDirectFeedThrough(S, 1, 1);
+ ssSetInputPortDirectFeedThrough(S, 2, 1);
+ ssSetInputPortDirectFeedThrough(S, 3, 1);
+ ssSetInputPortDirectFeedThrough(S, 4, 1);
+ ssSetInputPortDirectFeedThrough(S, 5, 1);
+ ssSetInputPortDirectFeedThrough(S, 6, 1);
+ ssSetInputPortDirectFeedThrough(S, 7, 1);
+
+ // one sample time
+ ssSetNumSampleTimes(S, 1);
+}
+
+
+#if defined(MATLAB_MEX_FILE)
+
+#define MDL_SET_INPUT_PORT_DIMENSION_INFO
+#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
+
+static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
+{
+ if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
+ return;
+}
+
+static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
+{
+ if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
+ return;
+}
+
+#endif /* MATLAB_MEX_FILE */
+
+
+static void mdlInitializeSampleTimes(SimStruct *S)
+{
+ ssSetSampleTime(S, 0, SAMPLINGTIME);
+ ssSetOffsetTime(S, 0, 0.0);
+}
+
+
+static void mdlStart(SimStruct *S)
+{
+ lat_solver_capsule *capsule = lat_acados_create_capsule();
+ lat_acados_create(capsule);
+
+ ssSetUserData(S, (void*)capsule);
+}
+
+
+static void mdlOutputs(SimStruct *S, int_T tid)
+{
+ lat_solver_capsule *capsule = ssGetUserData(S);
+ ocp_nlp_config *nlp_config = lat_acados_get_nlp_config(capsule);
+ ocp_nlp_dims *nlp_dims = lat_acados_get_nlp_dims(capsule);
+ ocp_nlp_in *nlp_in = lat_acados_get_nlp_in(capsule);
+ ocp_nlp_out *nlp_out = lat_acados_get_nlp_out(capsule);
+
+ InputRealPtrsType in_sign;
+
+ // local buffer
+ real_t buffer[4];
+
+ /* go through inputs */
+ // lbx_0
+ in_sign = ssGetInputPortRealSignalPtrs(S, 0);
+ for (int i = 0; i < 4; i++)
+ buffer[i] = (double)(*in_sign[i]);
+
+ ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", buffer);
+ // ubx_0
+ in_sign = ssGetInputPortRealSignalPtrs(S, 1);
+ for (int i = 0; i < 4; i++)
+ buffer[i] = (double)(*in_sign[i]);
+ ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", buffer);
+ // parameters - stage-variant !!!
+ in_sign = ssGetInputPortRealSignalPtrs(S, 2);
+
+ // update value of parameters
+ for (int ii = 0; ii <= 16; ii++)
+ {
+ for (int jj = 0; jj < 2; jj++)
+ buffer[jj] = (double)(*in_sign[ii*2+jj]);
+ lat_acados_update_params(capsule, ii, buffer, 2);
+ }
+
+
+ // y_ref_0
+ in_sign = ssGetInputPortRealSignalPtrs(S, 3);
+
+ for (int i = 0; i < 3; i++)
+ buffer[i] = (double)(*in_sign[i]);
+
+ ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", (void *) buffer);
+
+
+ // y_ref - for stages 1 to N-1
+ in_sign = ssGetInputPortRealSignalPtrs(S, 4);
+
+ for (int ii = 1; ii < 16; ii++)
+ {
+ for (int jj = 0; jj < 3; jj++)
+ buffer[jj] = (double)(*in_sign[(ii-1)*3+jj]);
+ ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "yref", (void *) buffer);
+ }
+
+
+ // y_ref_e
+ in_sign = ssGetInputPortRealSignalPtrs(S, 5);
+
+ for (int i = 0; i < 2; i++)
+ buffer[i] = (double)(*in_sign[i]);
+
+ ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 16, "yref", (void *) buffer);
+ // lbx
+ in_sign = ssGetInputPortRealSignalPtrs(S, 6);
+ for (int ii = 1; ii < 16; ii++)
+ {
+ for (int jj = 0; jj < 2; jj++)
+ buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]);
+ ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbx", (void *) buffer);
+ }
+ // ubx
+ in_sign = ssGetInputPortRealSignalPtrs(S, 7);
+ for (int ii = 1; ii < 16; ii++)
+ {
+ for (int jj = 0; jj < 2; jj++)
+ buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]);
+ ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubx", (void *) buffer);
+ }
+
+ /* call solver */
+ int rti_phase = 0;
+ ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "rti_phase", &rti_phase);
+ int acados_status = lat_acados_solve(capsule);
+
+
+ /* set outputs */
+ // assign pointers to output signals
+ real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin;
+ int tmp_int;
+ out_u0 = ssGetOutputPortRealSignal(S, 0);
+ ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", (void *) out_u0);
+
+
+ out_status = ssGetOutputPortRealSignal(S, 1);
+ *out_status = (real_t) acados_status;
+ out_KKT_res = ssGetOutputPortRealSignal(S, 2);
+ *out_KKT_res = (real_t) nlp_out->inf_norm_res;
+ out_x1 = ssGetOutputPortRealSignal(S, 3);
+ ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 1, "x", (void *) out_x1);
+ out_cpu_time = ssGetOutputPortRealSignal(S, 4);
+ // get solution time
+ ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_tot", (void *) out_cpu_time);
+ out_sqp_iter = ssGetOutputPortRealSignal(S, 5);
+ // get sqp iter
+ ocp_nlp_get(nlp_config, capsule->nlp_solver, "sqp_iter", (void *) &tmp_int);
+ *out_sqp_iter = (real_t) tmp_int;
+
+}
+
+static void mdlTerminate(SimStruct *S)
+{
+ lat_solver_capsule *capsule = ssGetUserData(S);
+
+ lat_acados_free(capsule);
+ lat_acados_free_capsule(capsule);
+}
+
+
+#ifdef MATLAB_MEX_FILE
+#include "simulink.c"
+#else
+#include "cg_sfun.h"
+#endif
diff --git a/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c b/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c
new file mode 100644
index 000000000..c536654fd
--- /dev/null
+++ b/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c
@@ -0,0 +1,128 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+// standard
+#include
+#include
+// acados
+#include "acados/utils/print.h"
+#include "acados/utils/math.h"
+#include "acados_c/sim_interface.h"
+#include "acados_sim_solver_lat.h"
+
+#define NX LAT_NX
+#define NZ LAT_NZ
+#define NU LAT_NU
+#define NP LAT_NP
+
+
+int main()
+{
+ int status = 0;
+ sim_solver_capsule *capsule = lat_acados_sim_solver_create_capsule();
+ status = lat_acados_sim_create(capsule);
+
+ if (status)
+ {
+ printf("acados_create() returned status %d. Exiting.\n", status);
+ exit(1);
+ }
+
+ sim_config *acados_sim_config = lat_acados_get_sim_config(capsule);
+ sim_in *acados_sim_in = lat_acados_get_sim_in(capsule);
+ sim_out *acados_sim_out = lat_acados_get_sim_out(capsule);
+ void *acados_sim_dims = lat_acados_get_sim_dims(capsule);
+
+ // initial condition
+ double x_current[NX];
+ x_current[0] = 0.0;
+ x_current[1] = 0.0;
+ x_current[2] = 0.0;
+ x_current[3] = 0.0;
+
+
+ x_current[0] = 0;
+ x_current[1] = 0;
+ x_current[2] = 0;
+ x_current[3] = 0;
+
+
+
+
+ // initial value for control input
+ double u0[NU];
+ u0[0] = 0.0;
+ // set parameters
+ double p[NP];
+ p[0] = 0;
+ p[1] = 0;
+
+ lat_acados_sim_update_params(capsule, p, NP);
+
+
+ int n_sim_steps = 3;
+ // solve ocp in loop
+ for (int ii = 0; ii < n_sim_steps; ii++)
+ {
+ sim_in_set(acados_sim_config, acados_sim_dims,
+ acados_sim_in, "x", x_current);
+ status = lat_acados_sim_solve(capsule);
+
+ if (status != ACADOS_SUCCESS)
+ {
+ printf("acados_solve() failed with status %d.\n", status);
+ }
+
+ sim_out_get(acados_sim_config, acados_sim_dims,
+ acados_sim_out, "x", x_current);
+
+ printf("\nx_current, %d\n", ii);
+ for (int jj = 0; jj < NX; jj++)
+ {
+ printf("%e\n", x_current[jj]);
+ }
+ }
+
+ printf("\nPerformed %d simulation steps with acados integrator successfully.\n\n", n_sim_steps);
+
+ // free solver
+ status = lat_acados_sim_free(capsule);
+ if (status) {
+ printf("lat_acados_sim_free() returned status %d. \n", status);
+ }
+
+ lat_acados_sim_solver_free_capsule(capsule);
+
+ return status;
+}
diff --git a/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m b/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m
new file mode 100644
index 000000000..2c6679ecb
--- /dev/null
+++ b/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m
@@ -0,0 +1,125 @@
+%
+% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+%
+% This file is part of acados.
+%
+% The 2-Clause BSD License
+%
+% Redistribution and use in source and binary forms, with or without
+% modification, are permitted provided that the following conditions are met:
+%
+% 1. Redistributions of source code must retain the above copyright notice,
+% this list of conditions and the following disclaimer.
+%
+% 2. Redistributions in binary form must reproduce the above copyright notice,
+% this list of conditions and the following disclaimer in the documentation
+% and/or other materials provided with the distribution.
+%
+% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+% POSSIBILITY OF SUCH DAMAGE.;
+%
+
+SOURCES = { ...
+ 'lat_model/lat_expl_ode_fun.c', ...
+ 'lat_model/lat_expl_vde_forw.c',...
+ 'lat_cost/lat_cost_y_0_fun.c',...
+ 'lat_cost/lat_cost_y_0_fun_jac_ut_xt.c',...
+ 'lat_cost/lat_cost_y_0_hess.c',...
+ 'lat_cost/lat_cost_y_fun.c',...
+ 'lat_cost/lat_cost_y_fun_jac_ut_xt.c',...
+ 'lat_cost/lat_cost_y_hess.c',...
+ 'lat_cost/lat_cost_y_e_fun.c',...
+ 'lat_cost/lat_cost_y_e_fun_jac_ut_xt.c',...
+ 'lat_cost/lat_cost_y_e_hess.c',...
+ 'acados_solver_sfunction_lat.c', ...
+ 'acados_solver_lat.c'
+ };
+
+INC_PATH = '/data/openpilot/third_party/acados/include/acados/include';
+
+INCS = {['-I', fullfile(INC_PATH, 'blasfeo', 'include')], ...
+ ['-I', fullfile(INC_PATH, 'hpipm', 'include')], ...
+ ['-I', fullfile(INC_PATH, 'acados')], ...
+ ['-I', fullfile(INC_PATH)]};
+
+
+
+CFLAGS = 'CFLAGS=$CFLAGS';
+LDFLAGS = 'LDFLAGS=$LDFLAGS';
+COMPFLAGS = 'COMPFLAGS=$COMPFLAGS';
+COMPDEFINES = 'COMPDEFINES=$COMPDEFINES';
+
+
+
+LIB_PATH = ['-L', fullfile('/data/openpilot/third_party/acados/include/acados/lib')];
+
+LIBS = {'-lacados', '-lhpipm', '-lblasfeo'};
+
+% acados linking libraries and flags
+
+
+mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ...
+ LIB_PATH, LIBS{:}, SOURCES{:}, ...
+ '-output', 'acados_solver_sfunction_lat' );
+
+fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_lat', '.', ...
+ eval('mexext')] );
+
+
+%% print note on usage of s-function
+fprintf('\n\nNote: Usage of Sfunction is as follows:\n')
+input_note = 'Inputs are:\n';
+i_in = 1;
+input_note = strcat(input_note, num2str(i_in), ') lbx_0 - lower bound on x for stage 0,',...
+ ' size [4]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',...
+ ' size [4]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all shooting nodes 0 to N+1,',...
+ ' size [34]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [3]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') y_ref - concatenated for shooting nodes 1 to N-1,',...
+ ' size [45]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [2]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') lbx for shooting nodes 1 to N-1, size [30]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') ubx for shooting nodes 1 to N-1, size [30]\n ');
+i_in = i_in + 1;
+
+fprintf(input_note)
+
+disp(' ')
+
+output_note = 'Outputs are:\n';
+i_out = 0;
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') u0, control input at node 0, size [1]\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') acados solver status (0 = SUCCESS)\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') KKT residual\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') x1, state at node 1\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') CPU time\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n ');
+
+fprintf(output_note)
diff --git a/selfdrive/dragonpilot/controls_0813/lib/lateral_planner.py b/selfdrive/dragonpilot/controls_0813/lib/lateral_planner.py
index 646ea5872..bee2764c4 100644
--- a/selfdrive/dragonpilot/controls_0813/lib/lateral_planner.py
+++ b/selfdrive/dragonpilot/controls_0813/lib/lateral_planner.py
@@ -80,10 +80,13 @@ class LateralPlanner:
# dynamic laneline/laneless logic
# decide if we want to use lanelines or laneless
- self.dp_lanelines_active = get_lane_laneless_mode(self.LP.lll_prob, self.LP.rll_prob, self.dp_lanelines_active)
+ if self.dp_lanelines_enable:
+ self.dp_lanelines_active = get_lane_laneless_mode(self.LP.lll_prob, self.LP.rll_prob, self.dp_lanelines_active)
+ else:
+ self.dp_lanelines_active = False
# Calculate final driving path and set MPC costs
- if self.dp_lanelines_enable and self.dp_lanelines_active:
+ if self.dp_lanelines_active:
self.d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, self.steer_rate_cost)
else:
@@ -139,7 +142,7 @@ class LateralPlanner:
lateralPlan.solverExecutionTime = self.lat_mpc.solve_time
lateralPlan.desire = self.DH.desire
- lateralPlan.useLaneLines = self.dp_lanelines_enable and self.dp_lanelines_active
+ lateralPlan.useLaneLines = self.dp_lanelines_active
lateralPlan.laneChangeState = self.DH.lane_change_state
lateralPlan.laneChangeDirection = self.DH.lane_change_direction
diff --git a/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/acados_ocp_lat.json b/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/acados_ocp_lat.json
new file mode 100644
index 000000000..f7b5bb7f8
--- /dev/null
+++ b/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/acados_ocp_lat.json
@@ -0,0 +1,450 @@
+{
+ "acados_include_path": "/data/openpilot/third_party/acados/include/acados/include",
+ "acados_lib_path": "/data/openpilot/third_party/acados/include/acados/lib",
+ "code_export_directory": "/data/openpilot/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code",
+ "constraints": {
+ "C": [],
+ "C_e": [],
+ "D": [],
+ "constr_type": "BGH",
+ "constr_type_e": "BGH",
+ "idxbu": [],
+ "idxbx": [
+ 2,
+ 3
+ ],
+ "idxbx_0": [
+ 0,
+ 1,
+ 2,
+ 3
+ ],
+ "idxbx_e": [],
+ "idxbxe_0": [
+ 0,
+ 1,
+ 2,
+ 3
+ ],
+ "idxsbu": [],
+ "idxsbx": [],
+ "idxsbx_e": [],
+ "idxsg": [],
+ "idxsg_e": [],
+ "idxsh": [],
+ "idxsh_e": [],
+ "idxsphi": [],
+ "idxsphi_e": [],
+ "lbu": [],
+ "lbx": [
+ -1.5707963267948966,
+ -0.8726646259971648
+ ],
+ "lbx_0": [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ "lbx_e": [],
+ "lg": [],
+ "lg_e": [],
+ "lh": [],
+ "lh_e": [],
+ "lphi": [],
+ "lphi_e": [],
+ "lsbu": [],
+ "lsbx": [],
+ "lsbx_e": [],
+ "lsg": [],
+ "lsg_e": [],
+ "lsh": [],
+ "lsh_e": [],
+ "lsphi": [],
+ "lsphi_e": [],
+ "ubu": [],
+ "ubx": [
+ 1.5707963267948966,
+ 0.8726646259971648
+ ],
+ "ubx_0": [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ "ubx_e": [],
+ "ug": [],
+ "ug_e": [],
+ "uh": [],
+ "uh_e": [],
+ "uphi": [],
+ "uphi_e": [],
+ "usbu": [],
+ "usbx": [],
+ "usbx_e": [],
+ "usg": [],
+ "usg_e": [],
+ "ush": [],
+ "ush_e": [],
+ "usphi": [],
+ "usphi_e": []
+ },
+ "cost": {
+ "Vu": [
+ [
+ 0.0
+ ],
+ [
+ 0.0
+ ],
+ [
+ 0.0
+ ]
+ ],
+ "Vu_0": [
+ [
+ 0.0
+ ],
+ [
+ 0.0
+ ],
+ [
+ 0.0
+ ]
+ ],
+ "Vx": [
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ]
+ ],
+ "Vx_0": [
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ]
+ ],
+ "Vx_e": [
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ]
+ ],
+ "Vz": [],
+ "Vz_0": [],
+ "W": [
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ]
+ ],
+ "W_0": [
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ]
+ ],
+ "W_e": [
+ [
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0
+ ]
+ ],
+ "Zl": [],
+ "Zl_e": [],
+ "Zu": [],
+ "Zu_e": [],
+ "cost_ext_fun_type": "casadi",
+ "cost_ext_fun_type_0": "casadi",
+ "cost_ext_fun_type_e": "casadi",
+ "cost_type": "NONLINEAR_LS",
+ "cost_type_0": "NONLINEAR_LS",
+ "cost_type_e": "NONLINEAR_LS",
+ "yref": [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ "yref_0": [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ "yref_e": [
+ 0.0,
+ 0.0
+ ],
+ "zl": [],
+ "zl_e": [],
+ "zu": [],
+ "zu_e": []
+ },
+ "cython_include_dirs": "/data/data/com.termux/files/usr/lib/python3.8/site-packages/numpy/core/include",
+ "dims": {
+ "N": 16,
+ "nbu": 0,
+ "nbx": 2,
+ "nbx_0": 4,
+ "nbx_e": 0,
+ "nbxe_0": 4,
+ "ng": 0,
+ "ng_e": 0,
+ "nh": 0,
+ "nh_e": 0,
+ "np": 2,
+ "nphi": 0,
+ "nphi_e": 0,
+ "nr": 0,
+ "nr_e": 0,
+ "ns": 0,
+ "ns_e": 0,
+ "nsbu": 0,
+ "nsbx": 0,
+ "nsbx_e": 0,
+ "nsg": 0,
+ "nsg_e": 0,
+ "nsh": 0,
+ "nsh_e": 0,
+ "nsphi": 0,
+ "nsphi_e": 0,
+ "nu": 1,
+ "nx": 4,
+ "ny": 3,
+ "ny_0": 3,
+ "ny_e": 2,
+ "nz": 0
+ },
+ "model": {
+ "dyn_disc_fun": null,
+ "dyn_disc_fun_jac": null,
+ "dyn_disc_fun_jac_hess": null,
+ "dyn_ext_fun_type": "casadi",
+ "dyn_source_discrete": null,
+ "gnsf": {
+ "nontrivial_f_LO": 1,
+ "purely_linear": 0
+ },
+ "name": "lat"
+ },
+ "parameter_values": [
+ 0.0,
+ 0.0
+ ],
+ "problem_class": "OCP",
+ "simulink_opts": {
+ "inputs": {
+ "cost_W": 0,
+ "cost_W_0": 0,
+ "cost_W_e": 0,
+ "lbu": 1,
+ "lbx": 1,
+ "lbx_0": 1,
+ "lbx_e": 1,
+ "lg": 1,
+ "lh": 1,
+ "parameter_traj": 1,
+ "reset_solver": 0,
+ "u_init": 0,
+ "ubu": 1,
+ "ubx": 1,
+ "ubx_0": 1,
+ "ubx_e": 1,
+ "ug": 1,
+ "uh": 1,
+ "x_init": 0,
+ "y_ref": 1,
+ "y_ref_0": 1,
+ "y_ref_e": 1
+ },
+ "outputs": {
+ "CPU_time": 1,
+ "CPU_time_lin": 0,
+ "CPU_time_qp": 0,
+ "CPU_time_sim": 0,
+ "KKT_residual": 1,
+ "solver_status": 1,
+ "sqp_iter": 1,
+ "u0": 1,
+ "utraj": 0,
+ "x1": 1,
+ "xtraj": 0
+ },
+ "samplingtime": "t0"
+ },
+ "solver_options": {
+ "Tsim": 0.009765625,
+ "alpha_min": 0.05,
+ "alpha_reduction": 0.7,
+ "collocation_type": "GAUSS_LEGENDRE",
+ "eps_sufficient_descent": 0.0001,
+ "exact_hess_constr": 1,
+ "exact_hess_cost": 1,
+ "exact_hess_dyn": 1,
+ "ext_cost_num_hess": 0,
+ "full_step_dual": 0,
+ "globalization": "FIXED_STEP",
+ "globalization_use_SOC": 0,
+ "hessian_approx": "GAUSS_NEWTON",
+ "hpipm_mode": "BALANCE",
+ "initialize_t_slacks": 0,
+ "integrator_type": "ERK",
+ "levenberg_marquardt": 0.0,
+ "line_search_use_sufficient_descent": 0,
+ "model_external_shared_lib_dir": null,
+ "model_external_shared_lib_name": null,
+ "nlp_solver_max_iter": 100,
+ "nlp_solver_step_length": 1.0,
+ "nlp_solver_tol_comp": 1e-06,
+ "nlp_solver_tol_eq": 1e-06,
+ "nlp_solver_tol_ineq": 1e-06,
+ "nlp_solver_tol_stat": 1e-06,
+ "nlp_solver_type": "SQP_RTI",
+ "print_level": 0,
+ "qp_solver": "PARTIAL_CONDENSING_HPIPM",
+ "qp_solver_cond_N": 1,
+ "qp_solver_iter_max": 1,
+ "qp_solver_tol_comp": null,
+ "qp_solver_tol_eq": null,
+ "qp_solver_tol_ineq": null,
+ "qp_solver_tol_stat": null,
+ "qp_solver_warm_start": 0,
+ "regularize_method": null,
+ "sim_method_jac_reuse": [
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0
+ ],
+ "sim_method_newton_iter": 3,
+ "sim_method_num_stages": [
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4
+ ],
+ "sim_method_num_steps": [
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1
+ ],
+ "tf": 2.5,
+ "time_steps": [
+ 0.009765625,
+ 0.029296875,
+ 0.048828125,
+ 0.068359375,
+ 0.087890625,
+ 0.107421875,
+ 0.126953125,
+ 0.146484375,
+ 0.166015625,
+ 0.185546875,
+ 0.205078125,
+ 0.224609375,
+ 0.244140625,
+ 0.263671875,
+ 0.283203125,
+ 0.302734375
+ ]
+ }
+}
\ No newline at end of file
diff --git a/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c b/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c
new file mode 100644
index 000000000..d0c541d3b
--- /dev/null
+++ b/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c
@@ -0,0 +1,278 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+// standard
+#include
+#include
+
+// acados
+#include "acados_c/external_function_interface.h"
+#include "acados_c/sim_interface.h"
+#include "acados_c/external_function_interface.h"
+
+#include "acados/sim/sim_common.h"
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/print.h"
+
+
+// example specific
+#include "lat_model/lat_model.h"
+#include "acados_sim_solver_lat.h"
+
+
+// ** solver data **
+
+sim_solver_capsule * lat_acados_sim_solver_create_capsule()
+{
+ void* capsule_mem = malloc(sizeof(sim_solver_capsule));
+ sim_solver_capsule *capsule = (sim_solver_capsule *) capsule_mem;
+
+ return capsule;
+}
+
+
+int lat_acados_sim_solver_free_capsule(sim_solver_capsule * capsule)
+{
+ free(capsule);
+ return 0;
+}
+
+
+int lat_acados_sim_create(sim_solver_capsule * capsule)
+{
+ // initialize
+ const int nx = LAT_NX;
+ const int nu = LAT_NU;
+ const int nz = LAT_NZ;
+ const int np = LAT_NP;
+ bool tmp_bool;
+
+
+ double Tsim = 0.009765625;
+
+
+ // explicit ode
+ capsule->sim_forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+ capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+
+ capsule->sim_forw_vde_casadi->casadi_fun = &lat_expl_vde_forw;
+ capsule->sim_forw_vde_casadi->casadi_n_in = &lat_expl_vde_forw_n_in;
+ capsule->sim_forw_vde_casadi->casadi_n_out = &lat_expl_vde_forw_n_out;
+ capsule->sim_forw_vde_casadi->casadi_sparsity_in = &lat_expl_vde_forw_sparsity_in;
+ capsule->sim_forw_vde_casadi->casadi_sparsity_out = &lat_expl_vde_forw_sparsity_out;
+ capsule->sim_forw_vde_casadi->casadi_work = &lat_expl_vde_forw_work;
+ external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np);
+
+ capsule->sim_expl_ode_fun_casadi->casadi_fun = &lat_expl_ode_fun;
+ capsule->sim_expl_ode_fun_casadi->casadi_n_in = &lat_expl_ode_fun_n_in;
+ capsule->sim_expl_ode_fun_casadi->casadi_n_out = &lat_expl_ode_fun_n_out;
+ capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &lat_expl_ode_fun_sparsity_in;
+ capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &lat_expl_ode_fun_sparsity_out;
+ capsule->sim_expl_ode_fun_casadi->casadi_work = &lat_expl_ode_fun_work;
+ external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, np);
+
+
+
+ // sim plan & config
+ sim_solver_plan_t plan;
+ plan.sim_solver = ERK;
+
+ // create correct config based on plan
+ sim_config * lat_sim_config = sim_config_create(plan);
+ capsule->acados_sim_config = lat_sim_config;
+
+ // sim dims
+ void *lat_sim_dims = sim_dims_create(lat_sim_config);
+ capsule->acados_sim_dims = lat_sim_dims;
+ sim_dims_set(lat_sim_config, lat_sim_dims, "nx", &nx);
+ sim_dims_set(lat_sim_config, lat_sim_dims, "nu", &nu);
+ sim_dims_set(lat_sim_config, lat_sim_dims, "nz", &nz);
+
+
+ // sim opts
+ sim_opts *lat_sim_opts = sim_opts_create(lat_sim_config, lat_sim_dims);
+ capsule->acados_sim_opts = lat_sim_opts;
+ int tmp_int = 3;
+ sim_opts_set(lat_sim_config, lat_sim_opts, "newton_iter", &tmp_int);
+ sim_collocation_type collocation_type = GAUSS_LEGENDRE;
+ sim_opts_set(lat_sim_config, lat_sim_opts, "collocation_type", &collocation_type);
+
+
+ tmp_int = 4;
+ sim_opts_set(lat_sim_config, lat_sim_opts, "num_stages", &tmp_int);
+ tmp_int = 1;
+ sim_opts_set(lat_sim_config, lat_sim_opts, "num_steps", &tmp_int);
+ tmp_bool = 0;
+ sim_opts_set(lat_sim_config, lat_sim_opts, "jac_reuse", &tmp_bool);
+
+
+ // sim in / out
+ sim_in *lat_sim_in = sim_in_create(lat_sim_config, lat_sim_dims);
+ capsule->acados_sim_in = lat_sim_in;
+ sim_out *lat_sim_out = sim_out_create(lat_sim_config, lat_sim_dims);
+ capsule->acados_sim_out = lat_sim_out;
+
+ sim_in_set(lat_sim_config, lat_sim_dims,
+ lat_sim_in, "T", &Tsim);
+
+ // model functions
+ lat_sim_config->model_set(lat_sim_in->model,
+ "expl_vde_for", capsule->sim_forw_vde_casadi);
+ lat_sim_config->model_set(lat_sim_in->model,
+ "expl_ode_fun", capsule->sim_expl_ode_fun_casadi);
+
+ // sim solver
+ sim_solver *lat_sim_solver = sim_solver_create(lat_sim_config,
+ lat_sim_dims, lat_sim_opts);
+ capsule->acados_sim_solver = lat_sim_solver;
+
+
+ /* initialize parameter values */
+ double* p = calloc(np, sizeof(double));
+
+
+ lat_acados_sim_update_params(capsule, p, np);
+ free(p);
+
+
+ /* initialize input */
+ // x
+ double x0[4];
+ for (int ii = 0; ii < 4; ii++)
+ x0[ii] = 0.0;
+
+ sim_in_set(lat_sim_config, lat_sim_dims,
+ lat_sim_in, "x", x0);
+
+
+ // u
+ double u0[1];
+ for (int ii = 0; ii < 1; ii++)
+ u0[ii] = 0.0;
+
+ sim_in_set(lat_sim_config, lat_sim_dims,
+ lat_sim_in, "u", u0);
+
+ // S_forw
+ double S_forw[20];
+ for (int ii = 0; ii < 20; ii++)
+ S_forw[ii] = 0.0;
+ for (int ii = 0; ii < 4; ii++)
+ S_forw[ii + ii * 4 ] = 1.0;
+
+
+ sim_in_set(lat_sim_config, lat_sim_dims,
+ lat_sim_in, "S_forw", S_forw);
+
+ int status = sim_precompute(lat_sim_solver, lat_sim_in, lat_sim_out);
+
+ return status;
+}
+
+
+int lat_acados_sim_solve(sim_solver_capsule *capsule)
+{
+ // integrate dynamics using acados sim_solver
+ int status = sim_solve(capsule->acados_sim_solver,
+ capsule->acados_sim_in, capsule->acados_sim_out);
+ if (status != 0)
+ printf("error in lat_acados_sim_solve()! Exiting.\n");
+
+ return status;
+}
+
+
+int lat_acados_sim_free(sim_solver_capsule *capsule)
+{
+ // free memory
+ sim_solver_destroy(capsule->acados_sim_solver);
+ sim_in_destroy(capsule->acados_sim_in);
+ sim_out_destroy(capsule->acados_sim_out);
+ sim_opts_destroy(capsule->acados_sim_opts);
+ sim_dims_destroy(capsule->acados_sim_dims);
+ sim_config_destroy(capsule->acados_sim_config);
+
+ // free external function
+ external_function_param_casadi_free(capsule->sim_forw_vde_casadi);
+ external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi);
+
+ return 0;
+}
+
+
+int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np)
+{
+ int status = 0;
+ int casadi_np = LAT_NP;
+
+ if (casadi_np != np) {
+ printf("lat_acados_sim_update_params: trying to set %i parameters for external functions."
+ " External function has %i parameters. Exiting.\n", np, casadi_np);
+ exit(1);
+ }
+ capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p);
+ capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p);
+
+ return status;
+}
+
+/* getters pointers to C objects*/
+sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule)
+{
+ return capsule->acados_sim_config;
+};
+
+sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule)
+{
+ return capsule->acados_sim_in;
+};
+
+sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule)
+{
+ return capsule->acados_sim_out;
+};
+
+void * lat_acados_get_sim_dims(sim_solver_capsule *capsule)
+{
+ return capsule->acados_sim_dims;
+};
+
+sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule)
+{
+ return capsule->acados_sim_opts;
+};
+
+sim_solver * lat_acados_get_sim_solver(sim_solver_capsule *capsule)
+{
+ return capsule->acados_sim_solver;
+};
+
diff --git a/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h b/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h
new file mode 100644
index 000000000..86b9c84c9
--- /dev/null
+++ b/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h
@@ -0,0 +1,103 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+#ifndef ACADOS_SIM_lat_H_
+#define ACADOS_SIM_lat_H_
+
+#include "acados_c/sim_interface.h"
+#include "acados_c/external_function_interface.h"
+
+#define LAT_NX 4
+#define LAT_NZ 0
+#define LAT_NU 1
+#define LAT_NP 2
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+// ** capsule for solver data **
+typedef struct sim_solver_capsule
+{
+ // acados objects
+ sim_in *acados_sim_in;
+ sim_out *acados_sim_out;
+ sim_solver *acados_sim_solver;
+ sim_opts *acados_sim_opts;
+ sim_config *acados_sim_config;
+ void *acados_sim_dims;
+
+ /* external functions */
+ // ERK
+ external_function_param_casadi * sim_forw_vde_casadi;
+ external_function_param_casadi * sim_expl_ode_fun_casadi;
+ external_function_param_casadi * sim_expl_ode_hess;
+
+ // IRK
+ external_function_param_casadi * sim_impl_dae_fun;
+ external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z;
+ external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z;
+ external_function_param_casadi * sim_impl_dae_hess;
+
+ // GNSF
+ external_function_param_casadi * sim_gnsf_phi_fun;
+ external_function_param_casadi * sim_gnsf_phi_fun_jac_y;
+ external_function_param_casadi * sim_gnsf_phi_jac_y_uhat;
+ external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z;
+ external_function_param_casadi * sim_gnsf_get_matrices_fun;
+
+} sim_solver_capsule;
+
+
+ACADOS_SYMBOL_EXPORT int lat_acados_sim_create(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT int lat_acados_sim_solve(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT int lat_acados_sim_free(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *value, int np);
+
+ACADOS_SYMBOL_EXPORT sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT void * lat_acados_get_sim_dims(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT sim_solver * lat_acados_get_sim_solver(sim_solver_capsule *capsule);
+
+
+ACADOS_SYMBOL_EXPORT sim_solver_capsule * lat_acados_sim_solver_create_capsule(void);
+ACADOS_SYMBOL_EXPORT int lat_acados_sim_solver_free_capsule(sim_solver_capsule *capsule);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // ACADOS_SIM_lat_H_
diff --git a/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c b/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c
new file mode 100644
index 000000000..827b6c7f0
--- /dev/null
+++ b/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c
@@ -0,0 +1,264 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+#define S_FUNCTION_NAME acados_solver_sfunction_lat
+#define S_FUNCTION_LEVEL 2
+
+#define MDL_START
+
+// acados
+// #include "acados/utils/print.h"
+#include "acados_c/sim_interface.h"
+#include "acados_c/external_function_interface.h"
+
+// example specific
+#include "lat_model/lat_model.h"
+#include "acados_solver_lat.h"
+
+#include "simstruc.h"
+
+#define SAMPLINGTIME 0.009765625
+
+static void mdlInitializeSizes (SimStruct *S)
+{
+ // specify the number of continuous and discrete states
+ ssSetNumContStates(S, 0);
+ ssSetNumDiscStates(S, 0);// specify the number of input ports
+ if ( !ssSetNumInputPorts(S, 8) )
+ return;
+
+ // specify the number of output ports
+ if ( !ssSetNumOutputPorts(S, 6) )
+ return;
+
+ // specify dimension information for the input ports
+ // lbx_0
+ ssSetInputPortVectorDimension(S, 0, 4);
+ // ubx_0
+ ssSetInputPortVectorDimension(S, 1, 4);
+ // parameters
+ ssSetInputPortVectorDimension(S, 2, (16+1) * 2);
+ // y_ref_0
+ ssSetInputPortVectorDimension(S, 3, 3);
+ // y_ref
+ ssSetInputPortVectorDimension(S, 4, 45);
+ // y_ref_e
+ ssSetInputPortVectorDimension(S, 5, 2);
+ // lbx
+ ssSetInputPortVectorDimension(S, 6, 30);
+ // ubx
+ ssSetInputPortVectorDimension(S, 7, 30);/* specify dimension information for the OUTPUT ports */
+ ssSetOutputPortVectorDimension(S, 0, 1 );
+ ssSetOutputPortVectorDimension(S, 1, 1 );
+ ssSetOutputPortVectorDimension(S, 2, 1 );
+ ssSetOutputPortVectorDimension(S, 3, 4 ); // state at shooting node 1
+ ssSetOutputPortVectorDimension(S, 4, 1);
+ ssSetOutputPortVectorDimension(S, 5, 1 );
+
+ // specify the direct feedthrough status
+ // should be set to 1 for all inputs used in mdlOutputs
+ ssSetInputPortDirectFeedThrough(S, 0, 1);
+ ssSetInputPortDirectFeedThrough(S, 1, 1);
+ ssSetInputPortDirectFeedThrough(S, 2, 1);
+ ssSetInputPortDirectFeedThrough(S, 3, 1);
+ ssSetInputPortDirectFeedThrough(S, 4, 1);
+ ssSetInputPortDirectFeedThrough(S, 5, 1);
+ ssSetInputPortDirectFeedThrough(S, 6, 1);
+ ssSetInputPortDirectFeedThrough(S, 7, 1);
+
+ // one sample time
+ ssSetNumSampleTimes(S, 1);
+}
+
+
+#if defined(MATLAB_MEX_FILE)
+
+#define MDL_SET_INPUT_PORT_DIMENSION_INFO
+#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
+
+static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
+{
+ if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
+ return;
+}
+
+static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
+{
+ if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
+ return;
+}
+
+#endif /* MATLAB_MEX_FILE */
+
+
+static void mdlInitializeSampleTimes(SimStruct *S)
+{
+ ssSetSampleTime(S, 0, SAMPLINGTIME);
+ ssSetOffsetTime(S, 0, 0.0);
+}
+
+
+static void mdlStart(SimStruct *S)
+{
+ lat_solver_capsule *capsule = lat_acados_create_capsule();
+ lat_acados_create(capsule);
+
+ ssSetUserData(S, (void*)capsule);
+}
+
+
+static void mdlOutputs(SimStruct *S, int_T tid)
+{
+ lat_solver_capsule *capsule = ssGetUserData(S);
+ ocp_nlp_config *nlp_config = lat_acados_get_nlp_config(capsule);
+ ocp_nlp_dims *nlp_dims = lat_acados_get_nlp_dims(capsule);
+ ocp_nlp_in *nlp_in = lat_acados_get_nlp_in(capsule);
+ ocp_nlp_out *nlp_out = lat_acados_get_nlp_out(capsule);
+
+ InputRealPtrsType in_sign;
+
+ // local buffer
+ real_t buffer[4];
+
+ /* go through inputs */
+ // lbx_0
+ in_sign = ssGetInputPortRealSignalPtrs(S, 0);
+ for (int i = 0; i < 4; i++)
+ buffer[i] = (double)(*in_sign[i]);
+
+ ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", buffer);
+ // ubx_0
+ in_sign = ssGetInputPortRealSignalPtrs(S, 1);
+ for (int i = 0; i < 4; i++)
+ buffer[i] = (double)(*in_sign[i]);
+ ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", buffer);
+ // parameters - stage-variant !!!
+ in_sign = ssGetInputPortRealSignalPtrs(S, 2);
+
+ // update value of parameters
+ for (int ii = 0; ii <= 16; ii++)
+ {
+ for (int jj = 0; jj < 2; jj++)
+ buffer[jj] = (double)(*in_sign[ii*2+jj]);
+ lat_acados_update_params(capsule, ii, buffer, 2);
+ }
+
+
+ // y_ref_0
+ in_sign = ssGetInputPortRealSignalPtrs(S, 3);
+
+ for (int i = 0; i < 3; i++)
+ buffer[i] = (double)(*in_sign[i]);
+
+ ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", (void *) buffer);
+
+
+ // y_ref - for stages 1 to N-1
+ in_sign = ssGetInputPortRealSignalPtrs(S, 4);
+
+ for (int ii = 1; ii < 16; ii++)
+ {
+ for (int jj = 0; jj < 3; jj++)
+ buffer[jj] = (double)(*in_sign[(ii-1)*3+jj]);
+ ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "yref", (void *) buffer);
+ }
+
+
+ // y_ref_e
+ in_sign = ssGetInputPortRealSignalPtrs(S, 5);
+
+ for (int i = 0; i < 2; i++)
+ buffer[i] = (double)(*in_sign[i]);
+
+ ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 16, "yref", (void *) buffer);
+ // lbx
+ in_sign = ssGetInputPortRealSignalPtrs(S, 6);
+ for (int ii = 1; ii < 16; ii++)
+ {
+ for (int jj = 0; jj < 2; jj++)
+ buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]);
+ ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbx", (void *) buffer);
+ }
+ // ubx
+ in_sign = ssGetInputPortRealSignalPtrs(S, 7);
+ for (int ii = 1; ii < 16; ii++)
+ {
+ for (int jj = 0; jj < 2; jj++)
+ buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]);
+ ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubx", (void *) buffer);
+ }
+
+ /* call solver */
+ int rti_phase = 0;
+ ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "rti_phase", &rti_phase);
+ int acados_status = lat_acados_solve(capsule);
+
+
+ /* set outputs */
+ // assign pointers to output signals
+ real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin;
+ int tmp_int;
+ out_u0 = ssGetOutputPortRealSignal(S, 0);
+ ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", (void *) out_u0);
+
+
+ out_status = ssGetOutputPortRealSignal(S, 1);
+ *out_status = (real_t) acados_status;
+ out_KKT_res = ssGetOutputPortRealSignal(S, 2);
+ *out_KKT_res = (real_t) nlp_out->inf_norm_res;
+ out_x1 = ssGetOutputPortRealSignal(S, 3);
+ ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 1, "x", (void *) out_x1);
+ out_cpu_time = ssGetOutputPortRealSignal(S, 4);
+ // get solution time
+ ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_tot", (void *) out_cpu_time);
+ out_sqp_iter = ssGetOutputPortRealSignal(S, 5);
+ // get sqp iter
+ ocp_nlp_get(nlp_config, capsule->nlp_solver, "sqp_iter", (void *) &tmp_int);
+ *out_sqp_iter = (real_t) tmp_int;
+
+}
+
+static void mdlTerminate(SimStruct *S)
+{
+ lat_solver_capsule *capsule = ssGetUserData(S);
+
+ lat_acados_free(capsule);
+ lat_acados_free_capsule(capsule);
+}
+
+
+#ifdef MATLAB_MEX_FILE
+#include "simulink.c"
+#else
+#include "cg_sfun.h"
+#endif
diff --git a/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c b/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c
new file mode 100644
index 000000000..c536654fd
--- /dev/null
+++ b/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c
@@ -0,0 +1,128 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+// standard
+#include
+#include
+// acados
+#include "acados/utils/print.h"
+#include "acados/utils/math.h"
+#include "acados_c/sim_interface.h"
+#include "acados_sim_solver_lat.h"
+
+#define NX LAT_NX
+#define NZ LAT_NZ
+#define NU LAT_NU
+#define NP LAT_NP
+
+
+int main()
+{
+ int status = 0;
+ sim_solver_capsule *capsule = lat_acados_sim_solver_create_capsule();
+ status = lat_acados_sim_create(capsule);
+
+ if (status)
+ {
+ printf("acados_create() returned status %d. Exiting.\n", status);
+ exit(1);
+ }
+
+ sim_config *acados_sim_config = lat_acados_get_sim_config(capsule);
+ sim_in *acados_sim_in = lat_acados_get_sim_in(capsule);
+ sim_out *acados_sim_out = lat_acados_get_sim_out(capsule);
+ void *acados_sim_dims = lat_acados_get_sim_dims(capsule);
+
+ // initial condition
+ double x_current[NX];
+ x_current[0] = 0.0;
+ x_current[1] = 0.0;
+ x_current[2] = 0.0;
+ x_current[3] = 0.0;
+
+
+ x_current[0] = 0;
+ x_current[1] = 0;
+ x_current[2] = 0;
+ x_current[3] = 0;
+
+
+
+
+ // initial value for control input
+ double u0[NU];
+ u0[0] = 0.0;
+ // set parameters
+ double p[NP];
+ p[0] = 0;
+ p[1] = 0;
+
+ lat_acados_sim_update_params(capsule, p, NP);
+
+
+ int n_sim_steps = 3;
+ // solve ocp in loop
+ for (int ii = 0; ii < n_sim_steps; ii++)
+ {
+ sim_in_set(acados_sim_config, acados_sim_dims,
+ acados_sim_in, "x", x_current);
+ status = lat_acados_sim_solve(capsule);
+
+ if (status != ACADOS_SUCCESS)
+ {
+ printf("acados_solve() failed with status %d.\n", status);
+ }
+
+ sim_out_get(acados_sim_config, acados_sim_dims,
+ acados_sim_out, "x", x_current);
+
+ printf("\nx_current, %d\n", ii);
+ for (int jj = 0; jj < NX; jj++)
+ {
+ printf("%e\n", x_current[jj]);
+ }
+ }
+
+ printf("\nPerformed %d simulation steps with acados integrator successfully.\n\n", n_sim_steps);
+
+ // free solver
+ status = lat_acados_sim_free(capsule);
+ if (status) {
+ printf("lat_acados_sim_free() returned status %d. \n", status);
+ }
+
+ lat_acados_sim_solver_free_capsule(capsule);
+
+ return status;
+}
diff --git a/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m b/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m
new file mode 100644
index 000000000..2c6679ecb
--- /dev/null
+++ b/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m
@@ -0,0 +1,125 @@
+%
+% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+%
+% This file is part of acados.
+%
+% The 2-Clause BSD License
+%
+% Redistribution and use in source and binary forms, with or without
+% modification, are permitted provided that the following conditions are met:
+%
+% 1. Redistributions of source code must retain the above copyright notice,
+% this list of conditions and the following disclaimer.
+%
+% 2. Redistributions in binary form must reproduce the above copyright notice,
+% this list of conditions and the following disclaimer in the documentation
+% and/or other materials provided with the distribution.
+%
+% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+% POSSIBILITY OF SUCH DAMAGE.;
+%
+
+SOURCES = { ...
+ 'lat_model/lat_expl_ode_fun.c', ...
+ 'lat_model/lat_expl_vde_forw.c',...
+ 'lat_cost/lat_cost_y_0_fun.c',...
+ 'lat_cost/lat_cost_y_0_fun_jac_ut_xt.c',...
+ 'lat_cost/lat_cost_y_0_hess.c',...
+ 'lat_cost/lat_cost_y_fun.c',...
+ 'lat_cost/lat_cost_y_fun_jac_ut_xt.c',...
+ 'lat_cost/lat_cost_y_hess.c',...
+ 'lat_cost/lat_cost_y_e_fun.c',...
+ 'lat_cost/lat_cost_y_e_fun_jac_ut_xt.c',...
+ 'lat_cost/lat_cost_y_e_hess.c',...
+ 'acados_solver_sfunction_lat.c', ...
+ 'acados_solver_lat.c'
+ };
+
+INC_PATH = '/data/openpilot/third_party/acados/include/acados/include';
+
+INCS = {['-I', fullfile(INC_PATH, 'blasfeo', 'include')], ...
+ ['-I', fullfile(INC_PATH, 'hpipm', 'include')], ...
+ ['-I', fullfile(INC_PATH, 'acados')], ...
+ ['-I', fullfile(INC_PATH)]};
+
+
+
+CFLAGS = 'CFLAGS=$CFLAGS';
+LDFLAGS = 'LDFLAGS=$LDFLAGS';
+COMPFLAGS = 'COMPFLAGS=$COMPFLAGS';
+COMPDEFINES = 'COMPDEFINES=$COMPDEFINES';
+
+
+
+LIB_PATH = ['-L', fullfile('/data/openpilot/third_party/acados/include/acados/lib')];
+
+LIBS = {'-lacados', '-lhpipm', '-lblasfeo'};
+
+% acados linking libraries and flags
+
+
+mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ...
+ LIB_PATH, LIBS{:}, SOURCES{:}, ...
+ '-output', 'acados_solver_sfunction_lat' );
+
+fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_lat', '.', ...
+ eval('mexext')] );
+
+
+%% print note on usage of s-function
+fprintf('\n\nNote: Usage of Sfunction is as follows:\n')
+input_note = 'Inputs are:\n';
+i_in = 1;
+input_note = strcat(input_note, num2str(i_in), ') lbx_0 - lower bound on x for stage 0,',...
+ ' size [4]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',...
+ ' size [4]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all shooting nodes 0 to N+1,',...
+ ' size [34]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [3]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') y_ref - concatenated for shooting nodes 1 to N-1,',...
+ ' size [45]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [2]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') lbx for shooting nodes 1 to N-1, size [30]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') ubx for shooting nodes 1 to N-1, size [30]\n ');
+i_in = i_in + 1;
+
+fprintf(input_note)
+
+disp(' ')
+
+output_note = 'Outputs are:\n';
+i_out = 0;
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') u0, control input at node 0, size [1]\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') acados solver status (0 = SUCCESS)\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') KKT residual\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') x1, state at node 1\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') CPU time\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n ');
+
+fprintf(output_note)
diff --git a/selfdrive/dragonpilot/controls_0816/lib/lateral_planner.py b/selfdrive/dragonpilot/controls_0816/lib/lateral_planner.py
index 42dabf305..5ffd68139 100644
--- a/selfdrive/dragonpilot/controls_0816/lib/lateral_planner.py
+++ b/selfdrive/dragonpilot/controls_0816/lib/lateral_planner.py
@@ -77,10 +77,13 @@ class LateralPlanner:
# dynamic laneline/laneless logic
# decide if we want to use lanelines or laneless
- self.dp_lanelines_active = get_lane_laneless_mode(self.LP.lll_prob, self.LP.rll_prob, self.dp_lanelines_active)
+ if self.dp_lanelines_enable:
+ self.dp_lanelines_active = get_lane_laneless_mode(self.LP.lll_prob, self.LP.rll_prob, self.dp_lanelines_active)
+ else:
+ self.dp_lanelines_active = False
# Calculate final driving path and set MPC costs
- if self.dp_lanelines_enable and self.dp_lanelines_active:
+ if self.dp_lanelines_active:
self.d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, MPC_COST_LAT.STEER_RATE)
else:
@@ -141,7 +144,7 @@ class LateralPlanner:
lateralPlan.solverExecutionTime = self.lat_mpc.solve_time
lateralPlan.desire = self.DH.desire
- lateralPlan.useLaneLines = self.dp_lanelines_enable and self.dp_lanelines_active
+ lateralPlan.useLaneLines = self.dp_lanelines_active
lateralPlan.laneChangeState = self.DH.lane_change_state
lateralPlan.laneChangeDirection = self.DH.lane_change_direction
diff --git a/selfdrive/locationd/locationd b/selfdrive/locationd/locationd
index 9b1e24370..0ca8e5ee5 100755
Binary files a/selfdrive/locationd/locationd and b/selfdrive/locationd/locationd differ
diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp
index 65b861dc1..ad7efc0e0 100644
--- a/selfdrive/locationd/models/generated/car.cpp
+++ b/selfdrive/locationd/models/generated/car.cpp
@@ -45,326 +45,326 @@ const static double MAHA_THRESH_31 = 3.8414588206941227;
* *
* This file is part of 'ekf' *
******************************************************************************/
-void err_fun(double *nom_x, double *delta_x, double *out_5634330020296848070) {
- out_5634330020296848070[0] = delta_x[0] + nom_x[0];
- out_5634330020296848070[1] = delta_x[1] + nom_x[1];
- out_5634330020296848070[2] = delta_x[2] + nom_x[2];
- out_5634330020296848070[3] = delta_x[3] + nom_x[3];
- out_5634330020296848070[4] = delta_x[4] + nom_x[4];
- out_5634330020296848070[5] = delta_x[5] + nom_x[5];
- out_5634330020296848070[6] = delta_x[6] + nom_x[6];
- out_5634330020296848070[7] = delta_x[7] + nom_x[7];
- out_5634330020296848070[8] = delta_x[8] + nom_x[8];
+void err_fun(double *nom_x, double *delta_x, double *out_9008082079438066370) {
+ out_9008082079438066370[0] = delta_x[0] + nom_x[0];
+ out_9008082079438066370[1] = delta_x[1] + nom_x[1];
+ out_9008082079438066370[2] = delta_x[2] + nom_x[2];
+ out_9008082079438066370[3] = delta_x[3] + nom_x[3];
+ out_9008082079438066370[4] = delta_x[4] + nom_x[4];
+ out_9008082079438066370[5] = delta_x[5] + nom_x[5];
+ out_9008082079438066370[6] = delta_x[6] + nom_x[6];
+ out_9008082079438066370[7] = delta_x[7] + nom_x[7];
+ out_9008082079438066370[8] = delta_x[8] + nom_x[8];
}
-void inv_err_fun(double *nom_x, double *true_x, double *out_1061559027503315281) {
- out_1061559027503315281[0] = -nom_x[0] + true_x[0];
- out_1061559027503315281[1] = -nom_x[1] + true_x[1];
- out_1061559027503315281[2] = -nom_x[2] + true_x[2];
- out_1061559027503315281[3] = -nom_x[3] + true_x[3];
- out_1061559027503315281[4] = -nom_x[4] + true_x[4];
- out_1061559027503315281[5] = -nom_x[5] + true_x[5];
- out_1061559027503315281[6] = -nom_x[6] + true_x[6];
- out_1061559027503315281[7] = -nom_x[7] + true_x[7];
- out_1061559027503315281[8] = -nom_x[8] + true_x[8];
+void inv_err_fun(double *nom_x, double *true_x, double *out_7705390806256602072) {
+ out_7705390806256602072[0] = -nom_x[0] + true_x[0];
+ out_7705390806256602072[1] = -nom_x[1] + true_x[1];
+ out_7705390806256602072[2] = -nom_x[2] + true_x[2];
+ out_7705390806256602072[3] = -nom_x[3] + true_x[3];
+ out_7705390806256602072[4] = -nom_x[4] + true_x[4];
+ out_7705390806256602072[5] = -nom_x[5] + true_x[5];
+ out_7705390806256602072[6] = -nom_x[6] + true_x[6];
+ out_7705390806256602072[7] = -nom_x[7] + true_x[7];
+ out_7705390806256602072[8] = -nom_x[8] + true_x[8];
}
-void H_mod_fun(double *state, double *out_1567569310651922730) {
- out_1567569310651922730[0] = 1.0;
- out_1567569310651922730[1] = 0;
- out_1567569310651922730[2] = 0;
- out_1567569310651922730[3] = 0;
- out_1567569310651922730[4] = 0;
- out_1567569310651922730[5] = 0;
- out_1567569310651922730[6] = 0;
- out_1567569310651922730[7] = 0;
- out_1567569310651922730[8] = 0;
- out_1567569310651922730[9] = 0;
- out_1567569310651922730[10] = 1.0;
- out_1567569310651922730[11] = 0;
- out_1567569310651922730[12] = 0;
- out_1567569310651922730[13] = 0;
- out_1567569310651922730[14] = 0;
- out_1567569310651922730[15] = 0;
- out_1567569310651922730[16] = 0;
- out_1567569310651922730[17] = 0;
- out_1567569310651922730[18] = 0;
- out_1567569310651922730[19] = 0;
- out_1567569310651922730[20] = 1.0;
- out_1567569310651922730[21] = 0;
- out_1567569310651922730[22] = 0;
- out_1567569310651922730[23] = 0;
- out_1567569310651922730[24] = 0;
- out_1567569310651922730[25] = 0;
- out_1567569310651922730[26] = 0;
- out_1567569310651922730[27] = 0;
- out_1567569310651922730[28] = 0;
- out_1567569310651922730[29] = 0;
- out_1567569310651922730[30] = 1.0;
- out_1567569310651922730[31] = 0;
- out_1567569310651922730[32] = 0;
- out_1567569310651922730[33] = 0;
- out_1567569310651922730[34] = 0;
- out_1567569310651922730[35] = 0;
- out_1567569310651922730[36] = 0;
- out_1567569310651922730[37] = 0;
- out_1567569310651922730[38] = 0;
- out_1567569310651922730[39] = 0;
- out_1567569310651922730[40] = 1.0;
- out_1567569310651922730[41] = 0;
- out_1567569310651922730[42] = 0;
- out_1567569310651922730[43] = 0;
- out_1567569310651922730[44] = 0;
- out_1567569310651922730[45] = 0;
- out_1567569310651922730[46] = 0;
- out_1567569310651922730[47] = 0;
- out_1567569310651922730[48] = 0;
- out_1567569310651922730[49] = 0;
- out_1567569310651922730[50] = 1.0;
- out_1567569310651922730[51] = 0;
- out_1567569310651922730[52] = 0;
- out_1567569310651922730[53] = 0;
- out_1567569310651922730[54] = 0;
- out_1567569310651922730[55] = 0;
- out_1567569310651922730[56] = 0;
- out_1567569310651922730[57] = 0;
- out_1567569310651922730[58] = 0;
- out_1567569310651922730[59] = 0;
- out_1567569310651922730[60] = 1.0;
- out_1567569310651922730[61] = 0;
- out_1567569310651922730[62] = 0;
- out_1567569310651922730[63] = 0;
- out_1567569310651922730[64] = 0;
- out_1567569310651922730[65] = 0;
- out_1567569310651922730[66] = 0;
- out_1567569310651922730[67] = 0;
- out_1567569310651922730[68] = 0;
- out_1567569310651922730[69] = 0;
- out_1567569310651922730[70] = 1.0;
- out_1567569310651922730[71] = 0;
- out_1567569310651922730[72] = 0;
- out_1567569310651922730[73] = 0;
- out_1567569310651922730[74] = 0;
- out_1567569310651922730[75] = 0;
- out_1567569310651922730[76] = 0;
- out_1567569310651922730[77] = 0;
- out_1567569310651922730[78] = 0;
- out_1567569310651922730[79] = 0;
- out_1567569310651922730[80] = 1.0;
+void H_mod_fun(double *state, double *out_4044238657628669408) {
+ out_4044238657628669408[0] = 1.0;
+ out_4044238657628669408[1] = 0;
+ out_4044238657628669408[2] = 0;
+ out_4044238657628669408[3] = 0;
+ out_4044238657628669408[4] = 0;
+ out_4044238657628669408[5] = 0;
+ out_4044238657628669408[6] = 0;
+ out_4044238657628669408[7] = 0;
+ out_4044238657628669408[8] = 0;
+ out_4044238657628669408[9] = 0;
+ out_4044238657628669408[10] = 1.0;
+ out_4044238657628669408[11] = 0;
+ out_4044238657628669408[12] = 0;
+ out_4044238657628669408[13] = 0;
+ out_4044238657628669408[14] = 0;
+ out_4044238657628669408[15] = 0;
+ out_4044238657628669408[16] = 0;
+ out_4044238657628669408[17] = 0;
+ out_4044238657628669408[18] = 0;
+ out_4044238657628669408[19] = 0;
+ out_4044238657628669408[20] = 1.0;
+ out_4044238657628669408[21] = 0;
+ out_4044238657628669408[22] = 0;
+ out_4044238657628669408[23] = 0;
+ out_4044238657628669408[24] = 0;
+ out_4044238657628669408[25] = 0;
+ out_4044238657628669408[26] = 0;
+ out_4044238657628669408[27] = 0;
+ out_4044238657628669408[28] = 0;
+ out_4044238657628669408[29] = 0;
+ out_4044238657628669408[30] = 1.0;
+ out_4044238657628669408[31] = 0;
+ out_4044238657628669408[32] = 0;
+ out_4044238657628669408[33] = 0;
+ out_4044238657628669408[34] = 0;
+ out_4044238657628669408[35] = 0;
+ out_4044238657628669408[36] = 0;
+ out_4044238657628669408[37] = 0;
+ out_4044238657628669408[38] = 0;
+ out_4044238657628669408[39] = 0;
+ out_4044238657628669408[40] = 1.0;
+ out_4044238657628669408[41] = 0;
+ out_4044238657628669408[42] = 0;
+ out_4044238657628669408[43] = 0;
+ out_4044238657628669408[44] = 0;
+ out_4044238657628669408[45] = 0;
+ out_4044238657628669408[46] = 0;
+ out_4044238657628669408[47] = 0;
+ out_4044238657628669408[48] = 0;
+ out_4044238657628669408[49] = 0;
+ out_4044238657628669408[50] = 1.0;
+ out_4044238657628669408[51] = 0;
+ out_4044238657628669408[52] = 0;
+ out_4044238657628669408[53] = 0;
+ out_4044238657628669408[54] = 0;
+ out_4044238657628669408[55] = 0;
+ out_4044238657628669408[56] = 0;
+ out_4044238657628669408[57] = 0;
+ out_4044238657628669408[58] = 0;
+ out_4044238657628669408[59] = 0;
+ out_4044238657628669408[60] = 1.0;
+ out_4044238657628669408[61] = 0;
+ out_4044238657628669408[62] = 0;
+ out_4044238657628669408[63] = 0;
+ out_4044238657628669408[64] = 0;
+ out_4044238657628669408[65] = 0;
+ out_4044238657628669408[66] = 0;
+ out_4044238657628669408[67] = 0;
+ out_4044238657628669408[68] = 0;
+ out_4044238657628669408[69] = 0;
+ out_4044238657628669408[70] = 1.0;
+ out_4044238657628669408[71] = 0;
+ out_4044238657628669408[72] = 0;
+ out_4044238657628669408[73] = 0;
+ out_4044238657628669408[74] = 0;
+ out_4044238657628669408[75] = 0;
+ out_4044238657628669408[76] = 0;
+ out_4044238657628669408[77] = 0;
+ out_4044238657628669408[78] = 0;
+ out_4044238657628669408[79] = 0;
+ out_4044238657628669408[80] = 1.0;
}
-void f_fun(double *state, double dt, double *out_1208381054458531187) {
- out_1208381054458531187[0] = state[0];
- out_1208381054458531187[1] = state[1];
- out_1208381054458531187[2] = state[2];
- out_1208381054458531187[3] = state[3];
- out_1208381054458531187[4] = state[4];
- out_1208381054458531187[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8000000000000007*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
- out_1208381054458531187[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
- out_1208381054458531187[7] = state[7];
- out_1208381054458531187[8] = state[8];
+void f_fun(double *state, double dt, double *out_6587028471138044914) {
+ out_6587028471138044914[0] = state[0];
+ out_6587028471138044914[1] = state[1];
+ out_6587028471138044914[2] = state[2];
+ out_6587028471138044914[3] = state[3];
+ out_6587028471138044914[4] = state[4];
+ out_6587028471138044914[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8000000000000007*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
+ out_6587028471138044914[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
+ out_6587028471138044914[7] = state[7];
+ out_6587028471138044914[8] = state[8];
}
-void F_fun(double *state, double dt, double *out_8824004521572161416) {
- out_8824004521572161416[0] = 1;
- out_8824004521572161416[1] = 0;
- out_8824004521572161416[2] = 0;
- out_8824004521572161416[3] = 0;
- out_8824004521572161416[4] = 0;
- out_8824004521572161416[5] = 0;
- out_8824004521572161416[6] = 0;
- out_8824004521572161416[7] = 0;
- out_8824004521572161416[8] = 0;
- out_8824004521572161416[9] = 0;
- out_8824004521572161416[10] = 1;
- out_8824004521572161416[11] = 0;
- out_8824004521572161416[12] = 0;
- out_8824004521572161416[13] = 0;
- out_8824004521572161416[14] = 0;
- out_8824004521572161416[15] = 0;
- out_8824004521572161416[16] = 0;
- out_8824004521572161416[17] = 0;
- out_8824004521572161416[18] = 0;
- out_8824004521572161416[19] = 0;
- out_8824004521572161416[20] = 1;
- out_8824004521572161416[21] = 0;
- out_8824004521572161416[22] = 0;
- out_8824004521572161416[23] = 0;
- out_8824004521572161416[24] = 0;
- out_8824004521572161416[25] = 0;
- out_8824004521572161416[26] = 0;
- out_8824004521572161416[27] = 0;
- out_8824004521572161416[28] = 0;
- out_8824004521572161416[29] = 0;
- out_8824004521572161416[30] = 1;
- out_8824004521572161416[31] = 0;
- out_8824004521572161416[32] = 0;
- out_8824004521572161416[33] = 0;
- out_8824004521572161416[34] = 0;
- out_8824004521572161416[35] = 0;
- out_8824004521572161416[36] = 0;
- out_8824004521572161416[37] = 0;
- out_8824004521572161416[38] = 0;
- out_8824004521572161416[39] = 0;
- out_8824004521572161416[40] = 1;
- out_8824004521572161416[41] = 0;
- out_8824004521572161416[42] = 0;
- out_8824004521572161416[43] = 0;
- out_8824004521572161416[44] = 0;
- out_8824004521572161416[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
- out_8824004521572161416[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
- out_8824004521572161416[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
- out_8824004521572161416[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
- out_8824004521572161416[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
- out_8824004521572161416[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
- out_8824004521572161416[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
- out_8824004521572161416[52] = dt*stiffness_front*state[0]/(mass*state[1]);
- out_8824004521572161416[53] = -9.8000000000000007*dt;
- out_8824004521572161416[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
- out_8824004521572161416[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
- out_8824004521572161416[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
- out_8824004521572161416[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
- out_8824004521572161416[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
- out_8824004521572161416[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
- out_8824004521572161416[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
- out_8824004521572161416[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
- out_8824004521572161416[62] = 0;
- out_8824004521572161416[63] = 0;
- out_8824004521572161416[64] = 0;
- out_8824004521572161416[65] = 0;
- out_8824004521572161416[66] = 0;
- out_8824004521572161416[67] = 0;
- out_8824004521572161416[68] = 0;
- out_8824004521572161416[69] = 0;
- out_8824004521572161416[70] = 1;
- out_8824004521572161416[71] = 0;
- out_8824004521572161416[72] = 0;
- out_8824004521572161416[73] = 0;
- out_8824004521572161416[74] = 0;
- out_8824004521572161416[75] = 0;
- out_8824004521572161416[76] = 0;
- out_8824004521572161416[77] = 0;
- out_8824004521572161416[78] = 0;
- out_8824004521572161416[79] = 0;
- out_8824004521572161416[80] = 1;
+void F_fun(double *state, double dt, double *out_7531965082275181187) {
+ out_7531965082275181187[0] = 1;
+ out_7531965082275181187[1] = 0;
+ out_7531965082275181187[2] = 0;
+ out_7531965082275181187[3] = 0;
+ out_7531965082275181187[4] = 0;
+ out_7531965082275181187[5] = 0;
+ out_7531965082275181187[6] = 0;
+ out_7531965082275181187[7] = 0;
+ out_7531965082275181187[8] = 0;
+ out_7531965082275181187[9] = 0;
+ out_7531965082275181187[10] = 1;
+ out_7531965082275181187[11] = 0;
+ out_7531965082275181187[12] = 0;
+ out_7531965082275181187[13] = 0;
+ out_7531965082275181187[14] = 0;
+ out_7531965082275181187[15] = 0;
+ out_7531965082275181187[16] = 0;
+ out_7531965082275181187[17] = 0;
+ out_7531965082275181187[18] = 0;
+ out_7531965082275181187[19] = 0;
+ out_7531965082275181187[20] = 1;
+ out_7531965082275181187[21] = 0;
+ out_7531965082275181187[22] = 0;
+ out_7531965082275181187[23] = 0;
+ out_7531965082275181187[24] = 0;
+ out_7531965082275181187[25] = 0;
+ out_7531965082275181187[26] = 0;
+ out_7531965082275181187[27] = 0;
+ out_7531965082275181187[28] = 0;
+ out_7531965082275181187[29] = 0;
+ out_7531965082275181187[30] = 1;
+ out_7531965082275181187[31] = 0;
+ out_7531965082275181187[32] = 0;
+ out_7531965082275181187[33] = 0;
+ out_7531965082275181187[34] = 0;
+ out_7531965082275181187[35] = 0;
+ out_7531965082275181187[36] = 0;
+ out_7531965082275181187[37] = 0;
+ out_7531965082275181187[38] = 0;
+ out_7531965082275181187[39] = 0;
+ out_7531965082275181187[40] = 1;
+ out_7531965082275181187[41] = 0;
+ out_7531965082275181187[42] = 0;
+ out_7531965082275181187[43] = 0;
+ out_7531965082275181187[44] = 0;
+ out_7531965082275181187[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
+ out_7531965082275181187[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
+ out_7531965082275181187[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
+ out_7531965082275181187[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
+ out_7531965082275181187[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
+ out_7531965082275181187[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
+ out_7531965082275181187[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
+ out_7531965082275181187[52] = dt*stiffness_front*state[0]/(mass*state[1]);
+ out_7531965082275181187[53] = -9.8000000000000007*dt;
+ out_7531965082275181187[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
+ out_7531965082275181187[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
+ out_7531965082275181187[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+ out_7531965082275181187[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+ out_7531965082275181187[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
+ out_7531965082275181187[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
+ out_7531965082275181187[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
+ out_7531965082275181187[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+ out_7531965082275181187[62] = 0;
+ out_7531965082275181187[63] = 0;
+ out_7531965082275181187[64] = 0;
+ out_7531965082275181187[65] = 0;
+ out_7531965082275181187[66] = 0;
+ out_7531965082275181187[67] = 0;
+ out_7531965082275181187[68] = 0;
+ out_7531965082275181187[69] = 0;
+ out_7531965082275181187[70] = 1;
+ out_7531965082275181187[71] = 0;
+ out_7531965082275181187[72] = 0;
+ out_7531965082275181187[73] = 0;
+ out_7531965082275181187[74] = 0;
+ out_7531965082275181187[75] = 0;
+ out_7531965082275181187[76] = 0;
+ out_7531965082275181187[77] = 0;
+ out_7531965082275181187[78] = 0;
+ out_7531965082275181187[79] = 0;
+ out_7531965082275181187[80] = 1;
}
-void h_25(double *state, double *unused, double *out_1789446864250922111) {
- out_1789446864250922111[0] = state[6];
+void h_25(double *state, double *unused, double *out_7956124124599872411) {
+ out_7956124124599872411[0] = state[6];
}
-void H_25(double *state, double *unused, double *out_9129886545195087354) {
- out_9129886545195087354[0] = 0;
- out_9129886545195087354[1] = 0;
- out_9129886545195087354[2] = 0;
- out_9129886545195087354[3] = 0;
- out_9129886545195087354[4] = 0;
- out_9129886545195087354[5] = 0;
- out_9129886545195087354[6] = 1;
- out_9129886545195087354[7] = 0;
- out_9129886545195087354[8] = 0;
+void H_25(double *state, double *unused, double *out_1152149855673758191) {
+ out_1152149855673758191[0] = 0;
+ out_1152149855673758191[1] = 0;
+ out_1152149855673758191[2] = 0;
+ out_1152149855673758191[3] = 0;
+ out_1152149855673758191[4] = 0;
+ out_1152149855673758191[5] = 0;
+ out_1152149855673758191[6] = 1;
+ out_1152149855673758191[7] = 0;
+ out_1152149855673758191[8] = 0;
}
-void h_24(double *state, double *unused, double *out_3299160385053087312) {
- out_3299160385053087312[0] = state[4];
- out_3299160385053087312[1] = state[5];
+void h_24(double *state, double *unused, double *out_8831563141601233713) {
+ out_8831563141601233713[0] = state[4];
+ out_8831563141601233713[1] = state[5];
}
-void H_24(double *state, double *unused, double *out_7144207929508964696) {
- out_7144207929508964696[0] = 0;
- out_7144207929508964696[1] = 0;
- out_7144207929508964696[2] = 0;
- out_7144207929508964696[3] = 0;
- out_7144207929508964696[4] = 1;
- out_7144207929508964696[5] = 0;
- out_7144207929508964696[6] = 0;
- out_7144207929508964696[7] = 0;
- out_7144207929508964696[8] = 0;
- out_7144207929508964696[9] = 0;
- out_7144207929508964696[10] = 0;
- out_7144207929508964696[11] = 0;
- out_7144207929508964696[12] = 0;
- out_7144207929508964696[13] = 0;
- out_7144207929508964696[14] = 1;
- out_7144207929508964696[15] = 0;
- out_7144207929508964696[16] = 0;
- out_7144207929508964696[17] = 0;
+void H_24(double *state, double *unused, double *out_1020499743331741375) {
+ out_1020499743331741375[0] = 0;
+ out_1020499743331741375[1] = 0;
+ out_1020499743331741375[2] = 0;
+ out_1020499743331741375[3] = 0;
+ out_1020499743331741375[4] = 1;
+ out_1020499743331741375[5] = 0;
+ out_1020499743331741375[6] = 0;
+ out_1020499743331741375[7] = 0;
+ out_1020499743331741375[8] = 0;
+ out_1020499743331741375[9] = 0;
+ out_1020499743331741375[10] = 0;
+ out_1020499743331741375[11] = 0;
+ out_1020499743331741375[12] = 0;
+ out_1020499743331741375[13] = 0;
+ out_1020499743331741375[14] = 1;
+ out_1020499743331741375[15] = 0;
+ out_1020499743331741375[16] = 0;
+ out_1020499743331741375[17] = 0;
}
-void h_30(double *state, double *unused, double *out_7348270420432929071) {
- out_7348270420432929071[0] = state[4];
+void h_30(double *state, double *unused, double *out_6628936472681177553) {
+ out_6628936472681177553[0] = state[4];
}
-void H_30(double *state, double *unused, double *out_6611553586687838727) {
- out_6611553586687838727[0] = 0;
- out_6611553586687838727[1] = 0;
- out_6611553586687838727[2] = 0;
- out_6611553586687838727[3] = 0;
- out_6611553586687838727[4] = 1;
- out_6611553586687838727[5] = 0;
- out_6611553586687838727[6] = 0;
- out_6611553586687838727[7] = 0;
- out_6611553586687838727[8] = 0;
+void H_30(double *state, double *unused, double *out_3375546474453850007) {
+ out_3375546474453850007[0] = 0;
+ out_3375546474453850007[1] = 0;
+ out_3375546474453850007[2] = 0;
+ out_3375546474453850007[3] = 0;
+ out_3375546474453850007[4] = 1;
+ out_3375546474453850007[5] = 0;
+ out_3375546474453850007[6] = 0;
+ out_3375546474453850007[7] = 0;
+ out_3375546474453850007[8] = 0;
}
-void h_26(double *state, double *unused, double *out_6096800385615776900) {
- out_6096800385615776900[0] = state[7];
+void h_26(double *state, double *unused, double *out_3816384369411324170) {
+ out_3816384369411324170[0] = state[7];
}
-void H_26(double *state, double *unused, double *out_5575354209640408038) {
- out_5575354209640408038[0] = 0;
- out_5575354209640408038[1] = 0;
- out_5575354209640408038[2] = 0;
- out_5575354209640408038[3] = 0;
- out_5575354209640408038[4] = 0;
- out_5575354209640408038[5] = 0;
- out_5575354209640408038[6] = 0;
- out_5575354209640408038[7] = 1;
- out_5575354209640408038[8] = 0;
+void H_26(double *state, double *unused, double *out_2589353463200298033) {
+ out_2589353463200298033[0] = 0;
+ out_2589353463200298033[1] = 0;
+ out_2589353463200298033[2] = 0;
+ out_2589353463200298033[3] = 0;
+ out_2589353463200298033[4] = 0;
+ out_2589353463200298033[5] = 0;
+ out_2589353463200298033[6] = 0;
+ out_2589353463200298033[7] = 1;
+ out_2589353463200298033[8] = 0;
}
-void h_27(double *state, double *unused, double *out_1247339187193410574) {
- out_1247339187193410574[0] = state[3];
+void h_27(double *state, double *unused, double *out_7012614271698209976) {
+ out_7012614271698209976[0] = state[3];
}
-void H_27(double *state, double *unused, double *out_2614397886586431153) {
- out_2614397886586431153[0] = 0;
- out_2614397886586431153[1] = 0;
- out_2614397886586431153[2] = 0;
- out_2614397886586431153[3] = 1;
- out_2614397886586431153[4] = 0;
- out_2614397886586431153[5] = 0;
- out_2614397886586431153[6] = 0;
- out_2614397886586431153[7] = 0;
- out_2614397886586431153[8] = 0;
+void H_27(double *state, double *unused, double *out_5550309786254274918) {
+ out_5550309786254274918[0] = 0;
+ out_5550309786254274918[1] = 0;
+ out_5550309786254274918[2] = 0;
+ out_5550309786254274918[3] = 1;
+ out_5550309786254274918[4] = 0;
+ out_5550309786254274918[5] = 0;
+ out_5550309786254274918[6] = 0;
+ out_5550309786254274918[7] = 0;
+ out_5550309786254274918[8] = 0;
}
-void h_29(double *state, double *unused, double *out_1705540549351972236) {
- out_1705540549351972236[0] = state[1];
+void h_29(double *state, double *unused, double *out_1627263205020331908) {
+ out_1627263205020331908[0] = state[1];
}
-void H_29(double *state, double *unused, double *out_5299392542701248248) {
- out_5299392542701248248[0] = 0;
- out_5299392542701248248[1] = 1;
- out_5299392542701248248[2] = 0;
- out_5299392542701248248[3] = 0;
- out_5299392542701248248[4] = 0;
- out_5299392542701248248[5] = 0;
- out_5299392542701248248[6] = 0;
- out_5299392542701248248[7] = 0;
- out_5299392542701248248[8] = 0;
+void H_29(double *state, double *unused, double *out_2865315130139457823) {
+ out_2865315130139457823[0] = 0;
+ out_2865315130139457823[1] = 1;
+ out_2865315130139457823[2] = 0;
+ out_2865315130139457823[3] = 0;
+ out_2865315130139457823[4] = 0;
+ out_2865315130139457823[5] = 0;
+ out_2865315130139457823[6] = 0;
+ out_2865315130139457823[7] = 0;
+ out_2865315130139457823[8] = 0;
}
-void h_28(double *state, double *unused, double *out_4170451758575142464) {
- out_4170451758575142464[0] = state[0];
+void h_28(double *state, double *unused, double *out_7994698365033508088) {
+ out_7994698365033508088[0] = state[0];
}
-void H_28(double *state, double *unused, double *out_7263022814266574499) {
- out_7263022814266574499[0] = 1;
- out_7263022814266574499[1] = 0;
- out_7263022814266574499[2] = 0;
- out_7263022814266574499[3] = 0;
- out_7263022814266574499[4] = 0;
- out_7263022814266574499[5] = 0;
- out_7263022814266574499[6] = 0;
- out_7263022814266574499[7] = 0;
- out_7263022814266574499[8] = 0;
+void H_28(double *state, double *unused, double *out_901684858574131572) {
+ out_901684858574131572[0] = 1;
+ out_901684858574131572[1] = 0;
+ out_901684858574131572[2] = 0;
+ out_901684858574131572[3] = 0;
+ out_901684858574131572[4] = 0;
+ out_901684858574131572[5] = 0;
+ out_901684858574131572[6] = 0;
+ out_901684858574131572[7] = 0;
+ out_901684858574131572[8] = 0;
}
-void h_31(double *state, double *unused, double *out_1514252801966416222) {
- out_1514252801966416222[0] = state[8];
+void h_31(double *state, double *unused, double *out_988808959918839917) {
+ out_988808959918839917[0] = state[8];
}
-void H_31(double *state, double *unused, double *out_4949146107407056562) {
- out_4949146107407056562[0] = 0;
- out_4949146107407056562[1] = 0;
- out_4949146107407056562[2] = 0;
- out_4949146107407056562[3] = 0;
- out_4949146107407056562[4] = 0;
- out_4949146107407056562[5] = 0;
- out_4949146107407056562[6] = 0;
- out_4949146107407056562[7] = 0;
- out_4949146107407056562[8] = 1;
+void H_31(double *state, double *unused, double *out_3215561565433649509) {
+ out_3215561565433649509[0] = 0;
+ out_3215561565433649509[1] = 0;
+ out_3215561565433649509[2] = 0;
+ out_3215561565433649509[3] = 0;
+ out_3215561565433649509[4] = 0;
+ out_3215561565433649509[5] = 0;
+ out_3215561565433649509[6] = 0;
+ out_3215561565433649509[7] = 0;
+ out_3215561565433649509[8] = 1;
}
#include
#include
@@ -518,68 +518,68 @@ void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, doubl
void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
update<1, 3, 0>(in_x, in_P, h_31, H_31, NULL, in_z, in_R, in_ea, MAHA_THRESH_31);
}
-void car_err_fun(double *nom_x, double *delta_x, double *out_5634330020296848070) {
- err_fun(nom_x, delta_x, out_5634330020296848070);
+void car_err_fun(double *nom_x, double *delta_x, double *out_9008082079438066370) {
+ err_fun(nom_x, delta_x, out_9008082079438066370);
}
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_1061559027503315281) {
- inv_err_fun(nom_x, true_x, out_1061559027503315281);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_7705390806256602072) {
+ inv_err_fun(nom_x, true_x, out_7705390806256602072);
}
-void car_H_mod_fun(double *state, double *out_1567569310651922730) {
- H_mod_fun(state, out_1567569310651922730);
+void car_H_mod_fun(double *state, double *out_4044238657628669408) {
+ H_mod_fun(state, out_4044238657628669408);
}
-void car_f_fun(double *state, double dt, double *out_1208381054458531187) {
- f_fun(state, dt, out_1208381054458531187);
+void car_f_fun(double *state, double dt, double *out_6587028471138044914) {
+ f_fun(state, dt, out_6587028471138044914);
}
-void car_F_fun(double *state, double dt, double *out_8824004521572161416) {
- F_fun(state, dt, out_8824004521572161416);
+void car_F_fun(double *state, double dt, double *out_7531965082275181187) {
+ F_fun(state, dt, out_7531965082275181187);
}
-void car_h_25(double *state, double *unused, double *out_1789446864250922111) {
- h_25(state, unused, out_1789446864250922111);
+void car_h_25(double *state, double *unused, double *out_7956124124599872411) {
+ h_25(state, unused, out_7956124124599872411);
}
-void car_H_25(double *state, double *unused, double *out_9129886545195087354) {
- H_25(state, unused, out_9129886545195087354);
+void car_H_25(double *state, double *unused, double *out_1152149855673758191) {
+ H_25(state, unused, out_1152149855673758191);
}
-void car_h_24(double *state, double *unused, double *out_3299160385053087312) {
- h_24(state, unused, out_3299160385053087312);
+void car_h_24(double *state, double *unused, double *out_8831563141601233713) {
+ h_24(state, unused, out_8831563141601233713);
}
-void car_H_24(double *state, double *unused, double *out_7144207929508964696) {
- H_24(state, unused, out_7144207929508964696);
+void car_H_24(double *state, double *unused, double *out_1020499743331741375) {
+ H_24(state, unused, out_1020499743331741375);
}
-void car_h_30(double *state, double *unused, double *out_7348270420432929071) {
- h_30(state, unused, out_7348270420432929071);
+void car_h_30(double *state, double *unused, double *out_6628936472681177553) {
+ h_30(state, unused, out_6628936472681177553);
}
-void car_H_30(double *state, double *unused, double *out_6611553586687838727) {
- H_30(state, unused, out_6611553586687838727);
+void car_H_30(double *state, double *unused, double *out_3375546474453850007) {
+ H_30(state, unused, out_3375546474453850007);
}
-void car_h_26(double *state, double *unused, double *out_6096800385615776900) {
- h_26(state, unused, out_6096800385615776900);
+void car_h_26(double *state, double *unused, double *out_3816384369411324170) {
+ h_26(state, unused, out_3816384369411324170);
}
-void car_H_26(double *state, double *unused, double *out_5575354209640408038) {
- H_26(state, unused, out_5575354209640408038);
+void car_H_26(double *state, double *unused, double *out_2589353463200298033) {
+ H_26(state, unused, out_2589353463200298033);
}
-void car_h_27(double *state, double *unused, double *out_1247339187193410574) {
- h_27(state, unused, out_1247339187193410574);
+void car_h_27(double *state, double *unused, double *out_7012614271698209976) {
+ h_27(state, unused, out_7012614271698209976);
}
-void car_H_27(double *state, double *unused, double *out_2614397886586431153) {
- H_27(state, unused, out_2614397886586431153);
+void car_H_27(double *state, double *unused, double *out_5550309786254274918) {
+ H_27(state, unused, out_5550309786254274918);
}
-void car_h_29(double *state, double *unused, double *out_1705540549351972236) {
- h_29(state, unused, out_1705540549351972236);
+void car_h_29(double *state, double *unused, double *out_1627263205020331908) {
+ h_29(state, unused, out_1627263205020331908);
}
-void car_H_29(double *state, double *unused, double *out_5299392542701248248) {
- H_29(state, unused, out_5299392542701248248);
+void car_H_29(double *state, double *unused, double *out_2865315130139457823) {
+ H_29(state, unused, out_2865315130139457823);
}
-void car_h_28(double *state, double *unused, double *out_4170451758575142464) {
- h_28(state, unused, out_4170451758575142464);
+void car_h_28(double *state, double *unused, double *out_7994698365033508088) {
+ h_28(state, unused, out_7994698365033508088);
}
-void car_H_28(double *state, double *unused, double *out_7263022814266574499) {
- H_28(state, unused, out_7263022814266574499);
+void car_H_28(double *state, double *unused, double *out_901684858574131572) {
+ H_28(state, unused, out_901684858574131572);
}
-void car_h_31(double *state, double *unused, double *out_1514252801966416222) {
- h_31(state, unused, out_1514252801966416222);
+void car_h_31(double *state, double *unused, double *out_988808959918839917) {
+ h_31(state, unused, out_988808959918839917);
}
-void car_H_31(double *state, double *unused, double *out_4949146107407056562) {
- H_31(state, unused, out_4949146107407056562);
+void car_H_31(double *state, double *unused, double *out_3215561565433649509) {
+ H_31(state, unused, out_3215561565433649509);
}
void car_predict(double *in_x, double *in_P, double *in_Q, double dt) {
predict(in_x, in_P, in_Q, dt);
diff --git a/selfdrive/locationd/models/generated/car.h b/selfdrive/locationd/models/generated/car.h
index 3fd634938..63b2261c0 100644
--- a/selfdrive/locationd/models/generated/car.h
+++ b/selfdrive/locationd/models/generated/car.h
@@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl
void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void car_err_fun(double *nom_x, double *delta_x, double *out_5634330020296848070);
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_1061559027503315281);
-void car_H_mod_fun(double *state, double *out_1567569310651922730);
-void car_f_fun(double *state, double dt, double *out_1208381054458531187);
-void car_F_fun(double *state, double dt, double *out_8824004521572161416);
-void car_h_25(double *state, double *unused, double *out_1789446864250922111);
-void car_H_25(double *state, double *unused, double *out_9129886545195087354);
-void car_h_24(double *state, double *unused, double *out_3299160385053087312);
-void car_H_24(double *state, double *unused, double *out_7144207929508964696);
-void car_h_30(double *state, double *unused, double *out_7348270420432929071);
-void car_H_30(double *state, double *unused, double *out_6611553586687838727);
-void car_h_26(double *state, double *unused, double *out_6096800385615776900);
-void car_H_26(double *state, double *unused, double *out_5575354209640408038);
-void car_h_27(double *state, double *unused, double *out_1247339187193410574);
-void car_H_27(double *state, double *unused, double *out_2614397886586431153);
-void car_h_29(double *state, double *unused, double *out_1705540549351972236);
-void car_H_29(double *state, double *unused, double *out_5299392542701248248);
-void car_h_28(double *state, double *unused, double *out_4170451758575142464);
-void car_H_28(double *state, double *unused, double *out_7263022814266574499);
-void car_h_31(double *state, double *unused, double *out_1514252801966416222);
-void car_H_31(double *state, double *unused, double *out_4949146107407056562);
+void car_err_fun(double *nom_x, double *delta_x, double *out_9008082079438066370);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_7705390806256602072);
+void car_H_mod_fun(double *state, double *out_4044238657628669408);
+void car_f_fun(double *state, double dt, double *out_6587028471138044914);
+void car_F_fun(double *state, double dt, double *out_7531965082275181187);
+void car_h_25(double *state, double *unused, double *out_7956124124599872411);
+void car_H_25(double *state, double *unused, double *out_1152149855673758191);
+void car_h_24(double *state, double *unused, double *out_8831563141601233713);
+void car_H_24(double *state, double *unused, double *out_1020499743331741375);
+void car_h_30(double *state, double *unused, double *out_6628936472681177553);
+void car_H_30(double *state, double *unused, double *out_3375546474453850007);
+void car_h_26(double *state, double *unused, double *out_3816384369411324170);
+void car_H_26(double *state, double *unused, double *out_2589353463200298033);
+void car_h_27(double *state, double *unused, double *out_7012614271698209976);
+void car_H_27(double *state, double *unused, double *out_5550309786254274918);
+void car_h_29(double *state, double *unused, double *out_1627263205020331908);
+void car_H_29(double *state, double *unused, double *out_2865315130139457823);
+void car_h_28(double *state, double *unused, double *out_7994698365033508088);
+void car_H_28(double *state, double *unused, double *out_901684858574131572);
+void car_h_31(double *state, double *unused, double *out_988808959918839917);
+void car_H_31(double *state, double *unused, double *out_3215561565433649509);
void car_predict(double *in_x, double *in_P, double *in_Q, double dt);
void car_set_mass(double x);
void car_set_rotational_inertia(double x);
diff --git a/selfdrive/locationd/models/generated/gnss.cpp b/selfdrive/locationd/models/generated/gnss.cpp
index 481ee2b48..a3b1ee34f 100644
--- a/selfdrive/locationd/models/generated/gnss.cpp
+++ b/selfdrive/locationd/models/generated/gnss.cpp
@@ -17,354 +17,354 @@ const static double MAHA_THRESH_21 = 3.8414588206941227;
* *
* This file is part of 'ekf' *
******************************************************************************/
-void err_fun(double *nom_x, double *delta_x, double *out_8218999594846759964) {
- out_8218999594846759964[0] = delta_x[0] + nom_x[0];
- out_8218999594846759964[1] = delta_x[1] + nom_x[1];
- out_8218999594846759964[2] = delta_x[2] + nom_x[2];
- out_8218999594846759964[3] = delta_x[3] + nom_x[3];
- out_8218999594846759964[4] = delta_x[4] + nom_x[4];
- out_8218999594846759964[5] = delta_x[5] + nom_x[5];
- out_8218999594846759964[6] = delta_x[6] + nom_x[6];
- out_8218999594846759964[7] = delta_x[7] + nom_x[7];
- out_8218999594846759964[8] = delta_x[8] + nom_x[8];
- out_8218999594846759964[9] = delta_x[9] + nom_x[9];
- out_8218999594846759964[10] = delta_x[10] + nom_x[10];
+void err_fun(double *nom_x, double *delta_x, double *out_9093778295671839447) {
+ out_9093778295671839447[0] = delta_x[0] + nom_x[0];
+ out_9093778295671839447[1] = delta_x[1] + nom_x[1];
+ out_9093778295671839447[2] = delta_x[2] + nom_x[2];
+ out_9093778295671839447[3] = delta_x[3] + nom_x[3];
+ out_9093778295671839447[4] = delta_x[4] + nom_x[4];
+ out_9093778295671839447[5] = delta_x[5] + nom_x[5];
+ out_9093778295671839447[6] = delta_x[6] + nom_x[6];
+ out_9093778295671839447[7] = delta_x[7] + nom_x[7];
+ out_9093778295671839447[8] = delta_x[8] + nom_x[8];
+ out_9093778295671839447[9] = delta_x[9] + nom_x[9];
+ out_9093778295671839447[10] = delta_x[10] + nom_x[10];
}
-void inv_err_fun(double *nom_x, double *true_x, double *out_6004490369099356165) {
- out_6004490369099356165[0] = -nom_x[0] + true_x[0];
- out_6004490369099356165[1] = -nom_x[1] + true_x[1];
- out_6004490369099356165[2] = -nom_x[2] + true_x[2];
- out_6004490369099356165[3] = -nom_x[3] + true_x[3];
- out_6004490369099356165[4] = -nom_x[4] + true_x[4];
- out_6004490369099356165[5] = -nom_x[5] + true_x[5];
- out_6004490369099356165[6] = -nom_x[6] + true_x[6];
- out_6004490369099356165[7] = -nom_x[7] + true_x[7];
- out_6004490369099356165[8] = -nom_x[8] + true_x[8];
- out_6004490369099356165[9] = -nom_x[9] + true_x[9];
- out_6004490369099356165[10] = -nom_x[10] + true_x[10];
+void inv_err_fun(double *nom_x, double *true_x, double *out_9126699124174503695) {
+ out_9126699124174503695[0] = -nom_x[0] + true_x[0];
+ out_9126699124174503695[1] = -nom_x[1] + true_x[1];
+ out_9126699124174503695[2] = -nom_x[2] + true_x[2];
+ out_9126699124174503695[3] = -nom_x[3] + true_x[3];
+ out_9126699124174503695[4] = -nom_x[4] + true_x[4];
+ out_9126699124174503695[5] = -nom_x[5] + true_x[5];
+ out_9126699124174503695[6] = -nom_x[6] + true_x[6];
+ out_9126699124174503695[7] = -nom_x[7] + true_x[7];
+ out_9126699124174503695[8] = -nom_x[8] + true_x[8];
+ out_9126699124174503695[9] = -nom_x[9] + true_x[9];
+ out_9126699124174503695[10] = -nom_x[10] + true_x[10];
}
-void H_mod_fun(double *state, double *out_5465143194872640210) {
- out_5465143194872640210[0] = 1.0;
- out_5465143194872640210[1] = 0;
- out_5465143194872640210[2] = 0;
- out_5465143194872640210[3] = 0;
- out_5465143194872640210[4] = 0;
- out_5465143194872640210[5] = 0;
- out_5465143194872640210[6] = 0;
- out_5465143194872640210[7] = 0;
- out_5465143194872640210[8] = 0;
- out_5465143194872640210[9] = 0;
- out_5465143194872640210[10] = 0;
- out_5465143194872640210[11] = 0;
- out_5465143194872640210[12] = 1.0;
- out_5465143194872640210[13] = 0;
- out_5465143194872640210[14] = 0;
- out_5465143194872640210[15] = 0;
- out_5465143194872640210[16] = 0;
- out_5465143194872640210[17] = 0;
- out_5465143194872640210[18] = 0;
- out_5465143194872640210[19] = 0;
- out_5465143194872640210[20] = 0;
- out_5465143194872640210[21] = 0;
- out_5465143194872640210[22] = 0;
- out_5465143194872640210[23] = 0;
- out_5465143194872640210[24] = 1.0;
- out_5465143194872640210[25] = 0;
- out_5465143194872640210[26] = 0;
- out_5465143194872640210[27] = 0;
- out_5465143194872640210[28] = 0;
- out_5465143194872640210[29] = 0;
- out_5465143194872640210[30] = 0;
- out_5465143194872640210[31] = 0;
- out_5465143194872640210[32] = 0;
- out_5465143194872640210[33] = 0;
- out_5465143194872640210[34] = 0;
- out_5465143194872640210[35] = 0;
- out_5465143194872640210[36] = 1.0;
- out_5465143194872640210[37] = 0;
- out_5465143194872640210[38] = 0;
- out_5465143194872640210[39] = 0;
- out_5465143194872640210[40] = 0;
- out_5465143194872640210[41] = 0;
- out_5465143194872640210[42] = 0;
- out_5465143194872640210[43] = 0;
- out_5465143194872640210[44] = 0;
- out_5465143194872640210[45] = 0;
- out_5465143194872640210[46] = 0;
- out_5465143194872640210[47] = 0;
- out_5465143194872640210[48] = 1.0;
- out_5465143194872640210[49] = 0;
- out_5465143194872640210[50] = 0;
- out_5465143194872640210[51] = 0;
- out_5465143194872640210[52] = 0;
- out_5465143194872640210[53] = 0;
- out_5465143194872640210[54] = 0;
- out_5465143194872640210[55] = 0;
- out_5465143194872640210[56] = 0;
- out_5465143194872640210[57] = 0;
- out_5465143194872640210[58] = 0;
- out_5465143194872640210[59] = 0;
- out_5465143194872640210[60] = 1.0;
- out_5465143194872640210[61] = 0;
- out_5465143194872640210[62] = 0;
- out_5465143194872640210[63] = 0;
- out_5465143194872640210[64] = 0;
- out_5465143194872640210[65] = 0;
- out_5465143194872640210[66] = 0;
- out_5465143194872640210[67] = 0;
- out_5465143194872640210[68] = 0;
- out_5465143194872640210[69] = 0;
- out_5465143194872640210[70] = 0;
- out_5465143194872640210[71] = 0;
- out_5465143194872640210[72] = 1.0;
- out_5465143194872640210[73] = 0;
- out_5465143194872640210[74] = 0;
- out_5465143194872640210[75] = 0;
- out_5465143194872640210[76] = 0;
- out_5465143194872640210[77] = 0;
- out_5465143194872640210[78] = 0;
- out_5465143194872640210[79] = 0;
- out_5465143194872640210[80] = 0;
- out_5465143194872640210[81] = 0;
- out_5465143194872640210[82] = 0;
- out_5465143194872640210[83] = 0;
- out_5465143194872640210[84] = 1.0;
- out_5465143194872640210[85] = 0;
- out_5465143194872640210[86] = 0;
- out_5465143194872640210[87] = 0;
- out_5465143194872640210[88] = 0;
- out_5465143194872640210[89] = 0;
- out_5465143194872640210[90] = 0;
- out_5465143194872640210[91] = 0;
- out_5465143194872640210[92] = 0;
- out_5465143194872640210[93] = 0;
- out_5465143194872640210[94] = 0;
- out_5465143194872640210[95] = 0;
- out_5465143194872640210[96] = 1.0;
- out_5465143194872640210[97] = 0;
- out_5465143194872640210[98] = 0;
- out_5465143194872640210[99] = 0;
- out_5465143194872640210[100] = 0;
- out_5465143194872640210[101] = 0;
- out_5465143194872640210[102] = 0;
- out_5465143194872640210[103] = 0;
- out_5465143194872640210[104] = 0;
- out_5465143194872640210[105] = 0;
- out_5465143194872640210[106] = 0;
- out_5465143194872640210[107] = 0;
- out_5465143194872640210[108] = 1.0;
- out_5465143194872640210[109] = 0;
- out_5465143194872640210[110] = 0;
- out_5465143194872640210[111] = 0;
- out_5465143194872640210[112] = 0;
- out_5465143194872640210[113] = 0;
- out_5465143194872640210[114] = 0;
- out_5465143194872640210[115] = 0;
- out_5465143194872640210[116] = 0;
- out_5465143194872640210[117] = 0;
- out_5465143194872640210[118] = 0;
- out_5465143194872640210[119] = 0;
- out_5465143194872640210[120] = 1.0;
+void H_mod_fun(double *state, double *out_854507982997558257) {
+ out_854507982997558257[0] = 1.0;
+ out_854507982997558257[1] = 0;
+ out_854507982997558257[2] = 0;
+ out_854507982997558257[3] = 0;
+ out_854507982997558257[4] = 0;
+ out_854507982997558257[5] = 0;
+ out_854507982997558257[6] = 0;
+ out_854507982997558257[7] = 0;
+ out_854507982997558257[8] = 0;
+ out_854507982997558257[9] = 0;
+ out_854507982997558257[10] = 0;
+ out_854507982997558257[11] = 0;
+ out_854507982997558257[12] = 1.0;
+ out_854507982997558257[13] = 0;
+ out_854507982997558257[14] = 0;
+ out_854507982997558257[15] = 0;
+ out_854507982997558257[16] = 0;
+ out_854507982997558257[17] = 0;
+ out_854507982997558257[18] = 0;
+ out_854507982997558257[19] = 0;
+ out_854507982997558257[20] = 0;
+ out_854507982997558257[21] = 0;
+ out_854507982997558257[22] = 0;
+ out_854507982997558257[23] = 0;
+ out_854507982997558257[24] = 1.0;
+ out_854507982997558257[25] = 0;
+ out_854507982997558257[26] = 0;
+ out_854507982997558257[27] = 0;
+ out_854507982997558257[28] = 0;
+ out_854507982997558257[29] = 0;
+ out_854507982997558257[30] = 0;
+ out_854507982997558257[31] = 0;
+ out_854507982997558257[32] = 0;
+ out_854507982997558257[33] = 0;
+ out_854507982997558257[34] = 0;
+ out_854507982997558257[35] = 0;
+ out_854507982997558257[36] = 1.0;
+ out_854507982997558257[37] = 0;
+ out_854507982997558257[38] = 0;
+ out_854507982997558257[39] = 0;
+ out_854507982997558257[40] = 0;
+ out_854507982997558257[41] = 0;
+ out_854507982997558257[42] = 0;
+ out_854507982997558257[43] = 0;
+ out_854507982997558257[44] = 0;
+ out_854507982997558257[45] = 0;
+ out_854507982997558257[46] = 0;
+ out_854507982997558257[47] = 0;
+ out_854507982997558257[48] = 1.0;
+ out_854507982997558257[49] = 0;
+ out_854507982997558257[50] = 0;
+ out_854507982997558257[51] = 0;
+ out_854507982997558257[52] = 0;
+ out_854507982997558257[53] = 0;
+ out_854507982997558257[54] = 0;
+ out_854507982997558257[55] = 0;
+ out_854507982997558257[56] = 0;
+ out_854507982997558257[57] = 0;
+ out_854507982997558257[58] = 0;
+ out_854507982997558257[59] = 0;
+ out_854507982997558257[60] = 1.0;
+ out_854507982997558257[61] = 0;
+ out_854507982997558257[62] = 0;
+ out_854507982997558257[63] = 0;
+ out_854507982997558257[64] = 0;
+ out_854507982997558257[65] = 0;
+ out_854507982997558257[66] = 0;
+ out_854507982997558257[67] = 0;
+ out_854507982997558257[68] = 0;
+ out_854507982997558257[69] = 0;
+ out_854507982997558257[70] = 0;
+ out_854507982997558257[71] = 0;
+ out_854507982997558257[72] = 1.0;
+ out_854507982997558257[73] = 0;
+ out_854507982997558257[74] = 0;
+ out_854507982997558257[75] = 0;
+ out_854507982997558257[76] = 0;
+ out_854507982997558257[77] = 0;
+ out_854507982997558257[78] = 0;
+ out_854507982997558257[79] = 0;
+ out_854507982997558257[80] = 0;
+ out_854507982997558257[81] = 0;
+ out_854507982997558257[82] = 0;
+ out_854507982997558257[83] = 0;
+ out_854507982997558257[84] = 1.0;
+ out_854507982997558257[85] = 0;
+ out_854507982997558257[86] = 0;
+ out_854507982997558257[87] = 0;
+ out_854507982997558257[88] = 0;
+ out_854507982997558257[89] = 0;
+ out_854507982997558257[90] = 0;
+ out_854507982997558257[91] = 0;
+ out_854507982997558257[92] = 0;
+ out_854507982997558257[93] = 0;
+ out_854507982997558257[94] = 0;
+ out_854507982997558257[95] = 0;
+ out_854507982997558257[96] = 1.0;
+ out_854507982997558257[97] = 0;
+ out_854507982997558257[98] = 0;
+ out_854507982997558257[99] = 0;
+ out_854507982997558257[100] = 0;
+ out_854507982997558257[101] = 0;
+ out_854507982997558257[102] = 0;
+ out_854507982997558257[103] = 0;
+ out_854507982997558257[104] = 0;
+ out_854507982997558257[105] = 0;
+ out_854507982997558257[106] = 0;
+ out_854507982997558257[107] = 0;
+ out_854507982997558257[108] = 1.0;
+ out_854507982997558257[109] = 0;
+ out_854507982997558257[110] = 0;
+ out_854507982997558257[111] = 0;
+ out_854507982997558257[112] = 0;
+ out_854507982997558257[113] = 0;
+ out_854507982997558257[114] = 0;
+ out_854507982997558257[115] = 0;
+ out_854507982997558257[116] = 0;
+ out_854507982997558257[117] = 0;
+ out_854507982997558257[118] = 0;
+ out_854507982997558257[119] = 0;
+ out_854507982997558257[120] = 1.0;
}
-void f_fun(double *state, double dt, double *out_6477589900664112980) {
- out_6477589900664112980[0] = dt*state[3] + state[0];
- out_6477589900664112980[1] = dt*state[4] + state[1];
- out_6477589900664112980[2] = dt*state[5] + state[2];
- out_6477589900664112980[3] = state[3];
- out_6477589900664112980[4] = state[4];
- out_6477589900664112980[5] = state[5];
- out_6477589900664112980[6] = dt*state[7] + state[6];
- out_6477589900664112980[7] = dt*state[8] + state[7];
- out_6477589900664112980[8] = state[8];
- out_6477589900664112980[9] = state[9];
- out_6477589900664112980[10] = state[10];
+void f_fun(double *state, double dt, double *out_7770403001027613956) {
+ out_7770403001027613956[0] = dt*state[3] + state[0];
+ out_7770403001027613956[1] = dt*state[4] + state[1];
+ out_7770403001027613956[2] = dt*state[5] + state[2];
+ out_7770403001027613956[3] = state[3];
+ out_7770403001027613956[4] = state[4];
+ out_7770403001027613956[5] = state[5];
+ out_7770403001027613956[6] = dt*state[7] + state[6];
+ out_7770403001027613956[7] = dt*state[8] + state[7];
+ out_7770403001027613956[8] = state[8];
+ out_7770403001027613956[9] = state[9];
+ out_7770403001027613956[10] = state[10];
}
-void F_fun(double *state, double dt, double *out_159332577828599777) {
- out_159332577828599777[0] = 1;
- out_159332577828599777[1] = 0;
- out_159332577828599777[2] = 0;
- out_159332577828599777[3] = dt;
- out_159332577828599777[4] = 0;
- out_159332577828599777[5] = 0;
- out_159332577828599777[6] = 0;
- out_159332577828599777[7] = 0;
- out_159332577828599777[8] = 0;
- out_159332577828599777[9] = 0;
- out_159332577828599777[10] = 0;
- out_159332577828599777[11] = 0;
- out_159332577828599777[12] = 1;
- out_159332577828599777[13] = 0;
- out_159332577828599777[14] = 0;
- out_159332577828599777[15] = dt;
- out_159332577828599777[16] = 0;
- out_159332577828599777[17] = 0;
- out_159332577828599777[18] = 0;
- out_159332577828599777[19] = 0;
- out_159332577828599777[20] = 0;
- out_159332577828599777[21] = 0;
- out_159332577828599777[22] = 0;
- out_159332577828599777[23] = 0;
- out_159332577828599777[24] = 1;
- out_159332577828599777[25] = 0;
- out_159332577828599777[26] = 0;
- out_159332577828599777[27] = dt;
- out_159332577828599777[28] = 0;
- out_159332577828599777[29] = 0;
- out_159332577828599777[30] = 0;
- out_159332577828599777[31] = 0;
- out_159332577828599777[32] = 0;
- out_159332577828599777[33] = 0;
- out_159332577828599777[34] = 0;
- out_159332577828599777[35] = 0;
- out_159332577828599777[36] = 1;
- out_159332577828599777[37] = 0;
- out_159332577828599777[38] = 0;
- out_159332577828599777[39] = 0;
- out_159332577828599777[40] = 0;
- out_159332577828599777[41] = 0;
- out_159332577828599777[42] = 0;
- out_159332577828599777[43] = 0;
- out_159332577828599777[44] = 0;
- out_159332577828599777[45] = 0;
- out_159332577828599777[46] = 0;
- out_159332577828599777[47] = 0;
- out_159332577828599777[48] = 1;
- out_159332577828599777[49] = 0;
- out_159332577828599777[50] = 0;
- out_159332577828599777[51] = 0;
- out_159332577828599777[52] = 0;
- out_159332577828599777[53] = 0;
- out_159332577828599777[54] = 0;
- out_159332577828599777[55] = 0;
- out_159332577828599777[56] = 0;
- out_159332577828599777[57] = 0;
- out_159332577828599777[58] = 0;
- out_159332577828599777[59] = 0;
- out_159332577828599777[60] = 1;
- out_159332577828599777[61] = 0;
- out_159332577828599777[62] = 0;
- out_159332577828599777[63] = 0;
- out_159332577828599777[64] = 0;
- out_159332577828599777[65] = 0;
- out_159332577828599777[66] = 0;
- out_159332577828599777[67] = 0;
- out_159332577828599777[68] = 0;
- out_159332577828599777[69] = 0;
- out_159332577828599777[70] = 0;
- out_159332577828599777[71] = 0;
- out_159332577828599777[72] = 1;
- out_159332577828599777[73] = dt;
- out_159332577828599777[74] = 0;
- out_159332577828599777[75] = 0;
- out_159332577828599777[76] = 0;
- out_159332577828599777[77] = 0;
- out_159332577828599777[78] = 0;
- out_159332577828599777[79] = 0;
- out_159332577828599777[80] = 0;
- out_159332577828599777[81] = 0;
- out_159332577828599777[82] = 0;
- out_159332577828599777[83] = 0;
- out_159332577828599777[84] = 1;
- out_159332577828599777[85] = dt;
- out_159332577828599777[86] = 0;
- out_159332577828599777[87] = 0;
- out_159332577828599777[88] = 0;
- out_159332577828599777[89] = 0;
- out_159332577828599777[90] = 0;
- out_159332577828599777[91] = 0;
- out_159332577828599777[92] = 0;
- out_159332577828599777[93] = 0;
- out_159332577828599777[94] = 0;
- out_159332577828599777[95] = 0;
- out_159332577828599777[96] = 1;
- out_159332577828599777[97] = 0;
- out_159332577828599777[98] = 0;
- out_159332577828599777[99] = 0;
- out_159332577828599777[100] = 0;
- out_159332577828599777[101] = 0;
- out_159332577828599777[102] = 0;
- out_159332577828599777[103] = 0;
- out_159332577828599777[104] = 0;
- out_159332577828599777[105] = 0;
- out_159332577828599777[106] = 0;
- out_159332577828599777[107] = 0;
- out_159332577828599777[108] = 1;
- out_159332577828599777[109] = 0;
- out_159332577828599777[110] = 0;
- out_159332577828599777[111] = 0;
- out_159332577828599777[112] = 0;
- out_159332577828599777[113] = 0;
- out_159332577828599777[114] = 0;
- out_159332577828599777[115] = 0;
- out_159332577828599777[116] = 0;
- out_159332577828599777[117] = 0;
- out_159332577828599777[118] = 0;
- out_159332577828599777[119] = 0;
- out_159332577828599777[120] = 1;
+void F_fun(double *state, double dt, double *out_7798555812212859819) {
+ out_7798555812212859819[0] = 1;
+ out_7798555812212859819[1] = 0;
+ out_7798555812212859819[2] = 0;
+ out_7798555812212859819[3] = dt;
+ out_7798555812212859819[4] = 0;
+ out_7798555812212859819[5] = 0;
+ out_7798555812212859819[6] = 0;
+ out_7798555812212859819[7] = 0;
+ out_7798555812212859819[8] = 0;
+ out_7798555812212859819[9] = 0;
+ out_7798555812212859819[10] = 0;
+ out_7798555812212859819[11] = 0;
+ out_7798555812212859819[12] = 1;
+ out_7798555812212859819[13] = 0;
+ out_7798555812212859819[14] = 0;
+ out_7798555812212859819[15] = dt;
+ out_7798555812212859819[16] = 0;
+ out_7798555812212859819[17] = 0;
+ out_7798555812212859819[18] = 0;
+ out_7798555812212859819[19] = 0;
+ out_7798555812212859819[20] = 0;
+ out_7798555812212859819[21] = 0;
+ out_7798555812212859819[22] = 0;
+ out_7798555812212859819[23] = 0;
+ out_7798555812212859819[24] = 1;
+ out_7798555812212859819[25] = 0;
+ out_7798555812212859819[26] = 0;
+ out_7798555812212859819[27] = dt;
+ out_7798555812212859819[28] = 0;
+ out_7798555812212859819[29] = 0;
+ out_7798555812212859819[30] = 0;
+ out_7798555812212859819[31] = 0;
+ out_7798555812212859819[32] = 0;
+ out_7798555812212859819[33] = 0;
+ out_7798555812212859819[34] = 0;
+ out_7798555812212859819[35] = 0;
+ out_7798555812212859819[36] = 1;
+ out_7798555812212859819[37] = 0;
+ out_7798555812212859819[38] = 0;
+ out_7798555812212859819[39] = 0;
+ out_7798555812212859819[40] = 0;
+ out_7798555812212859819[41] = 0;
+ out_7798555812212859819[42] = 0;
+ out_7798555812212859819[43] = 0;
+ out_7798555812212859819[44] = 0;
+ out_7798555812212859819[45] = 0;
+ out_7798555812212859819[46] = 0;
+ out_7798555812212859819[47] = 0;
+ out_7798555812212859819[48] = 1;
+ out_7798555812212859819[49] = 0;
+ out_7798555812212859819[50] = 0;
+ out_7798555812212859819[51] = 0;
+ out_7798555812212859819[52] = 0;
+ out_7798555812212859819[53] = 0;
+ out_7798555812212859819[54] = 0;
+ out_7798555812212859819[55] = 0;
+ out_7798555812212859819[56] = 0;
+ out_7798555812212859819[57] = 0;
+ out_7798555812212859819[58] = 0;
+ out_7798555812212859819[59] = 0;
+ out_7798555812212859819[60] = 1;
+ out_7798555812212859819[61] = 0;
+ out_7798555812212859819[62] = 0;
+ out_7798555812212859819[63] = 0;
+ out_7798555812212859819[64] = 0;
+ out_7798555812212859819[65] = 0;
+ out_7798555812212859819[66] = 0;
+ out_7798555812212859819[67] = 0;
+ out_7798555812212859819[68] = 0;
+ out_7798555812212859819[69] = 0;
+ out_7798555812212859819[70] = 0;
+ out_7798555812212859819[71] = 0;
+ out_7798555812212859819[72] = 1;
+ out_7798555812212859819[73] = dt;
+ out_7798555812212859819[74] = 0;
+ out_7798555812212859819[75] = 0;
+ out_7798555812212859819[76] = 0;
+ out_7798555812212859819[77] = 0;
+ out_7798555812212859819[78] = 0;
+ out_7798555812212859819[79] = 0;
+ out_7798555812212859819[80] = 0;
+ out_7798555812212859819[81] = 0;
+ out_7798555812212859819[82] = 0;
+ out_7798555812212859819[83] = 0;
+ out_7798555812212859819[84] = 1;
+ out_7798555812212859819[85] = dt;
+ out_7798555812212859819[86] = 0;
+ out_7798555812212859819[87] = 0;
+ out_7798555812212859819[88] = 0;
+ out_7798555812212859819[89] = 0;
+ out_7798555812212859819[90] = 0;
+ out_7798555812212859819[91] = 0;
+ out_7798555812212859819[92] = 0;
+ out_7798555812212859819[93] = 0;
+ out_7798555812212859819[94] = 0;
+ out_7798555812212859819[95] = 0;
+ out_7798555812212859819[96] = 1;
+ out_7798555812212859819[97] = 0;
+ out_7798555812212859819[98] = 0;
+ out_7798555812212859819[99] = 0;
+ out_7798555812212859819[100] = 0;
+ out_7798555812212859819[101] = 0;
+ out_7798555812212859819[102] = 0;
+ out_7798555812212859819[103] = 0;
+ out_7798555812212859819[104] = 0;
+ out_7798555812212859819[105] = 0;
+ out_7798555812212859819[106] = 0;
+ out_7798555812212859819[107] = 0;
+ out_7798555812212859819[108] = 1;
+ out_7798555812212859819[109] = 0;
+ out_7798555812212859819[110] = 0;
+ out_7798555812212859819[111] = 0;
+ out_7798555812212859819[112] = 0;
+ out_7798555812212859819[113] = 0;
+ out_7798555812212859819[114] = 0;
+ out_7798555812212859819[115] = 0;
+ out_7798555812212859819[116] = 0;
+ out_7798555812212859819[117] = 0;
+ out_7798555812212859819[118] = 0;
+ out_7798555812212859819[119] = 0;
+ out_7798555812212859819[120] = 1;
}
-void h_6(double *state, double *sat_pos, double *out_822185378282268145) {
- out_822185378282268145[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + state[6];
+void h_6(double *state, double *sat_pos, double *out_9201175564716128809) {
+ out_9201175564716128809[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + state[6];
}
-void H_6(double *state, double *sat_pos, double *out_2954217689973778176) {
- out_2954217689973778176[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
- out_2954217689973778176[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
- out_2954217689973778176[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
- out_2954217689973778176[3] = 0;
- out_2954217689973778176[4] = 0;
- out_2954217689973778176[5] = 0;
- out_2954217689973778176[6] = 1;
- out_2954217689973778176[7] = 0;
- out_2954217689973778176[8] = 0;
- out_2954217689973778176[9] = 0;
- out_2954217689973778176[10] = 0;
+void H_6(double *state, double *sat_pos, double *out_1419005819748336970) {
+ out_1419005819748336970[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
+ out_1419005819748336970[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
+ out_1419005819748336970[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
+ out_1419005819748336970[3] = 0;
+ out_1419005819748336970[4] = 0;
+ out_1419005819748336970[5] = 0;
+ out_1419005819748336970[6] = 1;
+ out_1419005819748336970[7] = 0;
+ out_1419005819748336970[8] = 0;
+ out_1419005819748336970[9] = 0;
+ out_1419005819748336970[10] = 0;
}
-void h_20(double *state, double *sat_pos, double *out_3775340380998125465) {
- out_3775340380998125465[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + sat_pos[3]*state[10] + state[6] + state[9];
+void h_20(double *state, double *sat_pos, double *out_8590157039648027835) {
+ out_8590157039648027835[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + sat_pos[3]*state[10] + state[6] + state[9];
}
-void H_20(double *state, double *sat_pos, double *out_4124080333257632986) {
- out_4124080333257632986[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
- out_4124080333257632986[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
- out_4124080333257632986[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
- out_4124080333257632986[3] = 0;
- out_4124080333257632986[4] = 0;
- out_4124080333257632986[5] = 0;
- out_4124080333257632986[6] = 1;
- out_4124080333257632986[7] = 0;
- out_4124080333257632986[8] = 0;
- out_4124080333257632986[9] = 1;
- out_4124080333257632986[10] = sat_pos[3];
+void H_20(double *state, double *sat_pos, double *out_5617129456073796795) {
+ out_5617129456073796795[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
+ out_5617129456073796795[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
+ out_5617129456073796795[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
+ out_5617129456073796795[3] = 0;
+ out_5617129456073796795[4] = 0;
+ out_5617129456073796795[5] = 0;
+ out_5617129456073796795[6] = 1;
+ out_5617129456073796795[7] = 0;
+ out_5617129456073796795[8] = 0;
+ out_5617129456073796795[9] = 1;
+ out_5617129456073796795[10] = sat_pos[3];
}
-void h_7(double *state, double *sat_pos_vel, double *out_750344228235882460) {
- out_750344228235882460[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
+void h_7(double *state, double *sat_pos_vel, double *out_8069802722407087087) {
+ out_8069802722407087087[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
}
-void H_7(double *state, double *sat_pos_vel, double *out_1618262330099536806) {
- out_1618262330099536806[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
- out_1618262330099536806[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
- out_1618262330099536806[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
- out_1618262330099536806[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
- out_1618262330099536806[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
- out_1618262330099536806[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
- out_1618262330099536806[6] = 0;
- out_1618262330099536806[7] = 1;
- out_1618262330099536806[8] = 0;
- out_1618262330099536806[9] = 0;
- out_1618262330099536806[10] = 0;
+void H_7(double *state, double *sat_pos_vel, double *out_7177713928341367106) {
+ out_7177713928341367106[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+ out_7177713928341367106[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+ out_7177713928341367106[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+ out_7177713928341367106[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+ out_7177713928341367106[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+ out_7177713928341367106[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+ out_7177713928341367106[6] = 0;
+ out_7177713928341367106[7] = 1;
+ out_7177713928341367106[8] = 0;
+ out_7177713928341367106[9] = 0;
+ out_7177713928341367106[10] = 0;
}
-void h_21(double *state, double *sat_pos_vel, double *out_750344228235882460) {
- out_750344228235882460[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
+void h_21(double *state, double *sat_pos_vel, double *out_8069802722407087087) {
+ out_8069802722407087087[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
}
-void H_21(double *state, double *sat_pos_vel, double *out_1618262330099536806) {
- out_1618262330099536806[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
- out_1618262330099536806[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
- out_1618262330099536806[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
- out_1618262330099536806[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
- out_1618262330099536806[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
- out_1618262330099536806[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
- out_1618262330099536806[6] = 0;
- out_1618262330099536806[7] = 1;
- out_1618262330099536806[8] = 0;
- out_1618262330099536806[9] = 0;
- out_1618262330099536806[10] = 0;
+void H_21(double *state, double *sat_pos_vel, double *out_7177713928341367106) {
+ out_7177713928341367106[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+ out_7177713928341367106[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+ out_7177713928341367106[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+ out_7177713928341367106[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+ out_7177713928341367106[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+ out_7177713928341367106[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+ out_7177713928341367106[6] = 0;
+ out_7177713928341367106[7] = 1;
+ out_7177713928341367106[8] = 0;
+ out_7177713928341367106[9] = 0;
+ out_7177713928341367106[10] = 0;
}
#include
#include
@@ -506,44 +506,44 @@ void gnss_update_7(double *in_x, double *in_P, double *in_z, double *in_R, doubl
void gnss_update_21(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
update<1, 3, 0>(in_x, in_P, h_21, H_21, NULL, in_z, in_R, in_ea, MAHA_THRESH_21);
}
-void gnss_err_fun(double *nom_x, double *delta_x, double *out_8218999594846759964) {
- err_fun(nom_x, delta_x, out_8218999594846759964);
+void gnss_err_fun(double *nom_x, double *delta_x, double *out_9093778295671839447) {
+ err_fun(nom_x, delta_x, out_9093778295671839447);
}
-void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_6004490369099356165) {
- inv_err_fun(nom_x, true_x, out_6004490369099356165);
+void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_9126699124174503695) {
+ inv_err_fun(nom_x, true_x, out_9126699124174503695);
}
-void gnss_H_mod_fun(double *state, double *out_5465143194872640210) {
- H_mod_fun(state, out_5465143194872640210);
+void gnss_H_mod_fun(double *state, double *out_854507982997558257) {
+ H_mod_fun(state, out_854507982997558257);
}
-void gnss_f_fun(double *state, double dt, double *out_6477589900664112980) {
- f_fun(state, dt, out_6477589900664112980);
+void gnss_f_fun(double *state, double dt, double *out_7770403001027613956) {
+ f_fun(state, dt, out_7770403001027613956);
}
-void gnss_F_fun(double *state, double dt, double *out_159332577828599777) {
- F_fun(state, dt, out_159332577828599777);
+void gnss_F_fun(double *state, double dt, double *out_7798555812212859819) {
+ F_fun(state, dt, out_7798555812212859819);
}
-void gnss_h_6(double *state, double *sat_pos, double *out_822185378282268145) {
- h_6(state, sat_pos, out_822185378282268145);
+void gnss_h_6(double *state, double *sat_pos, double *out_9201175564716128809) {
+ h_6(state, sat_pos, out_9201175564716128809);
}
-void gnss_H_6(double *state, double *sat_pos, double *out_2954217689973778176) {
- H_6(state, sat_pos, out_2954217689973778176);
+void gnss_H_6(double *state, double *sat_pos, double *out_1419005819748336970) {
+ H_6(state, sat_pos, out_1419005819748336970);
}
-void gnss_h_20(double *state, double *sat_pos, double *out_3775340380998125465) {
- h_20(state, sat_pos, out_3775340380998125465);
+void gnss_h_20(double *state, double *sat_pos, double *out_8590157039648027835) {
+ h_20(state, sat_pos, out_8590157039648027835);
}
-void gnss_H_20(double *state, double *sat_pos, double *out_4124080333257632986) {
- H_20(state, sat_pos, out_4124080333257632986);
+void gnss_H_20(double *state, double *sat_pos, double *out_5617129456073796795) {
+ H_20(state, sat_pos, out_5617129456073796795);
}
-void gnss_h_7(double *state, double *sat_pos_vel, double *out_750344228235882460) {
- h_7(state, sat_pos_vel, out_750344228235882460);
+void gnss_h_7(double *state, double *sat_pos_vel, double *out_8069802722407087087) {
+ h_7(state, sat_pos_vel, out_8069802722407087087);
}
-void gnss_H_7(double *state, double *sat_pos_vel, double *out_1618262330099536806) {
- H_7(state, sat_pos_vel, out_1618262330099536806);
+void gnss_H_7(double *state, double *sat_pos_vel, double *out_7177713928341367106) {
+ H_7(state, sat_pos_vel, out_7177713928341367106);
}
-void gnss_h_21(double *state, double *sat_pos_vel, double *out_750344228235882460) {
- h_21(state, sat_pos_vel, out_750344228235882460);
+void gnss_h_21(double *state, double *sat_pos_vel, double *out_8069802722407087087) {
+ h_21(state, sat_pos_vel, out_8069802722407087087);
}
-void gnss_H_21(double *state, double *sat_pos_vel, double *out_1618262330099536806) {
- H_21(state, sat_pos_vel, out_1618262330099536806);
+void gnss_H_21(double *state, double *sat_pos_vel, double *out_7177713928341367106) {
+ H_21(state, sat_pos_vel, out_7177713928341367106);
}
void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt) {
predict(in_x, in_P, in_Q, dt);
diff --git a/selfdrive/locationd/models/generated/gnss.h b/selfdrive/locationd/models/generated/gnss.h
index 8cb47ea78..7baa518ae 100644
--- a/selfdrive/locationd/models/generated/gnss.h
+++ b/selfdrive/locationd/models/generated/gnss.h
@@ -5,18 +5,18 @@ void gnss_update_6(double *in_x, double *in_P, double *in_z, double *in_R, doubl
void gnss_update_20(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void gnss_update_7(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void gnss_update_21(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void gnss_err_fun(double *nom_x, double *delta_x, double *out_8218999594846759964);
-void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_6004490369099356165);
-void gnss_H_mod_fun(double *state, double *out_5465143194872640210);
-void gnss_f_fun(double *state, double dt, double *out_6477589900664112980);
-void gnss_F_fun(double *state, double dt, double *out_159332577828599777);
-void gnss_h_6(double *state, double *sat_pos, double *out_822185378282268145);
-void gnss_H_6(double *state, double *sat_pos, double *out_2954217689973778176);
-void gnss_h_20(double *state, double *sat_pos, double *out_3775340380998125465);
-void gnss_H_20(double *state, double *sat_pos, double *out_4124080333257632986);
-void gnss_h_7(double *state, double *sat_pos_vel, double *out_750344228235882460);
-void gnss_H_7(double *state, double *sat_pos_vel, double *out_1618262330099536806);
-void gnss_h_21(double *state, double *sat_pos_vel, double *out_750344228235882460);
-void gnss_H_21(double *state, double *sat_pos_vel, double *out_1618262330099536806);
+void gnss_err_fun(double *nom_x, double *delta_x, double *out_9093778295671839447);
+void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_9126699124174503695);
+void gnss_H_mod_fun(double *state, double *out_854507982997558257);
+void gnss_f_fun(double *state, double dt, double *out_7770403001027613956);
+void gnss_F_fun(double *state, double dt, double *out_7798555812212859819);
+void gnss_h_6(double *state, double *sat_pos, double *out_9201175564716128809);
+void gnss_H_6(double *state, double *sat_pos, double *out_1419005819748336970);
+void gnss_h_20(double *state, double *sat_pos, double *out_8590157039648027835);
+void gnss_H_20(double *state, double *sat_pos, double *out_5617129456073796795);
+void gnss_h_7(double *state, double *sat_pos_vel, double *out_8069802722407087087);
+void gnss_H_7(double *state, double *sat_pos_vel, double *out_7177713928341367106);
+void gnss_h_21(double *state, double *sat_pos_vel, double *out_8069802722407087087);
+void gnss_H_21(double *state, double *sat_pos_vel, double *out_7177713928341367106);
void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt);
}
\ No newline at end of file
diff --git a/selfdrive/locationd/models/generated/libkf.so b/selfdrive/locationd/models/generated/libkf.so
index 49360ab1f..da0564b36 100755
Binary files a/selfdrive/locationd/models/generated/libkf.so and b/selfdrive/locationd/models/generated/libkf.so differ
diff --git a/selfdrive/locationd/models/generated/live.cpp b/selfdrive/locationd/models/generated/live.cpp
index cfa051d01..90906c461 100644
--- a/selfdrive/locationd/models/generated/live.cpp
+++ b/selfdrive/locationd/models/generated/live.cpp
@@ -22,1683 +22,1683 @@ const static double MAHA_THRESH_33 = 7.814727903251177;
* *
* This file is part of 'ekf' *
******************************************************************************/
-void H(double *in_vec, double *out_7959907782308194627) {
- out_7959907782308194627[0] = 0;
- out_7959907782308194627[1] = -sin(in_vec[1])*sin(in_vec[2])*in_vec[4] - sin(in_vec[1])*cos(in_vec[2])*in_vec[3] - cos(in_vec[1])*in_vec[5];
- out_7959907782308194627[2] = -sin(in_vec[2])*cos(in_vec[1])*in_vec[3] + cos(in_vec[1])*cos(in_vec[2])*in_vec[4];
- out_7959907782308194627[3] = cos(in_vec[1])*cos(in_vec[2]);
- out_7959907782308194627[4] = sin(in_vec[2])*cos(in_vec[1]);
- out_7959907782308194627[5] = -sin(in_vec[1]);
- out_7959907782308194627[6] = (sin(in_vec[0])*sin(in_vec[2]) + sin(in_vec[1])*cos(in_vec[0])*cos(in_vec[2]))*in_vec[3] + (-sin(in_vec[0])*cos(in_vec[2]) + sin(in_vec[1])*sin(in_vec[2])*cos(in_vec[0]))*in_vec[4] + cos(in_vec[0])*cos(in_vec[1])*in_vec[5];
- out_7959907782308194627[7] = -sin(in_vec[0])*sin(in_vec[1])*in_vec[5] + sin(in_vec[0])*sin(in_vec[2])*cos(in_vec[1])*in_vec[4] + sin(in_vec[0])*cos(in_vec[1])*cos(in_vec[2])*in_vec[3];
- out_7959907782308194627[8] = (-sin(in_vec[0])*sin(in_vec[1])*sin(in_vec[2]) - cos(in_vec[0])*cos(in_vec[2]))*in_vec[3] + (sin(in_vec[0])*sin(in_vec[1])*cos(in_vec[2]) - sin(in_vec[2])*cos(in_vec[0]))*in_vec[4];
- out_7959907782308194627[9] = sin(in_vec[0])*sin(in_vec[1])*cos(in_vec[2]) - sin(in_vec[2])*cos(in_vec[0]);
- out_7959907782308194627[10] = sin(in_vec[0])*sin(in_vec[1])*sin(in_vec[2]) + cos(in_vec[0])*cos(in_vec[2]);
- out_7959907782308194627[11] = sin(in_vec[0])*cos(in_vec[1]);
- out_7959907782308194627[12] = (-sin(in_vec[0])*sin(in_vec[1])*sin(in_vec[2]) - cos(in_vec[0])*cos(in_vec[2]))*in_vec[4] + (-sin(in_vec[0])*sin(in_vec[1])*cos(in_vec[2]) + sin(in_vec[2])*cos(in_vec[0]))*in_vec[3] - sin(in_vec[0])*cos(in_vec[1])*in_vec[5];
- out_7959907782308194627[13] = -sin(in_vec[1])*cos(in_vec[0])*in_vec[5] + sin(in_vec[2])*cos(in_vec[0])*cos(in_vec[1])*in_vec[4] + cos(in_vec[0])*cos(in_vec[1])*cos(in_vec[2])*in_vec[3];
- out_7959907782308194627[14] = (sin(in_vec[0])*sin(in_vec[2]) + sin(in_vec[1])*cos(in_vec[0])*cos(in_vec[2]))*in_vec[4] + (sin(in_vec[0])*cos(in_vec[2]) - sin(in_vec[1])*sin(in_vec[2])*cos(in_vec[0]))*in_vec[3];
- out_7959907782308194627[15] = sin(in_vec[0])*sin(in_vec[2]) + sin(in_vec[1])*cos(in_vec[0])*cos(in_vec[2]);
- out_7959907782308194627[16] = -sin(in_vec[0])*cos(in_vec[2]) + sin(in_vec[1])*sin(in_vec[2])*cos(in_vec[0]);
- out_7959907782308194627[17] = cos(in_vec[0])*cos(in_vec[1]);
+void H(double *in_vec, double *out_7310749464307319056) {
+ out_7310749464307319056[0] = 0;
+ out_7310749464307319056[1] = -sin(in_vec[1])*sin(in_vec[2])*in_vec[4] - sin(in_vec[1])*cos(in_vec[2])*in_vec[3] - cos(in_vec[1])*in_vec[5];
+ out_7310749464307319056[2] = -sin(in_vec[2])*cos(in_vec[1])*in_vec[3] + cos(in_vec[1])*cos(in_vec[2])*in_vec[4];
+ out_7310749464307319056[3] = cos(in_vec[1])*cos(in_vec[2]);
+ out_7310749464307319056[4] = sin(in_vec[2])*cos(in_vec[1]);
+ out_7310749464307319056[5] = -sin(in_vec[1]);
+ out_7310749464307319056[6] = (sin(in_vec[0])*sin(in_vec[2]) + sin(in_vec[1])*cos(in_vec[0])*cos(in_vec[2]))*in_vec[3] + (-sin(in_vec[0])*cos(in_vec[2]) + sin(in_vec[1])*sin(in_vec[2])*cos(in_vec[0]))*in_vec[4] + cos(in_vec[0])*cos(in_vec[1])*in_vec[5];
+ out_7310749464307319056[7] = -sin(in_vec[0])*sin(in_vec[1])*in_vec[5] + sin(in_vec[0])*sin(in_vec[2])*cos(in_vec[1])*in_vec[4] + sin(in_vec[0])*cos(in_vec[1])*cos(in_vec[2])*in_vec[3];
+ out_7310749464307319056[8] = (-sin(in_vec[0])*sin(in_vec[1])*sin(in_vec[2]) - cos(in_vec[0])*cos(in_vec[2]))*in_vec[3] + (sin(in_vec[0])*sin(in_vec[1])*cos(in_vec[2]) - sin(in_vec[2])*cos(in_vec[0]))*in_vec[4];
+ out_7310749464307319056[9] = sin(in_vec[0])*sin(in_vec[1])*cos(in_vec[2]) - sin(in_vec[2])*cos(in_vec[0]);
+ out_7310749464307319056[10] = sin(in_vec[0])*sin(in_vec[1])*sin(in_vec[2]) + cos(in_vec[0])*cos(in_vec[2]);
+ out_7310749464307319056[11] = sin(in_vec[0])*cos(in_vec[1]);
+ out_7310749464307319056[12] = (-sin(in_vec[0])*sin(in_vec[1])*sin(in_vec[2]) - cos(in_vec[0])*cos(in_vec[2]))*in_vec[4] + (-sin(in_vec[0])*sin(in_vec[1])*cos(in_vec[2]) + sin(in_vec[2])*cos(in_vec[0]))*in_vec[3] - sin(in_vec[0])*cos(in_vec[1])*in_vec[5];
+ out_7310749464307319056[13] = -sin(in_vec[1])*cos(in_vec[0])*in_vec[5] + sin(in_vec[2])*cos(in_vec[0])*cos(in_vec[1])*in_vec[4] + cos(in_vec[0])*cos(in_vec[1])*cos(in_vec[2])*in_vec[3];
+ out_7310749464307319056[14] = (sin(in_vec[0])*sin(in_vec[2]) + sin(in_vec[1])*cos(in_vec[0])*cos(in_vec[2]))*in_vec[4] + (sin(in_vec[0])*cos(in_vec[2]) - sin(in_vec[1])*sin(in_vec[2])*cos(in_vec[0]))*in_vec[3];
+ out_7310749464307319056[15] = sin(in_vec[0])*sin(in_vec[2]) + sin(in_vec[1])*cos(in_vec[0])*cos(in_vec[2]);
+ out_7310749464307319056[16] = -sin(in_vec[0])*cos(in_vec[2]) + sin(in_vec[1])*sin(in_vec[2])*cos(in_vec[0]);
+ out_7310749464307319056[17] = cos(in_vec[0])*cos(in_vec[1]);
}
-void err_fun(double *nom_x, double *delta_x, double *out_6929774310989775988) {
- out_6929774310989775988[0] = delta_x[0] + nom_x[0];
- out_6929774310989775988[1] = delta_x[1] + nom_x[1];
- out_6929774310989775988[2] = delta_x[2] + nom_x[2];
- out_6929774310989775988[3] = -0.5*delta_x[3]*nom_x[4] - 0.5*delta_x[4]*nom_x[5] - 0.5*delta_x[5]*nom_x[6] + 1.0*nom_x[3];
- out_6929774310989775988[4] = 0.5*delta_x[3]*nom_x[3] + 0.5*delta_x[4]*nom_x[6] - 0.5*delta_x[5]*nom_x[5] + 1.0*nom_x[4];
- out_6929774310989775988[5] = -0.5*delta_x[3]*nom_x[6] + 0.5*delta_x[4]*nom_x[3] + 0.5*delta_x[5]*nom_x[4] + 1.0*nom_x[5];
- out_6929774310989775988[6] = 0.5*delta_x[3]*nom_x[5] - 0.5*delta_x[4]*nom_x[4] + 0.5*delta_x[5]*nom_x[3] + 1.0*nom_x[6];
- out_6929774310989775988[7] = delta_x[6] + nom_x[7];
- out_6929774310989775988[8] = delta_x[7] + nom_x[8];
- out_6929774310989775988[9] = delta_x[8] + nom_x[9];
- out_6929774310989775988[10] = delta_x[9] + nom_x[10];
- out_6929774310989775988[11] = delta_x[10] + nom_x[11];
- out_6929774310989775988[12] = delta_x[11] + nom_x[12];
- out_6929774310989775988[13] = delta_x[12] + nom_x[13];
- out_6929774310989775988[14] = delta_x[13] + nom_x[14];
- out_6929774310989775988[15] = delta_x[14] + nom_x[15];
- out_6929774310989775988[16] = delta_x[15] + nom_x[16];
- out_6929774310989775988[17] = delta_x[16] + nom_x[17];
- out_6929774310989775988[18] = delta_x[17] + nom_x[18];
- out_6929774310989775988[19] = delta_x[18] + nom_x[19];
- out_6929774310989775988[20] = delta_x[19] + nom_x[20];
- out_6929774310989775988[21] = delta_x[20] + nom_x[21];
+void err_fun(double *nom_x, double *delta_x, double *out_8236526131657428764) {
+ out_8236526131657428764[0] = delta_x[0] + nom_x[0];
+ out_8236526131657428764[1] = delta_x[1] + nom_x[1];
+ out_8236526131657428764[2] = delta_x[2] + nom_x[2];
+ out_8236526131657428764[3] = -0.5*delta_x[3]*nom_x[4] - 0.5*delta_x[4]*nom_x[5] - 0.5*delta_x[5]*nom_x[6] + 1.0*nom_x[3];
+ out_8236526131657428764[4] = 0.5*delta_x[3]*nom_x[3] + 0.5*delta_x[4]*nom_x[6] - 0.5*delta_x[5]*nom_x[5] + 1.0*nom_x[4];
+ out_8236526131657428764[5] = -0.5*delta_x[3]*nom_x[6] + 0.5*delta_x[4]*nom_x[3] + 0.5*delta_x[5]*nom_x[4] + 1.0*nom_x[5];
+ out_8236526131657428764[6] = 0.5*delta_x[3]*nom_x[5] - 0.5*delta_x[4]*nom_x[4] + 0.5*delta_x[5]*nom_x[3] + 1.0*nom_x[6];
+ out_8236526131657428764[7] = delta_x[6] + nom_x[7];
+ out_8236526131657428764[8] = delta_x[7] + nom_x[8];
+ out_8236526131657428764[9] = delta_x[8] + nom_x[9];
+ out_8236526131657428764[10] = delta_x[9] + nom_x[10];
+ out_8236526131657428764[11] = delta_x[10] + nom_x[11];
+ out_8236526131657428764[12] = delta_x[11] + nom_x[12];
+ out_8236526131657428764[13] = delta_x[12] + nom_x[13];
+ out_8236526131657428764[14] = delta_x[13] + nom_x[14];
+ out_8236526131657428764[15] = delta_x[14] + nom_x[15];
+ out_8236526131657428764[16] = delta_x[15] + nom_x[16];
+ out_8236526131657428764[17] = delta_x[16] + nom_x[17];
+ out_8236526131657428764[18] = delta_x[17] + nom_x[18];
+ out_8236526131657428764[19] = delta_x[18] + nom_x[19];
+ out_8236526131657428764[20] = delta_x[19] + nom_x[20];
+ out_8236526131657428764[21] = delta_x[20] + nom_x[21];
}
-void inv_err_fun(double *nom_x, double *true_x, double *out_6193789776824585571) {
- out_6193789776824585571[0] = -nom_x[0] + true_x[0];
- out_6193789776824585571[1] = -nom_x[1] + true_x[1];
- out_6193789776824585571[2] = -nom_x[2] + true_x[2];
- out_6193789776824585571[3] = 2*nom_x[3]*true_x[4] - 2*nom_x[4]*true_x[3] + 2*nom_x[5]*true_x[6] - 2*nom_x[6]*true_x[5];
- out_6193789776824585571[4] = 2*nom_x[3]*true_x[5] - 2*nom_x[4]*true_x[6] - 2*nom_x[5]*true_x[3] + 2*nom_x[6]*true_x[4];
- out_6193789776824585571[5] = 2*nom_x[3]*true_x[6] + 2*nom_x[4]*true_x[5] - 2*nom_x[5]*true_x[4] - 2*nom_x[6]*true_x[3];
- out_6193789776824585571[6] = -nom_x[7] + true_x[7];
- out_6193789776824585571[7] = -nom_x[8] + true_x[8];
- out_6193789776824585571[8] = -nom_x[9] + true_x[9];
- out_6193789776824585571[9] = -nom_x[10] + true_x[10];
- out_6193789776824585571[10] = -nom_x[11] + true_x[11];
- out_6193789776824585571[11] = -nom_x[12] + true_x[12];
- out_6193789776824585571[12] = -nom_x[13] + true_x[13];
- out_6193789776824585571[13] = -nom_x[14] + true_x[14];
- out_6193789776824585571[14] = -nom_x[15] + true_x[15];
- out_6193789776824585571[15] = -nom_x[16] + true_x[16];
- out_6193789776824585571[16] = -nom_x[17] + true_x[17];
- out_6193789776824585571[17] = -nom_x[18] + true_x[18];
- out_6193789776824585571[18] = -nom_x[19] + true_x[19];
- out_6193789776824585571[19] = -nom_x[20] + true_x[20];
- out_6193789776824585571[20] = -nom_x[21] + true_x[21];
+void inv_err_fun(double *nom_x, double *true_x, double *out_4747477926419252764) {
+ out_4747477926419252764[0] = -nom_x[0] + true_x[0];
+ out_4747477926419252764[1] = -nom_x[1] + true_x[1];
+ out_4747477926419252764[2] = -nom_x[2] + true_x[2];
+ out_4747477926419252764[3] = 2*nom_x[3]*true_x[4] - 2*nom_x[4]*true_x[3] + 2*nom_x[5]*true_x[6] - 2*nom_x[6]*true_x[5];
+ out_4747477926419252764[4] = 2*nom_x[3]*true_x[5] - 2*nom_x[4]*true_x[6] - 2*nom_x[5]*true_x[3] + 2*nom_x[6]*true_x[4];
+ out_4747477926419252764[5] = 2*nom_x[3]*true_x[6] + 2*nom_x[4]*true_x[5] - 2*nom_x[5]*true_x[4] - 2*nom_x[6]*true_x[3];
+ out_4747477926419252764[6] = -nom_x[7] + true_x[7];
+ out_4747477926419252764[7] = -nom_x[8] + true_x[8];
+ out_4747477926419252764[8] = -nom_x[9] + true_x[9];
+ out_4747477926419252764[9] = -nom_x[10] + true_x[10];
+ out_4747477926419252764[10] = -nom_x[11] + true_x[11];
+ out_4747477926419252764[11] = -nom_x[12] + true_x[12];
+ out_4747477926419252764[12] = -nom_x[13] + true_x[13];
+ out_4747477926419252764[13] = -nom_x[14] + true_x[14];
+ out_4747477926419252764[14] = -nom_x[15] + true_x[15];
+ out_4747477926419252764[15] = -nom_x[16] + true_x[16];
+ out_4747477926419252764[16] = -nom_x[17] + true_x[17];
+ out_4747477926419252764[17] = -nom_x[18] + true_x[18];
+ out_4747477926419252764[18] = -nom_x[19] + true_x[19];
+ out_4747477926419252764[19] = -nom_x[20] + true_x[20];
+ out_4747477926419252764[20] = -nom_x[21] + true_x[21];
}
-void H_mod_fun(double *state, double *out_8547509152961498922) {
- out_8547509152961498922[0] = 1.0;
- out_8547509152961498922[1] = 0;
- out_8547509152961498922[2] = 0;
- out_8547509152961498922[3] = 0;
- out_8547509152961498922[4] = 0;
- out_8547509152961498922[5] = 0;
- out_8547509152961498922[6] = 0;
- out_8547509152961498922[7] = 0;
- out_8547509152961498922[8] = 0;
- out_8547509152961498922[9] = 0;
- out_8547509152961498922[10] = 0;
- out_8547509152961498922[11] = 0;
- out_8547509152961498922[12] = 0;
- out_8547509152961498922[13] = 0;
- out_8547509152961498922[14] = 0;
- out_8547509152961498922[15] = 0;
- out_8547509152961498922[16] = 0;
- out_8547509152961498922[17] = 0;
- out_8547509152961498922[18] = 0;
- out_8547509152961498922[19] = 0;
- out_8547509152961498922[20] = 0;
- out_8547509152961498922[21] = 0;
- out_8547509152961498922[22] = 1.0;
- out_8547509152961498922[23] = 0;
- out_8547509152961498922[24] = 0;
- out_8547509152961498922[25] = 0;
- out_8547509152961498922[26] = 0;
- out_8547509152961498922[27] = 0;
- out_8547509152961498922[28] = 0;
- out_8547509152961498922[29] = 0;
- out_8547509152961498922[30] = 0;
- out_8547509152961498922[31] = 0;
- out_8547509152961498922[32] = 0;
- out_8547509152961498922[33] = 0;
- out_8547509152961498922[34] = 0;
- out_8547509152961498922[35] = 0;
- out_8547509152961498922[36] = 0;
- out_8547509152961498922[37] = 0;
- out_8547509152961498922[38] = 0;
- out_8547509152961498922[39] = 0;
- out_8547509152961498922[40] = 0;
- out_8547509152961498922[41] = 0;
- out_8547509152961498922[42] = 0;
- out_8547509152961498922[43] = 0;
- out_8547509152961498922[44] = 1.0;
- out_8547509152961498922[45] = 0;
- out_8547509152961498922[46] = 0;
- out_8547509152961498922[47] = 0;
- out_8547509152961498922[48] = 0;
- out_8547509152961498922[49] = 0;
- out_8547509152961498922[50] = 0;
- out_8547509152961498922[51] = 0;
- out_8547509152961498922[52] = 0;
- out_8547509152961498922[53] = 0;
- out_8547509152961498922[54] = 0;
- out_8547509152961498922[55] = 0;
- out_8547509152961498922[56] = 0;
- out_8547509152961498922[57] = 0;
- out_8547509152961498922[58] = 0;
- out_8547509152961498922[59] = 0;
- out_8547509152961498922[60] = 0;
- out_8547509152961498922[61] = 0;
- out_8547509152961498922[62] = 0;
- out_8547509152961498922[63] = 0;
- out_8547509152961498922[64] = 0;
- out_8547509152961498922[65] = 0;
- out_8547509152961498922[66] = -0.5*state[4];
- out_8547509152961498922[67] = -0.5*state[5];
- out_8547509152961498922[68] = -0.5*state[6];
- out_8547509152961498922[69] = 0;
- out_8547509152961498922[70] = 0;
- out_8547509152961498922[71] = 0;
- out_8547509152961498922[72] = 0;
- out_8547509152961498922[73] = 0;
- out_8547509152961498922[74] = 0;
- out_8547509152961498922[75] = 0;
- out_8547509152961498922[76] = 0;
- out_8547509152961498922[77] = 0;
- out_8547509152961498922[78] = 0;
- out_8547509152961498922[79] = 0;
- out_8547509152961498922[80] = 0;
- out_8547509152961498922[81] = 0;
- out_8547509152961498922[82] = 0;
- out_8547509152961498922[83] = 0;
- out_8547509152961498922[84] = 0;
- out_8547509152961498922[85] = 0;
- out_8547509152961498922[86] = 0;
- out_8547509152961498922[87] = 0.5*state[3];
- out_8547509152961498922[88] = 0.5*state[6];
- out_8547509152961498922[89] = -0.5*state[5];
- out_8547509152961498922[90] = 0;
- out_8547509152961498922[91] = 0;
- out_8547509152961498922[92] = 0;
- out_8547509152961498922[93] = 0;
- out_8547509152961498922[94] = 0;
- out_8547509152961498922[95] = 0;
- out_8547509152961498922[96] = 0;
- out_8547509152961498922[97] = 0;
- out_8547509152961498922[98] = 0;
- out_8547509152961498922[99] = 0;
- out_8547509152961498922[100] = 0;
- out_8547509152961498922[101] = 0;
- out_8547509152961498922[102] = 0;
- out_8547509152961498922[103] = 0;
- out_8547509152961498922[104] = 0;
- out_8547509152961498922[105] = 0;
- out_8547509152961498922[106] = 0;
- out_8547509152961498922[107] = 0;
- out_8547509152961498922[108] = -0.5*state[6];
- out_8547509152961498922[109] = 0.5*state[3];
- out_8547509152961498922[110] = 0.5*state[4];
- out_8547509152961498922[111] = 0;
- out_8547509152961498922[112] = 0;
- out_8547509152961498922[113] = 0;
- out_8547509152961498922[114] = 0;
- out_8547509152961498922[115] = 0;
- out_8547509152961498922[116] = 0;
- out_8547509152961498922[117] = 0;
- out_8547509152961498922[118] = 0;
- out_8547509152961498922[119] = 0;
- out_8547509152961498922[120] = 0;
- out_8547509152961498922[121] = 0;
- out_8547509152961498922[122] = 0;
- out_8547509152961498922[123] = 0;
- out_8547509152961498922[124] = 0;
- out_8547509152961498922[125] = 0;
- out_8547509152961498922[126] = 0;
- out_8547509152961498922[127] = 0;
- out_8547509152961498922[128] = 0;
- out_8547509152961498922[129] = 0.5*state[5];
- out_8547509152961498922[130] = -0.5*state[4];
- out_8547509152961498922[131] = 0.5*state[3];
- out_8547509152961498922[132] = 0;
- out_8547509152961498922[133] = 0;
- out_8547509152961498922[134] = 0;
- out_8547509152961498922[135] = 0;
- out_8547509152961498922[136] = 0;
- out_8547509152961498922[137] = 0;
- out_8547509152961498922[138] = 0;
- out_8547509152961498922[139] = 0;
- out_8547509152961498922[140] = 0;
- out_8547509152961498922[141] = 0;
- out_8547509152961498922[142] = 0;
- out_8547509152961498922[143] = 0;
- out_8547509152961498922[144] = 0;
- out_8547509152961498922[145] = 0;
- out_8547509152961498922[146] = 0;
- out_8547509152961498922[147] = 0;
- out_8547509152961498922[148] = 0;
- out_8547509152961498922[149] = 0;
- out_8547509152961498922[150] = 0;
- out_8547509152961498922[151] = 0;
- out_8547509152961498922[152] = 0;
- out_8547509152961498922[153] = 1.0;
- out_8547509152961498922[154] = 0;
- out_8547509152961498922[155] = 0;
- out_8547509152961498922[156] = 0;
- out_8547509152961498922[157] = 0;
- out_8547509152961498922[158] = 0;
- out_8547509152961498922[159] = 0;
- out_8547509152961498922[160] = 0;
- out_8547509152961498922[161] = 0;
- out_8547509152961498922[162] = 0;
- out_8547509152961498922[163] = 0;
- out_8547509152961498922[164] = 0;
- out_8547509152961498922[165] = 0;
- out_8547509152961498922[166] = 0;
- out_8547509152961498922[167] = 0;
- out_8547509152961498922[168] = 0;
- out_8547509152961498922[169] = 0;
- out_8547509152961498922[170] = 0;
- out_8547509152961498922[171] = 0;
- out_8547509152961498922[172] = 0;
- out_8547509152961498922[173] = 0;
- out_8547509152961498922[174] = 0;
- out_8547509152961498922[175] = 1.0;
- out_8547509152961498922[176] = 0;
- out_8547509152961498922[177] = 0;
- out_8547509152961498922[178] = 0;
- out_8547509152961498922[179] = 0;
- out_8547509152961498922[180] = 0;
- out_8547509152961498922[181] = 0;
- out_8547509152961498922[182] = 0;
- out_8547509152961498922[183] = 0;
- out_8547509152961498922[184] = 0;
- out_8547509152961498922[185] = 0;
- out_8547509152961498922[186] = 0;
- out_8547509152961498922[187] = 0;
- out_8547509152961498922[188] = 0;
- out_8547509152961498922[189] = 0;
- out_8547509152961498922[190] = 0;
- out_8547509152961498922[191] = 0;
- out_8547509152961498922[192] = 0;
- out_8547509152961498922[193] = 0;
- out_8547509152961498922[194] = 0;
- out_8547509152961498922[195] = 0;
- out_8547509152961498922[196] = 0;
- out_8547509152961498922[197] = 1.0;
- out_8547509152961498922[198] = 0;
- out_8547509152961498922[199] = 0;
- out_8547509152961498922[200] = 0;
- out_8547509152961498922[201] = 0;
- out_8547509152961498922[202] = 0;
- out_8547509152961498922[203] = 0;
- out_8547509152961498922[204] = 0;
- out_8547509152961498922[205] = 0;
- out_8547509152961498922[206] = 0;
- out_8547509152961498922[207] = 0;
- out_8547509152961498922[208] = 0;
- out_8547509152961498922[209] = 0;
- out_8547509152961498922[210] = 0;
- out_8547509152961498922[211] = 0;
- out_8547509152961498922[212] = 0;
- out_8547509152961498922[213] = 0;
- out_8547509152961498922[214] = 0;
- out_8547509152961498922[215] = 0;
- out_8547509152961498922[216] = 0;
- out_8547509152961498922[217] = 0;
- out_8547509152961498922[218] = 0;
- out_8547509152961498922[219] = 1.0;
- out_8547509152961498922[220] = 0;
- out_8547509152961498922[221] = 0;
- out_8547509152961498922[222] = 0;
- out_8547509152961498922[223] = 0;
- out_8547509152961498922[224] = 0;
- out_8547509152961498922[225] = 0;
- out_8547509152961498922[226] = 0;
- out_8547509152961498922[227] = 0;
- out_8547509152961498922[228] = 0;
- out_8547509152961498922[229] = 0;
- out_8547509152961498922[230] = 0;
- out_8547509152961498922[231] = 0;
- out_8547509152961498922[232] = 0;
- out_8547509152961498922[233] = 0;
- out_8547509152961498922[234] = 0;
- out_8547509152961498922[235] = 0;
- out_8547509152961498922[236] = 0;
- out_8547509152961498922[237] = 0;
- out_8547509152961498922[238] = 0;
- out_8547509152961498922[239] = 0;
- out_8547509152961498922[240] = 0;
- out_8547509152961498922[241] = 1.0;
- out_8547509152961498922[242] = 0;
- out_8547509152961498922[243] = 0;
- out_8547509152961498922[244] = 0;
- out_8547509152961498922[245] = 0;
- out_8547509152961498922[246] = 0;
- out_8547509152961498922[247] = 0;
- out_8547509152961498922[248] = 0;
- out_8547509152961498922[249] = 0;
- out_8547509152961498922[250] = 0;
- out_8547509152961498922[251] = 0;
- out_8547509152961498922[252] = 0;
- out_8547509152961498922[253] = 0;
- out_8547509152961498922[254] = 0;
- out_8547509152961498922[255] = 0;
- out_8547509152961498922[256] = 0;
- out_8547509152961498922[257] = 0;
- out_8547509152961498922[258] = 0;
- out_8547509152961498922[259] = 0;
- out_8547509152961498922[260] = 0;
- out_8547509152961498922[261] = 0;
- out_8547509152961498922[262] = 0;
- out_8547509152961498922[263] = 1.0;
- out_8547509152961498922[264] = 0;
- out_8547509152961498922[265] = 0;
- out_8547509152961498922[266] = 0;
- out_8547509152961498922[267] = 0;
- out_8547509152961498922[268] = 0;
- out_8547509152961498922[269] = 0;
- out_8547509152961498922[270] = 0;
- out_8547509152961498922[271] = 0;
- out_8547509152961498922[272] = 0;
- out_8547509152961498922[273] = 0;
- out_8547509152961498922[274] = 0;
- out_8547509152961498922[275] = 0;
- out_8547509152961498922[276] = 0;
- out_8547509152961498922[277] = 0;
- out_8547509152961498922[278] = 0;
- out_8547509152961498922[279] = 0;
- out_8547509152961498922[280] = 0;
- out_8547509152961498922[281] = 0;
- out_8547509152961498922[282] = 0;
- out_8547509152961498922[283] = 0;
- out_8547509152961498922[284] = 0;
- out_8547509152961498922[285] = 1.0;
- out_8547509152961498922[286] = 0;
- out_8547509152961498922[287] = 0;
- out_8547509152961498922[288] = 0;
- out_8547509152961498922[289] = 0;
- out_8547509152961498922[290] = 0;
- out_8547509152961498922[291] = 0;
- out_8547509152961498922[292] = 0;
- out_8547509152961498922[293] = 0;
- out_8547509152961498922[294] = 0;
- out_8547509152961498922[295] = 0;
- out_8547509152961498922[296] = 0;
- out_8547509152961498922[297] = 0;
- out_8547509152961498922[298] = 0;
- out_8547509152961498922[299] = 0;
- out_8547509152961498922[300] = 0;
- out_8547509152961498922[301] = 0;
- out_8547509152961498922[302] = 0;
- out_8547509152961498922[303] = 0;
- out_8547509152961498922[304] = 0;
- out_8547509152961498922[305] = 0;
- out_8547509152961498922[306] = 0;
- out_8547509152961498922[307] = 1.0;
- out_8547509152961498922[308] = 0;
- out_8547509152961498922[309] = 0;
- out_8547509152961498922[310] = 0;
- out_8547509152961498922[311] = 0;
- out_8547509152961498922[312] = 0;
- out_8547509152961498922[313] = 0;
- out_8547509152961498922[314] = 0;
- out_8547509152961498922[315] = 0;
- out_8547509152961498922[316] = 0;
- out_8547509152961498922[317] = 0;
- out_8547509152961498922[318] = 0;
- out_8547509152961498922[319] = 0;
- out_8547509152961498922[320] = 0;
- out_8547509152961498922[321] = 0;
- out_8547509152961498922[322] = 0;
- out_8547509152961498922[323] = 0;
- out_8547509152961498922[324] = 0;
- out_8547509152961498922[325] = 0;
- out_8547509152961498922[326] = 0;
- out_8547509152961498922[327] = 0;
- out_8547509152961498922[328] = 0;
- out_8547509152961498922[329] = 1.0;
- out_8547509152961498922[330] = 0;
- out_8547509152961498922[331] = 0;
- out_8547509152961498922[332] = 0;
- out_8547509152961498922[333] = 0;
- out_8547509152961498922[334] = 0;
- out_8547509152961498922[335] = 0;
- out_8547509152961498922[336] = 0;
- out_8547509152961498922[337] = 0;
- out_8547509152961498922[338] = 0;
- out_8547509152961498922[339] = 0;
- out_8547509152961498922[340] = 0;
- out_8547509152961498922[341] = 0;
- out_8547509152961498922[342] = 0;
- out_8547509152961498922[343] = 0;
- out_8547509152961498922[344] = 0;
- out_8547509152961498922[345] = 0;
- out_8547509152961498922[346] = 0;
- out_8547509152961498922[347] = 0;
- out_8547509152961498922[348] = 0;
- out_8547509152961498922[349] = 0;
- out_8547509152961498922[350] = 0;
- out_8547509152961498922[351] = 1.0;
- out_8547509152961498922[352] = 0;
- out_8547509152961498922[353] = 0;
- out_8547509152961498922[354] = 0;
- out_8547509152961498922[355] = 0;
- out_8547509152961498922[356] = 0;
- out_8547509152961498922[357] = 0;
- out_8547509152961498922[358] = 0;
- out_8547509152961498922[359] = 0;
- out_8547509152961498922[360] = 0;
- out_8547509152961498922[361] = 0;
- out_8547509152961498922[362] = 0;
- out_8547509152961498922[363] = 0;
- out_8547509152961498922[364] = 0;
- out_8547509152961498922[365] = 0;
- out_8547509152961498922[366] = 0;
- out_8547509152961498922[367] = 0;
- out_8547509152961498922[368] = 0;
- out_8547509152961498922[369] = 0;
- out_8547509152961498922[370] = 0;
- out_8547509152961498922[371] = 0;
- out_8547509152961498922[372] = 0;
- out_8547509152961498922[373] = 1.0;
- out_8547509152961498922[374] = 0;
- out_8547509152961498922[375] = 0;
- out_8547509152961498922[376] = 0;
- out_8547509152961498922[377] = 0;
- out_8547509152961498922[378] = 0;
- out_8547509152961498922[379] = 0;
- out_8547509152961498922[380] = 0;
- out_8547509152961498922[381] = 0;
- out_8547509152961498922[382] = 0;
- out_8547509152961498922[383] = 0;
- out_8547509152961498922[384] = 0;
- out_8547509152961498922[385] = 0;
- out_8547509152961498922[386] = 0;
- out_8547509152961498922[387] = 0;
- out_8547509152961498922[388] = 0;
- out_8547509152961498922[389] = 0;
- out_8547509152961498922[390] = 0;
- out_8547509152961498922[391] = 0;
- out_8547509152961498922[392] = 0;
- out_8547509152961498922[393] = 0;
- out_8547509152961498922[394] = 0;
- out_8547509152961498922[395] = 1.0;
- out_8547509152961498922[396] = 0;
- out_8547509152961498922[397] = 0;
- out_8547509152961498922[398] = 0;
- out_8547509152961498922[399] = 0;
- out_8547509152961498922[400] = 0;
- out_8547509152961498922[401] = 0;
- out_8547509152961498922[402] = 0;
- out_8547509152961498922[403] = 0;
- out_8547509152961498922[404] = 0;
- out_8547509152961498922[405] = 0;
- out_8547509152961498922[406] = 0;
- out_8547509152961498922[407] = 0;
- out_8547509152961498922[408] = 0;
- out_8547509152961498922[409] = 0;
- out_8547509152961498922[410] = 0;
- out_8547509152961498922[411] = 0;
- out_8547509152961498922[412] = 0;
- out_8547509152961498922[413] = 0;
- out_8547509152961498922[414] = 0;
- out_8547509152961498922[415] = 0;
- out_8547509152961498922[416] = 0;
- out_8547509152961498922[417] = 1.0;
- out_8547509152961498922[418] = 0;
- out_8547509152961498922[419] = 0;
- out_8547509152961498922[420] = 0;
- out_8547509152961498922[421] = 0;
- out_8547509152961498922[422] = 0;
- out_8547509152961498922[423] = 0;
- out_8547509152961498922[424] = 0;
- out_8547509152961498922[425] = 0;
- out_8547509152961498922[426] = 0;
- out_8547509152961498922[427] = 0;
- out_8547509152961498922[428] = 0;
- out_8547509152961498922[429] = 0;
- out_8547509152961498922[430] = 0;
- out_8547509152961498922[431] = 0;
- out_8547509152961498922[432] = 0;
- out_8547509152961498922[433] = 0;
- out_8547509152961498922[434] = 0;
- out_8547509152961498922[435] = 0;
- out_8547509152961498922[436] = 0;
- out_8547509152961498922[437] = 0;
- out_8547509152961498922[438] = 0;
- out_8547509152961498922[439] = 1.0;
- out_8547509152961498922[440] = 0;
- out_8547509152961498922[441] = 0;
- out_8547509152961498922[442] = 0;
- out_8547509152961498922[443] = 0;
- out_8547509152961498922[444] = 0;
- out_8547509152961498922[445] = 0;
- out_8547509152961498922[446] = 0;
- out_8547509152961498922[447] = 0;
- out_8547509152961498922[448] = 0;
- out_8547509152961498922[449] = 0;
- out_8547509152961498922[450] = 0;
- out_8547509152961498922[451] = 0;
- out_8547509152961498922[452] = 0;
- out_8547509152961498922[453] = 0;
- out_8547509152961498922[454] = 0;
- out_8547509152961498922[455] = 0;
- out_8547509152961498922[456] = 0;
- out_8547509152961498922[457] = 0;
- out_8547509152961498922[458] = 0;
- out_8547509152961498922[459] = 0;
- out_8547509152961498922[460] = 0;
- out_8547509152961498922[461] = 1.0;
+void H_mod_fun(double *state, double *out_341303559384211768) {
+ out_341303559384211768[0] = 1.0;
+ out_341303559384211768[1] = 0;
+ out_341303559384211768[2] = 0;
+ out_341303559384211768[3] = 0;
+ out_341303559384211768[4] = 0;
+ out_341303559384211768[5] = 0;
+ out_341303559384211768[6] = 0;
+ out_341303559384211768[7] = 0;
+ out_341303559384211768[8] = 0;
+ out_341303559384211768[9] = 0;
+ out_341303559384211768[10] = 0;
+ out_341303559384211768[11] = 0;
+ out_341303559384211768[12] = 0;
+ out_341303559384211768[13] = 0;
+ out_341303559384211768[14] = 0;
+ out_341303559384211768[15] = 0;
+ out_341303559384211768[16] = 0;
+ out_341303559384211768[17] = 0;
+ out_341303559384211768[18] = 0;
+ out_341303559384211768[19] = 0;
+ out_341303559384211768[20] = 0;
+ out_341303559384211768[21] = 0;
+ out_341303559384211768[22] = 1.0;
+ out_341303559384211768[23] = 0;
+ out_341303559384211768[24] = 0;
+ out_341303559384211768[25] = 0;
+ out_341303559384211768[26] = 0;
+ out_341303559384211768[27] = 0;
+ out_341303559384211768[28] = 0;
+ out_341303559384211768[29] = 0;
+ out_341303559384211768[30] = 0;
+ out_341303559384211768[31] = 0;
+ out_341303559384211768[32] = 0;
+ out_341303559384211768[33] = 0;
+ out_341303559384211768[34] = 0;
+ out_341303559384211768[35] = 0;
+ out_341303559384211768[36] = 0;
+ out_341303559384211768[37] = 0;
+ out_341303559384211768[38] = 0;
+ out_341303559384211768[39] = 0;
+ out_341303559384211768[40] = 0;
+ out_341303559384211768[41] = 0;
+ out_341303559384211768[42] = 0;
+ out_341303559384211768[43] = 0;
+ out_341303559384211768[44] = 1.0;
+ out_341303559384211768[45] = 0;
+ out_341303559384211768[46] = 0;
+ out_341303559384211768[47] = 0;
+ out_341303559384211768[48] = 0;
+ out_341303559384211768[49] = 0;
+ out_341303559384211768[50] = 0;
+ out_341303559384211768[51] = 0;
+ out_341303559384211768[52] = 0;
+ out_341303559384211768[53] = 0;
+ out_341303559384211768[54] = 0;
+ out_341303559384211768[55] = 0;
+ out_341303559384211768[56] = 0;
+ out_341303559384211768[57] = 0;
+ out_341303559384211768[58] = 0;
+ out_341303559384211768[59] = 0;
+ out_341303559384211768[60] = 0;
+ out_341303559384211768[61] = 0;
+ out_341303559384211768[62] = 0;
+ out_341303559384211768[63] = 0;
+ out_341303559384211768[64] = 0;
+ out_341303559384211768[65] = 0;
+ out_341303559384211768[66] = -0.5*state[4];
+ out_341303559384211768[67] = -0.5*state[5];
+ out_341303559384211768[68] = -0.5*state[6];
+ out_341303559384211768[69] = 0;
+ out_341303559384211768[70] = 0;
+ out_341303559384211768[71] = 0;
+ out_341303559384211768[72] = 0;
+ out_341303559384211768[73] = 0;
+ out_341303559384211768[74] = 0;
+ out_341303559384211768[75] = 0;
+ out_341303559384211768[76] = 0;
+ out_341303559384211768[77] = 0;
+ out_341303559384211768[78] = 0;
+ out_341303559384211768[79] = 0;
+ out_341303559384211768[80] = 0;
+ out_341303559384211768[81] = 0;
+ out_341303559384211768[82] = 0;
+ out_341303559384211768[83] = 0;
+ out_341303559384211768[84] = 0;
+ out_341303559384211768[85] = 0;
+ out_341303559384211768[86] = 0;
+ out_341303559384211768[87] = 0.5*state[3];
+ out_341303559384211768[88] = 0.5*state[6];
+ out_341303559384211768[89] = -0.5*state[5];
+ out_341303559384211768[90] = 0;
+ out_341303559384211768[91] = 0;
+ out_341303559384211768[92] = 0;
+ out_341303559384211768[93] = 0;
+ out_341303559384211768[94] = 0;
+ out_341303559384211768[95] = 0;
+ out_341303559384211768[96] = 0;
+ out_341303559384211768[97] = 0;
+ out_341303559384211768[98] = 0;
+ out_341303559384211768[99] = 0;
+ out_341303559384211768[100] = 0;
+ out_341303559384211768[101] = 0;
+ out_341303559384211768[102] = 0;
+ out_341303559384211768[103] = 0;
+ out_341303559384211768[104] = 0;
+ out_341303559384211768[105] = 0;
+ out_341303559384211768[106] = 0;
+ out_341303559384211768[107] = 0;
+ out_341303559384211768[108] = -0.5*state[6];
+ out_341303559384211768[109] = 0.5*state[3];
+ out_341303559384211768[110] = 0.5*state[4];
+ out_341303559384211768[111] = 0;
+ out_341303559384211768[112] = 0;
+ out_341303559384211768[113] = 0;
+ out_341303559384211768[114] = 0;
+ out_341303559384211768[115] = 0;
+ out_341303559384211768[116] = 0;
+ out_341303559384211768[117] = 0;
+ out_341303559384211768[118] = 0;
+ out_341303559384211768[119] = 0;
+ out_341303559384211768[120] = 0;
+ out_341303559384211768[121] = 0;
+ out_341303559384211768[122] = 0;
+ out_341303559384211768[123] = 0;
+ out_341303559384211768[124] = 0;
+ out_341303559384211768[125] = 0;
+ out_341303559384211768[126] = 0;
+ out_341303559384211768[127] = 0;
+ out_341303559384211768[128] = 0;
+ out_341303559384211768[129] = 0.5*state[5];
+ out_341303559384211768[130] = -0.5*state[4];
+ out_341303559384211768[131] = 0.5*state[3];
+ out_341303559384211768[132] = 0;
+ out_341303559384211768[133] = 0;
+ out_341303559384211768[134] = 0;
+ out_341303559384211768[135] = 0;
+ out_341303559384211768[136] = 0;
+ out_341303559384211768[137] = 0;
+ out_341303559384211768[138] = 0;
+ out_341303559384211768[139] = 0;
+ out_341303559384211768[140] = 0;
+ out_341303559384211768[141] = 0;
+ out_341303559384211768[142] = 0;
+ out_341303559384211768[143] = 0;
+ out_341303559384211768[144] = 0;
+ out_341303559384211768[145] = 0;
+ out_341303559384211768[146] = 0;
+ out_341303559384211768[147] = 0;
+ out_341303559384211768[148] = 0;
+ out_341303559384211768[149] = 0;
+ out_341303559384211768[150] = 0;
+ out_341303559384211768[151] = 0;
+ out_341303559384211768[152] = 0;
+ out_341303559384211768[153] = 1.0;
+ out_341303559384211768[154] = 0;
+ out_341303559384211768[155] = 0;
+ out_341303559384211768[156] = 0;
+ out_341303559384211768[157] = 0;
+ out_341303559384211768[158] = 0;
+ out_341303559384211768[159] = 0;
+ out_341303559384211768[160] = 0;
+ out_341303559384211768[161] = 0;
+ out_341303559384211768[162] = 0;
+ out_341303559384211768[163] = 0;
+ out_341303559384211768[164] = 0;
+ out_341303559384211768[165] = 0;
+ out_341303559384211768[166] = 0;
+ out_341303559384211768[167] = 0;
+ out_341303559384211768[168] = 0;
+ out_341303559384211768[169] = 0;
+ out_341303559384211768[170] = 0;
+ out_341303559384211768[171] = 0;
+ out_341303559384211768[172] = 0;
+ out_341303559384211768[173] = 0;
+ out_341303559384211768[174] = 0;
+ out_341303559384211768[175] = 1.0;
+ out_341303559384211768[176] = 0;
+ out_341303559384211768[177] = 0;
+ out_341303559384211768[178] = 0;
+ out_341303559384211768[179] = 0;
+ out_341303559384211768[180] = 0;
+ out_341303559384211768[181] = 0;
+ out_341303559384211768[182] = 0;
+ out_341303559384211768[183] = 0;
+ out_341303559384211768[184] = 0;
+ out_341303559384211768[185] = 0;
+ out_341303559384211768[186] = 0;
+ out_341303559384211768[187] = 0;
+ out_341303559384211768[188] = 0;
+ out_341303559384211768[189] = 0;
+ out_341303559384211768[190] = 0;
+ out_341303559384211768[191] = 0;
+ out_341303559384211768[192] = 0;
+ out_341303559384211768[193] = 0;
+ out_341303559384211768[194] = 0;
+ out_341303559384211768[195] = 0;
+ out_341303559384211768[196] = 0;
+ out_341303559384211768[197] = 1.0;
+ out_341303559384211768[198] = 0;
+ out_341303559384211768[199] = 0;
+ out_341303559384211768[200] = 0;
+ out_341303559384211768[201] = 0;
+ out_341303559384211768[202] = 0;
+ out_341303559384211768[203] = 0;
+ out_341303559384211768[204] = 0;
+ out_341303559384211768[205] = 0;
+ out_341303559384211768[206] = 0;
+ out_341303559384211768[207] = 0;
+ out_341303559384211768[208] = 0;
+ out_341303559384211768[209] = 0;
+ out_341303559384211768[210] = 0;
+ out_341303559384211768[211] = 0;
+ out_341303559384211768[212] = 0;
+ out_341303559384211768[213] = 0;
+ out_341303559384211768[214] = 0;
+ out_341303559384211768[215] = 0;
+ out_341303559384211768[216] = 0;
+ out_341303559384211768[217] = 0;
+ out_341303559384211768[218] = 0;
+ out_341303559384211768[219] = 1.0;
+ out_341303559384211768[220] = 0;
+ out_341303559384211768[221] = 0;
+ out_341303559384211768[222] = 0;
+ out_341303559384211768[223] = 0;
+ out_341303559384211768[224] = 0;
+ out_341303559384211768[225] = 0;
+ out_341303559384211768[226] = 0;
+ out_341303559384211768[227] = 0;
+ out_341303559384211768[228] = 0;
+ out_341303559384211768[229] = 0;
+ out_341303559384211768[230] = 0;
+ out_341303559384211768[231] = 0;
+ out_341303559384211768[232] = 0;
+ out_341303559384211768[233] = 0;
+ out_341303559384211768[234] = 0;
+ out_341303559384211768[235] = 0;
+ out_341303559384211768[236] = 0;
+ out_341303559384211768[237] = 0;
+ out_341303559384211768[238] = 0;
+ out_341303559384211768[239] = 0;
+ out_341303559384211768[240] = 0;
+ out_341303559384211768[241] = 1.0;
+ out_341303559384211768[242] = 0;
+ out_341303559384211768[243] = 0;
+ out_341303559384211768[244] = 0;
+ out_341303559384211768[245] = 0;
+ out_341303559384211768[246] = 0;
+ out_341303559384211768[247] = 0;
+ out_341303559384211768[248] = 0;
+ out_341303559384211768[249] = 0;
+ out_341303559384211768[250] = 0;
+ out_341303559384211768[251] = 0;
+ out_341303559384211768[252] = 0;
+ out_341303559384211768[253] = 0;
+ out_341303559384211768[254] = 0;
+ out_341303559384211768[255] = 0;
+ out_341303559384211768[256] = 0;
+ out_341303559384211768[257] = 0;
+ out_341303559384211768[258] = 0;
+ out_341303559384211768[259] = 0;
+ out_341303559384211768[260] = 0;
+ out_341303559384211768[261] = 0;
+ out_341303559384211768[262] = 0;
+ out_341303559384211768[263] = 1.0;
+ out_341303559384211768[264] = 0;
+ out_341303559384211768[265] = 0;
+ out_341303559384211768[266] = 0;
+ out_341303559384211768[267] = 0;
+ out_341303559384211768[268] = 0;
+ out_341303559384211768[269] = 0;
+ out_341303559384211768[270] = 0;
+ out_341303559384211768[271] = 0;
+ out_341303559384211768[272] = 0;
+ out_341303559384211768[273] = 0;
+ out_341303559384211768[274] = 0;
+ out_341303559384211768[275] = 0;
+ out_341303559384211768[276] = 0;
+ out_341303559384211768[277] = 0;
+ out_341303559384211768[278] = 0;
+ out_341303559384211768[279] = 0;
+ out_341303559384211768[280] = 0;
+ out_341303559384211768[281] = 0;
+ out_341303559384211768[282] = 0;
+ out_341303559384211768[283] = 0;
+ out_341303559384211768[284] = 0;
+ out_341303559384211768[285] = 1.0;
+ out_341303559384211768[286] = 0;
+ out_341303559384211768[287] = 0;
+ out_341303559384211768[288] = 0;
+ out_341303559384211768[289] = 0;
+ out_341303559384211768[290] = 0;
+ out_341303559384211768[291] = 0;
+ out_341303559384211768[292] = 0;
+ out_341303559384211768[293] = 0;
+ out_341303559384211768[294] = 0;
+ out_341303559384211768[295] = 0;
+ out_341303559384211768[296] = 0;
+ out_341303559384211768[297] = 0;
+ out_341303559384211768[298] = 0;
+ out_341303559384211768[299] = 0;
+ out_341303559384211768[300] = 0;
+ out_341303559384211768[301] = 0;
+ out_341303559384211768[302] = 0;
+ out_341303559384211768[303] = 0;
+ out_341303559384211768[304] = 0;
+ out_341303559384211768[305] = 0;
+ out_341303559384211768[306] = 0;
+ out_341303559384211768[307] = 1.0;
+ out_341303559384211768[308] = 0;
+ out_341303559384211768[309] = 0;
+ out_341303559384211768[310] = 0;
+ out_341303559384211768[311] = 0;
+ out_341303559384211768[312] = 0;
+ out_341303559384211768[313] = 0;
+ out_341303559384211768[314] = 0;
+ out_341303559384211768[315] = 0;
+ out_341303559384211768[316] = 0;
+ out_341303559384211768[317] = 0;
+ out_341303559384211768[318] = 0;
+ out_341303559384211768[319] = 0;
+ out_341303559384211768[320] = 0;
+ out_341303559384211768[321] = 0;
+ out_341303559384211768[322] = 0;
+ out_341303559384211768[323] = 0;
+ out_341303559384211768[324] = 0;
+ out_341303559384211768[325] = 0;
+ out_341303559384211768[326] = 0;
+ out_341303559384211768[327] = 0;
+ out_341303559384211768[328] = 0;
+ out_341303559384211768[329] = 1.0;
+ out_341303559384211768[330] = 0;
+ out_341303559384211768[331] = 0;
+ out_341303559384211768[332] = 0;
+ out_341303559384211768[333] = 0;
+ out_341303559384211768[334] = 0;
+ out_341303559384211768[335] = 0;
+ out_341303559384211768[336] = 0;
+ out_341303559384211768[337] = 0;
+ out_341303559384211768[338] = 0;
+ out_341303559384211768[339] = 0;
+ out_341303559384211768[340] = 0;
+ out_341303559384211768[341] = 0;
+ out_341303559384211768[342] = 0;
+ out_341303559384211768[343] = 0;
+ out_341303559384211768[344] = 0;
+ out_341303559384211768[345] = 0;
+ out_341303559384211768[346] = 0;
+ out_341303559384211768[347] = 0;
+ out_341303559384211768[348] = 0;
+ out_341303559384211768[349] = 0;
+ out_341303559384211768[350] = 0;
+ out_341303559384211768[351] = 1.0;
+ out_341303559384211768[352] = 0;
+ out_341303559384211768[353] = 0;
+ out_341303559384211768[354] = 0;
+ out_341303559384211768[355] = 0;
+ out_341303559384211768[356] = 0;
+ out_341303559384211768[357] = 0;
+ out_341303559384211768[358] = 0;
+ out_341303559384211768[359] = 0;
+ out_341303559384211768[360] = 0;
+ out_341303559384211768[361] = 0;
+ out_341303559384211768[362] = 0;
+ out_341303559384211768[363] = 0;
+ out_341303559384211768[364] = 0;
+ out_341303559384211768[365] = 0;
+ out_341303559384211768[366] = 0;
+ out_341303559384211768[367] = 0;
+ out_341303559384211768[368] = 0;
+ out_341303559384211768[369] = 0;
+ out_341303559384211768[370] = 0;
+ out_341303559384211768[371] = 0;
+ out_341303559384211768[372] = 0;
+ out_341303559384211768[373] = 1.0;
+ out_341303559384211768[374] = 0;
+ out_341303559384211768[375] = 0;
+ out_341303559384211768[376] = 0;
+ out_341303559384211768[377] = 0;
+ out_341303559384211768[378] = 0;
+ out_341303559384211768[379] = 0;
+ out_341303559384211768[380] = 0;
+ out_341303559384211768[381] = 0;
+ out_341303559384211768[382] = 0;
+ out_341303559384211768[383] = 0;
+ out_341303559384211768[384] = 0;
+ out_341303559384211768[385] = 0;
+ out_341303559384211768[386] = 0;
+ out_341303559384211768[387] = 0;
+ out_341303559384211768[388] = 0;
+ out_341303559384211768[389] = 0;
+ out_341303559384211768[390] = 0;
+ out_341303559384211768[391] = 0;
+ out_341303559384211768[392] = 0;
+ out_341303559384211768[393] = 0;
+ out_341303559384211768[394] = 0;
+ out_341303559384211768[395] = 1.0;
+ out_341303559384211768[396] = 0;
+ out_341303559384211768[397] = 0;
+ out_341303559384211768[398] = 0;
+ out_341303559384211768[399] = 0;
+ out_341303559384211768[400] = 0;
+ out_341303559384211768[401] = 0;
+ out_341303559384211768[402] = 0;
+ out_341303559384211768[403] = 0;
+ out_341303559384211768[404] = 0;
+ out_341303559384211768[405] = 0;
+ out_341303559384211768[406] = 0;
+ out_341303559384211768[407] = 0;
+ out_341303559384211768[408] = 0;
+ out_341303559384211768[409] = 0;
+ out_341303559384211768[410] = 0;
+ out_341303559384211768[411] = 0;
+ out_341303559384211768[412] = 0;
+ out_341303559384211768[413] = 0;
+ out_341303559384211768[414] = 0;
+ out_341303559384211768[415] = 0;
+ out_341303559384211768[416] = 0;
+ out_341303559384211768[417] = 1.0;
+ out_341303559384211768[418] = 0;
+ out_341303559384211768[419] = 0;
+ out_341303559384211768[420] = 0;
+ out_341303559384211768[421] = 0;
+ out_341303559384211768[422] = 0;
+ out_341303559384211768[423] = 0;
+ out_341303559384211768[424] = 0;
+ out_341303559384211768[425] = 0;
+ out_341303559384211768[426] = 0;
+ out_341303559384211768[427] = 0;
+ out_341303559384211768[428] = 0;
+ out_341303559384211768[429] = 0;
+ out_341303559384211768[430] = 0;
+ out_341303559384211768[431] = 0;
+ out_341303559384211768[432] = 0;
+ out_341303559384211768[433] = 0;
+ out_341303559384211768[434] = 0;
+ out_341303559384211768[435] = 0;
+ out_341303559384211768[436] = 0;
+ out_341303559384211768[437] = 0;
+ out_341303559384211768[438] = 0;
+ out_341303559384211768[439] = 1.0;
+ out_341303559384211768[440] = 0;
+ out_341303559384211768[441] = 0;
+ out_341303559384211768[442] = 0;
+ out_341303559384211768[443] = 0;
+ out_341303559384211768[444] = 0;
+ out_341303559384211768[445] = 0;
+ out_341303559384211768[446] = 0;
+ out_341303559384211768[447] = 0;
+ out_341303559384211768[448] = 0;
+ out_341303559384211768[449] = 0;
+ out_341303559384211768[450] = 0;
+ out_341303559384211768[451] = 0;
+ out_341303559384211768[452] = 0;
+ out_341303559384211768[453] = 0;
+ out_341303559384211768[454] = 0;
+ out_341303559384211768[455] = 0;
+ out_341303559384211768[456] = 0;
+ out_341303559384211768[457] = 0;
+ out_341303559384211768[458] = 0;
+ out_341303559384211768[459] = 0;
+ out_341303559384211768[460] = 0;
+ out_341303559384211768[461] = 1.0;
}
-void f_fun(double *state, double dt, double *out_8100393315288221642) {
- out_8100393315288221642[0] = dt*state[7] + state[0];
- out_8100393315288221642[1] = dt*state[8] + state[1];
- out_8100393315288221642[2] = dt*state[9] + state[2];
- out_8100393315288221642[3] = dt*(-0.5*state[4]*state[10] - 0.5*state[5]*state[11] - 0.5*state[6]*state[12]) + state[3];
- out_8100393315288221642[4] = dt*(0.5*state[3]*state[10] + 0.5*state[5]*state[12] - 0.5*state[6]*state[11]) + state[4];
- out_8100393315288221642[5] = dt*(0.5*state[3]*state[11] - 0.5*state[4]*state[12] + 0.5*state[6]*state[10]) + state[5];
- out_8100393315288221642[6] = dt*(0.5*state[3]*state[12] + 0.5*state[4]*state[11] - 0.5*state[5]*state[10]) + state[6];
- out_8100393315288221642[7] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[18] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[17] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[16]) + state[7];
- out_8100393315288221642[8] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[18] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[16] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[17]) + state[8];
- out_8100393315288221642[9] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[17] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[16] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[18]) + state[9];
- out_8100393315288221642[10] = state[10];
- out_8100393315288221642[11] = state[11];
- out_8100393315288221642[12] = state[12];
- out_8100393315288221642[13] = state[13];
- out_8100393315288221642[14] = state[14];
- out_8100393315288221642[15] = state[15];
- out_8100393315288221642[16] = state[16];
- out_8100393315288221642[17] = state[17];
- out_8100393315288221642[18] = state[18];
- out_8100393315288221642[19] = state[19];
- out_8100393315288221642[20] = state[20];
- out_8100393315288221642[21] = state[21];
+void f_fun(double *state, double dt, double *out_4549525036970607503) {
+ out_4549525036970607503[0] = dt*state[7] + state[0];
+ out_4549525036970607503[1] = dt*state[8] + state[1];
+ out_4549525036970607503[2] = dt*state[9] + state[2];
+ out_4549525036970607503[3] = dt*(-0.5*state[4]*state[10] - 0.5*state[5]*state[11] - 0.5*state[6]*state[12]) + state[3];
+ out_4549525036970607503[4] = dt*(0.5*state[3]*state[10] + 0.5*state[5]*state[12] - 0.5*state[6]*state[11]) + state[4];
+ out_4549525036970607503[5] = dt*(0.5*state[3]*state[11] - 0.5*state[4]*state[12] + 0.5*state[6]*state[10]) + state[5];
+ out_4549525036970607503[6] = dt*(0.5*state[3]*state[12] + 0.5*state[4]*state[11] - 0.5*state[5]*state[10]) + state[6];
+ out_4549525036970607503[7] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[18] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[17] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[16]) + state[7];
+ out_4549525036970607503[8] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[18] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[16] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[17]) + state[8];
+ out_4549525036970607503[9] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[17] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[16] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[18]) + state[9];
+ out_4549525036970607503[10] = state[10];
+ out_4549525036970607503[11] = state[11];
+ out_4549525036970607503[12] = state[12];
+ out_4549525036970607503[13] = state[13];
+ out_4549525036970607503[14] = state[14];
+ out_4549525036970607503[15] = state[15];
+ out_4549525036970607503[16] = state[16];
+ out_4549525036970607503[17] = state[17];
+ out_4549525036970607503[18] = state[18];
+ out_4549525036970607503[19] = state[19];
+ out_4549525036970607503[20] = state[20];
+ out_4549525036970607503[21] = state[21];
}
-void F_fun(double *state, double dt, double *out_7769089323050102648) {
- out_7769089323050102648[0] = 1;
- out_7769089323050102648[1] = 0;
- out_7769089323050102648[2] = 0;
- out_7769089323050102648[3] = 0;
- out_7769089323050102648[4] = 0;
- out_7769089323050102648[5] = 0;
- out_7769089323050102648[6] = dt;
- out_7769089323050102648[7] = 0;
- out_7769089323050102648[8] = 0;
- out_7769089323050102648[9] = 0;
- out_7769089323050102648[10] = 0;
- out_7769089323050102648[11] = 0;
- out_7769089323050102648[12] = 0;
- out_7769089323050102648[13] = 0;
- out_7769089323050102648[14] = 0;
- out_7769089323050102648[15] = 0;
- out_7769089323050102648[16] = 0;
- out_7769089323050102648[17] = 0;
- out_7769089323050102648[18] = 0;
- out_7769089323050102648[19] = 0;
- out_7769089323050102648[20] = 0;
- out_7769089323050102648[21] = 0;
- out_7769089323050102648[22] = 1;
- out_7769089323050102648[23] = 0;
- out_7769089323050102648[24] = 0;
- out_7769089323050102648[25] = 0;
- out_7769089323050102648[26] = 0;
- out_7769089323050102648[27] = 0;
- out_7769089323050102648[28] = dt;
- out_7769089323050102648[29] = 0;
- out_7769089323050102648[30] = 0;
- out_7769089323050102648[31] = 0;
- out_7769089323050102648[32] = 0;
- out_7769089323050102648[33] = 0;
- out_7769089323050102648[34] = 0;
- out_7769089323050102648[35] = 0;
- out_7769089323050102648[36] = 0;
- out_7769089323050102648[37] = 0;
- out_7769089323050102648[38] = 0;
- out_7769089323050102648[39] = 0;
- out_7769089323050102648[40] = 0;
- out_7769089323050102648[41] = 0;
- out_7769089323050102648[42] = 0;
- out_7769089323050102648[43] = 0;
- out_7769089323050102648[44] = 1;
- out_7769089323050102648[45] = 0;
- out_7769089323050102648[46] = 0;
- out_7769089323050102648[47] = 0;
- out_7769089323050102648[48] = 0;
- out_7769089323050102648[49] = 0;
- out_7769089323050102648[50] = dt;
- out_7769089323050102648[51] = 0;
- out_7769089323050102648[52] = 0;
- out_7769089323050102648[53] = 0;
- out_7769089323050102648[54] = 0;
- out_7769089323050102648[55] = 0;
- out_7769089323050102648[56] = 0;
- out_7769089323050102648[57] = 0;
- out_7769089323050102648[58] = 0;
- out_7769089323050102648[59] = 0;
- out_7769089323050102648[60] = 0;
- out_7769089323050102648[61] = 0;
- out_7769089323050102648[62] = 0;
- out_7769089323050102648[63] = 0;
- out_7769089323050102648[64] = 0;
- out_7769089323050102648[65] = 0;
- out_7769089323050102648[66] = 1;
- out_7769089323050102648[67] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[11] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[10] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[12]);
- out_7769089323050102648[68] = dt*((2*state[3]*state[4] - 2*state[5]*state[6])*state[12] + (-2*state[3]*state[6] - 2*state[4]*state[5])*state[10] + (-pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[11]);
- out_7769089323050102648[69] = 0;
- out_7769089323050102648[70] = 0;
- out_7769089323050102648[71] = 0;
- out_7769089323050102648[72] = dt*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2));
- out_7769089323050102648[73] = dt*(-2*state[3]*state[6] + 2*state[4]*state[5]);
- out_7769089323050102648[74] = dt*(2*state[3]*state[5] + 2*state[4]*state[6]);
- out_7769089323050102648[75] = 0;
- out_7769089323050102648[76] = 0;
- out_7769089323050102648[77] = 0;
- out_7769089323050102648[78] = 0;
- out_7769089323050102648[79] = 0;
- out_7769089323050102648[80] = 0;
- out_7769089323050102648[81] = 0;
- out_7769089323050102648[82] = 0;
- out_7769089323050102648[83] = 0;
- out_7769089323050102648[84] = 0;
- out_7769089323050102648[85] = 0;
- out_7769089323050102648[86] = 0;
- out_7769089323050102648[87] = dt*(-(2*state[3]*state[4] + 2*state[5]*state[6])*state[11] - (-2*state[3]*state[5] + 2*state[4]*state[6])*state[10] - (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[12]);
- out_7769089323050102648[88] = 1;
- out_7769089323050102648[89] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[12] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[11] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[10]);
- out_7769089323050102648[90] = 0;
- out_7769089323050102648[91] = 0;
- out_7769089323050102648[92] = 0;
- out_7769089323050102648[93] = dt*(2*state[3]*state[6] + 2*state[4]*state[5]);
- out_7769089323050102648[94] = dt*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2));
- out_7769089323050102648[95] = dt*(-2*state[3]*state[4] + 2*state[5]*state[6]);
- out_7769089323050102648[96] = 0;
- out_7769089323050102648[97] = 0;
- out_7769089323050102648[98] = 0;
- out_7769089323050102648[99] = 0;
- out_7769089323050102648[100] = 0;
- out_7769089323050102648[101] = 0;
- out_7769089323050102648[102] = 0;
- out_7769089323050102648[103] = 0;
- out_7769089323050102648[104] = 0;
- out_7769089323050102648[105] = 0;
- out_7769089323050102648[106] = 0;
- out_7769089323050102648[107] = 0;
- out_7769089323050102648[108] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[12] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[10] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[11]);
- out_7769089323050102648[109] = dt*((-2*state[3]*state[5] - 2*state[4]*state[6])*state[12] + (2*state[3]*state[6] - 2*state[4]*state[5])*state[11] + (-pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) + pow(state[6], 2))*state[10]);
- out_7769089323050102648[110] = 1;
- out_7769089323050102648[111] = 0;
- out_7769089323050102648[112] = 0;
- out_7769089323050102648[113] = 0;
- out_7769089323050102648[114] = dt*(-2*state[3]*state[5] + 2*state[4]*state[6]);
- out_7769089323050102648[115] = dt*(2*state[3]*state[4] + 2*state[5]*state[6]);
- out_7769089323050102648[116] = dt*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2));
- out_7769089323050102648[117] = 0;
- out_7769089323050102648[118] = 0;
- out_7769089323050102648[119] = 0;
- out_7769089323050102648[120] = 0;
- out_7769089323050102648[121] = 0;
- out_7769089323050102648[122] = 0;
- out_7769089323050102648[123] = 0;
- out_7769089323050102648[124] = 0;
- out_7769089323050102648[125] = 0;
- out_7769089323050102648[126] = 0;
- out_7769089323050102648[127] = 0;
- out_7769089323050102648[128] = 0;
- out_7769089323050102648[129] = 0;
- out_7769089323050102648[130] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[17] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[16] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[18]);
- out_7769089323050102648[131] = dt*((2*state[3]*state[4] - 2*state[5]*state[6])*state[18] + (-2*state[3]*state[6] - 2*state[4]*state[5])*state[16] + (-pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[17]);
- out_7769089323050102648[132] = 1;
- out_7769089323050102648[133] = 0;
- out_7769089323050102648[134] = 0;
- out_7769089323050102648[135] = 0;
- out_7769089323050102648[136] = 0;
- out_7769089323050102648[137] = 0;
- out_7769089323050102648[138] = 0;
- out_7769089323050102648[139] = 0;
- out_7769089323050102648[140] = 0;
- out_7769089323050102648[141] = dt*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2));
- out_7769089323050102648[142] = dt*(-2*state[3]*state[6] + 2*state[4]*state[5]);
- out_7769089323050102648[143] = dt*(2*state[3]*state[5] + 2*state[4]*state[6]);
- out_7769089323050102648[144] = 0;
- out_7769089323050102648[145] = 0;
- out_7769089323050102648[146] = 0;
- out_7769089323050102648[147] = 0;
- out_7769089323050102648[148] = 0;
- out_7769089323050102648[149] = 0;
- out_7769089323050102648[150] = dt*(-(2*state[3]*state[4] + 2*state[5]*state[6])*state[17] - (-2*state[3]*state[5] + 2*state[4]*state[6])*state[16] - (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[18]);
- out_7769089323050102648[151] = 0;
- out_7769089323050102648[152] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[18] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[17] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[16]);
- out_7769089323050102648[153] = 0;
- out_7769089323050102648[154] = 1;
- out_7769089323050102648[155] = 0;
- out_7769089323050102648[156] = 0;
- out_7769089323050102648[157] = 0;
- out_7769089323050102648[158] = 0;
- out_7769089323050102648[159] = 0;
- out_7769089323050102648[160] = 0;
- out_7769089323050102648[161] = 0;
- out_7769089323050102648[162] = dt*(2*state[3]*state[6] + 2*state[4]*state[5]);
- out_7769089323050102648[163] = dt*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2));
- out_7769089323050102648[164] = dt*(-2*state[3]*state[4] + 2*state[5]*state[6]);
- out_7769089323050102648[165] = 0;
- out_7769089323050102648[166] = 0;
- out_7769089323050102648[167] = 0;
- out_7769089323050102648[168] = 0;
- out_7769089323050102648[169] = 0;
- out_7769089323050102648[170] = 0;
- out_7769089323050102648[171] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[18] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[16] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[17]);
- out_7769089323050102648[172] = dt*((-2*state[3]*state[5] - 2*state[4]*state[6])*state[18] + (2*state[3]*state[6] - 2*state[4]*state[5])*state[17] + (-pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) + pow(state[6], 2))*state[16]);
- out_7769089323050102648[173] = 0;
- out_7769089323050102648[174] = 0;
- out_7769089323050102648[175] = 0;
- out_7769089323050102648[176] = 1;
- out_7769089323050102648[177] = 0;
- out_7769089323050102648[178] = 0;
- out_7769089323050102648[179] = 0;
- out_7769089323050102648[180] = 0;
- out_7769089323050102648[181] = 0;
- out_7769089323050102648[182] = 0;
- out_7769089323050102648[183] = dt*(-2*state[3]*state[5] + 2*state[4]*state[6]);
- out_7769089323050102648[184] = dt*(2*state[3]*state[4] + 2*state[5]*state[6]);
- out_7769089323050102648[185] = dt*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2));
- out_7769089323050102648[186] = 0;
- out_7769089323050102648[187] = 0;
- out_7769089323050102648[188] = 0;
- out_7769089323050102648[189] = 0;
- out_7769089323050102648[190] = 0;
- out_7769089323050102648[191] = 0;
- out_7769089323050102648[192] = 0;
- out_7769089323050102648[193] = 0;
- out_7769089323050102648[194] = 0;
- out_7769089323050102648[195] = 0;
- out_7769089323050102648[196] = 0;
- out_7769089323050102648[197] = 0;
- out_7769089323050102648[198] = 1;
- out_7769089323050102648[199] = 0;
- out_7769089323050102648[200] = 0;
- out_7769089323050102648[201] = 0;
- out_7769089323050102648[202] = 0;
- out_7769089323050102648[203] = 0;
- out_7769089323050102648[204] = 0;
- out_7769089323050102648[205] = 0;
- out_7769089323050102648[206] = 0;
- out_7769089323050102648[207] = 0;
- out_7769089323050102648[208] = 0;
- out_7769089323050102648[209] = 0;
- out_7769089323050102648[210] = 0;
- out_7769089323050102648[211] = 0;
- out_7769089323050102648[212] = 0;
- out_7769089323050102648[213] = 0;
- out_7769089323050102648[214] = 0;
- out_7769089323050102648[215] = 0;
- out_7769089323050102648[216] = 0;
- out_7769089323050102648[217] = 0;
- out_7769089323050102648[218] = 0;
- out_7769089323050102648[219] = 0;
- out_7769089323050102648[220] = 1;
- out_7769089323050102648[221] = 0;
- out_7769089323050102648[222] = 0;
- out_7769089323050102648[223] = 0;
- out_7769089323050102648[224] = 0;
- out_7769089323050102648[225] = 0;
- out_7769089323050102648[226] = 0;
- out_7769089323050102648[227] = 0;
- out_7769089323050102648[228] = 0;
- out_7769089323050102648[229] = 0;
- out_7769089323050102648[230] = 0;
- out_7769089323050102648[231] = 0;
- out_7769089323050102648[232] = 0;
- out_7769089323050102648[233] = 0;
- out_7769089323050102648[234] = 0;
- out_7769089323050102648[235] = 0;
- out_7769089323050102648[236] = 0;
- out_7769089323050102648[237] = 0;
- out_7769089323050102648[238] = 0;
- out_7769089323050102648[239] = 0;
- out_7769089323050102648[240] = 0;
- out_7769089323050102648[241] = 0;
- out_7769089323050102648[242] = 1;
- out_7769089323050102648[243] = 0;
- out_7769089323050102648[244] = 0;
- out_7769089323050102648[245] = 0;
- out_7769089323050102648[246] = 0;
- out_7769089323050102648[247] = 0;
- out_7769089323050102648[248] = 0;
- out_7769089323050102648[249] = 0;
- out_7769089323050102648[250] = 0;
- out_7769089323050102648[251] = 0;
- out_7769089323050102648[252] = 0;
- out_7769089323050102648[253] = 0;
- out_7769089323050102648[254] = 0;
- out_7769089323050102648[255] = 0;
- out_7769089323050102648[256] = 0;
- out_7769089323050102648[257] = 0;
- out_7769089323050102648[258] = 0;
- out_7769089323050102648[259] = 0;
- out_7769089323050102648[260] = 0;
- out_7769089323050102648[261] = 0;
- out_7769089323050102648[262] = 0;
- out_7769089323050102648[263] = 0;
- out_7769089323050102648[264] = 1;
- out_7769089323050102648[265] = 0;
- out_7769089323050102648[266] = 0;
- out_7769089323050102648[267] = 0;
- out_7769089323050102648[268] = 0;
- out_7769089323050102648[269] = 0;
- out_7769089323050102648[270] = 0;
- out_7769089323050102648[271] = 0;
- out_7769089323050102648[272] = 0;
- out_7769089323050102648[273] = 0;
- out_7769089323050102648[274] = 0;
- out_7769089323050102648[275] = 0;
- out_7769089323050102648[276] = 0;
- out_7769089323050102648[277] = 0;
- out_7769089323050102648[278] = 0;
- out_7769089323050102648[279] = 0;
- out_7769089323050102648[280] = 0;
- out_7769089323050102648[281] = 0;
- out_7769089323050102648[282] = 0;
- out_7769089323050102648[283] = 0;
- out_7769089323050102648[284] = 0;
- out_7769089323050102648[285] = 0;
- out_7769089323050102648[286] = 1;
- out_7769089323050102648[287] = 0;
- out_7769089323050102648[288] = 0;
- out_7769089323050102648[289] = 0;
- out_7769089323050102648[290] = 0;
- out_7769089323050102648[291] = 0;
- out_7769089323050102648[292] = 0;
- out_7769089323050102648[293] = 0;
- out_7769089323050102648[294] = 0;
- out_7769089323050102648[295] = 0;
- out_7769089323050102648[296] = 0;
- out_7769089323050102648[297] = 0;
- out_7769089323050102648[298] = 0;
- out_7769089323050102648[299] = 0;
- out_7769089323050102648[300] = 0;
- out_7769089323050102648[301] = 0;
- out_7769089323050102648[302] = 0;
- out_7769089323050102648[303] = 0;
- out_7769089323050102648[304] = 0;
- out_7769089323050102648[305] = 0;
- out_7769089323050102648[306] = 0;
- out_7769089323050102648[307] = 0;
- out_7769089323050102648[308] = 1;
- out_7769089323050102648[309] = 0;
- out_7769089323050102648[310] = 0;
- out_7769089323050102648[311] = 0;
- out_7769089323050102648[312] = 0;
- out_7769089323050102648[313] = 0;
- out_7769089323050102648[314] = 0;
- out_7769089323050102648[315] = 0;
- out_7769089323050102648[316] = 0;
- out_7769089323050102648[317] = 0;
- out_7769089323050102648[318] = 0;
- out_7769089323050102648[319] = 0;
- out_7769089323050102648[320] = 0;
- out_7769089323050102648[321] = 0;
- out_7769089323050102648[322] = 0;
- out_7769089323050102648[323] = 0;
- out_7769089323050102648[324] = 0;
- out_7769089323050102648[325] = 0;
- out_7769089323050102648[326] = 0;
- out_7769089323050102648[327] = 0;
- out_7769089323050102648[328] = 0;
- out_7769089323050102648[329] = 0;
- out_7769089323050102648[330] = 1;
- out_7769089323050102648[331] = 0;
- out_7769089323050102648[332] = 0;
- out_7769089323050102648[333] = 0;
- out_7769089323050102648[334] = 0;
- out_7769089323050102648[335] = 0;
- out_7769089323050102648[336] = 0;
- out_7769089323050102648[337] = 0;
- out_7769089323050102648[338] = 0;
- out_7769089323050102648[339] = 0;
- out_7769089323050102648[340] = 0;
- out_7769089323050102648[341] = 0;
- out_7769089323050102648[342] = 0;
- out_7769089323050102648[343] = 0;
- out_7769089323050102648[344] = 0;
- out_7769089323050102648[345] = 0;
- out_7769089323050102648[346] = 0;
- out_7769089323050102648[347] = 0;
- out_7769089323050102648[348] = 0;
- out_7769089323050102648[349] = 0;
- out_7769089323050102648[350] = 0;
- out_7769089323050102648[351] = 0;
- out_7769089323050102648[352] = 1;
- out_7769089323050102648[353] = 0;
- out_7769089323050102648[354] = 0;
- out_7769089323050102648[355] = 0;
- out_7769089323050102648[356] = 0;
- out_7769089323050102648[357] = 0;
- out_7769089323050102648[358] = 0;
- out_7769089323050102648[359] = 0;
- out_7769089323050102648[360] = 0;
- out_7769089323050102648[361] = 0;
- out_7769089323050102648[362] = 0;
- out_7769089323050102648[363] = 0;
- out_7769089323050102648[364] = 0;
- out_7769089323050102648[365] = 0;
- out_7769089323050102648[366] = 0;
- out_7769089323050102648[367] = 0;
- out_7769089323050102648[368] = 0;
- out_7769089323050102648[369] = 0;
- out_7769089323050102648[370] = 0;
- out_7769089323050102648[371] = 0;
- out_7769089323050102648[372] = 0;
- out_7769089323050102648[373] = 0;
- out_7769089323050102648[374] = 1;
- out_7769089323050102648[375] = 0;
- out_7769089323050102648[376] = 0;
- out_7769089323050102648[377] = 0;
- out_7769089323050102648[378] = 0;
- out_7769089323050102648[379] = 0;
- out_7769089323050102648[380] = 0;
- out_7769089323050102648[381] = 0;
- out_7769089323050102648[382] = 0;
- out_7769089323050102648[383] = 0;
- out_7769089323050102648[384] = 0;
- out_7769089323050102648[385] = 0;
- out_7769089323050102648[386] = 0;
- out_7769089323050102648[387] = 0;
- out_7769089323050102648[388] = 0;
- out_7769089323050102648[389] = 0;
- out_7769089323050102648[390] = 0;
- out_7769089323050102648[391] = 0;
- out_7769089323050102648[392] = 0;
- out_7769089323050102648[393] = 0;
- out_7769089323050102648[394] = 0;
- out_7769089323050102648[395] = 0;
- out_7769089323050102648[396] = 1;
- out_7769089323050102648[397] = 0;
- out_7769089323050102648[398] = 0;
- out_7769089323050102648[399] = 0;
- out_7769089323050102648[400] = 0;
- out_7769089323050102648[401] = 0;
- out_7769089323050102648[402] = 0;
- out_7769089323050102648[403] = 0;
- out_7769089323050102648[404] = 0;
- out_7769089323050102648[405] = 0;
- out_7769089323050102648[406] = 0;
- out_7769089323050102648[407] = 0;
- out_7769089323050102648[408] = 0;
- out_7769089323050102648[409] = 0;
- out_7769089323050102648[410] = 0;
- out_7769089323050102648[411] = 0;
- out_7769089323050102648[412] = 0;
- out_7769089323050102648[413] = 0;
- out_7769089323050102648[414] = 0;
- out_7769089323050102648[415] = 0;
- out_7769089323050102648[416] = 0;
- out_7769089323050102648[417] = 0;
- out_7769089323050102648[418] = 1;
- out_7769089323050102648[419] = 0;
- out_7769089323050102648[420] = 0;
- out_7769089323050102648[421] = 0;
- out_7769089323050102648[422] = 0;
- out_7769089323050102648[423] = 0;
- out_7769089323050102648[424] = 0;
- out_7769089323050102648[425] = 0;
- out_7769089323050102648[426] = 0;
- out_7769089323050102648[427] = 0;
- out_7769089323050102648[428] = 0;
- out_7769089323050102648[429] = 0;
- out_7769089323050102648[430] = 0;
- out_7769089323050102648[431] = 0;
- out_7769089323050102648[432] = 0;
- out_7769089323050102648[433] = 0;
- out_7769089323050102648[434] = 0;
- out_7769089323050102648[435] = 0;
- out_7769089323050102648[436] = 0;
- out_7769089323050102648[437] = 0;
- out_7769089323050102648[438] = 0;
- out_7769089323050102648[439] = 0;
- out_7769089323050102648[440] = 1;
+void F_fun(double *state, double dt, double *out_5133759507898990925) {
+ out_5133759507898990925[0] = 1;
+ out_5133759507898990925[1] = 0;
+ out_5133759507898990925[2] = 0;
+ out_5133759507898990925[3] = 0;
+ out_5133759507898990925[4] = 0;
+ out_5133759507898990925[5] = 0;
+ out_5133759507898990925[6] = dt;
+ out_5133759507898990925[7] = 0;
+ out_5133759507898990925[8] = 0;
+ out_5133759507898990925[9] = 0;
+ out_5133759507898990925[10] = 0;
+ out_5133759507898990925[11] = 0;
+ out_5133759507898990925[12] = 0;
+ out_5133759507898990925[13] = 0;
+ out_5133759507898990925[14] = 0;
+ out_5133759507898990925[15] = 0;
+ out_5133759507898990925[16] = 0;
+ out_5133759507898990925[17] = 0;
+ out_5133759507898990925[18] = 0;
+ out_5133759507898990925[19] = 0;
+ out_5133759507898990925[20] = 0;
+ out_5133759507898990925[21] = 0;
+ out_5133759507898990925[22] = 1;
+ out_5133759507898990925[23] = 0;
+ out_5133759507898990925[24] = 0;
+ out_5133759507898990925[25] = 0;
+ out_5133759507898990925[26] = 0;
+ out_5133759507898990925[27] = 0;
+ out_5133759507898990925[28] = dt;
+ out_5133759507898990925[29] = 0;
+ out_5133759507898990925[30] = 0;
+ out_5133759507898990925[31] = 0;
+ out_5133759507898990925[32] = 0;
+ out_5133759507898990925[33] = 0;
+ out_5133759507898990925[34] = 0;
+ out_5133759507898990925[35] = 0;
+ out_5133759507898990925[36] = 0;
+ out_5133759507898990925[37] = 0;
+ out_5133759507898990925[38] = 0;
+ out_5133759507898990925[39] = 0;
+ out_5133759507898990925[40] = 0;
+ out_5133759507898990925[41] = 0;
+ out_5133759507898990925[42] = 0;
+ out_5133759507898990925[43] = 0;
+ out_5133759507898990925[44] = 1;
+ out_5133759507898990925[45] = 0;
+ out_5133759507898990925[46] = 0;
+ out_5133759507898990925[47] = 0;
+ out_5133759507898990925[48] = 0;
+ out_5133759507898990925[49] = 0;
+ out_5133759507898990925[50] = dt;
+ out_5133759507898990925[51] = 0;
+ out_5133759507898990925[52] = 0;
+ out_5133759507898990925[53] = 0;
+ out_5133759507898990925[54] = 0;
+ out_5133759507898990925[55] = 0;
+ out_5133759507898990925[56] = 0;
+ out_5133759507898990925[57] = 0;
+ out_5133759507898990925[58] = 0;
+ out_5133759507898990925[59] = 0;
+ out_5133759507898990925[60] = 0;
+ out_5133759507898990925[61] = 0;
+ out_5133759507898990925[62] = 0;
+ out_5133759507898990925[63] = 0;
+ out_5133759507898990925[64] = 0;
+ out_5133759507898990925[65] = 0;
+ out_5133759507898990925[66] = 1;
+ out_5133759507898990925[67] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[11] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[10] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[12]);
+ out_5133759507898990925[68] = dt*((2*state[3]*state[4] - 2*state[5]*state[6])*state[12] + (-2*state[3]*state[6] - 2*state[4]*state[5])*state[10] + (-pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[11]);
+ out_5133759507898990925[69] = 0;
+ out_5133759507898990925[70] = 0;
+ out_5133759507898990925[71] = 0;
+ out_5133759507898990925[72] = dt*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2));
+ out_5133759507898990925[73] = dt*(-2*state[3]*state[6] + 2*state[4]*state[5]);
+ out_5133759507898990925[74] = dt*(2*state[3]*state[5] + 2*state[4]*state[6]);
+ out_5133759507898990925[75] = 0;
+ out_5133759507898990925[76] = 0;
+ out_5133759507898990925[77] = 0;
+ out_5133759507898990925[78] = 0;
+ out_5133759507898990925[79] = 0;
+ out_5133759507898990925[80] = 0;
+ out_5133759507898990925[81] = 0;
+ out_5133759507898990925[82] = 0;
+ out_5133759507898990925[83] = 0;
+ out_5133759507898990925[84] = 0;
+ out_5133759507898990925[85] = 0;
+ out_5133759507898990925[86] = 0;
+ out_5133759507898990925[87] = dt*(-(2*state[3]*state[4] + 2*state[5]*state[6])*state[11] - (-2*state[3]*state[5] + 2*state[4]*state[6])*state[10] - (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[12]);
+ out_5133759507898990925[88] = 1;
+ out_5133759507898990925[89] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[12] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[11] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[10]);
+ out_5133759507898990925[90] = 0;
+ out_5133759507898990925[91] = 0;
+ out_5133759507898990925[92] = 0;
+ out_5133759507898990925[93] = dt*(2*state[3]*state[6] + 2*state[4]*state[5]);
+ out_5133759507898990925[94] = dt*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2));
+ out_5133759507898990925[95] = dt*(-2*state[3]*state[4] + 2*state[5]*state[6]);
+ out_5133759507898990925[96] = 0;
+ out_5133759507898990925[97] = 0;
+ out_5133759507898990925[98] = 0;
+ out_5133759507898990925[99] = 0;
+ out_5133759507898990925[100] = 0;
+ out_5133759507898990925[101] = 0;
+ out_5133759507898990925[102] = 0;
+ out_5133759507898990925[103] = 0;
+ out_5133759507898990925[104] = 0;
+ out_5133759507898990925[105] = 0;
+ out_5133759507898990925[106] = 0;
+ out_5133759507898990925[107] = 0;
+ out_5133759507898990925[108] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[12] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[10] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[11]);
+ out_5133759507898990925[109] = dt*((-2*state[3]*state[5] - 2*state[4]*state[6])*state[12] + (2*state[3]*state[6] - 2*state[4]*state[5])*state[11] + (-pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) + pow(state[6], 2))*state[10]);
+ out_5133759507898990925[110] = 1;
+ out_5133759507898990925[111] = 0;
+ out_5133759507898990925[112] = 0;
+ out_5133759507898990925[113] = 0;
+ out_5133759507898990925[114] = dt*(-2*state[3]*state[5] + 2*state[4]*state[6]);
+ out_5133759507898990925[115] = dt*(2*state[3]*state[4] + 2*state[5]*state[6]);
+ out_5133759507898990925[116] = dt*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2));
+ out_5133759507898990925[117] = 0;
+ out_5133759507898990925[118] = 0;
+ out_5133759507898990925[119] = 0;
+ out_5133759507898990925[120] = 0;
+ out_5133759507898990925[121] = 0;
+ out_5133759507898990925[122] = 0;
+ out_5133759507898990925[123] = 0;
+ out_5133759507898990925[124] = 0;
+ out_5133759507898990925[125] = 0;
+ out_5133759507898990925[126] = 0;
+ out_5133759507898990925[127] = 0;
+ out_5133759507898990925[128] = 0;
+ out_5133759507898990925[129] = 0;
+ out_5133759507898990925[130] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[17] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[16] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[18]);
+ out_5133759507898990925[131] = dt*((2*state[3]*state[4] - 2*state[5]*state[6])*state[18] + (-2*state[3]*state[6] - 2*state[4]*state[5])*state[16] + (-pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[17]);
+ out_5133759507898990925[132] = 1;
+ out_5133759507898990925[133] = 0;
+ out_5133759507898990925[134] = 0;
+ out_5133759507898990925[135] = 0;
+ out_5133759507898990925[136] = 0;
+ out_5133759507898990925[137] = 0;
+ out_5133759507898990925[138] = 0;
+ out_5133759507898990925[139] = 0;
+ out_5133759507898990925[140] = 0;
+ out_5133759507898990925[141] = dt*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2));
+ out_5133759507898990925[142] = dt*(-2*state[3]*state[6] + 2*state[4]*state[5]);
+ out_5133759507898990925[143] = dt*(2*state[3]*state[5] + 2*state[4]*state[6]);
+ out_5133759507898990925[144] = 0;
+ out_5133759507898990925[145] = 0;
+ out_5133759507898990925[146] = 0;
+ out_5133759507898990925[147] = 0;
+ out_5133759507898990925[148] = 0;
+ out_5133759507898990925[149] = 0;
+ out_5133759507898990925[150] = dt*(-(2*state[3]*state[4] + 2*state[5]*state[6])*state[17] - (-2*state[3]*state[5] + 2*state[4]*state[6])*state[16] - (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[18]);
+ out_5133759507898990925[151] = 0;
+ out_5133759507898990925[152] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[18] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[17] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[16]);
+ out_5133759507898990925[153] = 0;
+ out_5133759507898990925[154] = 1;
+ out_5133759507898990925[155] = 0;
+ out_5133759507898990925[156] = 0;
+ out_5133759507898990925[157] = 0;
+ out_5133759507898990925[158] = 0;
+ out_5133759507898990925[159] = 0;
+ out_5133759507898990925[160] = 0;
+ out_5133759507898990925[161] = 0;
+ out_5133759507898990925[162] = dt*(2*state[3]*state[6] + 2*state[4]*state[5]);
+ out_5133759507898990925[163] = dt*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2));
+ out_5133759507898990925[164] = dt*(-2*state[3]*state[4] + 2*state[5]*state[6]);
+ out_5133759507898990925[165] = 0;
+ out_5133759507898990925[166] = 0;
+ out_5133759507898990925[167] = 0;
+ out_5133759507898990925[168] = 0;
+ out_5133759507898990925[169] = 0;
+ out_5133759507898990925[170] = 0;
+ out_5133759507898990925[171] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[18] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[16] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[17]);
+ out_5133759507898990925[172] = dt*((-2*state[3]*state[5] - 2*state[4]*state[6])*state[18] + (2*state[3]*state[6] - 2*state[4]*state[5])*state[17] + (-pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) + pow(state[6], 2))*state[16]);
+ out_5133759507898990925[173] = 0;
+ out_5133759507898990925[174] = 0;
+ out_5133759507898990925[175] = 0;
+ out_5133759507898990925[176] = 1;
+ out_5133759507898990925[177] = 0;
+ out_5133759507898990925[178] = 0;
+ out_5133759507898990925[179] = 0;
+ out_5133759507898990925[180] = 0;
+ out_5133759507898990925[181] = 0;
+ out_5133759507898990925[182] = 0;
+ out_5133759507898990925[183] = dt*(-2*state[3]*state[5] + 2*state[4]*state[6]);
+ out_5133759507898990925[184] = dt*(2*state[3]*state[4] + 2*state[5]*state[6]);
+ out_5133759507898990925[185] = dt*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2));
+ out_5133759507898990925[186] = 0;
+ out_5133759507898990925[187] = 0;
+ out_5133759507898990925[188] = 0;
+ out_5133759507898990925[189] = 0;
+ out_5133759507898990925[190] = 0;
+ out_5133759507898990925[191] = 0;
+ out_5133759507898990925[192] = 0;
+ out_5133759507898990925[193] = 0;
+ out_5133759507898990925[194] = 0;
+ out_5133759507898990925[195] = 0;
+ out_5133759507898990925[196] = 0;
+ out_5133759507898990925[197] = 0;
+ out_5133759507898990925[198] = 1;
+ out_5133759507898990925[199] = 0;
+ out_5133759507898990925[200] = 0;
+ out_5133759507898990925[201] = 0;
+ out_5133759507898990925[202] = 0;
+ out_5133759507898990925[203] = 0;
+ out_5133759507898990925[204] = 0;
+ out_5133759507898990925[205] = 0;
+ out_5133759507898990925[206] = 0;
+ out_5133759507898990925[207] = 0;
+ out_5133759507898990925[208] = 0;
+ out_5133759507898990925[209] = 0;
+ out_5133759507898990925[210] = 0;
+ out_5133759507898990925[211] = 0;
+ out_5133759507898990925[212] = 0;
+ out_5133759507898990925[213] = 0;
+ out_5133759507898990925[214] = 0;
+ out_5133759507898990925[215] = 0;
+ out_5133759507898990925[216] = 0;
+ out_5133759507898990925[217] = 0;
+ out_5133759507898990925[218] = 0;
+ out_5133759507898990925[219] = 0;
+ out_5133759507898990925[220] = 1;
+ out_5133759507898990925[221] = 0;
+ out_5133759507898990925[222] = 0;
+ out_5133759507898990925[223] = 0;
+ out_5133759507898990925[224] = 0;
+ out_5133759507898990925[225] = 0;
+ out_5133759507898990925[226] = 0;
+ out_5133759507898990925[227] = 0;
+ out_5133759507898990925[228] = 0;
+ out_5133759507898990925[229] = 0;
+ out_5133759507898990925[230] = 0;
+ out_5133759507898990925[231] = 0;
+ out_5133759507898990925[232] = 0;
+ out_5133759507898990925[233] = 0;
+ out_5133759507898990925[234] = 0;
+ out_5133759507898990925[235] = 0;
+ out_5133759507898990925[236] = 0;
+ out_5133759507898990925[237] = 0;
+ out_5133759507898990925[238] = 0;
+ out_5133759507898990925[239] = 0;
+ out_5133759507898990925[240] = 0;
+ out_5133759507898990925[241] = 0;
+ out_5133759507898990925[242] = 1;
+ out_5133759507898990925[243] = 0;
+ out_5133759507898990925[244] = 0;
+ out_5133759507898990925[245] = 0;
+ out_5133759507898990925[246] = 0;
+ out_5133759507898990925[247] = 0;
+ out_5133759507898990925[248] = 0;
+ out_5133759507898990925[249] = 0;
+ out_5133759507898990925[250] = 0;
+ out_5133759507898990925[251] = 0;
+ out_5133759507898990925[252] = 0;
+ out_5133759507898990925[253] = 0;
+ out_5133759507898990925[254] = 0;
+ out_5133759507898990925[255] = 0;
+ out_5133759507898990925[256] = 0;
+ out_5133759507898990925[257] = 0;
+ out_5133759507898990925[258] = 0;
+ out_5133759507898990925[259] = 0;
+ out_5133759507898990925[260] = 0;
+ out_5133759507898990925[261] = 0;
+ out_5133759507898990925[262] = 0;
+ out_5133759507898990925[263] = 0;
+ out_5133759507898990925[264] = 1;
+ out_5133759507898990925[265] = 0;
+ out_5133759507898990925[266] = 0;
+ out_5133759507898990925[267] = 0;
+ out_5133759507898990925[268] = 0;
+ out_5133759507898990925[269] = 0;
+ out_5133759507898990925[270] = 0;
+ out_5133759507898990925[271] = 0;
+ out_5133759507898990925[272] = 0;
+ out_5133759507898990925[273] = 0;
+ out_5133759507898990925[274] = 0;
+ out_5133759507898990925[275] = 0;
+ out_5133759507898990925[276] = 0;
+ out_5133759507898990925[277] = 0;
+ out_5133759507898990925[278] = 0;
+ out_5133759507898990925[279] = 0;
+ out_5133759507898990925[280] = 0;
+ out_5133759507898990925[281] = 0;
+ out_5133759507898990925[282] = 0;
+ out_5133759507898990925[283] = 0;
+ out_5133759507898990925[284] = 0;
+ out_5133759507898990925[285] = 0;
+ out_5133759507898990925[286] = 1;
+ out_5133759507898990925[287] = 0;
+ out_5133759507898990925[288] = 0;
+ out_5133759507898990925[289] = 0;
+ out_5133759507898990925[290] = 0;
+ out_5133759507898990925[291] = 0;
+ out_5133759507898990925[292] = 0;
+ out_5133759507898990925[293] = 0;
+ out_5133759507898990925[294] = 0;
+ out_5133759507898990925[295] = 0;
+ out_5133759507898990925[296] = 0;
+ out_5133759507898990925[297] = 0;
+ out_5133759507898990925[298] = 0;
+ out_5133759507898990925[299] = 0;
+ out_5133759507898990925[300] = 0;
+ out_5133759507898990925[301] = 0;
+ out_5133759507898990925[302] = 0;
+ out_5133759507898990925[303] = 0;
+ out_5133759507898990925[304] = 0;
+ out_5133759507898990925[305] = 0;
+ out_5133759507898990925[306] = 0;
+ out_5133759507898990925[307] = 0;
+ out_5133759507898990925[308] = 1;
+ out_5133759507898990925[309] = 0;
+ out_5133759507898990925[310] = 0;
+ out_5133759507898990925[311] = 0;
+ out_5133759507898990925[312] = 0;
+ out_5133759507898990925[313] = 0;
+ out_5133759507898990925[314] = 0;
+ out_5133759507898990925[315] = 0;
+ out_5133759507898990925[316] = 0;
+ out_5133759507898990925[317] = 0;
+ out_5133759507898990925[318] = 0;
+ out_5133759507898990925[319] = 0;
+ out_5133759507898990925[320] = 0;
+ out_5133759507898990925[321] = 0;
+ out_5133759507898990925[322] = 0;
+ out_5133759507898990925[323] = 0;
+ out_5133759507898990925[324] = 0;
+ out_5133759507898990925[325] = 0;
+ out_5133759507898990925[326] = 0;
+ out_5133759507898990925[327] = 0;
+ out_5133759507898990925[328] = 0;
+ out_5133759507898990925[329] = 0;
+ out_5133759507898990925[330] = 1;
+ out_5133759507898990925[331] = 0;
+ out_5133759507898990925[332] = 0;
+ out_5133759507898990925[333] = 0;
+ out_5133759507898990925[334] = 0;
+ out_5133759507898990925[335] = 0;
+ out_5133759507898990925[336] = 0;
+ out_5133759507898990925[337] = 0;
+ out_5133759507898990925[338] = 0;
+ out_5133759507898990925[339] = 0;
+ out_5133759507898990925[340] = 0;
+ out_5133759507898990925[341] = 0;
+ out_5133759507898990925[342] = 0;
+ out_5133759507898990925[343] = 0;
+ out_5133759507898990925[344] = 0;
+ out_5133759507898990925[345] = 0;
+ out_5133759507898990925[346] = 0;
+ out_5133759507898990925[347] = 0;
+ out_5133759507898990925[348] = 0;
+ out_5133759507898990925[349] = 0;
+ out_5133759507898990925[350] = 0;
+ out_5133759507898990925[351] = 0;
+ out_5133759507898990925[352] = 1;
+ out_5133759507898990925[353] = 0;
+ out_5133759507898990925[354] = 0;
+ out_5133759507898990925[355] = 0;
+ out_5133759507898990925[356] = 0;
+ out_5133759507898990925[357] = 0;
+ out_5133759507898990925[358] = 0;
+ out_5133759507898990925[359] = 0;
+ out_5133759507898990925[360] = 0;
+ out_5133759507898990925[361] = 0;
+ out_5133759507898990925[362] = 0;
+ out_5133759507898990925[363] = 0;
+ out_5133759507898990925[364] = 0;
+ out_5133759507898990925[365] = 0;
+ out_5133759507898990925[366] = 0;
+ out_5133759507898990925[367] = 0;
+ out_5133759507898990925[368] = 0;
+ out_5133759507898990925[369] = 0;
+ out_5133759507898990925[370] = 0;
+ out_5133759507898990925[371] = 0;
+ out_5133759507898990925[372] = 0;
+ out_5133759507898990925[373] = 0;
+ out_5133759507898990925[374] = 1;
+ out_5133759507898990925[375] = 0;
+ out_5133759507898990925[376] = 0;
+ out_5133759507898990925[377] = 0;
+ out_5133759507898990925[378] = 0;
+ out_5133759507898990925[379] = 0;
+ out_5133759507898990925[380] = 0;
+ out_5133759507898990925[381] = 0;
+ out_5133759507898990925[382] = 0;
+ out_5133759507898990925[383] = 0;
+ out_5133759507898990925[384] = 0;
+ out_5133759507898990925[385] = 0;
+ out_5133759507898990925[386] = 0;
+ out_5133759507898990925[387] = 0;
+ out_5133759507898990925[388] = 0;
+ out_5133759507898990925[389] = 0;
+ out_5133759507898990925[390] = 0;
+ out_5133759507898990925[391] = 0;
+ out_5133759507898990925[392] = 0;
+ out_5133759507898990925[393] = 0;
+ out_5133759507898990925[394] = 0;
+ out_5133759507898990925[395] = 0;
+ out_5133759507898990925[396] = 1;
+ out_5133759507898990925[397] = 0;
+ out_5133759507898990925[398] = 0;
+ out_5133759507898990925[399] = 0;
+ out_5133759507898990925[400] = 0;
+ out_5133759507898990925[401] = 0;
+ out_5133759507898990925[402] = 0;
+ out_5133759507898990925[403] = 0;
+ out_5133759507898990925[404] = 0;
+ out_5133759507898990925[405] = 0;
+ out_5133759507898990925[406] = 0;
+ out_5133759507898990925[407] = 0;
+ out_5133759507898990925[408] = 0;
+ out_5133759507898990925[409] = 0;
+ out_5133759507898990925[410] = 0;
+ out_5133759507898990925[411] = 0;
+ out_5133759507898990925[412] = 0;
+ out_5133759507898990925[413] = 0;
+ out_5133759507898990925[414] = 0;
+ out_5133759507898990925[415] = 0;
+ out_5133759507898990925[416] = 0;
+ out_5133759507898990925[417] = 0;
+ out_5133759507898990925[418] = 1;
+ out_5133759507898990925[419] = 0;
+ out_5133759507898990925[420] = 0;
+ out_5133759507898990925[421] = 0;
+ out_5133759507898990925[422] = 0;
+ out_5133759507898990925[423] = 0;
+ out_5133759507898990925[424] = 0;
+ out_5133759507898990925[425] = 0;
+ out_5133759507898990925[426] = 0;
+ out_5133759507898990925[427] = 0;
+ out_5133759507898990925[428] = 0;
+ out_5133759507898990925[429] = 0;
+ out_5133759507898990925[430] = 0;
+ out_5133759507898990925[431] = 0;
+ out_5133759507898990925[432] = 0;
+ out_5133759507898990925[433] = 0;
+ out_5133759507898990925[434] = 0;
+ out_5133759507898990925[435] = 0;
+ out_5133759507898990925[436] = 0;
+ out_5133759507898990925[437] = 0;
+ out_5133759507898990925[438] = 0;
+ out_5133759507898990925[439] = 0;
+ out_5133759507898990925[440] = 1;
}
-void h_4(double *state, double *unused, double *out_6579737165675804813) {
- out_6579737165675804813[0] = state[10] + state[13];
- out_6579737165675804813[1] = state[11] + state[14];
- out_6579737165675804813[2] = state[12] + state[15];
+void h_4(double *state, double *unused, double *out_6872450210111746980) {
+ out_6872450210111746980[0] = state[10] + state[13];
+ out_6872450210111746980[1] = state[11] + state[14];
+ out_6872450210111746980[2] = state[12] + state[15];
}
-void H_4(double *state, double *unused, double *out_8985066658053328462) {
- out_8985066658053328462[0] = 0;
- out_8985066658053328462[1] = 0;
- out_8985066658053328462[2] = 0;
- out_8985066658053328462[3] = 0;
- out_8985066658053328462[4] = 0;
- out_8985066658053328462[5] = 0;
- out_8985066658053328462[6] = 0;
- out_8985066658053328462[7] = 0;
- out_8985066658053328462[8] = 0;
- out_8985066658053328462[9] = 0;
- out_8985066658053328462[10] = 1;
- out_8985066658053328462[11] = 0;
- out_8985066658053328462[12] = 0;
- out_8985066658053328462[13] = 1;
- out_8985066658053328462[14] = 0;
- out_8985066658053328462[15] = 0;
- out_8985066658053328462[16] = 0;
- out_8985066658053328462[17] = 0;
- out_8985066658053328462[18] = 0;
- out_8985066658053328462[19] = 0;
- out_8985066658053328462[20] = 0;
- out_8985066658053328462[21] = 0;
- out_8985066658053328462[22] = 0;
- out_8985066658053328462[23] = 0;
- out_8985066658053328462[24] = 0;
- out_8985066658053328462[25] = 0;
- out_8985066658053328462[26] = 0;
- out_8985066658053328462[27] = 0;
- out_8985066658053328462[28] = 0;
- out_8985066658053328462[29] = 0;
- out_8985066658053328462[30] = 0;
- out_8985066658053328462[31] = 0;
- out_8985066658053328462[32] = 0;
- out_8985066658053328462[33] = 1;
- out_8985066658053328462[34] = 0;
- out_8985066658053328462[35] = 0;
- out_8985066658053328462[36] = 1;
- out_8985066658053328462[37] = 0;
- out_8985066658053328462[38] = 0;
- out_8985066658053328462[39] = 0;
- out_8985066658053328462[40] = 0;
- out_8985066658053328462[41] = 0;
- out_8985066658053328462[42] = 0;
- out_8985066658053328462[43] = 0;
- out_8985066658053328462[44] = 0;
- out_8985066658053328462[45] = 0;
- out_8985066658053328462[46] = 0;
- out_8985066658053328462[47] = 0;
- out_8985066658053328462[48] = 0;
- out_8985066658053328462[49] = 0;
- out_8985066658053328462[50] = 0;
- out_8985066658053328462[51] = 0;
- out_8985066658053328462[52] = 0;
- out_8985066658053328462[53] = 0;
- out_8985066658053328462[54] = 0;
- out_8985066658053328462[55] = 0;
- out_8985066658053328462[56] = 1;
- out_8985066658053328462[57] = 0;
- out_8985066658053328462[58] = 0;
- out_8985066658053328462[59] = 1;
- out_8985066658053328462[60] = 0;
- out_8985066658053328462[61] = 0;
- out_8985066658053328462[62] = 0;
- out_8985066658053328462[63] = 0;
- out_8985066658053328462[64] = 0;
- out_8985066658053328462[65] = 0;
+void H_4(double *state, double *unused, double *out_2937396917593955740) {
+ out_2937396917593955740[0] = 0;
+ out_2937396917593955740[1] = 0;
+ out_2937396917593955740[2] = 0;
+ out_2937396917593955740[3] = 0;
+ out_2937396917593955740[4] = 0;
+ out_2937396917593955740[5] = 0;
+ out_2937396917593955740[6] = 0;
+ out_2937396917593955740[7] = 0;
+ out_2937396917593955740[8] = 0;
+ out_2937396917593955740[9] = 0;
+ out_2937396917593955740[10] = 1;
+ out_2937396917593955740[11] = 0;
+ out_2937396917593955740[12] = 0;
+ out_2937396917593955740[13] = 1;
+ out_2937396917593955740[14] = 0;
+ out_2937396917593955740[15] = 0;
+ out_2937396917593955740[16] = 0;
+ out_2937396917593955740[17] = 0;
+ out_2937396917593955740[18] = 0;
+ out_2937396917593955740[19] = 0;
+ out_2937396917593955740[20] = 0;
+ out_2937396917593955740[21] = 0;
+ out_2937396917593955740[22] = 0;
+ out_2937396917593955740[23] = 0;
+ out_2937396917593955740[24] = 0;
+ out_2937396917593955740[25] = 0;
+ out_2937396917593955740[26] = 0;
+ out_2937396917593955740[27] = 0;
+ out_2937396917593955740[28] = 0;
+ out_2937396917593955740[29] = 0;
+ out_2937396917593955740[30] = 0;
+ out_2937396917593955740[31] = 0;
+ out_2937396917593955740[32] = 0;
+ out_2937396917593955740[33] = 1;
+ out_2937396917593955740[34] = 0;
+ out_2937396917593955740[35] = 0;
+ out_2937396917593955740[36] = 1;
+ out_2937396917593955740[37] = 0;
+ out_2937396917593955740[38] = 0;
+ out_2937396917593955740[39] = 0;
+ out_2937396917593955740[40] = 0;
+ out_2937396917593955740[41] = 0;
+ out_2937396917593955740[42] = 0;
+ out_2937396917593955740[43] = 0;
+ out_2937396917593955740[44] = 0;
+ out_2937396917593955740[45] = 0;
+ out_2937396917593955740[46] = 0;
+ out_2937396917593955740[47] = 0;
+ out_2937396917593955740[48] = 0;
+ out_2937396917593955740[49] = 0;
+ out_2937396917593955740[50] = 0;
+ out_2937396917593955740[51] = 0;
+ out_2937396917593955740[52] = 0;
+ out_2937396917593955740[53] = 0;
+ out_2937396917593955740[54] = 0;
+ out_2937396917593955740[55] = 0;
+ out_2937396917593955740[56] = 1;
+ out_2937396917593955740[57] = 0;
+ out_2937396917593955740[58] = 0;
+ out_2937396917593955740[59] = 1;
+ out_2937396917593955740[60] = 0;
+ out_2937396917593955740[61] = 0;
+ out_2937396917593955740[62] = 0;
+ out_2937396917593955740[63] = 0;
+ out_2937396917593955740[64] = 0;
+ out_2937396917593955740[65] = 0;
}
-void h_9(double *state, double *unused, double *out_6592276556226636800) {
- out_6592276556226636800[0] = state[10];
- out_6592276556226636800[1] = state[11];
- out_6592276556226636800[2] = state[12];
+void h_9(double *state, double *unused, double *out_8735722694958550472) {
+ out_8735722694958550472[0] = state[10];
+ out_8735722694958550472[1] = state[11];
+ out_8735722694958550472[2] = state[12];
}
-void H_9(double *state, double *unused, double *out_6096205105773249120) {
- out_6096205105773249120[0] = 0;
- out_6096205105773249120[1] = 0;
- out_6096205105773249120[2] = 0;
- out_6096205105773249120[3] = 0;
- out_6096205105773249120[4] = 0;
- out_6096205105773249120[5] = 0;
- out_6096205105773249120[6] = 0;
- out_6096205105773249120[7] = 0;
- out_6096205105773249120[8] = 0;
- out_6096205105773249120[9] = 0;
- out_6096205105773249120[10] = 1;
- out_6096205105773249120[11] = 0;
- out_6096205105773249120[12] = 0;
- out_6096205105773249120[13] = 0;
- out_6096205105773249120[14] = 0;
- out_6096205105773249120[15] = 0;
- out_6096205105773249120[16] = 0;
- out_6096205105773249120[17] = 0;
- out_6096205105773249120[18] = 0;
- out_6096205105773249120[19] = 0;
- out_6096205105773249120[20] = 0;
- out_6096205105773249120[21] = 0;
- out_6096205105773249120[22] = 0;
- out_6096205105773249120[23] = 0;
- out_6096205105773249120[24] = 0;
- out_6096205105773249120[25] = 0;
- out_6096205105773249120[26] = 0;
- out_6096205105773249120[27] = 0;
- out_6096205105773249120[28] = 0;
- out_6096205105773249120[29] = 0;
- out_6096205105773249120[30] = 0;
- out_6096205105773249120[31] = 0;
- out_6096205105773249120[32] = 0;
- out_6096205105773249120[33] = 1;
- out_6096205105773249120[34] = 0;
- out_6096205105773249120[35] = 0;
- out_6096205105773249120[36] = 0;
- out_6096205105773249120[37] = 0;
- out_6096205105773249120[38] = 0;
- out_6096205105773249120[39] = 0;
- out_6096205105773249120[40] = 0;
- out_6096205105773249120[41] = 0;
- out_6096205105773249120[42] = 0;
- out_6096205105773249120[43] = 0;
- out_6096205105773249120[44] = 0;
- out_6096205105773249120[45] = 0;
- out_6096205105773249120[46] = 0;
- out_6096205105773249120[47] = 0;
- out_6096205105773249120[48] = 0;
- out_6096205105773249120[49] = 0;
- out_6096205105773249120[50] = 0;
- out_6096205105773249120[51] = 0;
- out_6096205105773249120[52] = 0;
- out_6096205105773249120[53] = 0;
- out_6096205105773249120[54] = 0;
- out_6096205105773249120[55] = 0;
- out_6096205105773249120[56] = 1;
- out_6096205105773249120[57] = 0;
- out_6096205105773249120[58] = 0;
- out_6096205105773249120[59] = 0;
- out_6096205105773249120[60] = 0;
- out_6096205105773249120[61] = 0;
- out_6096205105773249120[62] = 0;
- out_6096205105773249120[63] = 0;
- out_6096205105773249120[64] = 0;
- out_6096205105773249120[65] = 0;
+void H_9(double *state, double *unused, double *out_3178586564223546385) {
+ out_3178586564223546385[0] = 0;
+ out_3178586564223546385[1] = 0;
+ out_3178586564223546385[2] = 0;
+ out_3178586564223546385[3] = 0;
+ out_3178586564223546385[4] = 0;
+ out_3178586564223546385[5] = 0;
+ out_3178586564223546385[6] = 0;
+ out_3178586564223546385[7] = 0;
+ out_3178586564223546385[8] = 0;
+ out_3178586564223546385[9] = 0;
+ out_3178586564223546385[10] = 1;
+ out_3178586564223546385[11] = 0;
+ out_3178586564223546385[12] = 0;
+ out_3178586564223546385[13] = 0;
+ out_3178586564223546385[14] = 0;
+ out_3178586564223546385[15] = 0;
+ out_3178586564223546385[16] = 0;
+ out_3178586564223546385[17] = 0;
+ out_3178586564223546385[18] = 0;
+ out_3178586564223546385[19] = 0;
+ out_3178586564223546385[20] = 0;
+ out_3178586564223546385[21] = 0;
+ out_3178586564223546385[22] = 0;
+ out_3178586564223546385[23] = 0;
+ out_3178586564223546385[24] = 0;
+ out_3178586564223546385[25] = 0;
+ out_3178586564223546385[26] = 0;
+ out_3178586564223546385[27] = 0;
+ out_3178586564223546385[28] = 0;
+ out_3178586564223546385[29] = 0;
+ out_3178586564223546385[30] = 0;
+ out_3178586564223546385[31] = 0;
+ out_3178586564223546385[32] = 0;
+ out_3178586564223546385[33] = 1;
+ out_3178586564223546385[34] = 0;
+ out_3178586564223546385[35] = 0;
+ out_3178586564223546385[36] = 0;
+ out_3178586564223546385[37] = 0;
+ out_3178586564223546385[38] = 0;
+ out_3178586564223546385[39] = 0;
+ out_3178586564223546385[40] = 0;
+ out_3178586564223546385[41] = 0;
+ out_3178586564223546385[42] = 0;
+ out_3178586564223546385[43] = 0;
+ out_3178586564223546385[44] = 0;
+ out_3178586564223546385[45] = 0;
+ out_3178586564223546385[46] = 0;
+ out_3178586564223546385[47] = 0;
+ out_3178586564223546385[48] = 0;
+ out_3178586564223546385[49] = 0;
+ out_3178586564223546385[50] = 0;
+ out_3178586564223546385[51] = 0;
+ out_3178586564223546385[52] = 0;
+ out_3178586564223546385[53] = 0;
+ out_3178586564223546385[54] = 0;
+ out_3178586564223546385[55] = 0;
+ out_3178586564223546385[56] = 1;
+ out_3178586564223546385[57] = 0;
+ out_3178586564223546385[58] = 0;
+ out_3178586564223546385[59] = 0;
+ out_3178586564223546385[60] = 0;
+ out_3178586564223546385[61] = 0;
+ out_3178586564223546385[62] = 0;
+ out_3178586564223546385[63] = 0;
+ out_3178586564223546385[64] = 0;
+ out_3178586564223546385[65] = 0;
}
-void h_10(double *state, double *unused, double *out_3043512482999151809) {
- out_3043512482999151809[0] = 398600500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2] + 398600500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0] + state[16] + state[19];
- out_3043512482999151809[1] = 398600500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2] + 398600500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[1] + state[17] + state[20];
- out_3043512482999151809[2] = 398600500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1] + 398600500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[2] + state[18] + state[21];
+void h_10(double *state, double *unused, double *out_4239705948005809237) {
+ out_4239705948005809237[0] = 398600500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2] + 398600500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0] + state[16] + state[19];
+ out_4239705948005809237[1] = 398600500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2] + 398600500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[1] + state[17] + state[20];
+ out_4239705948005809237[2] = 398600500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1] + 398600500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[2] + state[18] + state[21];
}
-void H_10(double *state, double *unused, double *out_7192958685017806166) {
- out_7192958685017806166[0] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*pow(state[0], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2));
- out_7192958685017806166[1] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[1], 2) + 398600500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0]*state[1];
- out_7192958685017806166[2] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[2], 2) + 398600500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0]*state[2];
- out_7192958685017806166[3] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5];
- out_7192958685017806166[4] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6];
- out_7192958685017806166[5] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3];
- out_7192958685017806166[6] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4];
- out_7192958685017806166[7] = 0;
- out_7192958685017806166[8] = 0;
- out_7192958685017806166[9] = 0;
- out_7192958685017806166[10] = 0;
- out_7192958685017806166[11] = 0;
- out_7192958685017806166[12] = 0;
- out_7192958685017806166[13] = 0;
- out_7192958685017806166[14] = 0;
- out_7192958685017806166[15] = 0;
- out_7192958685017806166[16] = 1;
- out_7192958685017806166[17] = 0;
- out_7192958685017806166[18] = 0;
- out_7192958685017806166[19] = 1;
- out_7192958685017806166[20] = 0;
- out_7192958685017806166[21] = 0;
- out_7192958685017806166[22] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[0], 2) + 398600500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[0]*state[1];
- out_7192958685017806166[23] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*pow(state[1], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2));
- out_7192958685017806166[24] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[2], 2) + 398600500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[1]*state[2];
- out_7192958685017806166[25] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4];
- out_7192958685017806166[26] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3];
- out_7192958685017806166[27] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6];
- out_7192958685017806166[28] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5];
- out_7192958685017806166[29] = 0;
- out_7192958685017806166[30] = 0;
- out_7192958685017806166[31] = 0;
- out_7192958685017806166[32] = 0;
- out_7192958685017806166[33] = 0;
- out_7192958685017806166[34] = 0;
- out_7192958685017806166[35] = 0;
- out_7192958685017806166[36] = 0;
- out_7192958685017806166[37] = 0;
- out_7192958685017806166[38] = 0;
- out_7192958685017806166[39] = 1;
- out_7192958685017806166[40] = 0;
- out_7192958685017806166[41] = 0;
- out_7192958685017806166[42] = 1;
- out_7192958685017806166[43] = 0;
- out_7192958685017806166[44] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[0], 2) + 398600500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[0]*state[2];
- out_7192958685017806166[45] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[1], 2) + 398600500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[1]*state[2];
- out_7192958685017806166[46] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*pow(state[2], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2));
- out_7192958685017806166[47] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3];
- out_7192958685017806166[48] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4];
- out_7192958685017806166[49] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5];
- out_7192958685017806166[50] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6];
- out_7192958685017806166[51] = 0;
- out_7192958685017806166[52] = 0;
- out_7192958685017806166[53] = 0;
- out_7192958685017806166[54] = 0;
- out_7192958685017806166[55] = 0;
- out_7192958685017806166[56] = 0;
- out_7192958685017806166[57] = 0;
- out_7192958685017806166[58] = 0;
- out_7192958685017806166[59] = 0;
- out_7192958685017806166[60] = 0;
- out_7192958685017806166[61] = 0;
- out_7192958685017806166[62] = 1;
- out_7192958685017806166[63] = 0;
- out_7192958685017806166[64] = 0;
- out_7192958685017806166[65] = 1;
+void H_10(double *state, double *unused, double *out_4956943889339789869) {
+ out_4956943889339789869[0] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*pow(state[0], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2));
+ out_4956943889339789869[1] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[1], 2) + 398600500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0]*state[1];
+ out_4956943889339789869[2] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[2], 2) + 398600500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0]*state[2];
+ out_4956943889339789869[3] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5];
+ out_4956943889339789869[4] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6];
+ out_4956943889339789869[5] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3];
+ out_4956943889339789869[6] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4];
+ out_4956943889339789869[7] = 0;
+ out_4956943889339789869[8] = 0;
+ out_4956943889339789869[9] = 0;
+ out_4956943889339789869[10] = 0;
+ out_4956943889339789869[11] = 0;
+ out_4956943889339789869[12] = 0;
+ out_4956943889339789869[13] = 0;
+ out_4956943889339789869[14] = 0;
+ out_4956943889339789869[15] = 0;
+ out_4956943889339789869[16] = 1;
+ out_4956943889339789869[17] = 0;
+ out_4956943889339789869[18] = 0;
+ out_4956943889339789869[19] = 1;
+ out_4956943889339789869[20] = 0;
+ out_4956943889339789869[21] = 0;
+ out_4956943889339789869[22] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[0], 2) + 398600500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[0]*state[1];
+ out_4956943889339789869[23] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*pow(state[1], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2));
+ out_4956943889339789869[24] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[2], 2) + 398600500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[1]*state[2];
+ out_4956943889339789869[25] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4];
+ out_4956943889339789869[26] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3];
+ out_4956943889339789869[27] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6];
+ out_4956943889339789869[28] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5];
+ out_4956943889339789869[29] = 0;
+ out_4956943889339789869[30] = 0;
+ out_4956943889339789869[31] = 0;
+ out_4956943889339789869[32] = 0;
+ out_4956943889339789869[33] = 0;
+ out_4956943889339789869[34] = 0;
+ out_4956943889339789869[35] = 0;
+ out_4956943889339789869[36] = 0;
+ out_4956943889339789869[37] = 0;
+ out_4956943889339789869[38] = 0;
+ out_4956943889339789869[39] = 1;
+ out_4956943889339789869[40] = 0;
+ out_4956943889339789869[41] = 0;
+ out_4956943889339789869[42] = 1;
+ out_4956943889339789869[43] = 0;
+ out_4956943889339789869[44] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[0], 2) + 398600500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[0]*state[2];
+ out_4956943889339789869[45] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[1], 2) + 398600500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[1]*state[2];
+ out_4956943889339789869[46] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*pow(state[2], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2));
+ out_4956943889339789869[47] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3];
+ out_4956943889339789869[48] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4];
+ out_4956943889339789869[49] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5];
+ out_4956943889339789869[50] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6];
+ out_4956943889339789869[51] = 0;
+ out_4956943889339789869[52] = 0;
+ out_4956943889339789869[53] = 0;
+ out_4956943889339789869[54] = 0;
+ out_4956943889339789869[55] = 0;
+ out_4956943889339789869[56] = 0;
+ out_4956943889339789869[57] = 0;
+ out_4956943889339789869[58] = 0;
+ out_4956943889339789869[59] = 0;
+ out_4956943889339789869[60] = 0;
+ out_4956943889339789869[61] = 0;
+ out_4956943889339789869[62] = 1;
+ out_4956943889339789869[63] = 0;
+ out_4956943889339789869[64] = 0;
+ out_4956943889339789869[65] = 1;
}
-void h_12(double *state, double *unused, double *out_6151089362108428494) {
- out_6151089362108428494[0] = state[0];
- out_6151089362108428494[1] = state[1];
- out_6151089362108428494[2] = state[2];
+void h_12(double *state, double *unused, double *out_3848117259651593996) {
+ out_3848117259651593996[0] = state[0];
+ out_3848117259651593996[1] = state[1];
+ out_3848117259651593996[2] = state[2];
}
-void H_12(double *state, double *unused, double *out_1317938344370877970) {
- out_1317938344370877970[0] = 1;
- out_1317938344370877970[1] = 0;
- out_1317938344370877970[2] = 0;
- out_1317938344370877970[3] = 0;
- out_1317938344370877970[4] = 0;
- out_1317938344370877970[5] = 0;
- out_1317938344370877970[6] = 0;
- out_1317938344370877970[7] = 0;
- out_1317938344370877970[8] = 0;
- out_1317938344370877970[9] = 0;
- out_1317938344370877970[10] = 0;
- out_1317938344370877970[11] = 0;
- out_1317938344370877970[12] = 0;
- out_1317938344370877970[13] = 0;
- out_1317938344370877970[14] = 0;
- out_1317938344370877970[15] = 0;
- out_1317938344370877970[16] = 0;
- out_1317938344370877970[17] = 0;
- out_1317938344370877970[18] = 0;
- out_1317938344370877970[19] = 0;
- out_1317938344370877970[20] = 0;
- out_1317938344370877970[21] = 0;
- out_1317938344370877970[22] = 0;
- out_1317938344370877970[23] = 1;
- out_1317938344370877970[24] = 0;
- out_1317938344370877970[25] = 0;
- out_1317938344370877970[26] = 0;
- out_1317938344370877970[27] = 0;
- out_1317938344370877970[28] = 0;
- out_1317938344370877970[29] = 0;
- out_1317938344370877970[30] = 0;
- out_1317938344370877970[31] = 0;
- out_1317938344370877970[32] = 0;
- out_1317938344370877970[33] = 0;
- out_1317938344370877970[34] = 0;
- out_1317938344370877970[35] = 0;
- out_1317938344370877970[36] = 0;
- out_1317938344370877970[37] = 0;
- out_1317938344370877970[38] = 0;
- out_1317938344370877970[39] = 0;
- out_1317938344370877970[40] = 0;
- out_1317938344370877970[41] = 0;
- out_1317938344370877970[42] = 0;
- out_1317938344370877970[43] = 0;
- out_1317938344370877970[44] = 0;
- out_1317938344370877970[45] = 0;
- out_1317938344370877970[46] = 1;
- out_1317938344370877970[47] = 0;
- out_1317938344370877970[48] = 0;
- out_1317938344370877970[49] = 0;
- out_1317938344370877970[50] = 0;
- out_1317938344370877970[51] = 0;
- out_1317938344370877970[52] = 0;
- out_1317938344370877970[53] = 0;
- out_1317938344370877970[54] = 0;
- out_1317938344370877970[55] = 0;
- out_1317938344370877970[56] = 0;
- out_1317938344370877970[57] = 0;
- out_1317938344370877970[58] = 0;
- out_1317938344370877970[59] = 0;
- out_1317938344370877970[60] = 0;
- out_1317938344370877970[61] = 0;
- out_1317938344370877970[62] = 0;
- out_1317938344370877970[63] = 0;
- out_1317938344370877970[64] = 0;
- out_1317938344370877970[65] = 0;
+void H_12(double *state, double *unused, double *out_3558495942641549407) {
+ out_3558495942641549407[0] = 1;
+ out_3558495942641549407[1] = 0;
+ out_3558495942641549407[2] = 0;
+ out_3558495942641549407[3] = 0;
+ out_3558495942641549407[4] = 0;
+ out_3558495942641549407[5] = 0;
+ out_3558495942641549407[6] = 0;
+ out_3558495942641549407[7] = 0;
+ out_3558495942641549407[8] = 0;
+ out_3558495942641549407[9] = 0;
+ out_3558495942641549407[10] = 0;
+ out_3558495942641549407[11] = 0;
+ out_3558495942641549407[12] = 0;
+ out_3558495942641549407[13] = 0;
+ out_3558495942641549407[14] = 0;
+ out_3558495942641549407[15] = 0;
+ out_3558495942641549407[16] = 0;
+ out_3558495942641549407[17] = 0;
+ out_3558495942641549407[18] = 0;
+ out_3558495942641549407[19] = 0;
+ out_3558495942641549407[20] = 0;
+ out_3558495942641549407[21] = 0;
+ out_3558495942641549407[22] = 0;
+ out_3558495942641549407[23] = 1;
+ out_3558495942641549407[24] = 0;
+ out_3558495942641549407[25] = 0;
+ out_3558495942641549407[26] = 0;
+ out_3558495942641549407[27] = 0;
+ out_3558495942641549407[28] = 0;
+ out_3558495942641549407[29] = 0;
+ out_3558495942641549407[30] = 0;
+ out_3558495942641549407[31] = 0;
+ out_3558495942641549407[32] = 0;
+ out_3558495942641549407[33] = 0;
+ out_3558495942641549407[34] = 0;
+ out_3558495942641549407[35] = 0;
+ out_3558495942641549407[36] = 0;
+ out_3558495942641549407[37] = 0;
+ out_3558495942641549407[38] = 0;
+ out_3558495942641549407[39] = 0;
+ out_3558495942641549407[40] = 0;
+ out_3558495942641549407[41] = 0;
+ out_3558495942641549407[42] = 0;
+ out_3558495942641549407[43] = 0;
+ out_3558495942641549407[44] = 0;
+ out_3558495942641549407[45] = 0;
+ out_3558495942641549407[46] = 1;
+ out_3558495942641549407[47] = 0;
+ out_3558495942641549407[48] = 0;
+ out_3558495942641549407[49] = 0;
+ out_3558495942641549407[50] = 0;
+ out_3558495942641549407[51] = 0;
+ out_3558495942641549407[52] = 0;
+ out_3558495942641549407[53] = 0;
+ out_3558495942641549407[54] = 0;
+ out_3558495942641549407[55] = 0;
+ out_3558495942641549407[56] = 0;
+ out_3558495942641549407[57] = 0;
+ out_3558495942641549407[58] = 0;
+ out_3558495942641549407[59] = 0;
+ out_3558495942641549407[60] = 0;
+ out_3558495942641549407[61] = 0;
+ out_3558495942641549407[62] = 0;
+ out_3558495942641549407[63] = 0;
+ out_3558495942641549407[64] = 0;
+ out_3558495942641549407[65] = 0;
}
-void h_35(double *state, double *unused, double *out_1134950865722680502) {
- out_1134950865722680502[0] = state[7];
- out_1134950865722680502[1] = state[8];
- out_1134950865722680502[2] = state[9];
+void h_35(double *state, double *unused, double *out_854595621797169640) {
+ out_854595621797169640[0] = state[7];
+ out_854595621797169640[1] = state[8];
+ out_854595621797169640[2] = state[9];
}
-void H_35(double *state, double *unused, double *out_1427624687954135739) {
- out_1427624687954135739[0] = 0;
- out_1427624687954135739[1] = 0;
- out_1427624687954135739[2] = 0;
- out_1427624687954135739[3] = 0;
- out_1427624687954135739[4] = 0;
- out_1427624687954135739[5] = 0;
- out_1427624687954135739[6] = 0;
- out_1427624687954135739[7] = 1;
- out_1427624687954135739[8] = 0;
- out_1427624687954135739[9] = 0;
- out_1427624687954135739[10] = 0;
- out_1427624687954135739[11] = 0;
- out_1427624687954135739[12] = 0;
- out_1427624687954135739[13] = 0;
- out_1427624687954135739[14] = 0;
- out_1427624687954135739[15] = 0;
- out_1427624687954135739[16] = 0;
- out_1427624687954135739[17] = 0;
- out_1427624687954135739[18] = 0;
- out_1427624687954135739[19] = 0;
- out_1427624687954135739[20] = 0;
- out_1427624687954135739[21] = 0;
- out_1427624687954135739[22] = 0;
- out_1427624687954135739[23] = 0;
- out_1427624687954135739[24] = 0;
- out_1427624687954135739[25] = 0;
- out_1427624687954135739[26] = 0;
- out_1427624687954135739[27] = 0;
- out_1427624687954135739[28] = 0;
- out_1427624687954135739[29] = 0;
- out_1427624687954135739[30] = 1;
- out_1427624687954135739[31] = 0;
- out_1427624687954135739[32] = 0;
- out_1427624687954135739[33] = 0;
- out_1427624687954135739[34] = 0;
- out_1427624687954135739[35] = 0;
- out_1427624687954135739[36] = 0;
- out_1427624687954135739[37] = 0;
- out_1427624687954135739[38] = 0;
- out_1427624687954135739[39] = 0;
- out_1427624687954135739[40] = 0;
- out_1427624687954135739[41] = 0;
- out_1427624687954135739[42] = 0;
- out_1427624687954135739[43] = 0;
- out_1427624687954135739[44] = 0;
- out_1427624687954135739[45] = 0;
- out_1427624687954135739[46] = 0;
- out_1427624687954135739[47] = 0;
- out_1427624687954135739[48] = 0;
- out_1427624687954135739[49] = 0;
- out_1427624687954135739[50] = 0;
- out_1427624687954135739[51] = 0;
- out_1427624687954135739[52] = 0;
- out_1427624687954135739[53] = 1;
- out_1427624687954135739[54] = 0;
- out_1427624687954135739[55] = 0;
- out_1427624687954135739[56] = 0;
- out_1427624687954135739[57] = 0;
- out_1427624687954135739[58] = 0;
- out_1427624687954135739[59] = 0;
- out_1427624687954135739[60] = 0;
- out_1427624687954135739[61] = 0;
- out_1427624687954135739[62] = 0;
- out_1427624687954135739[63] = 0;
- out_1427624687954135739[64] = 0;
- out_1427624687954135739[65] = 0;
+void H_35(double *state, double *unused, double *out_6304058974966563116) {
+ out_6304058974966563116[0] = 0;
+ out_6304058974966563116[1] = 0;
+ out_6304058974966563116[2] = 0;
+ out_6304058974966563116[3] = 0;
+ out_6304058974966563116[4] = 0;
+ out_6304058974966563116[5] = 0;
+ out_6304058974966563116[6] = 0;
+ out_6304058974966563116[7] = 1;
+ out_6304058974966563116[8] = 0;
+ out_6304058974966563116[9] = 0;
+ out_6304058974966563116[10] = 0;
+ out_6304058974966563116[11] = 0;
+ out_6304058974966563116[12] = 0;
+ out_6304058974966563116[13] = 0;
+ out_6304058974966563116[14] = 0;
+ out_6304058974966563116[15] = 0;
+ out_6304058974966563116[16] = 0;
+ out_6304058974966563116[17] = 0;
+ out_6304058974966563116[18] = 0;
+ out_6304058974966563116[19] = 0;
+ out_6304058974966563116[20] = 0;
+ out_6304058974966563116[21] = 0;
+ out_6304058974966563116[22] = 0;
+ out_6304058974966563116[23] = 0;
+ out_6304058974966563116[24] = 0;
+ out_6304058974966563116[25] = 0;
+ out_6304058974966563116[26] = 0;
+ out_6304058974966563116[27] = 0;
+ out_6304058974966563116[28] = 0;
+ out_6304058974966563116[29] = 0;
+ out_6304058974966563116[30] = 1;
+ out_6304058974966563116[31] = 0;
+ out_6304058974966563116[32] = 0;
+ out_6304058974966563116[33] = 0;
+ out_6304058974966563116[34] = 0;
+ out_6304058974966563116[35] = 0;
+ out_6304058974966563116[36] = 0;
+ out_6304058974966563116[37] = 0;
+ out_6304058974966563116[38] = 0;
+ out_6304058974966563116[39] = 0;
+ out_6304058974966563116[40] = 0;
+ out_6304058974966563116[41] = 0;
+ out_6304058974966563116[42] = 0;
+ out_6304058974966563116[43] = 0;
+ out_6304058974966563116[44] = 0;
+ out_6304058974966563116[45] = 0;
+ out_6304058974966563116[46] = 0;
+ out_6304058974966563116[47] = 0;
+ out_6304058974966563116[48] = 0;
+ out_6304058974966563116[49] = 0;
+ out_6304058974966563116[50] = 0;
+ out_6304058974966563116[51] = 0;
+ out_6304058974966563116[52] = 0;
+ out_6304058974966563116[53] = 1;
+ out_6304058974966563116[54] = 0;
+ out_6304058974966563116[55] = 0;
+ out_6304058974966563116[56] = 0;
+ out_6304058974966563116[57] = 0;
+ out_6304058974966563116[58] = 0;
+ out_6304058974966563116[59] = 0;
+ out_6304058974966563116[60] = 0;
+ out_6304058974966563116[61] = 0;
+ out_6304058974966563116[62] = 0;
+ out_6304058974966563116[63] = 0;
+ out_6304058974966563116[64] = 0;
+ out_6304058974966563116[65] = 0;
}
-void h_32(double *state, double *unused, double *out_4721207292266360875) {
- out_4721207292266360875[0] = state[3];
- out_4721207292266360875[1] = state[4];
- out_4721207292266360875[2] = state[5];
- out_4721207292266360875[3] = state[6];
+void h_32(double *state, double *unused, double *out_2979708909891080243) {
+ out_2979708909891080243[0] = state[3];
+ out_2979708909891080243[1] = state[4];
+ out_2979708909891080243[2] = state[5];
+ out_2979708909891080243[3] = state[6];
}
-void H_32(double *state, double *unused, double *out_2913380087778971149) {
- out_2913380087778971149[0] = 0;
- out_2913380087778971149[1] = 0;
- out_2913380087778971149[2] = 0;
- out_2913380087778971149[3] = 1;
- out_2913380087778971149[4] = 0;
- out_2913380087778971149[5] = 0;
- out_2913380087778971149[6] = 0;
- out_2913380087778971149[7] = 0;
- out_2913380087778971149[8] = 0;
- out_2913380087778971149[9] = 0;
- out_2913380087778971149[10] = 0;
- out_2913380087778971149[11] = 0;
- out_2913380087778971149[12] = 0;
- out_2913380087778971149[13] = 0;
- out_2913380087778971149[14] = 0;
- out_2913380087778971149[15] = 0;
- out_2913380087778971149[16] = 0;
- out_2913380087778971149[17] = 0;
- out_2913380087778971149[18] = 0;
- out_2913380087778971149[19] = 0;
- out_2913380087778971149[20] = 0;
- out_2913380087778971149[21] = 0;
- out_2913380087778971149[22] = 0;
- out_2913380087778971149[23] = 0;
- out_2913380087778971149[24] = 0;
- out_2913380087778971149[25] = 0;
- out_2913380087778971149[26] = 1;
- out_2913380087778971149[27] = 0;
- out_2913380087778971149[28] = 0;
- out_2913380087778971149[29] = 0;
- out_2913380087778971149[30] = 0;
- out_2913380087778971149[31] = 0;
- out_2913380087778971149[32] = 0;
- out_2913380087778971149[33] = 0;
- out_2913380087778971149[34] = 0;
- out_2913380087778971149[35] = 0;
- out_2913380087778971149[36] = 0;
- out_2913380087778971149[37] = 0;
- out_2913380087778971149[38] = 0;
- out_2913380087778971149[39] = 0;
- out_2913380087778971149[40] = 0;
- out_2913380087778971149[41] = 0;
- out_2913380087778971149[42] = 0;
- out_2913380087778971149[43] = 0;
- out_2913380087778971149[44] = 0;
- out_2913380087778971149[45] = 0;
- out_2913380087778971149[46] = 0;
- out_2913380087778971149[47] = 0;
- out_2913380087778971149[48] = 0;
- out_2913380087778971149[49] = 1;
- out_2913380087778971149[50] = 0;
- out_2913380087778971149[51] = 0;
- out_2913380087778971149[52] = 0;
- out_2913380087778971149[53] = 0;
- out_2913380087778971149[54] = 0;
- out_2913380087778971149[55] = 0;
- out_2913380087778971149[56] = 0;
- out_2913380087778971149[57] = 0;
- out_2913380087778971149[58] = 0;
- out_2913380087778971149[59] = 0;
- out_2913380087778971149[60] = 0;
- out_2913380087778971149[61] = 0;
- out_2913380087778971149[62] = 0;
- out_2913380087778971149[63] = 0;
- out_2913380087778971149[64] = 0;
- out_2913380087778971149[65] = 0;
- out_2913380087778971149[66] = 0;
- out_2913380087778971149[67] = 0;
- out_2913380087778971149[68] = 0;
- out_2913380087778971149[69] = 0;
- out_2913380087778971149[70] = 0;
- out_2913380087778971149[71] = 0;
- out_2913380087778971149[72] = 1;
- out_2913380087778971149[73] = 0;
- out_2913380087778971149[74] = 0;
- out_2913380087778971149[75] = 0;
- out_2913380087778971149[76] = 0;
- out_2913380087778971149[77] = 0;
- out_2913380087778971149[78] = 0;
- out_2913380087778971149[79] = 0;
- out_2913380087778971149[80] = 0;
- out_2913380087778971149[81] = 0;
- out_2913380087778971149[82] = 0;
- out_2913380087778971149[83] = 0;
- out_2913380087778971149[84] = 0;
- out_2913380087778971149[85] = 0;
- out_2913380087778971149[86] = 0;
- out_2913380087778971149[87] = 0;
+void H_32(double *state, double *unused, double *out_9087717139384980709) {
+ out_9087717139384980709[0] = 0;
+ out_9087717139384980709[1] = 0;
+ out_9087717139384980709[2] = 0;
+ out_9087717139384980709[3] = 1;
+ out_9087717139384980709[4] = 0;
+ out_9087717139384980709[5] = 0;
+ out_9087717139384980709[6] = 0;
+ out_9087717139384980709[7] = 0;
+ out_9087717139384980709[8] = 0;
+ out_9087717139384980709[9] = 0;
+ out_9087717139384980709[10] = 0;
+ out_9087717139384980709[11] = 0;
+ out_9087717139384980709[12] = 0;
+ out_9087717139384980709[13] = 0;
+ out_9087717139384980709[14] = 0;
+ out_9087717139384980709[15] = 0;
+ out_9087717139384980709[16] = 0;
+ out_9087717139384980709[17] = 0;
+ out_9087717139384980709[18] = 0;
+ out_9087717139384980709[19] = 0;
+ out_9087717139384980709[20] = 0;
+ out_9087717139384980709[21] = 0;
+ out_9087717139384980709[22] = 0;
+ out_9087717139384980709[23] = 0;
+ out_9087717139384980709[24] = 0;
+ out_9087717139384980709[25] = 0;
+ out_9087717139384980709[26] = 1;
+ out_9087717139384980709[27] = 0;
+ out_9087717139384980709[28] = 0;
+ out_9087717139384980709[29] = 0;
+ out_9087717139384980709[30] = 0;
+ out_9087717139384980709[31] = 0;
+ out_9087717139384980709[32] = 0;
+ out_9087717139384980709[33] = 0;
+ out_9087717139384980709[34] = 0;
+ out_9087717139384980709[35] = 0;
+ out_9087717139384980709[36] = 0;
+ out_9087717139384980709[37] = 0;
+ out_9087717139384980709[38] = 0;
+ out_9087717139384980709[39] = 0;
+ out_9087717139384980709[40] = 0;
+ out_9087717139384980709[41] = 0;
+ out_9087717139384980709[42] = 0;
+ out_9087717139384980709[43] = 0;
+ out_9087717139384980709[44] = 0;
+ out_9087717139384980709[45] = 0;
+ out_9087717139384980709[46] = 0;
+ out_9087717139384980709[47] = 0;
+ out_9087717139384980709[48] = 0;
+ out_9087717139384980709[49] = 1;
+ out_9087717139384980709[50] = 0;
+ out_9087717139384980709[51] = 0;
+ out_9087717139384980709[52] = 0;
+ out_9087717139384980709[53] = 0;
+ out_9087717139384980709[54] = 0;
+ out_9087717139384980709[55] = 0;
+ out_9087717139384980709[56] = 0;
+ out_9087717139384980709[57] = 0;
+ out_9087717139384980709[58] = 0;
+ out_9087717139384980709[59] = 0;
+ out_9087717139384980709[60] = 0;
+ out_9087717139384980709[61] = 0;
+ out_9087717139384980709[62] = 0;
+ out_9087717139384980709[63] = 0;
+ out_9087717139384980709[64] = 0;
+ out_9087717139384980709[65] = 0;
+ out_9087717139384980709[66] = 0;
+ out_9087717139384980709[67] = 0;
+ out_9087717139384980709[68] = 0;
+ out_9087717139384980709[69] = 0;
+ out_9087717139384980709[70] = 0;
+ out_9087717139384980709[71] = 0;
+ out_9087717139384980709[72] = 1;
+ out_9087717139384980709[73] = 0;
+ out_9087717139384980709[74] = 0;
+ out_9087717139384980709[75] = 0;
+ out_9087717139384980709[76] = 0;
+ out_9087717139384980709[77] = 0;
+ out_9087717139384980709[78] = 0;
+ out_9087717139384980709[79] = 0;
+ out_9087717139384980709[80] = 0;
+ out_9087717139384980709[81] = 0;
+ out_9087717139384980709[82] = 0;
+ out_9087717139384980709[83] = 0;
+ out_9087717139384980709[84] = 0;
+ out_9087717139384980709[85] = 0;
+ out_9087717139384980709[86] = 0;
+ out_9087717139384980709[87] = 0;
}
-void h_13(double *state, double *unused, double *out_364134050257467252) {
- out_364134050257467252[0] = (-2*state[3]*state[5] + 2*state[4]*state[6])*state[9] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[8] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[7];
- out_364134050257467252[1] = (2*state[3]*state[4] + 2*state[5]*state[6])*state[9] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[7] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[8];
- out_364134050257467252[2] = (-2*state[3]*state[4] + 2*state[5]*state[6])*state[8] + (2*state[3]*state[5] + 2*state[4]*state[6])*state[7] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[9];
+void h_13(double *state, double *unused, double *out_7896661243212334910) {
+ out_7896661243212334910[0] = (-2*state[3]*state[5] + 2*state[4]*state[6])*state[9] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[8] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[7];
+ out_7896661243212334910[1] = (2*state[3]*state[4] + 2*state[5]*state[6])*state[9] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[7] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[8];
+ out_7896661243212334910[2] = (-2*state[3]*state[4] + 2*state[5]*state[6])*state[8] + (2*state[3]*state[5] + 2*state[4]*state[6])*state[7] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[9];
}
-void H_13(double *state, double *unused, double *out_5486592313647626777) {
- out_5486592313647626777[0] = 0;
- out_5486592313647626777[1] = 0;
- out_5486592313647626777[2] = 0;
- out_5486592313647626777[3] = 2*state[3]*state[7] - 2*state[5]*state[9] + 2*state[6]*state[8];
- out_5486592313647626777[4] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9];
- out_5486592313647626777[5] = -2*state[3]*state[9] + 2*state[4]*state[8] - 2*state[5]*state[7];
- out_5486592313647626777[6] = 2*state[3]*state[8] + 2*state[4]*state[9] - 2*state[6]*state[7];
- out_5486592313647626777[7] = pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2);
- out_5486592313647626777[8] = 2*state[3]*state[6] + 2*state[4]*state[5];
- out_5486592313647626777[9] = -2*state[3]*state[5] + 2*state[4]*state[6];
- out_5486592313647626777[10] = 0;
- out_5486592313647626777[11] = 0;
- out_5486592313647626777[12] = 0;
- out_5486592313647626777[13] = 0;
- out_5486592313647626777[14] = 0;
- out_5486592313647626777[15] = 0;
- out_5486592313647626777[16] = 0;
- out_5486592313647626777[17] = 0;
- out_5486592313647626777[18] = 0;
- out_5486592313647626777[19] = 0;
- out_5486592313647626777[20] = 0;
- out_5486592313647626777[21] = 0;
- out_5486592313647626777[22] = 0;
- out_5486592313647626777[23] = 0;
- out_5486592313647626777[24] = 0;
- out_5486592313647626777[25] = 2*state[3]*state[8] + 2*state[4]*state[9] - 2*state[6]*state[7];
- out_5486592313647626777[26] = 2*state[3]*state[9] - 2*state[4]*state[8] + 2*state[5]*state[7];
- out_5486592313647626777[27] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9];
- out_5486592313647626777[28] = -2*state[3]*state[7] + 2*state[5]*state[9] - 2*state[6]*state[8];
- out_5486592313647626777[29] = -2*state[3]*state[6] + 2*state[4]*state[5];
- out_5486592313647626777[30] = pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2);
- out_5486592313647626777[31] = 2*state[3]*state[4] + 2*state[5]*state[6];
- out_5486592313647626777[32] = 0;
- out_5486592313647626777[33] = 0;
- out_5486592313647626777[34] = 0;
- out_5486592313647626777[35] = 0;
- out_5486592313647626777[36] = 0;
- out_5486592313647626777[37] = 0;
- out_5486592313647626777[38] = 0;
- out_5486592313647626777[39] = 0;
- out_5486592313647626777[40] = 0;
- out_5486592313647626777[41] = 0;
- out_5486592313647626777[42] = 0;
- out_5486592313647626777[43] = 0;
- out_5486592313647626777[44] = 0;
- out_5486592313647626777[45] = 0;
- out_5486592313647626777[46] = 0;
- out_5486592313647626777[47] = 2*state[3]*state[9] - 2*state[4]*state[8] + 2*state[5]*state[7];
- out_5486592313647626777[48] = -2*state[3]*state[8] - 2*state[4]*state[9] + 2*state[6]*state[7];
- out_5486592313647626777[49] = 2*state[3]*state[7] - 2*state[5]*state[9] + 2*state[6]*state[8];
- out_5486592313647626777[50] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9];
- out_5486592313647626777[51] = 2*state[3]*state[5] + 2*state[4]*state[6];
- out_5486592313647626777[52] = -2*state[3]*state[4] + 2*state[5]*state[6];
- out_5486592313647626777[53] = pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2);
- out_5486592313647626777[54] = 0;
- out_5486592313647626777[55] = 0;
- out_5486592313647626777[56] = 0;
- out_5486592313647626777[57] = 0;
- out_5486592313647626777[58] = 0;
- out_5486592313647626777[59] = 0;
- out_5486592313647626777[60] = 0;
- out_5486592313647626777[61] = 0;
- out_5486592313647626777[62] = 0;
- out_5486592313647626777[63] = 0;
- out_5486592313647626777[64] = 0;
- out_5486592313647626777[65] = 0;
+void H_13(double *state, double *unused, double *out_83856576196215707) {
+ out_83856576196215707[0] = 0;
+ out_83856576196215707[1] = 0;
+ out_83856576196215707[2] = 0;
+ out_83856576196215707[3] = 2*state[3]*state[7] - 2*state[5]*state[9] + 2*state[6]*state[8];
+ out_83856576196215707[4] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9];
+ out_83856576196215707[5] = -2*state[3]*state[9] + 2*state[4]*state[8] - 2*state[5]*state[7];
+ out_83856576196215707[6] = 2*state[3]*state[8] + 2*state[4]*state[9] - 2*state[6]*state[7];
+ out_83856576196215707[7] = pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2);
+ out_83856576196215707[8] = 2*state[3]*state[6] + 2*state[4]*state[5];
+ out_83856576196215707[9] = -2*state[3]*state[5] + 2*state[4]*state[6];
+ out_83856576196215707[10] = 0;
+ out_83856576196215707[11] = 0;
+ out_83856576196215707[12] = 0;
+ out_83856576196215707[13] = 0;
+ out_83856576196215707[14] = 0;
+ out_83856576196215707[15] = 0;
+ out_83856576196215707[16] = 0;
+ out_83856576196215707[17] = 0;
+ out_83856576196215707[18] = 0;
+ out_83856576196215707[19] = 0;
+ out_83856576196215707[20] = 0;
+ out_83856576196215707[21] = 0;
+ out_83856576196215707[22] = 0;
+ out_83856576196215707[23] = 0;
+ out_83856576196215707[24] = 0;
+ out_83856576196215707[25] = 2*state[3]*state[8] + 2*state[4]*state[9] - 2*state[6]*state[7];
+ out_83856576196215707[26] = 2*state[3]*state[9] - 2*state[4]*state[8] + 2*state[5]*state[7];
+ out_83856576196215707[27] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9];
+ out_83856576196215707[28] = -2*state[3]*state[7] + 2*state[5]*state[9] - 2*state[6]*state[8];
+ out_83856576196215707[29] = -2*state[3]*state[6] + 2*state[4]*state[5];
+ out_83856576196215707[30] = pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2);
+ out_83856576196215707[31] = 2*state[3]*state[4] + 2*state[5]*state[6];
+ out_83856576196215707[32] = 0;
+ out_83856576196215707[33] = 0;
+ out_83856576196215707[34] = 0;
+ out_83856576196215707[35] = 0;
+ out_83856576196215707[36] = 0;
+ out_83856576196215707[37] = 0;
+ out_83856576196215707[38] = 0;
+ out_83856576196215707[39] = 0;
+ out_83856576196215707[40] = 0;
+ out_83856576196215707[41] = 0;
+ out_83856576196215707[42] = 0;
+ out_83856576196215707[43] = 0;
+ out_83856576196215707[44] = 0;
+ out_83856576196215707[45] = 0;
+ out_83856576196215707[46] = 0;
+ out_83856576196215707[47] = 2*state[3]*state[9] - 2*state[4]*state[8] + 2*state[5]*state[7];
+ out_83856576196215707[48] = -2*state[3]*state[8] - 2*state[4]*state[9] + 2*state[6]*state[7];
+ out_83856576196215707[49] = 2*state[3]*state[7] - 2*state[5]*state[9] + 2*state[6]*state[8];
+ out_83856576196215707[50] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9];
+ out_83856576196215707[51] = 2*state[3]*state[5] + 2*state[4]*state[6];
+ out_83856576196215707[52] = -2*state[3]*state[4] + 2*state[5]*state[6];
+ out_83856576196215707[53] = pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2);
+ out_83856576196215707[54] = 0;
+ out_83856576196215707[55] = 0;
+ out_83856576196215707[56] = 0;
+ out_83856576196215707[57] = 0;
+ out_83856576196215707[58] = 0;
+ out_83856576196215707[59] = 0;
+ out_83856576196215707[60] = 0;
+ out_83856576196215707[61] = 0;
+ out_83856576196215707[62] = 0;
+ out_83856576196215707[63] = 0;
+ out_83856576196215707[64] = 0;
+ out_83856576196215707[65] = 0;
}
-void h_14(double *state, double *unused, double *out_6592276556226636800) {
- out_6592276556226636800[0] = state[10];
- out_6592276556226636800[1] = state[11];
- out_6592276556226636800[2] = state[12];
+void h_14(double *state, double *unused, double *out_8735722694958550472) {
+ out_8735722694958550472[0] = state[10];
+ out_8735722694958550472[1] = state[11];
+ out_8735722694958550472[2] = state[12];
}
-void H_14(double *state, double *unused, double *out_6096205105773249120) {
- out_6096205105773249120[0] = 0;
- out_6096205105773249120[1] = 0;
- out_6096205105773249120[2] = 0;
- out_6096205105773249120[3] = 0;
- out_6096205105773249120[4] = 0;
- out_6096205105773249120[5] = 0;
- out_6096205105773249120[6] = 0;
- out_6096205105773249120[7] = 0;
- out_6096205105773249120[8] = 0;
- out_6096205105773249120[9] = 0;
- out_6096205105773249120[10] = 1;
- out_6096205105773249120[11] = 0;
- out_6096205105773249120[12] = 0;
- out_6096205105773249120[13] = 0;
- out_6096205105773249120[14] = 0;
- out_6096205105773249120[15] = 0;
- out_6096205105773249120[16] = 0;
- out_6096205105773249120[17] = 0;
- out_6096205105773249120[18] = 0;
- out_6096205105773249120[19] = 0;
- out_6096205105773249120[20] = 0;
- out_6096205105773249120[21] = 0;
- out_6096205105773249120[22] = 0;
- out_6096205105773249120[23] = 0;
- out_6096205105773249120[24] = 0;
- out_6096205105773249120[25] = 0;
- out_6096205105773249120[26] = 0;
- out_6096205105773249120[27] = 0;
- out_6096205105773249120[28] = 0;
- out_6096205105773249120[29] = 0;
- out_6096205105773249120[30] = 0;
- out_6096205105773249120[31] = 0;
- out_6096205105773249120[32] = 0;
- out_6096205105773249120[33] = 1;
- out_6096205105773249120[34] = 0;
- out_6096205105773249120[35] = 0;
- out_6096205105773249120[36] = 0;
- out_6096205105773249120[37] = 0;
- out_6096205105773249120[38] = 0;
- out_6096205105773249120[39] = 0;
- out_6096205105773249120[40] = 0;
- out_6096205105773249120[41] = 0;
- out_6096205105773249120[42] = 0;
- out_6096205105773249120[43] = 0;
- out_6096205105773249120[44] = 0;
- out_6096205105773249120[45] = 0;
- out_6096205105773249120[46] = 0;
- out_6096205105773249120[47] = 0;
- out_6096205105773249120[48] = 0;
- out_6096205105773249120[49] = 0;
- out_6096205105773249120[50] = 0;
- out_6096205105773249120[51] = 0;
- out_6096205105773249120[52] = 0;
- out_6096205105773249120[53] = 0;
- out_6096205105773249120[54] = 0;
- out_6096205105773249120[55] = 0;
- out_6096205105773249120[56] = 1;
- out_6096205105773249120[57] = 0;
- out_6096205105773249120[58] = 0;
- out_6096205105773249120[59] = 0;
- out_6096205105773249120[60] = 0;
- out_6096205105773249120[61] = 0;
- out_6096205105773249120[62] = 0;
- out_6096205105773249120[63] = 0;
- out_6096205105773249120[64] = 0;
- out_6096205105773249120[65] = 0;
+void H_14(double *state, double *unused, double *out_3178586564223546385) {
+ out_3178586564223546385[0] = 0;
+ out_3178586564223546385[1] = 0;
+ out_3178586564223546385[2] = 0;
+ out_3178586564223546385[3] = 0;
+ out_3178586564223546385[4] = 0;
+ out_3178586564223546385[5] = 0;
+ out_3178586564223546385[6] = 0;
+ out_3178586564223546385[7] = 0;
+ out_3178586564223546385[8] = 0;
+ out_3178586564223546385[9] = 0;
+ out_3178586564223546385[10] = 1;
+ out_3178586564223546385[11] = 0;
+ out_3178586564223546385[12] = 0;
+ out_3178586564223546385[13] = 0;
+ out_3178586564223546385[14] = 0;
+ out_3178586564223546385[15] = 0;
+ out_3178586564223546385[16] = 0;
+ out_3178586564223546385[17] = 0;
+ out_3178586564223546385[18] = 0;
+ out_3178586564223546385[19] = 0;
+ out_3178586564223546385[20] = 0;
+ out_3178586564223546385[21] = 0;
+ out_3178586564223546385[22] = 0;
+ out_3178586564223546385[23] = 0;
+ out_3178586564223546385[24] = 0;
+ out_3178586564223546385[25] = 0;
+ out_3178586564223546385[26] = 0;
+ out_3178586564223546385[27] = 0;
+ out_3178586564223546385[28] = 0;
+ out_3178586564223546385[29] = 0;
+ out_3178586564223546385[30] = 0;
+ out_3178586564223546385[31] = 0;
+ out_3178586564223546385[32] = 0;
+ out_3178586564223546385[33] = 1;
+ out_3178586564223546385[34] = 0;
+ out_3178586564223546385[35] = 0;
+ out_3178586564223546385[36] = 0;
+ out_3178586564223546385[37] = 0;
+ out_3178586564223546385[38] = 0;
+ out_3178586564223546385[39] = 0;
+ out_3178586564223546385[40] = 0;
+ out_3178586564223546385[41] = 0;
+ out_3178586564223546385[42] = 0;
+ out_3178586564223546385[43] = 0;
+ out_3178586564223546385[44] = 0;
+ out_3178586564223546385[45] = 0;
+ out_3178586564223546385[46] = 0;
+ out_3178586564223546385[47] = 0;
+ out_3178586564223546385[48] = 0;
+ out_3178586564223546385[49] = 0;
+ out_3178586564223546385[50] = 0;
+ out_3178586564223546385[51] = 0;
+ out_3178586564223546385[52] = 0;
+ out_3178586564223546385[53] = 0;
+ out_3178586564223546385[54] = 0;
+ out_3178586564223546385[55] = 0;
+ out_3178586564223546385[56] = 1;
+ out_3178586564223546385[57] = 0;
+ out_3178586564223546385[58] = 0;
+ out_3178586564223546385[59] = 0;
+ out_3178586564223546385[60] = 0;
+ out_3178586564223546385[61] = 0;
+ out_3178586564223546385[62] = 0;
+ out_3178586564223546385[63] = 0;
+ out_3178586564223546385[64] = 0;
+ out_3178586564223546385[65] = 0;
}
-void h_33(double *state, double *unused, double *out_3737322617129334857) {
- out_3737322617129334857[0] = state[16];
- out_3737322617129334857[1] = state[17];
- out_3737322617129334857[2] = state[18];
+void h_33(double *state, double *unused, double *out_9184153656412830582) {
+ out_9184153656412830582[0] = state[16];
+ out_9184153656412830582[1] = state[17];
+ out_9184153656412830582[2] = state[18];
}
-void H_33(double *state, double *unused, double *out_4578181692592993343) {
- out_4578181692592993343[0] = 0;
- out_4578181692592993343[1] = 0;
- out_4578181692592993343[2] = 0;
- out_4578181692592993343[3] = 0;
- out_4578181692592993343[4] = 0;
- out_4578181692592993343[5] = 0;
- out_4578181692592993343[6] = 0;
- out_4578181692592993343[7] = 0;
- out_4578181692592993343[8] = 0;
- out_4578181692592993343[9] = 0;
- out_4578181692592993343[10] = 0;
- out_4578181692592993343[11] = 0;
- out_4578181692592993343[12] = 0;
- out_4578181692592993343[13] = 0;
- out_4578181692592993343[14] = 0;
- out_4578181692592993343[15] = 0;
- out_4578181692592993343[16] = 1;
- out_4578181692592993343[17] = 0;
- out_4578181692592993343[18] = 0;
- out_4578181692592993343[19] = 0;
- out_4578181692592993343[20] = 0;
- out_4578181692592993343[21] = 0;
- out_4578181692592993343[22] = 0;
- out_4578181692592993343[23] = 0;
- out_4578181692592993343[24] = 0;
- out_4578181692592993343[25] = 0;
- out_4578181692592993343[26] = 0;
- out_4578181692592993343[27] = 0;
- out_4578181692592993343[28] = 0;
- out_4578181692592993343[29] = 0;
- out_4578181692592993343[30] = 0;
- out_4578181692592993343[31] = 0;
- out_4578181692592993343[32] = 0;
- out_4578181692592993343[33] = 0;
- out_4578181692592993343[34] = 0;
- out_4578181692592993343[35] = 0;
- out_4578181692592993343[36] = 0;
- out_4578181692592993343[37] = 0;
- out_4578181692592993343[38] = 0;
- out_4578181692592993343[39] = 1;
- out_4578181692592993343[40] = 0;
- out_4578181692592993343[41] = 0;
- out_4578181692592993343[42] = 0;
- out_4578181692592993343[43] = 0;
- out_4578181692592993343[44] = 0;
- out_4578181692592993343[45] = 0;
- out_4578181692592993343[46] = 0;
- out_4578181692592993343[47] = 0;
- out_4578181692592993343[48] = 0;
- out_4578181692592993343[49] = 0;
- out_4578181692592993343[50] = 0;
- out_4578181692592993343[51] = 0;
- out_4578181692592993343[52] = 0;
- out_4578181692592993343[53] = 0;
- out_4578181692592993343[54] = 0;
- out_4578181692592993343[55] = 0;
- out_4578181692592993343[56] = 0;
- out_4578181692592993343[57] = 0;
- out_4578181692592993343[58] = 0;
- out_4578181692592993343[59] = 0;
- out_4578181692592993343[60] = 0;
- out_4578181692592993343[61] = 0;
- out_4578181692592993343[62] = 1;
- out_4578181692592993343[63] = 0;
- out_4578181692592993343[64] = 0;
- out_4578181692592993343[65] = 0;
+void H_33(double *state, double *unused, double *out_8992128094104130896) {
+ out_8992128094104130896[0] = 0;
+ out_8992128094104130896[1] = 0;
+ out_8992128094104130896[2] = 0;
+ out_8992128094104130896[3] = 0;
+ out_8992128094104130896[4] = 0;
+ out_8992128094104130896[5] = 0;
+ out_8992128094104130896[6] = 0;
+ out_8992128094104130896[7] = 0;
+ out_8992128094104130896[8] = 0;
+ out_8992128094104130896[9] = 0;
+ out_8992128094104130896[10] = 0;
+ out_8992128094104130896[11] = 0;
+ out_8992128094104130896[12] = 0;
+ out_8992128094104130896[13] = 0;
+ out_8992128094104130896[14] = 0;
+ out_8992128094104130896[15] = 0;
+ out_8992128094104130896[16] = 1;
+ out_8992128094104130896[17] = 0;
+ out_8992128094104130896[18] = 0;
+ out_8992128094104130896[19] = 0;
+ out_8992128094104130896[20] = 0;
+ out_8992128094104130896[21] = 0;
+ out_8992128094104130896[22] = 0;
+ out_8992128094104130896[23] = 0;
+ out_8992128094104130896[24] = 0;
+ out_8992128094104130896[25] = 0;
+ out_8992128094104130896[26] = 0;
+ out_8992128094104130896[27] = 0;
+ out_8992128094104130896[28] = 0;
+ out_8992128094104130896[29] = 0;
+ out_8992128094104130896[30] = 0;
+ out_8992128094104130896[31] = 0;
+ out_8992128094104130896[32] = 0;
+ out_8992128094104130896[33] = 0;
+ out_8992128094104130896[34] = 0;
+ out_8992128094104130896[35] = 0;
+ out_8992128094104130896[36] = 0;
+ out_8992128094104130896[37] = 0;
+ out_8992128094104130896[38] = 0;
+ out_8992128094104130896[39] = 1;
+ out_8992128094104130896[40] = 0;
+ out_8992128094104130896[41] = 0;
+ out_8992128094104130896[42] = 0;
+ out_8992128094104130896[43] = 0;
+ out_8992128094104130896[44] = 0;
+ out_8992128094104130896[45] = 0;
+ out_8992128094104130896[46] = 0;
+ out_8992128094104130896[47] = 0;
+ out_8992128094104130896[48] = 0;
+ out_8992128094104130896[49] = 0;
+ out_8992128094104130896[50] = 0;
+ out_8992128094104130896[51] = 0;
+ out_8992128094104130896[52] = 0;
+ out_8992128094104130896[53] = 0;
+ out_8992128094104130896[54] = 0;
+ out_8992128094104130896[55] = 0;
+ out_8992128094104130896[56] = 0;
+ out_8992128094104130896[57] = 0;
+ out_8992128094104130896[58] = 0;
+ out_8992128094104130896[59] = 0;
+ out_8992128094104130896[60] = 0;
+ out_8992128094104130896[61] = 0;
+ out_8992128094104130896[62] = 1;
+ out_8992128094104130896[63] = 0;
+ out_8992128094104130896[64] = 0;
+ out_8992128094104130896[65] = 0;
}
#include
#include
@@ -1855,77 +1855,77 @@ void live_update_14(double *in_x, double *in_P, double *in_z, double *in_R, doub
void live_update_33(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
update<3, 3, 0>(in_x, in_P, h_33, H_33, NULL, in_z, in_R, in_ea, MAHA_THRESH_33);
}
-void live_H(double *in_vec, double *out_7959907782308194627) {
- H(in_vec, out_7959907782308194627);
+void live_H(double *in_vec, double *out_7310749464307319056) {
+ H(in_vec, out_7310749464307319056);
}
-void live_err_fun(double *nom_x, double *delta_x, double *out_6929774310989775988) {
- err_fun(nom_x, delta_x, out_6929774310989775988);
+void live_err_fun(double *nom_x, double *delta_x, double *out_8236526131657428764) {
+ err_fun(nom_x, delta_x, out_8236526131657428764);
}
-void live_inv_err_fun(double *nom_x, double *true_x, double *out_6193789776824585571) {
- inv_err_fun(nom_x, true_x, out_6193789776824585571);
+void live_inv_err_fun(double *nom_x, double *true_x, double *out_4747477926419252764) {
+ inv_err_fun(nom_x, true_x, out_4747477926419252764);
}
-void live_H_mod_fun(double *state, double *out_8547509152961498922) {
- H_mod_fun(state, out_8547509152961498922);
+void live_H_mod_fun(double *state, double *out_341303559384211768) {
+ H_mod_fun(state, out_341303559384211768);
}
-void live_f_fun(double *state, double dt, double *out_8100393315288221642) {
- f_fun(state, dt, out_8100393315288221642);
+void live_f_fun(double *state, double dt, double *out_4549525036970607503) {
+ f_fun(state, dt, out_4549525036970607503);
}
-void live_F_fun(double *state, double dt, double *out_7769089323050102648) {
- F_fun(state, dt, out_7769089323050102648);
+void live_F_fun(double *state, double dt, double *out_5133759507898990925) {
+ F_fun(state, dt, out_5133759507898990925);
}
-void live_h_4(double *state, double *unused, double *out_6579737165675804813) {
- h_4(state, unused, out_6579737165675804813);
+void live_h_4(double *state, double *unused, double *out_6872450210111746980) {
+ h_4(state, unused, out_6872450210111746980);
}
-void live_H_4(double *state, double *unused, double *out_8985066658053328462) {
- H_4(state, unused, out_8985066658053328462);
+void live_H_4(double *state, double *unused, double *out_2937396917593955740) {
+ H_4(state, unused, out_2937396917593955740);
}
-void live_h_9(double *state, double *unused, double *out_6592276556226636800) {
- h_9(state, unused, out_6592276556226636800);
+void live_h_9(double *state, double *unused, double *out_8735722694958550472) {
+ h_9(state, unused, out_8735722694958550472);
}
-void live_H_9(double *state, double *unused, double *out_6096205105773249120) {
- H_9(state, unused, out_6096205105773249120);
+void live_H_9(double *state, double *unused, double *out_3178586564223546385) {
+ H_9(state, unused, out_3178586564223546385);
}
-void live_h_10(double *state, double *unused, double *out_3043512482999151809) {
- h_10(state, unused, out_3043512482999151809);
+void live_h_10(double *state, double *unused, double *out_4239705948005809237) {
+ h_10(state, unused, out_4239705948005809237);
}
-void live_H_10(double *state, double *unused, double *out_7192958685017806166) {
- H_10(state, unused, out_7192958685017806166);
+void live_H_10(double *state, double *unused, double *out_4956943889339789869) {
+ H_10(state, unused, out_4956943889339789869);
}
-void live_h_12(double *state, double *unused, double *out_6151089362108428494) {
- h_12(state, unused, out_6151089362108428494);
+void live_h_12(double *state, double *unused, double *out_3848117259651593996) {
+ h_12(state, unused, out_3848117259651593996);
}
-void live_H_12(double *state, double *unused, double *out_1317938344370877970) {
- H_12(state, unused, out_1317938344370877970);
+void live_H_12(double *state, double *unused, double *out_3558495942641549407) {
+ H_12(state, unused, out_3558495942641549407);
}
-void live_h_35(double *state, double *unused, double *out_1134950865722680502) {
- h_35(state, unused, out_1134950865722680502);
+void live_h_35(double *state, double *unused, double *out_854595621797169640) {
+ h_35(state, unused, out_854595621797169640);
}
-void live_H_35(double *state, double *unused, double *out_1427624687954135739) {
- H_35(state, unused, out_1427624687954135739);
+void live_H_35(double *state, double *unused, double *out_6304058974966563116) {
+ H_35(state, unused, out_6304058974966563116);
}
-void live_h_32(double *state, double *unused, double *out_4721207292266360875) {
- h_32(state, unused, out_4721207292266360875);
+void live_h_32(double *state, double *unused, double *out_2979708909891080243) {
+ h_32(state, unused, out_2979708909891080243);
}
-void live_H_32(double *state, double *unused, double *out_2913380087778971149) {
- H_32(state, unused, out_2913380087778971149);
+void live_H_32(double *state, double *unused, double *out_9087717139384980709) {
+ H_32(state, unused, out_9087717139384980709);
}
-void live_h_13(double *state, double *unused, double *out_364134050257467252) {
- h_13(state, unused, out_364134050257467252);
+void live_h_13(double *state, double *unused, double *out_7896661243212334910) {
+ h_13(state, unused, out_7896661243212334910);
}
-void live_H_13(double *state, double *unused, double *out_5486592313647626777) {
- H_13(state, unused, out_5486592313647626777);
+void live_H_13(double *state, double *unused, double *out_83856576196215707) {
+ H_13(state, unused, out_83856576196215707);
}
-void live_h_14(double *state, double *unused, double *out_6592276556226636800) {
- h_14(state, unused, out_6592276556226636800);
+void live_h_14(double *state, double *unused, double *out_8735722694958550472) {
+ h_14(state, unused, out_8735722694958550472);
}
-void live_H_14(double *state, double *unused, double *out_6096205105773249120) {
- H_14(state, unused, out_6096205105773249120);
+void live_H_14(double *state, double *unused, double *out_3178586564223546385) {
+ H_14(state, unused, out_3178586564223546385);
}
-void live_h_33(double *state, double *unused, double *out_3737322617129334857) {
- h_33(state, unused, out_3737322617129334857);
+void live_h_33(double *state, double *unused, double *out_9184153656412830582) {
+ h_33(state, unused, out_9184153656412830582);
}
-void live_H_33(double *state, double *unused, double *out_4578181692592993343) {
- H_33(state, unused, out_4578181692592993343);
+void live_H_33(double *state, double *unused, double *out_8992128094104130896) {
+ H_33(state, unused, out_8992128094104130896);
}
void live_predict(double *in_x, double *in_P, double *in_Q, double dt) {
predict(in_x, in_P, in_Q, dt);
diff --git a/selfdrive/locationd/models/generated/live.h b/selfdrive/locationd/models/generated/live.h
index 88f8b3be8..7e5c26d5e 100644
--- a/selfdrive/locationd/models/generated/live.h
+++ b/selfdrive/locationd/models/generated/live.h
@@ -10,29 +10,29 @@ void live_update_32(double *in_x, double *in_P, double *in_z, double *in_R, doub
void live_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void live_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void live_update_33(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void live_H(double *in_vec, double *out_7959907782308194627);
-void live_err_fun(double *nom_x, double *delta_x, double *out_6929774310989775988);
-void live_inv_err_fun(double *nom_x, double *true_x, double *out_6193789776824585571);
-void live_H_mod_fun(double *state, double *out_8547509152961498922);
-void live_f_fun(double *state, double dt, double *out_8100393315288221642);
-void live_F_fun(double *state, double dt, double *out_7769089323050102648);
-void live_h_4(double *state, double *unused, double *out_6579737165675804813);
-void live_H_4(double *state, double *unused, double *out_8985066658053328462);
-void live_h_9(double *state, double *unused, double *out_6592276556226636800);
-void live_H_9(double *state, double *unused, double *out_6096205105773249120);
-void live_h_10(double *state, double *unused, double *out_3043512482999151809);
-void live_H_10(double *state, double *unused, double *out_7192958685017806166);
-void live_h_12(double *state, double *unused, double *out_6151089362108428494);
-void live_H_12(double *state, double *unused, double *out_1317938344370877970);
-void live_h_35(double *state, double *unused, double *out_1134950865722680502);
-void live_H_35(double *state, double *unused, double *out_1427624687954135739);
-void live_h_32(double *state, double *unused, double *out_4721207292266360875);
-void live_H_32(double *state, double *unused, double *out_2913380087778971149);
-void live_h_13(double *state, double *unused, double *out_364134050257467252);
-void live_H_13(double *state, double *unused, double *out_5486592313647626777);
-void live_h_14(double *state, double *unused, double *out_6592276556226636800);
-void live_H_14(double *state, double *unused, double *out_6096205105773249120);
-void live_h_33(double *state, double *unused, double *out_3737322617129334857);
-void live_H_33(double *state, double *unused, double *out_4578181692592993343);
+void live_H(double *in_vec, double *out_7310749464307319056);
+void live_err_fun(double *nom_x, double *delta_x, double *out_8236526131657428764);
+void live_inv_err_fun(double *nom_x, double *true_x, double *out_4747477926419252764);
+void live_H_mod_fun(double *state, double *out_341303559384211768);
+void live_f_fun(double *state, double dt, double *out_4549525036970607503);
+void live_F_fun(double *state, double dt, double *out_5133759507898990925);
+void live_h_4(double *state, double *unused, double *out_6872450210111746980);
+void live_H_4(double *state, double *unused, double *out_2937396917593955740);
+void live_h_9(double *state, double *unused, double *out_8735722694958550472);
+void live_H_9(double *state, double *unused, double *out_3178586564223546385);
+void live_h_10(double *state, double *unused, double *out_4239705948005809237);
+void live_H_10(double *state, double *unused, double *out_4956943889339789869);
+void live_h_12(double *state, double *unused, double *out_3848117259651593996);
+void live_H_12(double *state, double *unused, double *out_3558495942641549407);
+void live_h_35(double *state, double *unused, double *out_854595621797169640);
+void live_H_35(double *state, double *unused, double *out_6304058974966563116);
+void live_h_32(double *state, double *unused, double *out_2979708909891080243);
+void live_H_32(double *state, double *unused, double *out_9087717139384980709);
+void live_h_13(double *state, double *unused, double *out_7896661243212334910);
+void live_H_13(double *state, double *unused, double *out_83856576196215707);
+void live_h_14(double *state, double *unused, double *out_8735722694958550472);
+void live_H_14(double *state, double *unused, double *out_3178586564223546385);
+void live_h_33(double *state, double *unused, double *out_9184153656412830582);
+void live_H_33(double *state, double *unused, double *out_8992128094104130896);
void live_predict(double *in_x, double *in_P, double *in_Q, double dt);
}
\ No newline at end of file
diff --git a/selfdrive/loggerd/bootlog b/selfdrive/loggerd/bootlog
index 5363a262f..f94800dbb 100755
Binary files a/selfdrive/loggerd/bootlog and b/selfdrive/loggerd/bootlog differ
diff --git a/selfdrive/loggerd/loggerd b/selfdrive/loggerd/loggerd
index a7c6e6660..8fe8b4f71 100755
Binary files a/selfdrive/loggerd/loggerd and b/selfdrive/loggerd/loggerd differ
diff --git a/selfdrive/modeld/_dmonitoringmodeld b/selfdrive/modeld/_dmonitoringmodeld
index 9f3ea12f8..407a05012 100755
Binary files a/selfdrive/modeld/_dmonitoringmodeld and b/selfdrive/modeld/_dmonitoringmodeld differ
diff --git a/selfdrive/modeld/_modeld b/selfdrive/modeld/_modeld
index c2c93f21c..b7b203747 100755
Binary files a/selfdrive/modeld/_modeld and b/selfdrive/modeld/_modeld differ
diff --git a/selfdrive/modeld/models/supercombo.thneed b/selfdrive/modeld/models/supercombo.thneed
index b8f6f9bc4..ceafe2af8 100644
Binary files a/selfdrive/modeld/models/supercombo.thneed and b/selfdrive/modeld/models/supercombo.thneed differ
diff --git a/selfdrive/proclogd/proclogd b/selfdrive/proclogd/proclogd
index d709adf84..3867e383d 100755
Binary files a/selfdrive/proclogd/proclogd and b/selfdrive/proclogd/proclogd differ
diff --git a/selfdrive/ui/_ui b/selfdrive/ui/_ui
index 85e57457c..9737f2679 100755
Binary files a/selfdrive/ui/_ui and b/selfdrive/ui/_ui differ
diff --git a/selfdrive/ui/_ui_nonav b/selfdrive/ui/_ui_nonav
index 2ba82a000..ecaaf52e1 100755
Binary files a/selfdrive/ui/_ui_nonav and b/selfdrive/ui/_ui_nonav differ
diff --git a/selfdrive/ui/qt/spinner b/selfdrive/ui/qt/spinner
index 41abba802..4c26b77ed 100755
Binary files a/selfdrive/ui/qt/spinner and b/selfdrive/ui/qt/spinner differ
diff --git a/selfdrive/ui/qt/text b/selfdrive/ui/qt/text
index b1d6d1b10..572cacd55 100755
Binary files a/selfdrive/ui/qt/text and b/selfdrive/ui/qt/text differ
diff --git a/selfdrive/ui/soundd/_soundd b/selfdrive/ui/soundd/_soundd
index 087edf5a6..7c3935892 100755
Binary files a/selfdrive/ui/soundd/_soundd and b/selfdrive/ui/soundd/_soundd differ
diff --git a/system/camerad/camerad b/system/camerad/camerad
index 808ab3d40..0abc544dc 100755
Binary files a/system/camerad/camerad and b/system/camerad/camerad differ
diff --git a/system/sensord/_sensord b/system/sensord/_sensord
index 32bb50aca..7fca9d185 100755
Binary files a/system/sensord/_sensord and b/system/sensord/_sensord differ
diff --git a/system/ubloxd/ubloxd b/system/ubloxd/ubloxd
index ee7fffb94..0d78ceb74 100755
Binary files a/system/ubloxd/ubloxd and b/system/ubloxd/ubloxd differ