diff --git a/common/libcommon.a b/common/libcommon.a index 8a947bc151..7fe60755ef 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 2a4ef1e1b1..81c4a8fef8 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 10429c9e28..c9025a1ac8 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 0314fbaddf..44775f8e5e 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 182b7f0c45..de13e50896 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 c68bee5651..0f1cc09650 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 5952d24e7b..afbb127daf 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 230de4f868..c76b438ee3 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 230de4f868..c76b438ee3 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 463bb6710a..ed656ffadb 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 463bb6710a..ed656ffadb 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 463bb6710a..ed656ffadb 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 463bb6710a..ed656ffadb 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 463bb6710a..ed656ffadb 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 463bb6710a..ed656ffadb 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 463bb6710a..ed656ffadb 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 463bb6710a..ed656ffadb 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 230de4f868..c76b438ee3 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 230de4f868..c76b438ee3 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 b32960f1d6..71ccfb275c 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 230de4f868..c76b438ee3 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 230de4f868..c76b438ee3 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 230de4f868..c76b438ee3 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 230de4f868..c76b438ee3 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 059d541f63..5a39d4173f 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-96ef704d-DEBUG"; +const uint8_t gitversion[19] = "DEV-20ff7931-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 2afdbb0d58..a24d18fd01 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 a523633668..8adb8bb443 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 f71e1e0d0b..ba40a23fef 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 29097b6c18..ffb2eca4ee 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 00171271f9..46f13b0db3 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 6b2c9bb95c..ffd2929ca1 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 112f44769e..f97a7cc681 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 a77e55da4e..fba8100ced 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 fae4924e64..0be86812ce 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 66a25681bf..90295cd004 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 fc2c6d1c0b..7dbc75f329 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 bd73ff2d69..116e35e61f 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 7f3e96f7e9..7e060d45ff 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 526c8061a5..e330e35e95 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 59d11871fd..583ce2a8fb 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 737253adbe..b27b8091c9 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 9765a14e10..6dde544d8a 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 a010f1053c..c101741a4e 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 c1b12d7d37..b1585166d4 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 cb44a2d7c5..569c9a9eab 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 faf41279cd..b731618037 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 7a78b09159..28a2a7a24f 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 470c8bb7e6..f452011aa4 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 b1d32ac32c..4cdff29d93 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 89be00c650..639ea08241 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 335ea68c8b..48605c29ee 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 c22682a948..979179d221 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 274506c3f3..b6f680c38c 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 5922d4d662..3ad07d601c 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 a9ab56b22b..c941383b64 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 ce5338ac27..8be3d7bc76 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 a12ab912cf..113c212d54 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 cb4055a4a5..a07e429f30 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 062572e7be..8cbb1db54e 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 f0aaecf4b8..a1ed0375bf 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 3877466451..5eb8432567 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 e15ce05c15..ecbda34eb2 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 e55408be6c..454cfe7d11 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 0b4c885e16..08630c0271 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 58086ddd18..f41af7ecab 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 4d3282c173..af150af836 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 fe8b4b98e9..ccd8d211aa 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 ea0916fc8f..95eeb89756 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 f8b068bb82..82481cb44a 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 b266a1c307..7db7a84468 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 b118550e5f..10480a9748 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 47d2ae9995..8cb543ead0 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 4f2e228c37..321f605ab8 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 28a0ff1b20..5ad1b97969 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 20759999a2..b2f2edb8bb 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 7dc1475b99..d9430a00c0 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 bf84645c9c..2f1d9e4495 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 84393cb62d..ae50a94070 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 36f8204e7f..2501c9dded 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 778c045909..8fba578d33 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 23771cf93b..4cb49fd25c 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 8bc8b18312..916291e4e8 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 03f345958d..e70c9ec262 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 39fd177456..468369067e 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 464bf34603..c9e2bdb4cd 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 37ecbc5462..231dd92a19 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 8b8f4537d8..bd302694a8 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 c25c2e80ca..8bedec6097 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 cf2c43f027..a1e3e24c2b 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 793c22c49d..dedbf13511 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 8323b0a0d0..860b7521ba 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 064f2cf96b..97b122a46e 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 e54e6e8f8a..a822569474 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 9ca48e8900..5842890482 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-96ef704d-DEBUG \ No newline at end of file +DEV-20ff7931-DEBUG \ No newline at end of file diff --git a/rednose_repo/rednose/helpers/ekf_sym_pyx.so b/rednose_repo/rednose/helpers/ekf_sym_pyx.so index 985b6c7854..0c2e6104d9 100755 Binary files a/rednose_repo/rednose/helpers/ekf_sym_pyx.so and b/rednose_repo/rednose/helpers/ekf_sym_pyx.so differ diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 1f04a764b6..b90c283f79 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_870685266903427678) { - out_870685266903427678[0] = delta_x[0] + nom_x[0]; - out_870685266903427678[1] = delta_x[1] + nom_x[1]; - out_870685266903427678[2] = delta_x[2] + nom_x[2]; - out_870685266903427678[3] = delta_x[3] + nom_x[3]; - out_870685266903427678[4] = delta_x[4] + nom_x[4]; - out_870685266903427678[5] = delta_x[5] + nom_x[5]; - out_870685266903427678[6] = delta_x[6] + nom_x[6]; - out_870685266903427678[7] = delta_x[7] + nom_x[7]; - out_870685266903427678[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_1527400592183565183) { + out_1527400592183565183[0] = delta_x[0] + nom_x[0]; + out_1527400592183565183[1] = delta_x[1] + nom_x[1]; + out_1527400592183565183[2] = delta_x[2] + nom_x[2]; + out_1527400592183565183[3] = delta_x[3] + nom_x[3]; + out_1527400592183565183[4] = delta_x[4] + nom_x[4]; + out_1527400592183565183[5] = delta_x[5] + nom_x[5]; + out_1527400592183565183[6] = delta_x[6] + nom_x[6]; + out_1527400592183565183[7] = delta_x[7] + nom_x[7]; + out_1527400592183565183[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_8535551015639214025) { - out_8535551015639214025[0] = -nom_x[0] + true_x[0]; - out_8535551015639214025[1] = -nom_x[1] + true_x[1]; - out_8535551015639214025[2] = -nom_x[2] + true_x[2]; - out_8535551015639214025[3] = -nom_x[3] + true_x[3]; - out_8535551015639214025[4] = -nom_x[4] + true_x[4]; - out_8535551015639214025[5] = -nom_x[5] + true_x[5]; - out_8535551015639214025[6] = -nom_x[6] + true_x[6]; - out_8535551015639214025[7] = -nom_x[7] + true_x[7]; - out_8535551015639214025[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_4038333929682939877) { + out_4038333929682939877[0] = -nom_x[0] + true_x[0]; + out_4038333929682939877[1] = -nom_x[1] + true_x[1]; + out_4038333929682939877[2] = -nom_x[2] + true_x[2]; + out_4038333929682939877[3] = -nom_x[3] + true_x[3]; + out_4038333929682939877[4] = -nom_x[4] + true_x[4]; + out_4038333929682939877[5] = -nom_x[5] + true_x[5]; + out_4038333929682939877[6] = -nom_x[6] + true_x[6]; + out_4038333929682939877[7] = -nom_x[7] + true_x[7]; + out_4038333929682939877[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_5722367516210042485) { - out_5722367516210042485[0] = 1.0; - out_5722367516210042485[1] = 0.0; - out_5722367516210042485[2] = 0.0; - out_5722367516210042485[3] = 0.0; - out_5722367516210042485[4] = 0.0; - out_5722367516210042485[5] = 0.0; - out_5722367516210042485[6] = 0.0; - out_5722367516210042485[7] = 0.0; - out_5722367516210042485[8] = 0.0; - out_5722367516210042485[9] = 0.0; - out_5722367516210042485[10] = 1.0; - out_5722367516210042485[11] = 0.0; - out_5722367516210042485[12] = 0.0; - out_5722367516210042485[13] = 0.0; - out_5722367516210042485[14] = 0.0; - out_5722367516210042485[15] = 0.0; - out_5722367516210042485[16] = 0.0; - out_5722367516210042485[17] = 0.0; - out_5722367516210042485[18] = 0.0; - out_5722367516210042485[19] = 0.0; - out_5722367516210042485[20] = 1.0; - out_5722367516210042485[21] = 0.0; - out_5722367516210042485[22] = 0.0; - out_5722367516210042485[23] = 0.0; - out_5722367516210042485[24] = 0.0; - out_5722367516210042485[25] = 0.0; - out_5722367516210042485[26] = 0.0; - out_5722367516210042485[27] = 0.0; - out_5722367516210042485[28] = 0.0; - out_5722367516210042485[29] = 0.0; - out_5722367516210042485[30] = 1.0; - out_5722367516210042485[31] = 0.0; - out_5722367516210042485[32] = 0.0; - out_5722367516210042485[33] = 0.0; - out_5722367516210042485[34] = 0.0; - out_5722367516210042485[35] = 0.0; - out_5722367516210042485[36] = 0.0; - out_5722367516210042485[37] = 0.0; - out_5722367516210042485[38] = 0.0; - out_5722367516210042485[39] = 0.0; - out_5722367516210042485[40] = 1.0; - out_5722367516210042485[41] = 0.0; - out_5722367516210042485[42] = 0.0; - out_5722367516210042485[43] = 0.0; - out_5722367516210042485[44] = 0.0; - out_5722367516210042485[45] = 0.0; - out_5722367516210042485[46] = 0.0; - out_5722367516210042485[47] = 0.0; - out_5722367516210042485[48] = 0.0; - out_5722367516210042485[49] = 0.0; - out_5722367516210042485[50] = 1.0; - out_5722367516210042485[51] = 0.0; - out_5722367516210042485[52] = 0.0; - out_5722367516210042485[53] = 0.0; - out_5722367516210042485[54] = 0.0; - out_5722367516210042485[55] = 0.0; - out_5722367516210042485[56] = 0.0; - out_5722367516210042485[57] = 0.0; - out_5722367516210042485[58] = 0.0; - out_5722367516210042485[59] = 0.0; - out_5722367516210042485[60] = 1.0; - out_5722367516210042485[61] = 0.0; - out_5722367516210042485[62] = 0.0; - out_5722367516210042485[63] = 0.0; - out_5722367516210042485[64] = 0.0; - out_5722367516210042485[65] = 0.0; - out_5722367516210042485[66] = 0.0; - out_5722367516210042485[67] = 0.0; - out_5722367516210042485[68] = 0.0; - out_5722367516210042485[69] = 0.0; - out_5722367516210042485[70] = 1.0; - out_5722367516210042485[71] = 0.0; - out_5722367516210042485[72] = 0.0; - out_5722367516210042485[73] = 0.0; - out_5722367516210042485[74] = 0.0; - out_5722367516210042485[75] = 0.0; - out_5722367516210042485[76] = 0.0; - out_5722367516210042485[77] = 0.0; - out_5722367516210042485[78] = 0.0; - out_5722367516210042485[79] = 0.0; - out_5722367516210042485[80] = 1.0; +void H_mod_fun(double *state, double *out_8049619905800951591) { + out_8049619905800951591[0] = 1.0; + out_8049619905800951591[1] = 0.0; + out_8049619905800951591[2] = 0.0; + out_8049619905800951591[3] = 0.0; + out_8049619905800951591[4] = 0.0; + out_8049619905800951591[5] = 0.0; + out_8049619905800951591[6] = 0.0; + out_8049619905800951591[7] = 0.0; + out_8049619905800951591[8] = 0.0; + out_8049619905800951591[9] = 0.0; + out_8049619905800951591[10] = 1.0; + out_8049619905800951591[11] = 0.0; + out_8049619905800951591[12] = 0.0; + out_8049619905800951591[13] = 0.0; + out_8049619905800951591[14] = 0.0; + out_8049619905800951591[15] = 0.0; + out_8049619905800951591[16] = 0.0; + out_8049619905800951591[17] = 0.0; + out_8049619905800951591[18] = 0.0; + out_8049619905800951591[19] = 0.0; + out_8049619905800951591[20] = 1.0; + out_8049619905800951591[21] = 0.0; + out_8049619905800951591[22] = 0.0; + out_8049619905800951591[23] = 0.0; + out_8049619905800951591[24] = 0.0; + out_8049619905800951591[25] = 0.0; + out_8049619905800951591[26] = 0.0; + out_8049619905800951591[27] = 0.0; + out_8049619905800951591[28] = 0.0; + out_8049619905800951591[29] = 0.0; + out_8049619905800951591[30] = 1.0; + out_8049619905800951591[31] = 0.0; + out_8049619905800951591[32] = 0.0; + out_8049619905800951591[33] = 0.0; + out_8049619905800951591[34] = 0.0; + out_8049619905800951591[35] = 0.0; + out_8049619905800951591[36] = 0.0; + out_8049619905800951591[37] = 0.0; + out_8049619905800951591[38] = 0.0; + out_8049619905800951591[39] = 0.0; + out_8049619905800951591[40] = 1.0; + out_8049619905800951591[41] = 0.0; + out_8049619905800951591[42] = 0.0; + out_8049619905800951591[43] = 0.0; + out_8049619905800951591[44] = 0.0; + out_8049619905800951591[45] = 0.0; + out_8049619905800951591[46] = 0.0; + out_8049619905800951591[47] = 0.0; + out_8049619905800951591[48] = 0.0; + out_8049619905800951591[49] = 0.0; + out_8049619905800951591[50] = 1.0; + out_8049619905800951591[51] = 0.0; + out_8049619905800951591[52] = 0.0; + out_8049619905800951591[53] = 0.0; + out_8049619905800951591[54] = 0.0; + out_8049619905800951591[55] = 0.0; + out_8049619905800951591[56] = 0.0; + out_8049619905800951591[57] = 0.0; + out_8049619905800951591[58] = 0.0; + out_8049619905800951591[59] = 0.0; + out_8049619905800951591[60] = 1.0; + out_8049619905800951591[61] = 0.0; + out_8049619905800951591[62] = 0.0; + out_8049619905800951591[63] = 0.0; + out_8049619905800951591[64] = 0.0; + out_8049619905800951591[65] = 0.0; + out_8049619905800951591[66] = 0.0; + out_8049619905800951591[67] = 0.0; + out_8049619905800951591[68] = 0.0; + out_8049619905800951591[69] = 0.0; + out_8049619905800951591[70] = 1.0; + out_8049619905800951591[71] = 0.0; + out_8049619905800951591[72] = 0.0; + out_8049619905800951591[73] = 0.0; + out_8049619905800951591[74] = 0.0; + out_8049619905800951591[75] = 0.0; + out_8049619905800951591[76] = 0.0; + out_8049619905800951591[77] = 0.0; + out_8049619905800951591[78] = 0.0; + out_8049619905800951591[79] = 0.0; + out_8049619905800951591[80] = 1.0; } -void f_fun(double *state, double dt, double *out_6327711019370239094) { - out_6327711019370239094[0] = state[0]; - out_6327711019370239094[1] = state[1]; - out_6327711019370239094[2] = state[2]; - out_6327711019370239094[3] = state[3]; - out_6327711019370239094[4] = state[4]; - out_6327711019370239094[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_6327711019370239094[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_6327711019370239094[7] = state[7]; - out_6327711019370239094[8] = state[8]; +void f_fun(double *state, double dt, double *out_7680940422259587756) { + out_7680940422259587756[0] = state[0]; + out_7680940422259587756[1] = state[1]; + out_7680940422259587756[2] = state[2]; + out_7680940422259587756[3] = state[3]; + out_7680940422259587756[4] = state[4]; + out_7680940422259587756[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_7680940422259587756[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_7680940422259587756[7] = state[7]; + out_7680940422259587756[8] = state[8]; } -void F_fun(double *state, double dt, double *out_334732449615445707) { - out_334732449615445707[0] = 1; - out_334732449615445707[1] = 0; - out_334732449615445707[2] = 0; - out_334732449615445707[3] = 0; - out_334732449615445707[4] = 0; - out_334732449615445707[5] = 0; - out_334732449615445707[6] = 0; - out_334732449615445707[7] = 0; - out_334732449615445707[8] = 0; - out_334732449615445707[9] = 0; - out_334732449615445707[10] = 1; - out_334732449615445707[11] = 0; - out_334732449615445707[12] = 0; - out_334732449615445707[13] = 0; - out_334732449615445707[14] = 0; - out_334732449615445707[15] = 0; - out_334732449615445707[16] = 0; - out_334732449615445707[17] = 0; - out_334732449615445707[18] = 0; - out_334732449615445707[19] = 0; - out_334732449615445707[20] = 1; - out_334732449615445707[21] = 0; - out_334732449615445707[22] = 0; - out_334732449615445707[23] = 0; - out_334732449615445707[24] = 0; - out_334732449615445707[25] = 0; - out_334732449615445707[26] = 0; - out_334732449615445707[27] = 0; - out_334732449615445707[28] = 0; - out_334732449615445707[29] = 0; - out_334732449615445707[30] = 1; - out_334732449615445707[31] = 0; - out_334732449615445707[32] = 0; - out_334732449615445707[33] = 0; - out_334732449615445707[34] = 0; - out_334732449615445707[35] = 0; - out_334732449615445707[36] = 0; - out_334732449615445707[37] = 0; - out_334732449615445707[38] = 0; - out_334732449615445707[39] = 0; - out_334732449615445707[40] = 1; - out_334732449615445707[41] = 0; - out_334732449615445707[42] = 0; - out_334732449615445707[43] = 0; - out_334732449615445707[44] = 0; - out_334732449615445707[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_334732449615445707[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_334732449615445707[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_334732449615445707[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_334732449615445707[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_334732449615445707[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_334732449615445707[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_334732449615445707[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_334732449615445707[53] = -9.8100000000000005*dt; - out_334732449615445707[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_334732449615445707[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_334732449615445707[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_334732449615445707[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_334732449615445707[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_334732449615445707[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_334732449615445707[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_334732449615445707[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_334732449615445707[62] = 0; - out_334732449615445707[63] = 0; - out_334732449615445707[64] = 0; - out_334732449615445707[65] = 0; - out_334732449615445707[66] = 0; - out_334732449615445707[67] = 0; - out_334732449615445707[68] = 0; - out_334732449615445707[69] = 0; - out_334732449615445707[70] = 1; - out_334732449615445707[71] = 0; - out_334732449615445707[72] = 0; - out_334732449615445707[73] = 0; - out_334732449615445707[74] = 0; - out_334732449615445707[75] = 0; - out_334732449615445707[76] = 0; - out_334732449615445707[77] = 0; - out_334732449615445707[78] = 0; - out_334732449615445707[79] = 0; - out_334732449615445707[80] = 1; +void F_fun(double *state, double dt, double *out_8373137223162851881) { + out_8373137223162851881[0] = 1; + out_8373137223162851881[1] = 0; + out_8373137223162851881[2] = 0; + out_8373137223162851881[3] = 0; + out_8373137223162851881[4] = 0; + out_8373137223162851881[5] = 0; + out_8373137223162851881[6] = 0; + out_8373137223162851881[7] = 0; + out_8373137223162851881[8] = 0; + out_8373137223162851881[9] = 0; + out_8373137223162851881[10] = 1; + out_8373137223162851881[11] = 0; + out_8373137223162851881[12] = 0; + out_8373137223162851881[13] = 0; + out_8373137223162851881[14] = 0; + out_8373137223162851881[15] = 0; + out_8373137223162851881[16] = 0; + out_8373137223162851881[17] = 0; + out_8373137223162851881[18] = 0; + out_8373137223162851881[19] = 0; + out_8373137223162851881[20] = 1; + out_8373137223162851881[21] = 0; + out_8373137223162851881[22] = 0; + out_8373137223162851881[23] = 0; + out_8373137223162851881[24] = 0; + out_8373137223162851881[25] = 0; + out_8373137223162851881[26] = 0; + out_8373137223162851881[27] = 0; + out_8373137223162851881[28] = 0; + out_8373137223162851881[29] = 0; + out_8373137223162851881[30] = 1; + out_8373137223162851881[31] = 0; + out_8373137223162851881[32] = 0; + out_8373137223162851881[33] = 0; + out_8373137223162851881[34] = 0; + out_8373137223162851881[35] = 0; + out_8373137223162851881[36] = 0; + out_8373137223162851881[37] = 0; + out_8373137223162851881[38] = 0; + out_8373137223162851881[39] = 0; + out_8373137223162851881[40] = 1; + out_8373137223162851881[41] = 0; + out_8373137223162851881[42] = 0; + out_8373137223162851881[43] = 0; + out_8373137223162851881[44] = 0; + out_8373137223162851881[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_8373137223162851881[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_8373137223162851881[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_8373137223162851881[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_8373137223162851881[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_8373137223162851881[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_8373137223162851881[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_8373137223162851881[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_8373137223162851881[53] = -9.8100000000000005*dt; + out_8373137223162851881[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_8373137223162851881[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_8373137223162851881[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_8373137223162851881[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_8373137223162851881[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_8373137223162851881[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_8373137223162851881[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_8373137223162851881[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_8373137223162851881[62] = 0; + out_8373137223162851881[63] = 0; + out_8373137223162851881[64] = 0; + out_8373137223162851881[65] = 0; + out_8373137223162851881[66] = 0; + out_8373137223162851881[67] = 0; + out_8373137223162851881[68] = 0; + out_8373137223162851881[69] = 0; + out_8373137223162851881[70] = 1; + out_8373137223162851881[71] = 0; + out_8373137223162851881[72] = 0; + out_8373137223162851881[73] = 0; + out_8373137223162851881[74] = 0; + out_8373137223162851881[75] = 0; + out_8373137223162851881[76] = 0; + out_8373137223162851881[77] = 0; + out_8373137223162851881[78] = 0; + out_8373137223162851881[79] = 0; + out_8373137223162851881[80] = 1; } -void h_25(double *state, double *unused, double *out_4988354507217303652) { - out_4988354507217303652[0] = state[6]; +void h_25(double *state, double *unused, double *out_5355327156585439901) { + out_5355327156585439901[0] = state[6]; } -void H_25(double *state, double *unused, double *out_3470820709702194752) { - out_3470820709702194752[0] = 0; - out_3470820709702194752[1] = 0; - out_3470820709702194752[2] = 0; - out_3470820709702194752[3] = 0; - out_3470820709702194752[4] = 0; - out_3470820709702194752[5] = 0; - out_3470820709702194752[6] = 1; - out_3470820709702194752[7] = 0; - out_3470820709702194752[8] = 0; +void H_25(double *state, double *unused, double *out_2475786134934740220) { + out_2475786134934740220[0] = 0; + out_2475786134934740220[1] = 0; + out_2475786134934740220[2] = 0; + out_2475786134934740220[3] = 0; + out_2475786134934740220[4] = 0; + out_2475786134934740220[5] = 0; + out_2475786134934740220[6] = 1; + out_2475786134934740220[7] = 0; + out_2475786134934740220[8] = 0; } -void h_24(double *state, double *unused, double *out_8320591788748654668) { - out_8320591788748654668[0] = state[4]; - out_8320591788748654668[1] = state[5]; +void h_24(double *state, double *unused, double *out_4948890380757191940) { + out_4948890380757191940[0] = state[4]; + out_4948890380757191940[1] = state[5]; } -void H_24(double *state, double *unused, double *out_5643470308707694318) { - out_5643470308707694318[0] = 0; - out_5643470308707694318[1] = 0; - out_5643470308707694318[2] = 0; - out_5643470308707694318[3] = 0; - out_5643470308707694318[4] = 1; - out_5643470308707694318[5] = 0; - out_5643470308707694318[6] = 0; - out_5643470308707694318[7] = 0; - out_5643470308707694318[8] = 0; - out_5643470308707694318[9] = 0; - out_5643470308707694318[10] = 0; - out_5643470308707694318[11] = 0; - out_5643470308707694318[12] = 0; - out_5643470308707694318[13] = 0; - out_5643470308707694318[14] = 1; - out_5643470308707694318[15] = 0; - out_5643470308707694318[16] = 0; - out_5643470308707694318[17] = 0; +void H_24(double *state, double *unused, double *out_3316217919116785212) { + out_3316217919116785212[0] = 0; + out_3316217919116785212[1] = 0; + out_3316217919116785212[2] = 0; + out_3316217919116785212[3] = 0; + out_3316217919116785212[4] = 1; + out_3316217919116785212[5] = 0; + out_3316217919116785212[6] = 0; + out_3316217919116785212[7] = 0; + out_3316217919116785212[8] = 0; + out_3316217919116785212[9] = 0; + out_3316217919116785212[10] = 0; + out_3316217919116785212[11] = 0; + out_3316217919116785212[12] = 0; + out_3316217919116785212[13] = 0; + out_3316217919116785212[14] = 1; + out_3316217919116785212[15] = 0; + out_3316217919116785212[16] = 0; + out_3316217919116785212[17] = 0; } -void h_30(double *state, double *unused, double *out_6271436662034189635) { - out_6271436662034189635[0] = state[4]; +void h_30(double *state, double *unused, double *out_6805958549214323068) { + out_6805958549214323068[0] = state[4]; } -void H_30(double *state, double *unused, double *out_952487751194946125) { - out_952487751194946125[0] = 0; - out_952487751194946125[1] = 0; - out_952487751194946125[2] = 0; - out_952487751194946125[3] = 0; - out_952487751194946125[4] = 1; - out_952487751194946125[5] = 0; - out_952487751194946125[6] = 0; - out_952487751194946125[7] = 0; - out_952487751194946125[8] = 0; +void H_30(double *state, double *unused, double *out_42546823572508407) { + out_42546823572508407[0] = 0; + out_42546823572508407[1] = 0; + out_42546823572508407[2] = 0; + out_42546823572508407[3] = 0; + out_42546823572508407[4] = 1; + out_42546823572508407[5] = 0; + out_42546823572508407[6] = 0; + out_42546823572508407[7] = 0; + out_42546823572508407[8] = 0; } -void h_26(double *state, double *unused, double *out_628706790953232800) { - out_628706790953232800[0] = state[7]; +void h_26(double *state, double *unused, double *out_2475727773492236361) { + out_2475727773492236361[0] = state[7]; } -void H_26(double *state, double *unused, double *out_7212324028576250976) { - out_7212324028576250976[0] = 0; - out_7212324028576250976[1] = 0; - out_7212324028576250976[2] = 0; - out_7212324028576250976[3] = 0; - out_7212324028576250976[4] = 0; - out_7212324028576250976[5] = 0; - out_7212324028576250976[6] = 0; - out_7212324028576250976[7] = 1; - out_7212324028576250976[8] = 0; +void H_26(double *state, double *unused, double *out_6217289453808796444) { + out_6217289453808796444[0] = 0; + out_6217289453808796444[1] = 0; + out_6217289453808796444[2] = 0; + out_6217289453808796444[3] = 0; + out_6217289453808796444[4] = 0; + out_6217289453808796444[5] = 0; + out_6217289453808796444[6] = 0; + out_6217289453808796444[7] = 1; + out_6217289453808796444[8] = 0; } -void h_27(double *state, double *unused, double *out_7592759828026001303) { - out_7592759828026001303[0] = state[3]; +void h_27(double *state, double *unused, double *out_353653888924594256) { + out_353653888924594256[0] = state[3]; } -void H_27(double *state, double *unused, double *out_5774922968645859733) { - out_5774922968645859733[0] = 0; - out_5774922968645859733[1] = 0; - out_5774922968645859733[2] = 0; - out_5774922968645859733[3] = 1; - out_5774922968645859733[4] = 0; - out_5774922968645859733[5] = 0; - out_5774922968645859733[6] = 0; - out_5774922968645859733[7] = 0; - out_5774922968645859733[8] = 0; +void H_27(double *state, double *unused, double *out_9178245776862773329) { + out_9178245776862773329[0] = 0; + out_9178245776862773329[1] = 0; + out_9178245776862773329[2] = 0; + out_9178245776862773329[3] = 1; + out_9178245776862773329[4] = 0; + out_9178245776862773329[5] = 0; + out_9178245776862773329[6] = 0; + out_9178245776862773329[7] = 0; + out_9178245776862773329[8] = 0; } -void h_29(double *state, double *unused, double *out_7749946496377130448) { - out_7749946496377130448[0] = state[1]; +void h_29(double *state, double *unused, double *out_1804285281553477423) { + out_1804285281553477423[0] = state[1]; } -void H_29(double *state, double *unused, double *out_7488285695515410766) { - out_7488285695515410766[0] = 0; - out_7488285695515410766[1] = 1; - out_7488285695515410766[2] = 0; - out_7488285695515410766[3] = 0; - out_7488285695515410766[4] = 0; - out_7488285695515410766[5] = 0; - out_7488285695515410766[6] = 0; - out_7488285695515410766[7] = 0; - out_7488285695515410766[8] = 0; +void H_29(double *state, double *unused, double *out_6493251120747956234) { + out_6493251120747956234[0] = 0; + out_6493251120747956234[1] = 1; + out_6493251120747956234[2] = 0; + out_6493251120747956234[3] = 0; + out_6493251120747956234[4] = 0; + out_6493251120747956234[5] = 0; + out_6493251120747956234[6] = 0; + out_6493251120747956234[7] = 0; + out_6493251120747956234[8] = 0; } -void h_28(double *state, double *unused, double *out_1346308770565827835) { - out_1346308770565827835[0] = state[0]; +void h_28(double *state, double *unused, double *out_4712763629793748467) { + out_4712763629793748467[0] = state[0]; } -void H_28(double *state, double *unused, double *out_5524655423950084515) { - out_5524655423950084515[0] = 1; - out_5524655423950084515[1] = 0; - out_5524655423950084515[2] = 0; - out_5524655423950084515[3] = 0; - out_5524655423950084515[4] = 0; - out_5524655423950084515[5] = 0; - out_5524655423950084515[6] = 0; - out_5524655423950084515[7] = 0; - out_5524655423950084515[8] = 0; +void H_28(double *state, double *unused, double *out_4529620849182629983) { + out_4529620849182629983[0] = 1; + out_4529620849182629983[1] = 0; + out_4529620849182629983[2] = 0; + out_4529620849182629983[3] = 0; + out_4529620849182629983[4] = 0; + out_4529620849182629983[5] = 0; + out_4529620849182629983[6] = 0; + out_4529620849182629983[7] = 0; + out_4529620849182629983[8] = 0; } -void h_31(double *state, double *unused, double *out_2214861449768682318) { - out_2214861449768682318[0] = state[8]; +void h_31(double *state, double *unused, double *out_5630521218869945790) { + out_5630521218869945790[0] = state[8]; } -void H_31(double *state, double *unused, double *out_7838532130809602452) { - out_7838532130809602452[0] = 0; - out_7838532130809602452[1] = 0; - out_7838532130809602452[2] = 0; - out_7838532130809602452[3] = 0; - out_7838532130809602452[4] = 0; - out_7838532130809602452[5] = 0; - out_7838532130809602452[6] = 0; - out_7838532130809602452[7] = 0; - out_7838532130809602452[8] = 1; +void H_31(double *state, double *unused, double *out_6843497556042147920) { + out_6843497556042147920[0] = 0; + out_6843497556042147920[1] = 0; + out_6843497556042147920[2] = 0; + out_6843497556042147920[3] = 0; + out_6843497556042147920[4] = 0; + out_6843497556042147920[5] = 0; + out_6843497556042147920[6] = 0; + out_6843497556042147920[7] = 0; + out_6843497556042147920[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_870685266903427678) { - err_fun(nom_x, delta_x, out_870685266903427678); +void car_err_fun(double *nom_x, double *delta_x, double *out_1527400592183565183) { + err_fun(nom_x, delta_x, out_1527400592183565183); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_8535551015639214025) { - inv_err_fun(nom_x, true_x, out_8535551015639214025); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_4038333929682939877) { + inv_err_fun(nom_x, true_x, out_4038333929682939877); } -void car_H_mod_fun(double *state, double *out_5722367516210042485) { - H_mod_fun(state, out_5722367516210042485); +void car_H_mod_fun(double *state, double *out_8049619905800951591) { + H_mod_fun(state, out_8049619905800951591); } -void car_f_fun(double *state, double dt, double *out_6327711019370239094) { - f_fun(state, dt, out_6327711019370239094); +void car_f_fun(double *state, double dt, double *out_7680940422259587756) { + f_fun(state, dt, out_7680940422259587756); } -void car_F_fun(double *state, double dt, double *out_334732449615445707) { - F_fun(state, dt, out_334732449615445707); +void car_F_fun(double *state, double dt, double *out_8373137223162851881) { + F_fun(state, dt, out_8373137223162851881); } -void car_h_25(double *state, double *unused, double *out_4988354507217303652) { - h_25(state, unused, out_4988354507217303652); +void car_h_25(double *state, double *unused, double *out_5355327156585439901) { + h_25(state, unused, out_5355327156585439901); } -void car_H_25(double *state, double *unused, double *out_3470820709702194752) { - H_25(state, unused, out_3470820709702194752); +void car_H_25(double *state, double *unused, double *out_2475786134934740220) { + H_25(state, unused, out_2475786134934740220); } -void car_h_24(double *state, double *unused, double *out_8320591788748654668) { - h_24(state, unused, out_8320591788748654668); +void car_h_24(double *state, double *unused, double *out_4948890380757191940) { + h_24(state, unused, out_4948890380757191940); } -void car_H_24(double *state, double *unused, double *out_5643470308707694318) { - H_24(state, unused, out_5643470308707694318); +void car_H_24(double *state, double *unused, double *out_3316217919116785212) { + H_24(state, unused, out_3316217919116785212); } -void car_h_30(double *state, double *unused, double *out_6271436662034189635) { - h_30(state, unused, out_6271436662034189635); +void car_h_30(double *state, double *unused, double *out_6805958549214323068) { + h_30(state, unused, out_6805958549214323068); } -void car_H_30(double *state, double *unused, double *out_952487751194946125) { - H_30(state, unused, out_952487751194946125); +void car_H_30(double *state, double *unused, double *out_42546823572508407) { + H_30(state, unused, out_42546823572508407); } -void car_h_26(double *state, double *unused, double *out_628706790953232800) { - h_26(state, unused, out_628706790953232800); +void car_h_26(double *state, double *unused, double *out_2475727773492236361) { + h_26(state, unused, out_2475727773492236361); } -void car_H_26(double *state, double *unused, double *out_7212324028576250976) { - H_26(state, unused, out_7212324028576250976); +void car_H_26(double *state, double *unused, double *out_6217289453808796444) { + H_26(state, unused, out_6217289453808796444); } -void car_h_27(double *state, double *unused, double *out_7592759828026001303) { - h_27(state, unused, out_7592759828026001303); +void car_h_27(double *state, double *unused, double *out_353653888924594256) { + h_27(state, unused, out_353653888924594256); } -void car_H_27(double *state, double *unused, double *out_5774922968645859733) { - H_27(state, unused, out_5774922968645859733); +void car_H_27(double *state, double *unused, double *out_9178245776862773329) { + H_27(state, unused, out_9178245776862773329); } -void car_h_29(double *state, double *unused, double *out_7749946496377130448) { - h_29(state, unused, out_7749946496377130448); +void car_h_29(double *state, double *unused, double *out_1804285281553477423) { + h_29(state, unused, out_1804285281553477423); } -void car_H_29(double *state, double *unused, double *out_7488285695515410766) { - H_29(state, unused, out_7488285695515410766); +void car_H_29(double *state, double *unused, double *out_6493251120747956234) { + H_29(state, unused, out_6493251120747956234); } -void car_h_28(double *state, double *unused, double *out_1346308770565827835) { - h_28(state, unused, out_1346308770565827835); +void car_h_28(double *state, double *unused, double *out_4712763629793748467) { + h_28(state, unused, out_4712763629793748467); } -void car_H_28(double *state, double *unused, double *out_5524655423950084515) { - H_28(state, unused, out_5524655423950084515); +void car_H_28(double *state, double *unused, double *out_4529620849182629983) { + H_28(state, unused, out_4529620849182629983); } -void car_h_31(double *state, double *unused, double *out_2214861449768682318) { - h_31(state, unused, out_2214861449768682318); +void car_h_31(double *state, double *unused, double *out_5630521218869945790) { + h_31(state, unused, out_5630521218869945790); } -void car_H_31(double *state, double *unused, double *out_7838532130809602452) { - H_31(state, unused, out_7838532130809602452); +void car_H_31(double *state, double *unused, double *out_6843497556042147920) { + H_31(state, unused, out_6843497556042147920); } 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 18397a5d8b..e8eaa292a2 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_870685266903427678); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_8535551015639214025); -void car_H_mod_fun(double *state, double *out_5722367516210042485); -void car_f_fun(double *state, double dt, double *out_6327711019370239094); -void car_F_fun(double *state, double dt, double *out_334732449615445707); -void car_h_25(double *state, double *unused, double *out_4988354507217303652); -void car_H_25(double *state, double *unused, double *out_3470820709702194752); -void car_h_24(double *state, double *unused, double *out_8320591788748654668); -void car_H_24(double *state, double *unused, double *out_5643470308707694318); -void car_h_30(double *state, double *unused, double *out_6271436662034189635); -void car_H_30(double *state, double *unused, double *out_952487751194946125); -void car_h_26(double *state, double *unused, double *out_628706790953232800); -void car_H_26(double *state, double *unused, double *out_7212324028576250976); -void car_h_27(double *state, double *unused, double *out_7592759828026001303); -void car_H_27(double *state, double *unused, double *out_5774922968645859733); -void car_h_29(double *state, double *unused, double *out_7749946496377130448); -void car_H_29(double *state, double *unused, double *out_7488285695515410766); -void car_h_28(double *state, double *unused, double *out_1346308770565827835); -void car_H_28(double *state, double *unused, double *out_5524655423950084515); -void car_h_31(double *state, double *unused, double *out_2214861449768682318); -void car_H_31(double *state, double *unused, double *out_7838532130809602452); +void car_err_fun(double *nom_x, double *delta_x, double *out_1527400592183565183); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_4038333929682939877); +void car_H_mod_fun(double *state, double *out_8049619905800951591); +void car_f_fun(double *state, double dt, double *out_7680940422259587756); +void car_F_fun(double *state, double dt, double *out_8373137223162851881); +void car_h_25(double *state, double *unused, double *out_5355327156585439901); +void car_H_25(double *state, double *unused, double *out_2475786134934740220); +void car_h_24(double *state, double *unused, double *out_4948890380757191940); +void car_H_24(double *state, double *unused, double *out_3316217919116785212); +void car_h_30(double *state, double *unused, double *out_6805958549214323068); +void car_H_30(double *state, double *unused, double *out_42546823572508407); +void car_h_26(double *state, double *unused, double *out_2475727773492236361); +void car_H_26(double *state, double *unused, double *out_6217289453808796444); +void car_h_27(double *state, double *unused, double *out_353653888924594256); +void car_H_27(double *state, double *unused, double *out_9178245776862773329); +void car_h_29(double *state, double *unused, double *out_1804285281553477423); +void car_H_29(double *state, double *unused, double *out_6493251120747956234); +void car_h_28(double *state, double *unused, double *out_4712763629793748467); +void car_H_28(double *state, double *unused, double *out_4529620849182629983); +void car_h_31(double *state, double *unused, double *out_5630521218869945790); +void car_H_31(double *state, double *unused, double *out_6843497556042147920); 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/car.os b/selfdrive/locationd/models/generated/car.os index 763f6afba2..6d7a588d97 100644 Binary files a/selfdrive/locationd/models/generated/car.os and b/selfdrive/locationd/models/generated/car.os differ diff --git a/selfdrive/locationd/models/generated/libcar.so b/selfdrive/locationd/models/generated/libcar.so index e66b841c4e..ec2c55555b 100755 Binary files a/selfdrive/locationd/models/generated/libcar.so and b/selfdrive/locationd/models/generated/libcar.so differ diff --git a/selfdrive/locationd/models/generated/libpose.so b/selfdrive/locationd/models/generated/libpose.so index 1661647d9a..d89955950b 100755 Binary files a/selfdrive/locationd/models/generated/libpose.so and b/selfdrive/locationd/models/generated/libpose.so differ diff --git a/selfdrive/locationd/models/generated/pose.cpp b/selfdrive/locationd/models/generated/pose.cpp index d8aa44dc8f..829ac49494 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_731387772078245243) { - out_731387772078245243[0] = delta_x[0] + nom_x[0]; - out_731387772078245243[1] = delta_x[1] + nom_x[1]; - out_731387772078245243[2] = delta_x[2] + nom_x[2]; - out_731387772078245243[3] = delta_x[3] + nom_x[3]; - out_731387772078245243[4] = delta_x[4] + nom_x[4]; - out_731387772078245243[5] = delta_x[5] + nom_x[5]; - out_731387772078245243[6] = delta_x[6] + nom_x[6]; - out_731387772078245243[7] = delta_x[7] + nom_x[7]; - out_731387772078245243[8] = delta_x[8] + nom_x[8]; - out_731387772078245243[9] = delta_x[9] + nom_x[9]; - out_731387772078245243[10] = delta_x[10] + nom_x[10]; - out_731387772078245243[11] = delta_x[11] + nom_x[11]; - out_731387772078245243[12] = delta_x[12] + nom_x[12]; - out_731387772078245243[13] = delta_x[13] + nom_x[13]; - out_731387772078245243[14] = delta_x[14] + nom_x[14]; - out_731387772078245243[15] = delta_x[15] + nom_x[15]; - out_731387772078245243[16] = delta_x[16] + nom_x[16]; - out_731387772078245243[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_5549195789352376606) { + out_5549195789352376606[0] = delta_x[0] + nom_x[0]; + out_5549195789352376606[1] = delta_x[1] + nom_x[1]; + out_5549195789352376606[2] = delta_x[2] + nom_x[2]; + out_5549195789352376606[3] = delta_x[3] + nom_x[3]; + out_5549195789352376606[4] = delta_x[4] + nom_x[4]; + out_5549195789352376606[5] = delta_x[5] + nom_x[5]; + out_5549195789352376606[6] = delta_x[6] + nom_x[6]; + out_5549195789352376606[7] = delta_x[7] + nom_x[7]; + out_5549195789352376606[8] = delta_x[8] + nom_x[8]; + out_5549195789352376606[9] = delta_x[9] + nom_x[9]; + out_5549195789352376606[10] = delta_x[10] + nom_x[10]; + out_5549195789352376606[11] = delta_x[11] + nom_x[11]; + out_5549195789352376606[12] = delta_x[12] + nom_x[12]; + out_5549195789352376606[13] = delta_x[13] + nom_x[13]; + out_5549195789352376606[14] = delta_x[14] + nom_x[14]; + out_5549195789352376606[15] = delta_x[15] + nom_x[15]; + out_5549195789352376606[16] = delta_x[16] + nom_x[16]; + out_5549195789352376606[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_4206130707332622452) { - out_4206130707332622452[0] = -nom_x[0] + true_x[0]; - out_4206130707332622452[1] = -nom_x[1] + true_x[1]; - out_4206130707332622452[2] = -nom_x[2] + true_x[2]; - out_4206130707332622452[3] = -nom_x[3] + true_x[3]; - out_4206130707332622452[4] = -nom_x[4] + true_x[4]; - out_4206130707332622452[5] = -nom_x[5] + true_x[5]; - out_4206130707332622452[6] = -nom_x[6] + true_x[6]; - out_4206130707332622452[7] = -nom_x[7] + true_x[7]; - out_4206130707332622452[8] = -nom_x[8] + true_x[8]; - out_4206130707332622452[9] = -nom_x[9] + true_x[9]; - out_4206130707332622452[10] = -nom_x[10] + true_x[10]; - out_4206130707332622452[11] = -nom_x[11] + true_x[11]; - out_4206130707332622452[12] = -nom_x[12] + true_x[12]; - out_4206130707332622452[13] = -nom_x[13] + true_x[13]; - out_4206130707332622452[14] = -nom_x[14] + true_x[14]; - out_4206130707332622452[15] = -nom_x[15] + true_x[15]; - out_4206130707332622452[16] = -nom_x[16] + true_x[16]; - out_4206130707332622452[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_6348441241420563996) { + out_6348441241420563996[0] = -nom_x[0] + true_x[0]; + out_6348441241420563996[1] = -nom_x[1] + true_x[1]; + out_6348441241420563996[2] = -nom_x[2] + true_x[2]; + out_6348441241420563996[3] = -nom_x[3] + true_x[3]; + out_6348441241420563996[4] = -nom_x[4] + true_x[4]; + out_6348441241420563996[5] = -nom_x[5] + true_x[5]; + out_6348441241420563996[6] = -nom_x[6] + true_x[6]; + out_6348441241420563996[7] = -nom_x[7] + true_x[7]; + out_6348441241420563996[8] = -nom_x[8] + true_x[8]; + out_6348441241420563996[9] = -nom_x[9] + true_x[9]; + out_6348441241420563996[10] = -nom_x[10] + true_x[10]; + out_6348441241420563996[11] = -nom_x[11] + true_x[11]; + out_6348441241420563996[12] = -nom_x[12] + true_x[12]; + out_6348441241420563996[13] = -nom_x[13] + true_x[13]; + out_6348441241420563996[14] = -nom_x[14] + true_x[14]; + out_6348441241420563996[15] = -nom_x[15] + true_x[15]; + out_6348441241420563996[16] = -nom_x[16] + true_x[16]; + out_6348441241420563996[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_2916011499501574438) { - out_2916011499501574438[0] = 1.0; - out_2916011499501574438[1] = 0.0; - out_2916011499501574438[2] = 0.0; - out_2916011499501574438[3] = 0.0; - out_2916011499501574438[4] = 0.0; - out_2916011499501574438[5] = 0.0; - out_2916011499501574438[6] = 0.0; - out_2916011499501574438[7] = 0.0; - out_2916011499501574438[8] = 0.0; - out_2916011499501574438[9] = 0.0; - out_2916011499501574438[10] = 0.0; - out_2916011499501574438[11] = 0.0; - out_2916011499501574438[12] = 0.0; - out_2916011499501574438[13] = 0.0; - out_2916011499501574438[14] = 0.0; - out_2916011499501574438[15] = 0.0; - out_2916011499501574438[16] = 0.0; - out_2916011499501574438[17] = 0.0; - out_2916011499501574438[18] = 0.0; - out_2916011499501574438[19] = 1.0; - out_2916011499501574438[20] = 0.0; - out_2916011499501574438[21] = 0.0; - out_2916011499501574438[22] = 0.0; - out_2916011499501574438[23] = 0.0; - out_2916011499501574438[24] = 0.0; - out_2916011499501574438[25] = 0.0; - out_2916011499501574438[26] = 0.0; - out_2916011499501574438[27] = 0.0; - out_2916011499501574438[28] = 0.0; - out_2916011499501574438[29] = 0.0; - out_2916011499501574438[30] = 0.0; - out_2916011499501574438[31] = 0.0; - out_2916011499501574438[32] = 0.0; - out_2916011499501574438[33] = 0.0; - out_2916011499501574438[34] = 0.0; - out_2916011499501574438[35] = 0.0; - out_2916011499501574438[36] = 0.0; - out_2916011499501574438[37] = 0.0; - out_2916011499501574438[38] = 1.0; - out_2916011499501574438[39] = 0.0; - out_2916011499501574438[40] = 0.0; - out_2916011499501574438[41] = 0.0; - out_2916011499501574438[42] = 0.0; - out_2916011499501574438[43] = 0.0; - out_2916011499501574438[44] = 0.0; - out_2916011499501574438[45] = 0.0; - out_2916011499501574438[46] = 0.0; - out_2916011499501574438[47] = 0.0; - out_2916011499501574438[48] = 0.0; - out_2916011499501574438[49] = 0.0; - out_2916011499501574438[50] = 0.0; - out_2916011499501574438[51] = 0.0; - out_2916011499501574438[52] = 0.0; - out_2916011499501574438[53] = 0.0; - out_2916011499501574438[54] = 0.0; - out_2916011499501574438[55] = 0.0; - out_2916011499501574438[56] = 0.0; - out_2916011499501574438[57] = 1.0; - out_2916011499501574438[58] = 0.0; - out_2916011499501574438[59] = 0.0; - out_2916011499501574438[60] = 0.0; - out_2916011499501574438[61] = 0.0; - out_2916011499501574438[62] = 0.0; - out_2916011499501574438[63] = 0.0; - out_2916011499501574438[64] = 0.0; - out_2916011499501574438[65] = 0.0; - out_2916011499501574438[66] = 0.0; - out_2916011499501574438[67] = 0.0; - out_2916011499501574438[68] = 0.0; - out_2916011499501574438[69] = 0.0; - out_2916011499501574438[70] = 0.0; - out_2916011499501574438[71] = 0.0; - out_2916011499501574438[72] = 0.0; - out_2916011499501574438[73] = 0.0; - out_2916011499501574438[74] = 0.0; - out_2916011499501574438[75] = 0.0; - out_2916011499501574438[76] = 1.0; - out_2916011499501574438[77] = 0.0; - out_2916011499501574438[78] = 0.0; - out_2916011499501574438[79] = 0.0; - out_2916011499501574438[80] = 0.0; - out_2916011499501574438[81] = 0.0; - out_2916011499501574438[82] = 0.0; - out_2916011499501574438[83] = 0.0; - out_2916011499501574438[84] = 0.0; - out_2916011499501574438[85] = 0.0; - out_2916011499501574438[86] = 0.0; - out_2916011499501574438[87] = 0.0; - out_2916011499501574438[88] = 0.0; - out_2916011499501574438[89] = 0.0; - out_2916011499501574438[90] = 0.0; - out_2916011499501574438[91] = 0.0; - out_2916011499501574438[92] = 0.0; - out_2916011499501574438[93] = 0.0; - out_2916011499501574438[94] = 0.0; - out_2916011499501574438[95] = 1.0; - out_2916011499501574438[96] = 0.0; - out_2916011499501574438[97] = 0.0; - out_2916011499501574438[98] = 0.0; - out_2916011499501574438[99] = 0.0; - out_2916011499501574438[100] = 0.0; - out_2916011499501574438[101] = 0.0; - out_2916011499501574438[102] = 0.0; - out_2916011499501574438[103] = 0.0; - out_2916011499501574438[104] = 0.0; - out_2916011499501574438[105] = 0.0; - out_2916011499501574438[106] = 0.0; - out_2916011499501574438[107] = 0.0; - out_2916011499501574438[108] = 0.0; - out_2916011499501574438[109] = 0.0; - out_2916011499501574438[110] = 0.0; - out_2916011499501574438[111] = 0.0; - out_2916011499501574438[112] = 0.0; - out_2916011499501574438[113] = 0.0; - out_2916011499501574438[114] = 1.0; - out_2916011499501574438[115] = 0.0; - out_2916011499501574438[116] = 0.0; - out_2916011499501574438[117] = 0.0; - out_2916011499501574438[118] = 0.0; - out_2916011499501574438[119] = 0.0; - out_2916011499501574438[120] = 0.0; - out_2916011499501574438[121] = 0.0; - out_2916011499501574438[122] = 0.0; - out_2916011499501574438[123] = 0.0; - out_2916011499501574438[124] = 0.0; - out_2916011499501574438[125] = 0.0; - out_2916011499501574438[126] = 0.0; - out_2916011499501574438[127] = 0.0; - out_2916011499501574438[128] = 0.0; - out_2916011499501574438[129] = 0.0; - out_2916011499501574438[130] = 0.0; - out_2916011499501574438[131] = 0.0; - out_2916011499501574438[132] = 0.0; - out_2916011499501574438[133] = 1.0; - out_2916011499501574438[134] = 0.0; - out_2916011499501574438[135] = 0.0; - out_2916011499501574438[136] = 0.0; - out_2916011499501574438[137] = 0.0; - out_2916011499501574438[138] = 0.0; - out_2916011499501574438[139] = 0.0; - out_2916011499501574438[140] = 0.0; - out_2916011499501574438[141] = 0.0; - out_2916011499501574438[142] = 0.0; - out_2916011499501574438[143] = 0.0; - out_2916011499501574438[144] = 0.0; - out_2916011499501574438[145] = 0.0; - out_2916011499501574438[146] = 0.0; - out_2916011499501574438[147] = 0.0; - out_2916011499501574438[148] = 0.0; - out_2916011499501574438[149] = 0.0; - out_2916011499501574438[150] = 0.0; - out_2916011499501574438[151] = 0.0; - out_2916011499501574438[152] = 1.0; - out_2916011499501574438[153] = 0.0; - out_2916011499501574438[154] = 0.0; - out_2916011499501574438[155] = 0.0; - out_2916011499501574438[156] = 0.0; - out_2916011499501574438[157] = 0.0; - out_2916011499501574438[158] = 0.0; - out_2916011499501574438[159] = 0.0; - out_2916011499501574438[160] = 0.0; - out_2916011499501574438[161] = 0.0; - out_2916011499501574438[162] = 0.0; - out_2916011499501574438[163] = 0.0; - out_2916011499501574438[164] = 0.0; - out_2916011499501574438[165] = 0.0; - out_2916011499501574438[166] = 0.0; - out_2916011499501574438[167] = 0.0; - out_2916011499501574438[168] = 0.0; - out_2916011499501574438[169] = 0.0; - out_2916011499501574438[170] = 0.0; - out_2916011499501574438[171] = 1.0; - out_2916011499501574438[172] = 0.0; - out_2916011499501574438[173] = 0.0; - out_2916011499501574438[174] = 0.0; - out_2916011499501574438[175] = 0.0; - out_2916011499501574438[176] = 0.0; - out_2916011499501574438[177] = 0.0; - out_2916011499501574438[178] = 0.0; - out_2916011499501574438[179] = 0.0; - out_2916011499501574438[180] = 0.0; - out_2916011499501574438[181] = 0.0; - out_2916011499501574438[182] = 0.0; - out_2916011499501574438[183] = 0.0; - out_2916011499501574438[184] = 0.0; - out_2916011499501574438[185] = 0.0; - out_2916011499501574438[186] = 0.0; - out_2916011499501574438[187] = 0.0; - out_2916011499501574438[188] = 0.0; - out_2916011499501574438[189] = 0.0; - out_2916011499501574438[190] = 1.0; - out_2916011499501574438[191] = 0.0; - out_2916011499501574438[192] = 0.0; - out_2916011499501574438[193] = 0.0; - out_2916011499501574438[194] = 0.0; - out_2916011499501574438[195] = 0.0; - out_2916011499501574438[196] = 0.0; - out_2916011499501574438[197] = 0.0; - out_2916011499501574438[198] = 0.0; - out_2916011499501574438[199] = 0.0; - out_2916011499501574438[200] = 0.0; - out_2916011499501574438[201] = 0.0; - out_2916011499501574438[202] = 0.0; - out_2916011499501574438[203] = 0.0; - out_2916011499501574438[204] = 0.0; - out_2916011499501574438[205] = 0.0; - out_2916011499501574438[206] = 0.0; - out_2916011499501574438[207] = 0.0; - out_2916011499501574438[208] = 0.0; - out_2916011499501574438[209] = 1.0; - out_2916011499501574438[210] = 0.0; - out_2916011499501574438[211] = 0.0; - out_2916011499501574438[212] = 0.0; - out_2916011499501574438[213] = 0.0; - out_2916011499501574438[214] = 0.0; - out_2916011499501574438[215] = 0.0; - out_2916011499501574438[216] = 0.0; - out_2916011499501574438[217] = 0.0; - out_2916011499501574438[218] = 0.0; - out_2916011499501574438[219] = 0.0; - out_2916011499501574438[220] = 0.0; - out_2916011499501574438[221] = 0.0; - out_2916011499501574438[222] = 0.0; - out_2916011499501574438[223] = 0.0; - out_2916011499501574438[224] = 0.0; - out_2916011499501574438[225] = 0.0; - out_2916011499501574438[226] = 0.0; - out_2916011499501574438[227] = 0.0; - out_2916011499501574438[228] = 1.0; - out_2916011499501574438[229] = 0.0; - out_2916011499501574438[230] = 0.0; - out_2916011499501574438[231] = 0.0; - out_2916011499501574438[232] = 0.0; - out_2916011499501574438[233] = 0.0; - out_2916011499501574438[234] = 0.0; - out_2916011499501574438[235] = 0.0; - out_2916011499501574438[236] = 0.0; - out_2916011499501574438[237] = 0.0; - out_2916011499501574438[238] = 0.0; - out_2916011499501574438[239] = 0.0; - out_2916011499501574438[240] = 0.0; - out_2916011499501574438[241] = 0.0; - out_2916011499501574438[242] = 0.0; - out_2916011499501574438[243] = 0.0; - out_2916011499501574438[244] = 0.0; - out_2916011499501574438[245] = 0.0; - out_2916011499501574438[246] = 0.0; - out_2916011499501574438[247] = 1.0; - out_2916011499501574438[248] = 0.0; - out_2916011499501574438[249] = 0.0; - out_2916011499501574438[250] = 0.0; - out_2916011499501574438[251] = 0.0; - out_2916011499501574438[252] = 0.0; - out_2916011499501574438[253] = 0.0; - out_2916011499501574438[254] = 0.0; - out_2916011499501574438[255] = 0.0; - out_2916011499501574438[256] = 0.0; - out_2916011499501574438[257] = 0.0; - out_2916011499501574438[258] = 0.0; - out_2916011499501574438[259] = 0.0; - out_2916011499501574438[260] = 0.0; - out_2916011499501574438[261] = 0.0; - out_2916011499501574438[262] = 0.0; - out_2916011499501574438[263] = 0.0; - out_2916011499501574438[264] = 0.0; - out_2916011499501574438[265] = 0.0; - out_2916011499501574438[266] = 1.0; - out_2916011499501574438[267] = 0.0; - out_2916011499501574438[268] = 0.0; - out_2916011499501574438[269] = 0.0; - out_2916011499501574438[270] = 0.0; - out_2916011499501574438[271] = 0.0; - out_2916011499501574438[272] = 0.0; - out_2916011499501574438[273] = 0.0; - out_2916011499501574438[274] = 0.0; - out_2916011499501574438[275] = 0.0; - out_2916011499501574438[276] = 0.0; - out_2916011499501574438[277] = 0.0; - out_2916011499501574438[278] = 0.0; - out_2916011499501574438[279] = 0.0; - out_2916011499501574438[280] = 0.0; - out_2916011499501574438[281] = 0.0; - out_2916011499501574438[282] = 0.0; - out_2916011499501574438[283] = 0.0; - out_2916011499501574438[284] = 0.0; - out_2916011499501574438[285] = 1.0; - out_2916011499501574438[286] = 0.0; - out_2916011499501574438[287] = 0.0; - out_2916011499501574438[288] = 0.0; - out_2916011499501574438[289] = 0.0; - out_2916011499501574438[290] = 0.0; - out_2916011499501574438[291] = 0.0; - out_2916011499501574438[292] = 0.0; - out_2916011499501574438[293] = 0.0; - out_2916011499501574438[294] = 0.0; - out_2916011499501574438[295] = 0.0; - out_2916011499501574438[296] = 0.0; - out_2916011499501574438[297] = 0.0; - out_2916011499501574438[298] = 0.0; - out_2916011499501574438[299] = 0.0; - out_2916011499501574438[300] = 0.0; - out_2916011499501574438[301] = 0.0; - out_2916011499501574438[302] = 0.0; - out_2916011499501574438[303] = 0.0; - out_2916011499501574438[304] = 1.0; - out_2916011499501574438[305] = 0.0; - out_2916011499501574438[306] = 0.0; - out_2916011499501574438[307] = 0.0; - out_2916011499501574438[308] = 0.0; - out_2916011499501574438[309] = 0.0; - out_2916011499501574438[310] = 0.0; - out_2916011499501574438[311] = 0.0; - out_2916011499501574438[312] = 0.0; - out_2916011499501574438[313] = 0.0; - out_2916011499501574438[314] = 0.0; - out_2916011499501574438[315] = 0.0; - out_2916011499501574438[316] = 0.0; - out_2916011499501574438[317] = 0.0; - out_2916011499501574438[318] = 0.0; - out_2916011499501574438[319] = 0.0; - out_2916011499501574438[320] = 0.0; - out_2916011499501574438[321] = 0.0; - out_2916011499501574438[322] = 0.0; - out_2916011499501574438[323] = 1.0; +void H_mod_fun(double *state, double *out_932088622648113863) { + out_932088622648113863[0] = 1.0; + out_932088622648113863[1] = 0.0; + out_932088622648113863[2] = 0.0; + out_932088622648113863[3] = 0.0; + out_932088622648113863[4] = 0.0; + out_932088622648113863[5] = 0.0; + out_932088622648113863[6] = 0.0; + out_932088622648113863[7] = 0.0; + out_932088622648113863[8] = 0.0; + out_932088622648113863[9] = 0.0; + out_932088622648113863[10] = 0.0; + out_932088622648113863[11] = 0.0; + out_932088622648113863[12] = 0.0; + out_932088622648113863[13] = 0.0; + out_932088622648113863[14] = 0.0; + out_932088622648113863[15] = 0.0; + out_932088622648113863[16] = 0.0; + out_932088622648113863[17] = 0.0; + out_932088622648113863[18] = 0.0; + out_932088622648113863[19] = 1.0; + out_932088622648113863[20] = 0.0; + out_932088622648113863[21] = 0.0; + out_932088622648113863[22] = 0.0; + out_932088622648113863[23] = 0.0; + out_932088622648113863[24] = 0.0; + out_932088622648113863[25] = 0.0; + out_932088622648113863[26] = 0.0; + out_932088622648113863[27] = 0.0; + out_932088622648113863[28] = 0.0; + out_932088622648113863[29] = 0.0; + out_932088622648113863[30] = 0.0; + out_932088622648113863[31] = 0.0; + out_932088622648113863[32] = 0.0; + out_932088622648113863[33] = 0.0; + out_932088622648113863[34] = 0.0; + out_932088622648113863[35] = 0.0; + out_932088622648113863[36] = 0.0; + out_932088622648113863[37] = 0.0; + out_932088622648113863[38] = 1.0; + out_932088622648113863[39] = 0.0; + out_932088622648113863[40] = 0.0; + out_932088622648113863[41] = 0.0; + out_932088622648113863[42] = 0.0; + out_932088622648113863[43] = 0.0; + out_932088622648113863[44] = 0.0; + out_932088622648113863[45] = 0.0; + out_932088622648113863[46] = 0.0; + out_932088622648113863[47] = 0.0; + out_932088622648113863[48] = 0.0; + out_932088622648113863[49] = 0.0; + out_932088622648113863[50] = 0.0; + out_932088622648113863[51] = 0.0; + out_932088622648113863[52] = 0.0; + out_932088622648113863[53] = 0.0; + out_932088622648113863[54] = 0.0; + out_932088622648113863[55] = 0.0; + out_932088622648113863[56] = 0.0; + out_932088622648113863[57] = 1.0; + out_932088622648113863[58] = 0.0; + out_932088622648113863[59] = 0.0; + out_932088622648113863[60] = 0.0; + out_932088622648113863[61] = 0.0; + out_932088622648113863[62] = 0.0; + out_932088622648113863[63] = 0.0; + out_932088622648113863[64] = 0.0; + out_932088622648113863[65] = 0.0; + out_932088622648113863[66] = 0.0; + out_932088622648113863[67] = 0.0; + out_932088622648113863[68] = 0.0; + out_932088622648113863[69] = 0.0; + out_932088622648113863[70] = 0.0; + out_932088622648113863[71] = 0.0; + out_932088622648113863[72] = 0.0; + out_932088622648113863[73] = 0.0; + out_932088622648113863[74] = 0.0; + out_932088622648113863[75] = 0.0; + out_932088622648113863[76] = 1.0; + out_932088622648113863[77] = 0.0; + out_932088622648113863[78] = 0.0; + out_932088622648113863[79] = 0.0; + out_932088622648113863[80] = 0.0; + out_932088622648113863[81] = 0.0; + out_932088622648113863[82] = 0.0; + out_932088622648113863[83] = 0.0; + out_932088622648113863[84] = 0.0; + out_932088622648113863[85] = 0.0; + out_932088622648113863[86] = 0.0; + out_932088622648113863[87] = 0.0; + out_932088622648113863[88] = 0.0; + out_932088622648113863[89] = 0.0; + out_932088622648113863[90] = 0.0; + out_932088622648113863[91] = 0.0; + out_932088622648113863[92] = 0.0; + out_932088622648113863[93] = 0.0; + out_932088622648113863[94] = 0.0; + out_932088622648113863[95] = 1.0; + out_932088622648113863[96] = 0.0; + out_932088622648113863[97] = 0.0; + out_932088622648113863[98] = 0.0; + out_932088622648113863[99] = 0.0; + out_932088622648113863[100] = 0.0; + out_932088622648113863[101] = 0.0; + out_932088622648113863[102] = 0.0; + out_932088622648113863[103] = 0.0; + out_932088622648113863[104] = 0.0; + out_932088622648113863[105] = 0.0; + out_932088622648113863[106] = 0.0; + out_932088622648113863[107] = 0.0; + out_932088622648113863[108] = 0.0; + out_932088622648113863[109] = 0.0; + out_932088622648113863[110] = 0.0; + out_932088622648113863[111] = 0.0; + out_932088622648113863[112] = 0.0; + out_932088622648113863[113] = 0.0; + out_932088622648113863[114] = 1.0; + out_932088622648113863[115] = 0.0; + out_932088622648113863[116] = 0.0; + out_932088622648113863[117] = 0.0; + out_932088622648113863[118] = 0.0; + out_932088622648113863[119] = 0.0; + out_932088622648113863[120] = 0.0; + out_932088622648113863[121] = 0.0; + out_932088622648113863[122] = 0.0; + out_932088622648113863[123] = 0.0; + out_932088622648113863[124] = 0.0; + out_932088622648113863[125] = 0.0; + out_932088622648113863[126] = 0.0; + out_932088622648113863[127] = 0.0; + out_932088622648113863[128] = 0.0; + out_932088622648113863[129] = 0.0; + out_932088622648113863[130] = 0.0; + out_932088622648113863[131] = 0.0; + out_932088622648113863[132] = 0.0; + out_932088622648113863[133] = 1.0; + out_932088622648113863[134] = 0.0; + out_932088622648113863[135] = 0.0; + out_932088622648113863[136] = 0.0; + out_932088622648113863[137] = 0.0; + out_932088622648113863[138] = 0.0; + out_932088622648113863[139] = 0.0; + out_932088622648113863[140] = 0.0; + out_932088622648113863[141] = 0.0; + out_932088622648113863[142] = 0.0; + out_932088622648113863[143] = 0.0; + out_932088622648113863[144] = 0.0; + out_932088622648113863[145] = 0.0; + out_932088622648113863[146] = 0.0; + out_932088622648113863[147] = 0.0; + out_932088622648113863[148] = 0.0; + out_932088622648113863[149] = 0.0; + out_932088622648113863[150] = 0.0; + out_932088622648113863[151] = 0.0; + out_932088622648113863[152] = 1.0; + out_932088622648113863[153] = 0.0; + out_932088622648113863[154] = 0.0; + out_932088622648113863[155] = 0.0; + out_932088622648113863[156] = 0.0; + out_932088622648113863[157] = 0.0; + out_932088622648113863[158] = 0.0; + out_932088622648113863[159] = 0.0; + out_932088622648113863[160] = 0.0; + out_932088622648113863[161] = 0.0; + out_932088622648113863[162] = 0.0; + out_932088622648113863[163] = 0.0; + out_932088622648113863[164] = 0.0; + out_932088622648113863[165] = 0.0; + out_932088622648113863[166] = 0.0; + out_932088622648113863[167] = 0.0; + out_932088622648113863[168] = 0.0; + out_932088622648113863[169] = 0.0; + out_932088622648113863[170] = 0.0; + out_932088622648113863[171] = 1.0; + out_932088622648113863[172] = 0.0; + out_932088622648113863[173] = 0.0; + out_932088622648113863[174] = 0.0; + out_932088622648113863[175] = 0.0; + out_932088622648113863[176] = 0.0; + out_932088622648113863[177] = 0.0; + out_932088622648113863[178] = 0.0; + out_932088622648113863[179] = 0.0; + out_932088622648113863[180] = 0.0; + out_932088622648113863[181] = 0.0; + out_932088622648113863[182] = 0.0; + out_932088622648113863[183] = 0.0; + out_932088622648113863[184] = 0.0; + out_932088622648113863[185] = 0.0; + out_932088622648113863[186] = 0.0; + out_932088622648113863[187] = 0.0; + out_932088622648113863[188] = 0.0; + out_932088622648113863[189] = 0.0; + out_932088622648113863[190] = 1.0; + out_932088622648113863[191] = 0.0; + out_932088622648113863[192] = 0.0; + out_932088622648113863[193] = 0.0; + out_932088622648113863[194] = 0.0; + out_932088622648113863[195] = 0.0; + out_932088622648113863[196] = 0.0; + out_932088622648113863[197] = 0.0; + out_932088622648113863[198] = 0.0; + out_932088622648113863[199] = 0.0; + out_932088622648113863[200] = 0.0; + out_932088622648113863[201] = 0.0; + out_932088622648113863[202] = 0.0; + out_932088622648113863[203] = 0.0; + out_932088622648113863[204] = 0.0; + out_932088622648113863[205] = 0.0; + out_932088622648113863[206] = 0.0; + out_932088622648113863[207] = 0.0; + out_932088622648113863[208] = 0.0; + out_932088622648113863[209] = 1.0; + out_932088622648113863[210] = 0.0; + out_932088622648113863[211] = 0.0; + out_932088622648113863[212] = 0.0; + out_932088622648113863[213] = 0.0; + out_932088622648113863[214] = 0.0; + out_932088622648113863[215] = 0.0; + out_932088622648113863[216] = 0.0; + out_932088622648113863[217] = 0.0; + out_932088622648113863[218] = 0.0; + out_932088622648113863[219] = 0.0; + out_932088622648113863[220] = 0.0; + out_932088622648113863[221] = 0.0; + out_932088622648113863[222] = 0.0; + out_932088622648113863[223] = 0.0; + out_932088622648113863[224] = 0.0; + out_932088622648113863[225] = 0.0; + out_932088622648113863[226] = 0.0; + out_932088622648113863[227] = 0.0; + out_932088622648113863[228] = 1.0; + out_932088622648113863[229] = 0.0; + out_932088622648113863[230] = 0.0; + out_932088622648113863[231] = 0.0; + out_932088622648113863[232] = 0.0; + out_932088622648113863[233] = 0.0; + out_932088622648113863[234] = 0.0; + out_932088622648113863[235] = 0.0; + out_932088622648113863[236] = 0.0; + out_932088622648113863[237] = 0.0; + out_932088622648113863[238] = 0.0; + out_932088622648113863[239] = 0.0; + out_932088622648113863[240] = 0.0; + out_932088622648113863[241] = 0.0; + out_932088622648113863[242] = 0.0; + out_932088622648113863[243] = 0.0; + out_932088622648113863[244] = 0.0; + out_932088622648113863[245] = 0.0; + out_932088622648113863[246] = 0.0; + out_932088622648113863[247] = 1.0; + out_932088622648113863[248] = 0.0; + out_932088622648113863[249] = 0.0; + out_932088622648113863[250] = 0.0; + out_932088622648113863[251] = 0.0; + out_932088622648113863[252] = 0.0; + out_932088622648113863[253] = 0.0; + out_932088622648113863[254] = 0.0; + out_932088622648113863[255] = 0.0; + out_932088622648113863[256] = 0.0; + out_932088622648113863[257] = 0.0; + out_932088622648113863[258] = 0.0; + out_932088622648113863[259] = 0.0; + out_932088622648113863[260] = 0.0; + out_932088622648113863[261] = 0.0; + out_932088622648113863[262] = 0.0; + out_932088622648113863[263] = 0.0; + out_932088622648113863[264] = 0.0; + out_932088622648113863[265] = 0.0; + out_932088622648113863[266] = 1.0; + out_932088622648113863[267] = 0.0; + out_932088622648113863[268] = 0.0; + out_932088622648113863[269] = 0.0; + out_932088622648113863[270] = 0.0; + out_932088622648113863[271] = 0.0; + out_932088622648113863[272] = 0.0; + out_932088622648113863[273] = 0.0; + out_932088622648113863[274] = 0.0; + out_932088622648113863[275] = 0.0; + out_932088622648113863[276] = 0.0; + out_932088622648113863[277] = 0.0; + out_932088622648113863[278] = 0.0; + out_932088622648113863[279] = 0.0; + out_932088622648113863[280] = 0.0; + out_932088622648113863[281] = 0.0; + out_932088622648113863[282] = 0.0; + out_932088622648113863[283] = 0.0; + out_932088622648113863[284] = 0.0; + out_932088622648113863[285] = 1.0; + out_932088622648113863[286] = 0.0; + out_932088622648113863[287] = 0.0; + out_932088622648113863[288] = 0.0; + out_932088622648113863[289] = 0.0; + out_932088622648113863[290] = 0.0; + out_932088622648113863[291] = 0.0; + out_932088622648113863[292] = 0.0; + out_932088622648113863[293] = 0.0; + out_932088622648113863[294] = 0.0; + out_932088622648113863[295] = 0.0; + out_932088622648113863[296] = 0.0; + out_932088622648113863[297] = 0.0; + out_932088622648113863[298] = 0.0; + out_932088622648113863[299] = 0.0; + out_932088622648113863[300] = 0.0; + out_932088622648113863[301] = 0.0; + out_932088622648113863[302] = 0.0; + out_932088622648113863[303] = 0.0; + out_932088622648113863[304] = 1.0; + out_932088622648113863[305] = 0.0; + out_932088622648113863[306] = 0.0; + out_932088622648113863[307] = 0.0; + out_932088622648113863[308] = 0.0; + out_932088622648113863[309] = 0.0; + out_932088622648113863[310] = 0.0; + out_932088622648113863[311] = 0.0; + out_932088622648113863[312] = 0.0; + out_932088622648113863[313] = 0.0; + out_932088622648113863[314] = 0.0; + out_932088622648113863[315] = 0.0; + out_932088622648113863[316] = 0.0; + out_932088622648113863[317] = 0.0; + out_932088622648113863[318] = 0.0; + out_932088622648113863[319] = 0.0; + out_932088622648113863[320] = 0.0; + out_932088622648113863[321] = 0.0; + out_932088622648113863[322] = 0.0; + out_932088622648113863[323] = 1.0; } -void f_fun(double *state, double dt, double *out_4420889553799640425) { - out_4420889553799640425[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_4420889553799640425[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_4420889553799640425[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_4420889553799640425[3] = dt*state[12] + state[3]; - out_4420889553799640425[4] = dt*state[13] + state[4]; - out_4420889553799640425[5] = dt*state[14] + state[5]; - out_4420889553799640425[6] = state[6]; - out_4420889553799640425[7] = state[7]; - out_4420889553799640425[8] = state[8]; - out_4420889553799640425[9] = state[9]; - out_4420889553799640425[10] = state[10]; - out_4420889553799640425[11] = state[11]; - out_4420889553799640425[12] = state[12]; - out_4420889553799640425[13] = state[13]; - out_4420889553799640425[14] = state[14]; - out_4420889553799640425[15] = state[15]; - out_4420889553799640425[16] = state[16]; - out_4420889553799640425[17] = state[17]; +void f_fun(double *state, double dt, double *out_65737351960750703) { + out_65737351960750703[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_65737351960750703[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_65737351960750703[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_65737351960750703[3] = dt*state[12] + state[3]; + out_65737351960750703[4] = dt*state[13] + state[4]; + out_65737351960750703[5] = dt*state[14] + state[5]; + out_65737351960750703[6] = state[6]; + out_65737351960750703[7] = state[7]; + out_65737351960750703[8] = state[8]; + out_65737351960750703[9] = state[9]; + out_65737351960750703[10] = state[10]; + out_65737351960750703[11] = state[11]; + out_65737351960750703[12] = state[12]; + out_65737351960750703[13] = state[13]; + out_65737351960750703[14] = state[14]; + out_65737351960750703[15] = state[15]; + out_65737351960750703[16] = state[16]; + out_65737351960750703[17] = state[17]; } -void F_fun(double *state, double dt, double *out_586752082486093199) { - out_586752082486093199[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_586752082486093199[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_586752082486093199[2] = 0; - out_586752082486093199[3] = 0; - out_586752082486093199[4] = 0; - out_586752082486093199[5] = 0; - out_586752082486093199[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_586752082486093199[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_586752082486093199[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_586752082486093199[9] = 0; - out_586752082486093199[10] = 0; - out_586752082486093199[11] = 0; - out_586752082486093199[12] = 0; - out_586752082486093199[13] = 0; - out_586752082486093199[14] = 0; - out_586752082486093199[15] = 0; - out_586752082486093199[16] = 0; - out_586752082486093199[17] = 0; - out_586752082486093199[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_586752082486093199[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_586752082486093199[20] = 0; - out_586752082486093199[21] = 0; - out_586752082486093199[22] = 0; - out_586752082486093199[23] = 0; - out_586752082486093199[24] = 0; - out_586752082486093199[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_586752082486093199[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_586752082486093199[27] = 0; - out_586752082486093199[28] = 0; - out_586752082486093199[29] = 0; - out_586752082486093199[30] = 0; - out_586752082486093199[31] = 0; - out_586752082486093199[32] = 0; - out_586752082486093199[33] = 0; - out_586752082486093199[34] = 0; - out_586752082486093199[35] = 0; - out_586752082486093199[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_586752082486093199[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_586752082486093199[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_586752082486093199[39] = 0; - out_586752082486093199[40] = 0; - out_586752082486093199[41] = 0; - out_586752082486093199[42] = 0; - out_586752082486093199[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_586752082486093199[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_586752082486093199[45] = 0; - out_586752082486093199[46] = 0; - out_586752082486093199[47] = 0; - out_586752082486093199[48] = 0; - out_586752082486093199[49] = 0; - out_586752082486093199[50] = 0; - out_586752082486093199[51] = 0; - out_586752082486093199[52] = 0; - out_586752082486093199[53] = 0; - out_586752082486093199[54] = 0; - out_586752082486093199[55] = 0; - out_586752082486093199[56] = 0; - out_586752082486093199[57] = 1; - out_586752082486093199[58] = 0; - out_586752082486093199[59] = 0; - out_586752082486093199[60] = 0; - out_586752082486093199[61] = 0; - out_586752082486093199[62] = 0; - out_586752082486093199[63] = 0; - out_586752082486093199[64] = 0; - out_586752082486093199[65] = 0; - out_586752082486093199[66] = dt; - out_586752082486093199[67] = 0; - out_586752082486093199[68] = 0; - out_586752082486093199[69] = 0; - out_586752082486093199[70] = 0; - out_586752082486093199[71] = 0; - out_586752082486093199[72] = 0; - out_586752082486093199[73] = 0; - out_586752082486093199[74] = 0; - out_586752082486093199[75] = 0; - out_586752082486093199[76] = 1; - out_586752082486093199[77] = 0; - out_586752082486093199[78] = 0; - out_586752082486093199[79] = 0; - out_586752082486093199[80] = 0; - out_586752082486093199[81] = 0; - out_586752082486093199[82] = 0; - out_586752082486093199[83] = 0; - out_586752082486093199[84] = 0; - out_586752082486093199[85] = dt; - out_586752082486093199[86] = 0; - out_586752082486093199[87] = 0; - out_586752082486093199[88] = 0; - out_586752082486093199[89] = 0; - out_586752082486093199[90] = 0; - out_586752082486093199[91] = 0; - out_586752082486093199[92] = 0; - out_586752082486093199[93] = 0; - out_586752082486093199[94] = 0; - out_586752082486093199[95] = 1; - out_586752082486093199[96] = 0; - out_586752082486093199[97] = 0; - out_586752082486093199[98] = 0; - out_586752082486093199[99] = 0; - out_586752082486093199[100] = 0; - out_586752082486093199[101] = 0; - out_586752082486093199[102] = 0; - out_586752082486093199[103] = 0; - out_586752082486093199[104] = dt; - out_586752082486093199[105] = 0; - out_586752082486093199[106] = 0; - out_586752082486093199[107] = 0; - out_586752082486093199[108] = 0; - out_586752082486093199[109] = 0; - out_586752082486093199[110] = 0; - out_586752082486093199[111] = 0; - out_586752082486093199[112] = 0; - out_586752082486093199[113] = 0; - out_586752082486093199[114] = 1; - out_586752082486093199[115] = 0; - out_586752082486093199[116] = 0; - out_586752082486093199[117] = 0; - out_586752082486093199[118] = 0; - out_586752082486093199[119] = 0; - out_586752082486093199[120] = 0; - out_586752082486093199[121] = 0; - out_586752082486093199[122] = 0; - out_586752082486093199[123] = 0; - out_586752082486093199[124] = 0; - out_586752082486093199[125] = 0; - out_586752082486093199[126] = 0; - out_586752082486093199[127] = 0; - out_586752082486093199[128] = 0; - out_586752082486093199[129] = 0; - out_586752082486093199[130] = 0; - out_586752082486093199[131] = 0; - out_586752082486093199[132] = 0; - out_586752082486093199[133] = 1; - out_586752082486093199[134] = 0; - out_586752082486093199[135] = 0; - out_586752082486093199[136] = 0; - out_586752082486093199[137] = 0; - out_586752082486093199[138] = 0; - out_586752082486093199[139] = 0; - out_586752082486093199[140] = 0; - out_586752082486093199[141] = 0; - out_586752082486093199[142] = 0; - out_586752082486093199[143] = 0; - out_586752082486093199[144] = 0; - out_586752082486093199[145] = 0; - out_586752082486093199[146] = 0; - out_586752082486093199[147] = 0; - out_586752082486093199[148] = 0; - out_586752082486093199[149] = 0; - out_586752082486093199[150] = 0; - out_586752082486093199[151] = 0; - out_586752082486093199[152] = 1; - out_586752082486093199[153] = 0; - out_586752082486093199[154] = 0; - out_586752082486093199[155] = 0; - out_586752082486093199[156] = 0; - out_586752082486093199[157] = 0; - out_586752082486093199[158] = 0; - out_586752082486093199[159] = 0; - out_586752082486093199[160] = 0; - out_586752082486093199[161] = 0; - out_586752082486093199[162] = 0; - out_586752082486093199[163] = 0; - out_586752082486093199[164] = 0; - out_586752082486093199[165] = 0; - out_586752082486093199[166] = 0; - out_586752082486093199[167] = 0; - out_586752082486093199[168] = 0; - out_586752082486093199[169] = 0; - out_586752082486093199[170] = 0; - out_586752082486093199[171] = 1; - out_586752082486093199[172] = 0; - out_586752082486093199[173] = 0; - out_586752082486093199[174] = 0; - out_586752082486093199[175] = 0; - out_586752082486093199[176] = 0; - out_586752082486093199[177] = 0; - out_586752082486093199[178] = 0; - out_586752082486093199[179] = 0; - out_586752082486093199[180] = 0; - out_586752082486093199[181] = 0; - out_586752082486093199[182] = 0; - out_586752082486093199[183] = 0; - out_586752082486093199[184] = 0; - out_586752082486093199[185] = 0; - out_586752082486093199[186] = 0; - out_586752082486093199[187] = 0; - out_586752082486093199[188] = 0; - out_586752082486093199[189] = 0; - out_586752082486093199[190] = 1; - out_586752082486093199[191] = 0; - out_586752082486093199[192] = 0; - out_586752082486093199[193] = 0; - out_586752082486093199[194] = 0; - out_586752082486093199[195] = 0; - out_586752082486093199[196] = 0; - out_586752082486093199[197] = 0; - out_586752082486093199[198] = 0; - out_586752082486093199[199] = 0; - out_586752082486093199[200] = 0; - out_586752082486093199[201] = 0; - out_586752082486093199[202] = 0; - out_586752082486093199[203] = 0; - out_586752082486093199[204] = 0; - out_586752082486093199[205] = 0; - out_586752082486093199[206] = 0; - out_586752082486093199[207] = 0; - out_586752082486093199[208] = 0; - out_586752082486093199[209] = 1; - out_586752082486093199[210] = 0; - out_586752082486093199[211] = 0; - out_586752082486093199[212] = 0; - out_586752082486093199[213] = 0; - out_586752082486093199[214] = 0; - out_586752082486093199[215] = 0; - out_586752082486093199[216] = 0; - out_586752082486093199[217] = 0; - out_586752082486093199[218] = 0; - out_586752082486093199[219] = 0; - out_586752082486093199[220] = 0; - out_586752082486093199[221] = 0; - out_586752082486093199[222] = 0; - out_586752082486093199[223] = 0; - out_586752082486093199[224] = 0; - out_586752082486093199[225] = 0; - out_586752082486093199[226] = 0; - out_586752082486093199[227] = 0; - out_586752082486093199[228] = 1; - out_586752082486093199[229] = 0; - out_586752082486093199[230] = 0; - out_586752082486093199[231] = 0; - out_586752082486093199[232] = 0; - out_586752082486093199[233] = 0; - out_586752082486093199[234] = 0; - out_586752082486093199[235] = 0; - out_586752082486093199[236] = 0; - out_586752082486093199[237] = 0; - out_586752082486093199[238] = 0; - out_586752082486093199[239] = 0; - out_586752082486093199[240] = 0; - out_586752082486093199[241] = 0; - out_586752082486093199[242] = 0; - out_586752082486093199[243] = 0; - out_586752082486093199[244] = 0; - out_586752082486093199[245] = 0; - out_586752082486093199[246] = 0; - out_586752082486093199[247] = 1; - out_586752082486093199[248] = 0; - out_586752082486093199[249] = 0; - out_586752082486093199[250] = 0; - out_586752082486093199[251] = 0; - out_586752082486093199[252] = 0; - out_586752082486093199[253] = 0; - out_586752082486093199[254] = 0; - out_586752082486093199[255] = 0; - out_586752082486093199[256] = 0; - out_586752082486093199[257] = 0; - out_586752082486093199[258] = 0; - out_586752082486093199[259] = 0; - out_586752082486093199[260] = 0; - out_586752082486093199[261] = 0; - out_586752082486093199[262] = 0; - out_586752082486093199[263] = 0; - out_586752082486093199[264] = 0; - out_586752082486093199[265] = 0; - out_586752082486093199[266] = 1; - out_586752082486093199[267] = 0; - out_586752082486093199[268] = 0; - out_586752082486093199[269] = 0; - out_586752082486093199[270] = 0; - out_586752082486093199[271] = 0; - out_586752082486093199[272] = 0; - out_586752082486093199[273] = 0; - out_586752082486093199[274] = 0; - out_586752082486093199[275] = 0; - out_586752082486093199[276] = 0; - out_586752082486093199[277] = 0; - out_586752082486093199[278] = 0; - out_586752082486093199[279] = 0; - out_586752082486093199[280] = 0; - out_586752082486093199[281] = 0; - out_586752082486093199[282] = 0; - out_586752082486093199[283] = 0; - out_586752082486093199[284] = 0; - out_586752082486093199[285] = 1; - out_586752082486093199[286] = 0; - out_586752082486093199[287] = 0; - out_586752082486093199[288] = 0; - out_586752082486093199[289] = 0; - out_586752082486093199[290] = 0; - out_586752082486093199[291] = 0; - out_586752082486093199[292] = 0; - out_586752082486093199[293] = 0; - out_586752082486093199[294] = 0; - out_586752082486093199[295] = 0; - out_586752082486093199[296] = 0; - out_586752082486093199[297] = 0; - out_586752082486093199[298] = 0; - out_586752082486093199[299] = 0; - out_586752082486093199[300] = 0; - out_586752082486093199[301] = 0; - out_586752082486093199[302] = 0; - out_586752082486093199[303] = 0; - out_586752082486093199[304] = 1; - out_586752082486093199[305] = 0; - out_586752082486093199[306] = 0; - out_586752082486093199[307] = 0; - out_586752082486093199[308] = 0; - out_586752082486093199[309] = 0; - out_586752082486093199[310] = 0; - out_586752082486093199[311] = 0; - out_586752082486093199[312] = 0; - out_586752082486093199[313] = 0; - out_586752082486093199[314] = 0; - out_586752082486093199[315] = 0; - out_586752082486093199[316] = 0; - out_586752082486093199[317] = 0; - out_586752082486093199[318] = 0; - out_586752082486093199[319] = 0; - out_586752082486093199[320] = 0; - out_586752082486093199[321] = 0; - out_586752082486093199[322] = 0; - out_586752082486093199[323] = 1; +void F_fun(double *state, double dt, double *out_3304224532205862059) { + out_3304224532205862059[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_3304224532205862059[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_3304224532205862059[2] = 0; + out_3304224532205862059[3] = 0; + out_3304224532205862059[4] = 0; + out_3304224532205862059[5] = 0; + out_3304224532205862059[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_3304224532205862059[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_3304224532205862059[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_3304224532205862059[9] = 0; + out_3304224532205862059[10] = 0; + out_3304224532205862059[11] = 0; + out_3304224532205862059[12] = 0; + out_3304224532205862059[13] = 0; + out_3304224532205862059[14] = 0; + out_3304224532205862059[15] = 0; + out_3304224532205862059[16] = 0; + out_3304224532205862059[17] = 0; + out_3304224532205862059[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_3304224532205862059[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_3304224532205862059[20] = 0; + out_3304224532205862059[21] = 0; + out_3304224532205862059[22] = 0; + out_3304224532205862059[23] = 0; + out_3304224532205862059[24] = 0; + out_3304224532205862059[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_3304224532205862059[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_3304224532205862059[27] = 0; + out_3304224532205862059[28] = 0; + out_3304224532205862059[29] = 0; + out_3304224532205862059[30] = 0; + out_3304224532205862059[31] = 0; + out_3304224532205862059[32] = 0; + out_3304224532205862059[33] = 0; + out_3304224532205862059[34] = 0; + out_3304224532205862059[35] = 0; + out_3304224532205862059[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_3304224532205862059[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_3304224532205862059[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_3304224532205862059[39] = 0; + out_3304224532205862059[40] = 0; + out_3304224532205862059[41] = 0; + out_3304224532205862059[42] = 0; + out_3304224532205862059[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_3304224532205862059[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_3304224532205862059[45] = 0; + out_3304224532205862059[46] = 0; + out_3304224532205862059[47] = 0; + out_3304224532205862059[48] = 0; + out_3304224532205862059[49] = 0; + out_3304224532205862059[50] = 0; + out_3304224532205862059[51] = 0; + out_3304224532205862059[52] = 0; + out_3304224532205862059[53] = 0; + out_3304224532205862059[54] = 0; + out_3304224532205862059[55] = 0; + out_3304224532205862059[56] = 0; + out_3304224532205862059[57] = 1; + out_3304224532205862059[58] = 0; + out_3304224532205862059[59] = 0; + out_3304224532205862059[60] = 0; + out_3304224532205862059[61] = 0; + out_3304224532205862059[62] = 0; + out_3304224532205862059[63] = 0; + out_3304224532205862059[64] = 0; + out_3304224532205862059[65] = 0; + out_3304224532205862059[66] = dt; + out_3304224532205862059[67] = 0; + out_3304224532205862059[68] = 0; + out_3304224532205862059[69] = 0; + out_3304224532205862059[70] = 0; + out_3304224532205862059[71] = 0; + out_3304224532205862059[72] = 0; + out_3304224532205862059[73] = 0; + out_3304224532205862059[74] = 0; + out_3304224532205862059[75] = 0; + out_3304224532205862059[76] = 1; + out_3304224532205862059[77] = 0; + out_3304224532205862059[78] = 0; + out_3304224532205862059[79] = 0; + out_3304224532205862059[80] = 0; + out_3304224532205862059[81] = 0; + out_3304224532205862059[82] = 0; + out_3304224532205862059[83] = 0; + out_3304224532205862059[84] = 0; + out_3304224532205862059[85] = dt; + out_3304224532205862059[86] = 0; + out_3304224532205862059[87] = 0; + out_3304224532205862059[88] = 0; + out_3304224532205862059[89] = 0; + out_3304224532205862059[90] = 0; + out_3304224532205862059[91] = 0; + out_3304224532205862059[92] = 0; + out_3304224532205862059[93] = 0; + out_3304224532205862059[94] = 0; + out_3304224532205862059[95] = 1; + out_3304224532205862059[96] = 0; + out_3304224532205862059[97] = 0; + out_3304224532205862059[98] = 0; + out_3304224532205862059[99] = 0; + out_3304224532205862059[100] = 0; + out_3304224532205862059[101] = 0; + out_3304224532205862059[102] = 0; + out_3304224532205862059[103] = 0; + out_3304224532205862059[104] = dt; + out_3304224532205862059[105] = 0; + out_3304224532205862059[106] = 0; + out_3304224532205862059[107] = 0; + out_3304224532205862059[108] = 0; + out_3304224532205862059[109] = 0; + out_3304224532205862059[110] = 0; + out_3304224532205862059[111] = 0; + out_3304224532205862059[112] = 0; + out_3304224532205862059[113] = 0; + out_3304224532205862059[114] = 1; + out_3304224532205862059[115] = 0; + out_3304224532205862059[116] = 0; + out_3304224532205862059[117] = 0; + out_3304224532205862059[118] = 0; + out_3304224532205862059[119] = 0; + out_3304224532205862059[120] = 0; + out_3304224532205862059[121] = 0; + out_3304224532205862059[122] = 0; + out_3304224532205862059[123] = 0; + out_3304224532205862059[124] = 0; + out_3304224532205862059[125] = 0; + out_3304224532205862059[126] = 0; + out_3304224532205862059[127] = 0; + out_3304224532205862059[128] = 0; + out_3304224532205862059[129] = 0; + out_3304224532205862059[130] = 0; + out_3304224532205862059[131] = 0; + out_3304224532205862059[132] = 0; + out_3304224532205862059[133] = 1; + out_3304224532205862059[134] = 0; + out_3304224532205862059[135] = 0; + out_3304224532205862059[136] = 0; + out_3304224532205862059[137] = 0; + out_3304224532205862059[138] = 0; + out_3304224532205862059[139] = 0; + out_3304224532205862059[140] = 0; + out_3304224532205862059[141] = 0; + out_3304224532205862059[142] = 0; + out_3304224532205862059[143] = 0; + out_3304224532205862059[144] = 0; + out_3304224532205862059[145] = 0; + out_3304224532205862059[146] = 0; + out_3304224532205862059[147] = 0; + out_3304224532205862059[148] = 0; + out_3304224532205862059[149] = 0; + out_3304224532205862059[150] = 0; + out_3304224532205862059[151] = 0; + out_3304224532205862059[152] = 1; + out_3304224532205862059[153] = 0; + out_3304224532205862059[154] = 0; + out_3304224532205862059[155] = 0; + out_3304224532205862059[156] = 0; + out_3304224532205862059[157] = 0; + out_3304224532205862059[158] = 0; + out_3304224532205862059[159] = 0; + out_3304224532205862059[160] = 0; + out_3304224532205862059[161] = 0; + out_3304224532205862059[162] = 0; + out_3304224532205862059[163] = 0; + out_3304224532205862059[164] = 0; + out_3304224532205862059[165] = 0; + out_3304224532205862059[166] = 0; + out_3304224532205862059[167] = 0; + out_3304224532205862059[168] = 0; + out_3304224532205862059[169] = 0; + out_3304224532205862059[170] = 0; + out_3304224532205862059[171] = 1; + out_3304224532205862059[172] = 0; + out_3304224532205862059[173] = 0; + out_3304224532205862059[174] = 0; + out_3304224532205862059[175] = 0; + out_3304224532205862059[176] = 0; + out_3304224532205862059[177] = 0; + out_3304224532205862059[178] = 0; + out_3304224532205862059[179] = 0; + out_3304224532205862059[180] = 0; + out_3304224532205862059[181] = 0; + out_3304224532205862059[182] = 0; + out_3304224532205862059[183] = 0; + out_3304224532205862059[184] = 0; + out_3304224532205862059[185] = 0; + out_3304224532205862059[186] = 0; + out_3304224532205862059[187] = 0; + out_3304224532205862059[188] = 0; + out_3304224532205862059[189] = 0; + out_3304224532205862059[190] = 1; + out_3304224532205862059[191] = 0; + out_3304224532205862059[192] = 0; + out_3304224532205862059[193] = 0; + out_3304224532205862059[194] = 0; + out_3304224532205862059[195] = 0; + out_3304224532205862059[196] = 0; + out_3304224532205862059[197] = 0; + out_3304224532205862059[198] = 0; + out_3304224532205862059[199] = 0; + out_3304224532205862059[200] = 0; + out_3304224532205862059[201] = 0; + out_3304224532205862059[202] = 0; + out_3304224532205862059[203] = 0; + out_3304224532205862059[204] = 0; + out_3304224532205862059[205] = 0; + out_3304224532205862059[206] = 0; + out_3304224532205862059[207] = 0; + out_3304224532205862059[208] = 0; + out_3304224532205862059[209] = 1; + out_3304224532205862059[210] = 0; + out_3304224532205862059[211] = 0; + out_3304224532205862059[212] = 0; + out_3304224532205862059[213] = 0; + out_3304224532205862059[214] = 0; + out_3304224532205862059[215] = 0; + out_3304224532205862059[216] = 0; + out_3304224532205862059[217] = 0; + out_3304224532205862059[218] = 0; + out_3304224532205862059[219] = 0; + out_3304224532205862059[220] = 0; + out_3304224532205862059[221] = 0; + out_3304224532205862059[222] = 0; + out_3304224532205862059[223] = 0; + out_3304224532205862059[224] = 0; + out_3304224532205862059[225] = 0; + out_3304224532205862059[226] = 0; + out_3304224532205862059[227] = 0; + out_3304224532205862059[228] = 1; + out_3304224532205862059[229] = 0; + out_3304224532205862059[230] = 0; + out_3304224532205862059[231] = 0; + out_3304224532205862059[232] = 0; + out_3304224532205862059[233] = 0; + out_3304224532205862059[234] = 0; + out_3304224532205862059[235] = 0; + out_3304224532205862059[236] = 0; + out_3304224532205862059[237] = 0; + out_3304224532205862059[238] = 0; + out_3304224532205862059[239] = 0; + out_3304224532205862059[240] = 0; + out_3304224532205862059[241] = 0; + out_3304224532205862059[242] = 0; + out_3304224532205862059[243] = 0; + out_3304224532205862059[244] = 0; + out_3304224532205862059[245] = 0; + out_3304224532205862059[246] = 0; + out_3304224532205862059[247] = 1; + out_3304224532205862059[248] = 0; + out_3304224532205862059[249] = 0; + out_3304224532205862059[250] = 0; + out_3304224532205862059[251] = 0; + out_3304224532205862059[252] = 0; + out_3304224532205862059[253] = 0; + out_3304224532205862059[254] = 0; + out_3304224532205862059[255] = 0; + out_3304224532205862059[256] = 0; + out_3304224532205862059[257] = 0; + out_3304224532205862059[258] = 0; + out_3304224532205862059[259] = 0; + out_3304224532205862059[260] = 0; + out_3304224532205862059[261] = 0; + out_3304224532205862059[262] = 0; + out_3304224532205862059[263] = 0; + out_3304224532205862059[264] = 0; + out_3304224532205862059[265] = 0; + out_3304224532205862059[266] = 1; + out_3304224532205862059[267] = 0; + out_3304224532205862059[268] = 0; + out_3304224532205862059[269] = 0; + out_3304224532205862059[270] = 0; + out_3304224532205862059[271] = 0; + out_3304224532205862059[272] = 0; + out_3304224532205862059[273] = 0; + out_3304224532205862059[274] = 0; + out_3304224532205862059[275] = 0; + out_3304224532205862059[276] = 0; + out_3304224532205862059[277] = 0; + out_3304224532205862059[278] = 0; + out_3304224532205862059[279] = 0; + out_3304224532205862059[280] = 0; + out_3304224532205862059[281] = 0; + out_3304224532205862059[282] = 0; + out_3304224532205862059[283] = 0; + out_3304224532205862059[284] = 0; + out_3304224532205862059[285] = 1; + out_3304224532205862059[286] = 0; + out_3304224532205862059[287] = 0; + out_3304224532205862059[288] = 0; + out_3304224532205862059[289] = 0; + out_3304224532205862059[290] = 0; + out_3304224532205862059[291] = 0; + out_3304224532205862059[292] = 0; + out_3304224532205862059[293] = 0; + out_3304224532205862059[294] = 0; + out_3304224532205862059[295] = 0; + out_3304224532205862059[296] = 0; + out_3304224532205862059[297] = 0; + out_3304224532205862059[298] = 0; + out_3304224532205862059[299] = 0; + out_3304224532205862059[300] = 0; + out_3304224532205862059[301] = 0; + out_3304224532205862059[302] = 0; + out_3304224532205862059[303] = 0; + out_3304224532205862059[304] = 1; + out_3304224532205862059[305] = 0; + out_3304224532205862059[306] = 0; + out_3304224532205862059[307] = 0; + out_3304224532205862059[308] = 0; + out_3304224532205862059[309] = 0; + out_3304224532205862059[310] = 0; + out_3304224532205862059[311] = 0; + out_3304224532205862059[312] = 0; + out_3304224532205862059[313] = 0; + out_3304224532205862059[314] = 0; + out_3304224532205862059[315] = 0; + out_3304224532205862059[316] = 0; + out_3304224532205862059[317] = 0; + out_3304224532205862059[318] = 0; + out_3304224532205862059[319] = 0; + out_3304224532205862059[320] = 0; + out_3304224532205862059[321] = 0; + out_3304224532205862059[322] = 0; + out_3304224532205862059[323] = 1; } -void h_4(double *state, double *unused, double *out_1330626957984611772) { - out_1330626957984611772[0] = state[6] + state[9]; - out_1330626957984611772[1] = state[7] + state[10]; - out_1330626957984611772[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_5378292505758393229) { + out_5378292505758393229[0] = state[6] + state[9]; + out_5378292505758393229[1] = state[7] + state[10]; + out_5378292505758393229[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_8068529884540655173) { - out_8068529884540655173[0] = 0; - out_8068529884540655173[1] = 0; - out_8068529884540655173[2] = 0; - out_8068529884540655173[3] = 0; - out_8068529884540655173[4] = 0; - out_8068529884540655173[5] = 0; - out_8068529884540655173[6] = 1; - out_8068529884540655173[7] = 0; - out_8068529884540655173[8] = 0; - out_8068529884540655173[9] = 1; - out_8068529884540655173[10] = 0; - out_8068529884540655173[11] = 0; - out_8068529884540655173[12] = 0; - out_8068529884540655173[13] = 0; - out_8068529884540655173[14] = 0; - out_8068529884540655173[15] = 0; - out_8068529884540655173[16] = 0; - out_8068529884540655173[17] = 0; - out_8068529884540655173[18] = 0; - out_8068529884540655173[19] = 0; - out_8068529884540655173[20] = 0; - out_8068529884540655173[21] = 0; - out_8068529884540655173[22] = 0; - out_8068529884540655173[23] = 0; - out_8068529884540655173[24] = 0; - out_8068529884540655173[25] = 1; - out_8068529884540655173[26] = 0; - out_8068529884540655173[27] = 0; - out_8068529884540655173[28] = 1; - out_8068529884540655173[29] = 0; - out_8068529884540655173[30] = 0; - out_8068529884540655173[31] = 0; - out_8068529884540655173[32] = 0; - out_8068529884540655173[33] = 0; - out_8068529884540655173[34] = 0; - out_8068529884540655173[35] = 0; - out_8068529884540655173[36] = 0; - out_8068529884540655173[37] = 0; - out_8068529884540655173[38] = 0; - out_8068529884540655173[39] = 0; - out_8068529884540655173[40] = 0; - out_8068529884540655173[41] = 0; - out_8068529884540655173[42] = 0; - out_8068529884540655173[43] = 0; - out_8068529884540655173[44] = 1; - out_8068529884540655173[45] = 0; - out_8068529884540655173[46] = 0; - out_8068529884540655173[47] = 1; - out_8068529884540655173[48] = 0; - out_8068529884540655173[49] = 0; - out_8068529884540655173[50] = 0; - out_8068529884540655173[51] = 0; - out_8068529884540655173[52] = 0; - out_8068529884540655173[53] = 0; +void H_4(double *state, double *unused, double *out_2130036104531011967) { + out_2130036104531011967[0] = 0; + out_2130036104531011967[1] = 0; + out_2130036104531011967[2] = 0; + out_2130036104531011967[3] = 0; + out_2130036104531011967[4] = 0; + out_2130036104531011967[5] = 0; + out_2130036104531011967[6] = 1; + out_2130036104531011967[7] = 0; + out_2130036104531011967[8] = 0; + out_2130036104531011967[9] = 1; + out_2130036104531011967[10] = 0; + out_2130036104531011967[11] = 0; + out_2130036104531011967[12] = 0; + out_2130036104531011967[13] = 0; + out_2130036104531011967[14] = 0; + out_2130036104531011967[15] = 0; + out_2130036104531011967[16] = 0; + out_2130036104531011967[17] = 0; + out_2130036104531011967[18] = 0; + out_2130036104531011967[19] = 0; + out_2130036104531011967[20] = 0; + out_2130036104531011967[21] = 0; + out_2130036104531011967[22] = 0; + out_2130036104531011967[23] = 0; + out_2130036104531011967[24] = 0; + out_2130036104531011967[25] = 1; + out_2130036104531011967[26] = 0; + out_2130036104531011967[27] = 0; + out_2130036104531011967[28] = 1; + out_2130036104531011967[29] = 0; + out_2130036104531011967[30] = 0; + out_2130036104531011967[31] = 0; + out_2130036104531011967[32] = 0; + out_2130036104531011967[33] = 0; + out_2130036104531011967[34] = 0; + out_2130036104531011967[35] = 0; + out_2130036104531011967[36] = 0; + out_2130036104531011967[37] = 0; + out_2130036104531011967[38] = 0; + out_2130036104531011967[39] = 0; + out_2130036104531011967[40] = 0; + out_2130036104531011967[41] = 0; + out_2130036104531011967[42] = 0; + out_2130036104531011967[43] = 0; + out_2130036104531011967[44] = 1; + out_2130036104531011967[45] = 0; + out_2130036104531011967[46] = 0; + out_2130036104531011967[47] = 1; + out_2130036104531011967[48] = 0; + out_2130036104531011967[49] = 0; + out_2130036104531011967[50] = 0; + out_2130036104531011967[51] = 0; + out_2130036104531011967[52] = 0; + out_2130036104531011967[53] = 0; } -void h_10(double *state, double *unused, double *out_5440536108172962293) { - out_5440536108172962293[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_5440536108172962293[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_5440536108172962293[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_2124950356332181460) { + out_2124950356332181460[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_2124950356332181460[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_2124950356332181460[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_3777395375513848052) { - out_3777395375513848052[0] = 0; - out_3777395375513848052[1] = 9.8100000000000005*cos(state[1]); - out_3777395375513848052[2] = 0; - out_3777395375513848052[3] = 0; - out_3777395375513848052[4] = -state[8]; - out_3777395375513848052[5] = state[7]; - out_3777395375513848052[6] = 0; - out_3777395375513848052[7] = state[5]; - out_3777395375513848052[8] = -state[4]; - out_3777395375513848052[9] = 0; - out_3777395375513848052[10] = 0; - out_3777395375513848052[11] = 0; - out_3777395375513848052[12] = 1; - out_3777395375513848052[13] = 0; - out_3777395375513848052[14] = 0; - out_3777395375513848052[15] = 1; - out_3777395375513848052[16] = 0; - out_3777395375513848052[17] = 0; - out_3777395375513848052[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_3777395375513848052[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_3777395375513848052[20] = 0; - out_3777395375513848052[21] = state[8]; - out_3777395375513848052[22] = 0; - out_3777395375513848052[23] = -state[6]; - out_3777395375513848052[24] = -state[5]; - out_3777395375513848052[25] = 0; - out_3777395375513848052[26] = state[3]; - out_3777395375513848052[27] = 0; - out_3777395375513848052[28] = 0; - out_3777395375513848052[29] = 0; - out_3777395375513848052[30] = 0; - out_3777395375513848052[31] = 1; - out_3777395375513848052[32] = 0; - out_3777395375513848052[33] = 0; - out_3777395375513848052[34] = 1; - out_3777395375513848052[35] = 0; - out_3777395375513848052[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_3777395375513848052[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_3777395375513848052[38] = 0; - out_3777395375513848052[39] = -state[7]; - out_3777395375513848052[40] = state[6]; - out_3777395375513848052[41] = 0; - out_3777395375513848052[42] = state[4]; - out_3777395375513848052[43] = -state[3]; - out_3777395375513848052[44] = 0; - out_3777395375513848052[45] = 0; - out_3777395375513848052[46] = 0; - out_3777395375513848052[47] = 0; - out_3777395375513848052[48] = 0; - out_3777395375513848052[49] = 0; - out_3777395375513848052[50] = 1; - out_3777395375513848052[51] = 0; - out_3777395375513848052[52] = 0; - out_3777395375513848052[53] = 1; +void H_10(double *state, double *unused, double *out_8589748122456887360) { + out_8589748122456887360[0] = 0; + out_8589748122456887360[1] = 9.8100000000000005*cos(state[1]); + out_8589748122456887360[2] = 0; + out_8589748122456887360[3] = 0; + out_8589748122456887360[4] = -state[8]; + out_8589748122456887360[5] = state[7]; + out_8589748122456887360[6] = 0; + out_8589748122456887360[7] = state[5]; + out_8589748122456887360[8] = -state[4]; + out_8589748122456887360[9] = 0; + out_8589748122456887360[10] = 0; + out_8589748122456887360[11] = 0; + out_8589748122456887360[12] = 1; + out_8589748122456887360[13] = 0; + out_8589748122456887360[14] = 0; + out_8589748122456887360[15] = 1; + out_8589748122456887360[16] = 0; + out_8589748122456887360[17] = 0; + out_8589748122456887360[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_8589748122456887360[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_8589748122456887360[20] = 0; + out_8589748122456887360[21] = state[8]; + out_8589748122456887360[22] = 0; + out_8589748122456887360[23] = -state[6]; + out_8589748122456887360[24] = -state[5]; + out_8589748122456887360[25] = 0; + out_8589748122456887360[26] = state[3]; + out_8589748122456887360[27] = 0; + out_8589748122456887360[28] = 0; + out_8589748122456887360[29] = 0; + out_8589748122456887360[30] = 0; + out_8589748122456887360[31] = 1; + out_8589748122456887360[32] = 0; + out_8589748122456887360[33] = 0; + out_8589748122456887360[34] = 1; + out_8589748122456887360[35] = 0; + out_8589748122456887360[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_8589748122456887360[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_8589748122456887360[38] = 0; + out_8589748122456887360[39] = -state[7]; + out_8589748122456887360[40] = state[6]; + out_8589748122456887360[41] = 0; + out_8589748122456887360[42] = state[4]; + out_8589748122456887360[43] = -state[3]; + out_8589748122456887360[44] = 0; + out_8589748122456887360[45] = 0; + out_8589748122456887360[46] = 0; + out_8589748122456887360[47] = 0; + out_8589748122456887360[48] = 0; + out_8589748122456887360[49] = 0; + out_8589748122456887360[50] = 1; + out_8589748122456887360[51] = 0; + out_8589748122456887360[52] = 0; + out_8589748122456887360[53] = 1; } -void h_13(double *state, double *unused, double *out_7911476404081815631) { - out_7911476404081815631[0] = state[3]; - out_7911476404081815631[1] = state[4]; - out_7911476404081815631[2] = state[5]; +void h_13(double *state, double *unused, double *out_7381262973170721868) { + out_7381262973170721868[0] = state[3]; + out_7381262973170721868[1] = state[4]; + out_7381262973170721868[2] = state[5]; } -void H_13(double *state, double *unused, double *out_7165940363836563642) { - out_7165940363836563642[0] = 0; - out_7165940363836563642[1] = 0; - out_7165940363836563642[2] = 0; - out_7165940363836563642[3] = 1; - out_7165940363836563642[4] = 0; - out_7165940363836563642[5] = 0; - out_7165940363836563642[6] = 0; - out_7165940363836563642[7] = 0; - out_7165940363836563642[8] = 0; - out_7165940363836563642[9] = 0; - out_7165940363836563642[10] = 0; - out_7165940363836563642[11] = 0; - out_7165940363836563642[12] = 0; - out_7165940363836563642[13] = 0; - out_7165940363836563642[14] = 0; - out_7165940363836563642[15] = 0; - out_7165940363836563642[16] = 0; - out_7165940363836563642[17] = 0; - out_7165940363836563642[18] = 0; - out_7165940363836563642[19] = 0; - out_7165940363836563642[20] = 0; - out_7165940363836563642[21] = 0; - out_7165940363836563642[22] = 1; - out_7165940363836563642[23] = 0; - out_7165940363836563642[24] = 0; - out_7165940363836563642[25] = 0; - out_7165940363836563642[26] = 0; - out_7165940363836563642[27] = 0; - out_7165940363836563642[28] = 0; - out_7165940363836563642[29] = 0; - out_7165940363836563642[30] = 0; - out_7165940363836563642[31] = 0; - out_7165940363836563642[32] = 0; - out_7165940363836563642[33] = 0; - out_7165940363836563642[34] = 0; - out_7165940363836563642[35] = 0; - out_7165940363836563642[36] = 0; - out_7165940363836563642[37] = 0; - out_7165940363836563642[38] = 0; - out_7165940363836563642[39] = 0; - out_7165940363836563642[40] = 0; - out_7165940363836563642[41] = 1; - out_7165940363836563642[42] = 0; - out_7165940363836563642[43] = 0; - out_7165940363836563642[44] = 0; - out_7165940363836563642[45] = 0; - out_7165940363836563642[46] = 0; - out_7165940363836563642[47] = 0; - out_7165940363836563642[48] = 0; - out_7165940363836563642[49] = 0; - out_7165940363836563642[50] = 0; - out_7165940363836563642[51] = 0; - out_7165940363836563642[52] = 0; - out_7165940363836563642[53] = 0; +void H_13(double *state, double *unused, double *out_5342309929863344768) { + out_5342309929863344768[0] = 0; + out_5342309929863344768[1] = 0; + out_5342309929863344768[2] = 0; + out_5342309929863344768[3] = 1; + out_5342309929863344768[4] = 0; + out_5342309929863344768[5] = 0; + out_5342309929863344768[6] = 0; + out_5342309929863344768[7] = 0; + out_5342309929863344768[8] = 0; + out_5342309929863344768[9] = 0; + out_5342309929863344768[10] = 0; + out_5342309929863344768[11] = 0; + out_5342309929863344768[12] = 0; + out_5342309929863344768[13] = 0; + out_5342309929863344768[14] = 0; + out_5342309929863344768[15] = 0; + out_5342309929863344768[16] = 0; + out_5342309929863344768[17] = 0; + out_5342309929863344768[18] = 0; + out_5342309929863344768[19] = 0; + out_5342309929863344768[20] = 0; + out_5342309929863344768[21] = 0; + out_5342309929863344768[22] = 1; + out_5342309929863344768[23] = 0; + out_5342309929863344768[24] = 0; + out_5342309929863344768[25] = 0; + out_5342309929863344768[26] = 0; + out_5342309929863344768[27] = 0; + out_5342309929863344768[28] = 0; + out_5342309929863344768[29] = 0; + out_5342309929863344768[30] = 0; + out_5342309929863344768[31] = 0; + out_5342309929863344768[32] = 0; + out_5342309929863344768[33] = 0; + out_5342309929863344768[34] = 0; + out_5342309929863344768[35] = 0; + out_5342309929863344768[36] = 0; + out_5342309929863344768[37] = 0; + out_5342309929863344768[38] = 0; + out_5342309929863344768[39] = 0; + out_5342309929863344768[40] = 0; + out_5342309929863344768[41] = 1; + out_5342309929863344768[42] = 0; + out_5342309929863344768[43] = 0; + out_5342309929863344768[44] = 0; + out_5342309929863344768[45] = 0; + out_5342309929863344768[46] = 0; + out_5342309929863344768[47] = 0; + out_5342309929863344768[48] = 0; + out_5342309929863344768[49] = 0; + out_5342309929863344768[50] = 0; + out_5342309929863344768[51] = 0; + out_5342309929863344768[52] = 0; + out_5342309929863344768[53] = 0; } -void h_14(double *state, double *unused, double *out_5129805537420960347) { - out_5129805537420960347[0] = state[6]; - out_5129805537420960347[1] = state[7]; - out_5129805537420960347[2] = state[8]; +void h_14(double *state, double *unused, double *out_2697166449345768413) { + out_2697166449345768413[0] = state[6]; + out_2697166449345768413[1] = state[7]; + out_2697166449345768413[2] = state[8]; } -void H_14(double *state, double *unused, double *out_4985741452245282877) { - out_4985741452245282877[0] = 0; - out_4985741452245282877[1] = 0; - out_4985741452245282877[2] = 0; - out_4985741452245282877[3] = 0; - out_4985741452245282877[4] = 0; - out_4985741452245282877[5] = 0; - out_4985741452245282877[6] = 1; - out_4985741452245282877[7] = 0; - out_4985741452245282877[8] = 0; - out_4985741452245282877[9] = 0; - out_4985741452245282877[10] = 0; - out_4985741452245282877[11] = 0; - out_4985741452245282877[12] = 0; - out_4985741452245282877[13] = 0; - out_4985741452245282877[14] = 0; - out_4985741452245282877[15] = 0; - out_4985741452245282877[16] = 0; - out_4985741452245282877[17] = 0; - out_4985741452245282877[18] = 0; - out_4985741452245282877[19] = 0; - out_4985741452245282877[20] = 0; - out_4985741452245282877[21] = 0; - out_4985741452245282877[22] = 0; - out_4985741452245282877[23] = 0; - out_4985741452245282877[24] = 0; - out_4985741452245282877[25] = 1; - out_4985741452245282877[26] = 0; - out_4985741452245282877[27] = 0; - out_4985741452245282877[28] = 0; - out_4985741452245282877[29] = 0; - out_4985741452245282877[30] = 0; - out_4985741452245282877[31] = 0; - out_4985741452245282877[32] = 0; - out_4985741452245282877[33] = 0; - out_4985741452245282877[34] = 0; - out_4985741452245282877[35] = 0; - out_4985741452245282877[36] = 0; - out_4985741452245282877[37] = 0; - out_4985741452245282877[38] = 0; - out_4985741452245282877[39] = 0; - out_4985741452245282877[40] = 0; - out_4985741452245282877[41] = 0; - out_4985741452245282877[42] = 0; - out_4985741452245282877[43] = 0; - out_4985741452245282877[44] = 1; - out_4985741452245282877[45] = 0; - out_4985741452245282877[46] = 0; - out_4985741452245282877[47] = 0; - out_4985741452245282877[48] = 0; - out_4985741452245282877[49] = 0; - out_4985741452245282877[50] = 0; - out_4985741452245282877[51] = 0; - out_4985741452245282877[52] = 0; - out_4985741452245282877[53] = 0; +void H_14(double *state, double *unused, double *out_952752327764360329) { + out_952752327764360329[0] = 0; + out_952752327764360329[1] = 0; + out_952752327764360329[2] = 0; + out_952752327764360329[3] = 0; + out_952752327764360329[4] = 0; + out_952752327764360329[5] = 0; + out_952752327764360329[6] = 1; + out_952752327764360329[7] = 0; + out_952752327764360329[8] = 0; + out_952752327764360329[9] = 0; + out_952752327764360329[10] = 0; + out_952752327764360329[11] = 0; + out_952752327764360329[12] = 0; + out_952752327764360329[13] = 0; + out_952752327764360329[14] = 0; + out_952752327764360329[15] = 0; + out_952752327764360329[16] = 0; + out_952752327764360329[17] = 0; + out_952752327764360329[18] = 0; + out_952752327764360329[19] = 0; + out_952752327764360329[20] = 0; + out_952752327764360329[21] = 0; + out_952752327764360329[22] = 0; + out_952752327764360329[23] = 0; + out_952752327764360329[24] = 0; + out_952752327764360329[25] = 1; + out_952752327764360329[26] = 0; + out_952752327764360329[27] = 0; + out_952752327764360329[28] = 0; + out_952752327764360329[29] = 0; + out_952752327764360329[30] = 0; + out_952752327764360329[31] = 0; + out_952752327764360329[32] = 0; + out_952752327764360329[33] = 0; + out_952752327764360329[34] = 0; + out_952752327764360329[35] = 0; + out_952752327764360329[36] = 0; + out_952752327764360329[37] = 0; + out_952752327764360329[38] = 0; + out_952752327764360329[39] = 0; + out_952752327764360329[40] = 0; + out_952752327764360329[41] = 0; + out_952752327764360329[42] = 0; + out_952752327764360329[43] = 0; + out_952752327764360329[44] = 1; + out_952752327764360329[45] = 0; + out_952752327764360329[46] = 0; + out_952752327764360329[47] = 0; + out_952752327764360329[48] = 0; + out_952752327764360329[49] = 0; + out_952752327764360329[50] = 0; + out_952752327764360329[51] = 0; + out_952752327764360329[52] = 0; + out_952752327764360329[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_731387772078245243) { - err_fun(nom_x, delta_x, out_731387772078245243); +void pose_err_fun(double *nom_x, double *delta_x, double *out_5549195789352376606) { + err_fun(nom_x, delta_x, out_5549195789352376606); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_4206130707332622452) { - inv_err_fun(nom_x, true_x, out_4206130707332622452); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6348441241420563996) { + inv_err_fun(nom_x, true_x, out_6348441241420563996); } -void pose_H_mod_fun(double *state, double *out_2916011499501574438) { - H_mod_fun(state, out_2916011499501574438); +void pose_H_mod_fun(double *state, double *out_932088622648113863) { + H_mod_fun(state, out_932088622648113863); } -void pose_f_fun(double *state, double dt, double *out_4420889553799640425) { - f_fun(state, dt, out_4420889553799640425); +void pose_f_fun(double *state, double dt, double *out_65737351960750703) { + f_fun(state, dt, out_65737351960750703); } -void pose_F_fun(double *state, double dt, double *out_586752082486093199) { - F_fun(state, dt, out_586752082486093199); +void pose_F_fun(double *state, double dt, double *out_3304224532205862059) { + F_fun(state, dt, out_3304224532205862059); } -void pose_h_4(double *state, double *unused, double *out_1330626957984611772) { - h_4(state, unused, out_1330626957984611772); +void pose_h_4(double *state, double *unused, double *out_5378292505758393229) { + h_4(state, unused, out_5378292505758393229); } -void pose_H_4(double *state, double *unused, double *out_8068529884540655173) { - H_4(state, unused, out_8068529884540655173); +void pose_H_4(double *state, double *unused, double *out_2130036104531011967) { + H_4(state, unused, out_2130036104531011967); } -void pose_h_10(double *state, double *unused, double *out_5440536108172962293) { - h_10(state, unused, out_5440536108172962293); +void pose_h_10(double *state, double *unused, double *out_2124950356332181460) { + h_10(state, unused, out_2124950356332181460); } -void pose_H_10(double *state, double *unused, double *out_3777395375513848052) { - H_10(state, unused, out_3777395375513848052); +void pose_H_10(double *state, double *unused, double *out_8589748122456887360) { + H_10(state, unused, out_8589748122456887360); } -void pose_h_13(double *state, double *unused, double *out_7911476404081815631) { - h_13(state, unused, out_7911476404081815631); +void pose_h_13(double *state, double *unused, double *out_7381262973170721868) { + h_13(state, unused, out_7381262973170721868); } -void pose_H_13(double *state, double *unused, double *out_7165940363836563642) { - H_13(state, unused, out_7165940363836563642); +void pose_H_13(double *state, double *unused, double *out_5342309929863344768) { + H_13(state, unused, out_5342309929863344768); } -void pose_h_14(double *state, double *unused, double *out_5129805537420960347) { - h_14(state, unused, out_5129805537420960347); +void pose_h_14(double *state, double *unused, double *out_2697166449345768413) { + h_14(state, unused, out_2697166449345768413); } -void pose_H_14(double *state, double *unused, double *out_4985741452245282877) { - H_14(state, unused, out_4985741452245282877); +void pose_H_14(double *state, double *unused, double *out_952752327764360329) { + H_14(state, unused, out_952752327764360329); } 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 98c89066d0..47245cf5a9 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_731387772078245243); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_4206130707332622452); -void pose_H_mod_fun(double *state, double *out_2916011499501574438); -void pose_f_fun(double *state, double dt, double *out_4420889553799640425); -void pose_F_fun(double *state, double dt, double *out_586752082486093199); -void pose_h_4(double *state, double *unused, double *out_1330626957984611772); -void pose_H_4(double *state, double *unused, double *out_8068529884540655173); -void pose_h_10(double *state, double *unused, double *out_5440536108172962293); -void pose_H_10(double *state, double *unused, double *out_3777395375513848052); -void pose_h_13(double *state, double *unused, double *out_7911476404081815631); -void pose_H_13(double *state, double *unused, double *out_7165940363836563642); -void pose_h_14(double *state, double *unused, double *out_5129805537420960347); -void pose_H_14(double *state, double *unused, double *out_4985741452245282877); +void pose_err_fun(double *nom_x, double *delta_x, double *out_5549195789352376606); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6348441241420563996); +void pose_H_mod_fun(double *state, double *out_932088622648113863); +void pose_f_fun(double *state, double dt, double *out_65737351960750703); +void pose_F_fun(double *state, double dt, double *out_3304224532205862059); +void pose_h_4(double *state, double *unused, double *out_5378292505758393229); +void pose_H_4(double *state, double *unused, double *out_2130036104531011967); +void pose_h_10(double *state, double *unused, double *out_2124950356332181460); +void pose_H_10(double *state, double *unused, double *out_8589748122456887360); +void pose_h_13(double *state, double *unused, double *out_7381262973170721868); +void pose_H_13(double *state, double *unused, double *out_5342309929863344768); +void pose_h_14(double *state, double *unused, double *out_2697166449345768413); +void pose_H_14(double *state, double *unused, double *out_952752327764360329); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/pose.os b/selfdrive/locationd/models/generated/pose.os index b9449ef3d7..d32b2d3bba 100644 Binary files a/selfdrive/locationd/models/generated/pose.os and b/selfdrive/locationd/models/generated/pose.os differ diff --git a/selfdrive/pandad/pandad b/selfdrive/pandad/pandad index a8cd14249c..ab9dc2c82a 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/system/camerad/camerad b/system/camerad/camerad index ca9381f056..70cdb18604 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index b79360c1ed..9b9dbcaa88 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index 3730355887..54294b21e2 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index 372fe1445b..8f9c4e9db6 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ