diff --git a/common/libcommon.a b/common/libcommon.a index 7fe60755ef..6e5485c32d 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 81c4a8fef8..405636011c 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 c9025a1ac8..03e83b06b6 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 44775f8e5e..b061a0ce06 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 de13e50896..6bb18dd107 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 0f1cc09650..b6bcfb080f 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 afbb127daf..6733227514 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 c76b438ee3..0f4ac97891 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 c76b438ee3..0f4ac97891 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 ed656ffadb..0da375e03a 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 ed656ffadb..0da375e03a 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 ed656ffadb..0da375e03a 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 ed656ffadb..0da375e03a 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 ed656ffadb..0da375e03a 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 ed656ffadb..0da375e03a 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 ed656ffadb..0da375e03a 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 ed656ffadb..0da375e03a 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 c76b438ee3..0f4ac97891 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 c76b438ee3..0f4ac97891 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 71ccfb275c..5ed3e620bb 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 c76b438ee3..0f4ac97891 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 c76b438ee3..0f4ac97891 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 c76b438ee3..0f4ac97891 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 c76b438ee3..0f4ac97891 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 5a39d4173f..6332241d90 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-20ff7931-DEBUG"; +const uint8_t gitversion[19] = "DEV-dc492787-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index a24d18fd01..ba3e062926 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 8adb8bb443..c3562406d8 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 ba40a23fef..39b915e2eb 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 ffb2eca4ee..913d10f2b1 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 46f13b0db3..99816994a3 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 ffd2929ca1..b6c341be12 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 f97a7cc681..45e056da5f 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 fba8100ced..1b627f6e50 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 0be86812ce..06a3947efc 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 90295cd004..6adee07557 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 7dbc75f329..35ea221bdb 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 116e35e61f..dcb6505de8 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 7e060d45ff..23b46e7c92 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 e330e35e95..374118a516 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 583ce2a8fb..702833df7a 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 b27b8091c9..6339f9f164 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 6dde544d8a..8bb253b4cd 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 c101741a4e..a3c72f108f 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 b1585166d4..cb0dd2690b 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 569c9a9eab..d10be897d5 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 b731618037..e72596e36a 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 28a2a7a24f..f2811df552 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 f452011aa4..61db426790 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 4cdff29d93..bc99e4ff76 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 639ea08241..2a39d39f3d 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 48605c29ee..0aa4e9c0eb 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 979179d221..380de287b8 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 b6f680c38c..f9660ee68c 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 3ad07d601c..a11bb4b208 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 c941383b64..f7474183f5 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 8be3d7bc76..8f99439791 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 113c212d54..099196698b 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 a07e429f30..c8c94087de 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 8cbb1db54e..8d22dbbd29 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 a1ed0375bf..f67248f2eb 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 5eb8432567..1dc31b2341 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 ecbda34eb2..923b2ab860 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 454cfe7d11..d3d94be0b0 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 08630c0271..26bc9405a1 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 f41af7ecab..16a531e6d6 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 af150af836..b0dd321dfd 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 ccd8d211aa..7e7a5281a7 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 95eeb89756..0130318053 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 82481cb44a..0ef7cdbd8f 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 7db7a84468..9eb564a8dc 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 10480a9748..eac3f18f92 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 8cb543ead0..c05cab2570 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 321f605ab8..040570999d 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 5ad1b97969..4a513a264a 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 b2f2edb8bb..cb0cf43f7c 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 d9430a00c0..20bec30677 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 2f1d9e4495..cc2f6eee44 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 ae50a94070..b86711c141 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 2501c9dded..92c019c4e5 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 8fba578d33..3369a40c42 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 4cb49fd25c..167c23a20d 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 916291e4e8..e0219a64e7 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 e70c9ec262..44a64c2e54 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 468369067e..56c40cffbf 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 c9e2bdb4cd..4e873c9d8d 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 231dd92a19..bf851ce41c 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 bd302694a8..875717dc54 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 8bedec6097..dcc5064491 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 a1e3e24c2b..1c8e78c53d 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 dedbf13511..ec2232a522 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 860b7521ba..877c355f10 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 97b122a46e..fef79aadb5 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 a822569474..97cd053e8b 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 5842890482..f4966ec23d 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-20ff7931-DEBUG \ No newline at end of file +DEV-dc492787-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index b90c283f79..8a074baa6e 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_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 err_fun(double *nom_x, double *delta_x, double *out_8059887522619628636) { + out_8059887522619628636[0] = delta_x[0] + nom_x[0]; + out_8059887522619628636[1] = delta_x[1] + nom_x[1]; + out_8059887522619628636[2] = delta_x[2] + nom_x[2]; + out_8059887522619628636[3] = delta_x[3] + nom_x[3]; + out_8059887522619628636[4] = delta_x[4] + nom_x[4]; + out_8059887522619628636[5] = delta_x[5] + nom_x[5]; + out_8059887522619628636[6] = delta_x[6] + nom_x[6]; + out_8059887522619628636[7] = delta_x[7] + nom_x[7]; + out_8059887522619628636[8] = delta_x[8] + nom_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 inv_err_fun(double *nom_x, double *true_x, double *out_3995293944301919807) { + out_3995293944301919807[0] = -nom_x[0] + true_x[0]; + out_3995293944301919807[1] = -nom_x[1] + true_x[1]; + out_3995293944301919807[2] = -nom_x[2] + true_x[2]; + out_3995293944301919807[3] = -nom_x[3] + true_x[3]; + out_3995293944301919807[4] = -nom_x[4] + true_x[4]; + out_3995293944301919807[5] = -nom_x[5] + true_x[5]; + out_3995293944301919807[6] = -nom_x[6] + true_x[6]; + out_3995293944301919807[7] = -nom_x[7] + true_x[7]; + out_3995293944301919807[8] = -nom_x[8] + true_x[8]; } -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 H_mod_fun(double *state, double *out_1466751874933723746) { + out_1466751874933723746[0] = 1.0; + out_1466751874933723746[1] = 0.0; + out_1466751874933723746[2] = 0.0; + out_1466751874933723746[3] = 0.0; + out_1466751874933723746[4] = 0.0; + out_1466751874933723746[5] = 0.0; + out_1466751874933723746[6] = 0.0; + out_1466751874933723746[7] = 0.0; + out_1466751874933723746[8] = 0.0; + out_1466751874933723746[9] = 0.0; + out_1466751874933723746[10] = 1.0; + out_1466751874933723746[11] = 0.0; + out_1466751874933723746[12] = 0.0; + out_1466751874933723746[13] = 0.0; + out_1466751874933723746[14] = 0.0; + out_1466751874933723746[15] = 0.0; + out_1466751874933723746[16] = 0.0; + out_1466751874933723746[17] = 0.0; + out_1466751874933723746[18] = 0.0; + out_1466751874933723746[19] = 0.0; + out_1466751874933723746[20] = 1.0; + out_1466751874933723746[21] = 0.0; + out_1466751874933723746[22] = 0.0; + out_1466751874933723746[23] = 0.0; + out_1466751874933723746[24] = 0.0; + out_1466751874933723746[25] = 0.0; + out_1466751874933723746[26] = 0.0; + out_1466751874933723746[27] = 0.0; + out_1466751874933723746[28] = 0.0; + out_1466751874933723746[29] = 0.0; + out_1466751874933723746[30] = 1.0; + out_1466751874933723746[31] = 0.0; + out_1466751874933723746[32] = 0.0; + out_1466751874933723746[33] = 0.0; + out_1466751874933723746[34] = 0.0; + out_1466751874933723746[35] = 0.0; + out_1466751874933723746[36] = 0.0; + out_1466751874933723746[37] = 0.0; + out_1466751874933723746[38] = 0.0; + out_1466751874933723746[39] = 0.0; + out_1466751874933723746[40] = 1.0; + out_1466751874933723746[41] = 0.0; + out_1466751874933723746[42] = 0.0; + out_1466751874933723746[43] = 0.0; + out_1466751874933723746[44] = 0.0; + out_1466751874933723746[45] = 0.0; + out_1466751874933723746[46] = 0.0; + out_1466751874933723746[47] = 0.0; + out_1466751874933723746[48] = 0.0; + out_1466751874933723746[49] = 0.0; + out_1466751874933723746[50] = 1.0; + out_1466751874933723746[51] = 0.0; + out_1466751874933723746[52] = 0.0; + out_1466751874933723746[53] = 0.0; + out_1466751874933723746[54] = 0.0; + out_1466751874933723746[55] = 0.0; + out_1466751874933723746[56] = 0.0; + out_1466751874933723746[57] = 0.0; + out_1466751874933723746[58] = 0.0; + out_1466751874933723746[59] = 0.0; + out_1466751874933723746[60] = 1.0; + out_1466751874933723746[61] = 0.0; + out_1466751874933723746[62] = 0.0; + out_1466751874933723746[63] = 0.0; + out_1466751874933723746[64] = 0.0; + out_1466751874933723746[65] = 0.0; + out_1466751874933723746[66] = 0.0; + out_1466751874933723746[67] = 0.0; + out_1466751874933723746[68] = 0.0; + out_1466751874933723746[69] = 0.0; + out_1466751874933723746[70] = 1.0; + out_1466751874933723746[71] = 0.0; + out_1466751874933723746[72] = 0.0; + out_1466751874933723746[73] = 0.0; + out_1466751874933723746[74] = 0.0; + out_1466751874933723746[75] = 0.0; + out_1466751874933723746[76] = 0.0; + out_1466751874933723746[77] = 0.0; + out_1466751874933723746[78] = 0.0; + out_1466751874933723746[79] = 0.0; + out_1466751874933723746[80] = 1.0; } -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_1681767971590425073) { + out_1681767971590425073[0] = state[0]; + out_1681767971590425073[1] = state[1]; + out_1681767971590425073[2] = state[2]; + out_1681767971590425073[3] = state[3]; + out_1681767971590425073[4] = state[4]; + out_1681767971590425073[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_1681767971590425073[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_1681767971590425073[7] = state[7]; + out_1681767971590425073[8] = state[8]; } -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 F_fun(double *state, double dt, double *out_7509233405601621058) { + out_7509233405601621058[0] = 1; + out_7509233405601621058[1] = 0; + out_7509233405601621058[2] = 0; + out_7509233405601621058[3] = 0; + out_7509233405601621058[4] = 0; + out_7509233405601621058[5] = 0; + out_7509233405601621058[6] = 0; + out_7509233405601621058[7] = 0; + out_7509233405601621058[8] = 0; + out_7509233405601621058[9] = 0; + out_7509233405601621058[10] = 1; + out_7509233405601621058[11] = 0; + out_7509233405601621058[12] = 0; + out_7509233405601621058[13] = 0; + out_7509233405601621058[14] = 0; + out_7509233405601621058[15] = 0; + out_7509233405601621058[16] = 0; + out_7509233405601621058[17] = 0; + out_7509233405601621058[18] = 0; + out_7509233405601621058[19] = 0; + out_7509233405601621058[20] = 1; + out_7509233405601621058[21] = 0; + out_7509233405601621058[22] = 0; + out_7509233405601621058[23] = 0; + out_7509233405601621058[24] = 0; + out_7509233405601621058[25] = 0; + out_7509233405601621058[26] = 0; + out_7509233405601621058[27] = 0; + out_7509233405601621058[28] = 0; + out_7509233405601621058[29] = 0; + out_7509233405601621058[30] = 1; + out_7509233405601621058[31] = 0; + out_7509233405601621058[32] = 0; + out_7509233405601621058[33] = 0; + out_7509233405601621058[34] = 0; + out_7509233405601621058[35] = 0; + out_7509233405601621058[36] = 0; + out_7509233405601621058[37] = 0; + out_7509233405601621058[38] = 0; + out_7509233405601621058[39] = 0; + out_7509233405601621058[40] = 1; + out_7509233405601621058[41] = 0; + out_7509233405601621058[42] = 0; + out_7509233405601621058[43] = 0; + out_7509233405601621058[44] = 0; + out_7509233405601621058[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_7509233405601621058[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_7509233405601621058[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_7509233405601621058[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_7509233405601621058[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_7509233405601621058[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_7509233405601621058[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_7509233405601621058[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_7509233405601621058[53] = -9.8100000000000005*dt; + out_7509233405601621058[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_7509233405601621058[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_7509233405601621058[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_7509233405601621058[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_7509233405601621058[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_7509233405601621058[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_7509233405601621058[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_7509233405601621058[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_7509233405601621058[62] = 0; + out_7509233405601621058[63] = 0; + out_7509233405601621058[64] = 0; + out_7509233405601621058[65] = 0; + out_7509233405601621058[66] = 0; + out_7509233405601621058[67] = 0; + out_7509233405601621058[68] = 0; + out_7509233405601621058[69] = 0; + out_7509233405601621058[70] = 1; + out_7509233405601621058[71] = 0; + out_7509233405601621058[72] = 0; + out_7509233405601621058[73] = 0; + out_7509233405601621058[74] = 0; + out_7509233405601621058[75] = 0; + out_7509233405601621058[76] = 0; + out_7509233405601621058[77] = 0; + out_7509233405601621058[78] = 0; + out_7509233405601621058[79] = 0; + out_7509233405601621058[80] = 1; } -void h_25(double *state, double *unused, double *out_5355327156585439901) { - out_5355327156585439901[0] = state[6]; +void h_25(double *state, double *unused, double *out_1246716073205151034) { + out_1246716073205151034[0] = state[6]; } -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_25(double *state, double *unused, double *out_7786803972863590633) { + out_7786803972863590633[0] = 0; + out_7786803972863590633[1] = 0; + out_7786803972863590633[2] = 0; + out_7786803972863590633[3] = 0; + out_7786803972863590633[4] = 0; + out_7786803972863590633[5] = 0; + out_7786803972863590633[6] = 1; + out_7786803972863590633[7] = 0; + out_7786803972863590633[8] = 0; } -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_1348262878792507760) { + out_1348262878792507760[0] = state[4]; + out_1348262878792507760[1] = state[5]; } -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_24(double *state, double *unused, double *out_3040294645897081599) { + out_3040294645897081599[0] = 0; + out_3040294645897081599[1] = 0; + out_3040294645897081599[2] = 0; + out_3040294645897081599[3] = 0; + out_3040294645897081599[4] = 1; + out_3040294645897081599[5] = 0; + out_3040294645897081599[6] = 0; + out_3040294645897081599[7] = 0; + out_3040294645897081599[8] = 0; + out_3040294645897081599[9] = 0; + out_3040294645897081599[10] = 0; + out_3040294645897081599[11] = 0; + out_3040294645897081599[12] = 0; + out_3040294645897081599[13] = 0; + out_3040294645897081599[14] = 1; + out_3040294645897081599[15] = 0; + out_3040294645897081599[16] = 0; + out_3040294645897081599[17] = 0; } -void h_30(double *state, double *unused, double *out_6805958549214323068) { - out_6805958549214323068[0] = state[4]; +void h_30(double *state, double *unused, double *out_600163749077985182) { + out_600163749077985182[0] = state[4]; } -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_30(double *state, double *unused, double *out_3743249759354344228) { + out_3743249759354344228[0] = 0; + out_3743249759354344228[1] = 0; + out_3743249759354344228[2] = 0; + out_3743249759354344228[3] = 0; + out_3743249759354344228[4] = 1; + out_3743249759354344228[5] = 0; + out_3743249759354344228[6] = 0; + out_3743249759354344228[7] = 0; + out_3743249759354344228[8] = 0; } -void h_26(double *state, double *unused, double *out_2475727773492236361) { - out_2475727773492236361[0] = state[7]; +void h_26(double *state, double *unused, double *out_5829263656854511712) { + out_5829263656854511712[0] = state[7]; } -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_26(double *state, double *unused, double *out_4045300653989534409) { + out_4045300653989534409[0] = 0; + out_4045300653989534409[1] = 0; + out_4045300653989534409[2] = 0; + out_4045300653989534409[3] = 0; + out_4045300653989534409[4] = 0; + out_4045300653989534409[5] = 0; + out_4045300653989534409[6] = 0; + out_4045300653989534409[7] = 1; + out_4045300653989534409[8] = 0; } -void h_27(double *state, double *unused, double *out_353653888924594256) { - out_353653888924594256[0] = state[3]; +void h_27(double *state, double *unused, double *out_3754957194455694611) { + out_3754957194455694611[0] = state[3]; } -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_27(double *state, double *unused, double *out_5918013071154769139) { + out_5918013071154769139[0] = 0; + out_5918013071154769139[1] = 0; + out_5918013071154769139[2] = 0; + out_5918013071154769139[3] = 1; + out_5918013071154769139[4] = 0; + out_5918013071154769139[5] = 0; + out_5918013071154769139[6] = 0; + out_5918013071154769139[7] = 0; + out_5918013071154769139[8] = 0; } -void h_29(double *state, double *unused, double *out_1804285281553477423) { - out_1804285281553477423[0] = state[1]; +void h_29(double *state, double *unused, double *out_3597770526104565466) { + out_3597770526104565466[0] = state[1]; } -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_29(double *state, double *unused, double *out_7631375798024320172) { + out_7631375798024320172[0] = 0; + out_7631375798024320172[1] = 1; + out_7631375798024320172[2] = 0; + out_7631375798024320172[3] = 0; + out_7631375798024320172[4] = 0; + out_7631375798024320172[5] = 0; + out_7631375798024320172[6] = 0; + out_7631375798024320172[7] = 0; + out_7631375798024320172[8] = 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_718171143011361926) { + out_718171143011361926[0] = state[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_28(double *state, double *unused, double *out_5732969258615700870) { + out_5732969258615700870[0] = 1; + out_5732969258615700870[1] = 0; + out_5732969258615700870[2] = 0; + out_5732969258615700870[3] = 0; + out_5732969258615700870[4] = 0; + out_5732969258615700870[5] = 0; + out_5732969258615700870[6] = 0; + out_5732969258615700870[7] = 0; + out_5732969258615700870[8] = 0; } -void h_31(double *state, double *unused, double *out_5630521218869945790) { - out_5630521218869945790[0] = state[8]; +void h_31(double *state, double *unused, double *out_1521910135489656923) { + out_1521910135489656923[0] = state[8]; } -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; +void H_31(double *state, double *unused, double *out_7817449934740551061) { + out_7817449934740551061[0] = 0; + out_7817449934740551061[1] = 0; + out_7817449934740551061[2] = 0; + out_7817449934740551061[3] = 0; + out_7817449934740551061[4] = 0; + out_7817449934740551061[5] = 0; + out_7817449934740551061[6] = 0; + out_7817449934740551061[7] = 0; + out_7817449934740551061[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_1527400592183565183) { - err_fun(nom_x, delta_x, out_1527400592183565183); +void car_err_fun(double *nom_x, double *delta_x, double *out_8059887522619628636) { + err_fun(nom_x, delta_x, out_8059887522619628636); } -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_inv_err_fun(double *nom_x, double *true_x, double *out_3995293944301919807) { + inv_err_fun(nom_x, true_x, out_3995293944301919807); } -void car_H_mod_fun(double *state, double *out_8049619905800951591) { - H_mod_fun(state, out_8049619905800951591); +void car_H_mod_fun(double *state, double *out_1466751874933723746) { + H_mod_fun(state, out_1466751874933723746); } -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_1681767971590425073) { + f_fun(state, dt, out_1681767971590425073); } -void car_F_fun(double *state, double dt, double *out_8373137223162851881) { - F_fun(state, dt, out_8373137223162851881); +void car_F_fun(double *state, double dt, double *out_7509233405601621058) { + F_fun(state, dt, out_7509233405601621058); } -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_1246716073205151034) { + h_25(state, unused, out_1246716073205151034); } -void car_H_25(double *state, double *unused, double *out_2475786134934740220) { - H_25(state, unused, out_2475786134934740220); +void car_H_25(double *state, double *unused, double *out_7786803972863590633) { + H_25(state, unused, out_7786803972863590633); } -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_1348262878792507760) { + h_24(state, unused, out_1348262878792507760); } -void car_H_24(double *state, double *unused, double *out_3316217919116785212) { - H_24(state, unused, out_3316217919116785212); +void car_H_24(double *state, double *unused, double *out_3040294645897081599) { + H_24(state, unused, out_3040294645897081599); } -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_600163749077985182) { + h_30(state, unused, out_600163749077985182); } -void car_H_30(double *state, double *unused, double *out_42546823572508407) { - H_30(state, unused, out_42546823572508407); +void car_H_30(double *state, double *unused, double *out_3743249759354344228) { + H_30(state, unused, out_3743249759354344228); } -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_5829263656854511712) { + h_26(state, unused, out_5829263656854511712); } -void car_H_26(double *state, double *unused, double *out_6217289453808796444) { - H_26(state, unused, out_6217289453808796444); +void car_H_26(double *state, double *unused, double *out_4045300653989534409) { + H_26(state, unused, out_4045300653989534409); } -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_3754957194455694611) { + h_27(state, unused, out_3754957194455694611); } -void car_H_27(double *state, double *unused, double *out_9178245776862773329) { - H_27(state, unused, out_9178245776862773329); +void car_H_27(double *state, double *unused, double *out_5918013071154769139) { + H_27(state, unused, out_5918013071154769139); } -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_3597770526104565466) { + h_29(state, unused, out_3597770526104565466); } -void car_H_29(double *state, double *unused, double *out_6493251120747956234) { - H_29(state, unused, out_6493251120747956234); +void car_H_29(double *state, double *unused, double *out_7631375798024320172) { + H_29(state, unused, out_7631375798024320172); } -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_718171143011361926) { + h_28(state, unused, out_718171143011361926); } -void car_H_28(double *state, double *unused, double *out_4529620849182629983) { - H_28(state, unused, out_4529620849182629983); +void car_H_28(double *state, double *unused, double *out_5732969258615700870) { + H_28(state, unused, out_5732969258615700870); } -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_1521910135489656923) { + h_31(state, unused, out_1521910135489656923); } -void car_H_31(double *state, double *unused, double *out_6843497556042147920) { - H_31(state, unused, out_6843497556042147920); +void car_H_31(double *state, double *unused, double *out_7817449934740551061) { + H_31(state, unused, out_7817449934740551061); } 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 e8eaa292a2..2fa2f91101 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_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_err_fun(double *nom_x, double *delta_x, double *out_8059887522619628636); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_3995293944301919807); +void car_H_mod_fun(double *state, double *out_1466751874933723746); +void car_f_fun(double *state, double dt, double *out_1681767971590425073); +void car_F_fun(double *state, double dt, double *out_7509233405601621058); +void car_h_25(double *state, double *unused, double *out_1246716073205151034); +void car_H_25(double *state, double *unused, double *out_7786803972863590633); +void car_h_24(double *state, double *unused, double *out_1348262878792507760); +void car_H_24(double *state, double *unused, double *out_3040294645897081599); +void car_h_30(double *state, double *unused, double *out_600163749077985182); +void car_H_30(double *state, double *unused, double *out_3743249759354344228); +void car_h_26(double *state, double *unused, double *out_5829263656854511712); +void car_H_26(double *state, double *unused, double *out_4045300653989534409); +void car_h_27(double *state, double *unused, double *out_3754957194455694611); +void car_H_27(double *state, double *unused, double *out_5918013071154769139); +void car_h_29(double *state, double *unused, double *out_3597770526104565466); +void car_H_29(double *state, double *unused, double *out_7631375798024320172); +void car_h_28(double *state, double *unused, double *out_718171143011361926); +void car_H_28(double *state, double *unused, double *out_5732969258615700870); +void car_h_31(double *state, double *unused, double *out_1521910135489656923); +void car_H_31(double *state, double *unused, double *out_7817449934740551061); void car_predict(double *in_x, double *in_P, double *in_Q, double dt); void car_set_mass(double x); void car_set_rotational_inertia(double x); diff --git a/selfdrive/locationd/models/generated/pose.cpp b/selfdrive/locationd/models/generated/pose.cpp index 829ac49494..42ff5d9d90 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_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 err_fun(double *nom_x, double *delta_x, double *out_2128814047154352200) { + out_2128814047154352200[0] = delta_x[0] + nom_x[0]; + out_2128814047154352200[1] = delta_x[1] + nom_x[1]; + out_2128814047154352200[2] = delta_x[2] + nom_x[2]; + out_2128814047154352200[3] = delta_x[3] + nom_x[3]; + out_2128814047154352200[4] = delta_x[4] + nom_x[4]; + out_2128814047154352200[5] = delta_x[5] + nom_x[5]; + out_2128814047154352200[6] = delta_x[6] + nom_x[6]; + out_2128814047154352200[7] = delta_x[7] + nom_x[7]; + out_2128814047154352200[8] = delta_x[8] + nom_x[8]; + out_2128814047154352200[9] = delta_x[9] + nom_x[9]; + out_2128814047154352200[10] = delta_x[10] + nom_x[10]; + out_2128814047154352200[11] = delta_x[11] + nom_x[11]; + out_2128814047154352200[12] = delta_x[12] + nom_x[12]; + out_2128814047154352200[13] = delta_x[13] + nom_x[13]; + out_2128814047154352200[14] = delta_x[14] + nom_x[14]; + out_2128814047154352200[15] = delta_x[15] + nom_x[15]; + out_2128814047154352200[16] = delta_x[16] + nom_x[16]; + out_2128814047154352200[17] = delta_x[17] + nom_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 inv_err_fun(double *nom_x, double *true_x, double *out_7001406389145399648) { + out_7001406389145399648[0] = -nom_x[0] + true_x[0]; + out_7001406389145399648[1] = -nom_x[1] + true_x[1]; + out_7001406389145399648[2] = -nom_x[2] + true_x[2]; + out_7001406389145399648[3] = -nom_x[3] + true_x[3]; + out_7001406389145399648[4] = -nom_x[4] + true_x[4]; + out_7001406389145399648[5] = -nom_x[5] + true_x[5]; + out_7001406389145399648[6] = -nom_x[6] + true_x[6]; + out_7001406389145399648[7] = -nom_x[7] + true_x[7]; + out_7001406389145399648[8] = -nom_x[8] + true_x[8]; + out_7001406389145399648[9] = -nom_x[9] + true_x[9]; + out_7001406389145399648[10] = -nom_x[10] + true_x[10]; + out_7001406389145399648[11] = -nom_x[11] + true_x[11]; + out_7001406389145399648[12] = -nom_x[12] + true_x[12]; + out_7001406389145399648[13] = -nom_x[13] + true_x[13]; + out_7001406389145399648[14] = -nom_x[14] + true_x[14]; + out_7001406389145399648[15] = -nom_x[15] + true_x[15]; + out_7001406389145399648[16] = -nom_x[16] + true_x[16]; + out_7001406389145399648[17] = -nom_x[17] + true_x[17]; } -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 H_mod_fun(double *state, double *out_1783591311774877036) { + out_1783591311774877036[0] = 1.0; + out_1783591311774877036[1] = 0.0; + out_1783591311774877036[2] = 0.0; + out_1783591311774877036[3] = 0.0; + out_1783591311774877036[4] = 0.0; + out_1783591311774877036[5] = 0.0; + out_1783591311774877036[6] = 0.0; + out_1783591311774877036[7] = 0.0; + out_1783591311774877036[8] = 0.0; + out_1783591311774877036[9] = 0.0; + out_1783591311774877036[10] = 0.0; + out_1783591311774877036[11] = 0.0; + out_1783591311774877036[12] = 0.0; + out_1783591311774877036[13] = 0.0; + out_1783591311774877036[14] = 0.0; + out_1783591311774877036[15] = 0.0; + out_1783591311774877036[16] = 0.0; + out_1783591311774877036[17] = 0.0; + out_1783591311774877036[18] = 0.0; + out_1783591311774877036[19] = 1.0; + out_1783591311774877036[20] = 0.0; + out_1783591311774877036[21] = 0.0; + out_1783591311774877036[22] = 0.0; + out_1783591311774877036[23] = 0.0; + out_1783591311774877036[24] = 0.0; + out_1783591311774877036[25] = 0.0; + out_1783591311774877036[26] = 0.0; + out_1783591311774877036[27] = 0.0; + out_1783591311774877036[28] = 0.0; + out_1783591311774877036[29] = 0.0; + out_1783591311774877036[30] = 0.0; + out_1783591311774877036[31] = 0.0; + out_1783591311774877036[32] = 0.0; + out_1783591311774877036[33] = 0.0; + out_1783591311774877036[34] = 0.0; + out_1783591311774877036[35] = 0.0; + out_1783591311774877036[36] = 0.0; + out_1783591311774877036[37] = 0.0; + out_1783591311774877036[38] = 1.0; + out_1783591311774877036[39] = 0.0; + out_1783591311774877036[40] = 0.0; + out_1783591311774877036[41] = 0.0; + out_1783591311774877036[42] = 0.0; + out_1783591311774877036[43] = 0.0; + out_1783591311774877036[44] = 0.0; + out_1783591311774877036[45] = 0.0; + out_1783591311774877036[46] = 0.0; + out_1783591311774877036[47] = 0.0; + out_1783591311774877036[48] = 0.0; + out_1783591311774877036[49] = 0.0; + out_1783591311774877036[50] = 0.0; + out_1783591311774877036[51] = 0.0; + out_1783591311774877036[52] = 0.0; + out_1783591311774877036[53] = 0.0; + out_1783591311774877036[54] = 0.0; + out_1783591311774877036[55] = 0.0; + out_1783591311774877036[56] = 0.0; + out_1783591311774877036[57] = 1.0; + out_1783591311774877036[58] = 0.0; + out_1783591311774877036[59] = 0.0; + out_1783591311774877036[60] = 0.0; + out_1783591311774877036[61] = 0.0; + out_1783591311774877036[62] = 0.0; + out_1783591311774877036[63] = 0.0; + out_1783591311774877036[64] = 0.0; + out_1783591311774877036[65] = 0.0; + out_1783591311774877036[66] = 0.0; + out_1783591311774877036[67] = 0.0; + out_1783591311774877036[68] = 0.0; + out_1783591311774877036[69] = 0.0; + out_1783591311774877036[70] = 0.0; + out_1783591311774877036[71] = 0.0; + out_1783591311774877036[72] = 0.0; + out_1783591311774877036[73] = 0.0; + out_1783591311774877036[74] = 0.0; + out_1783591311774877036[75] = 0.0; + out_1783591311774877036[76] = 1.0; + out_1783591311774877036[77] = 0.0; + out_1783591311774877036[78] = 0.0; + out_1783591311774877036[79] = 0.0; + out_1783591311774877036[80] = 0.0; + out_1783591311774877036[81] = 0.0; + out_1783591311774877036[82] = 0.0; + out_1783591311774877036[83] = 0.0; + out_1783591311774877036[84] = 0.0; + out_1783591311774877036[85] = 0.0; + out_1783591311774877036[86] = 0.0; + out_1783591311774877036[87] = 0.0; + out_1783591311774877036[88] = 0.0; + out_1783591311774877036[89] = 0.0; + out_1783591311774877036[90] = 0.0; + out_1783591311774877036[91] = 0.0; + out_1783591311774877036[92] = 0.0; + out_1783591311774877036[93] = 0.0; + out_1783591311774877036[94] = 0.0; + out_1783591311774877036[95] = 1.0; + out_1783591311774877036[96] = 0.0; + out_1783591311774877036[97] = 0.0; + out_1783591311774877036[98] = 0.0; + out_1783591311774877036[99] = 0.0; + out_1783591311774877036[100] = 0.0; + out_1783591311774877036[101] = 0.0; + out_1783591311774877036[102] = 0.0; + out_1783591311774877036[103] = 0.0; + out_1783591311774877036[104] = 0.0; + out_1783591311774877036[105] = 0.0; + out_1783591311774877036[106] = 0.0; + out_1783591311774877036[107] = 0.0; + out_1783591311774877036[108] = 0.0; + out_1783591311774877036[109] = 0.0; + out_1783591311774877036[110] = 0.0; + out_1783591311774877036[111] = 0.0; + out_1783591311774877036[112] = 0.0; + out_1783591311774877036[113] = 0.0; + out_1783591311774877036[114] = 1.0; + out_1783591311774877036[115] = 0.0; + out_1783591311774877036[116] = 0.0; + out_1783591311774877036[117] = 0.0; + out_1783591311774877036[118] = 0.0; + out_1783591311774877036[119] = 0.0; + out_1783591311774877036[120] = 0.0; + out_1783591311774877036[121] = 0.0; + out_1783591311774877036[122] = 0.0; + out_1783591311774877036[123] = 0.0; + out_1783591311774877036[124] = 0.0; + out_1783591311774877036[125] = 0.0; + out_1783591311774877036[126] = 0.0; + out_1783591311774877036[127] = 0.0; + out_1783591311774877036[128] = 0.0; + out_1783591311774877036[129] = 0.0; + out_1783591311774877036[130] = 0.0; + out_1783591311774877036[131] = 0.0; + out_1783591311774877036[132] = 0.0; + out_1783591311774877036[133] = 1.0; + out_1783591311774877036[134] = 0.0; + out_1783591311774877036[135] = 0.0; + out_1783591311774877036[136] = 0.0; + out_1783591311774877036[137] = 0.0; + out_1783591311774877036[138] = 0.0; + out_1783591311774877036[139] = 0.0; + out_1783591311774877036[140] = 0.0; + out_1783591311774877036[141] = 0.0; + out_1783591311774877036[142] = 0.0; + out_1783591311774877036[143] = 0.0; + out_1783591311774877036[144] = 0.0; + out_1783591311774877036[145] = 0.0; + out_1783591311774877036[146] = 0.0; + out_1783591311774877036[147] = 0.0; + out_1783591311774877036[148] = 0.0; + out_1783591311774877036[149] = 0.0; + out_1783591311774877036[150] = 0.0; + out_1783591311774877036[151] = 0.0; + out_1783591311774877036[152] = 1.0; + out_1783591311774877036[153] = 0.0; + out_1783591311774877036[154] = 0.0; + out_1783591311774877036[155] = 0.0; + out_1783591311774877036[156] = 0.0; + out_1783591311774877036[157] = 0.0; + out_1783591311774877036[158] = 0.0; + out_1783591311774877036[159] = 0.0; + out_1783591311774877036[160] = 0.0; + out_1783591311774877036[161] = 0.0; + out_1783591311774877036[162] = 0.0; + out_1783591311774877036[163] = 0.0; + out_1783591311774877036[164] = 0.0; + out_1783591311774877036[165] = 0.0; + out_1783591311774877036[166] = 0.0; + out_1783591311774877036[167] = 0.0; + out_1783591311774877036[168] = 0.0; + out_1783591311774877036[169] = 0.0; + out_1783591311774877036[170] = 0.0; + out_1783591311774877036[171] = 1.0; + out_1783591311774877036[172] = 0.0; + out_1783591311774877036[173] = 0.0; + out_1783591311774877036[174] = 0.0; + out_1783591311774877036[175] = 0.0; + out_1783591311774877036[176] = 0.0; + out_1783591311774877036[177] = 0.0; + out_1783591311774877036[178] = 0.0; + out_1783591311774877036[179] = 0.0; + out_1783591311774877036[180] = 0.0; + out_1783591311774877036[181] = 0.0; + out_1783591311774877036[182] = 0.0; + out_1783591311774877036[183] = 0.0; + out_1783591311774877036[184] = 0.0; + out_1783591311774877036[185] = 0.0; + out_1783591311774877036[186] = 0.0; + out_1783591311774877036[187] = 0.0; + out_1783591311774877036[188] = 0.0; + out_1783591311774877036[189] = 0.0; + out_1783591311774877036[190] = 1.0; + out_1783591311774877036[191] = 0.0; + out_1783591311774877036[192] = 0.0; + out_1783591311774877036[193] = 0.0; + out_1783591311774877036[194] = 0.0; + out_1783591311774877036[195] = 0.0; + out_1783591311774877036[196] = 0.0; + out_1783591311774877036[197] = 0.0; + out_1783591311774877036[198] = 0.0; + out_1783591311774877036[199] = 0.0; + out_1783591311774877036[200] = 0.0; + out_1783591311774877036[201] = 0.0; + out_1783591311774877036[202] = 0.0; + out_1783591311774877036[203] = 0.0; + out_1783591311774877036[204] = 0.0; + out_1783591311774877036[205] = 0.0; + out_1783591311774877036[206] = 0.0; + out_1783591311774877036[207] = 0.0; + out_1783591311774877036[208] = 0.0; + out_1783591311774877036[209] = 1.0; + out_1783591311774877036[210] = 0.0; + out_1783591311774877036[211] = 0.0; + out_1783591311774877036[212] = 0.0; + out_1783591311774877036[213] = 0.0; + out_1783591311774877036[214] = 0.0; + out_1783591311774877036[215] = 0.0; + out_1783591311774877036[216] = 0.0; + out_1783591311774877036[217] = 0.0; + out_1783591311774877036[218] = 0.0; + out_1783591311774877036[219] = 0.0; + out_1783591311774877036[220] = 0.0; + out_1783591311774877036[221] = 0.0; + out_1783591311774877036[222] = 0.0; + out_1783591311774877036[223] = 0.0; + out_1783591311774877036[224] = 0.0; + out_1783591311774877036[225] = 0.0; + out_1783591311774877036[226] = 0.0; + out_1783591311774877036[227] = 0.0; + out_1783591311774877036[228] = 1.0; + out_1783591311774877036[229] = 0.0; + out_1783591311774877036[230] = 0.0; + out_1783591311774877036[231] = 0.0; + out_1783591311774877036[232] = 0.0; + out_1783591311774877036[233] = 0.0; + out_1783591311774877036[234] = 0.0; + out_1783591311774877036[235] = 0.0; + out_1783591311774877036[236] = 0.0; + out_1783591311774877036[237] = 0.0; + out_1783591311774877036[238] = 0.0; + out_1783591311774877036[239] = 0.0; + out_1783591311774877036[240] = 0.0; + out_1783591311774877036[241] = 0.0; + out_1783591311774877036[242] = 0.0; + out_1783591311774877036[243] = 0.0; + out_1783591311774877036[244] = 0.0; + out_1783591311774877036[245] = 0.0; + out_1783591311774877036[246] = 0.0; + out_1783591311774877036[247] = 1.0; + out_1783591311774877036[248] = 0.0; + out_1783591311774877036[249] = 0.0; + out_1783591311774877036[250] = 0.0; + out_1783591311774877036[251] = 0.0; + out_1783591311774877036[252] = 0.0; + out_1783591311774877036[253] = 0.0; + out_1783591311774877036[254] = 0.0; + out_1783591311774877036[255] = 0.0; + out_1783591311774877036[256] = 0.0; + out_1783591311774877036[257] = 0.0; + out_1783591311774877036[258] = 0.0; + out_1783591311774877036[259] = 0.0; + out_1783591311774877036[260] = 0.0; + out_1783591311774877036[261] = 0.0; + out_1783591311774877036[262] = 0.0; + out_1783591311774877036[263] = 0.0; + out_1783591311774877036[264] = 0.0; + out_1783591311774877036[265] = 0.0; + out_1783591311774877036[266] = 1.0; + out_1783591311774877036[267] = 0.0; + out_1783591311774877036[268] = 0.0; + out_1783591311774877036[269] = 0.0; + out_1783591311774877036[270] = 0.0; + out_1783591311774877036[271] = 0.0; + out_1783591311774877036[272] = 0.0; + out_1783591311774877036[273] = 0.0; + out_1783591311774877036[274] = 0.0; + out_1783591311774877036[275] = 0.0; + out_1783591311774877036[276] = 0.0; + out_1783591311774877036[277] = 0.0; + out_1783591311774877036[278] = 0.0; + out_1783591311774877036[279] = 0.0; + out_1783591311774877036[280] = 0.0; + out_1783591311774877036[281] = 0.0; + out_1783591311774877036[282] = 0.0; + out_1783591311774877036[283] = 0.0; + out_1783591311774877036[284] = 0.0; + out_1783591311774877036[285] = 1.0; + out_1783591311774877036[286] = 0.0; + out_1783591311774877036[287] = 0.0; + out_1783591311774877036[288] = 0.0; + out_1783591311774877036[289] = 0.0; + out_1783591311774877036[290] = 0.0; + out_1783591311774877036[291] = 0.0; + out_1783591311774877036[292] = 0.0; + out_1783591311774877036[293] = 0.0; + out_1783591311774877036[294] = 0.0; + out_1783591311774877036[295] = 0.0; + out_1783591311774877036[296] = 0.0; + out_1783591311774877036[297] = 0.0; + out_1783591311774877036[298] = 0.0; + out_1783591311774877036[299] = 0.0; + out_1783591311774877036[300] = 0.0; + out_1783591311774877036[301] = 0.0; + out_1783591311774877036[302] = 0.0; + out_1783591311774877036[303] = 0.0; + out_1783591311774877036[304] = 1.0; + out_1783591311774877036[305] = 0.0; + out_1783591311774877036[306] = 0.0; + out_1783591311774877036[307] = 0.0; + out_1783591311774877036[308] = 0.0; + out_1783591311774877036[309] = 0.0; + out_1783591311774877036[310] = 0.0; + out_1783591311774877036[311] = 0.0; + out_1783591311774877036[312] = 0.0; + out_1783591311774877036[313] = 0.0; + out_1783591311774877036[314] = 0.0; + out_1783591311774877036[315] = 0.0; + out_1783591311774877036[316] = 0.0; + out_1783591311774877036[317] = 0.0; + out_1783591311774877036[318] = 0.0; + out_1783591311774877036[319] = 0.0; + out_1783591311774877036[320] = 0.0; + out_1783591311774877036[321] = 0.0; + out_1783591311774877036[322] = 0.0; + out_1783591311774877036[323] = 1.0; } -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_3415300158525823285) { + out_3415300158525823285[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_3415300158525823285[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_3415300158525823285[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_3415300158525823285[3] = dt*state[12] + state[3]; + out_3415300158525823285[4] = dt*state[13] + state[4]; + out_3415300158525823285[5] = dt*state[14] + state[5]; + out_3415300158525823285[6] = state[6]; + out_3415300158525823285[7] = state[7]; + out_3415300158525823285[8] = state[8]; + out_3415300158525823285[9] = state[9]; + out_3415300158525823285[10] = state[10]; + out_3415300158525823285[11] = state[11]; + out_3415300158525823285[12] = state[12]; + out_3415300158525823285[13] = state[13]; + out_3415300158525823285[14] = state[14]; + out_3415300158525823285[15] = state[15]; + out_3415300158525823285[16] = state[16]; + out_3415300158525823285[17] = state[17]; } -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 F_fun(double *state, double dt, double *out_5538305373514744468) { + out_5538305373514744468[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_5538305373514744468[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_5538305373514744468[2] = 0; + out_5538305373514744468[3] = 0; + out_5538305373514744468[4] = 0; + out_5538305373514744468[5] = 0; + out_5538305373514744468[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_5538305373514744468[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_5538305373514744468[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_5538305373514744468[9] = 0; + out_5538305373514744468[10] = 0; + out_5538305373514744468[11] = 0; + out_5538305373514744468[12] = 0; + out_5538305373514744468[13] = 0; + out_5538305373514744468[14] = 0; + out_5538305373514744468[15] = 0; + out_5538305373514744468[16] = 0; + out_5538305373514744468[17] = 0; + out_5538305373514744468[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_5538305373514744468[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_5538305373514744468[20] = 0; + out_5538305373514744468[21] = 0; + out_5538305373514744468[22] = 0; + out_5538305373514744468[23] = 0; + out_5538305373514744468[24] = 0; + out_5538305373514744468[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_5538305373514744468[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_5538305373514744468[27] = 0; + out_5538305373514744468[28] = 0; + out_5538305373514744468[29] = 0; + out_5538305373514744468[30] = 0; + out_5538305373514744468[31] = 0; + out_5538305373514744468[32] = 0; + out_5538305373514744468[33] = 0; + out_5538305373514744468[34] = 0; + out_5538305373514744468[35] = 0; + out_5538305373514744468[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_5538305373514744468[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_5538305373514744468[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_5538305373514744468[39] = 0; + out_5538305373514744468[40] = 0; + out_5538305373514744468[41] = 0; + out_5538305373514744468[42] = 0; + out_5538305373514744468[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_5538305373514744468[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_5538305373514744468[45] = 0; + out_5538305373514744468[46] = 0; + out_5538305373514744468[47] = 0; + out_5538305373514744468[48] = 0; + out_5538305373514744468[49] = 0; + out_5538305373514744468[50] = 0; + out_5538305373514744468[51] = 0; + out_5538305373514744468[52] = 0; + out_5538305373514744468[53] = 0; + out_5538305373514744468[54] = 0; + out_5538305373514744468[55] = 0; + out_5538305373514744468[56] = 0; + out_5538305373514744468[57] = 1; + out_5538305373514744468[58] = 0; + out_5538305373514744468[59] = 0; + out_5538305373514744468[60] = 0; + out_5538305373514744468[61] = 0; + out_5538305373514744468[62] = 0; + out_5538305373514744468[63] = 0; + out_5538305373514744468[64] = 0; + out_5538305373514744468[65] = 0; + out_5538305373514744468[66] = dt; + out_5538305373514744468[67] = 0; + out_5538305373514744468[68] = 0; + out_5538305373514744468[69] = 0; + out_5538305373514744468[70] = 0; + out_5538305373514744468[71] = 0; + out_5538305373514744468[72] = 0; + out_5538305373514744468[73] = 0; + out_5538305373514744468[74] = 0; + out_5538305373514744468[75] = 0; + out_5538305373514744468[76] = 1; + out_5538305373514744468[77] = 0; + out_5538305373514744468[78] = 0; + out_5538305373514744468[79] = 0; + out_5538305373514744468[80] = 0; + out_5538305373514744468[81] = 0; + out_5538305373514744468[82] = 0; + out_5538305373514744468[83] = 0; + out_5538305373514744468[84] = 0; + out_5538305373514744468[85] = dt; + out_5538305373514744468[86] = 0; + out_5538305373514744468[87] = 0; + out_5538305373514744468[88] = 0; + out_5538305373514744468[89] = 0; + out_5538305373514744468[90] = 0; + out_5538305373514744468[91] = 0; + out_5538305373514744468[92] = 0; + out_5538305373514744468[93] = 0; + out_5538305373514744468[94] = 0; + out_5538305373514744468[95] = 1; + out_5538305373514744468[96] = 0; + out_5538305373514744468[97] = 0; + out_5538305373514744468[98] = 0; + out_5538305373514744468[99] = 0; + out_5538305373514744468[100] = 0; + out_5538305373514744468[101] = 0; + out_5538305373514744468[102] = 0; + out_5538305373514744468[103] = 0; + out_5538305373514744468[104] = dt; + out_5538305373514744468[105] = 0; + out_5538305373514744468[106] = 0; + out_5538305373514744468[107] = 0; + out_5538305373514744468[108] = 0; + out_5538305373514744468[109] = 0; + out_5538305373514744468[110] = 0; + out_5538305373514744468[111] = 0; + out_5538305373514744468[112] = 0; + out_5538305373514744468[113] = 0; + out_5538305373514744468[114] = 1; + out_5538305373514744468[115] = 0; + out_5538305373514744468[116] = 0; + out_5538305373514744468[117] = 0; + out_5538305373514744468[118] = 0; + out_5538305373514744468[119] = 0; + out_5538305373514744468[120] = 0; + out_5538305373514744468[121] = 0; + out_5538305373514744468[122] = 0; + out_5538305373514744468[123] = 0; + out_5538305373514744468[124] = 0; + out_5538305373514744468[125] = 0; + out_5538305373514744468[126] = 0; + out_5538305373514744468[127] = 0; + out_5538305373514744468[128] = 0; + out_5538305373514744468[129] = 0; + out_5538305373514744468[130] = 0; + out_5538305373514744468[131] = 0; + out_5538305373514744468[132] = 0; + out_5538305373514744468[133] = 1; + out_5538305373514744468[134] = 0; + out_5538305373514744468[135] = 0; + out_5538305373514744468[136] = 0; + out_5538305373514744468[137] = 0; + out_5538305373514744468[138] = 0; + out_5538305373514744468[139] = 0; + out_5538305373514744468[140] = 0; + out_5538305373514744468[141] = 0; + out_5538305373514744468[142] = 0; + out_5538305373514744468[143] = 0; + out_5538305373514744468[144] = 0; + out_5538305373514744468[145] = 0; + out_5538305373514744468[146] = 0; + out_5538305373514744468[147] = 0; + out_5538305373514744468[148] = 0; + out_5538305373514744468[149] = 0; + out_5538305373514744468[150] = 0; + out_5538305373514744468[151] = 0; + out_5538305373514744468[152] = 1; + out_5538305373514744468[153] = 0; + out_5538305373514744468[154] = 0; + out_5538305373514744468[155] = 0; + out_5538305373514744468[156] = 0; + out_5538305373514744468[157] = 0; + out_5538305373514744468[158] = 0; + out_5538305373514744468[159] = 0; + out_5538305373514744468[160] = 0; + out_5538305373514744468[161] = 0; + out_5538305373514744468[162] = 0; + out_5538305373514744468[163] = 0; + out_5538305373514744468[164] = 0; + out_5538305373514744468[165] = 0; + out_5538305373514744468[166] = 0; + out_5538305373514744468[167] = 0; + out_5538305373514744468[168] = 0; + out_5538305373514744468[169] = 0; + out_5538305373514744468[170] = 0; + out_5538305373514744468[171] = 1; + out_5538305373514744468[172] = 0; + out_5538305373514744468[173] = 0; + out_5538305373514744468[174] = 0; + out_5538305373514744468[175] = 0; + out_5538305373514744468[176] = 0; + out_5538305373514744468[177] = 0; + out_5538305373514744468[178] = 0; + out_5538305373514744468[179] = 0; + out_5538305373514744468[180] = 0; + out_5538305373514744468[181] = 0; + out_5538305373514744468[182] = 0; + out_5538305373514744468[183] = 0; + out_5538305373514744468[184] = 0; + out_5538305373514744468[185] = 0; + out_5538305373514744468[186] = 0; + out_5538305373514744468[187] = 0; + out_5538305373514744468[188] = 0; + out_5538305373514744468[189] = 0; + out_5538305373514744468[190] = 1; + out_5538305373514744468[191] = 0; + out_5538305373514744468[192] = 0; + out_5538305373514744468[193] = 0; + out_5538305373514744468[194] = 0; + out_5538305373514744468[195] = 0; + out_5538305373514744468[196] = 0; + out_5538305373514744468[197] = 0; + out_5538305373514744468[198] = 0; + out_5538305373514744468[199] = 0; + out_5538305373514744468[200] = 0; + out_5538305373514744468[201] = 0; + out_5538305373514744468[202] = 0; + out_5538305373514744468[203] = 0; + out_5538305373514744468[204] = 0; + out_5538305373514744468[205] = 0; + out_5538305373514744468[206] = 0; + out_5538305373514744468[207] = 0; + out_5538305373514744468[208] = 0; + out_5538305373514744468[209] = 1; + out_5538305373514744468[210] = 0; + out_5538305373514744468[211] = 0; + out_5538305373514744468[212] = 0; + out_5538305373514744468[213] = 0; + out_5538305373514744468[214] = 0; + out_5538305373514744468[215] = 0; + out_5538305373514744468[216] = 0; + out_5538305373514744468[217] = 0; + out_5538305373514744468[218] = 0; + out_5538305373514744468[219] = 0; + out_5538305373514744468[220] = 0; + out_5538305373514744468[221] = 0; + out_5538305373514744468[222] = 0; + out_5538305373514744468[223] = 0; + out_5538305373514744468[224] = 0; + out_5538305373514744468[225] = 0; + out_5538305373514744468[226] = 0; + out_5538305373514744468[227] = 0; + out_5538305373514744468[228] = 1; + out_5538305373514744468[229] = 0; + out_5538305373514744468[230] = 0; + out_5538305373514744468[231] = 0; + out_5538305373514744468[232] = 0; + out_5538305373514744468[233] = 0; + out_5538305373514744468[234] = 0; + out_5538305373514744468[235] = 0; + out_5538305373514744468[236] = 0; + out_5538305373514744468[237] = 0; + out_5538305373514744468[238] = 0; + out_5538305373514744468[239] = 0; + out_5538305373514744468[240] = 0; + out_5538305373514744468[241] = 0; + out_5538305373514744468[242] = 0; + out_5538305373514744468[243] = 0; + out_5538305373514744468[244] = 0; + out_5538305373514744468[245] = 0; + out_5538305373514744468[246] = 0; + out_5538305373514744468[247] = 1; + out_5538305373514744468[248] = 0; + out_5538305373514744468[249] = 0; + out_5538305373514744468[250] = 0; + out_5538305373514744468[251] = 0; + out_5538305373514744468[252] = 0; + out_5538305373514744468[253] = 0; + out_5538305373514744468[254] = 0; + out_5538305373514744468[255] = 0; + out_5538305373514744468[256] = 0; + out_5538305373514744468[257] = 0; + out_5538305373514744468[258] = 0; + out_5538305373514744468[259] = 0; + out_5538305373514744468[260] = 0; + out_5538305373514744468[261] = 0; + out_5538305373514744468[262] = 0; + out_5538305373514744468[263] = 0; + out_5538305373514744468[264] = 0; + out_5538305373514744468[265] = 0; + out_5538305373514744468[266] = 1; + out_5538305373514744468[267] = 0; + out_5538305373514744468[268] = 0; + out_5538305373514744468[269] = 0; + out_5538305373514744468[270] = 0; + out_5538305373514744468[271] = 0; + out_5538305373514744468[272] = 0; + out_5538305373514744468[273] = 0; + out_5538305373514744468[274] = 0; + out_5538305373514744468[275] = 0; + out_5538305373514744468[276] = 0; + out_5538305373514744468[277] = 0; + out_5538305373514744468[278] = 0; + out_5538305373514744468[279] = 0; + out_5538305373514744468[280] = 0; + out_5538305373514744468[281] = 0; + out_5538305373514744468[282] = 0; + out_5538305373514744468[283] = 0; + out_5538305373514744468[284] = 0; + out_5538305373514744468[285] = 1; + out_5538305373514744468[286] = 0; + out_5538305373514744468[287] = 0; + out_5538305373514744468[288] = 0; + out_5538305373514744468[289] = 0; + out_5538305373514744468[290] = 0; + out_5538305373514744468[291] = 0; + out_5538305373514744468[292] = 0; + out_5538305373514744468[293] = 0; + out_5538305373514744468[294] = 0; + out_5538305373514744468[295] = 0; + out_5538305373514744468[296] = 0; + out_5538305373514744468[297] = 0; + out_5538305373514744468[298] = 0; + out_5538305373514744468[299] = 0; + out_5538305373514744468[300] = 0; + out_5538305373514744468[301] = 0; + out_5538305373514744468[302] = 0; + out_5538305373514744468[303] = 0; + out_5538305373514744468[304] = 1; + out_5538305373514744468[305] = 0; + out_5538305373514744468[306] = 0; + out_5538305373514744468[307] = 0; + out_5538305373514744468[308] = 0; + out_5538305373514744468[309] = 0; + out_5538305373514744468[310] = 0; + out_5538305373514744468[311] = 0; + out_5538305373514744468[312] = 0; + out_5538305373514744468[313] = 0; + out_5538305373514744468[314] = 0; + out_5538305373514744468[315] = 0; + out_5538305373514744468[316] = 0; + out_5538305373514744468[317] = 0; + out_5538305373514744468[318] = 0; + out_5538305373514744468[319] = 0; + out_5538305373514744468[320] = 0; + out_5538305373514744468[321] = 0; + out_5538305373514744468[322] = 0; + out_5538305373514744468[323] = 1; } -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_1030168054688066876) { + out_1030168054688066876[0] = state[6] + state[9]; + out_1030168054688066876[1] = state[7] + state[10]; + out_1030168054688066876[2] = state[8] + state[11]; } -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_4(double *state, double *unused, double *out_8462847877961449813) { + out_8462847877961449813[0] = 0; + out_8462847877961449813[1] = 0; + out_8462847877961449813[2] = 0; + out_8462847877961449813[3] = 0; + out_8462847877961449813[4] = 0; + out_8462847877961449813[5] = 0; + out_8462847877961449813[6] = 1; + out_8462847877961449813[7] = 0; + out_8462847877961449813[8] = 0; + out_8462847877961449813[9] = 1; + out_8462847877961449813[10] = 0; + out_8462847877961449813[11] = 0; + out_8462847877961449813[12] = 0; + out_8462847877961449813[13] = 0; + out_8462847877961449813[14] = 0; + out_8462847877961449813[15] = 0; + out_8462847877961449813[16] = 0; + out_8462847877961449813[17] = 0; + out_8462847877961449813[18] = 0; + out_8462847877961449813[19] = 0; + out_8462847877961449813[20] = 0; + out_8462847877961449813[21] = 0; + out_8462847877961449813[22] = 0; + out_8462847877961449813[23] = 0; + out_8462847877961449813[24] = 0; + out_8462847877961449813[25] = 1; + out_8462847877961449813[26] = 0; + out_8462847877961449813[27] = 0; + out_8462847877961449813[28] = 1; + out_8462847877961449813[29] = 0; + out_8462847877961449813[30] = 0; + out_8462847877961449813[31] = 0; + out_8462847877961449813[32] = 0; + out_8462847877961449813[33] = 0; + out_8462847877961449813[34] = 0; + out_8462847877961449813[35] = 0; + out_8462847877961449813[36] = 0; + out_8462847877961449813[37] = 0; + out_8462847877961449813[38] = 0; + out_8462847877961449813[39] = 0; + out_8462847877961449813[40] = 0; + out_8462847877961449813[41] = 0; + out_8462847877961449813[42] = 0; + out_8462847877961449813[43] = 0; + out_8462847877961449813[44] = 1; + out_8462847877961449813[45] = 0; + out_8462847877961449813[46] = 0; + out_8462847877961449813[47] = 1; + out_8462847877961449813[48] = 0; + out_8462847877961449813[49] = 0; + out_8462847877961449813[50] = 0; + out_8462847877961449813[51] = 0; + out_8462847877961449813[52] = 0; + out_8462847877961449813[53] = 0; } -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_1708780435725268150) { + out_1708780435725268150[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_1708780435725268150[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_1708780435725268150[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_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_10(double *state, double *unused, double *out_764250186211597429) { + out_764250186211597429[0] = 0; + out_764250186211597429[1] = 9.8100000000000005*cos(state[1]); + out_764250186211597429[2] = 0; + out_764250186211597429[3] = 0; + out_764250186211597429[4] = -state[8]; + out_764250186211597429[5] = state[7]; + out_764250186211597429[6] = 0; + out_764250186211597429[7] = state[5]; + out_764250186211597429[8] = -state[4]; + out_764250186211597429[9] = 0; + out_764250186211597429[10] = 0; + out_764250186211597429[11] = 0; + out_764250186211597429[12] = 1; + out_764250186211597429[13] = 0; + out_764250186211597429[14] = 0; + out_764250186211597429[15] = 1; + out_764250186211597429[16] = 0; + out_764250186211597429[17] = 0; + out_764250186211597429[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_764250186211597429[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_764250186211597429[20] = 0; + out_764250186211597429[21] = state[8]; + out_764250186211597429[22] = 0; + out_764250186211597429[23] = -state[6]; + out_764250186211597429[24] = -state[5]; + out_764250186211597429[25] = 0; + out_764250186211597429[26] = state[3]; + out_764250186211597429[27] = 0; + out_764250186211597429[28] = 0; + out_764250186211597429[29] = 0; + out_764250186211597429[30] = 0; + out_764250186211597429[31] = 1; + out_764250186211597429[32] = 0; + out_764250186211597429[33] = 0; + out_764250186211597429[34] = 1; + out_764250186211597429[35] = 0; + out_764250186211597429[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_764250186211597429[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_764250186211597429[38] = 0; + out_764250186211597429[39] = -state[7]; + out_764250186211597429[40] = state[6]; + out_764250186211597429[41] = 0; + out_764250186211597429[42] = state[4]; + out_764250186211597429[43] = -state[3]; + out_764250186211597429[44] = 0; + out_764250186211597429[45] = 0; + out_764250186211597429[46] = 0; + out_764250186211597429[47] = 0; + out_764250186211597429[48] = 0; + out_764250186211597429[49] = 0; + out_764250186211597429[50] = 1; + out_764250186211597429[51] = 0; + out_764250186211597429[52] = 0; + out_764250186211597429[53] = 1; } -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_7179790802110218228) { + out_7179790802110218228[0] = state[3]; + out_7179790802110218228[1] = state[4]; + out_7179790802110218228[2] = state[5]; } -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_13(double *state, double *unused, double *out_852216669644748884) { + out_852216669644748884[0] = 0; + out_852216669644748884[1] = 0; + out_852216669644748884[2] = 0; + out_852216669644748884[3] = 1; + out_852216669644748884[4] = 0; + out_852216669644748884[5] = 0; + out_852216669644748884[6] = 0; + out_852216669644748884[7] = 0; + out_852216669644748884[8] = 0; + out_852216669644748884[9] = 0; + out_852216669644748884[10] = 0; + out_852216669644748884[11] = 0; + out_852216669644748884[12] = 0; + out_852216669644748884[13] = 0; + out_852216669644748884[14] = 0; + out_852216669644748884[15] = 0; + out_852216669644748884[16] = 0; + out_852216669644748884[17] = 0; + out_852216669644748884[18] = 0; + out_852216669644748884[19] = 0; + out_852216669644748884[20] = 0; + out_852216669644748884[21] = 0; + out_852216669644748884[22] = 1; + out_852216669644748884[23] = 0; + out_852216669644748884[24] = 0; + out_852216669644748884[25] = 0; + out_852216669644748884[26] = 0; + out_852216669644748884[27] = 0; + out_852216669644748884[28] = 0; + out_852216669644748884[29] = 0; + out_852216669644748884[30] = 0; + out_852216669644748884[31] = 0; + out_852216669644748884[32] = 0; + out_852216669644748884[33] = 0; + out_852216669644748884[34] = 0; + out_852216669644748884[35] = 0; + out_852216669644748884[36] = 0; + out_852216669644748884[37] = 0; + out_852216669644748884[38] = 0; + out_852216669644748884[39] = 0; + out_852216669644748884[40] = 0; + out_852216669644748884[41] = 1; + out_852216669644748884[42] = 0; + out_852216669644748884[43] = 0; + out_852216669644748884[44] = 0; + out_852216669644748884[45] = 0; + out_852216669644748884[46] = 0; + out_852216669644748884[47] = 0; + out_852216669644748884[48] = 0; + out_852216669644748884[49] = 0; + out_852216669644748884[50] = 0; + out_852216669644748884[51] = 0; + out_852216669644748884[52] = 0; + out_852216669644748884[53] = 0; } -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_8927767218490339382) { + out_8927767218490339382[0] = state[6]; + out_8927767218490339382[1] = state[7]; + out_8927767218490339382[2] = state[8]; } -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; +void H_14(double *state, double *unused, double *out_4499607021621965284) { + out_4499607021621965284[0] = 0; + out_4499607021621965284[1] = 0; + out_4499607021621965284[2] = 0; + out_4499607021621965284[3] = 0; + out_4499607021621965284[4] = 0; + out_4499607021621965284[5] = 0; + out_4499607021621965284[6] = 1; + out_4499607021621965284[7] = 0; + out_4499607021621965284[8] = 0; + out_4499607021621965284[9] = 0; + out_4499607021621965284[10] = 0; + out_4499607021621965284[11] = 0; + out_4499607021621965284[12] = 0; + out_4499607021621965284[13] = 0; + out_4499607021621965284[14] = 0; + out_4499607021621965284[15] = 0; + out_4499607021621965284[16] = 0; + out_4499607021621965284[17] = 0; + out_4499607021621965284[18] = 0; + out_4499607021621965284[19] = 0; + out_4499607021621965284[20] = 0; + out_4499607021621965284[21] = 0; + out_4499607021621965284[22] = 0; + out_4499607021621965284[23] = 0; + out_4499607021621965284[24] = 0; + out_4499607021621965284[25] = 1; + out_4499607021621965284[26] = 0; + out_4499607021621965284[27] = 0; + out_4499607021621965284[28] = 0; + out_4499607021621965284[29] = 0; + out_4499607021621965284[30] = 0; + out_4499607021621965284[31] = 0; + out_4499607021621965284[32] = 0; + out_4499607021621965284[33] = 0; + out_4499607021621965284[34] = 0; + out_4499607021621965284[35] = 0; + out_4499607021621965284[36] = 0; + out_4499607021621965284[37] = 0; + out_4499607021621965284[38] = 0; + out_4499607021621965284[39] = 0; + out_4499607021621965284[40] = 0; + out_4499607021621965284[41] = 0; + out_4499607021621965284[42] = 0; + out_4499607021621965284[43] = 0; + out_4499607021621965284[44] = 1; + out_4499607021621965284[45] = 0; + out_4499607021621965284[46] = 0; + out_4499607021621965284[47] = 0; + out_4499607021621965284[48] = 0; + out_4499607021621965284[49] = 0; + out_4499607021621965284[50] = 0; + out_4499607021621965284[51] = 0; + out_4499607021621965284[52] = 0; + out_4499607021621965284[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_5549195789352376606) { - err_fun(nom_x, delta_x, out_5549195789352376606); +void pose_err_fun(double *nom_x, double *delta_x, double *out_2128814047154352200) { + err_fun(nom_x, delta_x, out_2128814047154352200); } -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_inv_err_fun(double *nom_x, double *true_x, double *out_7001406389145399648) { + inv_err_fun(nom_x, true_x, out_7001406389145399648); } -void pose_H_mod_fun(double *state, double *out_932088622648113863) { - H_mod_fun(state, out_932088622648113863); +void pose_H_mod_fun(double *state, double *out_1783591311774877036) { + H_mod_fun(state, out_1783591311774877036); } -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_3415300158525823285) { + f_fun(state, dt, out_3415300158525823285); } -void pose_F_fun(double *state, double dt, double *out_3304224532205862059) { - F_fun(state, dt, out_3304224532205862059); +void pose_F_fun(double *state, double dt, double *out_5538305373514744468) { + F_fun(state, dt, out_5538305373514744468); } -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_1030168054688066876) { + h_4(state, unused, out_1030168054688066876); } -void pose_H_4(double *state, double *unused, double *out_2130036104531011967) { - H_4(state, unused, out_2130036104531011967); +void pose_H_4(double *state, double *unused, double *out_8462847877961449813) { + H_4(state, unused, out_8462847877961449813); } -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_1708780435725268150) { + h_10(state, unused, out_1708780435725268150); } -void pose_H_10(double *state, double *unused, double *out_8589748122456887360) { - H_10(state, unused, out_8589748122456887360); +void pose_H_10(double *state, double *unused, double *out_764250186211597429) { + H_10(state, unused, out_764250186211597429); } -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_7179790802110218228) { + h_13(state, unused, out_7179790802110218228); } -void pose_H_13(double *state, double *unused, double *out_5342309929863344768) { - H_13(state, unused, out_5342309929863344768); +void pose_H_13(double *state, double *unused, double *out_852216669644748884) { + H_13(state, unused, out_852216669644748884); } -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_8927767218490339382) { + h_14(state, unused, out_8927767218490339382); } -void pose_H_14(double *state, double *unused, double *out_952752327764360329) { - H_14(state, unused, out_952752327764360329); +void pose_H_14(double *state, double *unused, double *out_4499607021621965284) { + H_14(state, unused, out_4499607021621965284); } 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 47245cf5a9..0eb831f270 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_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_err_fun(double *nom_x, double *delta_x, double *out_2128814047154352200); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_7001406389145399648); +void pose_H_mod_fun(double *state, double *out_1783591311774877036); +void pose_f_fun(double *state, double dt, double *out_3415300158525823285); +void pose_F_fun(double *state, double dt, double *out_5538305373514744468); +void pose_h_4(double *state, double *unused, double *out_1030168054688066876); +void pose_H_4(double *state, double *unused, double *out_8462847877961449813); +void pose_h_10(double *state, double *unused, double *out_1708780435725268150); +void pose_H_10(double *state, double *unused, double *out_764250186211597429); +void pose_h_13(double *state, double *unused, double *out_7179790802110218228); +void pose_H_13(double *state, double *unused, double *out_852216669644748884); +void pose_h_14(double *state, double *unused, double *out_8927767218490339382); +void pose_H_14(double *state, double *unused, double *out_4499607021621965284); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/pandad/pandad b/selfdrive/pandad/pandad index ab9dc2c82a..732e7e92f1 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/system/camerad/camerad b/system/camerad/camerad index 70cdb18604..9061c55f7f 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index 9b9dbcaa88..e66604ef44 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index 54294b21e2..597d54a0c1 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index 8f9c4e9db6..aad05222d3 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ