diff --git a/common/libcommon.a b/common/libcommon.a index b1e4156207..101f8f688f 100644 Binary files a/common/libcommon.a and b/common/libcommon.a differ diff --git a/common/params_pyx.so b/common/params_pyx.so index 07f50262c7..51e6895820 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index 132dfa4a61..b61ae25024 100644 Binary files a/panda/board/obj/body_h7.bin.signed and b/panda/board/obj/body_h7.bin.signed differ diff --git a/panda/board/obj/body_h7/bootstub.elf b/panda/board/obj/body_h7/bootstub.elf index fd221b2850..b49703daec 100755 Binary files a/panda/board/obj/body_h7/bootstub.elf and b/panda/board/obj/body_h7/bootstub.elf differ diff --git a/panda/board/obj/body_h7/main.bin b/panda/board/obj/body_h7/main.bin index 2d1a06d7b1..0bb9ca5650 100755 Binary files a/panda/board/obj/body_h7/main.bin and b/panda/board/obj/body_h7/main.bin differ diff --git a/panda/board/obj/body_h7/main.elf b/panda/board/obj/body_h7/main.elf index ad5499cdeb..69ccc59405 100755 Binary files a/panda/board/obj/body_h7/main.elf and b/panda/board/obj/body_h7/main.elf differ diff --git a/panda/board/obj/bootstub.body_h7.bin b/panda/board/obj/bootstub.body_h7.bin index 5756733954..9b7e062997 100755 Binary files a/panda/board/obj/bootstub.body_h7.bin and b/panda/board/obj/bootstub.body_h7.bin differ diff --git a/panda/board/obj/bootstub.panda.bin b/panda/board/obj/bootstub.panda.bin index fc49547bfc..86b5751449 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/bootstub.panda_can_ignition_only.bin b/panda/board/obj/bootstub.panda_can_ignition_only.bin index fc49547bfc..86b5751449 100755 Binary files a/panda/board/obj/bootstub.panda_can_ignition_only.bin and b/panda/board/obj/bootstub.panda_can_ignition_only.bin differ diff --git a/panda/board/obj/bootstub.panda_h7.bin b/panda/board/obj/bootstub.panda_h7.bin index da67a70b8c..a08827842f 100755 Binary files a/panda/board/obj/bootstub.panda_h7.bin and b/panda/board/obj/bootstub.panda_h7.bin differ diff --git a/panda/board/obj/bootstub.panda_h7_can_ignition_only.bin b/panda/board/obj/bootstub.panda_h7_can_ignition_only.bin index da67a70b8c..a08827842f 100755 Binary files a/panda/board/obj/bootstub.panda_h7_can_ignition_only.bin and b/panda/board/obj/bootstub.panda_h7_can_ignition_only.bin differ diff --git a/panda/board/obj/bootstub.panda_h7_hkg_remote.bin b/panda/board/obj/bootstub.panda_h7_hkg_remote.bin index da67a70b8c..a08827842f 100755 Binary files a/panda/board/obj/bootstub.panda_h7_hkg_remote.bin and b/panda/board/obj/bootstub.panda_h7_hkg_remote.bin differ diff --git a/panda/board/obj/bootstub.panda_h7_hkg_remote_can_ignition_only.bin b/panda/board/obj/bootstub.panda_h7_hkg_remote_can_ignition_only.bin index da67a70b8c..a08827842f 100755 Binary files a/panda/board/obj/bootstub.panda_h7_hkg_remote_can_ignition_only.bin and b/panda/board/obj/bootstub.panda_h7_hkg_remote_can_ignition_only.bin differ diff --git a/panda/board/obj/bootstub.panda_h7_remote.bin b/panda/board/obj/bootstub.panda_h7_remote.bin index da67a70b8c..a08827842f 100755 Binary files a/panda/board/obj/bootstub.panda_h7_remote.bin and b/panda/board/obj/bootstub.panda_h7_remote.bin differ diff --git a/panda/board/obj/bootstub.panda_h7_remote_can_ignition_only.bin b/panda/board/obj/bootstub.panda_h7_remote_can_ignition_only.bin index da67a70b8c..a08827842f 100755 Binary files a/panda/board/obj/bootstub.panda_h7_remote_can_ignition_only.bin and b/panda/board/obj/bootstub.panda_h7_remote_can_ignition_only.bin differ diff --git a/panda/board/obj/bootstub.panda_h7_tesla_wake.bin b/panda/board/obj/bootstub.panda_h7_tesla_wake.bin index da67a70b8c..a08827842f 100755 Binary files a/panda/board/obj/bootstub.panda_h7_tesla_wake.bin and b/panda/board/obj/bootstub.panda_h7_tesla_wake.bin differ diff --git a/panda/board/obj/bootstub.panda_h7_tesla_wake_can_ignition_only.bin b/panda/board/obj/bootstub.panda_h7_tesla_wake_can_ignition_only.bin index da67a70b8c..a08827842f 100755 Binary files a/panda/board/obj/bootstub.panda_h7_tesla_wake_can_ignition_only.bin and b/panda/board/obj/bootstub.panda_h7_tesla_wake_can_ignition_only.bin differ diff --git a/panda/board/obj/bootstub.panda_hkg_remote.bin b/panda/board/obj/bootstub.panda_hkg_remote.bin index fc49547bfc..86b5751449 100755 Binary files a/panda/board/obj/bootstub.panda_hkg_remote.bin and b/panda/board/obj/bootstub.panda_hkg_remote.bin differ diff --git a/panda/board/obj/bootstub.panda_hkg_remote_can_ignition_only.bin b/panda/board/obj/bootstub.panda_hkg_remote_can_ignition_only.bin index fc49547bfc..86b5751449 100755 Binary files a/panda/board/obj/bootstub.panda_hkg_remote_can_ignition_only.bin and b/panda/board/obj/bootstub.panda_hkg_remote_can_ignition_only.bin differ diff --git a/panda/board/obj/bootstub.panda_jungle_h7.bin b/panda/board/obj/bootstub.panda_jungle_h7.bin index 3f5c5b974c..4e9a5f674a 100755 Binary files a/panda/board/obj/bootstub.panda_jungle_h7.bin and b/panda/board/obj/bootstub.panda_jungle_h7.bin differ diff --git a/panda/board/obj/bootstub.panda_remote.bin b/panda/board/obj/bootstub.panda_remote.bin index fc49547bfc..86b5751449 100755 Binary files a/panda/board/obj/bootstub.panda_remote.bin and b/panda/board/obj/bootstub.panda_remote.bin differ diff --git a/panda/board/obj/bootstub.panda_remote_can_ignition_only.bin b/panda/board/obj/bootstub.panda_remote_can_ignition_only.bin index fc49547bfc..86b5751449 100755 Binary files a/panda/board/obj/bootstub.panda_remote_can_ignition_only.bin and b/panda/board/obj/bootstub.panda_remote_can_ignition_only.bin differ diff --git a/panda/board/obj/bootstub.panda_tesla_wake.bin b/panda/board/obj/bootstub.panda_tesla_wake.bin index fc49547bfc..86b5751449 100755 Binary files a/panda/board/obj/bootstub.panda_tesla_wake.bin and b/panda/board/obj/bootstub.panda_tesla_wake.bin differ diff --git a/panda/board/obj/bootstub.panda_tesla_wake_can_ignition_only.bin b/panda/board/obj/bootstub.panda_tesla_wake_can_ignition_only.bin index fc49547bfc..86b5751449 100755 Binary files a/panda/board/obj/bootstub.panda_tesla_wake_can_ignition_only.bin and b/panda/board/obj/bootstub.panda_tesla_wake_can_ignition_only.bin differ diff --git a/panda/board/obj/gitversion.h b/panda/board/obj/gitversion.h index 048d623b57..53356de31a 100644 --- a/panda/board/obj/gitversion.h +++ b/panda/board/obj/gitversion.h @@ -1,2 +1,2 @@ extern const uint8_t gitversion[19]; -const uint8_t gitversion[19] = "DEV-14370fe9-DEBUG"; +const uint8_t gitversion[19] = "DEV-5925aecd-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 2c74c4dea1..dca26a5ce4 100644 Binary files a/panda/board/obj/panda.bin.signed and b/panda/board/obj/panda.bin.signed differ diff --git a/panda/board/obj/panda/bootstub.elf b/panda/board/obj/panda/bootstub.elf index 8da208df13..75ae586a43 100755 Binary files a/panda/board/obj/panda/bootstub.elf and b/panda/board/obj/panda/bootstub.elf differ diff --git a/panda/board/obj/panda/main.bin b/panda/board/obj/panda/main.bin index 6851c0c242..1f8f3177ae 100755 Binary files a/panda/board/obj/panda/main.bin and b/panda/board/obj/panda/main.bin differ diff --git a/panda/board/obj/panda/main.elf b/panda/board/obj/panda/main.elf index 86392f60c3..21da41de94 100755 Binary files a/panda/board/obj/panda/main.elf and b/panda/board/obj/panda/main.elf differ diff --git a/panda/board/obj/panda_can_ignition_only.bin.signed b/panda/board/obj/panda_can_ignition_only.bin.signed index d254d9797a..f79b78221d 100644 Binary files a/panda/board/obj/panda_can_ignition_only.bin.signed and b/panda/board/obj/panda_can_ignition_only.bin.signed differ diff --git a/panda/board/obj/panda_can_ignition_only/bootstub.elf b/panda/board/obj/panda_can_ignition_only/bootstub.elf index ed85d98591..b70ceb2b09 100755 Binary files a/panda/board/obj/panda_can_ignition_only/bootstub.elf and b/panda/board/obj/panda_can_ignition_only/bootstub.elf differ diff --git a/panda/board/obj/panda_can_ignition_only/main.bin b/panda/board/obj/panda_can_ignition_only/main.bin index baf15fa984..0815134c6d 100755 Binary files a/panda/board/obj/panda_can_ignition_only/main.bin and b/panda/board/obj/panda_can_ignition_only/main.bin differ diff --git a/panda/board/obj/panda_can_ignition_only/main.elf b/panda/board/obj/panda_can_ignition_only/main.elf index e2a5b2e375..e546c8e9f1 100755 Binary files a/panda/board/obj/panda_can_ignition_only/main.elf and b/panda/board/obj/panda_can_ignition_only/main.elf differ diff --git a/panda/board/obj/panda_h7.bin.signed b/panda/board/obj/panda_h7.bin.signed index ece8a6279b..a25f55c374 100644 Binary files a/panda/board/obj/panda_h7.bin.signed and b/panda/board/obj/panda_h7.bin.signed differ diff --git a/panda/board/obj/panda_h7/bootstub.elf b/panda/board/obj/panda_h7/bootstub.elf index 218e7e506a..4dab7027f5 100755 Binary files a/panda/board/obj/panda_h7/bootstub.elf and b/panda/board/obj/panda_h7/bootstub.elf differ diff --git a/panda/board/obj/panda_h7/main.bin b/panda/board/obj/panda_h7/main.bin index 34a60ad268..821a06c0e3 100755 Binary files a/panda/board/obj/panda_h7/main.bin and b/panda/board/obj/panda_h7/main.bin differ diff --git a/panda/board/obj/panda_h7/main.elf b/panda/board/obj/panda_h7/main.elf index 3bd694fa08..093060ef61 100755 Binary files a/panda/board/obj/panda_h7/main.elf and b/panda/board/obj/panda_h7/main.elf differ diff --git a/panda/board/obj/panda_h7_can_ignition_only.bin.signed b/panda/board/obj/panda_h7_can_ignition_only.bin.signed index 150501e8fb..5087d2c273 100644 Binary files a/panda/board/obj/panda_h7_can_ignition_only.bin.signed and b/panda/board/obj/panda_h7_can_ignition_only.bin.signed differ diff --git a/panda/board/obj/panda_h7_can_ignition_only/bootstub.elf b/panda/board/obj/panda_h7_can_ignition_only/bootstub.elf index 56948ddeb1..ea3fd76c59 100755 Binary files a/panda/board/obj/panda_h7_can_ignition_only/bootstub.elf and b/panda/board/obj/panda_h7_can_ignition_only/bootstub.elf differ diff --git a/panda/board/obj/panda_h7_can_ignition_only/main.bin b/panda/board/obj/panda_h7_can_ignition_only/main.bin index 7701832c87..08aea49922 100755 Binary files a/panda/board/obj/panda_h7_can_ignition_only/main.bin and b/panda/board/obj/panda_h7_can_ignition_only/main.bin differ diff --git a/panda/board/obj/panda_h7_can_ignition_only/main.elf b/panda/board/obj/panda_h7_can_ignition_only/main.elf index e7426f9f46..51102985ec 100755 Binary files a/panda/board/obj/panda_h7_can_ignition_only/main.elf and b/panda/board/obj/panda_h7_can_ignition_only/main.elf differ diff --git a/panda/board/obj/panda_h7_hkg_remote.bin.signed b/panda/board/obj/panda_h7_hkg_remote.bin.signed index ec45221611..8fc39e1563 100644 Binary files a/panda/board/obj/panda_h7_hkg_remote.bin.signed and b/panda/board/obj/panda_h7_hkg_remote.bin.signed differ diff --git a/panda/board/obj/panda_h7_hkg_remote/bootstub.elf b/panda/board/obj/panda_h7_hkg_remote/bootstub.elf index b78a90f6aa..2d8b011842 100755 Binary files a/panda/board/obj/panda_h7_hkg_remote/bootstub.elf and b/panda/board/obj/panda_h7_hkg_remote/bootstub.elf differ diff --git a/panda/board/obj/panda_h7_hkg_remote/main.bin b/panda/board/obj/panda_h7_hkg_remote/main.bin index 82b31668c2..55b8b3aeda 100755 Binary files a/panda/board/obj/panda_h7_hkg_remote/main.bin and b/panda/board/obj/panda_h7_hkg_remote/main.bin differ diff --git a/panda/board/obj/panda_h7_hkg_remote/main.elf b/panda/board/obj/panda_h7_hkg_remote/main.elf index cb377a73a6..84e95a4f62 100755 Binary files a/panda/board/obj/panda_h7_hkg_remote/main.elf and b/panda/board/obj/panda_h7_hkg_remote/main.elf differ diff --git a/panda/board/obj/panda_h7_hkg_remote_can_ignition_only.bin.signed b/panda/board/obj/panda_h7_hkg_remote_can_ignition_only.bin.signed index b094b25a56..e568e8b367 100644 Binary files a/panda/board/obj/panda_h7_hkg_remote_can_ignition_only.bin.signed and b/panda/board/obj/panda_h7_hkg_remote_can_ignition_only.bin.signed differ diff --git a/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/bootstub.elf b/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/bootstub.elf index 70d5fb44c9..f4b15296be 100755 Binary files a/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/bootstub.elf and b/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/bootstub.elf differ diff --git a/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.bin b/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.bin index a0825a8cad..8c538733e2 100755 Binary files a/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.bin and b/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.bin differ diff --git a/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.elf b/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.elf index 3eeface422..d1600c646e 100755 Binary files a/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.elf and b/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.elf differ diff --git a/panda/board/obj/panda_h7_remote.bin.signed b/panda/board/obj/panda_h7_remote.bin.signed index c175e003a2..528f6b9150 100644 Binary files a/panda/board/obj/panda_h7_remote.bin.signed and b/panda/board/obj/panda_h7_remote.bin.signed differ diff --git a/panda/board/obj/panda_h7_remote/bootstub.elf b/panda/board/obj/panda_h7_remote/bootstub.elf index de5e5d1707..2a15514843 100755 Binary files a/panda/board/obj/panda_h7_remote/bootstub.elf and b/panda/board/obj/panda_h7_remote/bootstub.elf differ diff --git a/panda/board/obj/panda_h7_remote/main.bin b/panda/board/obj/panda_h7_remote/main.bin index aae1ec6c8d..0fab6d028d 100755 Binary files a/panda/board/obj/panda_h7_remote/main.bin and b/panda/board/obj/panda_h7_remote/main.bin differ diff --git a/panda/board/obj/panda_h7_remote/main.elf b/panda/board/obj/panda_h7_remote/main.elf index 9fe86cb5d7..c463e03eed 100755 Binary files a/panda/board/obj/panda_h7_remote/main.elf and b/panda/board/obj/panda_h7_remote/main.elf differ diff --git a/panda/board/obj/panda_h7_remote_can_ignition_only.bin.signed b/panda/board/obj/panda_h7_remote_can_ignition_only.bin.signed index c00fcd6d3d..8dda9c69f4 100644 Binary files a/panda/board/obj/panda_h7_remote_can_ignition_only.bin.signed and b/panda/board/obj/panda_h7_remote_can_ignition_only.bin.signed differ diff --git a/panda/board/obj/panda_h7_remote_can_ignition_only/bootstub.elf b/panda/board/obj/panda_h7_remote_can_ignition_only/bootstub.elf index 49db04db7a..a7de9c28c4 100755 Binary files a/panda/board/obj/panda_h7_remote_can_ignition_only/bootstub.elf and b/panda/board/obj/panda_h7_remote_can_ignition_only/bootstub.elf differ diff --git a/panda/board/obj/panda_h7_remote_can_ignition_only/main.bin b/panda/board/obj/panda_h7_remote_can_ignition_only/main.bin index 96858abeb2..26741c20a8 100755 Binary files a/panda/board/obj/panda_h7_remote_can_ignition_only/main.bin and b/panda/board/obj/panda_h7_remote_can_ignition_only/main.bin differ diff --git a/panda/board/obj/panda_h7_remote_can_ignition_only/main.elf b/panda/board/obj/panda_h7_remote_can_ignition_only/main.elf index 477b411ea2..a938a93c2f 100755 Binary files a/panda/board/obj/panda_h7_remote_can_ignition_only/main.elf and b/panda/board/obj/panda_h7_remote_can_ignition_only/main.elf differ diff --git a/panda/board/obj/panda_h7_tesla_wake.bin.signed b/panda/board/obj/panda_h7_tesla_wake.bin.signed index 5ff7d898d1..c4467d99cb 100644 Binary files a/panda/board/obj/panda_h7_tesla_wake.bin.signed and b/panda/board/obj/panda_h7_tesla_wake.bin.signed differ diff --git a/panda/board/obj/panda_h7_tesla_wake/bootstub.elf b/panda/board/obj/panda_h7_tesla_wake/bootstub.elf index 3bbd8fae9f..9c9415150d 100755 Binary files a/panda/board/obj/panda_h7_tesla_wake/bootstub.elf and b/panda/board/obj/panda_h7_tesla_wake/bootstub.elf differ diff --git a/panda/board/obj/panda_h7_tesla_wake/main.bin b/panda/board/obj/panda_h7_tesla_wake/main.bin index 1f3c8b53dd..2a31d214ee 100755 Binary files a/panda/board/obj/panda_h7_tesla_wake/main.bin and b/panda/board/obj/panda_h7_tesla_wake/main.bin differ diff --git a/panda/board/obj/panda_h7_tesla_wake/main.elf b/panda/board/obj/panda_h7_tesla_wake/main.elf index 95c9da2126..507a4579a7 100755 Binary files a/panda/board/obj/panda_h7_tesla_wake/main.elf and b/panda/board/obj/panda_h7_tesla_wake/main.elf differ diff --git a/panda/board/obj/panda_h7_tesla_wake_can_ignition_only.bin.signed b/panda/board/obj/panda_h7_tesla_wake_can_ignition_only.bin.signed index 1cb003bfd0..a72fd0f707 100644 Binary files a/panda/board/obj/panda_h7_tesla_wake_can_ignition_only.bin.signed and b/panda/board/obj/panda_h7_tesla_wake_can_ignition_only.bin.signed differ diff --git a/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/bootstub.elf b/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/bootstub.elf index 42a2560ce4..7c6cb1cd40 100755 Binary files a/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/bootstub.elf and b/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/bootstub.elf differ diff --git a/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/main.bin b/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/main.bin index dab715e12a..a7fca06818 100755 Binary files a/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/main.bin and b/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/main.bin differ diff --git a/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/main.elf b/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/main.elf index d9dc76cba6..57fe342e1d 100755 Binary files a/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/main.elf and b/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/main.elf differ diff --git a/panda/board/obj/panda_hkg_remote.bin.signed b/panda/board/obj/panda_hkg_remote.bin.signed index 30ae17e5ae..8a88d53606 100644 Binary files a/panda/board/obj/panda_hkg_remote.bin.signed and b/panda/board/obj/panda_hkg_remote.bin.signed differ diff --git a/panda/board/obj/panda_hkg_remote/bootstub.elf b/panda/board/obj/panda_hkg_remote/bootstub.elf index 9b10eb06e2..7ec488396e 100755 Binary files a/panda/board/obj/panda_hkg_remote/bootstub.elf and b/panda/board/obj/panda_hkg_remote/bootstub.elf differ diff --git a/panda/board/obj/panda_hkg_remote/main.bin b/panda/board/obj/panda_hkg_remote/main.bin index daee8677d2..6c1eeaedb5 100755 Binary files a/panda/board/obj/panda_hkg_remote/main.bin and b/panda/board/obj/panda_hkg_remote/main.bin differ diff --git a/panda/board/obj/panda_hkg_remote/main.elf b/panda/board/obj/panda_hkg_remote/main.elf index 4813a3c923..38b139124b 100755 Binary files a/panda/board/obj/panda_hkg_remote/main.elf and b/panda/board/obj/panda_hkg_remote/main.elf differ diff --git a/panda/board/obj/panda_hkg_remote_can_ignition_only.bin.signed b/panda/board/obj/panda_hkg_remote_can_ignition_only.bin.signed index 2fdbcb040a..13e9675d2b 100644 Binary files a/panda/board/obj/panda_hkg_remote_can_ignition_only.bin.signed and b/panda/board/obj/panda_hkg_remote_can_ignition_only.bin.signed differ diff --git a/panda/board/obj/panda_hkg_remote_can_ignition_only/bootstub.elf b/panda/board/obj/panda_hkg_remote_can_ignition_only/bootstub.elf index 2b54184adb..fc0c57fcbf 100755 Binary files a/panda/board/obj/panda_hkg_remote_can_ignition_only/bootstub.elf and b/panda/board/obj/panda_hkg_remote_can_ignition_only/bootstub.elf differ diff --git a/panda/board/obj/panda_hkg_remote_can_ignition_only/main.bin b/panda/board/obj/panda_hkg_remote_can_ignition_only/main.bin index b65be9af77..0454cb528e 100755 Binary files a/panda/board/obj/panda_hkg_remote_can_ignition_only/main.bin and b/panda/board/obj/panda_hkg_remote_can_ignition_only/main.bin differ diff --git a/panda/board/obj/panda_hkg_remote_can_ignition_only/main.elf b/panda/board/obj/panda_hkg_remote_can_ignition_only/main.elf index 5d6d02421d..d45c2c3464 100755 Binary files a/panda/board/obj/panda_hkg_remote_can_ignition_only/main.elf and b/panda/board/obj/panda_hkg_remote_can_ignition_only/main.elf differ diff --git a/panda/board/obj/panda_jungle_h7.bin.signed b/panda/board/obj/panda_jungle_h7.bin.signed index 38c6926606..b5e1d88732 100644 Binary files a/panda/board/obj/panda_jungle_h7.bin.signed and b/panda/board/obj/panda_jungle_h7.bin.signed differ diff --git a/panda/board/obj/panda_jungle_h7/bootstub.elf b/panda/board/obj/panda_jungle_h7/bootstub.elf index 0b251a368d..fbea809c7c 100755 Binary files a/panda/board/obj/panda_jungle_h7/bootstub.elf and b/panda/board/obj/panda_jungle_h7/bootstub.elf differ diff --git a/panda/board/obj/panda_jungle_h7/main.bin b/panda/board/obj/panda_jungle_h7/main.bin index 44aab407f2..6a5e1fd2a4 100755 Binary files a/panda/board/obj/panda_jungle_h7/main.bin and b/panda/board/obj/panda_jungle_h7/main.bin differ diff --git a/panda/board/obj/panda_jungle_h7/main.elf b/panda/board/obj/panda_jungle_h7/main.elf index c56bdb01e1..9087bd18b6 100755 Binary files a/panda/board/obj/panda_jungle_h7/main.elf and b/panda/board/obj/panda_jungle_h7/main.elf differ diff --git a/panda/board/obj/panda_remote.bin.signed b/panda/board/obj/panda_remote.bin.signed index 0d3b4fd0b2..44f8ee2070 100644 Binary files a/panda/board/obj/panda_remote.bin.signed and b/panda/board/obj/panda_remote.bin.signed differ diff --git a/panda/board/obj/panda_remote/bootstub.elf b/panda/board/obj/panda_remote/bootstub.elf index 48d7d2b748..f63d3b9c10 100755 Binary files a/panda/board/obj/panda_remote/bootstub.elf and b/panda/board/obj/panda_remote/bootstub.elf differ diff --git a/panda/board/obj/panda_remote/main.bin b/panda/board/obj/panda_remote/main.bin index 2172f591c2..c831081f3e 100755 Binary files a/panda/board/obj/panda_remote/main.bin and b/panda/board/obj/panda_remote/main.bin differ diff --git a/panda/board/obj/panda_remote/main.elf b/panda/board/obj/panda_remote/main.elf index 5a83b4c085..e64dff12e1 100755 Binary files a/panda/board/obj/panda_remote/main.elf and b/panda/board/obj/panda_remote/main.elf differ diff --git a/panda/board/obj/panda_remote_can_ignition_only.bin.signed b/panda/board/obj/panda_remote_can_ignition_only.bin.signed index 0f82f3aea3..65d1f86d7d 100644 Binary files a/panda/board/obj/panda_remote_can_ignition_only.bin.signed and b/panda/board/obj/panda_remote_can_ignition_only.bin.signed differ diff --git a/panda/board/obj/panda_remote_can_ignition_only/bootstub.elf b/panda/board/obj/panda_remote_can_ignition_only/bootstub.elf index bc141088ce..d74133701e 100755 Binary files a/panda/board/obj/panda_remote_can_ignition_only/bootstub.elf and b/panda/board/obj/panda_remote_can_ignition_only/bootstub.elf differ diff --git a/panda/board/obj/panda_remote_can_ignition_only/main.bin b/panda/board/obj/panda_remote_can_ignition_only/main.bin index 419d992cba..bb3b2d8073 100755 Binary files a/panda/board/obj/panda_remote_can_ignition_only/main.bin and b/panda/board/obj/panda_remote_can_ignition_only/main.bin differ diff --git a/panda/board/obj/panda_remote_can_ignition_only/main.elf b/panda/board/obj/panda_remote_can_ignition_only/main.elf index 592e14d570..ccc0122a73 100755 Binary files a/panda/board/obj/panda_remote_can_ignition_only/main.elf and b/panda/board/obj/panda_remote_can_ignition_only/main.elf differ diff --git a/panda/board/obj/panda_tesla_wake.bin.signed b/panda/board/obj/panda_tesla_wake.bin.signed index f6999e30d2..faa4712b85 100644 Binary files a/panda/board/obj/panda_tesla_wake.bin.signed and b/panda/board/obj/panda_tesla_wake.bin.signed differ diff --git a/panda/board/obj/panda_tesla_wake/bootstub.elf b/panda/board/obj/panda_tesla_wake/bootstub.elf index 0feec8e51c..95ea7f7531 100755 Binary files a/panda/board/obj/panda_tesla_wake/bootstub.elf and b/panda/board/obj/panda_tesla_wake/bootstub.elf differ diff --git a/panda/board/obj/panda_tesla_wake/main.bin b/panda/board/obj/panda_tesla_wake/main.bin index 45207317d4..b6922da429 100755 Binary files a/panda/board/obj/panda_tesla_wake/main.bin and b/panda/board/obj/panda_tesla_wake/main.bin differ diff --git a/panda/board/obj/panda_tesla_wake/main.elf b/panda/board/obj/panda_tesla_wake/main.elf index d5a02f81ae..f838133165 100755 Binary files a/panda/board/obj/panda_tesla_wake/main.elf and b/panda/board/obj/panda_tesla_wake/main.elf differ diff --git a/panda/board/obj/panda_tesla_wake_can_ignition_only.bin.signed b/panda/board/obj/panda_tesla_wake_can_ignition_only.bin.signed index 7884e40b50..4b51424d45 100644 Binary files a/panda/board/obj/panda_tesla_wake_can_ignition_only.bin.signed and b/panda/board/obj/panda_tesla_wake_can_ignition_only.bin.signed differ diff --git a/panda/board/obj/panda_tesla_wake_can_ignition_only/bootstub.elf b/panda/board/obj/panda_tesla_wake_can_ignition_only/bootstub.elf index f0e7097508..bdf3cef2b3 100755 Binary files a/panda/board/obj/panda_tesla_wake_can_ignition_only/bootstub.elf and b/panda/board/obj/panda_tesla_wake_can_ignition_only/bootstub.elf differ diff --git a/panda/board/obj/panda_tesla_wake_can_ignition_only/main.bin b/panda/board/obj/panda_tesla_wake_can_ignition_only/main.bin index 90178ee825..8b3fb90ece 100755 Binary files a/panda/board/obj/panda_tesla_wake_can_ignition_only/main.bin and b/panda/board/obj/panda_tesla_wake_can_ignition_only/main.bin differ diff --git a/panda/board/obj/panda_tesla_wake_can_ignition_only/main.elf b/panda/board/obj/panda_tesla_wake_can_ignition_only/main.elf index 3408cdef1e..2b63b31d90 100755 Binary files a/panda/board/obj/panda_tesla_wake_can_ignition_only/main.elf and b/panda/board/obj/panda_tesla_wake_can_ignition_only/main.elf differ diff --git a/panda/board/obj/version b/panda/board/obj/version index f565edc88d..c6613aa6e5 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-14370fe9-DEBUG \ No newline at end of file +DEV-5925aecd-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 141d2f9843..72852c091f 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_6314153535178174931) { - out_6314153535178174931[0] = delta_x[0] + nom_x[0]; - out_6314153535178174931[1] = delta_x[1] + nom_x[1]; - out_6314153535178174931[2] = delta_x[2] + nom_x[2]; - out_6314153535178174931[3] = delta_x[3] + nom_x[3]; - out_6314153535178174931[4] = delta_x[4] + nom_x[4]; - out_6314153535178174931[5] = delta_x[5] + nom_x[5]; - out_6314153535178174931[6] = delta_x[6] + nom_x[6]; - out_6314153535178174931[7] = delta_x[7] + nom_x[7]; - out_6314153535178174931[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_6680589116319572635) { + out_6680589116319572635[0] = delta_x[0] + nom_x[0]; + out_6680589116319572635[1] = delta_x[1] + nom_x[1]; + out_6680589116319572635[2] = delta_x[2] + nom_x[2]; + out_6680589116319572635[3] = delta_x[3] + nom_x[3]; + out_6680589116319572635[4] = delta_x[4] + nom_x[4]; + out_6680589116319572635[5] = delta_x[5] + nom_x[5]; + out_6680589116319572635[6] = delta_x[6] + nom_x[6]; + out_6680589116319572635[7] = delta_x[7] + nom_x[7]; + out_6680589116319572635[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_7049921009392106379) { - out_7049921009392106379[0] = -nom_x[0] + true_x[0]; - out_7049921009392106379[1] = -nom_x[1] + true_x[1]; - out_7049921009392106379[2] = -nom_x[2] + true_x[2]; - out_7049921009392106379[3] = -nom_x[3] + true_x[3]; - out_7049921009392106379[4] = -nom_x[4] + true_x[4]; - out_7049921009392106379[5] = -nom_x[5] + true_x[5]; - out_7049921009392106379[6] = -nom_x[6] + true_x[6]; - out_7049921009392106379[7] = -nom_x[7] + true_x[7]; - out_7049921009392106379[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_1421263034200107343) { + out_1421263034200107343[0] = -nom_x[0] + true_x[0]; + out_1421263034200107343[1] = -nom_x[1] + true_x[1]; + out_1421263034200107343[2] = -nom_x[2] + true_x[2]; + out_1421263034200107343[3] = -nom_x[3] + true_x[3]; + out_1421263034200107343[4] = -nom_x[4] + true_x[4]; + out_1421263034200107343[5] = -nom_x[5] + true_x[5]; + out_1421263034200107343[6] = -nom_x[6] + true_x[6]; + out_1421263034200107343[7] = -nom_x[7] + true_x[7]; + out_1421263034200107343[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_9070114660809295187) { - out_9070114660809295187[0] = 1.0; - out_9070114660809295187[1] = 0.0; - out_9070114660809295187[2] = 0.0; - out_9070114660809295187[3] = 0.0; - out_9070114660809295187[4] = 0.0; - out_9070114660809295187[5] = 0.0; - out_9070114660809295187[6] = 0.0; - out_9070114660809295187[7] = 0.0; - out_9070114660809295187[8] = 0.0; - out_9070114660809295187[9] = 0.0; - out_9070114660809295187[10] = 1.0; - out_9070114660809295187[11] = 0.0; - out_9070114660809295187[12] = 0.0; - out_9070114660809295187[13] = 0.0; - out_9070114660809295187[14] = 0.0; - out_9070114660809295187[15] = 0.0; - out_9070114660809295187[16] = 0.0; - out_9070114660809295187[17] = 0.0; - out_9070114660809295187[18] = 0.0; - out_9070114660809295187[19] = 0.0; - out_9070114660809295187[20] = 1.0; - out_9070114660809295187[21] = 0.0; - out_9070114660809295187[22] = 0.0; - out_9070114660809295187[23] = 0.0; - out_9070114660809295187[24] = 0.0; - out_9070114660809295187[25] = 0.0; - out_9070114660809295187[26] = 0.0; - out_9070114660809295187[27] = 0.0; - out_9070114660809295187[28] = 0.0; - out_9070114660809295187[29] = 0.0; - out_9070114660809295187[30] = 1.0; - out_9070114660809295187[31] = 0.0; - out_9070114660809295187[32] = 0.0; - out_9070114660809295187[33] = 0.0; - out_9070114660809295187[34] = 0.0; - out_9070114660809295187[35] = 0.0; - out_9070114660809295187[36] = 0.0; - out_9070114660809295187[37] = 0.0; - out_9070114660809295187[38] = 0.0; - out_9070114660809295187[39] = 0.0; - out_9070114660809295187[40] = 1.0; - out_9070114660809295187[41] = 0.0; - out_9070114660809295187[42] = 0.0; - out_9070114660809295187[43] = 0.0; - out_9070114660809295187[44] = 0.0; - out_9070114660809295187[45] = 0.0; - out_9070114660809295187[46] = 0.0; - out_9070114660809295187[47] = 0.0; - out_9070114660809295187[48] = 0.0; - out_9070114660809295187[49] = 0.0; - out_9070114660809295187[50] = 1.0; - out_9070114660809295187[51] = 0.0; - out_9070114660809295187[52] = 0.0; - out_9070114660809295187[53] = 0.0; - out_9070114660809295187[54] = 0.0; - out_9070114660809295187[55] = 0.0; - out_9070114660809295187[56] = 0.0; - out_9070114660809295187[57] = 0.0; - out_9070114660809295187[58] = 0.0; - out_9070114660809295187[59] = 0.0; - out_9070114660809295187[60] = 1.0; - out_9070114660809295187[61] = 0.0; - out_9070114660809295187[62] = 0.0; - out_9070114660809295187[63] = 0.0; - out_9070114660809295187[64] = 0.0; - out_9070114660809295187[65] = 0.0; - out_9070114660809295187[66] = 0.0; - out_9070114660809295187[67] = 0.0; - out_9070114660809295187[68] = 0.0; - out_9070114660809295187[69] = 0.0; - out_9070114660809295187[70] = 1.0; - out_9070114660809295187[71] = 0.0; - out_9070114660809295187[72] = 0.0; - out_9070114660809295187[73] = 0.0; - out_9070114660809295187[74] = 0.0; - out_9070114660809295187[75] = 0.0; - out_9070114660809295187[76] = 0.0; - out_9070114660809295187[77] = 0.0; - out_9070114660809295187[78] = 0.0; - out_9070114660809295187[79] = 0.0; - out_9070114660809295187[80] = 1.0; +void H_mod_fun(double *state, double *out_8968580721052785900) { + out_8968580721052785900[0] = 1.0; + out_8968580721052785900[1] = 0.0; + out_8968580721052785900[2] = 0.0; + out_8968580721052785900[3] = 0.0; + out_8968580721052785900[4] = 0.0; + out_8968580721052785900[5] = 0.0; + out_8968580721052785900[6] = 0.0; + out_8968580721052785900[7] = 0.0; + out_8968580721052785900[8] = 0.0; + out_8968580721052785900[9] = 0.0; + out_8968580721052785900[10] = 1.0; + out_8968580721052785900[11] = 0.0; + out_8968580721052785900[12] = 0.0; + out_8968580721052785900[13] = 0.0; + out_8968580721052785900[14] = 0.0; + out_8968580721052785900[15] = 0.0; + out_8968580721052785900[16] = 0.0; + out_8968580721052785900[17] = 0.0; + out_8968580721052785900[18] = 0.0; + out_8968580721052785900[19] = 0.0; + out_8968580721052785900[20] = 1.0; + out_8968580721052785900[21] = 0.0; + out_8968580721052785900[22] = 0.0; + out_8968580721052785900[23] = 0.0; + out_8968580721052785900[24] = 0.0; + out_8968580721052785900[25] = 0.0; + out_8968580721052785900[26] = 0.0; + out_8968580721052785900[27] = 0.0; + out_8968580721052785900[28] = 0.0; + out_8968580721052785900[29] = 0.0; + out_8968580721052785900[30] = 1.0; + out_8968580721052785900[31] = 0.0; + out_8968580721052785900[32] = 0.0; + out_8968580721052785900[33] = 0.0; + out_8968580721052785900[34] = 0.0; + out_8968580721052785900[35] = 0.0; + out_8968580721052785900[36] = 0.0; + out_8968580721052785900[37] = 0.0; + out_8968580721052785900[38] = 0.0; + out_8968580721052785900[39] = 0.0; + out_8968580721052785900[40] = 1.0; + out_8968580721052785900[41] = 0.0; + out_8968580721052785900[42] = 0.0; + out_8968580721052785900[43] = 0.0; + out_8968580721052785900[44] = 0.0; + out_8968580721052785900[45] = 0.0; + out_8968580721052785900[46] = 0.0; + out_8968580721052785900[47] = 0.0; + out_8968580721052785900[48] = 0.0; + out_8968580721052785900[49] = 0.0; + out_8968580721052785900[50] = 1.0; + out_8968580721052785900[51] = 0.0; + out_8968580721052785900[52] = 0.0; + out_8968580721052785900[53] = 0.0; + out_8968580721052785900[54] = 0.0; + out_8968580721052785900[55] = 0.0; + out_8968580721052785900[56] = 0.0; + out_8968580721052785900[57] = 0.0; + out_8968580721052785900[58] = 0.0; + out_8968580721052785900[59] = 0.0; + out_8968580721052785900[60] = 1.0; + out_8968580721052785900[61] = 0.0; + out_8968580721052785900[62] = 0.0; + out_8968580721052785900[63] = 0.0; + out_8968580721052785900[64] = 0.0; + out_8968580721052785900[65] = 0.0; + out_8968580721052785900[66] = 0.0; + out_8968580721052785900[67] = 0.0; + out_8968580721052785900[68] = 0.0; + out_8968580721052785900[69] = 0.0; + out_8968580721052785900[70] = 1.0; + out_8968580721052785900[71] = 0.0; + out_8968580721052785900[72] = 0.0; + out_8968580721052785900[73] = 0.0; + out_8968580721052785900[74] = 0.0; + out_8968580721052785900[75] = 0.0; + out_8968580721052785900[76] = 0.0; + out_8968580721052785900[77] = 0.0; + out_8968580721052785900[78] = 0.0; + out_8968580721052785900[79] = 0.0; + out_8968580721052785900[80] = 1.0; } -void f_fun(double *state, double dt, double *out_7847765386725654972) { - out_7847765386725654972[0] = state[0]; - out_7847765386725654972[1] = state[1]; - out_7847765386725654972[2] = state[2]; - out_7847765386725654972[3] = state[3]; - out_7847765386725654972[4] = state[4]; - out_7847765386725654972[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*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_7847765386725654972[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_7847765386725654972[7] = state[7]; - out_7847765386725654972[8] = state[8]; +void f_fun(double *state, double dt, double *out_3852536059217665062) { + out_3852536059217665062[0] = state[0]; + out_3852536059217665062[1] = state[1]; + out_3852536059217665062[2] = state[2]; + out_3852536059217665062[3] = state[3]; + out_3852536059217665062[4] = state[4]; + out_3852536059217665062[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*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_3852536059217665062[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_3852536059217665062[7] = state[7]; + out_3852536059217665062[8] = state[8]; } -void F_fun(double *state, double dt, double *out_6490326381461081645) { - out_6490326381461081645[0] = 1; - out_6490326381461081645[1] = 0; - out_6490326381461081645[2] = 0; - out_6490326381461081645[3] = 0; - out_6490326381461081645[4] = 0; - out_6490326381461081645[5] = 0; - out_6490326381461081645[6] = 0; - out_6490326381461081645[7] = 0; - out_6490326381461081645[8] = 0; - out_6490326381461081645[9] = 0; - out_6490326381461081645[10] = 1; - out_6490326381461081645[11] = 0; - out_6490326381461081645[12] = 0; - out_6490326381461081645[13] = 0; - out_6490326381461081645[14] = 0; - out_6490326381461081645[15] = 0; - out_6490326381461081645[16] = 0; - out_6490326381461081645[17] = 0; - out_6490326381461081645[18] = 0; - out_6490326381461081645[19] = 0; - out_6490326381461081645[20] = 1; - out_6490326381461081645[21] = 0; - out_6490326381461081645[22] = 0; - out_6490326381461081645[23] = 0; - out_6490326381461081645[24] = 0; - out_6490326381461081645[25] = 0; - out_6490326381461081645[26] = 0; - out_6490326381461081645[27] = 0; - out_6490326381461081645[28] = 0; - out_6490326381461081645[29] = 0; - out_6490326381461081645[30] = 1; - out_6490326381461081645[31] = 0; - out_6490326381461081645[32] = 0; - out_6490326381461081645[33] = 0; - out_6490326381461081645[34] = 0; - out_6490326381461081645[35] = 0; - out_6490326381461081645[36] = 0; - out_6490326381461081645[37] = 0; - out_6490326381461081645[38] = 0; - out_6490326381461081645[39] = 0; - out_6490326381461081645[40] = 1; - out_6490326381461081645[41] = 0; - out_6490326381461081645[42] = 0; - out_6490326381461081645[43] = 0; - out_6490326381461081645[44] = 0; - out_6490326381461081645[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_6490326381461081645[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_6490326381461081645[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_6490326381461081645[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_6490326381461081645[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_6490326381461081645[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_6490326381461081645[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_6490326381461081645[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_6490326381461081645[53] = -9.8100000000000005*dt; - out_6490326381461081645[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_6490326381461081645[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_6490326381461081645[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6490326381461081645[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6490326381461081645[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_6490326381461081645[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_6490326381461081645[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_6490326381461081645[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6490326381461081645[62] = 0; - out_6490326381461081645[63] = 0; - out_6490326381461081645[64] = 0; - out_6490326381461081645[65] = 0; - out_6490326381461081645[66] = 0; - out_6490326381461081645[67] = 0; - out_6490326381461081645[68] = 0; - out_6490326381461081645[69] = 0; - out_6490326381461081645[70] = 1; - out_6490326381461081645[71] = 0; - out_6490326381461081645[72] = 0; - out_6490326381461081645[73] = 0; - out_6490326381461081645[74] = 0; - out_6490326381461081645[75] = 0; - out_6490326381461081645[76] = 0; - out_6490326381461081645[77] = 0; - out_6490326381461081645[78] = 0; - out_6490326381461081645[79] = 0; - out_6490326381461081645[80] = 1; +void F_fun(double *state, double dt, double *out_7217781527381652799) { + out_7217781527381652799[0] = 1; + out_7217781527381652799[1] = 0; + out_7217781527381652799[2] = 0; + out_7217781527381652799[3] = 0; + out_7217781527381652799[4] = 0; + out_7217781527381652799[5] = 0; + out_7217781527381652799[6] = 0; + out_7217781527381652799[7] = 0; + out_7217781527381652799[8] = 0; + out_7217781527381652799[9] = 0; + out_7217781527381652799[10] = 1; + out_7217781527381652799[11] = 0; + out_7217781527381652799[12] = 0; + out_7217781527381652799[13] = 0; + out_7217781527381652799[14] = 0; + out_7217781527381652799[15] = 0; + out_7217781527381652799[16] = 0; + out_7217781527381652799[17] = 0; + out_7217781527381652799[18] = 0; + out_7217781527381652799[19] = 0; + out_7217781527381652799[20] = 1; + out_7217781527381652799[21] = 0; + out_7217781527381652799[22] = 0; + out_7217781527381652799[23] = 0; + out_7217781527381652799[24] = 0; + out_7217781527381652799[25] = 0; + out_7217781527381652799[26] = 0; + out_7217781527381652799[27] = 0; + out_7217781527381652799[28] = 0; + out_7217781527381652799[29] = 0; + out_7217781527381652799[30] = 1; + out_7217781527381652799[31] = 0; + out_7217781527381652799[32] = 0; + out_7217781527381652799[33] = 0; + out_7217781527381652799[34] = 0; + out_7217781527381652799[35] = 0; + out_7217781527381652799[36] = 0; + out_7217781527381652799[37] = 0; + out_7217781527381652799[38] = 0; + out_7217781527381652799[39] = 0; + out_7217781527381652799[40] = 1; + out_7217781527381652799[41] = 0; + out_7217781527381652799[42] = 0; + out_7217781527381652799[43] = 0; + out_7217781527381652799[44] = 0; + out_7217781527381652799[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_7217781527381652799[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_7217781527381652799[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_7217781527381652799[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_7217781527381652799[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_7217781527381652799[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_7217781527381652799[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_7217781527381652799[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_7217781527381652799[53] = -9.8100000000000005*dt; + out_7217781527381652799[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_7217781527381652799[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_7217781527381652799[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_7217781527381652799[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_7217781527381652799[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_7217781527381652799[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_7217781527381652799[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_7217781527381652799[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_7217781527381652799[62] = 0; + out_7217781527381652799[63] = 0; + out_7217781527381652799[64] = 0; + out_7217781527381652799[65] = 0; + out_7217781527381652799[66] = 0; + out_7217781527381652799[67] = 0; + out_7217781527381652799[68] = 0; + out_7217781527381652799[69] = 0; + out_7217781527381652799[70] = 1; + out_7217781527381652799[71] = 0; + out_7217781527381652799[72] = 0; + out_7217781527381652799[73] = 0; + out_7217781527381652799[74] = 0; + out_7217781527381652799[75] = 0; + out_7217781527381652799[76] = 0; + out_7217781527381652799[77] = 0; + out_7217781527381652799[78] = 0; + out_7217781527381652799[79] = 0; + out_7217781527381652799[80] = 1; } -void h_25(double *state, double *unused, double *out_258705256034994534) { - out_258705256034994534[0] = state[6]; +void h_25(double *state, double *unused, double *out_2838193808077509293) { + out_2838193808077509293[0] = state[6]; } -void H_25(double *state, double *unused, double *out_123073565102942050) { - out_123073565102942050[0] = 0; - out_123073565102942050[1] = 0; - out_123073565102942050[2] = 0; - out_123073565102942050[3] = 0; - out_123073565102942050[4] = 0; - out_123073565102942050[5] = 0; - out_123073565102942050[6] = 1; - out_123073565102942050[7] = 0; - out_123073565102942050[8] = 0; +void H_25(double *state, double *unused, double *out_4085073539734055094) { + out_4085073539734055094[0] = 0; + out_4085073539734055094[1] = 0; + out_4085073539734055094[2] = 0; + out_4085073539734055094[3] = 0; + out_4085073539734055094[4] = 0; + out_4085073539734055094[5] = 0; + out_4085073539734055094[6] = 1; + out_4085073539734055094[7] = 0; + out_4085073539734055094[8] = 0; } -void h_24(double *state, double *unused, double *out_5989007342487248666) { - out_5989007342487248666[0] = state[4]; - out_5989007342487248666[1] = state[5]; +void h_24(double *state, double *unused, double *out_7378709950093750440) { + out_7378709950093750440[0] = state[4]; + out_7378709950093750440[1] = state[5]; } -void H_24(double *state, double *unused, double *out_4869582892069451084) { - out_4869582892069451084[0] = 0; - out_4869582892069451084[1] = 0; - out_4869582892069451084[2] = 0; - out_4869582892069451084[3] = 0; - out_4869582892069451084[4] = 1; - out_4869582892069451084[5] = 0; - out_4869582892069451084[6] = 0; - out_4869582892069451084[7] = 0; - out_4869582892069451084[8] = 0; - out_4869582892069451084[9] = 0; - out_4869582892069451084[10] = 0; - out_4869582892069451084[11] = 0; - out_4869582892069451084[12] = 0; - out_4869582892069451084[13] = 0; - out_4869582892069451084[14] = 1; - out_4869582892069451084[15] = 0; - out_4869582892069451084[16] = 0; - out_4869582892069451084[17] = 0; +void H_24(double *state, double *unused, double *out_2001100279119417225) { + out_2001100279119417225[0] = 0; + out_2001100279119417225[1] = 0; + out_2001100279119417225[2] = 0; + out_2001100279119417225[3] = 0; + out_2001100279119417225[4] = 1; + out_2001100279119417225[5] = 0; + out_2001100279119417225[6] = 0; + out_2001100279119417225[7] = 0; + out_2001100279119417225[8] = 0; + out_2001100279119417225[9] = 0; + out_2001100279119417225[10] = 0; + out_2001100279119417225[11] = 0; + out_2001100279119417225[12] = 0; + out_2001100279119417225[13] = 0; + out_2001100279119417225[14] = 1; + out_2001100279119417225[15] = 0; + out_2001100279119417225[16] = 0; + out_2001100279119417225[17] = 0; } -void h_30(double *state, double *unused, double *out_179991132081170203) { - out_179991132081170203[0] = state[4]; +void h_30(double *state, double *unused, double *out_6475643336331531643) { + out_6475643336331531643[0] = state[4]; } -void H_30(double *state, double *unused, double *out_6793616776388674705) { - out_6793616776388674705[0] = 0; - out_6793616776388674705[1] = 0; - out_6793616776388674705[2] = 0; - out_6793616776388674705[3] = 0; - out_6793616776388674705[4] = 1; - out_6793616776388674705[5] = 0; - out_6793616776388674705[6] = 0; - out_6793616776388674705[7] = 0; - out_6793616776388674705[8] = 0; +void H_30(double *state, double *unused, double *out_4214412486877295164) { + out_4214412486877295164[0] = 0; + out_4214412486877295164[1] = 0; + out_4214412486877295164[2] = 0; + out_4214412486877295164[3] = 0; + out_4214412486877295164[4] = 1; + out_4214412486877295164[5] = 0; + out_4214412486877295164[6] = 0; + out_4214412486877295164[7] = 0; + out_4214412486877295164[8] = 0; } -void h_26(double *state, double *unused, double *out_4821682135579675442) { - out_4821682135579675442[0] = state[7]; +void h_26(double *state, double *unused, double *out_8554827447728949957) { + out_8554827447728949957[0] = state[7]; } -void H_26(double *state, double *unused, double *out_3181452404657858551) { - out_3181452404657858551[0] = 0; - out_3181452404657858551[1] = 0; - out_3181452404657858551[2] = 0; - out_3181452404657858551[3] = 0; - out_3181452404657858551[4] = 0; - out_3181452404657858551[5] = 0; - out_3181452404657858551[6] = 0; - out_3181452404657858551[7] = 1; - out_3181452404657858551[8] = 0; +void H_26(double *state, double *unused, double *out_3428219475623743190) { + out_3428219475623743190[0] = 0; + out_3428219475623743190[1] = 0; + out_3428219475623743190[2] = 0; + out_3428219475623743190[3] = 0; + out_3428219475623743190[4] = 0; + out_3428219475623743190[5] = 0; + out_3428219475623743190[6] = 0; + out_3428219475623743190[7] = 1; + out_3428219475623743190[8] = 0; } -void h_27(double *state, double *unused, double *out_5260378523695840179) { - out_5260378523695840179[0] = state[3]; +void h_27(double *state, double *unused, double *out_1670450007001367556) { + out_1670450007001367556[0] = state[3]; } -void H_27(double *state, double *unused, double *out_4618853464588249794) { - out_4618853464588249794[0] = 0; - out_4618853464588249794[1] = 0; - out_4618853464588249794[2] = 0; - out_4618853464588249794[3] = 1; - out_4618853464588249794[4] = 0; - out_4618853464588249794[5] = 0; - out_4618853464588249794[6] = 0; - out_4618853464588249794[7] = 0; - out_4618853464588249794[8] = 0; +void H_27(double *state, double *unused, double *out_6389175798677720075) { + out_6389175798677720075[0] = 0; + out_6389175798677720075[1] = 0; + out_6389175798677720075[2] = 0; + out_6389175798677720075[3] = 1; + out_6389175798677720075[4] = 0; + out_6389175798677720075[5] = 0; + out_6389175798677720075[6] = 0; + out_6389175798677720075[7] = 0; + out_6389175798677720075[8] = 0; } -void h_29(double *state, double *unused, double *out_5535572585980346068) { - out_5535572585980346068[0] = state[1]; +void h_29(double *state, double *unused, double *out_6969427469717174328) { + out_6969427469717174328[0] = state[1]; } -void H_29(double *state, double *unused, double *out_2905490737718698761) { - out_2905490737718698761[0] = 0; - out_2905490737718698761[1] = 1; - out_2905490737718698761[2] = 0; - out_2905490737718698761[3] = 0; - out_2905490737718698761[4] = 0; - out_2905490737718698761[5] = 0; - out_2905490737718698761[6] = 0; - out_2905490737718698761[7] = 0; - out_2905490737718698761[8] = 0; +void H_29(double *state, double *unused, double *out_3704181142562902980) { + out_3704181142562902980[0] = 0; + out_3704181142562902980[1] = 1; + out_3704181142562902980[2] = 0; + out_3704181142562902980[3] = 0; + out_3704181142562902980[4] = 0; + out_3704181142562902980[5] = 0; + out_3704181142562902980[6] = 0; + out_3704181142562902980[7] = 0; + out_3704181142562902980[8] = 0; } -void h_28(double *state, double *unused, double *out_4092634722619698442) { - out_4092634722619698442[0] = state[0]; +void h_28(double *state, double *unused, double *out_1142102096268485827) { + out_1142102096268485827[0] = state[0]; } -void H_28(double *state, double *unused, double *out_2176908279350831813) { - out_2176908279350831813[0] = 1; - out_2176908279350831813[1] = 0; - out_2176908279350831813[2] = 0; - out_2176908279350831813[3] = 0; - out_2176908279350831813[4] = 0; - out_2176908279350831813[5] = 0; - out_2176908279350831813[6] = 0; - out_2176908279350831813[7] = 0; - out_2176908279350831813[8] = 0; +void H_28(double *state, double *unused, double *out_1740550870997576729) { + out_1740550870997576729[0] = 1; + out_1740550870997576729[1] = 0; + out_1740550870997576729[2] = 0; + out_1740550870997576729[3] = 0; + out_1740550870997576729[4] = 0; + out_1740550870997576729[5] = 0; + out_1740550870997576729[6] = 0; + out_1740550870997576729[7] = 0; + out_1740550870997576729[8] = 0; } -void h_31(double *state, double *unused, double *out_1709336648663877701) { - out_1709336648663877701[0] = state[8]; +void h_31(double *state, double *unused, double *out_4288825200706392460) { + out_4288825200706392460[0] = state[8]; } -void H_31(double *state, double *unused, double *out_6953601685408875203) { - out_6953601685408875203[0] = 0; - out_6953601685408875203[1] = 0; - out_6953601685408875203[2] = 0; - out_6953601685408875203[3] = 0; - out_6953601685408875203[4] = 0; - out_6953601685408875203[5] = 0; - out_6953601685408875203[6] = 0; - out_6953601685408875203[7] = 0; - out_6953601685408875203[8] = 1; +void H_31(double *state, double *unused, double *out_4054427577857094666) { + out_4054427577857094666[0] = 0; + out_4054427577857094666[1] = 0; + out_4054427577857094666[2] = 0; + out_4054427577857094666[3] = 0; + out_4054427577857094666[4] = 0; + out_4054427577857094666[5] = 0; + out_4054427577857094666[6] = 0; + out_4054427577857094666[7] = 0; + out_4054427577857094666[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_6314153535178174931) { - err_fun(nom_x, delta_x, out_6314153535178174931); +void car_err_fun(double *nom_x, double *delta_x, double *out_6680589116319572635) { + err_fun(nom_x, delta_x, out_6680589116319572635); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_7049921009392106379) { - inv_err_fun(nom_x, true_x, out_7049921009392106379); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_1421263034200107343) { + inv_err_fun(nom_x, true_x, out_1421263034200107343); } -void car_H_mod_fun(double *state, double *out_9070114660809295187) { - H_mod_fun(state, out_9070114660809295187); +void car_H_mod_fun(double *state, double *out_8968580721052785900) { + H_mod_fun(state, out_8968580721052785900); } -void car_f_fun(double *state, double dt, double *out_7847765386725654972) { - f_fun(state, dt, out_7847765386725654972); +void car_f_fun(double *state, double dt, double *out_3852536059217665062) { + f_fun(state, dt, out_3852536059217665062); } -void car_F_fun(double *state, double dt, double *out_6490326381461081645) { - F_fun(state, dt, out_6490326381461081645); +void car_F_fun(double *state, double dt, double *out_7217781527381652799) { + F_fun(state, dt, out_7217781527381652799); } -void car_h_25(double *state, double *unused, double *out_258705256034994534) { - h_25(state, unused, out_258705256034994534); +void car_h_25(double *state, double *unused, double *out_2838193808077509293) { + h_25(state, unused, out_2838193808077509293); } -void car_H_25(double *state, double *unused, double *out_123073565102942050) { - H_25(state, unused, out_123073565102942050); +void car_H_25(double *state, double *unused, double *out_4085073539734055094) { + H_25(state, unused, out_4085073539734055094); } -void car_h_24(double *state, double *unused, double *out_5989007342487248666) { - h_24(state, unused, out_5989007342487248666); +void car_h_24(double *state, double *unused, double *out_7378709950093750440) { + h_24(state, unused, out_7378709950093750440); } -void car_H_24(double *state, double *unused, double *out_4869582892069451084) { - H_24(state, unused, out_4869582892069451084); +void car_H_24(double *state, double *unused, double *out_2001100279119417225) { + H_24(state, unused, out_2001100279119417225); } -void car_h_30(double *state, double *unused, double *out_179991132081170203) { - h_30(state, unused, out_179991132081170203); +void car_h_30(double *state, double *unused, double *out_6475643336331531643) { + h_30(state, unused, out_6475643336331531643); } -void car_H_30(double *state, double *unused, double *out_6793616776388674705) { - H_30(state, unused, out_6793616776388674705); +void car_H_30(double *state, double *unused, double *out_4214412486877295164) { + H_30(state, unused, out_4214412486877295164); } -void car_h_26(double *state, double *unused, double *out_4821682135579675442) { - h_26(state, unused, out_4821682135579675442); +void car_h_26(double *state, double *unused, double *out_8554827447728949957) { + h_26(state, unused, out_8554827447728949957); } -void car_H_26(double *state, double *unused, double *out_3181452404657858551) { - H_26(state, unused, out_3181452404657858551); +void car_H_26(double *state, double *unused, double *out_3428219475623743190) { + H_26(state, unused, out_3428219475623743190); } -void car_h_27(double *state, double *unused, double *out_5260378523695840179) { - h_27(state, unused, out_5260378523695840179); +void car_h_27(double *state, double *unused, double *out_1670450007001367556) { + h_27(state, unused, out_1670450007001367556); } -void car_H_27(double *state, double *unused, double *out_4618853464588249794) { - H_27(state, unused, out_4618853464588249794); +void car_H_27(double *state, double *unused, double *out_6389175798677720075) { + H_27(state, unused, out_6389175798677720075); } -void car_h_29(double *state, double *unused, double *out_5535572585980346068) { - h_29(state, unused, out_5535572585980346068); +void car_h_29(double *state, double *unused, double *out_6969427469717174328) { + h_29(state, unused, out_6969427469717174328); } -void car_H_29(double *state, double *unused, double *out_2905490737718698761) { - H_29(state, unused, out_2905490737718698761); +void car_H_29(double *state, double *unused, double *out_3704181142562902980) { + H_29(state, unused, out_3704181142562902980); } -void car_h_28(double *state, double *unused, double *out_4092634722619698442) { - h_28(state, unused, out_4092634722619698442); +void car_h_28(double *state, double *unused, double *out_1142102096268485827) { + h_28(state, unused, out_1142102096268485827); } -void car_H_28(double *state, double *unused, double *out_2176908279350831813) { - H_28(state, unused, out_2176908279350831813); +void car_H_28(double *state, double *unused, double *out_1740550870997576729) { + H_28(state, unused, out_1740550870997576729); } -void car_h_31(double *state, double *unused, double *out_1709336648663877701) { - h_31(state, unused, out_1709336648663877701); +void car_h_31(double *state, double *unused, double *out_4288825200706392460) { + h_31(state, unused, out_4288825200706392460); } -void car_H_31(double *state, double *unused, double *out_6953601685408875203) { - H_31(state, unused, out_6953601685408875203); +void car_H_31(double *state, double *unused, double *out_4054427577857094666) { + H_31(state, unused, out_4054427577857094666); } 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 96371e6dc6..ea673bafa4 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_6314153535178174931); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_7049921009392106379); -void car_H_mod_fun(double *state, double *out_9070114660809295187); -void car_f_fun(double *state, double dt, double *out_7847765386725654972); -void car_F_fun(double *state, double dt, double *out_6490326381461081645); -void car_h_25(double *state, double *unused, double *out_258705256034994534); -void car_H_25(double *state, double *unused, double *out_123073565102942050); -void car_h_24(double *state, double *unused, double *out_5989007342487248666); -void car_H_24(double *state, double *unused, double *out_4869582892069451084); -void car_h_30(double *state, double *unused, double *out_179991132081170203); -void car_H_30(double *state, double *unused, double *out_6793616776388674705); -void car_h_26(double *state, double *unused, double *out_4821682135579675442); -void car_H_26(double *state, double *unused, double *out_3181452404657858551); -void car_h_27(double *state, double *unused, double *out_5260378523695840179); -void car_H_27(double *state, double *unused, double *out_4618853464588249794); -void car_h_29(double *state, double *unused, double *out_5535572585980346068); -void car_H_29(double *state, double *unused, double *out_2905490737718698761); -void car_h_28(double *state, double *unused, double *out_4092634722619698442); -void car_H_28(double *state, double *unused, double *out_2176908279350831813); -void car_h_31(double *state, double *unused, double *out_1709336648663877701); -void car_H_31(double *state, double *unused, double *out_6953601685408875203); +void car_err_fun(double *nom_x, double *delta_x, double *out_6680589116319572635); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_1421263034200107343); +void car_H_mod_fun(double *state, double *out_8968580721052785900); +void car_f_fun(double *state, double dt, double *out_3852536059217665062); +void car_F_fun(double *state, double dt, double *out_7217781527381652799); +void car_h_25(double *state, double *unused, double *out_2838193808077509293); +void car_H_25(double *state, double *unused, double *out_4085073539734055094); +void car_h_24(double *state, double *unused, double *out_7378709950093750440); +void car_H_24(double *state, double *unused, double *out_2001100279119417225); +void car_h_30(double *state, double *unused, double *out_6475643336331531643); +void car_H_30(double *state, double *unused, double *out_4214412486877295164); +void car_h_26(double *state, double *unused, double *out_8554827447728949957); +void car_H_26(double *state, double *unused, double *out_3428219475623743190); +void car_h_27(double *state, double *unused, double *out_1670450007001367556); +void car_H_27(double *state, double *unused, double *out_6389175798677720075); +void car_h_29(double *state, double *unused, double *out_6969427469717174328); +void car_H_29(double *state, double *unused, double *out_3704181142562902980); +void car_h_28(double *state, double *unused, double *out_1142102096268485827); +void car_H_28(double *state, double *unused, double *out_1740550870997576729); +void car_h_31(double *state, double *unused, double *out_4288825200706392460); +void car_H_31(double *state, double *unused, double *out_4054427577857094666); 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/pose.cpp b/selfdrive/locationd/models/generated/pose.cpp index 8c823827e7..fe853a757e 100644 --- a/selfdrive/locationd/models/generated/pose.cpp +++ b/selfdrive/locationd/models/generated/pose.cpp @@ -17,961 +17,961 @@ const static double MAHA_THRESH_14 = 7.814727903251177; * * * This file is part of 'ekf' * ******************************************************************************/ -void err_fun(double *nom_x, double *delta_x, double *out_1227579335886564773) { - out_1227579335886564773[0] = delta_x[0] + nom_x[0]; - out_1227579335886564773[1] = delta_x[1] + nom_x[1]; - out_1227579335886564773[2] = delta_x[2] + nom_x[2]; - out_1227579335886564773[3] = delta_x[3] + nom_x[3]; - out_1227579335886564773[4] = delta_x[4] + nom_x[4]; - out_1227579335886564773[5] = delta_x[5] + nom_x[5]; - out_1227579335886564773[6] = delta_x[6] + nom_x[6]; - out_1227579335886564773[7] = delta_x[7] + nom_x[7]; - out_1227579335886564773[8] = delta_x[8] + nom_x[8]; - out_1227579335886564773[9] = delta_x[9] + nom_x[9]; - out_1227579335886564773[10] = delta_x[10] + nom_x[10]; - out_1227579335886564773[11] = delta_x[11] + nom_x[11]; - out_1227579335886564773[12] = delta_x[12] + nom_x[12]; - out_1227579335886564773[13] = delta_x[13] + nom_x[13]; - out_1227579335886564773[14] = delta_x[14] + nom_x[14]; - out_1227579335886564773[15] = delta_x[15] + nom_x[15]; - out_1227579335886564773[16] = delta_x[16] + nom_x[16]; - out_1227579335886564773[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_7165967535860238129) { + out_7165967535860238129[0] = delta_x[0] + nom_x[0]; + out_7165967535860238129[1] = delta_x[1] + nom_x[1]; + out_7165967535860238129[2] = delta_x[2] + nom_x[2]; + out_7165967535860238129[3] = delta_x[3] + nom_x[3]; + out_7165967535860238129[4] = delta_x[4] + nom_x[4]; + out_7165967535860238129[5] = delta_x[5] + nom_x[5]; + out_7165967535860238129[6] = delta_x[6] + nom_x[6]; + out_7165967535860238129[7] = delta_x[7] + nom_x[7]; + out_7165967535860238129[8] = delta_x[8] + nom_x[8]; + out_7165967535860238129[9] = delta_x[9] + nom_x[9]; + out_7165967535860238129[10] = delta_x[10] + nom_x[10]; + out_7165967535860238129[11] = delta_x[11] + nom_x[11]; + out_7165967535860238129[12] = delta_x[12] + nom_x[12]; + out_7165967535860238129[13] = delta_x[13] + nom_x[13]; + out_7165967535860238129[14] = delta_x[14] + nom_x[14]; + out_7165967535860238129[15] = delta_x[15] + nom_x[15]; + out_7165967535860238129[16] = delta_x[16] + nom_x[16]; + out_7165967535860238129[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_7708296302708828001) { - out_7708296302708828001[0] = -nom_x[0] + true_x[0]; - out_7708296302708828001[1] = -nom_x[1] + true_x[1]; - out_7708296302708828001[2] = -nom_x[2] + true_x[2]; - out_7708296302708828001[3] = -nom_x[3] + true_x[3]; - out_7708296302708828001[4] = -nom_x[4] + true_x[4]; - out_7708296302708828001[5] = -nom_x[5] + true_x[5]; - out_7708296302708828001[6] = -nom_x[6] + true_x[6]; - out_7708296302708828001[7] = -nom_x[7] + true_x[7]; - out_7708296302708828001[8] = -nom_x[8] + true_x[8]; - out_7708296302708828001[9] = -nom_x[9] + true_x[9]; - out_7708296302708828001[10] = -nom_x[10] + true_x[10]; - out_7708296302708828001[11] = -nom_x[11] + true_x[11]; - out_7708296302708828001[12] = -nom_x[12] + true_x[12]; - out_7708296302708828001[13] = -nom_x[13] + true_x[13]; - out_7708296302708828001[14] = -nom_x[14] + true_x[14]; - out_7708296302708828001[15] = -nom_x[15] + true_x[15]; - out_7708296302708828001[16] = -nom_x[16] + true_x[16]; - out_7708296302708828001[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_9032729807193459598) { + out_9032729807193459598[0] = -nom_x[0] + true_x[0]; + out_9032729807193459598[1] = -nom_x[1] + true_x[1]; + out_9032729807193459598[2] = -nom_x[2] + true_x[2]; + out_9032729807193459598[3] = -nom_x[3] + true_x[3]; + out_9032729807193459598[4] = -nom_x[4] + true_x[4]; + out_9032729807193459598[5] = -nom_x[5] + true_x[5]; + out_9032729807193459598[6] = -nom_x[6] + true_x[6]; + out_9032729807193459598[7] = -nom_x[7] + true_x[7]; + out_9032729807193459598[8] = -nom_x[8] + true_x[8]; + out_9032729807193459598[9] = -nom_x[9] + true_x[9]; + out_9032729807193459598[10] = -nom_x[10] + true_x[10]; + out_9032729807193459598[11] = -nom_x[11] + true_x[11]; + out_9032729807193459598[12] = -nom_x[12] + true_x[12]; + out_9032729807193459598[13] = -nom_x[13] + true_x[13]; + out_9032729807193459598[14] = -nom_x[14] + true_x[14]; + out_9032729807193459598[15] = -nom_x[15] + true_x[15]; + out_9032729807193459598[16] = -nom_x[16] + true_x[16]; + out_9032729807193459598[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_145151651698479708) { - out_145151651698479708[0] = 1.0; - out_145151651698479708[1] = 0.0; - out_145151651698479708[2] = 0.0; - out_145151651698479708[3] = 0.0; - out_145151651698479708[4] = 0.0; - out_145151651698479708[5] = 0.0; - out_145151651698479708[6] = 0.0; - out_145151651698479708[7] = 0.0; - out_145151651698479708[8] = 0.0; - out_145151651698479708[9] = 0.0; - out_145151651698479708[10] = 0.0; - out_145151651698479708[11] = 0.0; - out_145151651698479708[12] = 0.0; - out_145151651698479708[13] = 0.0; - out_145151651698479708[14] = 0.0; - out_145151651698479708[15] = 0.0; - out_145151651698479708[16] = 0.0; - out_145151651698479708[17] = 0.0; - out_145151651698479708[18] = 0.0; - out_145151651698479708[19] = 1.0; - out_145151651698479708[20] = 0.0; - out_145151651698479708[21] = 0.0; - out_145151651698479708[22] = 0.0; - out_145151651698479708[23] = 0.0; - out_145151651698479708[24] = 0.0; - out_145151651698479708[25] = 0.0; - out_145151651698479708[26] = 0.0; - out_145151651698479708[27] = 0.0; - out_145151651698479708[28] = 0.0; - out_145151651698479708[29] = 0.0; - out_145151651698479708[30] = 0.0; - out_145151651698479708[31] = 0.0; - out_145151651698479708[32] = 0.0; - out_145151651698479708[33] = 0.0; - out_145151651698479708[34] = 0.0; - out_145151651698479708[35] = 0.0; - out_145151651698479708[36] = 0.0; - out_145151651698479708[37] = 0.0; - out_145151651698479708[38] = 1.0; - out_145151651698479708[39] = 0.0; - out_145151651698479708[40] = 0.0; - out_145151651698479708[41] = 0.0; - out_145151651698479708[42] = 0.0; - out_145151651698479708[43] = 0.0; - out_145151651698479708[44] = 0.0; - out_145151651698479708[45] = 0.0; - out_145151651698479708[46] = 0.0; - out_145151651698479708[47] = 0.0; - out_145151651698479708[48] = 0.0; - out_145151651698479708[49] = 0.0; - out_145151651698479708[50] = 0.0; - out_145151651698479708[51] = 0.0; - out_145151651698479708[52] = 0.0; - out_145151651698479708[53] = 0.0; - out_145151651698479708[54] = 0.0; - out_145151651698479708[55] = 0.0; - out_145151651698479708[56] = 0.0; - out_145151651698479708[57] = 1.0; - out_145151651698479708[58] = 0.0; - out_145151651698479708[59] = 0.0; - out_145151651698479708[60] = 0.0; - out_145151651698479708[61] = 0.0; - out_145151651698479708[62] = 0.0; - out_145151651698479708[63] = 0.0; - out_145151651698479708[64] = 0.0; - out_145151651698479708[65] = 0.0; - out_145151651698479708[66] = 0.0; - out_145151651698479708[67] = 0.0; - out_145151651698479708[68] = 0.0; - out_145151651698479708[69] = 0.0; - out_145151651698479708[70] = 0.0; - out_145151651698479708[71] = 0.0; - out_145151651698479708[72] = 0.0; - out_145151651698479708[73] = 0.0; - out_145151651698479708[74] = 0.0; - out_145151651698479708[75] = 0.0; - out_145151651698479708[76] = 1.0; - out_145151651698479708[77] = 0.0; - out_145151651698479708[78] = 0.0; - out_145151651698479708[79] = 0.0; - out_145151651698479708[80] = 0.0; - out_145151651698479708[81] = 0.0; - out_145151651698479708[82] = 0.0; - out_145151651698479708[83] = 0.0; - out_145151651698479708[84] = 0.0; - out_145151651698479708[85] = 0.0; - out_145151651698479708[86] = 0.0; - out_145151651698479708[87] = 0.0; - out_145151651698479708[88] = 0.0; - out_145151651698479708[89] = 0.0; - out_145151651698479708[90] = 0.0; - out_145151651698479708[91] = 0.0; - out_145151651698479708[92] = 0.0; - out_145151651698479708[93] = 0.0; - out_145151651698479708[94] = 0.0; - out_145151651698479708[95] = 1.0; - out_145151651698479708[96] = 0.0; - out_145151651698479708[97] = 0.0; - out_145151651698479708[98] = 0.0; - out_145151651698479708[99] = 0.0; - out_145151651698479708[100] = 0.0; - out_145151651698479708[101] = 0.0; - out_145151651698479708[102] = 0.0; - out_145151651698479708[103] = 0.0; - out_145151651698479708[104] = 0.0; - out_145151651698479708[105] = 0.0; - out_145151651698479708[106] = 0.0; - out_145151651698479708[107] = 0.0; - out_145151651698479708[108] = 0.0; - out_145151651698479708[109] = 0.0; - out_145151651698479708[110] = 0.0; - out_145151651698479708[111] = 0.0; - out_145151651698479708[112] = 0.0; - out_145151651698479708[113] = 0.0; - out_145151651698479708[114] = 1.0; - out_145151651698479708[115] = 0.0; - out_145151651698479708[116] = 0.0; - out_145151651698479708[117] = 0.0; - out_145151651698479708[118] = 0.0; - out_145151651698479708[119] = 0.0; - out_145151651698479708[120] = 0.0; - out_145151651698479708[121] = 0.0; - out_145151651698479708[122] = 0.0; - out_145151651698479708[123] = 0.0; - out_145151651698479708[124] = 0.0; - out_145151651698479708[125] = 0.0; - out_145151651698479708[126] = 0.0; - out_145151651698479708[127] = 0.0; - out_145151651698479708[128] = 0.0; - out_145151651698479708[129] = 0.0; - out_145151651698479708[130] = 0.0; - out_145151651698479708[131] = 0.0; - out_145151651698479708[132] = 0.0; - out_145151651698479708[133] = 1.0; - out_145151651698479708[134] = 0.0; - out_145151651698479708[135] = 0.0; - out_145151651698479708[136] = 0.0; - out_145151651698479708[137] = 0.0; - out_145151651698479708[138] = 0.0; - out_145151651698479708[139] = 0.0; - out_145151651698479708[140] = 0.0; - out_145151651698479708[141] = 0.0; - out_145151651698479708[142] = 0.0; - out_145151651698479708[143] = 0.0; - out_145151651698479708[144] = 0.0; - out_145151651698479708[145] = 0.0; - out_145151651698479708[146] = 0.0; - out_145151651698479708[147] = 0.0; - out_145151651698479708[148] = 0.0; - out_145151651698479708[149] = 0.0; - out_145151651698479708[150] = 0.0; - out_145151651698479708[151] = 0.0; - out_145151651698479708[152] = 1.0; - out_145151651698479708[153] = 0.0; - out_145151651698479708[154] = 0.0; - out_145151651698479708[155] = 0.0; - out_145151651698479708[156] = 0.0; - out_145151651698479708[157] = 0.0; - out_145151651698479708[158] = 0.0; - out_145151651698479708[159] = 0.0; - out_145151651698479708[160] = 0.0; - out_145151651698479708[161] = 0.0; - out_145151651698479708[162] = 0.0; - out_145151651698479708[163] = 0.0; - out_145151651698479708[164] = 0.0; - out_145151651698479708[165] = 0.0; - out_145151651698479708[166] = 0.0; - out_145151651698479708[167] = 0.0; - out_145151651698479708[168] = 0.0; - out_145151651698479708[169] = 0.0; - out_145151651698479708[170] = 0.0; - out_145151651698479708[171] = 1.0; - out_145151651698479708[172] = 0.0; - out_145151651698479708[173] = 0.0; - out_145151651698479708[174] = 0.0; - out_145151651698479708[175] = 0.0; - out_145151651698479708[176] = 0.0; - out_145151651698479708[177] = 0.0; - out_145151651698479708[178] = 0.0; - out_145151651698479708[179] = 0.0; - out_145151651698479708[180] = 0.0; - out_145151651698479708[181] = 0.0; - out_145151651698479708[182] = 0.0; - out_145151651698479708[183] = 0.0; - out_145151651698479708[184] = 0.0; - out_145151651698479708[185] = 0.0; - out_145151651698479708[186] = 0.0; - out_145151651698479708[187] = 0.0; - out_145151651698479708[188] = 0.0; - out_145151651698479708[189] = 0.0; - out_145151651698479708[190] = 1.0; - out_145151651698479708[191] = 0.0; - out_145151651698479708[192] = 0.0; - out_145151651698479708[193] = 0.0; - out_145151651698479708[194] = 0.0; - out_145151651698479708[195] = 0.0; - out_145151651698479708[196] = 0.0; - out_145151651698479708[197] = 0.0; - out_145151651698479708[198] = 0.0; - out_145151651698479708[199] = 0.0; - out_145151651698479708[200] = 0.0; - out_145151651698479708[201] = 0.0; - out_145151651698479708[202] = 0.0; - out_145151651698479708[203] = 0.0; - out_145151651698479708[204] = 0.0; - out_145151651698479708[205] = 0.0; - out_145151651698479708[206] = 0.0; - out_145151651698479708[207] = 0.0; - out_145151651698479708[208] = 0.0; - out_145151651698479708[209] = 1.0; - out_145151651698479708[210] = 0.0; - out_145151651698479708[211] = 0.0; - out_145151651698479708[212] = 0.0; - out_145151651698479708[213] = 0.0; - out_145151651698479708[214] = 0.0; - out_145151651698479708[215] = 0.0; - out_145151651698479708[216] = 0.0; - out_145151651698479708[217] = 0.0; - out_145151651698479708[218] = 0.0; - out_145151651698479708[219] = 0.0; - out_145151651698479708[220] = 0.0; - out_145151651698479708[221] = 0.0; - out_145151651698479708[222] = 0.0; - out_145151651698479708[223] = 0.0; - out_145151651698479708[224] = 0.0; - out_145151651698479708[225] = 0.0; - out_145151651698479708[226] = 0.0; - out_145151651698479708[227] = 0.0; - out_145151651698479708[228] = 1.0; - out_145151651698479708[229] = 0.0; - out_145151651698479708[230] = 0.0; - out_145151651698479708[231] = 0.0; - out_145151651698479708[232] = 0.0; - out_145151651698479708[233] = 0.0; - out_145151651698479708[234] = 0.0; - out_145151651698479708[235] = 0.0; - out_145151651698479708[236] = 0.0; - out_145151651698479708[237] = 0.0; - out_145151651698479708[238] = 0.0; - out_145151651698479708[239] = 0.0; - out_145151651698479708[240] = 0.0; - out_145151651698479708[241] = 0.0; - out_145151651698479708[242] = 0.0; - out_145151651698479708[243] = 0.0; - out_145151651698479708[244] = 0.0; - out_145151651698479708[245] = 0.0; - out_145151651698479708[246] = 0.0; - out_145151651698479708[247] = 1.0; - out_145151651698479708[248] = 0.0; - out_145151651698479708[249] = 0.0; - out_145151651698479708[250] = 0.0; - out_145151651698479708[251] = 0.0; - out_145151651698479708[252] = 0.0; - out_145151651698479708[253] = 0.0; - out_145151651698479708[254] = 0.0; - out_145151651698479708[255] = 0.0; - out_145151651698479708[256] = 0.0; - out_145151651698479708[257] = 0.0; - out_145151651698479708[258] = 0.0; - out_145151651698479708[259] = 0.0; - out_145151651698479708[260] = 0.0; - out_145151651698479708[261] = 0.0; - out_145151651698479708[262] = 0.0; - out_145151651698479708[263] = 0.0; - out_145151651698479708[264] = 0.0; - out_145151651698479708[265] = 0.0; - out_145151651698479708[266] = 1.0; - out_145151651698479708[267] = 0.0; - out_145151651698479708[268] = 0.0; - out_145151651698479708[269] = 0.0; - out_145151651698479708[270] = 0.0; - out_145151651698479708[271] = 0.0; - out_145151651698479708[272] = 0.0; - out_145151651698479708[273] = 0.0; - out_145151651698479708[274] = 0.0; - out_145151651698479708[275] = 0.0; - out_145151651698479708[276] = 0.0; - out_145151651698479708[277] = 0.0; - out_145151651698479708[278] = 0.0; - out_145151651698479708[279] = 0.0; - out_145151651698479708[280] = 0.0; - out_145151651698479708[281] = 0.0; - out_145151651698479708[282] = 0.0; - out_145151651698479708[283] = 0.0; - out_145151651698479708[284] = 0.0; - out_145151651698479708[285] = 1.0; - out_145151651698479708[286] = 0.0; - out_145151651698479708[287] = 0.0; - out_145151651698479708[288] = 0.0; - out_145151651698479708[289] = 0.0; - out_145151651698479708[290] = 0.0; - out_145151651698479708[291] = 0.0; - out_145151651698479708[292] = 0.0; - out_145151651698479708[293] = 0.0; - out_145151651698479708[294] = 0.0; - out_145151651698479708[295] = 0.0; - out_145151651698479708[296] = 0.0; - out_145151651698479708[297] = 0.0; - out_145151651698479708[298] = 0.0; - out_145151651698479708[299] = 0.0; - out_145151651698479708[300] = 0.0; - out_145151651698479708[301] = 0.0; - out_145151651698479708[302] = 0.0; - out_145151651698479708[303] = 0.0; - out_145151651698479708[304] = 1.0; - out_145151651698479708[305] = 0.0; - out_145151651698479708[306] = 0.0; - out_145151651698479708[307] = 0.0; - out_145151651698479708[308] = 0.0; - out_145151651698479708[309] = 0.0; - out_145151651698479708[310] = 0.0; - out_145151651698479708[311] = 0.0; - out_145151651698479708[312] = 0.0; - out_145151651698479708[313] = 0.0; - out_145151651698479708[314] = 0.0; - out_145151651698479708[315] = 0.0; - out_145151651698479708[316] = 0.0; - out_145151651698479708[317] = 0.0; - out_145151651698479708[318] = 0.0; - out_145151651698479708[319] = 0.0; - out_145151651698479708[320] = 0.0; - out_145151651698479708[321] = 0.0; - out_145151651698479708[322] = 0.0; - out_145151651698479708[323] = 1.0; +void H_mod_fun(double *state, double *out_5054599202167139908) { + out_5054599202167139908[0] = 1.0; + out_5054599202167139908[1] = 0.0; + out_5054599202167139908[2] = 0.0; + out_5054599202167139908[3] = 0.0; + out_5054599202167139908[4] = 0.0; + out_5054599202167139908[5] = 0.0; + out_5054599202167139908[6] = 0.0; + out_5054599202167139908[7] = 0.0; + out_5054599202167139908[8] = 0.0; + out_5054599202167139908[9] = 0.0; + out_5054599202167139908[10] = 0.0; + out_5054599202167139908[11] = 0.0; + out_5054599202167139908[12] = 0.0; + out_5054599202167139908[13] = 0.0; + out_5054599202167139908[14] = 0.0; + out_5054599202167139908[15] = 0.0; + out_5054599202167139908[16] = 0.0; + out_5054599202167139908[17] = 0.0; + out_5054599202167139908[18] = 0.0; + out_5054599202167139908[19] = 1.0; + out_5054599202167139908[20] = 0.0; + out_5054599202167139908[21] = 0.0; + out_5054599202167139908[22] = 0.0; + out_5054599202167139908[23] = 0.0; + out_5054599202167139908[24] = 0.0; + out_5054599202167139908[25] = 0.0; + out_5054599202167139908[26] = 0.0; + out_5054599202167139908[27] = 0.0; + out_5054599202167139908[28] = 0.0; + out_5054599202167139908[29] = 0.0; + out_5054599202167139908[30] = 0.0; + out_5054599202167139908[31] = 0.0; + out_5054599202167139908[32] = 0.0; + out_5054599202167139908[33] = 0.0; + out_5054599202167139908[34] = 0.0; + out_5054599202167139908[35] = 0.0; + out_5054599202167139908[36] = 0.0; + out_5054599202167139908[37] = 0.0; + out_5054599202167139908[38] = 1.0; + out_5054599202167139908[39] = 0.0; + out_5054599202167139908[40] = 0.0; + out_5054599202167139908[41] = 0.0; + out_5054599202167139908[42] = 0.0; + out_5054599202167139908[43] = 0.0; + out_5054599202167139908[44] = 0.0; + out_5054599202167139908[45] = 0.0; + out_5054599202167139908[46] = 0.0; + out_5054599202167139908[47] = 0.0; + out_5054599202167139908[48] = 0.0; + out_5054599202167139908[49] = 0.0; + out_5054599202167139908[50] = 0.0; + out_5054599202167139908[51] = 0.0; + out_5054599202167139908[52] = 0.0; + out_5054599202167139908[53] = 0.0; + out_5054599202167139908[54] = 0.0; + out_5054599202167139908[55] = 0.0; + out_5054599202167139908[56] = 0.0; + out_5054599202167139908[57] = 1.0; + out_5054599202167139908[58] = 0.0; + out_5054599202167139908[59] = 0.0; + out_5054599202167139908[60] = 0.0; + out_5054599202167139908[61] = 0.0; + out_5054599202167139908[62] = 0.0; + out_5054599202167139908[63] = 0.0; + out_5054599202167139908[64] = 0.0; + out_5054599202167139908[65] = 0.0; + out_5054599202167139908[66] = 0.0; + out_5054599202167139908[67] = 0.0; + out_5054599202167139908[68] = 0.0; + out_5054599202167139908[69] = 0.0; + out_5054599202167139908[70] = 0.0; + out_5054599202167139908[71] = 0.0; + out_5054599202167139908[72] = 0.0; + out_5054599202167139908[73] = 0.0; + out_5054599202167139908[74] = 0.0; + out_5054599202167139908[75] = 0.0; + out_5054599202167139908[76] = 1.0; + out_5054599202167139908[77] = 0.0; + out_5054599202167139908[78] = 0.0; + out_5054599202167139908[79] = 0.0; + out_5054599202167139908[80] = 0.0; + out_5054599202167139908[81] = 0.0; + out_5054599202167139908[82] = 0.0; + out_5054599202167139908[83] = 0.0; + out_5054599202167139908[84] = 0.0; + out_5054599202167139908[85] = 0.0; + out_5054599202167139908[86] = 0.0; + out_5054599202167139908[87] = 0.0; + out_5054599202167139908[88] = 0.0; + out_5054599202167139908[89] = 0.0; + out_5054599202167139908[90] = 0.0; + out_5054599202167139908[91] = 0.0; + out_5054599202167139908[92] = 0.0; + out_5054599202167139908[93] = 0.0; + out_5054599202167139908[94] = 0.0; + out_5054599202167139908[95] = 1.0; + out_5054599202167139908[96] = 0.0; + out_5054599202167139908[97] = 0.0; + out_5054599202167139908[98] = 0.0; + out_5054599202167139908[99] = 0.0; + out_5054599202167139908[100] = 0.0; + out_5054599202167139908[101] = 0.0; + out_5054599202167139908[102] = 0.0; + out_5054599202167139908[103] = 0.0; + out_5054599202167139908[104] = 0.0; + out_5054599202167139908[105] = 0.0; + out_5054599202167139908[106] = 0.0; + out_5054599202167139908[107] = 0.0; + out_5054599202167139908[108] = 0.0; + out_5054599202167139908[109] = 0.0; + out_5054599202167139908[110] = 0.0; + out_5054599202167139908[111] = 0.0; + out_5054599202167139908[112] = 0.0; + out_5054599202167139908[113] = 0.0; + out_5054599202167139908[114] = 1.0; + out_5054599202167139908[115] = 0.0; + out_5054599202167139908[116] = 0.0; + out_5054599202167139908[117] = 0.0; + out_5054599202167139908[118] = 0.0; + out_5054599202167139908[119] = 0.0; + out_5054599202167139908[120] = 0.0; + out_5054599202167139908[121] = 0.0; + out_5054599202167139908[122] = 0.0; + out_5054599202167139908[123] = 0.0; + out_5054599202167139908[124] = 0.0; + out_5054599202167139908[125] = 0.0; + out_5054599202167139908[126] = 0.0; + out_5054599202167139908[127] = 0.0; + out_5054599202167139908[128] = 0.0; + out_5054599202167139908[129] = 0.0; + out_5054599202167139908[130] = 0.0; + out_5054599202167139908[131] = 0.0; + out_5054599202167139908[132] = 0.0; + out_5054599202167139908[133] = 1.0; + out_5054599202167139908[134] = 0.0; + out_5054599202167139908[135] = 0.0; + out_5054599202167139908[136] = 0.0; + out_5054599202167139908[137] = 0.0; + out_5054599202167139908[138] = 0.0; + out_5054599202167139908[139] = 0.0; + out_5054599202167139908[140] = 0.0; + out_5054599202167139908[141] = 0.0; + out_5054599202167139908[142] = 0.0; + out_5054599202167139908[143] = 0.0; + out_5054599202167139908[144] = 0.0; + out_5054599202167139908[145] = 0.0; + out_5054599202167139908[146] = 0.0; + out_5054599202167139908[147] = 0.0; + out_5054599202167139908[148] = 0.0; + out_5054599202167139908[149] = 0.0; + out_5054599202167139908[150] = 0.0; + out_5054599202167139908[151] = 0.0; + out_5054599202167139908[152] = 1.0; + out_5054599202167139908[153] = 0.0; + out_5054599202167139908[154] = 0.0; + out_5054599202167139908[155] = 0.0; + out_5054599202167139908[156] = 0.0; + out_5054599202167139908[157] = 0.0; + out_5054599202167139908[158] = 0.0; + out_5054599202167139908[159] = 0.0; + out_5054599202167139908[160] = 0.0; + out_5054599202167139908[161] = 0.0; + out_5054599202167139908[162] = 0.0; + out_5054599202167139908[163] = 0.0; + out_5054599202167139908[164] = 0.0; + out_5054599202167139908[165] = 0.0; + out_5054599202167139908[166] = 0.0; + out_5054599202167139908[167] = 0.0; + out_5054599202167139908[168] = 0.0; + out_5054599202167139908[169] = 0.0; + out_5054599202167139908[170] = 0.0; + out_5054599202167139908[171] = 1.0; + out_5054599202167139908[172] = 0.0; + out_5054599202167139908[173] = 0.0; + out_5054599202167139908[174] = 0.0; + out_5054599202167139908[175] = 0.0; + out_5054599202167139908[176] = 0.0; + out_5054599202167139908[177] = 0.0; + out_5054599202167139908[178] = 0.0; + out_5054599202167139908[179] = 0.0; + out_5054599202167139908[180] = 0.0; + out_5054599202167139908[181] = 0.0; + out_5054599202167139908[182] = 0.0; + out_5054599202167139908[183] = 0.0; + out_5054599202167139908[184] = 0.0; + out_5054599202167139908[185] = 0.0; + out_5054599202167139908[186] = 0.0; + out_5054599202167139908[187] = 0.0; + out_5054599202167139908[188] = 0.0; + out_5054599202167139908[189] = 0.0; + out_5054599202167139908[190] = 1.0; + out_5054599202167139908[191] = 0.0; + out_5054599202167139908[192] = 0.0; + out_5054599202167139908[193] = 0.0; + out_5054599202167139908[194] = 0.0; + out_5054599202167139908[195] = 0.0; + out_5054599202167139908[196] = 0.0; + out_5054599202167139908[197] = 0.0; + out_5054599202167139908[198] = 0.0; + out_5054599202167139908[199] = 0.0; + out_5054599202167139908[200] = 0.0; + out_5054599202167139908[201] = 0.0; + out_5054599202167139908[202] = 0.0; + out_5054599202167139908[203] = 0.0; + out_5054599202167139908[204] = 0.0; + out_5054599202167139908[205] = 0.0; + out_5054599202167139908[206] = 0.0; + out_5054599202167139908[207] = 0.0; + out_5054599202167139908[208] = 0.0; + out_5054599202167139908[209] = 1.0; + out_5054599202167139908[210] = 0.0; + out_5054599202167139908[211] = 0.0; + out_5054599202167139908[212] = 0.0; + out_5054599202167139908[213] = 0.0; + out_5054599202167139908[214] = 0.0; + out_5054599202167139908[215] = 0.0; + out_5054599202167139908[216] = 0.0; + out_5054599202167139908[217] = 0.0; + out_5054599202167139908[218] = 0.0; + out_5054599202167139908[219] = 0.0; + out_5054599202167139908[220] = 0.0; + out_5054599202167139908[221] = 0.0; + out_5054599202167139908[222] = 0.0; + out_5054599202167139908[223] = 0.0; + out_5054599202167139908[224] = 0.0; + out_5054599202167139908[225] = 0.0; + out_5054599202167139908[226] = 0.0; + out_5054599202167139908[227] = 0.0; + out_5054599202167139908[228] = 1.0; + out_5054599202167139908[229] = 0.0; + out_5054599202167139908[230] = 0.0; + out_5054599202167139908[231] = 0.0; + out_5054599202167139908[232] = 0.0; + out_5054599202167139908[233] = 0.0; + out_5054599202167139908[234] = 0.0; + out_5054599202167139908[235] = 0.0; + out_5054599202167139908[236] = 0.0; + out_5054599202167139908[237] = 0.0; + out_5054599202167139908[238] = 0.0; + out_5054599202167139908[239] = 0.0; + out_5054599202167139908[240] = 0.0; + out_5054599202167139908[241] = 0.0; + out_5054599202167139908[242] = 0.0; + out_5054599202167139908[243] = 0.0; + out_5054599202167139908[244] = 0.0; + out_5054599202167139908[245] = 0.0; + out_5054599202167139908[246] = 0.0; + out_5054599202167139908[247] = 1.0; + out_5054599202167139908[248] = 0.0; + out_5054599202167139908[249] = 0.0; + out_5054599202167139908[250] = 0.0; + out_5054599202167139908[251] = 0.0; + out_5054599202167139908[252] = 0.0; + out_5054599202167139908[253] = 0.0; + out_5054599202167139908[254] = 0.0; + out_5054599202167139908[255] = 0.0; + out_5054599202167139908[256] = 0.0; + out_5054599202167139908[257] = 0.0; + out_5054599202167139908[258] = 0.0; + out_5054599202167139908[259] = 0.0; + out_5054599202167139908[260] = 0.0; + out_5054599202167139908[261] = 0.0; + out_5054599202167139908[262] = 0.0; + out_5054599202167139908[263] = 0.0; + out_5054599202167139908[264] = 0.0; + out_5054599202167139908[265] = 0.0; + out_5054599202167139908[266] = 1.0; + out_5054599202167139908[267] = 0.0; + out_5054599202167139908[268] = 0.0; + out_5054599202167139908[269] = 0.0; + out_5054599202167139908[270] = 0.0; + out_5054599202167139908[271] = 0.0; + out_5054599202167139908[272] = 0.0; + out_5054599202167139908[273] = 0.0; + out_5054599202167139908[274] = 0.0; + out_5054599202167139908[275] = 0.0; + out_5054599202167139908[276] = 0.0; + out_5054599202167139908[277] = 0.0; + out_5054599202167139908[278] = 0.0; + out_5054599202167139908[279] = 0.0; + out_5054599202167139908[280] = 0.0; + out_5054599202167139908[281] = 0.0; + out_5054599202167139908[282] = 0.0; + out_5054599202167139908[283] = 0.0; + out_5054599202167139908[284] = 0.0; + out_5054599202167139908[285] = 1.0; + out_5054599202167139908[286] = 0.0; + out_5054599202167139908[287] = 0.0; + out_5054599202167139908[288] = 0.0; + out_5054599202167139908[289] = 0.0; + out_5054599202167139908[290] = 0.0; + out_5054599202167139908[291] = 0.0; + out_5054599202167139908[292] = 0.0; + out_5054599202167139908[293] = 0.0; + out_5054599202167139908[294] = 0.0; + out_5054599202167139908[295] = 0.0; + out_5054599202167139908[296] = 0.0; + out_5054599202167139908[297] = 0.0; + out_5054599202167139908[298] = 0.0; + out_5054599202167139908[299] = 0.0; + out_5054599202167139908[300] = 0.0; + out_5054599202167139908[301] = 0.0; + out_5054599202167139908[302] = 0.0; + out_5054599202167139908[303] = 0.0; + out_5054599202167139908[304] = 1.0; + out_5054599202167139908[305] = 0.0; + out_5054599202167139908[306] = 0.0; + out_5054599202167139908[307] = 0.0; + out_5054599202167139908[308] = 0.0; + out_5054599202167139908[309] = 0.0; + out_5054599202167139908[310] = 0.0; + out_5054599202167139908[311] = 0.0; + out_5054599202167139908[312] = 0.0; + out_5054599202167139908[313] = 0.0; + out_5054599202167139908[314] = 0.0; + out_5054599202167139908[315] = 0.0; + out_5054599202167139908[316] = 0.0; + out_5054599202167139908[317] = 0.0; + out_5054599202167139908[318] = 0.0; + out_5054599202167139908[319] = 0.0; + out_5054599202167139908[320] = 0.0; + out_5054599202167139908[321] = 0.0; + out_5054599202167139908[322] = 0.0; + out_5054599202167139908[323] = 1.0; } -void f_fun(double *state, double dt, double *out_6753877240357292479) { - out_6753877240357292479[0] = atan2((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), -(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1])); - out_6753877240357292479[1] = asin(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8])); - out_6753877240357292479[2] = atan2(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), -(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2])); - out_6753877240357292479[3] = dt*state[12] + state[3]; - out_6753877240357292479[4] = dt*state[13] + state[4]; - out_6753877240357292479[5] = dt*state[14] + state[5]; - out_6753877240357292479[6] = state[6]; - out_6753877240357292479[7] = state[7]; - out_6753877240357292479[8] = state[8]; - out_6753877240357292479[9] = state[9]; - out_6753877240357292479[10] = state[10]; - out_6753877240357292479[11] = state[11]; - out_6753877240357292479[12] = state[12]; - out_6753877240357292479[13] = state[13]; - out_6753877240357292479[14] = state[14]; - out_6753877240357292479[15] = state[15]; - out_6753877240357292479[16] = state[16]; - out_6753877240357292479[17] = state[17]; +void f_fun(double *state, double dt, double *out_5000035888964044284) { + out_5000035888964044284[0] = atan2((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), -(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1])); + out_5000035888964044284[1] = asin(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8])); + out_5000035888964044284[2] = atan2(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), -(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2])); + out_5000035888964044284[3] = dt*state[12] + state[3]; + out_5000035888964044284[4] = dt*state[13] + state[4]; + out_5000035888964044284[5] = dt*state[14] + state[5]; + out_5000035888964044284[6] = state[6]; + out_5000035888964044284[7] = state[7]; + out_5000035888964044284[8] = state[8]; + out_5000035888964044284[9] = state[9]; + out_5000035888964044284[10] = state[10]; + out_5000035888964044284[11] = state[11]; + out_5000035888964044284[12] = state[12]; + out_5000035888964044284[13] = state[13]; + out_5000035888964044284[14] = state[14]; + out_5000035888964044284[15] = state[15]; + out_5000035888964044284[16] = state[16]; + out_5000035888964044284[17] = state[17]; } -void F_fun(double *state, double dt, double *out_48451545294533288) { - out_48451545294533288[0] = ((-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*cos(state[0])*cos(state[1]) - sin(state[0])*cos(dt*state[6])*cos(dt*state[7])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + ((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*cos(state[0])*cos(state[1]) - sin(dt*state[6])*sin(state[0])*cos(dt*state[7])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_48451545294533288[1] = ((-sin(dt*state[6])*sin(dt*state[8]) - sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*cos(state[1]) - (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*sin(state[1]) - sin(state[1])*cos(dt*state[6])*cos(dt*state[7])*cos(state[0]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*sin(state[1]) + (-sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) + sin(dt*state[8])*cos(dt*state[6]))*cos(state[1]) - sin(dt*state[6])*sin(state[1])*cos(dt*state[7])*cos(state[0]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_48451545294533288[2] = 0; - out_48451545294533288[3] = 0; - out_48451545294533288[4] = 0; - out_48451545294533288[5] = 0; - out_48451545294533288[6] = (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(dt*cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*sin(dt*state[8]) - dt*sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-dt*sin(dt*state[6])*cos(dt*state[8]) + dt*sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) - dt*cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (dt*sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_48451545294533288[7] = (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[6])*sin(dt*state[7])*cos(state[0])*cos(state[1]) + dt*sin(dt*state[6])*sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) - dt*sin(dt*state[6])*sin(state[1])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[7])*cos(dt*state[6])*cos(state[0])*cos(state[1]) + dt*sin(dt*state[8])*sin(state[0])*cos(dt*state[6])*cos(dt*state[7])*cos(state[1]) - dt*sin(state[1])*cos(dt*state[6])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_48451545294533288[8] = ((dt*sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + dt*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (dt*sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + ((dt*sin(dt*state[6])*sin(dt*state[8]) + dt*sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*cos(dt*state[8]) + dt*sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_48451545294533288[9] = 0; - out_48451545294533288[10] = 0; - out_48451545294533288[11] = 0; - out_48451545294533288[12] = 0; - out_48451545294533288[13] = 0; - out_48451545294533288[14] = 0; - out_48451545294533288[15] = 0; - out_48451545294533288[16] = 0; - out_48451545294533288[17] = 0; - out_48451545294533288[18] = (-sin(dt*state[7])*sin(state[0])*cos(state[1]) - sin(dt*state[8])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); - out_48451545294533288[19] = (-sin(dt*state[7])*sin(state[1])*cos(state[0]) + sin(dt*state[8])*sin(state[0])*sin(state[1])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); - out_48451545294533288[20] = 0; - out_48451545294533288[21] = 0; - out_48451545294533288[22] = 0; - out_48451545294533288[23] = 0; - out_48451545294533288[24] = 0; - out_48451545294533288[25] = (dt*sin(dt*state[7])*sin(dt*state[8])*sin(state[0])*cos(state[1]) - dt*sin(dt*state[7])*sin(state[1])*cos(dt*state[8]) + dt*cos(dt*state[7])*cos(state[0])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); - out_48451545294533288[26] = (-dt*sin(dt*state[8])*sin(state[1])*cos(dt*state[7]) - dt*sin(state[0])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); - out_48451545294533288[27] = 0; - out_48451545294533288[28] = 0; - out_48451545294533288[29] = 0; - out_48451545294533288[30] = 0; - out_48451545294533288[31] = 0; - out_48451545294533288[32] = 0; - out_48451545294533288[33] = 0; - out_48451545294533288[34] = 0; - out_48451545294533288[35] = 0; - out_48451545294533288[36] = ((sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[7]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[7]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_48451545294533288[37] = (-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(-sin(dt*state[7])*sin(state[2])*cos(state[0])*cos(state[1]) + sin(dt*state[8])*sin(state[0])*sin(state[2])*cos(dt*state[7])*cos(state[1]) - sin(state[1])*sin(state[2])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*(-sin(dt*state[7])*cos(state[0])*cos(state[1])*cos(state[2]) + sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1])*cos(state[2]) - sin(state[1])*cos(dt*state[7])*cos(dt*state[8])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_48451545294533288[38] = ((-sin(state[0])*sin(state[2]) - sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (-sin(state[0])*sin(state[1])*sin(state[2]) - cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_48451545294533288[39] = 0; - out_48451545294533288[40] = 0; - out_48451545294533288[41] = 0; - out_48451545294533288[42] = 0; - out_48451545294533288[43] = (-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(dt*(sin(state[0])*cos(state[2]) - sin(state[1])*sin(state[2])*cos(state[0]))*cos(dt*state[7]) - dt*(sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[7])*sin(dt*state[8]) - dt*sin(dt*state[7])*sin(state[2])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*(dt*(-sin(state[0])*sin(state[2]) - sin(state[1])*cos(state[0])*cos(state[2]))*cos(dt*state[7]) - dt*(sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[7])*sin(dt*state[8]) - dt*sin(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_48451545294533288[44] = (dt*(sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*cos(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*sin(state[2])*cos(dt*state[7])*cos(state[1]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + (dt*(sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*cos(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[7])*cos(state[1])*cos(state[2]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_48451545294533288[45] = 0; - out_48451545294533288[46] = 0; - out_48451545294533288[47] = 0; - out_48451545294533288[48] = 0; - out_48451545294533288[49] = 0; - out_48451545294533288[50] = 0; - out_48451545294533288[51] = 0; - out_48451545294533288[52] = 0; - out_48451545294533288[53] = 0; - out_48451545294533288[54] = 0; - out_48451545294533288[55] = 0; - out_48451545294533288[56] = 0; - out_48451545294533288[57] = 1; - out_48451545294533288[58] = 0; - out_48451545294533288[59] = 0; - out_48451545294533288[60] = 0; - out_48451545294533288[61] = 0; - out_48451545294533288[62] = 0; - out_48451545294533288[63] = 0; - out_48451545294533288[64] = 0; - out_48451545294533288[65] = 0; - out_48451545294533288[66] = dt; - out_48451545294533288[67] = 0; - out_48451545294533288[68] = 0; - out_48451545294533288[69] = 0; - out_48451545294533288[70] = 0; - out_48451545294533288[71] = 0; - out_48451545294533288[72] = 0; - out_48451545294533288[73] = 0; - out_48451545294533288[74] = 0; - out_48451545294533288[75] = 0; - out_48451545294533288[76] = 1; - out_48451545294533288[77] = 0; - out_48451545294533288[78] = 0; - out_48451545294533288[79] = 0; - out_48451545294533288[80] = 0; - out_48451545294533288[81] = 0; - out_48451545294533288[82] = 0; - out_48451545294533288[83] = 0; - out_48451545294533288[84] = 0; - out_48451545294533288[85] = dt; - out_48451545294533288[86] = 0; - out_48451545294533288[87] = 0; - out_48451545294533288[88] = 0; - out_48451545294533288[89] = 0; - out_48451545294533288[90] = 0; - out_48451545294533288[91] = 0; - out_48451545294533288[92] = 0; - out_48451545294533288[93] = 0; - out_48451545294533288[94] = 0; - out_48451545294533288[95] = 1; - out_48451545294533288[96] = 0; - out_48451545294533288[97] = 0; - out_48451545294533288[98] = 0; - out_48451545294533288[99] = 0; - out_48451545294533288[100] = 0; - out_48451545294533288[101] = 0; - out_48451545294533288[102] = 0; - out_48451545294533288[103] = 0; - out_48451545294533288[104] = dt; - out_48451545294533288[105] = 0; - out_48451545294533288[106] = 0; - out_48451545294533288[107] = 0; - out_48451545294533288[108] = 0; - out_48451545294533288[109] = 0; - out_48451545294533288[110] = 0; - out_48451545294533288[111] = 0; - out_48451545294533288[112] = 0; - out_48451545294533288[113] = 0; - out_48451545294533288[114] = 1; - out_48451545294533288[115] = 0; - out_48451545294533288[116] = 0; - out_48451545294533288[117] = 0; - out_48451545294533288[118] = 0; - out_48451545294533288[119] = 0; - out_48451545294533288[120] = 0; - out_48451545294533288[121] = 0; - out_48451545294533288[122] = 0; - out_48451545294533288[123] = 0; - out_48451545294533288[124] = 0; - out_48451545294533288[125] = 0; - out_48451545294533288[126] = 0; - out_48451545294533288[127] = 0; - out_48451545294533288[128] = 0; - out_48451545294533288[129] = 0; - out_48451545294533288[130] = 0; - out_48451545294533288[131] = 0; - out_48451545294533288[132] = 0; - out_48451545294533288[133] = 1; - out_48451545294533288[134] = 0; - out_48451545294533288[135] = 0; - out_48451545294533288[136] = 0; - out_48451545294533288[137] = 0; - out_48451545294533288[138] = 0; - out_48451545294533288[139] = 0; - out_48451545294533288[140] = 0; - out_48451545294533288[141] = 0; - out_48451545294533288[142] = 0; - out_48451545294533288[143] = 0; - out_48451545294533288[144] = 0; - out_48451545294533288[145] = 0; - out_48451545294533288[146] = 0; - out_48451545294533288[147] = 0; - out_48451545294533288[148] = 0; - out_48451545294533288[149] = 0; - out_48451545294533288[150] = 0; - out_48451545294533288[151] = 0; - out_48451545294533288[152] = 1; - out_48451545294533288[153] = 0; - out_48451545294533288[154] = 0; - out_48451545294533288[155] = 0; - out_48451545294533288[156] = 0; - out_48451545294533288[157] = 0; - out_48451545294533288[158] = 0; - out_48451545294533288[159] = 0; - out_48451545294533288[160] = 0; - out_48451545294533288[161] = 0; - out_48451545294533288[162] = 0; - out_48451545294533288[163] = 0; - out_48451545294533288[164] = 0; - out_48451545294533288[165] = 0; - out_48451545294533288[166] = 0; - out_48451545294533288[167] = 0; - out_48451545294533288[168] = 0; - out_48451545294533288[169] = 0; - out_48451545294533288[170] = 0; - out_48451545294533288[171] = 1; - out_48451545294533288[172] = 0; - out_48451545294533288[173] = 0; - out_48451545294533288[174] = 0; - out_48451545294533288[175] = 0; - out_48451545294533288[176] = 0; - out_48451545294533288[177] = 0; - out_48451545294533288[178] = 0; - out_48451545294533288[179] = 0; - out_48451545294533288[180] = 0; - out_48451545294533288[181] = 0; - out_48451545294533288[182] = 0; - out_48451545294533288[183] = 0; - out_48451545294533288[184] = 0; - out_48451545294533288[185] = 0; - out_48451545294533288[186] = 0; - out_48451545294533288[187] = 0; - out_48451545294533288[188] = 0; - out_48451545294533288[189] = 0; - out_48451545294533288[190] = 1; - out_48451545294533288[191] = 0; - out_48451545294533288[192] = 0; - out_48451545294533288[193] = 0; - out_48451545294533288[194] = 0; - out_48451545294533288[195] = 0; - out_48451545294533288[196] = 0; - out_48451545294533288[197] = 0; - out_48451545294533288[198] = 0; - out_48451545294533288[199] = 0; - out_48451545294533288[200] = 0; - out_48451545294533288[201] = 0; - out_48451545294533288[202] = 0; - out_48451545294533288[203] = 0; - out_48451545294533288[204] = 0; - out_48451545294533288[205] = 0; - out_48451545294533288[206] = 0; - out_48451545294533288[207] = 0; - out_48451545294533288[208] = 0; - out_48451545294533288[209] = 1; - out_48451545294533288[210] = 0; - out_48451545294533288[211] = 0; - out_48451545294533288[212] = 0; - out_48451545294533288[213] = 0; - out_48451545294533288[214] = 0; - out_48451545294533288[215] = 0; - out_48451545294533288[216] = 0; - out_48451545294533288[217] = 0; - out_48451545294533288[218] = 0; - out_48451545294533288[219] = 0; - out_48451545294533288[220] = 0; - out_48451545294533288[221] = 0; - out_48451545294533288[222] = 0; - out_48451545294533288[223] = 0; - out_48451545294533288[224] = 0; - out_48451545294533288[225] = 0; - out_48451545294533288[226] = 0; - out_48451545294533288[227] = 0; - out_48451545294533288[228] = 1; - out_48451545294533288[229] = 0; - out_48451545294533288[230] = 0; - out_48451545294533288[231] = 0; - out_48451545294533288[232] = 0; - out_48451545294533288[233] = 0; - out_48451545294533288[234] = 0; - out_48451545294533288[235] = 0; - out_48451545294533288[236] = 0; - out_48451545294533288[237] = 0; - out_48451545294533288[238] = 0; - out_48451545294533288[239] = 0; - out_48451545294533288[240] = 0; - out_48451545294533288[241] = 0; - out_48451545294533288[242] = 0; - out_48451545294533288[243] = 0; - out_48451545294533288[244] = 0; - out_48451545294533288[245] = 0; - out_48451545294533288[246] = 0; - out_48451545294533288[247] = 1; - out_48451545294533288[248] = 0; - out_48451545294533288[249] = 0; - out_48451545294533288[250] = 0; - out_48451545294533288[251] = 0; - out_48451545294533288[252] = 0; - out_48451545294533288[253] = 0; - out_48451545294533288[254] = 0; - out_48451545294533288[255] = 0; - out_48451545294533288[256] = 0; - out_48451545294533288[257] = 0; - out_48451545294533288[258] = 0; - out_48451545294533288[259] = 0; - out_48451545294533288[260] = 0; - out_48451545294533288[261] = 0; - out_48451545294533288[262] = 0; - out_48451545294533288[263] = 0; - out_48451545294533288[264] = 0; - out_48451545294533288[265] = 0; - out_48451545294533288[266] = 1; - out_48451545294533288[267] = 0; - out_48451545294533288[268] = 0; - out_48451545294533288[269] = 0; - out_48451545294533288[270] = 0; - out_48451545294533288[271] = 0; - out_48451545294533288[272] = 0; - out_48451545294533288[273] = 0; - out_48451545294533288[274] = 0; - out_48451545294533288[275] = 0; - out_48451545294533288[276] = 0; - out_48451545294533288[277] = 0; - out_48451545294533288[278] = 0; - out_48451545294533288[279] = 0; - out_48451545294533288[280] = 0; - out_48451545294533288[281] = 0; - out_48451545294533288[282] = 0; - out_48451545294533288[283] = 0; - out_48451545294533288[284] = 0; - out_48451545294533288[285] = 1; - out_48451545294533288[286] = 0; - out_48451545294533288[287] = 0; - out_48451545294533288[288] = 0; - out_48451545294533288[289] = 0; - out_48451545294533288[290] = 0; - out_48451545294533288[291] = 0; - out_48451545294533288[292] = 0; - out_48451545294533288[293] = 0; - out_48451545294533288[294] = 0; - out_48451545294533288[295] = 0; - out_48451545294533288[296] = 0; - out_48451545294533288[297] = 0; - out_48451545294533288[298] = 0; - out_48451545294533288[299] = 0; - out_48451545294533288[300] = 0; - out_48451545294533288[301] = 0; - out_48451545294533288[302] = 0; - out_48451545294533288[303] = 0; - out_48451545294533288[304] = 1; - out_48451545294533288[305] = 0; - out_48451545294533288[306] = 0; - out_48451545294533288[307] = 0; - out_48451545294533288[308] = 0; - out_48451545294533288[309] = 0; - out_48451545294533288[310] = 0; - out_48451545294533288[311] = 0; - out_48451545294533288[312] = 0; - out_48451545294533288[313] = 0; - out_48451545294533288[314] = 0; - out_48451545294533288[315] = 0; - out_48451545294533288[316] = 0; - out_48451545294533288[317] = 0; - out_48451545294533288[318] = 0; - out_48451545294533288[319] = 0; - out_48451545294533288[320] = 0; - out_48451545294533288[321] = 0; - out_48451545294533288[322] = 0; - out_48451545294533288[323] = 1; +void F_fun(double *state, double dt, double *out_9018422947626896264) { + out_9018422947626896264[0] = ((-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*cos(state[0])*cos(state[1]) - sin(state[0])*cos(dt*state[6])*cos(dt*state[7])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + ((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*cos(state[0])*cos(state[1]) - sin(dt*state[6])*sin(state[0])*cos(dt*state[7])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); + out_9018422947626896264[1] = ((-sin(dt*state[6])*sin(dt*state[8]) - sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*cos(state[1]) - (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*sin(state[1]) - sin(state[1])*cos(dt*state[6])*cos(dt*state[7])*cos(state[0]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*sin(state[1]) + (-sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) + sin(dt*state[8])*cos(dt*state[6]))*cos(state[1]) - sin(dt*state[6])*sin(state[1])*cos(dt*state[7])*cos(state[0]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); + out_9018422947626896264[2] = 0; + out_9018422947626896264[3] = 0; + out_9018422947626896264[4] = 0; + out_9018422947626896264[5] = 0; + out_9018422947626896264[6] = (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(dt*cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*sin(dt*state[8]) - dt*sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-dt*sin(dt*state[6])*cos(dt*state[8]) + dt*sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) - dt*cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (dt*sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); + out_9018422947626896264[7] = (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[6])*sin(dt*state[7])*cos(state[0])*cos(state[1]) + dt*sin(dt*state[6])*sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) - dt*sin(dt*state[6])*sin(state[1])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[7])*cos(dt*state[6])*cos(state[0])*cos(state[1]) + dt*sin(dt*state[8])*sin(state[0])*cos(dt*state[6])*cos(dt*state[7])*cos(state[1]) - dt*sin(state[1])*cos(dt*state[6])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); + out_9018422947626896264[8] = ((dt*sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + dt*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (dt*sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + ((dt*sin(dt*state[6])*sin(dt*state[8]) + dt*sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*cos(dt*state[8]) + dt*sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); + out_9018422947626896264[9] = 0; + out_9018422947626896264[10] = 0; + out_9018422947626896264[11] = 0; + out_9018422947626896264[12] = 0; + out_9018422947626896264[13] = 0; + out_9018422947626896264[14] = 0; + out_9018422947626896264[15] = 0; + out_9018422947626896264[16] = 0; + out_9018422947626896264[17] = 0; + out_9018422947626896264[18] = (-sin(dt*state[7])*sin(state[0])*cos(state[1]) - sin(dt*state[8])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); + out_9018422947626896264[19] = (-sin(dt*state[7])*sin(state[1])*cos(state[0]) + sin(dt*state[8])*sin(state[0])*sin(state[1])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); + out_9018422947626896264[20] = 0; + out_9018422947626896264[21] = 0; + out_9018422947626896264[22] = 0; + out_9018422947626896264[23] = 0; + out_9018422947626896264[24] = 0; + out_9018422947626896264[25] = (dt*sin(dt*state[7])*sin(dt*state[8])*sin(state[0])*cos(state[1]) - dt*sin(dt*state[7])*sin(state[1])*cos(dt*state[8]) + dt*cos(dt*state[7])*cos(state[0])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); + out_9018422947626896264[26] = (-dt*sin(dt*state[8])*sin(state[1])*cos(dt*state[7]) - dt*sin(state[0])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); + out_9018422947626896264[27] = 0; + out_9018422947626896264[28] = 0; + out_9018422947626896264[29] = 0; + out_9018422947626896264[30] = 0; + out_9018422947626896264[31] = 0; + out_9018422947626896264[32] = 0; + out_9018422947626896264[33] = 0; + out_9018422947626896264[34] = 0; + out_9018422947626896264[35] = 0; + out_9018422947626896264[36] = ((sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[7]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[7]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); + out_9018422947626896264[37] = (-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(-sin(dt*state[7])*sin(state[2])*cos(state[0])*cos(state[1]) + sin(dt*state[8])*sin(state[0])*sin(state[2])*cos(dt*state[7])*cos(state[1]) - sin(state[1])*sin(state[2])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*(-sin(dt*state[7])*cos(state[0])*cos(state[1])*cos(state[2]) + sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1])*cos(state[2]) - sin(state[1])*cos(dt*state[7])*cos(dt*state[8])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); + out_9018422947626896264[38] = ((-sin(state[0])*sin(state[2]) - sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (-sin(state[0])*sin(state[1])*sin(state[2]) - cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); + out_9018422947626896264[39] = 0; + out_9018422947626896264[40] = 0; + out_9018422947626896264[41] = 0; + out_9018422947626896264[42] = 0; + out_9018422947626896264[43] = (-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(dt*(sin(state[0])*cos(state[2]) - sin(state[1])*sin(state[2])*cos(state[0]))*cos(dt*state[7]) - dt*(sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[7])*sin(dt*state[8]) - dt*sin(dt*state[7])*sin(state[2])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*(dt*(-sin(state[0])*sin(state[2]) - sin(state[1])*cos(state[0])*cos(state[2]))*cos(dt*state[7]) - dt*(sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[7])*sin(dt*state[8]) - dt*sin(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); + out_9018422947626896264[44] = (dt*(sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*cos(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*sin(state[2])*cos(dt*state[7])*cos(state[1]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + (dt*(sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*cos(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[7])*cos(state[1])*cos(state[2]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); + out_9018422947626896264[45] = 0; + out_9018422947626896264[46] = 0; + out_9018422947626896264[47] = 0; + out_9018422947626896264[48] = 0; + out_9018422947626896264[49] = 0; + out_9018422947626896264[50] = 0; + out_9018422947626896264[51] = 0; + out_9018422947626896264[52] = 0; + out_9018422947626896264[53] = 0; + out_9018422947626896264[54] = 0; + out_9018422947626896264[55] = 0; + out_9018422947626896264[56] = 0; + out_9018422947626896264[57] = 1; + out_9018422947626896264[58] = 0; + out_9018422947626896264[59] = 0; + out_9018422947626896264[60] = 0; + out_9018422947626896264[61] = 0; + out_9018422947626896264[62] = 0; + out_9018422947626896264[63] = 0; + out_9018422947626896264[64] = 0; + out_9018422947626896264[65] = 0; + out_9018422947626896264[66] = dt; + out_9018422947626896264[67] = 0; + out_9018422947626896264[68] = 0; + out_9018422947626896264[69] = 0; + out_9018422947626896264[70] = 0; + out_9018422947626896264[71] = 0; + out_9018422947626896264[72] = 0; + out_9018422947626896264[73] = 0; + out_9018422947626896264[74] = 0; + out_9018422947626896264[75] = 0; + out_9018422947626896264[76] = 1; + out_9018422947626896264[77] = 0; + out_9018422947626896264[78] = 0; + out_9018422947626896264[79] = 0; + out_9018422947626896264[80] = 0; + out_9018422947626896264[81] = 0; + out_9018422947626896264[82] = 0; + out_9018422947626896264[83] = 0; + out_9018422947626896264[84] = 0; + out_9018422947626896264[85] = dt; + out_9018422947626896264[86] = 0; + out_9018422947626896264[87] = 0; + out_9018422947626896264[88] = 0; + out_9018422947626896264[89] = 0; + out_9018422947626896264[90] = 0; + out_9018422947626896264[91] = 0; + out_9018422947626896264[92] = 0; + out_9018422947626896264[93] = 0; + out_9018422947626896264[94] = 0; + out_9018422947626896264[95] = 1; + out_9018422947626896264[96] = 0; + out_9018422947626896264[97] = 0; + out_9018422947626896264[98] = 0; + out_9018422947626896264[99] = 0; + out_9018422947626896264[100] = 0; + out_9018422947626896264[101] = 0; + out_9018422947626896264[102] = 0; + out_9018422947626896264[103] = 0; + out_9018422947626896264[104] = dt; + out_9018422947626896264[105] = 0; + out_9018422947626896264[106] = 0; + out_9018422947626896264[107] = 0; + out_9018422947626896264[108] = 0; + out_9018422947626896264[109] = 0; + out_9018422947626896264[110] = 0; + out_9018422947626896264[111] = 0; + out_9018422947626896264[112] = 0; + out_9018422947626896264[113] = 0; + out_9018422947626896264[114] = 1; + out_9018422947626896264[115] = 0; + out_9018422947626896264[116] = 0; + out_9018422947626896264[117] = 0; + out_9018422947626896264[118] = 0; + out_9018422947626896264[119] = 0; + out_9018422947626896264[120] = 0; + out_9018422947626896264[121] = 0; + out_9018422947626896264[122] = 0; + out_9018422947626896264[123] = 0; + out_9018422947626896264[124] = 0; + out_9018422947626896264[125] = 0; + out_9018422947626896264[126] = 0; + out_9018422947626896264[127] = 0; + out_9018422947626896264[128] = 0; + out_9018422947626896264[129] = 0; + out_9018422947626896264[130] = 0; + out_9018422947626896264[131] = 0; + out_9018422947626896264[132] = 0; + out_9018422947626896264[133] = 1; + out_9018422947626896264[134] = 0; + out_9018422947626896264[135] = 0; + out_9018422947626896264[136] = 0; + out_9018422947626896264[137] = 0; + out_9018422947626896264[138] = 0; + out_9018422947626896264[139] = 0; + out_9018422947626896264[140] = 0; + out_9018422947626896264[141] = 0; + out_9018422947626896264[142] = 0; + out_9018422947626896264[143] = 0; + out_9018422947626896264[144] = 0; + out_9018422947626896264[145] = 0; + out_9018422947626896264[146] = 0; + out_9018422947626896264[147] = 0; + out_9018422947626896264[148] = 0; + out_9018422947626896264[149] = 0; + out_9018422947626896264[150] = 0; + out_9018422947626896264[151] = 0; + out_9018422947626896264[152] = 1; + out_9018422947626896264[153] = 0; + out_9018422947626896264[154] = 0; + out_9018422947626896264[155] = 0; + out_9018422947626896264[156] = 0; + out_9018422947626896264[157] = 0; + out_9018422947626896264[158] = 0; + out_9018422947626896264[159] = 0; + out_9018422947626896264[160] = 0; + out_9018422947626896264[161] = 0; + out_9018422947626896264[162] = 0; + out_9018422947626896264[163] = 0; + out_9018422947626896264[164] = 0; + out_9018422947626896264[165] = 0; + out_9018422947626896264[166] = 0; + out_9018422947626896264[167] = 0; + out_9018422947626896264[168] = 0; + out_9018422947626896264[169] = 0; + out_9018422947626896264[170] = 0; + out_9018422947626896264[171] = 1; + out_9018422947626896264[172] = 0; + out_9018422947626896264[173] = 0; + out_9018422947626896264[174] = 0; + out_9018422947626896264[175] = 0; + out_9018422947626896264[176] = 0; + out_9018422947626896264[177] = 0; + out_9018422947626896264[178] = 0; + out_9018422947626896264[179] = 0; + out_9018422947626896264[180] = 0; + out_9018422947626896264[181] = 0; + out_9018422947626896264[182] = 0; + out_9018422947626896264[183] = 0; + out_9018422947626896264[184] = 0; + out_9018422947626896264[185] = 0; + out_9018422947626896264[186] = 0; + out_9018422947626896264[187] = 0; + out_9018422947626896264[188] = 0; + out_9018422947626896264[189] = 0; + out_9018422947626896264[190] = 1; + out_9018422947626896264[191] = 0; + out_9018422947626896264[192] = 0; + out_9018422947626896264[193] = 0; + out_9018422947626896264[194] = 0; + out_9018422947626896264[195] = 0; + out_9018422947626896264[196] = 0; + out_9018422947626896264[197] = 0; + out_9018422947626896264[198] = 0; + out_9018422947626896264[199] = 0; + out_9018422947626896264[200] = 0; + out_9018422947626896264[201] = 0; + out_9018422947626896264[202] = 0; + out_9018422947626896264[203] = 0; + out_9018422947626896264[204] = 0; + out_9018422947626896264[205] = 0; + out_9018422947626896264[206] = 0; + out_9018422947626896264[207] = 0; + out_9018422947626896264[208] = 0; + out_9018422947626896264[209] = 1; + out_9018422947626896264[210] = 0; + out_9018422947626896264[211] = 0; + out_9018422947626896264[212] = 0; + out_9018422947626896264[213] = 0; + out_9018422947626896264[214] = 0; + out_9018422947626896264[215] = 0; + out_9018422947626896264[216] = 0; + out_9018422947626896264[217] = 0; + out_9018422947626896264[218] = 0; + out_9018422947626896264[219] = 0; + out_9018422947626896264[220] = 0; + out_9018422947626896264[221] = 0; + out_9018422947626896264[222] = 0; + out_9018422947626896264[223] = 0; + out_9018422947626896264[224] = 0; + out_9018422947626896264[225] = 0; + out_9018422947626896264[226] = 0; + out_9018422947626896264[227] = 0; + out_9018422947626896264[228] = 1; + out_9018422947626896264[229] = 0; + out_9018422947626896264[230] = 0; + out_9018422947626896264[231] = 0; + out_9018422947626896264[232] = 0; + out_9018422947626896264[233] = 0; + out_9018422947626896264[234] = 0; + out_9018422947626896264[235] = 0; + out_9018422947626896264[236] = 0; + out_9018422947626896264[237] = 0; + out_9018422947626896264[238] = 0; + out_9018422947626896264[239] = 0; + out_9018422947626896264[240] = 0; + out_9018422947626896264[241] = 0; + out_9018422947626896264[242] = 0; + out_9018422947626896264[243] = 0; + out_9018422947626896264[244] = 0; + out_9018422947626896264[245] = 0; + out_9018422947626896264[246] = 0; + out_9018422947626896264[247] = 1; + out_9018422947626896264[248] = 0; + out_9018422947626896264[249] = 0; + out_9018422947626896264[250] = 0; + out_9018422947626896264[251] = 0; + out_9018422947626896264[252] = 0; + out_9018422947626896264[253] = 0; + out_9018422947626896264[254] = 0; + out_9018422947626896264[255] = 0; + out_9018422947626896264[256] = 0; + out_9018422947626896264[257] = 0; + out_9018422947626896264[258] = 0; + out_9018422947626896264[259] = 0; + out_9018422947626896264[260] = 0; + out_9018422947626896264[261] = 0; + out_9018422947626896264[262] = 0; + out_9018422947626896264[263] = 0; + out_9018422947626896264[264] = 0; + out_9018422947626896264[265] = 0; + out_9018422947626896264[266] = 1; + out_9018422947626896264[267] = 0; + out_9018422947626896264[268] = 0; + out_9018422947626896264[269] = 0; + out_9018422947626896264[270] = 0; + out_9018422947626896264[271] = 0; + out_9018422947626896264[272] = 0; + out_9018422947626896264[273] = 0; + out_9018422947626896264[274] = 0; + out_9018422947626896264[275] = 0; + out_9018422947626896264[276] = 0; + out_9018422947626896264[277] = 0; + out_9018422947626896264[278] = 0; + out_9018422947626896264[279] = 0; + out_9018422947626896264[280] = 0; + out_9018422947626896264[281] = 0; + out_9018422947626896264[282] = 0; + out_9018422947626896264[283] = 0; + out_9018422947626896264[284] = 0; + out_9018422947626896264[285] = 1; + out_9018422947626896264[286] = 0; + out_9018422947626896264[287] = 0; + out_9018422947626896264[288] = 0; + out_9018422947626896264[289] = 0; + out_9018422947626896264[290] = 0; + out_9018422947626896264[291] = 0; + out_9018422947626896264[292] = 0; + out_9018422947626896264[293] = 0; + out_9018422947626896264[294] = 0; + out_9018422947626896264[295] = 0; + out_9018422947626896264[296] = 0; + out_9018422947626896264[297] = 0; + out_9018422947626896264[298] = 0; + out_9018422947626896264[299] = 0; + out_9018422947626896264[300] = 0; + out_9018422947626896264[301] = 0; + out_9018422947626896264[302] = 0; + out_9018422947626896264[303] = 0; + out_9018422947626896264[304] = 1; + out_9018422947626896264[305] = 0; + out_9018422947626896264[306] = 0; + out_9018422947626896264[307] = 0; + out_9018422947626896264[308] = 0; + out_9018422947626896264[309] = 0; + out_9018422947626896264[310] = 0; + out_9018422947626896264[311] = 0; + out_9018422947626896264[312] = 0; + out_9018422947626896264[313] = 0; + out_9018422947626896264[314] = 0; + out_9018422947626896264[315] = 0; + out_9018422947626896264[316] = 0; + out_9018422947626896264[317] = 0; + out_9018422947626896264[318] = 0; + out_9018422947626896264[319] = 0; + out_9018422947626896264[320] = 0; + out_9018422947626896264[321] = 0; + out_9018422947626896264[322] = 0; + out_9018422947626896264[323] = 1; } -void h_4(double *state, double *unused, double *out_711207782401779392) { - out_711207782401779392[0] = state[6] + state[9]; - out_711207782401779392[1] = state[7] + state[10]; - out_711207782401779392[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_6773718258743003346) { + out_6773718258743003346[0] = state[6] + state[9]; + out_6773718258743003346[1] = state[7] + state[10]; + out_6773718258743003346[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_6437019938278623926) { - out_6437019938278623926[0] = 0; - out_6437019938278623926[1] = 0; - out_6437019938278623926[2] = 0; - out_6437019938278623926[3] = 0; - out_6437019938278623926[4] = 0; - out_6437019938278623926[5] = 0; - out_6437019938278623926[6] = 1; - out_6437019938278623926[7] = 0; - out_6437019938278623926[8] = 0; - out_6437019938278623926[9] = 1; - out_6437019938278623926[10] = 0; - out_6437019938278623926[11] = 0; - out_6437019938278623926[12] = 0; - out_6437019938278623926[13] = 0; - out_6437019938278623926[14] = 0; - out_6437019938278623926[15] = 0; - out_6437019938278623926[16] = 0; - out_6437019938278623926[17] = 0; - out_6437019938278623926[18] = 0; - out_6437019938278623926[19] = 0; - out_6437019938278623926[20] = 0; - out_6437019938278623926[21] = 0; - out_6437019938278623926[22] = 0; - out_6437019938278623926[23] = 0; - out_6437019938278623926[24] = 0; - out_6437019938278623926[25] = 1; - out_6437019938278623926[26] = 0; - out_6437019938278623926[27] = 0; - out_6437019938278623926[28] = 1; - out_6437019938278623926[29] = 0; - out_6437019938278623926[30] = 0; - out_6437019938278623926[31] = 0; - out_6437019938278623926[32] = 0; - out_6437019938278623926[33] = 0; - out_6437019938278623926[34] = 0; - out_6437019938278623926[35] = 0; - out_6437019938278623926[36] = 0; - out_6437019938278623926[37] = 0; - out_6437019938278623926[38] = 0; - out_6437019938278623926[39] = 0; - out_6437019938278623926[40] = 0; - out_6437019938278623926[41] = 0; - out_6437019938278623926[42] = 0; - out_6437019938278623926[43] = 0; - out_6437019938278623926[44] = 1; - out_6437019938278623926[45] = 0; - out_6437019938278623926[46] = 0; - out_6437019938278623926[47] = 1; - out_6437019938278623926[48] = 0; - out_6437019938278623926[49] = 0; - out_6437019938278623926[50] = 0; - out_6437019938278623926[51] = 0; - out_6437019938278623926[52] = 0; - out_6437019938278623926[53] = 0; +void H_4(double *state, double *unused, double *out_793482604584818813) { + out_793482604584818813[0] = 0; + out_793482604584818813[1] = 0; + out_793482604584818813[2] = 0; + out_793482604584818813[3] = 0; + out_793482604584818813[4] = 0; + out_793482604584818813[5] = 0; + out_793482604584818813[6] = 1; + out_793482604584818813[7] = 0; + out_793482604584818813[8] = 0; + out_793482604584818813[9] = 1; + out_793482604584818813[10] = 0; + out_793482604584818813[11] = 0; + out_793482604584818813[12] = 0; + out_793482604584818813[13] = 0; + out_793482604584818813[14] = 0; + out_793482604584818813[15] = 0; + out_793482604584818813[16] = 0; + out_793482604584818813[17] = 0; + out_793482604584818813[18] = 0; + out_793482604584818813[19] = 0; + out_793482604584818813[20] = 0; + out_793482604584818813[21] = 0; + out_793482604584818813[22] = 0; + out_793482604584818813[23] = 0; + out_793482604584818813[24] = 0; + out_793482604584818813[25] = 1; + out_793482604584818813[26] = 0; + out_793482604584818813[27] = 0; + out_793482604584818813[28] = 1; + out_793482604584818813[29] = 0; + out_793482604584818813[30] = 0; + out_793482604584818813[31] = 0; + out_793482604584818813[32] = 0; + out_793482604584818813[33] = 0; + out_793482604584818813[34] = 0; + out_793482604584818813[35] = 0; + out_793482604584818813[36] = 0; + out_793482604584818813[37] = 0; + out_793482604584818813[38] = 0; + out_793482604584818813[39] = 0; + out_793482604584818813[40] = 0; + out_793482604584818813[41] = 0; + out_793482604584818813[42] = 0; + out_793482604584818813[43] = 0; + out_793482604584818813[44] = 1; + out_793482604584818813[45] = 0; + out_793482604584818813[46] = 0; + out_793482604584818813[47] = 1; + out_793482604584818813[48] = 0; + out_793482604584818813[49] = 0; + out_793482604584818813[50] = 0; + out_793482604584818813[51] = 0; + out_793482604584818813[52] = 0; + out_793482604584818813[53] = 0; } -void h_10(double *state, double *unused, double *out_2942660564170460979) { - out_2942660564170460979[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_2942660564170460979[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_2942660564170460979[2] = -9.8100000000000005*cos(state[0])*cos(state[1]) - state[3]*state[7] + state[4]*state[6] + state[14] + state[17]; +void h_10(double *state, double *unused, double *out_3870110240018051215) { + out_3870110240018051215[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_3870110240018051215[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_3870110240018051215[2] = -9.8100000000000005*cos(state[0])*cos(state[1]) - state[3]*state[7] + state[4]*state[6] + state[14] + state[17]; } -void H_10(double *state, double *unused, double *out_7753365329259358525) { - out_7753365329259358525[0] = 0; - out_7753365329259358525[1] = 9.8100000000000005*cos(state[1]); - out_7753365329259358525[2] = 0; - out_7753365329259358525[3] = 0; - out_7753365329259358525[4] = -state[8]; - out_7753365329259358525[5] = state[7]; - out_7753365329259358525[6] = 0; - out_7753365329259358525[7] = state[5]; - out_7753365329259358525[8] = -state[4]; - out_7753365329259358525[9] = 0; - out_7753365329259358525[10] = 0; - out_7753365329259358525[11] = 0; - out_7753365329259358525[12] = 1; - out_7753365329259358525[13] = 0; - out_7753365329259358525[14] = 0; - out_7753365329259358525[15] = 1; - out_7753365329259358525[16] = 0; - out_7753365329259358525[17] = 0; - out_7753365329259358525[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_7753365329259358525[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_7753365329259358525[20] = 0; - out_7753365329259358525[21] = state[8]; - out_7753365329259358525[22] = 0; - out_7753365329259358525[23] = -state[6]; - out_7753365329259358525[24] = -state[5]; - out_7753365329259358525[25] = 0; - out_7753365329259358525[26] = state[3]; - out_7753365329259358525[27] = 0; - out_7753365329259358525[28] = 0; - out_7753365329259358525[29] = 0; - out_7753365329259358525[30] = 0; - out_7753365329259358525[31] = 1; - out_7753365329259358525[32] = 0; - out_7753365329259358525[33] = 0; - out_7753365329259358525[34] = 1; - out_7753365329259358525[35] = 0; - out_7753365329259358525[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_7753365329259358525[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_7753365329259358525[38] = 0; - out_7753365329259358525[39] = -state[7]; - out_7753365329259358525[40] = state[6]; - out_7753365329259358525[41] = 0; - out_7753365329259358525[42] = state[4]; - out_7753365329259358525[43] = -state[3]; - out_7753365329259358525[44] = 0; - out_7753365329259358525[45] = 0; - out_7753365329259358525[46] = 0; - out_7753365329259358525[47] = 0; - out_7753365329259358525[48] = 0; - out_7753365329259358525[49] = 0; - out_7753365329259358525[50] = 1; - out_7753365329259358525[51] = 0; - out_7753365329259358525[52] = 0; - out_7753365329259358525[53] = 1; +void H_10(double *state, double *unused, double *out_3089556940454179292) { + out_3089556940454179292[0] = 0; + out_3089556940454179292[1] = 9.8100000000000005*cos(state[1]); + out_3089556940454179292[2] = 0; + out_3089556940454179292[3] = 0; + out_3089556940454179292[4] = -state[8]; + out_3089556940454179292[5] = state[7]; + out_3089556940454179292[6] = 0; + out_3089556940454179292[7] = state[5]; + out_3089556940454179292[8] = -state[4]; + out_3089556940454179292[9] = 0; + out_3089556940454179292[10] = 0; + out_3089556940454179292[11] = 0; + out_3089556940454179292[12] = 1; + out_3089556940454179292[13] = 0; + out_3089556940454179292[14] = 0; + out_3089556940454179292[15] = 1; + out_3089556940454179292[16] = 0; + out_3089556940454179292[17] = 0; + out_3089556940454179292[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_3089556940454179292[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_3089556940454179292[20] = 0; + out_3089556940454179292[21] = state[8]; + out_3089556940454179292[22] = 0; + out_3089556940454179292[23] = -state[6]; + out_3089556940454179292[24] = -state[5]; + out_3089556940454179292[25] = 0; + out_3089556940454179292[26] = state[3]; + out_3089556940454179292[27] = 0; + out_3089556940454179292[28] = 0; + out_3089556940454179292[29] = 0; + out_3089556940454179292[30] = 0; + out_3089556940454179292[31] = 1; + out_3089556940454179292[32] = 0; + out_3089556940454179292[33] = 0; + out_3089556940454179292[34] = 1; + out_3089556940454179292[35] = 0; + out_3089556940454179292[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_3089556940454179292[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_3089556940454179292[38] = 0; + out_3089556940454179292[39] = -state[7]; + out_3089556940454179292[40] = state[6]; + out_3089556940454179292[41] = 0; + out_3089556940454179292[42] = state[4]; + out_3089556940454179292[43] = -state[3]; + out_3089556940454179292[44] = 0; + out_3089556940454179292[45] = 0; + out_3089556940454179292[46] = 0; + out_3089556940454179292[47] = 0; + out_3089556940454179292[48] = 0; + out_3089556940454179292[49] = 0; + out_3089556940454179292[50] = 1; + out_3089556940454179292[51] = 0; + out_3089556940454179292[52] = 0; + out_3089556940454179292[53] = 1; } -void h_13(double *state, double *unused, double *out_2914132243760424113) { - out_2914132243760424113[0] = state[3]; - out_2914132243760424113[1] = state[4]; - out_2914132243760424113[2] = state[5]; +void h_13(double *state, double *unused, double *out_7087737251706748874) { + out_7087737251706748874[0] = state[3]; + out_7087737251706748874[1] = state[4]; + out_7087737251706748874[2] = state[5]; } -void H_13(double *state, double *unused, double *out_3224746112946291125) { - out_3224746112946291125[0] = 0; - out_3224746112946291125[1] = 0; - out_3224746112946291125[2] = 0; - out_3224746112946291125[3] = 1; - out_3224746112946291125[4] = 0; - out_3224746112946291125[5] = 0; - out_3224746112946291125[6] = 0; - out_3224746112946291125[7] = 0; - out_3224746112946291125[8] = 0; - out_3224746112946291125[9] = 0; - out_3224746112946291125[10] = 0; - out_3224746112946291125[11] = 0; - out_3224746112946291125[12] = 0; - out_3224746112946291125[13] = 0; - out_3224746112946291125[14] = 0; - out_3224746112946291125[15] = 0; - out_3224746112946291125[16] = 0; - out_3224746112946291125[17] = 0; - out_3224746112946291125[18] = 0; - out_3224746112946291125[19] = 0; - out_3224746112946291125[20] = 0; - out_3224746112946291125[21] = 0; - out_3224746112946291125[22] = 1; - out_3224746112946291125[23] = 0; - out_3224746112946291125[24] = 0; - out_3224746112946291125[25] = 0; - out_3224746112946291125[26] = 0; - out_3224746112946291125[27] = 0; - out_3224746112946291125[28] = 0; - out_3224746112946291125[29] = 0; - out_3224746112946291125[30] = 0; - out_3224746112946291125[31] = 0; - out_3224746112946291125[32] = 0; - out_3224746112946291125[33] = 0; - out_3224746112946291125[34] = 0; - out_3224746112946291125[35] = 0; - out_3224746112946291125[36] = 0; - out_3224746112946291125[37] = 0; - out_3224746112946291125[38] = 0; - out_3224746112946291125[39] = 0; - out_3224746112946291125[40] = 0; - out_3224746112946291125[41] = 1; - out_3224746112946291125[42] = 0; - out_3224746112946291125[43] = 0; - out_3224746112946291125[44] = 0; - out_3224746112946291125[45] = 0; - out_3224746112946291125[46] = 0; - out_3224746112946291125[47] = 0; - out_3224746112946291125[48] = 0; - out_3224746112946291125[49] = 0; - out_3224746112946291125[50] = 0; - out_3224746112946291125[51] = 0; - out_3224746112946291125[52] = 0; - out_3224746112946291125[53] = 0; +void H_13(double *state, double *unused, double *out_2418791220747513988) { + out_2418791220747513988[0] = 0; + out_2418791220747513988[1] = 0; + out_2418791220747513988[2] = 0; + out_2418791220747513988[3] = 1; + out_2418791220747513988[4] = 0; + out_2418791220747513988[5] = 0; + out_2418791220747513988[6] = 0; + out_2418791220747513988[7] = 0; + out_2418791220747513988[8] = 0; + out_2418791220747513988[9] = 0; + out_2418791220747513988[10] = 0; + out_2418791220747513988[11] = 0; + out_2418791220747513988[12] = 0; + out_2418791220747513988[13] = 0; + out_2418791220747513988[14] = 0; + out_2418791220747513988[15] = 0; + out_2418791220747513988[16] = 0; + out_2418791220747513988[17] = 0; + out_2418791220747513988[18] = 0; + out_2418791220747513988[19] = 0; + out_2418791220747513988[20] = 0; + out_2418791220747513988[21] = 0; + out_2418791220747513988[22] = 1; + out_2418791220747513988[23] = 0; + out_2418791220747513988[24] = 0; + out_2418791220747513988[25] = 0; + out_2418791220747513988[26] = 0; + out_2418791220747513988[27] = 0; + out_2418791220747513988[28] = 0; + out_2418791220747513988[29] = 0; + out_2418791220747513988[30] = 0; + out_2418791220747513988[31] = 0; + out_2418791220747513988[32] = 0; + out_2418791220747513988[33] = 0; + out_2418791220747513988[34] = 0; + out_2418791220747513988[35] = 0; + out_2418791220747513988[36] = 0; + out_2418791220747513988[37] = 0; + out_2418791220747513988[38] = 0; + out_2418791220747513988[39] = 0; + out_2418791220747513988[40] = 0; + out_2418791220747513988[41] = 1; + out_2418791220747513988[42] = 0; + out_2418791220747513988[43] = 0; + out_2418791220747513988[44] = 0; + out_2418791220747513988[45] = 0; + out_2418791220747513988[46] = 0; + out_2418791220747513988[47] = 0; + out_2418791220747513988[48] = 0; + out_2418791220747513988[49] = 0; + out_2418791220747513988[50] = 0; + out_2418791220747513988[51] = 0; + out_2418791220747513988[52] = 0; + out_2418791220747513988[53] = 0; } -void h_14(double *state, double *unused, double *out_677306400367602538) { - out_677306400367602538[0] = state[6]; - out_677306400367602538[1] = state[7]; - out_677306400367602538[2] = state[8]; +void h_14(double *state, double *unused, double *out_8542055892000654086) { + out_8542055892000654086[0] = state[6]; + out_8542055892000654086[1] = state[7]; + out_8542055892000654086[2] = state[8]; } -void H_14(double *state, double *unused, double *out_2473779081939139397) { - out_2473779081939139397[0] = 0; - out_2473779081939139397[1] = 0; - out_2473779081939139397[2] = 0; - out_2473779081939139397[3] = 0; - out_2473779081939139397[4] = 0; - out_2473779081939139397[5] = 0; - out_2473779081939139397[6] = 1; - out_2473779081939139397[7] = 0; - out_2473779081939139397[8] = 0; - out_2473779081939139397[9] = 0; - out_2473779081939139397[10] = 0; - out_2473779081939139397[11] = 0; - out_2473779081939139397[12] = 0; - out_2473779081939139397[13] = 0; - out_2473779081939139397[14] = 0; - out_2473779081939139397[15] = 0; - out_2473779081939139397[16] = 0; - out_2473779081939139397[17] = 0; - out_2473779081939139397[18] = 0; - out_2473779081939139397[19] = 0; - out_2473779081939139397[20] = 0; - out_2473779081939139397[21] = 0; - out_2473779081939139397[22] = 0; - out_2473779081939139397[23] = 0; - out_2473779081939139397[24] = 0; - out_2473779081939139397[25] = 1; - out_2473779081939139397[26] = 0; - out_2473779081939139397[27] = 0; - out_2473779081939139397[28] = 0; - out_2473779081939139397[29] = 0; - out_2473779081939139397[30] = 0; - out_2473779081939139397[31] = 0; - out_2473779081939139397[32] = 0; - out_2473779081939139397[33] = 0; - out_2473779081939139397[34] = 0; - out_2473779081939139397[35] = 0; - out_2473779081939139397[36] = 0; - out_2473779081939139397[37] = 0; - out_2473779081939139397[38] = 0; - out_2473779081939139397[39] = 0; - out_2473779081939139397[40] = 0; - out_2473779081939139397[41] = 0; - out_2473779081939139397[42] = 0; - out_2473779081939139397[43] = 0; - out_2473779081939139397[44] = 1; - out_2473779081939139397[45] = 0; - out_2473779081939139397[46] = 0; - out_2473779081939139397[47] = 0; - out_2473779081939139397[48] = 0; - out_2473779081939139397[49] = 0; - out_2473779081939139397[50] = 0; - out_2473779081939139397[51] = 0; - out_2473779081939139397[52] = 0; - out_2473779081939139397[53] = 0; +void H_14(double *state, double *unused, double *out_3169758251754665716) { + out_3169758251754665716[0] = 0; + out_3169758251754665716[1] = 0; + out_3169758251754665716[2] = 0; + out_3169758251754665716[3] = 0; + out_3169758251754665716[4] = 0; + out_3169758251754665716[5] = 0; + out_3169758251754665716[6] = 1; + out_3169758251754665716[7] = 0; + out_3169758251754665716[8] = 0; + out_3169758251754665716[9] = 0; + out_3169758251754665716[10] = 0; + out_3169758251754665716[11] = 0; + out_3169758251754665716[12] = 0; + out_3169758251754665716[13] = 0; + out_3169758251754665716[14] = 0; + out_3169758251754665716[15] = 0; + out_3169758251754665716[16] = 0; + out_3169758251754665716[17] = 0; + out_3169758251754665716[18] = 0; + out_3169758251754665716[19] = 0; + out_3169758251754665716[20] = 0; + out_3169758251754665716[21] = 0; + out_3169758251754665716[22] = 0; + out_3169758251754665716[23] = 0; + out_3169758251754665716[24] = 0; + out_3169758251754665716[25] = 1; + out_3169758251754665716[26] = 0; + out_3169758251754665716[27] = 0; + out_3169758251754665716[28] = 0; + out_3169758251754665716[29] = 0; + out_3169758251754665716[30] = 0; + out_3169758251754665716[31] = 0; + out_3169758251754665716[32] = 0; + out_3169758251754665716[33] = 0; + out_3169758251754665716[34] = 0; + out_3169758251754665716[35] = 0; + out_3169758251754665716[36] = 0; + out_3169758251754665716[37] = 0; + out_3169758251754665716[38] = 0; + out_3169758251754665716[39] = 0; + out_3169758251754665716[40] = 0; + out_3169758251754665716[41] = 0; + out_3169758251754665716[42] = 0; + out_3169758251754665716[43] = 0; + out_3169758251754665716[44] = 1; + out_3169758251754665716[45] = 0; + out_3169758251754665716[46] = 0; + out_3169758251754665716[47] = 0; + out_3169758251754665716[48] = 0; + out_3169758251754665716[49] = 0; + out_3169758251754665716[50] = 0; + out_3169758251754665716[51] = 0; + out_3169758251754665716[52] = 0; + out_3169758251754665716[53] = 0; } #include #include @@ -1113,44 +1113,44 @@ void pose_update_13(double *in_x, double *in_P, double *in_z, double *in_R, doub void pose_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { update<3, 3, 0>(in_x, in_P, h_14, H_14, NULL, in_z, in_R, in_ea, MAHA_THRESH_14); } -void pose_err_fun(double *nom_x, double *delta_x, double *out_1227579335886564773) { - err_fun(nom_x, delta_x, out_1227579335886564773); +void pose_err_fun(double *nom_x, double *delta_x, double *out_7165967535860238129) { + err_fun(nom_x, delta_x, out_7165967535860238129); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_7708296302708828001) { - inv_err_fun(nom_x, true_x, out_7708296302708828001); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_9032729807193459598) { + inv_err_fun(nom_x, true_x, out_9032729807193459598); } -void pose_H_mod_fun(double *state, double *out_145151651698479708) { - H_mod_fun(state, out_145151651698479708); +void pose_H_mod_fun(double *state, double *out_5054599202167139908) { + H_mod_fun(state, out_5054599202167139908); } -void pose_f_fun(double *state, double dt, double *out_6753877240357292479) { - f_fun(state, dt, out_6753877240357292479); +void pose_f_fun(double *state, double dt, double *out_5000035888964044284) { + f_fun(state, dt, out_5000035888964044284); } -void pose_F_fun(double *state, double dt, double *out_48451545294533288) { - F_fun(state, dt, out_48451545294533288); +void pose_F_fun(double *state, double dt, double *out_9018422947626896264) { + F_fun(state, dt, out_9018422947626896264); } -void pose_h_4(double *state, double *unused, double *out_711207782401779392) { - h_4(state, unused, out_711207782401779392); +void pose_h_4(double *state, double *unused, double *out_6773718258743003346) { + h_4(state, unused, out_6773718258743003346); } -void pose_H_4(double *state, double *unused, double *out_6437019938278623926) { - H_4(state, unused, out_6437019938278623926); +void pose_H_4(double *state, double *unused, double *out_793482604584818813) { + H_4(state, unused, out_793482604584818813); } -void pose_h_10(double *state, double *unused, double *out_2942660564170460979) { - h_10(state, unused, out_2942660564170460979); +void pose_h_10(double *state, double *unused, double *out_3870110240018051215) { + h_10(state, unused, out_3870110240018051215); } -void pose_H_10(double *state, double *unused, double *out_7753365329259358525) { - H_10(state, unused, out_7753365329259358525); +void pose_H_10(double *state, double *unused, double *out_3089556940454179292) { + H_10(state, unused, out_3089556940454179292); } -void pose_h_13(double *state, double *unused, double *out_2914132243760424113) { - h_13(state, unused, out_2914132243760424113); +void pose_h_13(double *state, double *unused, double *out_7087737251706748874) { + h_13(state, unused, out_7087737251706748874); } -void pose_H_13(double *state, double *unused, double *out_3224746112946291125) { - H_13(state, unused, out_3224746112946291125); +void pose_H_13(double *state, double *unused, double *out_2418791220747513988) { + H_13(state, unused, out_2418791220747513988); } -void pose_h_14(double *state, double *unused, double *out_677306400367602538) { - h_14(state, unused, out_677306400367602538); +void pose_h_14(double *state, double *unused, double *out_8542055892000654086) { + h_14(state, unused, out_8542055892000654086); } -void pose_H_14(double *state, double *unused, double *out_2473779081939139397) { - H_14(state, unused, out_2473779081939139397); +void pose_H_14(double *state, double *unused, double *out_3169758251754665716) { + H_14(state, unused, out_3169758251754665716); } void pose_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/pose.h b/selfdrive/locationd/models/generated/pose.h index 54b20c0ce6..22d6eaac4b 100644 --- a/selfdrive/locationd/models/generated/pose.h +++ b/selfdrive/locationd/models/generated/pose.h @@ -5,18 +5,18 @@ void pose_update_4(double *in_x, double *in_P, double *in_z, double *in_R, doubl void pose_update_10(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); void pose_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); void pose_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); -void pose_err_fun(double *nom_x, double *delta_x, double *out_1227579335886564773); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_7708296302708828001); -void pose_H_mod_fun(double *state, double *out_145151651698479708); -void pose_f_fun(double *state, double dt, double *out_6753877240357292479); -void pose_F_fun(double *state, double dt, double *out_48451545294533288); -void pose_h_4(double *state, double *unused, double *out_711207782401779392); -void pose_H_4(double *state, double *unused, double *out_6437019938278623926); -void pose_h_10(double *state, double *unused, double *out_2942660564170460979); -void pose_H_10(double *state, double *unused, double *out_7753365329259358525); -void pose_h_13(double *state, double *unused, double *out_2914132243760424113); -void pose_H_13(double *state, double *unused, double *out_3224746112946291125); -void pose_h_14(double *state, double *unused, double *out_677306400367602538); -void pose_H_14(double *state, double *unused, double *out_2473779081939139397); +void pose_err_fun(double *nom_x, double *delta_x, double *out_7165967535860238129); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_9032729807193459598); +void pose_H_mod_fun(double *state, double *out_5054599202167139908); +void pose_f_fun(double *state, double dt, double *out_5000035888964044284); +void pose_F_fun(double *state, double dt, double *out_9018422947626896264); +void pose_h_4(double *state, double *unused, double *out_6773718258743003346); +void pose_H_4(double *state, double *unused, double *out_793482604584818813); +void pose_h_10(double *state, double *unused, double *out_3870110240018051215); +void pose_H_10(double *state, double *unused, double *out_3089556940454179292); +void pose_h_13(double *state, double *unused, double *out_7087737251706748874); +void pose_H_13(double *state, double *unused, double *out_2418791220747513988); +void pose_h_14(double *state, double *unused, double *out_8542055892000654086); +void pose_H_14(double *state, double *unused, double *out_3169758251754665716); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/pandad/pandad b/selfdrive/pandad/pandad index 6fc41f0af4..1b177aef04 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/system/camerad/camerad b/system/camerad/camerad index d32f2edf4f..582536b8fc 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index 24b6b3df58..af0a66fa4e 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index 3f9d5cdf22..d59851f0a9 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index ef838d5e28..208b1e31e5 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ