diff --git a/cereal/libcereal.a b/cereal/libcereal.a index a242c1f0f8..a6622f3d63 100644 Binary files a/cereal/libcereal.a and b/cereal/libcereal.a differ diff --git a/common/libcommon.a b/common/libcommon.a index b1e4156207..e7622ba0e9 100644 Binary files a/common/libcommon.a and b/common/libcommon.a differ diff --git a/common/params_pyx.so b/common/params_pyx.so index 07f50262c7..9c3933b07c 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index 132dfa4a61..0903ba171d 100644 Binary files a/panda/board/obj/body_h7.bin.signed and b/panda/board/obj/body_h7.bin.signed differ diff --git a/panda/board/obj/body_h7/bootstub.elf b/panda/board/obj/body_h7/bootstub.elf index fd221b2850..9e5ea90f3d 100755 Binary files a/panda/board/obj/body_h7/bootstub.elf and b/panda/board/obj/body_h7/bootstub.elf differ diff --git a/panda/board/obj/body_h7/main.bin b/panda/board/obj/body_h7/main.bin index 2d1a06d7b1..c95bc0ae8f 100755 Binary files a/panda/board/obj/body_h7/main.bin and b/panda/board/obj/body_h7/main.bin differ diff --git a/panda/board/obj/body_h7/main.elf b/panda/board/obj/body_h7/main.elf index ad5499cdeb..90f8a05932 100755 Binary files a/panda/board/obj/body_h7/main.elf and b/panda/board/obj/body_h7/main.elf differ diff --git a/panda/board/obj/bootstub.body_h7.bin b/panda/board/obj/bootstub.body_h7.bin index 5756733954..86d219d85b 100755 Binary files a/panda/board/obj/bootstub.body_h7.bin and b/panda/board/obj/bootstub.body_h7.bin differ diff --git a/panda/board/obj/bootstub.panda.bin b/panda/board/obj/bootstub.panda.bin index fc49547bfc..a9a0c7ce16 100755 Binary files a/panda/board/obj/bootstub.panda.bin and b/panda/board/obj/bootstub.panda.bin differ diff --git a/panda/board/obj/bootstub.panda_can_ignition_only.bin b/panda/board/obj/bootstub.panda_can_ignition_only.bin index fc49547bfc..a9a0c7ce16 100755 Binary files a/panda/board/obj/bootstub.panda_can_ignition_only.bin and b/panda/board/obj/bootstub.panda_can_ignition_only.bin differ diff --git a/panda/board/obj/bootstub.panda_h7.bin b/panda/board/obj/bootstub.panda_h7.bin index da67a70b8c..23a667f1dc 100755 Binary files a/panda/board/obj/bootstub.panda_h7.bin and b/panda/board/obj/bootstub.panda_h7.bin differ diff --git a/panda/board/obj/bootstub.panda_h7_can_ignition_only.bin b/panda/board/obj/bootstub.panda_h7_can_ignition_only.bin index da67a70b8c..23a667f1dc 100755 Binary files a/panda/board/obj/bootstub.panda_h7_can_ignition_only.bin and b/panda/board/obj/bootstub.panda_h7_can_ignition_only.bin differ diff --git a/panda/board/obj/bootstub.panda_h7_hkg_remote.bin b/panda/board/obj/bootstub.panda_h7_hkg_remote.bin index da67a70b8c..23a667f1dc 100755 Binary files a/panda/board/obj/bootstub.panda_h7_hkg_remote.bin and b/panda/board/obj/bootstub.panda_h7_hkg_remote.bin differ diff --git a/panda/board/obj/bootstub.panda_h7_hkg_remote_can_ignition_only.bin b/panda/board/obj/bootstub.panda_h7_hkg_remote_can_ignition_only.bin index da67a70b8c..23a667f1dc 100755 Binary files a/panda/board/obj/bootstub.panda_h7_hkg_remote_can_ignition_only.bin and b/panda/board/obj/bootstub.panda_h7_hkg_remote_can_ignition_only.bin differ diff --git a/panda/board/obj/bootstub.panda_h7_remote.bin b/panda/board/obj/bootstub.panda_h7_remote.bin index da67a70b8c..23a667f1dc 100755 Binary files a/panda/board/obj/bootstub.panda_h7_remote.bin and b/panda/board/obj/bootstub.panda_h7_remote.bin differ diff --git a/panda/board/obj/bootstub.panda_h7_remote_can_ignition_only.bin b/panda/board/obj/bootstub.panda_h7_remote_can_ignition_only.bin index da67a70b8c..23a667f1dc 100755 Binary files a/panda/board/obj/bootstub.panda_h7_remote_can_ignition_only.bin and b/panda/board/obj/bootstub.panda_h7_remote_can_ignition_only.bin differ diff --git a/panda/board/obj/bootstub.panda_h7_tesla_wake.bin b/panda/board/obj/bootstub.panda_h7_tesla_wake.bin index da67a70b8c..23a667f1dc 100755 Binary files a/panda/board/obj/bootstub.panda_h7_tesla_wake.bin and b/panda/board/obj/bootstub.panda_h7_tesla_wake.bin differ diff --git a/panda/board/obj/bootstub.panda_h7_tesla_wake_can_ignition_only.bin b/panda/board/obj/bootstub.panda_h7_tesla_wake_can_ignition_only.bin index da67a70b8c..23a667f1dc 100755 Binary files a/panda/board/obj/bootstub.panda_h7_tesla_wake_can_ignition_only.bin and b/panda/board/obj/bootstub.panda_h7_tesla_wake_can_ignition_only.bin differ diff --git a/panda/board/obj/bootstub.panda_hkg_remote.bin b/panda/board/obj/bootstub.panda_hkg_remote.bin index fc49547bfc..a9a0c7ce16 100755 Binary files a/panda/board/obj/bootstub.panda_hkg_remote.bin and b/panda/board/obj/bootstub.panda_hkg_remote.bin differ diff --git a/panda/board/obj/bootstub.panda_hkg_remote_can_ignition_only.bin b/panda/board/obj/bootstub.panda_hkg_remote_can_ignition_only.bin index fc49547bfc..a9a0c7ce16 100755 Binary files a/panda/board/obj/bootstub.panda_hkg_remote_can_ignition_only.bin and b/panda/board/obj/bootstub.panda_hkg_remote_can_ignition_only.bin differ diff --git a/panda/board/obj/bootstub.panda_jungle_h7.bin b/panda/board/obj/bootstub.panda_jungle_h7.bin index 3f5c5b974c..fd95d18696 100755 Binary files a/panda/board/obj/bootstub.panda_jungle_h7.bin and b/panda/board/obj/bootstub.panda_jungle_h7.bin differ diff --git a/panda/board/obj/bootstub.panda_remote.bin b/panda/board/obj/bootstub.panda_remote.bin index fc49547bfc..a9a0c7ce16 100755 Binary files a/panda/board/obj/bootstub.panda_remote.bin and b/panda/board/obj/bootstub.panda_remote.bin differ diff --git a/panda/board/obj/bootstub.panda_remote_can_ignition_only.bin b/panda/board/obj/bootstub.panda_remote_can_ignition_only.bin index fc49547bfc..a9a0c7ce16 100755 Binary files a/panda/board/obj/bootstub.panda_remote_can_ignition_only.bin and b/panda/board/obj/bootstub.panda_remote_can_ignition_only.bin differ diff --git a/panda/board/obj/bootstub.panda_tesla_wake.bin b/panda/board/obj/bootstub.panda_tesla_wake.bin index fc49547bfc..a9a0c7ce16 100755 Binary files a/panda/board/obj/bootstub.panda_tesla_wake.bin and b/panda/board/obj/bootstub.panda_tesla_wake.bin differ diff --git a/panda/board/obj/bootstub.panda_tesla_wake_can_ignition_only.bin b/panda/board/obj/bootstub.panda_tesla_wake_can_ignition_only.bin index fc49547bfc..a9a0c7ce16 100755 Binary files a/panda/board/obj/bootstub.panda_tesla_wake_can_ignition_only.bin and b/panda/board/obj/bootstub.panda_tesla_wake_can_ignition_only.bin differ diff --git a/panda/board/obj/gitversion.h b/panda/board/obj/gitversion.h index 048d623b57..294227b898 100644 --- a/panda/board/obj/gitversion.h +++ b/panda/board/obj/gitversion.h @@ -1,2 +1,2 @@ extern const uint8_t gitversion[19]; -const uint8_t gitversion[19] = "DEV-14370fe9-DEBUG"; +const uint8_t gitversion[19] = "DEV-e9b31807-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 2c74c4dea1..d437a5151a 100644 Binary files a/panda/board/obj/panda.bin.signed and b/panda/board/obj/panda.bin.signed differ diff --git a/panda/board/obj/panda/bootstub.elf b/panda/board/obj/panda/bootstub.elf index 8da208df13..a11c66a143 100755 Binary files a/panda/board/obj/panda/bootstub.elf and b/panda/board/obj/panda/bootstub.elf differ diff --git a/panda/board/obj/panda/main.bin b/panda/board/obj/panda/main.bin index 6851c0c242..74ebfeafb7 100755 Binary files a/panda/board/obj/panda/main.bin and b/panda/board/obj/panda/main.bin differ diff --git a/panda/board/obj/panda/main.elf b/panda/board/obj/panda/main.elf index 86392f60c3..5d95cdeefc 100755 Binary files a/panda/board/obj/panda/main.elf and b/panda/board/obj/panda/main.elf differ diff --git a/panda/board/obj/panda_can_ignition_only.bin.signed b/panda/board/obj/panda_can_ignition_only.bin.signed index d254d9797a..541ac2b3a0 100644 Binary files a/panda/board/obj/panda_can_ignition_only.bin.signed and b/panda/board/obj/panda_can_ignition_only.bin.signed differ diff --git a/panda/board/obj/panda_can_ignition_only/bootstub.elf b/panda/board/obj/panda_can_ignition_only/bootstub.elf index ed85d98591..391b46c1df 100755 Binary files a/panda/board/obj/panda_can_ignition_only/bootstub.elf and b/panda/board/obj/panda_can_ignition_only/bootstub.elf differ diff --git a/panda/board/obj/panda_can_ignition_only/main.bin b/panda/board/obj/panda_can_ignition_only/main.bin index baf15fa984..5119d759ab 100755 Binary files a/panda/board/obj/panda_can_ignition_only/main.bin and b/panda/board/obj/panda_can_ignition_only/main.bin differ diff --git a/panda/board/obj/panda_can_ignition_only/main.elf b/panda/board/obj/panda_can_ignition_only/main.elf index e2a5b2e375..0581c579de 100755 Binary files a/panda/board/obj/panda_can_ignition_only/main.elf and b/panda/board/obj/panda_can_ignition_only/main.elf differ diff --git a/panda/board/obj/panda_h7.bin.signed b/panda/board/obj/panda_h7.bin.signed index ece8a6279b..600640d627 100644 Binary files a/panda/board/obj/panda_h7.bin.signed and b/panda/board/obj/panda_h7.bin.signed differ diff --git a/panda/board/obj/panda_h7/bootstub.elf b/panda/board/obj/panda_h7/bootstub.elf index 218e7e506a..10c968854c 100755 Binary files a/panda/board/obj/panda_h7/bootstub.elf and b/panda/board/obj/panda_h7/bootstub.elf differ diff --git a/panda/board/obj/panda_h7/main.bin b/panda/board/obj/panda_h7/main.bin index 34a60ad268..db4db46b40 100755 Binary files a/panda/board/obj/panda_h7/main.bin and b/panda/board/obj/panda_h7/main.bin differ diff --git a/panda/board/obj/panda_h7/main.elf b/panda/board/obj/panda_h7/main.elf index 3bd694fa08..8f36813167 100755 Binary files a/panda/board/obj/panda_h7/main.elf and b/panda/board/obj/panda_h7/main.elf differ diff --git a/panda/board/obj/panda_h7_can_ignition_only.bin.signed b/panda/board/obj/panda_h7_can_ignition_only.bin.signed index 150501e8fb..99bb4c8a15 100644 Binary files a/panda/board/obj/panda_h7_can_ignition_only.bin.signed and b/panda/board/obj/panda_h7_can_ignition_only.bin.signed differ diff --git a/panda/board/obj/panda_h7_can_ignition_only/bootstub.elf b/panda/board/obj/panda_h7_can_ignition_only/bootstub.elf index 56948ddeb1..86aeb33964 100755 Binary files a/panda/board/obj/panda_h7_can_ignition_only/bootstub.elf and b/panda/board/obj/panda_h7_can_ignition_only/bootstub.elf differ diff --git a/panda/board/obj/panda_h7_can_ignition_only/main.bin b/panda/board/obj/panda_h7_can_ignition_only/main.bin index 7701832c87..c0fe08565b 100755 Binary files a/panda/board/obj/panda_h7_can_ignition_only/main.bin and b/panda/board/obj/panda_h7_can_ignition_only/main.bin differ diff --git a/panda/board/obj/panda_h7_can_ignition_only/main.elf b/panda/board/obj/panda_h7_can_ignition_only/main.elf index e7426f9f46..a52685481b 100755 Binary files a/panda/board/obj/panda_h7_can_ignition_only/main.elf and b/panda/board/obj/panda_h7_can_ignition_only/main.elf differ diff --git a/panda/board/obj/panda_h7_hkg_remote.bin.signed b/panda/board/obj/panda_h7_hkg_remote.bin.signed index ec45221611..3a6c2ac325 100644 Binary files a/panda/board/obj/panda_h7_hkg_remote.bin.signed and b/panda/board/obj/panda_h7_hkg_remote.bin.signed differ diff --git a/panda/board/obj/panda_h7_hkg_remote/bootstub.elf b/panda/board/obj/panda_h7_hkg_remote/bootstub.elf index b78a90f6aa..e7ba93d814 100755 Binary files a/panda/board/obj/panda_h7_hkg_remote/bootstub.elf and b/panda/board/obj/panda_h7_hkg_remote/bootstub.elf differ diff --git a/panda/board/obj/panda_h7_hkg_remote/main.bin b/panda/board/obj/panda_h7_hkg_remote/main.bin index 82b31668c2..2c4c5b4ebb 100755 Binary files a/panda/board/obj/panda_h7_hkg_remote/main.bin and b/panda/board/obj/panda_h7_hkg_remote/main.bin differ diff --git a/panda/board/obj/panda_h7_hkg_remote/main.elf b/panda/board/obj/panda_h7_hkg_remote/main.elf index cb377a73a6..5a58e2488f 100755 Binary files a/panda/board/obj/panda_h7_hkg_remote/main.elf and b/panda/board/obj/panda_h7_hkg_remote/main.elf differ diff --git a/panda/board/obj/panda_h7_hkg_remote_can_ignition_only.bin.signed b/panda/board/obj/panda_h7_hkg_remote_can_ignition_only.bin.signed index b094b25a56..8d21022c9f 100644 Binary files a/panda/board/obj/panda_h7_hkg_remote_can_ignition_only.bin.signed and b/panda/board/obj/panda_h7_hkg_remote_can_ignition_only.bin.signed differ diff --git a/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/bootstub.elf b/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/bootstub.elf index 70d5fb44c9..666adff531 100755 Binary files a/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/bootstub.elf and b/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/bootstub.elf differ diff --git a/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.bin b/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.bin index a0825a8cad..6bad3d3ba5 100755 Binary files a/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.bin and b/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.bin differ diff --git a/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.elf b/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.elf index 3eeface422..97a2a1e4dc 100755 Binary files a/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.elf and b/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.elf differ diff --git a/panda/board/obj/panda_h7_remote.bin.signed b/panda/board/obj/panda_h7_remote.bin.signed index c175e003a2..d8ebcd7b70 100644 Binary files a/panda/board/obj/panda_h7_remote.bin.signed and b/panda/board/obj/panda_h7_remote.bin.signed differ diff --git a/panda/board/obj/panda_h7_remote/bootstub.elf b/panda/board/obj/panda_h7_remote/bootstub.elf index de5e5d1707..0bb0c1b3ca 100755 Binary files a/panda/board/obj/panda_h7_remote/bootstub.elf and b/panda/board/obj/panda_h7_remote/bootstub.elf differ diff --git a/panda/board/obj/panda_h7_remote/main.bin b/panda/board/obj/panda_h7_remote/main.bin index aae1ec6c8d..c6e09c3a39 100755 Binary files a/panda/board/obj/panda_h7_remote/main.bin and b/panda/board/obj/panda_h7_remote/main.bin differ diff --git a/panda/board/obj/panda_h7_remote/main.elf b/panda/board/obj/panda_h7_remote/main.elf index 9fe86cb5d7..0c7b17f59f 100755 Binary files a/panda/board/obj/panda_h7_remote/main.elf and b/panda/board/obj/panda_h7_remote/main.elf differ diff --git a/panda/board/obj/panda_h7_remote_can_ignition_only.bin.signed b/panda/board/obj/panda_h7_remote_can_ignition_only.bin.signed index c00fcd6d3d..2802dc39ae 100644 Binary files a/panda/board/obj/panda_h7_remote_can_ignition_only.bin.signed and b/panda/board/obj/panda_h7_remote_can_ignition_only.bin.signed differ diff --git a/panda/board/obj/panda_h7_remote_can_ignition_only/bootstub.elf b/panda/board/obj/panda_h7_remote_can_ignition_only/bootstub.elf index 49db04db7a..6b2d06235c 100755 Binary files a/panda/board/obj/panda_h7_remote_can_ignition_only/bootstub.elf and b/panda/board/obj/panda_h7_remote_can_ignition_only/bootstub.elf differ diff --git a/panda/board/obj/panda_h7_remote_can_ignition_only/main.bin b/panda/board/obj/panda_h7_remote_can_ignition_only/main.bin index 96858abeb2..10f6f006d3 100755 Binary files a/panda/board/obj/panda_h7_remote_can_ignition_only/main.bin and b/panda/board/obj/panda_h7_remote_can_ignition_only/main.bin differ diff --git a/panda/board/obj/panda_h7_remote_can_ignition_only/main.elf b/panda/board/obj/panda_h7_remote_can_ignition_only/main.elf index 477b411ea2..5ea3ab5772 100755 Binary files a/panda/board/obj/panda_h7_remote_can_ignition_only/main.elf and b/panda/board/obj/panda_h7_remote_can_ignition_only/main.elf differ diff --git a/panda/board/obj/panda_h7_tesla_wake.bin.signed b/panda/board/obj/panda_h7_tesla_wake.bin.signed index 5ff7d898d1..0a7eada3c6 100644 Binary files a/panda/board/obj/panda_h7_tesla_wake.bin.signed and b/panda/board/obj/panda_h7_tesla_wake.bin.signed differ diff --git a/panda/board/obj/panda_h7_tesla_wake/bootstub.elf b/panda/board/obj/panda_h7_tesla_wake/bootstub.elf index 3bbd8fae9f..a2eaaca618 100755 Binary files a/panda/board/obj/panda_h7_tesla_wake/bootstub.elf and b/panda/board/obj/panda_h7_tesla_wake/bootstub.elf differ diff --git a/panda/board/obj/panda_h7_tesla_wake/main.bin b/panda/board/obj/panda_h7_tesla_wake/main.bin index 1f3c8b53dd..1d892d70c9 100755 Binary files a/panda/board/obj/panda_h7_tesla_wake/main.bin and b/panda/board/obj/panda_h7_tesla_wake/main.bin differ diff --git a/panda/board/obj/panda_h7_tesla_wake/main.elf b/panda/board/obj/panda_h7_tesla_wake/main.elf index 95c9da2126..189daaa9be 100755 Binary files a/panda/board/obj/panda_h7_tesla_wake/main.elf and b/panda/board/obj/panda_h7_tesla_wake/main.elf differ diff --git a/panda/board/obj/panda_h7_tesla_wake_can_ignition_only.bin.signed b/panda/board/obj/panda_h7_tesla_wake_can_ignition_only.bin.signed index 1cb003bfd0..ab5700e049 100644 Binary files a/panda/board/obj/panda_h7_tesla_wake_can_ignition_only.bin.signed and b/panda/board/obj/panda_h7_tesla_wake_can_ignition_only.bin.signed differ diff --git a/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/bootstub.elf b/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/bootstub.elf index 42a2560ce4..da19819447 100755 Binary files a/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/bootstub.elf and b/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/bootstub.elf differ diff --git a/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/main.bin b/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/main.bin index dab715e12a..789456c562 100755 Binary files a/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/main.bin and b/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/main.bin differ diff --git a/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/main.elf b/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/main.elf index d9dc76cba6..61b57f22b6 100755 Binary files a/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/main.elf and b/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/main.elf differ diff --git a/panda/board/obj/panda_hkg_remote.bin.signed b/panda/board/obj/panda_hkg_remote.bin.signed index 30ae17e5ae..e31408acb8 100644 Binary files a/panda/board/obj/panda_hkg_remote.bin.signed and b/panda/board/obj/panda_hkg_remote.bin.signed differ diff --git a/panda/board/obj/panda_hkg_remote/bootstub.elf b/panda/board/obj/panda_hkg_remote/bootstub.elf index 9b10eb06e2..43ed3d8aa0 100755 Binary files a/panda/board/obj/panda_hkg_remote/bootstub.elf and b/panda/board/obj/panda_hkg_remote/bootstub.elf differ diff --git a/panda/board/obj/panda_hkg_remote/main.bin b/panda/board/obj/panda_hkg_remote/main.bin index daee8677d2..b255d01d05 100755 Binary files a/panda/board/obj/panda_hkg_remote/main.bin and b/panda/board/obj/panda_hkg_remote/main.bin differ diff --git a/panda/board/obj/panda_hkg_remote/main.elf b/panda/board/obj/panda_hkg_remote/main.elf index 4813a3c923..eae2801e8a 100755 Binary files a/panda/board/obj/panda_hkg_remote/main.elf and b/panda/board/obj/panda_hkg_remote/main.elf differ diff --git a/panda/board/obj/panda_hkg_remote_can_ignition_only.bin.signed b/panda/board/obj/panda_hkg_remote_can_ignition_only.bin.signed index 2fdbcb040a..c3a031fac8 100644 Binary files a/panda/board/obj/panda_hkg_remote_can_ignition_only.bin.signed and b/panda/board/obj/panda_hkg_remote_can_ignition_only.bin.signed differ diff --git a/panda/board/obj/panda_hkg_remote_can_ignition_only/bootstub.elf b/panda/board/obj/panda_hkg_remote_can_ignition_only/bootstub.elf index 2b54184adb..ac68c47f72 100755 Binary files a/panda/board/obj/panda_hkg_remote_can_ignition_only/bootstub.elf and b/panda/board/obj/panda_hkg_remote_can_ignition_only/bootstub.elf differ diff --git a/panda/board/obj/panda_hkg_remote_can_ignition_only/main.bin b/panda/board/obj/panda_hkg_remote_can_ignition_only/main.bin index b65be9af77..1d61936ab9 100755 Binary files a/panda/board/obj/panda_hkg_remote_can_ignition_only/main.bin and b/panda/board/obj/panda_hkg_remote_can_ignition_only/main.bin differ diff --git a/panda/board/obj/panda_hkg_remote_can_ignition_only/main.elf b/panda/board/obj/panda_hkg_remote_can_ignition_only/main.elf index 5d6d02421d..fa942fe985 100755 Binary files a/panda/board/obj/panda_hkg_remote_can_ignition_only/main.elf and b/panda/board/obj/panda_hkg_remote_can_ignition_only/main.elf differ diff --git a/panda/board/obj/panda_jungle_h7.bin.signed b/panda/board/obj/panda_jungle_h7.bin.signed index 38c6926606..4cd69c9d54 100644 Binary files a/panda/board/obj/panda_jungle_h7.bin.signed and b/panda/board/obj/panda_jungle_h7.bin.signed differ diff --git a/panda/board/obj/panda_jungle_h7/bootstub.elf b/panda/board/obj/panda_jungle_h7/bootstub.elf index 0b251a368d..c1d476fdef 100755 Binary files a/panda/board/obj/panda_jungle_h7/bootstub.elf and b/panda/board/obj/panda_jungle_h7/bootstub.elf differ diff --git a/panda/board/obj/panda_jungle_h7/main.bin b/panda/board/obj/panda_jungle_h7/main.bin index 44aab407f2..80608ffcbb 100755 Binary files a/panda/board/obj/panda_jungle_h7/main.bin and b/panda/board/obj/panda_jungle_h7/main.bin differ diff --git a/panda/board/obj/panda_jungle_h7/main.elf b/panda/board/obj/panda_jungle_h7/main.elf index c56bdb01e1..02a7518150 100755 Binary files a/panda/board/obj/panda_jungle_h7/main.elf and b/panda/board/obj/panda_jungle_h7/main.elf differ diff --git a/panda/board/obj/panda_remote.bin.signed b/panda/board/obj/panda_remote.bin.signed index 0d3b4fd0b2..d39443517b 100644 Binary files a/panda/board/obj/panda_remote.bin.signed and b/panda/board/obj/panda_remote.bin.signed differ diff --git a/panda/board/obj/panda_remote/bootstub.elf b/panda/board/obj/panda_remote/bootstub.elf index 48d7d2b748..45a56104c7 100755 Binary files a/panda/board/obj/panda_remote/bootstub.elf and b/panda/board/obj/panda_remote/bootstub.elf differ diff --git a/panda/board/obj/panda_remote/main.bin b/panda/board/obj/panda_remote/main.bin index 2172f591c2..b32bc99f20 100755 Binary files a/panda/board/obj/panda_remote/main.bin and b/panda/board/obj/panda_remote/main.bin differ diff --git a/panda/board/obj/panda_remote/main.elf b/panda/board/obj/panda_remote/main.elf index 5a83b4c085..4a92faa66f 100755 Binary files a/panda/board/obj/panda_remote/main.elf and b/panda/board/obj/panda_remote/main.elf differ diff --git a/panda/board/obj/panda_remote_can_ignition_only.bin.signed b/panda/board/obj/panda_remote_can_ignition_only.bin.signed index 0f82f3aea3..31679c965f 100644 Binary files a/panda/board/obj/panda_remote_can_ignition_only.bin.signed and b/panda/board/obj/panda_remote_can_ignition_only.bin.signed differ diff --git a/panda/board/obj/panda_remote_can_ignition_only/bootstub.elf b/panda/board/obj/panda_remote_can_ignition_only/bootstub.elf index bc141088ce..19d50bd650 100755 Binary files a/panda/board/obj/panda_remote_can_ignition_only/bootstub.elf and b/panda/board/obj/panda_remote_can_ignition_only/bootstub.elf differ diff --git a/panda/board/obj/panda_remote_can_ignition_only/main.bin b/panda/board/obj/panda_remote_can_ignition_only/main.bin index 419d992cba..d66da5a4ab 100755 Binary files a/panda/board/obj/panda_remote_can_ignition_only/main.bin and b/panda/board/obj/panda_remote_can_ignition_only/main.bin differ diff --git a/panda/board/obj/panda_remote_can_ignition_only/main.elf b/panda/board/obj/panda_remote_can_ignition_only/main.elf index 592e14d570..fa527eb238 100755 Binary files a/panda/board/obj/panda_remote_can_ignition_only/main.elf and b/panda/board/obj/panda_remote_can_ignition_only/main.elf differ diff --git a/panda/board/obj/panda_tesla_wake.bin.signed b/panda/board/obj/panda_tesla_wake.bin.signed index f6999e30d2..8bb3950e87 100644 Binary files a/panda/board/obj/panda_tesla_wake.bin.signed and b/panda/board/obj/panda_tesla_wake.bin.signed differ diff --git a/panda/board/obj/panda_tesla_wake/bootstub.elf b/panda/board/obj/panda_tesla_wake/bootstub.elf index 0feec8e51c..9fd5d5fd18 100755 Binary files a/panda/board/obj/panda_tesla_wake/bootstub.elf and b/panda/board/obj/panda_tesla_wake/bootstub.elf differ diff --git a/panda/board/obj/panda_tesla_wake/main.bin b/panda/board/obj/panda_tesla_wake/main.bin index 45207317d4..de6c902225 100755 Binary files a/panda/board/obj/panda_tesla_wake/main.bin and b/panda/board/obj/panda_tesla_wake/main.bin differ diff --git a/panda/board/obj/panda_tesla_wake/main.elf b/panda/board/obj/panda_tesla_wake/main.elf index d5a02f81ae..6e26eb5a6d 100755 Binary files a/panda/board/obj/panda_tesla_wake/main.elf and b/panda/board/obj/panda_tesla_wake/main.elf differ diff --git a/panda/board/obj/panda_tesla_wake_can_ignition_only.bin.signed b/panda/board/obj/panda_tesla_wake_can_ignition_only.bin.signed index 7884e40b50..3e8d5fbbbd 100644 Binary files a/panda/board/obj/panda_tesla_wake_can_ignition_only.bin.signed and b/panda/board/obj/panda_tesla_wake_can_ignition_only.bin.signed differ diff --git a/panda/board/obj/panda_tesla_wake_can_ignition_only/bootstub.elf b/panda/board/obj/panda_tesla_wake_can_ignition_only/bootstub.elf index f0e7097508..83a25d7d46 100755 Binary files a/panda/board/obj/panda_tesla_wake_can_ignition_only/bootstub.elf and b/panda/board/obj/panda_tesla_wake_can_ignition_only/bootstub.elf differ diff --git a/panda/board/obj/panda_tesla_wake_can_ignition_only/main.bin b/panda/board/obj/panda_tesla_wake_can_ignition_only/main.bin index 90178ee825..d68fe8125d 100755 Binary files a/panda/board/obj/panda_tesla_wake_can_ignition_only/main.bin and b/panda/board/obj/panda_tesla_wake_can_ignition_only/main.bin differ diff --git a/panda/board/obj/panda_tesla_wake_can_ignition_only/main.elf b/panda/board/obj/panda_tesla_wake_can_ignition_only/main.elf index 3408cdef1e..eb1e3d1e90 100755 Binary files a/panda/board/obj/panda_tesla_wake_can_ignition_only/main.elf and b/panda/board/obj/panda_tesla_wake_can_ignition_only/main.elf differ diff --git a/panda/board/obj/version b/panda/board/obj/version index f565edc88d..6dd3d23f3e 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-14370fe9-DEBUG \ No newline at end of file +DEV-e9b31807-DEBUG \ No newline at end of file diff --git a/rednose_repo/rednose/helpers/ekf_sym_pyx.so b/rednose_repo/rednose/helpers/ekf_sym_pyx.so index 985b6c7854..0c2e6104d9 100755 Binary files a/rednose_repo/rednose/helpers/ekf_sym_pyx.so and b/rednose_repo/rednose/helpers/ekf_sym_pyx.so differ diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 141d2f9843..65f2e8eeb4 100644 --- a/selfdrive/locationd/models/generated/car.cpp +++ b/selfdrive/locationd/models/generated/car.cpp @@ -45,326 +45,326 @@ const static double MAHA_THRESH_31 = 3.8414588206941227; * * * This file is part of 'ekf' * ******************************************************************************/ -void err_fun(double *nom_x, double *delta_x, double *out_6314153535178174931) { - out_6314153535178174931[0] = delta_x[0] + nom_x[0]; - out_6314153535178174931[1] = delta_x[1] + nom_x[1]; - out_6314153535178174931[2] = delta_x[2] + nom_x[2]; - out_6314153535178174931[3] = delta_x[3] + nom_x[3]; - out_6314153535178174931[4] = delta_x[4] + nom_x[4]; - out_6314153535178174931[5] = delta_x[5] + nom_x[5]; - out_6314153535178174931[6] = delta_x[6] + nom_x[6]; - out_6314153535178174931[7] = delta_x[7] + nom_x[7]; - out_6314153535178174931[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_4276335218073679407) { + out_4276335218073679407[0] = delta_x[0] + nom_x[0]; + out_4276335218073679407[1] = delta_x[1] + nom_x[1]; + out_4276335218073679407[2] = delta_x[2] + nom_x[2]; + out_4276335218073679407[3] = delta_x[3] + nom_x[3]; + out_4276335218073679407[4] = delta_x[4] + nom_x[4]; + out_4276335218073679407[5] = delta_x[5] + nom_x[5]; + out_4276335218073679407[6] = delta_x[6] + nom_x[6]; + out_4276335218073679407[7] = delta_x[7] + nom_x[7]; + out_4276335218073679407[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_7049921009392106379) { - out_7049921009392106379[0] = -nom_x[0] + true_x[0]; - out_7049921009392106379[1] = -nom_x[1] + true_x[1]; - out_7049921009392106379[2] = -nom_x[2] + true_x[2]; - out_7049921009392106379[3] = -nom_x[3] + true_x[3]; - out_7049921009392106379[4] = -nom_x[4] + true_x[4]; - out_7049921009392106379[5] = -nom_x[5] + true_x[5]; - out_7049921009392106379[6] = -nom_x[6] + true_x[6]; - out_7049921009392106379[7] = -nom_x[7] + true_x[7]; - out_7049921009392106379[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_3738278089596769997) { + out_3738278089596769997[0] = -nom_x[0] + true_x[0]; + out_3738278089596769997[1] = -nom_x[1] + true_x[1]; + out_3738278089596769997[2] = -nom_x[2] + true_x[2]; + out_3738278089596769997[3] = -nom_x[3] + true_x[3]; + out_3738278089596769997[4] = -nom_x[4] + true_x[4]; + out_3738278089596769997[5] = -nom_x[5] + true_x[5]; + out_3738278089596769997[6] = -nom_x[6] + true_x[6]; + out_3738278089596769997[7] = -nom_x[7] + true_x[7]; + out_3738278089596769997[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_9070114660809295187) { - out_9070114660809295187[0] = 1.0; - out_9070114660809295187[1] = 0.0; - out_9070114660809295187[2] = 0.0; - out_9070114660809295187[3] = 0.0; - out_9070114660809295187[4] = 0.0; - out_9070114660809295187[5] = 0.0; - out_9070114660809295187[6] = 0.0; - out_9070114660809295187[7] = 0.0; - out_9070114660809295187[8] = 0.0; - out_9070114660809295187[9] = 0.0; - out_9070114660809295187[10] = 1.0; - out_9070114660809295187[11] = 0.0; - out_9070114660809295187[12] = 0.0; - out_9070114660809295187[13] = 0.0; - out_9070114660809295187[14] = 0.0; - out_9070114660809295187[15] = 0.0; - out_9070114660809295187[16] = 0.0; - out_9070114660809295187[17] = 0.0; - out_9070114660809295187[18] = 0.0; - out_9070114660809295187[19] = 0.0; - out_9070114660809295187[20] = 1.0; - out_9070114660809295187[21] = 0.0; - out_9070114660809295187[22] = 0.0; - out_9070114660809295187[23] = 0.0; - out_9070114660809295187[24] = 0.0; - out_9070114660809295187[25] = 0.0; - out_9070114660809295187[26] = 0.0; - out_9070114660809295187[27] = 0.0; - out_9070114660809295187[28] = 0.0; - out_9070114660809295187[29] = 0.0; - out_9070114660809295187[30] = 1.0; - out_9070114660809295187[31] = 0.0; - out_9070114660809295187[32] = 0.0; - out_9070114660809295187[33] = 0.0; - out_9070114660809295187[34] = 0.0; - out_9070114660809295187[35] = 0.0; - out_9070114660809295187[36] = 0.0; - out_9070114660809295187[37] = 0.0; - out_9070114660809295187[38] = 0.0; - out_9070114660809295187[39] = 0.0; - out_9070114660809295187[40] = 1.0; - out_9070114660809295187[41] = 0.0; - out_9070114660809295187[42] = 0.0; - out_9070114660809295187[43] = 0.0; - out_9070114660809295187[44] = 0.0; - out_9070114660809295187[45] = 0.0; - out_9070114660809295187[46] = 0.0; - out_9070114660809295187[47] = 0.0; - out_9070114660809295187[48] = 0.0; - out_9070114660809295187[49] = 0.0; - out_9070114660809295187[50] = 1.0; - out_9070114660809295187[51] = 0.0; - out_9070114660809295187[52] = 0.0; - out_9070114660809295187[53] = 0.0; - out_9070114660809295187[54] = 0.0; - out_9070114660809295187[55] = 0.0; - out_9070114660809295187[56] = 0.0; - out_9070114660809295187[57] = 0.0; - out_9070114660809295187[58] = 0.0; - out_9070114660809295187[59] = 0.0; - out_9070114660809295187[60] = 1.0; - out_9070114660809295187[61] = 0.0; - out_9070114660809295187[62] = 0.0; - out_9070114660809295187[63] = 0.0; - out_9070114660809295187[64] = 0.0; - out_9070114660809295187[65] = 0.0; - out_9070114660809295187[66] = 0.0; - out_9070114660809295187[67] = 0.0; - out_9070114660809295187[68] = 0.0; - out_9070114660809295187[69] = 0.0; - out_9070114660809295187[70] = 1.0; - out_9070114660809295187[71] = 0.0; - out_9070114660809295187[72] = 0.0; - out_9070114660809295187[73] = 0.0; - out_9070114660809295187[74] = 0.0; - out_9070114660809295187[75] = 0.0; - out_9070114660809295187[76] = 0.0; - out_9070114660809295187[77] = 0.0; - out_9070114660809295187[78] = 0.0; - out_9070114660809295187[79] = 0.0; - out_9070114660809295187[80] = 1.0; +void H_mod_fun(double *state, double *out_8858046684809825962) { + out_8858046684809825962[0] = 1.0; + out_8858046684809825962[1] = 0.0; + out_8858046684809825962[2] = 0.0; + out_8858046684809825962[3] = 0.0; + out_8858046684809825962[4] = 0.0; + out_8858046684809825962[5] = 0.0; + out_8858046684809825962[6] = 0.0; + out_8858046684809825962[7] = 0.0; + out_8858046684809825962[8] = 0.0; + out_8858046684809825962[9] = 0.0; + out_8858046684809825962[10] = 1.0; + out_8858046684809825962[11] = 0.0; + out_8858046684809825962[12] = 0.0; + out_8858046684809825962[13] = 0.0; + out_8858046684809825962[14] = 0.0; + out_8858046684809825962[15] = 0.0; + out_8858046684809825962[16] = 0.0; + out_8858046684809825962[17] = 0.0; + out_8858046684809825962[18] = 0.0; + out_8858046684809825962[19] = 0.0; + out_8858046684809825962[20] = 1.0; + out_8858046684809825962[21] = 0.0; + out_8858046684809825962[22] = 0.0; + out_8858046684809825962[23] = 0.0; + out_8858046684809825962[24] = 0.0; + out_8858046684809825962[25] = 0.0; + out_8858046684809825962[26] = 0.0; + out_8858046684809825962[27] = 0.0; + out_8858046684809825962[28] = 0.0; + out_8858046684809825962[29] = 0.0; + out_8858046684809825962[30] = 1.0; + out_8858046684809825962[31] = 0.0; + out_8858046684809825962[32] = 0.0; + out_8858046684809825962[33] = 0.0; + out_8858046684809825962[34] = 0.0; + out_8858046684809825962[35] = 0.0; + out_8858046684809825962[36] = 0.0; + out_8858046684809825962[37] = 0.0; + out_8858046684809825962[38] = 0.0; + out_8858046684809825962[39] = 0.0; + out_8858046684809825962[40] = 1.0; + out_8858046684809825962[41] = 0.0; + out_8858046684809825962[42] = 0.0; + out_8858046684809825962[43] = 0.0; + out_8858046684809825962[44] = 0.0; + out_8858046684809825962[45] = 0.0; + out_8858046684809825962[46] = 0.0; + out_8858046684809825962[47] = 0.0; + out_8858046684809825962[48] = 0.0; + out_8858046684809825962[49] = 0.0; + out_8858046684809825962[50] = 1.0; + out_8858046684809825962[51] = 0.0; + out_8858046684809825962[52] = 0.0; + out_8858046684809825962[53] = 0.0; + out_8858046684809825962[54] = 0.0; + out_8858046684809825962[55] = 0.0; + out_8858046684809825962[56] = 0.0; + out_8858046684809825962[57] = 0.0; + out_8858046684809825962[58] = 0.0; + out_8858046684809825962[59] = 0.0; + out_8858046684809825962[60] = 1.0; + out_8858046684809825962[61] = 0.0; + out_8858046684809825962[62] = 0.0; + out_8858046684809825962[63] = 0.0; + out_8858046684809825962[64] = 0.0; + out_8858046684809825962[65] = 0.0; + out_8858046684809825962[66] = 0.0; + out_8858046684809825962[67] = 0.0; + out_8858046684809825962[68] = 0.0; + out_8858046684809825962[69] = 0.0; + out_8858046684809825962[70] = 1.0; + out_8858046684809825962[71] = 0.0; + out_8858046684809825962[72] = 0.0; + out_8858046684809825962[73] = 0.0; + out_8858046684809825962[74] = 0.0; + out_8858046684809825962[75] = 0.0; + out_8858046684809825962[76] = 0.0; + out_8858046684809825962[77] = 0.0; + out_8858046684809825962[78] = 0.0; + out_8858046684809825962[79] = 0.0; + out_8858046684809825962[80] = 1.0; } -void f_fun(double *state, double dt, double *out_7847765386725654972) { - out_7847765386725654972[0] = state[0]; - out_7847765386725654972[1] = state[1]; - out_7847765386725654972[2] = state[2]; - out_7847765386725654972[3] = state[3]; - out_7847765386725654972[4] = state[4]; - out_7847765386725654972[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5]; - out_7847765386725654972[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6]; - out_7847765386725654972[7] = state[7]; - out_7847765386725654972[8] = state[8]; +void f_fun(double *state, double dt, double *out_2600173016712354575) { + out_2600173016712354575[0] = state[0]; + out_2600173016712354575[1] = state[1]; + out_2600173016712354575[2] = state[2]; + out_2600173016712354575[3] = state[3]; + out_2600173016712354575[4] = state[4]; + out_2600173016712354575[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_2600173016712354575[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_2600173016712354575[7] = state[7]; + out_2600173016712354575[8] = state[8]; } -void F_fun(double *state, double dt, double *out_6490326381461081645) { - out_6490326381461081645[0] = 1; - out_6490326381461081645[1] = 0; - out_6490326381461081645[2] = 0; - out_6490326381461081645[3] = 0; - out_6490326381461081645[4] = 0; - out_6490326381461081645[5] = 0; - out_6490326381461081645[6] = 0; - out_6490326381461081645[7] = 0; - out_6490326381461081645[8] = 0; - out_6490326381461081645[9] = 0; - out_6490326381461081645[10] = 1; - out_6490326381461081645[11] = 0; - out_6490326381461081645[12] = 0; - out_6490326381461081645[13] = 0; - out_6490326381461081645[14] = 0; - out_6490326381461081645[15] = 0; - out_6490326381461081645[16] = 0; - out_6490326381461081645[17] = 0; - out_6490326381461081645[18] = 0; - out_6490326381461081645[19] = 0; - out_6490326381461081645[20] = 1; - out_6490326381461081645[21] = 0; - out_6490326381461081645[22] = 0; - out_6490326381461081645[23] = 0; - out_6490326381461081645[24] = 0; - out_6490326381461081645[25] = 0; - out_6490326381461081645[26] = 0; - out_6490326381461081645[27] = 0; - out_6490326381461081645[28] = 0; - out_6490326381461081645[29] = 0; - out_6490326381461081645[30] = 1; - out_6490326381461081645[31] = 0; - out_6490326381461081645[32] = 0; - out_6490326381461081645[33] = 0; - out_6490326381461081645[34] = 0; - out_6490326381461081645[35] = 0; - out_6490326381461081645[36] = 0; - out_6490326381461081645[37] = 0; - out_6490326381461081645[38] = 0; - out_6490326381461081645[39] = 0; - out_6490326381461081645[40] = 1; - out_6490326381461081645[41] = 0; - out_6490326381461081645[42] = 0; - out_6490326381461081645[43] = 0; - out_6490326381461081645[44] = 0; - out_6490326381461081645[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4])); - out_6490326381461081645[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_6490326381461081645[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_6490326381461081645[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_6490326381461081645[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2))); - out_6490326381461081645[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_6490326381461081645[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_6490326381461081645[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_6490326381461081645[53] = -9.8100000000000005*dt; - out_6490326381461081645[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4])); - out_6490326381461081645[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_6490326381461081645[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6490326381461081645[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6490326381461081645[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2))); - out_6490326381461081645[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_6490326381461081645[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1; - out_6490326381461081645[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6490326381461081645[62] = 0; - out_6490326381461081645[63] = 0; - out_6490326381461081645[64] = 0; - out_6490326381461081645[65] = 0; - out_6490326381461081645[66] = 0; - out_6490326381461081645[67] = 0; - out_6490326381461081645[68] = 0; - out_6490326381461081645[69] = 0; - out_6490326381461081645[70] = 1; - out_6490326381461081645[71] = 0; - out_6490326381461081645[72] = 0; - out_6490326381461081645[73] = 0; - out_6490326381461081645[74] = 0; - out_6490326381461081645[75] = 0; - out_6490326381461081645[76] = 0; - out_6490326381461081645[77] = 0; - out_6490326381461081645[78] = 0; - out_6490326381461081645[79] = 0; - out_6490326381461081645[80] = 1; +void F_fun(double *state, double dt, double *out_2418560584693982231) { + out_2418560584693982231[0] = 1; + out_2418560584693982231[1] = 0; + out_2418560584693982231[2] = 0; + out_2418560584693982231[3] = 0; + out_2418560584693982231[4] = 0; + out_2418560584693982231[5] = 0; + out_2418560584693982231[6] = 0; + out_2418560584693982231[7] = 0; + out_2418560584693982231[8] = 0; + out_2418560584693982231[9] = 0; + out_2418560584693982231[10] = 1; + out_2418560584693982231[11] = 0; + out_2418560584693982231[12] = 0; + out_2418560584693982231[13] = 0; + out_2418560584693982231[14] = 0; + out_2418560584693982231[15] = 0; + out_2418560584693982231[16] = 0; + out_2418560584693982231[17] = 0; + out_2418560584693982231[18] = 0; + out_2418560584693982231[19] = 0; + out_2418560584693982231[20] = 1; + out_2418560584693982231[21] = 0; + out_2418560584693982231[22] = 0; + out_2418560584693982231[23] = 0; + out_2418560584693982231[24] = 0; + out_2418560584693982231[25] = 0; + out_2418560584693982231[26] = 0; + out_2418560584693982231[27] = 0; + out_2418560584693982231[28] = 0; + out_2418560584693982231[29] = 0; + out_2418560584693982231[30] = 1; + out_2418560584693982231[31] = 0; + out_2418560584693982231[32] = 0; + out_2418560584693982231[33] = 0; + out_2418560584693982231[34] = 0; + out_2418560584693982231[35] = 0; + out_2418560584693982231[36] = 0; + out_2418560584693982231[37] = 0; + out_2418560584693982231[38] = 0; + out_2418560584693982231[39] = 0; + out_2418560584693982231[40] = 1; + out_2418560584693982231[41] = 0; + out_2418560584693982231[42] = 0; + out_2418560584693982231[43] = 0; + out_2418560584693982231[44] = 0; + out_2418560584693982231[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_2418560584693982231[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_2418560584693982231[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_2418560584693982231[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_2418560584693982231[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_2418560584693982231[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_2418560584693982231[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_2418560584693982231[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_2418560584693982231[53] = -9.8100000000000005*dt; + out_2418560584693982231[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_2418560584693982231[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_2418560584693982231[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2418560584693982231[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2418560584693982231[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_2418560584693982231[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_2418560584693982231[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_2418560584693982231[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2418560584693982231[62] = 0; + out_2418560584693982231[63] = 0; + out_2418560584693982231[64] = 0; + out_2418560584693982231[65] = 0; + out_2418560584693982231[66] = 0; + out_2418560584693982231[67] = 0; + out_2418560584693982231[68] = 0; + out_2418560584693982231[69] = 0; + out_2418560584693982231[70] = 1; + out_2418560584693982231[71] = 0; + out_2418560584693982231[72] = 0; + out_2418560584693982231[73] = 0; + out_2418560584693982231[74] = 0; + out_2418560584693982231[75] = 0; + out_2418560584693982231[76] = 0; + out_2418560584693982231[77] = 0; + out_2418560584693982231[78] = 0; + out_2418560584693982231[79] = 0; + out_2418560584693982231[80] = 1; } -void h_25(double *state, double *unused, double *out_258705256034994534) { - out_258705256034994534[0] = state[6]; +void h_25(double *state, double *unused, double *out_7368322694207597671) { + out_7368322694207597671[0] = state[6]; } -void H_25(double *state, double *unused, double *out_123073565102942050) { - out_123073565102942050[0] = 0; - out_123073565102942050[1] = 0; - out_123073565102942050[2] = 0; - out_123073565102942050[3] = 0; - out_123073565102942050[4] = 0; - out_123073565102942050[5] = 0; - out_123073565102942050[6] = 1; - out_123073565102942050[7] = 0; - out_123073565102942050[8] = 0; +void H_25(double *state, double *unused, double *out_2361023724919796011) { + out_2361023724919796011[0] = 0; + out_2361023724919796011[1] = 0; + out_2361023724919796011[2] = 0; + out_2361023724919796011[3] = 0; + out_2361023724919796011[4] = 0; + out_2361023724919796011[5] = 0; + out_2361023724919796011[6] = 1; + out_2361023724919796011[7] = 0; + out_2361023724919796011[8] = 0; } -void h_24(double *state, double *unused, double *out_5989007342487248666) { - out_5989007342487248666[0] = state[4]; - out_5989007342487248666[1] = state[5]; +void h_24(double *state, double *unused, double *out_3157655832268166165) { + out_3157655832268166165[0] = state[4]; + out_3157655832268166165[1] = state[5]; } -void H_24(double *state, double *unused, double *out_4869582892069451084) { - out_4869582892069451084[0] = 0; - out_4869582892069451084[1] = 0; - out_4869582892069451084[2] = 0; - out_4869582892069451084[3] = 0; - out_4869582892069451084[4] = 1; - out_4869582892069451084[5] = 0; - out_4869582892069451084[6] = 0; - out_4869582892069451084[7] = 0; - out_4869582892069451084[8] = 0; - out_4869582892069451084[9] = 0; - out_4869582892069451084[10] = 0; - out_4869582892069451084[11] = 0; - out_4869582892069451084[12] = 0; - out_4869582892069451084[13] = 0; - out_4869582892069451084[14] = 1; - out_4869582892069451084[15] = 0; - out_4869582892069451084[16] = 0; - out_4869582892069451084[17] = 0; +void H_24(double *state, double *unused, double *out_1890566242876457287) { + out_1890566242876457287[0] = 0; + out_1890566242876457287[1] = 0; + out_1890566242876457287[2] = 0; + out_1890566242876457287[3] = 0; + out_1890566242876457287[4] = 1; + out_1890566242876457287[5] = 0; + out_1890566242876457287[6] = 0; + out_1890566242876457287[7] = 0; + out_1890566242876457287[8] = 0; + out_1890566242876457287[9] = 0; + out_1890566242876457287[10] = 0; + out_1890566242876457287[11] = 0; + out_1890566242876457287[12] = 0; + out_1890566242876457287[13] = 0; + out_1890566242876457287[14] = 1; + out_1890566242876457287[15] = 0; + out_1890566242876457287[16] = 0; + out_1890566242876457287[17] = 0; } -void h_30(double *state, double *unused, double *out_179991132081170203) { - out_179991132081170203[0] = state[4]; +void h_30(double *state, double *unused, double *out_3617862615253065620) { + out_3617862615253065620[0] = state[4]; } -void H_30(double *state, double *unused, double *out_6793616776388674705) { - out_6793616776388674705[0] = 0; - out_6793616776388674705[1] = 0; - out_6793616776388674705[2] = 0; - out_6793616776388674705[3] = 0; - out_6793616776388674705[4] = 1; - out_6793616776388674705[5] = 0; - out_6793616776388674705[6] = 0; - out_6793616776388674705[7] = 0; - out_6793616776388674705[8] = 0; +void H_30(double *state, double *unused, double *out_2166672605207812187) { + out_2166672605207812187[0] = 0; + out_2166672605207812187[1] = 0; + out_2166672605207812187[2] = 0; + out_2166672605207812187[3] = 0; + out_2166672605207812187[4] = 1; + out_2166672605207812187[5] = 0; + out_2166672605207812187[6] = 0; + out_2166672605207812187[7] = 0; + out_2166672605207812187[8] = 0; } -void h_26(double *state, double *unused, double *out_4821682135579675442) { - out_4821682135579675442[0] = state[7]; +void h_26(double *state, double *unused, double *out_5806150257954055878) { + out_5806150257954055878[0] = state[7]; } -void H_26(double *state, double *unused, double *out_3181452404657858551) { - out_3181452404657858551[0] = 0; - out_3181452404657858551[1] = 0; - out_3181452404657858551[2] = 0; - out_3181452404657858551[3] = 0; - out_3181452404657858551[4] = 0; - out_3181452404657858551[5] = 0; - out_3181452404657858551[6] = 0; - out_3181452404657858551[7] = 1; - out_3181452404657858551[8] = 0; +void H_26(double *state, double *unused, double *out_1380479593954260213) { + out_1380479593954260213[0] = 0; + out_1380479593954260213[1] = 0; + out_1380479593954260213[2] = 0; + out_1380479593954260213[3] = 0; + out_1380479593954260213[4] = 0; + out_1380479593954260213[5] = 0; + out_1380479593954260213[6] = 0; + out_1380479593954260213[7] = 1; + out_1380479593954260213[8] = 0; } -void h_27(double *state, double *unused, double *out_5260378523695840179) { - out_5260378523695840179[0] = state[3]; +void h_27(double *state, double *unused, double *out_8727950225216967499) { + out_8727950225216967499[0] = state[3]; } -void H_27(double *state, double *unused, double *out_4618853464588249794) { - out_4618853464588249794[0] = 0; - out_4618853464588249794[1] = 0; - out_4618853464588249794[2] = 0; - out_4618853464588249794[3] = 1; - out_4618853464588249794[4] = 0; - out_4618853464588249794[5] = 0; - out_4618853464588249794[6] = 0; - out_4618853464588249794[7] = 0; - out_4618853464588249794[8] = 0; +void H_27(double *state, double *unused, double *out_56921465976131030) { + out_56921465976131030[0] = 0; + out_56921465976131030[1] = 0; + out_56921465976131030[2] = 0; + out_56921465976131030[3] = 1; + out_56921465976131030[4] = 0; + out_56921465976131030[5] = 0; + out_56921465976131030[6] = 0; + out_56921465976131030[7] = 0; + out_56921465976131030[8] = 0; } -void h_29(double *state, double *unused, double *out_5535572585980346068) { - out_5535572585980346068[0] = state[1]; +void h_29(double *state, double *unused, double *out_2947958622142233181) { + out_2947958622142233181[0] = state[1]; } -void H_29(double *state, double *unused, double *out_2905490737718698761) { - out_2905490737718698761[0] = 0; - out_2905490737718698761[1] = 1; - out_2905490737718698761[2] = 0; - out_2905490737718698761[3] = 0; - out_2905490737718698761[4] = 0; - out_2905490737718698761[5] = 0; - out_2905490737718698761[6] = 0; - out_2905490737718698761[7] = 0; - out_2905490737718698761[8] = 0; +void H_29(double *state, double *unused, double *out_1656441260893420003) { + out_1656441260893420003[0] = 0; + out_1656441260893420003[1] = 1; + out_1656441260893420003[2] = 0; + out_1656441260893420003[3] = 0; + out_1656441260893420003[4] = 0; + out_1656441260893420003[5] = 0; + out_1656441260893420003[6] = 0; + out_1656441260893420003[7] = 0; + out_1656441260893420003[8] = 0; } -void h_28(double *state, double *unused, double *out_4092634722619698442) { - out_4092634722619698442[0] = state[0]; +void h_28(double *state, double *unused, double *out_8124634340540489982) { + out_8124634340540489982[0] = state[0]; } -void H_28(double *state, double *unused, double *out_2176908279350831813) { - out_2176908279350831813[0] = 1; - out_2176908279350831813[1] = 0; - out_2176908279350831813[2] = 0; - out_2176908279350831813[3] = 0; - out_2176908279350831813[4] = 0; - out_2176908279350831813[5] = 0; - out_2176908279350831813[6] = 0; - out_2176908279350831813[7] = 0; - out_2176908279350831813[8] = 0; +void H_28(double *state, double *unused, double *out_6738840277962950577) { + out_6738840277962950577[0] = 1; + out_6738840277962950577[1] = 0; + out_6738840277962950577[2] = 0; + out_6738840277962950577[3] = 0; + out_6738840277962950577[4] = 0; + out_6738840277962950577[5] = 0; + out_6738840277962950577[6] = 0; + out_6738840277962950577[7] = 0; + out_6738840277962950577[8] = 0; } -void h_31(double *state, double *unused, double *out_1709336648663877701) { - out_1709336648663877701[0] = state[8]; +void h_31(double *state, double *unused, double *out_5966358236057140703) { + out_5966358236057140703[0] = state[8]; } -void H_31(double *state, double *unused, double *out_6953601685408875203) { - out_6953601685408875203[0] = 0; - out_6953601685408875203[1] = 0; - out_6953601685408875203[2] = 0; - out_6953601685408875203[3] = 0; - out_6953601685408875203[4] = 0; - out_6953601685408875203[5] = 0; - out_6953601685408875203[6] = 0; - out_6953601685408875203[7] = 0; - out_6953601685408875203[8] = 1; +void H_31(double *state, double *unused, double *out_2391669686796756439) { + out_2391669686796756439[0] = 0; + out_2391669686796756439[1] = 0; + out_2391669686796756439[2] = 0; + out_2391669686796756439[3] = 0; + out_2391669686796756439[4] = 0; + out_2391669686796756439[5] = 0; + out_2391669686796756439[6] = 0; + out_2391669686796756439[7] = 0; + out_2391669686796756439[8] = 1; } #include #include @@ -518,68 +518,68 @@ void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, doubl void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { update<1, 3, 0>(in_x, in_P, h_31, H_31, NULL, in_z, in_R, in_ea, MAHA_THRESH_31); } -void car_err_fun(double *nom_x, double *delta_x, double *out_6314153535178174931) { - err_fun(nom_x, delta_x, out_6314153535178174931); +void car_err_fun(double *nom_x, double *delta_x, double *out_4276335218073679407) { + err_fun(nom_x, delta_x, out_4276335218073679407); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_7049921009392106379) { - inv_err_fun(nom_x, true_x, out_7049921009392106379); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_3738278089596769997) { + inv_err_fun(nom_x, true_x, out_3738278089596769997); } -void car_H_mod_fun(double *state, double *out_9070114660809295187) { - H_mod_fun(state, out_9070114660809295187); +void car_H_mod_fun(double *state, double *out_8858046684809825962) { + H_mod_fun(state, out_8858046684809825962); } -void car_f_fun(double *state, double dt, double *out_7847765386725654972) { - f_fun(state, dt, out_7847765386725654972); +void car_f_fun(double *state, double dt, double *out_2600173016712354575) { + f_fun(state, dt, out_2600173016712354575); } -void car_F_fun(double *state, double dt, double *out_6490326381461081645) { - F_fun(state, dt, out_6490326381461081645); +void car_F_fun(double *state, double dt, double *out_2418560584693982231) { + F_fun(state, dt, out_2418560584693982231); } -void car_h_25(double *state, double *unused, double *out_258705256034994534) { - h_25(state, unused, out_258705256034994534); +void car_h_25(double *state, double *unused, double *out_7368322694207597671) { + h_25(state, unused, out_7368322694207597671); } -void car_H_25(double *state, double *unused, double *out_123073565102942050) { - H_25(state, unused, out_123073565102942050); +void car_H_25(double *state, double *unused, double *out_2361023724919796011) { + H_25(state, unused, out_2361023724919796011); } -void car_h_24(double *state, double *unused, double *out_5989007342487248666) { - h_24(state, unused, out_5989007342487248666); +void car_h_24(double *state, double *unused, double *out_3157655832268166165) { + h_24(state, unused, out_3157655832268166165); } -void car_H_24(double *state, double *unused, double *out_4869582892069451084) { - H_24(state, unused, out_4869582892069451084); +void car_H_24(double *state, double *unused, double *out_1890566242876457287) { + H_24(state, unused, out_1890566242876457287); } -void car_h_30(double *state, double *unused, double *out_179991132081170203) { - h_30(state, unused, out_179991132081170203); +void car_h_30(double *state, double *unused, double *out_3617862615253065620) { + h_30(state, unused, out_3617862615253065620); } -void car_H_30(double *state, double *unused, double *out_6793616776388674705) { - H_30(state, unused, out_6793616776388674705); +void car_H_30(double *state, double *unused, double *out_2166672605207812187) { + H_30(state, unused, out_2166672605207812187); } -void car_h_26(double *state, double *unused, double *out_4821682135579675442) { - h_26(state, unused, out_4821682135579675442); +void car_h_26(double *state, double *unused, double *out_5806150257954055878) { + h_26(state, unused, out_5806150257954055878); } -void car_H_26(double *state, double *unused, double *out_3181452404657858551) { - H_26(state, unused, out_3181452404657858551); +void car_H_26(double *state, double *unused, double *out_1380479593954260213) { + H_26(state, unused, out_1380479593954260213); } -void car_h_27(double *state, double *unused, double *out_5260378523695840179) { - h_27(state, unused, out_5260378523695840179); +void car_h_27(double *state, double *unused, double *out_8727950225216967499) { + h_27(state, unused, out_8727950225216967499); } -void car_H_27(double *state, double *unused, double *out_4618853464588249794) { - H_27(state, unused, out_4618853464588249794); +void car_H_27(double *state, double *unused, double *out_56921465976131030) { + H_27(state, unused, out_56921465976131030); } -void car_h_29(double *state, double *unused, double *out_5535572585980346068) { - h_29(state, unused, out_5535572585980346068); +void car_h_29(double *state, double *unused, double *out_2947958622142233181) { + h_29(state, unused, out_2947958622142233181); } -void car_H_29(double *state, double *unused, double *out_2905490737718698761) { - H_29(state, unused, out_2905490737718698761); +void car_H_29(double *state, double *unused, double *out_1656441260893420003) { + H_29(state, unused, out_1656441260893420003); } -void car_h_28(double *state, double *unused, double *out_4092634722619698442) { - h_28(state, unused, out_4092634722619698442); +void car_h_28(double *state, double *unused, double *out_8124634340540489982) { + h_28(state, unused, out_8124634340540489982); } -void car_H_28(double *state, double *unused, double *out_2176908279350831813) { - H_28(state, unused, out_2176908279350831813); +void car_H_28(double *state, double *unused, double *out_6738840277962950577) { + H_28(state, unused, out_6738840277962950577); } -void car_h_31(double *state, double *unused, double *out_1709336648663877701) { - h_31(state, unused, out_1709336648663877701); +void car_h_31(double *state, double *unused, double *out_5966358236057140703) { + h_31(state, unused, out_5966358236057140703); } -void car_H_31(double *state, double *unused, double *out_6953601685408875203) { - H_31(state, unused, out_6953601685408875203); +void car_H_31(double *state, double *unused, double *out_2391669686796756439) { + H_31(state, unused, out_2391669686796756439); } void car_predict(double *in_x, double *in_P, double *in_Q, double dt) { predict(in_x, in_P, in_Q, dt); diff --git a/selfdrive/locationd/models/generated/car.h b/selfdrive/locationd/models/generated/car.h index 96371e6dc6..95223cc68d 100644 --- a/selfdrive/locationd/models/generated/car.h +++ b/selfdrive/locationd/models/generated/car.h @@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); -void car_err_fun(double *nom_x, double *delta_x, double *out_6314153535178174931); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_7049921009392106379); -void car_H_mod_fun(double *state, double *out_9070114660809295187); -void car_f_fun(double *state, double dt, double *out_7847765386725654972); -void car_F_fun(double *state, double dt, double *out_6490326381461081645); -void car_h_25(double *state, double *unused, double *out_258705256034994534); -void car_H_25(double *state, double *unused, double *out_123073565102942050); -void car_h_24(double *state, double *unused, double *out_5989007342487248666); -void car_H_24(double *state, double *unused, double *out_4869582892069451084); -void car_h_30(double *state, double *unused, double *out_179991132081170203); -void car_H_30(double *state, double *unused, double *out_6793616776388674705); -void car_h_26(double *state, double *unused, double *out_4821682135579675442); -void car_H_26(double *state, double *unused, double *out_3181452404657858551); -void car_h_27(double *state, double *unused, double *out_5260378523695840179); -void car_H_27(double *state, double *unused, double *out_4618853464588249794); -void car_h_29(double *state, double *unused, double *out_5535572585980346068); -void car_H_29(double *state, double *unused, double *out_2905490737718698761); -void car_h_28(double *state, double *unused, double *out_4092634722619698442); -void car_H_28(double *state, double *unused, double *out_2176908279350831813); -void car_h_31(double *state, double *unused, double *out_1709336648663877701); -void car_H_31(double *state, double *unused, double *out_6953601685408875203); +void car_err_fun(double *nom_x, double *delta_x, double *out_4276335218073679407); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_3738278089596769997); +void car_H_mod_fun(double *state, double *out_8858046684809825962); +void car_f_fun(double *state, double dt, double *out_2600173016712354575); +void car_F_fun(double *state, double dt, double *out_2418560584693982231); +void car_h_25(double *state, double *unused, double *out_7368322694207597671); +void car_H_25(double *state, double *unused, double *out_2361023724919796011); +void car_h_24(double *state, double *unused, double *out_3157655832268166165); +void car_H_24(double *state, double *unused, double *out_1890566242876457287); +void car_h_30(double *state, double *unused, double *out_3617862615253065620); +void car_H_30(double *state, double *unused, double *out_2166672605207812187); +void car_h_26(double *state, double *unused, double *out_5806150257954055878); +void car_H_26(double *state, double *unused, double *out_1380479593954260213); +void car_h_27(double *state, double *unused, double *out_8727950225216967499); +void car_H_27(double *state, double *unused, double *out_56921465976131030); +void car_h_29(double *state, double *unused, double *out_2947958622142233181); +void car_H_29(double *state, double *unused, double *out_1656441260893420003); +void car_h_28(double *state, double *unused, double *out_8124634340540489982); +void car_H_28(double *state, double *unused, double *out_6738840277962950577); +void car_h_31(double *state, double *unused, double *out_5966358236057140703); +void car_H_31(double *state, double *unused, double *out_2391669686796756439); void car_predict(double *in_x, double *in_P, double *in_Q, double dt); void car_set_mass(double x); void car_set_rotational_inertia(double x); diff --git a/selfdrive/locationd/models/generated/car.os b/selfdrive/locationd/models/generated/car.os index 763f6afba2..6d7a588d97 100644 Binary files a/selfdrive/locationd/models/generated/car.os and b/selfdrive/locationd/models/generated/car.os differ diff --git a/selfdrive/locationd/models/generated/libcar.so b/selfdrive/locationd/models/generated/libcar.so index e66b841c4e..ec2c55555b 100755 Binary files a/selfdrive/locationd/models/generated/libcar.so and b/selfdrive/locationd/models/generated/libcar.so differ diff --git a/selfdrive/locationd/models/generated/libpose.so b/selfdrive/locationd/models/generated/libpose.so index 1661647d9a..d89955950b 100755 Binary files a/selfdrive/locationd/models/generated/libpose.so and b/selfdrive/locationd/models/generated/libpose.so differ diff --git a/selfdrive/locationd/models/generated/pose.cpp b/selfdrive/locationd/models/generated/pose.cpp index 8c823827e7..47fce596d2 100644 --- a/selfdrive/locationd/models/generated/pose.cpp +++ b/selfdrive/locationd/models/generated/pose.cpp @@ -17,961 +17,961 @@ const static double MAHA_THRESH_14 = 7.814727903251177; * * * This file is part of 'ekf' * ******************************************************************************/ -void err_fun(double *nom_x, double *delta_x, double *out_1227579335886564773) { - out_1227579335886564773[0] = delta_x[0] + nom_x[0]; - out_1227579335886564773[1] = delta_x[1] + nom_x[1]; - out_1227579335886564773[2] = delta_x[2] + nom_x[2]; - out_1227579335886564773[3] = delta_x[3] + nom_x[3]; - out_1227579335886564773[4] = delta_x[4] + nom_x[4]; - out_1227579335886564773[5] = delta_x[5] + nom_x[5]; - out_1227579335886564773[6] = delta_x[6] + nom_x[6]; - out_1227579335886564773[7] = delta_x[7] + nom_x[7]; - out_1227579335886564773[8] = delta_x[8] + nom_x[8]; - out_1227579335886564773[9] = delta_x[9] + nom_x[9]; - out_1227579335886564773[10] = delta_x[10] + nom_x[10]; - out_1227579335886564773[11] = delta_x[11] + nom_x[11]; - out_1227579335886564773[12] = delta_x[12] + nom_x[12]; - out_1227579335886564773[13] = delta_x[13] + nom_x[13]; - out_1227579335886564773[14] = delta_x[14] + nom_x[14]; - out_1227579335886564773[15] = delta_x[15] + nom_x[15]; - out_1227579335886564773[16] = delta_x[16] + nom_x[16]; - out_1227579335886564773[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_4115870174312830998) { + out_4115870174312830998[0] = delta_x[0] + nom_x[0]; + out_4115870174312830998[1] = delta_x[1] + nom_x[1]; + out_4115870174312830998[2] = delta_x[2] + nom_x[2]; + out_4115870174312830998[3] = delta_x[3] + nom_x[3]; + out_4115870174312830998[4] = delta_x[4] + nom_x[4]; + out_4115870174312830998[5] = delta_x[5] + nom_x[5]; + out_4115870174312830998[6] = delta_x[6] + nom_x[6]; + out_4115870174312830998[7] = delta_x[7] + nom_x[7]; + out_4115870174312830998[8] = delta_x[8] + nom_x[8]; + out_4115870174312830998[9] = delta_x[9] + nom_x[9]; + out_4115870174312830998[10] = delta_x[10] + nom_x[10]; + out_4115870174312830998[11] = delta_x[11] + nom_x[11]; + out_4115870174312830998[12] = delta_x[12] + nom_x[12]; + out_4115870174312830998[13] = delta_x[13] + nom_x[13]; + out_4115870174312830998[14] = delta_x[14] + nom_x[14]; + out_4115870174312830998[15] = delta_x[15] + nom_x[15]; + out_4115870174312830998[16] = delta_x[16] + nom_x[16]; + out_4115870174312830998[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_7708296302708828001) { - out_7708296302708828001[0] = -nom_x[0] + true_x[0]; - out_7708296302708828001[1] = -nom_x[1] + true_x[1]; - out_7708296302708828001[2] = -nom_x[2] + true_x[2]; - out_7708296302708828001[3] = -nom_x[3] + true_x[3]; - out_7708296302708828001[4] = -nom_x[4] + true_x[4]; - out_7708296302708828001[5] = -nom_x[5] + true_x[5]; - out_7708296302708828001[6] = -nom_x[6] + true_x[6]; - out_7708296302708828001[7] = -nom_x[7] + true_x[7]; - out_7708296302708828001[8] = -nom_x[8] + true_x[8]; - out_7708296302708828001[9] = -nom_x[9] + true_x[9]; - out_7708296302708828001[10] = -nom_x[10] + true_x[10]; - out_7708296302708828001[11] = -nom_x[11] + true_x[11]; - out_7708296302708828001[12] = -nom_x[12] + true_x[12]; - out_7708296302708828001[13] = -nom_x[13] + true_x[13]; - out_7708296302708828001[14] = -nom_x[14] + true_x[14]; - out_7708296302708828001[15] = -nom_x[15] + true_x[15]; - out_7708296302708828001[16] = -nom_x[16] + true_x[16]; - out_7708296302708828001[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_2299838754417090386) { + out_2299838754417090386[0] = -nom_x[0] + true_x[0]; + out_2299838754417090386[1] = -nom_x[1] + true_x[1]; + out_2299838754417090386[2] = -nom_x[2] + true_x[2]; + out_2299838754417090386[3] = -nom_x[3] + true_x[3]; + out_2299838754417090386[4] = -nom_x[4] + true_x[4]; + out_2299838754417090386[5] = -nom_x[5] + true_x[5]; + out_2299838754417090386[6] = -nom_x[6] + true_x[6]; + out_2299838754417090386[7] = -nom_x[7] + true_x[7]; + out_2299838754417090386[8] = -nom_x[8] + true_x[8]; + out_2299838754417090386[9] = -nom_x[9] + true_x[9]; + out_2299838754417090386[10] = -nom_x[10] + true_x[10]; + out_2299838754417090386[11] = -nom_x[11] + true_x[11]; + out_2299838754417090386[12] = -nom_x[12] + true_x[12]; + out_2299838754417090386[13] = -nom_x[13] + true_x[13]; + out_2299838754417090386[14] = -nom_x[14] + true_x[14]; + out_2299838754417090386[15] = -nom_x[15] + true_x[15]; + out_2299838754417090386[16] = -nom_x[16] + true_x[16]; + out_2299838754417090386[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_145151651698479708) { - out_145151651698479708[0] = 1.0; - out_145151651698479708[1] = 0.0; - out_145151651698479708[2] = 0.0; - out_145151651698479708[3] = 0.0; - out_145151651698479708[4] = 0.0; - out_145151651698479708[5] = 0.0; - out_145151651698479708[6] = 0.0; - out_145151651698479708[7] = 0.0; - out_145151651698479708[8] = 0.0; - out_145151651698479708[9] = 0.0; - out_145151651698479708[10] = 0.0; - out_145151651698479708[11] = 0.0; - out_145151651698479708[12] = 0.0; - out_145151651698479708[13] = 0.0; - out_145151651698479708[14] = 0.0; - out_145151651698479708[15] = 0.0; - out_145151651698479708[16] = 0.0; - out_145151651698479708[17] = 0.0; - out_145151651698479708[18] = 0.0; - out_145151651698479708[19] = 1.0; - out_145151651698479708[20] = 0.0; - out_145151651698479708[21] = 0.0; - out_145151651698479708[22] = 0.0; - out_145151651698479708[23] = 0.0; - out_145151651698479708[24] = 0.0; - out_145151651698479708[25] = 0.0; - out_145151651698479708[26] = 0.0; - out_145151651698479708[27] = 0.0; - out_145151651698479708[28] = 0.0; - out_145151651698479708[29] = 0.0; - out_145151651698479708[30] = 0.0; - out_145151651698479708[31] = 0.0; - out_145151651698479708[32] = 0.0; - out_145151651698479708[33] = 0.0; - out_145151651698479708[34] = 0.0; - out_145151651698479708[35] = 0.0; - out_145151651698479708[36] = 0.0; - out_145151651698479708[37] = 0.0; - out_145151651698479708[38] = 1.0; - out_145151651698479708[39] = 0.0; - out_145151651698479708[40] = 0.0; - out_145151651698479708[41] = 0.0; - out_145151651698479708[42] = 0.0; - out_145151651698479708[43] = 0.0; - out_145151651698479708[44] = 0.0; - out_145151651698479708[45] = 0.0; - out_145151651698479708[46] = 0.0; - out_145151651698479708[47] = 0.0; - out_145151651698479708[48] = 0.0; - out_145151651698479708[49] = 0.0; - out_145151651698479708[50] = 0.0; - out_145151651698479708[51] = 0.0; - out_145151651698479708[52] = 0.0; - out_145151651698479708[53] = 0.0; - out_145151651698479708[54] = 0.0; - out_145151651698479708[55] = 0.0; - out_145151651698479708[56] = 0.0; - out_145151651698479708[57] = 1.0; - out_145151651698479708[58] = 0.0; - out_145151651698479708[59] = 0.0; - out_145151651698479708[60] = 0.0; - out_145151651698479708[61] = 0.0; - out_145151651698479708[62] = 0.0; - out_145151651698479708[63] = 0.0; - out_145151651698479708[64] = 0.0; - out_145151651698479708[65] = 0.0; - out_145151651698479708[66] = 0.0; - out_145151651698479708[67] = 0.0; - out_145151651698479708[68] = 0.0; - out_145151651698479708[69] = 0.0; - out_145151651698479708[70] = 0.0; - out_145151651698479708[71] = 0.0; - out_145151651698479708[72] = 0.0; - out_145151651698479708[73] = 0.0; - out_145151651698479708[74] = 0.0; - out_145151651698479708[75] = 0.0; - out_145151651698479708[76] = 1.0; - out_145151651698479708[77] = 0.0; - out_145151651698479708[78] = 0.0; - out_145151651698479708[79] = 0.0; - out_145151651698479708[80] = 0.0; - out_145151651698479708[81] = 0.0; - out_145151651698479708[82] = 0.0; - out_145151651698479708[83] = 0.0; - out_145151651698479708[84] = 0.0; - out_145151651698479708[85] = 0.0; - out_145151651698479708[86] = 0.0; - out_145151651698479708[87] = 0.0; - out_145151651698479708[88] = 0.0; - out_145151651698479708[89] = 0.0; - out_145151651698479708[90] = 0.0; - out_145151651698479708[91] = 0.0; - out_145151651698479708[92] = 0.0; - out_145151651698479708[93] = 0.0; - out_145151651698479708[94] = 0.0; - out_145151651698479708[95] = 1.0; - out_145151651698479708[96] = 0.0; - out_145151651698479708[97] = 0.0; - out_145151651698479708[98] = 0.0; - out_145151651698479708[99] = 0.0; - out_145151651698479708[100] = 0.0; - out_145151651698479708[101] = 0.0; - out_145151651698479708[102] = 0.0; - out_145151651698479708[103] = 0.0; - out_145151651698479708[104] = 0.0; - out_145151651698479708[105] = 0.0; - out_145151651698479708[106] = 0.0; - out_145151651698479708[107] = 0.0; - out_145151651698479708[108] = 0.0; - out_145151651698479708[109] = 0.0; - out_145151651698479708[110] = 0.0; - out_145151651698479708[111] = 0.0; - out_145151651698479708[112] = 0.0; - out_145151651698479708[113] = 0.0; - out_145151651698479708[114] = 1.0; - out_145151651698479708[115] = 0.0; - out_145151651698479708[116] = 0.0; - out_145151651698479708[117] = 0.0; - out_145151651698479708[118] = 0.0; - out_145151651698479708[119] = 0.0; - out_145151651698479708[120] = 0.0; - out_145151651698479708[121] = 0.0; - out_145151651698479708[122] = 0.0; - out_145151651698479708[123] = 0.0; - out_145151651698479708[124] = 0.0; - out_145151651698479708[125] = 0.0; - out_145151651698479708[126] = 0.0; - out_145151651698479708[127] = 0.0; - out_145151651698479708[128] = 0.0; - out_145151651698479708[129] = 0.0; - out_145151651698479708[130] = 0.0; - out_145151651698479708[131] = 0.0; - out_145151651698479708[132] = 0.0; - out_145151651698479708[133] = 1.0; - out_145151651698479708[134] = 0.0; - out_145151651698479708[135] = 0.0; - out_145151651698479708[136] = 0.0; - out_145151651698479708[137] = 0.0; - out_145151651698479708[138] = 0.0; - out_145151651698479708[139] = 0.0; - out_145151651698479708[140] = 0.0; - out_145151651698479708[141] = 0.0; - out_145151651698479708[142] = 0.0; - out_145151651698479708[143] = 0.0; - out_145151651698479708[144] = 0.0; - out_145151651698479708[145] = 0.0; - out_145151651698479708[146] = 0.0; - out_145151651698479708[147] = 0.0; - out_145151651698479708[148] = 0.0; - out_145151651698479708[149] = 0.0; - out_145151651698479708[150] = 0.0; - out_145151651698479708[151] = 0.0; - out_145151651698479708[152] = 1.0; - out_145151651698479708[153] = 0.0; - out_145151651698479708[154] = 0.0; - out_145151651698479708[155] = 0.0; - out_145151651698479708[156] = 0.0; - out_145151651698479708[157] = 0.0; - out_145151651698479708[158] = 0.0; - out_145151651698479708[159] = 0.0; - out_145151651698479708[160] = 0.0; - out_145151651698479708[161] = 0.0; - out_145151651698479708[162] = 0.0; - out_145151651698479708[163] = 0.0; - out_145151651698479708[164] = 0.0; - out_145151651698479708[165] = 0.0; - out_145151651698479708[166] = 0.0; - out_145151651698479708[167] = 0.0; - out_145151651698479708[168] = 0.0; - out_145151651698479708[169] = 0.0; - out_145151651698479708[170] = 0.0; - out_145151651698479708[171] = 1.0; - out_145151651698479708[172] = 0.0; - out_145151651698479708[173] = 0.0; - out_145151651698479708[174] = 0.0; - out_145151651698479708[175] = 0.0; - out_145151651698479708[176] = 0.0; - out_145151651698479708[177] = 0.0; - out_145151651698479708[178] = 0.0; - out_145151651698479708[179] = 0.0; - out_145151651698479708[180] = 0.0; - out_145151651698479708[181] = 0.0; - out_145151651698479708[182] = 0.0; - out_145151651698479708[183] = 0.0; - out_145151651698479708[184] = 0.0; - out_145151651698479708[185] = 0.0; - out_145151651698479708[186] = 0.0; - out_145151651698479708[187] = 0.0; - out_145151651698479708[188] = 0.0; - out_145151651698479708[189] = 0.0; - out_145151651698479708[190] = 1.0; - out_145151651698479708[191] = 0.0; - out_145151651698479708[192] = 0.0; - out_145151651698479708[193] = 0.0; - out_145151651698479708[194] = 0.0; - out_145151651698479708[195] = 0.0; - out_145151651698479708[196] = 0.0; - out_145151651698479708[197] = 0.0; - out_145151651698479708[198] = 0.0; - out_145151651698479708[199] = 0.0; - out_145151651698479708[200] = 0.0; - out_145151651698479708[201] = 0.0; - out_145151651698479708[202] = 0.0; - out_145151651698479708[203] = 0.0; - out_145151651698479708[204] = 0.0; - out_145151651698479708[205] = 0.0; - out_145151651698479708[206] = 0.0; - out_145151651698479708[207] = 0.0; - out_145151651698479708[208] = 0.0; - out_145151651698479708[209] = 1.0; - out_145151651698479708[210] = 0.0; - out_145151651698479708[211] = 0.0; - out_145151651698479708[212] = 0.0; - out_145151651698479708[213] = 0.0; - out_145151651698479708[214] = 0.0; - out_145151651698479708[215] = 0.0; - out_145151651698479708[216] = 0.0; - out_145151651698479708[217] = 0.0; - out_145151651698479708[218] = 0.0; - out_145151651698479708[219] = 0.0; - out_145151651698479708[220] = 0.0; - out_145151651698479708[221] = 0.0; - out_145151651698479708[222] = 0.0; - out_145151651698479708[223] = 0.0; - out_145151651698479708[224] = 0.0; - out_145151651698479708[225] = 0.0; - out_145151651698479708[226] = 0.0; - out_145151651698479708[227] = 0.0; - out_145151651698479708[228] = 1.0; - out_145151651698479708[229] = 0.0; - out_145151651698479708[230] = 0.0; - out_145151651698479708[231] = 0.0; - out_145151651698479708[232] = 0.0; - out_145151651698479708[233] = 0.0; - out_145151651698479708[234] = 0.0; - out_145151651698479708[235] = 0.0; - out_145151651698479708[236] = 0.0; - out_145151651698479708[237] = 0.0; - out_145151651698479708[238] = 0.0; - out_145151651698479708[239] = 0.0; - out_145151651698479708[240] = 0.0; - out_145151651698479708[241] = 0.0; - out_145151651698479708[242] = 0.0; - out_145151651698479708[243] = 0.0; - out_145151651698479708[244] = 0.0; - out_145151651698479708[245] = 0.0; - out_145151651698479708[246] = 0.0; - out_145151651698479708[247] = 1.0; - out_145151651698479708[248] = 0.0; - out_145151651698479708[249] = 0.0; - out_145151651698479708[250] = 0.0; - out_145151651698479708[251] = 0.0; - out_145151651698479708[252] = 0.0; - out_145151651698479708[253] = 0.0; - out_145151651698479708[254] = 0.0; - out_145151651698479708[255] = 0.0; - out_145151651698479708[256] = 0.0; - out_145151651698479708[257] = 0.0; - out_145151651698479708[258] = 0.0; - out_145151651698479708[259] = 0.0; - out_145151651698479708[260] = 0.0; - out_145151651698479708[261] = 0.0; - out_145151651698479708[262] = 0.0; - out_145151651698479708[263] = 0.0; - out_145151651698479708[264] = 0.0; - out_145151651698479708[265] = 0.0; - out_145151651698479708[266] = 1.0; - out_145151651698479708[267] = 0.0; - out_145151651698479708[268] = 0.0; - out_145151651698479708[269] = 0.0; - out_145151651698479708[270] = 0.0; - out_145151651698479708[271] = 0.0; - out_145151651698479708[272] = 0.0; - out_145151651698479708[273] = 0.0; - out_145151651698479708[274] = 0.0; - out_145151651698479708[275] = 0.0; - out_145151651698479708[276] = 0.0; - out_145151651698479708[277] = 0.0; - out_145151651698479708[278] = 0.0; - out_145151651698479708[279] = 0.0; - out_145151651698479708[280] = 0.0; - out_145151651698479708[281] = 0.0; - out_145151651698479708[282] = 0.0; - out_145151651698479708[283] = 0.0; - out_145151651698479708[284] = 0.0; - out_145151651698479708[285] = 1.0; - out_145151651698479708[286] = 0.0; - out_145151651698479708[287] = 0.0; - out_145151651698479708[288] = 0.0; - out_145151651698479708[289] = 0.0; - out_145151651698479708[290] = 0.0; - out_145151651698479708[291] = 0.0; - out_145151651698479708[292] = 0.0; - out_145151651698479708[293] = 0.0; - out_145151651698479708[294] = 0.0; - out_145151651698479708[295] = 0.0; - out_145151651698479708[296] = 0.0; - out_145151651698479708[297] = 0.0; - out_145151651698479708[298] = 0.0; - out_145151651698479708[299] = 0.0; - out_145151651698479708[300] = 0.0; - out_145151651698479708[301] = 0.0; - out_145151651698479708[302] = 0.0; - out_145151651698479708[303] = 0.0; - out_145151651698479708[304] = 1.0; - out_145151651698479708[305] = 0.0; - out_145151651698479708[306] = 0.0; - out_145151651698479708[307] = 0.0; - out_145151651698479708[308] = 0.0; - out_145151651698479708[309] = 0.0; - out_145151651698479708[310] = 0.0; - out_145151651698479708[311] = 0.0; - out_145151651698479708[312] = 0.0; - out_145151651698479708[313] = 0.0; - out_145151651698479708[314] = 0.0; - out_145151651698479708[315] = 0.0; - out_145151651698479708[316] = 0.0; - out_145151651698479708[317] = 0.0; - out_145151651698479708[318] = 0.0; - out_145151651698479708[319] = 0.0; - out_145151651698479708[320] = 0.0; - out_145151651698479708[321] = 0.0; - out_145151651698479708[322] = 0.0; - out_145151651698479708[323] = 1.0; +void H_mod_fun(double *state, double *out_652375008318869169) { + out_652375008318869169[0] = 1.0; + out_652375008318869169[1] = 0.0; + out_652375008318869169[2] = 0.0; + out_652375008318869169[3] = 0.0; + out_652375008318869169[4] = 0.0; + out_652375008318869169[5] = 0.0; + out_652375008318869169[6] = 0.0; + out_652375008318869169[7] = 0.0; + out_652375008318869169[8] = 0.0; + out_652375008318869169[9] = 0.0; + out_652375008318869169[10] = 0.0; + out_652375008318869169[11] = 0.0; + out_652375008318869169[12] = 0.0; + out_652375008318869169[13] = 0.0; + out_652375008318869169[14] = 0.0; + out_652375008318869169[15] = 0.0; + out_652375008318869169[16] = 0.0; + out_652375008318869169[17] = 0.0; + out_652375008318869169[18] = 0.0; + out_652375008318869169[19] = 1.0; + out_652375008318869169[20] = 0.0; + out_652375008318869169[21] = 0.0; + out_652375008318869169[22] = 0.0; + out_652375008318869169[23] = 0.0; + out_652375008318869169[24] = 0.0; + out_652375008318869169[25] = 0.0; + out_652375008318869169[26] = 0.0; + out_652375008318869169[27] = 0.0; + out_652375008318869169[28] = 0.0; + out_652375008318869169[29] = 0.0; + out_652375008318869169[30] = 0.0; + out_652375008318869169[31] = 0.0; + out_652375008318869169[32] = 0.0; + out_652375008318869169[33] = 0.0; + out_652375008318869169[34] = 0.0; + out_652375008318869169[35] = 0.0; + out_652375008318869169[36] = 0.0; + out_652375008318869169[37] = 0.0; + out_652375008318869169[38] = 1.0; + out_652375008318869169[39] = 0.0; + out_652375008318869169[40] = 0.0; + out_652375008318869169[41] = 0.0; + out_652375008318869169[42] = 0.0; + out_652375008318869169[43] = 0.0; + out_652375008318869169[44] = 0.0; + out_652375008318869169[45] = 0.0; + out_652375008318869169[46] = 0.0; + out_652375008318869169[47] = 0.0; + out_652375008318869169[48] = 0.0; + out_652375008318869169[49] = 0.0; + out_652375008318869169[50] = 0.0; + out_652375008318869169[51] = 0.0; + out_652375008318869169[52] = 0.0; + out_652375008318869169[53] = 0.0; + out_652375008318869169[54] = 0.0; + out_652375008318869169[55] = 0.0; + out_652375008318869169[56] = 0.0; + out_652375008318869169[57] = 1.0; + out_652375008318869169[58] = 0.0; + out_652375008318869169[59] = 0.0; + out_652375008318869169[60] = 0.0; + out_652375008318869169[61] = 0.0; + out_652375008318869169[62] = 0.0; + out_652375008318869169[63] = 0.0; + out_652375008318869169[64] = 0.0; + out_652375008318869169[65] = 0.0; + out_652375008318869169[66] = 0.0; + out_652375008318869169[67] = 0.0; + out_652375008318869169[68] = 0.0; + out_652375008318869169[69] = 0.0; + out_652375008318869169[70] = 0.0; + out_652375008318869169[71] = 0.0; + out_652375008318869169[72] = 0.0; + out_652375008318869169[73] = 0.0; + out_652375008318869169[74] = 0.0; + out_652375008318869169[75] = 0.0; + out_652375008318869169[76] = 1.0; + out_652375008318869169[77] = 0.0; + out_652375008318869169[78] = 0.0; + out_652375008318869169[79] = 0.0; + out_652375008318869169[80] = 0.0; + out_652375008318869169[81] = 0.0; + out_652375008318869169[82] = 0.0; + out_652375008318869169[83] = 0.0; + out_652375008318869169[84] = 0.0; + out_652375008318869169[85] = 0.0; + out_652375008318869169[86] = 0.0; + out_652375008318869169[87] = 0.0; + out_652375008318869169[88] = 0.0; + out_652375008318869169[89] = 0.0; + out_652375008318869169[90] = 0.0; + out_652375008318869169[91] = 0.0; + out_652375008318869169[92] = 0.0; + out_652375008318869169[93] = 0.0; + out_652375008318869169[94] = 0.0; + out_652375008318869169[95] = 1.0; + out_652375008318869169[96] = 0.0; + out_652375008318869169[97] = 0.0; + out_652375008318869169[98] = 0.0; + out_652375008318869169[99] = 0.0; + out_652375008318869169[100] = 0.0; + out_652375008318869169[101] = 0.0; + out_652375008318869169[102] = 0.0; + out_652375008318869169[103] = 0.0; + out_652375008318869169[104] = 0.0; + out_652375008318869169[105] = 0.0; + out_652375008318869169[106] = 0.0; + out_652375008318869169[107] = 0.0; + out_652375008318869169[108] = 0.0; + out_652375008318869169[109] = 0.0; + out_652375008318869169[110] = 0.0; + out_652375008318869169[111] = 0.0; + out_652375008318869169[112] = 0.0; + out_652375008318869169[113] = 0.0; + out_652375008318869169[114] = 1.0; + out_652375008318869169[115] = 0.0; + out_652375008318869169[116] = 0.0; + out_652375008318869169[117] = 0.0; + out_652375008318869169[118] = 0.0; + out_652375008318869169[119] = 0.0; + out_652375008318869169[120] = 0.0; + out_652375008318869169[121] = 0.0; + out_652375008318869169[122] = 0.0; + out_652375008318869169[123] = 0.0; + out_652375008318869169[124] = 0.0; + out_652375008318869169[125] = 0.0; + out_652375008318869169[126] = 0.0; + out_652375008318869169[127] = 0.0; + out_652375008318869169[128] = 0.0; + out_652375008318869169[129] = 0.0; + out_652375008318869169[130] = 0.0; + out_652375008318869169[131] = 0.0; + out_652375008318869169[132] = 0.0; + out_652375008318869169[133] = 1.0; + out_652375008318869169[134] = 0.0; + out_652375008318869169[135] = 0.0; + out_652375008318869169[136] = 0.0; + out_652375008318869169[137] = 0.0; + out_652375008318869169[138] = 0.0; + out_652375008318869169[139] = 0.0; + out_652375008318869169[140] = 0.0; + out_652375008318869169[141] = 0.0; + out_652375008318869169[142] = 0.0; + out_652375008318869169[143] = 0.0; + out_652375008318869169[144] = 0.0; + out_652375008318869169[145] = 0.0; + out_652375008318869169[146] = 0.0; + out_652375008318869169[147] = 0.0; + out_652375008318869169[148] = 0.0; + out_652375008318869169[149] = 0.0; + out_652375008318869169[150] = 0.0; + out_652375008318869169[151] = 0.0; + out_652375008318869169[152] = 1.0; + out_652375008318869169[153] = 0.0; + out_652375008318869169[154] = 0.0; + out_652375008318869169[155] = 0.0; + out_652375008318869169[156] = 0.0; + out_652375008318869169[157] = 0.0; + out_652375008318869169[158] = 0.0; + out_652375008318869169[159] = 0.0; + out_652375008318869169[160] = 0.0; + out_652375008318869169[161] = 0.0; + out_652375008318869169[162] = 0.0; + out_652375008318869169[163] = 0.0; + out_652375008318869169[164] = 0.0; + out_652375008318869169[165] = 0.0; + out_652375008318869169[166] = 0.0; + out_652375008318869169[167] = 0.0; + out_652375008318869169[168] = 0.0; + out_652375008318869169[169] = 0.0; + out_652375008318869169[170] = 0.0; + out_652375008318869169[171] = 1.0; + out_652375008318869169[172] = 0.0; + out_652375008318869169[173] = 0.0; + out_652375008318869169[174] = 0.0; + out_652375008318869169[175] = 0.0; + out_652375008318869169[176] = 0.0; + out_652375008318869169[177] = 0.0; + out_652375008318869169[178] = 0.0; + out_652375008318869169[179] = 0.0; + out_652375008318869169[180] = 0.0; + out_652375008318869169[181] = 0.0; + out_652375008318869169[182] = 0.0; + out_652375008318869169[183] = 0.0; + out_652375008318869169[184] = 0.0; + out_652375008318869169[185] = 0.0; + out_652375008318869169[186] = 0.0; + out_652375008318869169[187] = 0.0; + out_652375008318869169[188] = 0.0; + out_652375008318869169[189] = 0.0; + out_652375008318869169[190] = 1.0; + out_652375008318869169[191] = 0.0; + out_652375008318869169[192] = 0.0; + out_652375008318869169[193] = 0.0; + out_652375008318869169[194] = 0.0; + out_652375008318869169[195] = 0.0; + out_652375008318869169[196] = 0.0; + out_652375008318869169[197] = 0.0; + out_652375008318869169[198] = 0.0; + out_652375008318869169[199] = 0.0; + out_652375008318869169[200] = 0.0; + out_652375008318869169[201] = 0.0; + out_652375008318869169[202] = 0.0; + out_652375008318869169[203] = 0.0; + out_652375008318869169[204] = 0.0; + out_652375008318869169[205] = 0.0; + out_652375008318869169[206] = 0.0; + out_652375008318869169[207] = 0.0; + out_652375008318869169[208] = 0.0; + out_652375008318869169[209] = 1.0; + out_652375008318869169[210] = 0.0; + out_652375008318869169[211] = 0.0; + out_652375008318869169[212] = 0.0; + out_652375008318869169[213] = 0.0; + out_652375008318869169[214] = 0.0; + out_652375008318869169[215] = 0.0; + out_652375008318869169[216] = 0.0; + out_652375008318869169[217] = 0.0; + out_652375008318869169[218] = 0.0; + out_652375008318869169[219] = 0.0; + out_652375008318869169[220] = 0.0; + out_652375008318869169[221] = 0.0; + out_652375008318869169[222] = 0.0; + out_652375008318869169[223] = 0.0; + out_652375008318869169[224] = 0.0; + out_652375008318869169[225] = 0.0; + out_652375008318869169[226] = 0.0; + out_652375008318869169[227] = 0.0; + out_652375008318869169[228] = 1.0; + out_652375008318869169[229] = 0.0; + out_652375008318869169[230] = 0.0; + out_652375008318869169[231] = 0.0; + out_652375008318869169[232] = 0.0; + out_652375008318869169[233] = 0.0; + out_652375008318869169[234] = 0.0; + out_652375008318869169[235] = 0.0; + out_652375008318869169[236] = 0.0; + out_652375008318869169[237] = 0.0; + out_652375008318869169[238] = 0.0; + out_652375008318869169[239] = 0.0; + out_652375008318869169[240] = 0.0; + out_652375008318869169[241] = 0.0; + out_652375008318869169[242] = 0.0; + out_652375008318869169[243] = 0.0; + out_652375008318869169[244] = 0.0; + out_652375008318869169[245] = 0.0; + out_652375008318869169[246] = 0.0; + out_652375008318869169[247] = 1.0; + out_652375008318869169[248] = 0.0; + out_652375008318869169[249] = 0.0; + out_652375008318869169[250] = 0.0; + out_652375008318869169[251] = 0.0; + out_652375008318869169[252] = 0.0; + out_652375008318869169[253] = 0.0; + out_652375008318869169[254] = 0.0; + out_652375008318869169[255] = 0.0; + out_652375008318869169[256] = 0.0; + out_652375008318869169[257] = 0.0; + out_652375008318869169[258] = 0.0; + out_652375008318869169[259] = 0.0; + out_652375008318869169[260] = 0.0; + out_652375008318869169[261] = 0.0; + out_652375008318869169[262] = 0.0; + out_652375008318869169[263] = 0.0; + out_652375008318869169[264] = 0.0; + out_652375008318869169[265] = 0.0; + out_652375008318869169[266] = 1.0; + out_652375008318869169[267] = 0.0; + out_652375008318869169[268] = 0.0; + out_652375008318869169[269] = 0.0; + out_652375008318869169[270] = 0.0; + out_652375008318869169[271] = 0.0; + out_652375008318869169[272] = 0.0; + out_652375008318869169[273] = 0.0; + out_652375008318869169[274] = 0.0; + out_652375008318869169[275] = 0.0; + out_652375008318869169[276] = 0.0; + out_652375008318869169[277] = 0.0; + out_652375008318869169[278] = 0.0; + out_652375008318869169[279] = 0.0; + out_652375008318869169[280] = 0.0; + out_652375008318869169[281] = 0.0; + out_652375008318869169[282] = 0.0; + out_652375008318869169[283] = 0.0; + out_652375008318869169[284] = 0.0; + out_652375008318869169[285] = 1.0; + out_652375008318869169[286] = 0.0; + out_652375008318869169[287] = 0.0; + out_652375008318869169[288] = 0.0; + out_652375008318869169[289] = 0.0; + out_652375008318869169[290] = 0.0; + out_652375008318869169[291] = 0.0; + out_652375008318869169[292] = 0.0; + out_652375008318869169[293] = 0.0; + out_652375008318869169[294] = 0.0; + out_652375008318869169[295] = 0.0; + out_652375008318869169[296] = 0.0; + out_652375008318869169[297] = 0.0; + out_652375008318869169[298] = 0.0; + out_652375008318869169[299] = 0.0; + out_652375008318869169[300] = 0.0; + out_652375008318869169[301] = 0.0; + out_652375008318869169[302] = 0.0; + out_652375008318869169[303] = 0.0; + out_652375008318869169[304] = 1.0; + out_652375008318869169[305] = 0.0; + out_652375008318869169[306] = 0.0; + out_652375008318869169[307] = 0.0; + out_652375008318869169[308] = 0.0; + out_652375008318869169[309] = 0.0; + out_652375008318869169[310] = 0.0; + out_652375008318869169[311] = 0.0; + out_652375008318869169[312] = 0.0; + out_652375008318869169[313] = 0.0; + out_652375008318869169[314] = 0.0; + out_652375008318869169[315] = 0.0; + out_652375008318869169[316] = 0.0; + out_652375008318869169[317] = 0.0; + out_652375008318869169[318] = 0.0; + out_652375008318869169[319] = 0.0; + out_652375008318869169[320] = 0.0; + out_652375008318869169[321] = 0.0; + out_652375008318869169[322] = 0.0; + out_652375008318869169[323] = 1.0; } -void f_fun(double *state, double dt, double *out_6753877240357292479) { - out_6753877240357292479[0] = atan2((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), -(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1])); - out_6753877240357292479[1] = asin(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8])); - out_6753877240357292479[2] = atan2(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), -(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2])); - out_6753877240357292479[3] = dt*state[12] + state[3]; - out_6753877240357292479[4] = dt*state[13] + state[4]; - out_6753877240357292479[5] = dt*state[14] + state[5]; - out_6753877240357292479[6] = state[6]; - out_6753877240357292479[7] = state[7]; - out_6753877240357292479[8] = state[8]; - out_6753877240357292479[9] = state[9]; - out_6753877240357292479[10] = state[10]; - out_6753877240357292479[11] = state[11]; - out_6753877240357292479[12] = state[12]; - out_6753877240357292479[13] = state[13]; - out_6753877240357292479[14] = state[14]; - out_6753877240357292479[15] = state[15]; - out_6753877240357292479[16] = state[16]; - out_6753877240357292479[17] = state[17]; +void f_fun(double *state, double dt, double *out_1837308153381576760) { + out_1837308153381576760[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_1837308153381576760[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_1837308153381576760[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_1837308153381576760[3] = dt*state[12] + state[3]; + out_1837308153381576760[4] = dt*state[13] + state[4]; + out_1837308153381576760[5] = dt*state[14] + state[5]; + out_1837308153381576760[6] = state[6]; + out_1837308153381576760[7] = state[7]; + out_1837308153381576760[8] = state[8]; + out_1837308153381576760[9] = state[9]; + out_1837308153381576760[10] = state[10]; + out_1837308153381576760[11] = state[11]; + out_1837308153381576760[12] = state[12]; + out_1837308153381576760[13] = state[13]; + out_1837308153381576760[14] = state[14]; + out_1837308153381576760[15] = state[15]; + out_1837308153381576760[16] = state[16]; + out_1837308153381576760[17] = state[17]; } -void F_fun(double *state, double dt, double *out_48451545294533288) { - out_48451545294533288[0] = ((-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*cos(state[0])*cos(state[1]) - sin(state[0])*cos(dt*state[6])*cos(dt*state[7])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + ((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*cos(state[0])*cos(state[1]) - sin(dt*state[6])*sin(state[0])*cos(dt*state[7])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_48451545294533288[1] = ((-sin(dt*state[6])*sin(dt*state[8]) - sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*cos(state[1]) - (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*sin(state[1]) - sin(state[1])*cos(dt*state[6])*cos(dt*state[7])*cos(state[0]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*sin(state[1]) + (-sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) + sin(dt*state[8])*cos(dt*state[6]))*cos(state[1]) - sin(dt*state[6])*sin(state[1])*cos(dt*state[7])*cos(state[0]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_48451545294533288[2] = 0; - out_48451545294533288[3] = 0; - out_48451545294533288[4] = 0; - out_48451545294533288[5] = 0; - out_48451545294533288[6] = (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(dt*cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*sin(dt*state[8]) - dt*sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-dt*sin(dt*state[6])*cos(dt*state[8]) + dt*sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) - dt*cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (dt*sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_48451545294533288[7] = (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[6])*sin(dt*state[7])*cos(state[0])*cos(state[1]) + dt*sin(dt*state[6])*sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) - dt*sin(dt*state[6])*sin(state[1])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[7])*cos(dt*state[6])*cos(state[0])*cos(state[1]) + dt*sin(dt*state[8])*sin(state[0])*cos(dt*state[6])*cos(dt*state[7])*cos(state[1]) - dt*sin(state[1])*cos(dt*state[6])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_48451545294533288[8] = ((dt*sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + dt*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (dt*sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + ((dt*sin(dt*state[6])*sin(dt*state[8]) + dt*sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*cos(dt*state[8]) + dt*sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_48451545294533288[9] = 0; - out_48451545294533288[10] = 0; - out_48451545294533288[11] = 0; - out_48451545294533288[12] = 0; - out_48451545294533288[13] = 0; - out_48451545294533288[14] = 0; - out_48451545294533288[15] = 0; - out_48451545294533288[16] = 0; - out_48451545294533288[17] = 0; - out_48451545294533288[18] = (-sin(dt*state[7])*sin(state[0])*cos(state[1]) - sin(dt*state[8])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); - out_48451545294533288[19] = (-sin(dt*state[7])*sin(state[1])*cos(state[0]) + sin(dt*state[8])*sin(state[0])*sin(state[1])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); - out_48451545294533288[20] = 0; - out_48451545294533288[21] = 0; - out_48451545294533288[22] = 0; - out_48451545294533288[23] = 0; - out_48451545294533288[24] = 0; - out_48451545294533288[25] = (dt*sin(dt*state[7])*sin(dt*state[8])*sin(state[0])*cos(state[1]) - dt*sin(dt*state[7])*sin(state[1])*cos(dt*state[8]) + dt*cos(dt*state[7])*cos(state[0])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); - out_48451545294533288[26] = (-dt*sin(dt*state[8])*sin(state[1])*cos(dt*state[7]) - dt*sin(state[0])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); - out_48451545294533288[27] = 0; - out_48451545294533288[28] = 0; - out_48451545294533288[29] = 0; - out_48451545294533288[30] = 0; - out_48451545294533288[31] = 0; - out_48451545294533288[32] = 0; - out_48451545294533288[33] = 0; - out_48451545294533288[34] = 0; - out_48451545294533288[35] = 0; - out_48451545294533288[36] = ((sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[7]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[7]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_48451545294533288[37] = (-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(-sin(dt*state[7])*sin(state[2])*cos(state[0])*cos(state[1]) + sin(dt*state[8])*sin(state[0])*sin(state[2])*cos(dt*state[7])*cos(state[1]) - sin(state[1])*sin(state[2])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*(-sin(dt*state[7])*cos(state[0])*cos(state[1])*cos(state[2]) + sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1])*cos(state[2]) - sin(state[1])*cos(dt*state[7])*cos(dt*state[8])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_48451545294533288[38] = ((-sin(state[0])*sin(state[2]) - sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (-sin(state[0])*sin(state[1])*sin(state[2]) - cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_48451545294533288[39] = 0; - out_48451545294533288[40] = 0; - out_48451545294533288[41] = 0; - out_48451545294533288[42] = 0; - out_48451545294533288[43] = (-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(dt*(sin(state[0])*cos(state[2]) - sin(state[1])*sin(state[2])*cos(state[0]))*cos(dt*state[7]) - dt*(sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[7])*sin(dt*state[8]) - dt*sin(dt*state[7])*sin(state[2])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*(dt*(-sin(state[0])*sin(state[2]) - sin(state[1])*cos(state[0])*cos(state[2]))*cos(dt*state[7]) - dt*(sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[7])*sin(dt*state[8]) - dt*sin(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_48451545294533288[44] = (dt*(sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*cos(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*sin(state[2])*cos(dt*state[7])*cos(state[1]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + (dt*(sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*cos(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[7])*cos(state[1])*cos(state[2]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_48451545294533288[45] = 0; - out_48451545294533288[46] = 0; - out_48451545294533288[47] = 0; - out_48451545294533288[48] = 0; - out_48451545294533288[49] = 0; - out_48451545294533288[50] = 0; - out_48451545294533288[51] = 0; - out_48451545294533288[52] = 0; - out_48451545294533288[53] = 0; - out_48451545294533288[54] = 0; - out_48451545294533288[55] = 0; - out_48451545294533288[56] = 0; - out_48451545294533288[57] = 1; - out_48451545294533288[58] = 0; - out_48451545294533288[59] = 0; - out_48451545294533288[60] = 0; - out_48451545294533288[61] = 0; - out_48451545294533288[62] = 0; - out_48451545294533288[63] = 0; - out_48451545294533288[64] = 0; - out_48451545294533288[65] = 0; - out_48451545294533288[66] = dt; - out_48451545294533288[67] = 0; - out_48451545294533288[68] = 0; - out_48451545294533288[69] = 0; - out_48451545294533288[70] = 0; - out_48451545294533288[71] = 0; - out_48451545294533288[72] = 0; - out_48451545294533288[73] = 0; - out_48451545294533288[74] = 0; - out_48451545294533288[75] = 0; - out_48451545294533288[76] = 1; - out_48451545294533288[77] = 0; - out_48451545294533288[78] = 0; - out_48451545294533288[79] = 0; - out_48451545294533288[80] = 0; - out_48451545294533288[81] = 0; - out_48451545294533288[82] = 0; - out_48451545294533288[83] = 0; - out_48451545294533288[84] = 0; - out_48451545294533288[85] = dt; - out_48451545294533288[86] = 0; - out_48451545294533288[87] = 0; - out_48451545294533288[88] = 0; - out_48451545294533288[89] = 0; - out_48451545294533288[90] = 0; - out_48451545294533288[91] = 0; - out_48451545294533288[92] = 0; - out_48451545294533288[93] = 0; - out_48451545294533288[94] = 0; - out_48451545294533288[95] = 1; - out_48451545294533288[96] = 0; - out_48451545294533288[97] = 0; - out_48451545294533288[98] = 0; - out_48451545294533288[99] = 0; - out_48451545294533288[100] = 0; - out_48451545294533288[101] = 0; - out_48451545294533288[102] = 0; - out_48451545294533288[103] = 0; - out_48451545294533288[104] = dt; - out_48451545294533288[105] = 0; - out_48451545294533288[106] = 0; - out_48451545294533288[107] = 0; - out_48451545294533288[108] = 0; - out_48451545294533288[109] = 0; - out_48451545294533288[110] = 0; - out_48451545294533288[111] = 0; - out_48451545294533288[112] = 0; - out_48451545294533288[113] = 0; - out_48451545294533288[114] = 1; - out_48451545294533288[115] = 0; - out_48451545294533288[116] = 0; - out_48451545294533288[117] = 0; - out_48451545294533288[118] = 0; - out_48451545294533288[119] = 0; - out_48451545294533288[120] = 0; - out_48451545294533288[121] = 0; - out_48451545294533288[122] = 0; - out_48451545294533288[123] = 0; - out_48451545294533288[124] = 0; - out_48451545294533288[125] = 0; - out_48451545294533288[126] = 0; - out_48451545294533288[127] = 0; - out_48451545294533288[128] = 0; - out_48451545294533288[129] = 0; - out_48451545294533288[130] = 0; - out_48451545294533288[131] = 0; - out_48451545294533288[132] = 0; - out_48451545294533288[133] = 1; - out_48451545294533288[134] = 0; - out_48451545294533288[135] = 0; - out_48451545294533288[136] = 0; - out_48451545294533288[137] = 0; - out_48451545294533288[138] = 0; - out_48451545294533288[139] = 0; - out_48451545294533288[140] = 0; - out_48451545294533288[141] = 0; - out_48451545294533288[142] = 0; - out_48451545294533288[143] = 0; - out_48451545294533288[144] = 0; - out_48451545294533288[145] = 0; - out_48451545294533288[146] = 0; - out_48451545294533288[147] = 0; - out_48451545294533288[148] = 0; - out_48451545294533288[149] = 0; - out_48451545294533288[150] = 0; - out_48451545294533288[151] = 0; - out_48451545294533288[152] = 1; - out_48451545294533288[153] = 0; - out_48451545294533288[154] = 0; - out_48451545294533288[155] = 0; - out_48451545294533288[156] = 0; - out_48451545294533288[157] = 0; - out_48451545294533288[158] = 0; - out_48451545294533288[159] = 0; - out_48451545294533288[160] = 0; - out_48451545294533288[161] = 0; - out_48451545294533288[162] = 0; - out_48451545294533288[163] = 0; - out_48451545294533288[164] = 0; - out_48451545294533288[165] = 0; - out_48451545294533288[166] = 0; - out_48451545294533288[167] = 0; - out_48451545294533288[168] = 0; - out_48451545294533288[169] = 0; - out_48451545294533288[170] = 0; - out_48451545294533288[171] = 1; - out_48451545294533288[172] = 0; - out_48451545294533288[173] = 0; - out_48451545294533288[174] = 0; - out_48451545294533288[175] = 0; - out_48451545294533288[176] = 0; - out_48451545294533288[177] = 0; - out_48451545294533288[178] = 0; - out_48451545294533288[179] = 0; - out_48451545294533288[180] = 0; - out_48451545294533288[181] = 0; - out_48451545294533288[182] = 0; - out_48451545294533288[183] = 0; - out_48451545294533288[184] = 0; - out_48451545294533288[185] = 0; - out_48451545294533288[186] = 0; - out_48451545294533288[187] = 0; - out_48451545294533288[188] = 0; - out_48451545294533288[189] = 0; - out_48451545294533288[190] = 1; - out_48451545294533288[191] = 0; - out_48451545294533288[192] = 0; - out_48451545294533288[193] = 0; - out_48451545294533288[194] = 0; - out_48451545294533288[195] = 0; - out_48451545294533288[196] = 0; - out_48451545294533288[197] = 0; - out_48451545294533288[198] = 0; - out_48451545294533288[199] = 0; - out_48451545294533288[200] = 0; - out_48451545294533288[201] = 0; - out_48451545294533288[202] = 0; - out_48451545294533288[203] = 0; - out_48451545294533288[204] = 0; - out_48451545294533288[205] = 0; - out_48451545294533288[206] = 0; - out_48451545294533288[207] = 0; - out_48451545294533288[208] = 0; - out_48451545294533288[209] = 1; - out_48451545294533288[210] = 0; - out_48451545294533288[211] = 0; - out_48451545294533288[212] = 0; - out_48451545294533288[213] = 0; - out_48451545294533288[214] = 0; - out_48451545294533288[215] = 0; - out_48451545294533288[216] = 0; - out_48451545294533288[217] = 0; - out_48451545294533288[218] = 0; - out_48451545294533288[219] = 0; - out_48451545294533288[220] = 0; - out_48451545294533288[221] = 0; - out_48451545294533288[222] = 0; - out_48451545294533288[223] = 0; - out_48451545294533288[224] = 0; - out_48451545294533288[225] = 0; - out_48451545294533288[226] = 0; - out_48451545294533288[227] = 0; - out_48451545294533288[228] = 1; - out_48451545294533288[229] = 0; - out_48451545294533288[230] = 0; - out_48451545294533288[231] = 0; - out_48451545294533288[232] = 0; - out_48451545294533288[233] = 0; - out_48451545294533288[234] = 0; - out_48451545294533288[235] = 0; - out_48451545294533288[236] = 0; - out_48451545294533288[237] = 0; - out_48451545294533288[238] = 0; - out_48451545294533288[239] = 0; - out_48451545294533288[240] = 0; - out_48451545294533288[241] = 0; - out_48451545294533288[242] = 0; - out_48451545294533288[243] = 0; - out_48451545294533288[244] = 0; - out_48451545294533288[245] = 0; - out_48451545294533288[246] = 0; - out_48451545294533288[247] = 1; - out_48451545294533288[248] = 0; - out_48451545294533288[249] = 0; - out_48451545294533288[250] = 0; - out_48451545294533288[251] = 0; - out_48451545294533288[252] = 0; - out_48451545294533288[253] = 0; - out_48451545294533288[254] = 0; - out_48451545294533288[255] = 0; - out_48451545294533288[256] = 0; - out_48451545294533288[257] = 0; - out_48451545294533288[258] = 0; - out_48451545294533288[259] = 0; - out_48451545294533288[260] = 0; - out_48451545294533288[261] = 0; - out_48451545294533288[262] = 0; - out_48451545294533288[263] = 0; - out_48451545294533288[264] = 0; - out_48451545294533288[265] = 0; - out_48451545294533288[266] = 1; - out_48451545294533288[267] = 0; - out_48451545294533288[268] = 0; - out_48451545294533288[269] = 0; - out_48451545294533288[270] = 0; - out_48451545294533288[271] = 0; - out_48451545294533288[272] = 0; - out_48451545294533288[273] = 0; - out_48451545294533288[274] = 0; - out_48451545294533288[275] = 0; - out_48451545294533288[276] = 0; - out_48451545294533288[277] = 0; - out_48451545294533288[278] = 0; - out_48451545294533288[279] = 0; - out_48451545294533288[280] = 0; - out_48451545294533288[281] = 0; - out_48451545294533288[282] = 0; - out_48451545294533288[283] = 0; - out_48451545294533288[284] = 0; - out_48451545294533288[285] = 1; - out_48451545294533288[286] = 0; - out_48451545294533288[287] = 0; - out_48451545294533288[288] = 0; - out_48451545294533288[289] = 0; - out_48451545294533288[290] = 0; - out_48451545294533288[291] = 0; - out_48451545294533288[292] = 0; - out_48451545294533288[293] = 0; - out_48451545294533288[294] = 0; - out_48451545294533288[295] = 0; - out_48451545294533288[296] = 0; - out_48451545294533288[297] = 0; - out_48451545294533288[298] = 0; - out_48451545294533288[299] = 0; - out_48451545294533288[300] = 0; - out_48451545294533288[301] = 0; - out_48451545294533288[302] = 0; - out_48451545294533288[303] = 0; - out_48451545294533288[304] = 1; - out_48451545294533288[305] = 0; - out_48451545294533288[306] = 0; - out_48451545294533288[307] = 0; - out_48451545294533288[308] = 0; - out_48451545294533288[309] = 0; - out_48451545294533288[310] = 0; - out_48451545294533288[311] = 0; - out_48451545294533288[312] = 0; - out_48451545294533288[313] = 0; - out_48451545294533288[314] = 0; - out_48451545294533288[315] = 0; - out_48451545294533288[316] = 0; - out_48451545294533288[317] = 0; - out_48451545294533288[318] = 0; - out_48451545294533288[319] = 0; - out_48451545294533288[320] = 0; - out_48451545294533288[321] = 0; - out_48451545294533288[322] = 0; - out_48451545294533288[323] = 1; +void F_fun(double *state, double dt, double *out_4411443613772384488) { + out_4411443613772384488[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_4411443613772384488[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_4411443613772384488[2] = 0; + out_4411443613772384488[3] = 0; + out_4411443613772384488[4] = 0; + out_4411443613772384488[5] = 0; + out_4411443613772384488[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_4411443613772384488[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_4411443613772384488[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_4411443613772384488[9] = 0; + out_4411443613772384488[10] = 0; + out_4411443613772384488[11] = 0; + out_4411443613772384488[12] = 0; + out_4411443613772384488[13] = 0; + out_4411443613772384488[14] = 0; + out_4411443613772384488[15] = 0; + out_4411443613772384488[16] = 0; + out_4411443613772384488[17] = 0; + out_4411443613772384488[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_4411443613772384488[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_4411443613772384488[20] = 0; + out_4411443613772384488[21] = 0; + out_4411443613772384488[22] = 0; + out_4411443613772384488[23] = 0; + out_4411443613772384488[24] = 0; + out_4411443613772384488[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_4411443613772384488[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_4411443613772384488[27] = 0; + out_4411443613772384488[28] = 0; + out_4411443613772384488[29] = 0; + out_4411443613772384488[30] = 0; + out_4411443613772384488[31] = 0; + out_4411443613772384488[32] = 0; + out_4411443613772384488[33] = 0; + out_4411443613772384488[34] = 0; + out_4411443613772384488[35] = 0; + out_4411443613772384488[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_4411443613772384488[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_4411443613772384488[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_4411443613772384488[39] = 0; + out_4411443613772384488[40] = 0; + out_4411443613772384488[41] = 0; + out_4411443613772384488[42] = 0; + out_4411443613772384488[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_4411443613772384488[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_4411443613772384488[45] = 0; + out_4411443613772384488[46] = 0; + out_4411443613772384488[47] = 0; + out_4411443613772384488[48] = 0; + out_4411443613772384488[49] = 0; + out_4411443613772384488[50] = 0; + out_4411443613772384488[51] = 0; + out_4411443613772384488[52] = 0; + out_4411443613772384488[53] = 0; + out_4411443613772384488[54] = 0; + out_4411443613772384488[55] = 0; + out_4411443613772384488[56] = 0; + out_4411443613772384488[57] = 1; + out_4411443613772384488[58] = 0; + out_4411443613772384488[59] = 0; + out_4411443613772384488[60] = 0; + out_4411443613772384488[61] = 0; + out_4411443613772384488[62] = 0; + out_4411443613772384488[63] = 0; + out_4411443613772384488[64] = 0; + out_4411443613772384488[65] = 0; + out_4411443613772384488[66] = dt; + out_4411443613772384488[67] = 0; + out_4411443613772384488[68] = 0; + out_4411443613772384488[69] = 0; + out_4411443613772384488[70] = 0; + out_4411443613772384488[71] = 0; + out_4411443613772384488[72] = 0; + out_4411443613772384488[73] = 0; + out_4411443613772384488[74] = 0; + out_4411443613772384488[75] = 0; + out_4411443613772384488[76] = 1; + out_4411443613772384488[77] = 0; + out_4411443613772384488[78] = 0; + out_4411443613772384488[79] = 0; + out_4411443613772384488[80] = 0; + out_4411443613772384488[81] = 0; + out_4411443613772384488[82] = 0; + out_4411443613772384488[83] = 0; + out_4411443613772384488[84] = 0; + out_4411443613772384488[85] = dt; + out_4411443613772384488[86] = 0; + out_4411443613772384488[87] = 0; + out_4411443613772384488[88] = 0; + out_4411443613772384488[89] = 0; + out_4411443613772384488[90] = 0; + out_4411443613772384488[91] = 0; + out_4411443613772384488[92] = 0; + out_4411443613772384488[93] = 0; + out_4411443613772384488[94] = 0; + out_4411443613772384488[95] = 1; + out_4411443613772384488[96] = 0; + out_4411443613772384488[97] = 0; + out_4411443613772384488[98] = 0; + out_4411443613772384488[99] = 0; + out_4411443613772384488[100] = 0; + out_4411443613772384488[101] = 0; + out_4411443613772384488[102] = 0; + out_4411443613772384488[103] = 0; + out_4411443613772384488[104] = dt; + out_4411443613772384488[105] = 0; + out_4411443613772384488[106] = 0; + out_4411443613772384488[107] = 0; + out_4411443613772384488[108] = 0; + out_4411443613772384488[109] = 0; + out_4411443613772384488[110] = 0; + out_4411443613772384488[111] = 0; + out_4411443613772384488[112] = 0; + out_4411443613772384488[113] = 0; + out_4411443613772384488[114] = 1; + out_4411443613772384488[115] = 0; + out_4411443613772384488[116] = 0; + out_4411443613772384488[117] = 0; + out_4411443613772384488[118] = 0; + out_4411443613772384488[119] = 0; + out_4411443613772384488[120] = 0; + out_4411443613772384488[121] = 0; + out_4411443613772384488[122] = 0; + out_4411443613772384488[123] = 0; + out_4411443613772384488[124] = 0; + out_4411443613772384488[125] = 0; + out_4411443613772384488[126] = 0; + out_4411443613772384488[127] = 0; + out_4411443613772384488[128] = 0; + out_4411443613772384488[129] = 0; + out_4411443613772384488[130] = 0; + out_4411443613772384488[131] = 0; + out_4411443613772384488[132] = 0; + out_4411443613772384488[133] = 1; + out_4411443613772384488[134] = 0; + out_4411443613772384488[135] = 0; + out_4411443613772384488[136] = 0; + out_4411443613772384488[137] = 0; + out_4411443613772384488[138] = 0; + out_4411443613772384488[139] = 0; + out_4411443613772384488[140] = 0; + out_4411443613772384488[141] = 0; + out_4411443613772384488[142] = 0; + out_4411443613772384488[143] = 0; + out_4411443613772384488[144] = 0; + out_4411443613772384488[145] = 0; + out_4411443613772384488[146] = 0; + out_4411443613772384488[147] = 0; + out_4411443613772384488[148] = 0; + out_4411443613772384488[149] = 0; + out_4411443613772384488[150] = 0; + out_4411443613772384488[151] = 0; + out_4411443613772384488[152] = 1; + out_4411443613772384488[153] = 0; + out_4411443613772384488[154] = 0; + out_4411443613772384488[155] = 0; + out_4411443613772384488[156] = 0; + out_4411443613772384488[157] = 0; + out_4411443613772384488[158] = 0; + out_4411443613772384488[159] = 0; + out_4411443613772384488[160] = 0; + out_4411443613772384488[161] = 0; + out_4411443613772384488[162] = 0; + out_4411443613772384488[163] = 0; + out_4411443613772384488[164] = 0; + out_4411443613772384488[165] = 0; + out_4411443613772384488[166] = 0; + out_4411443613772384488[167] = 0; + out_4411443613772384488[168] = 0; + out_4411443613772384488[169] = 0; + out_4411443613772384488[170] = 0; + out_4411443613772384488[171] = 1; + out_4411443613772384488[172] = 0; + out_4411443613772384488[173] = 0; + out_4411443613772384488[174] = 0; + out_4411443613772384488[175] = 0; + out_4411443613772384488[176] = 0; + out_4411443613772384488[177] = 0; + out_4411443613772384488[178] = 0; + out_4411443613772384488[179] = 0; + out_4411443613772384488[180] = 0; + out_4411443613772384488[181] = 0; + out_4411443613772384488[182] = 0; + out_4411443613772384488[183] = 0; + out_4411443613772384488[184] = 0; + out_4411443613772384488[185] = 0; + out_4411443613772384488[186] = 0; + out_4411443613772384488[187] = 0; + out_4411443613772384488[188] = 0; + out_4411443613772384488[189] = 0; + out_4411443613772384488[190] = 1; + out_4411443613772384488[191] = 0; + out_4411443613772384488[192] = 0; + out_4411443613772384488[193] = 0; + out_4411443613772384488[194] = 0; + out_4411443613772384488[195] = 0; + out_4411443613772384488[196] = 0; + out_4411443613772384488[197] = 0; + out_4411443613772384488[198] = 0; + out_4411443613772384488[199] = 0; + out_4411443613772384488[200] = 0; + out_4411443613772384488[201] = 0; + out_4411443613772384488[202] = 0; + out_4411443613772384488[203] = 0; + out_4411443613772384488[204] = 0; + out_4411443613772384488[205] = 0; + out_4411443613772384488[206] = 0; + out_4411443613772384488[207] = 0; + out_4411443613772384488[208] = 0; + out_4411443613772384488[209] = 1; + out_4411443613772384488[210] = 0; + out_4411443613772384488[211] = 0; + out_4411443613772384488[212] = 0; + out_4411443613772384488[213] = 0; + out_4411443613772384488[214] = 0; + out_4411443613772384488[215] = 0; + out_4411443613772384488[216] = 0; + out_4411443613772384488[217] = 0; + out_4411443613772384488[218] = 0; + out_4411443613772384488[219] = 0; + out_4411443613772384488[220] = 0; + out_4411443613772384488[221] = 0; + out_4411443613772384488[222] = 0; + out_4411443613772384488[223] = 0; + out_4411443613772384488[224] = 0; + out_4411443613772384488[225] = 0; + out_4411443613772384488[226] = 0; + out_4411443613772384488[227] = 0; + out_4411443613772384488[228] = 1; + out_4411443613772384488[229] = 0; + out_4411443613772384488[230] = 0; + out_4411443613772384488[231] = 0; + out_4411443613772384488[232] = 0; + out_4411443613772384488[233] = 0; + out_4411443613772384488[234] = 0; + out_4411443613772384488[235] = 0; + out_4411443613772384488[236] = 0; + out_4411443613772384488[237] = 0; + out_4411443613772384488[238] = 0; + out_4411443613772384488[239] = 0; + out_4411443613772384488[240] = 0; + out_4411443613772384488[241] = 0; + out_4411443613772384488[242] = 0; + out_4411443613772384488[243] = 0; + out_4411443613772384488[244] = 0; + out_4411443613772384488[245] = 0; + out_4411443613772384488[246] = 0; + out_4411443613772384488[247] = 1; + out_4411443613772384488[248] = 0; + out_4411443613772384488[249] = 0; + out_4411443613772384488[250] = 0; + out_4411443613772384488[251] = 0; + out_4411443613772384488[252] = 0; + out_4411443613772384488[253] = 0; + out_4411443613772384488[254] = 0; + out_4411443613772384488[255] = 0; + out_4411443613772384488[256] = 0; + out_4411443613772384488[257] = 0; + out_4411443613772384488[258] = 0; + out_4411443613772384488[259] = 0; + out_4411443613772384488[260] = 0; + out_4411443613772384488[261] = 0; + out_4411443613772384488[262] = 0; + out_4411443613772384488[263] = 0; + out_4411443613772384488[264] = 0; + out_4411443613772384488[265] = 0; + out_4411443613772384488[266] = 1; + out_4411443613772384488[267] = 0; + out_4411443613772384488[268] = 0; + out_4411443613772384488[269] = 0; + out_4411443613772384488[270] = 0; + out_4411443613772384488[271] = 0; + out_4411443613772384488[272] = 0; + out_4411443613772384488[273] = 0; + out_4411443613772384488[274] = 0; + out_4411443613772384488[275] = 0; + out_4411443613772384488[276] = 0; + out_4411443613772384488[277] = 0; + out_4411443613772384488[278] = 0; + out_4411443613772384488[279] = 0; + out_4411443613772384488[280] = 0; + out_4411443613772384488[281] = 0; + out_4411443613772384488[282] = 0; + out_4411443613772384488[283] = 0; + out_4411443613772384488[284] = 0; + out_4411443613772384488[285] = 1; + out_4411443613772384488[286] = 0; + out_4411443613772384488[287] = 0; + out_4411443613772384488[288] = 0; + out_4411443613772384488[289] = 0; + out_4411443613772384488[290] = 0; + out_4411443613772384488[291] = 0; + out_4411443613772384488[292] = 0; + out_4411443613772384488[293] = 0; + out_4411443613772384488[294] = 0; + out_4411443613772384488[295] = 0; + out_4411443613772384488[296] = 0; + out_4411443613772384488[297] = 0; + out_4411443613772384488[298] = 0; + out_4411443613772384488[299] = 0; + out_4411443613772384488[300] = 0; + out_4411443613772384488[301] = 0; + out_4411443613772384488[302] = 0; + out_4411443613772384488[303] = 0; + out_4411443613772384488[304] = 1; + out_4411443613772384488[305] = 0; + out_4411443613772384488[306] = 0; + out_4411443613772384488[307] = 0; + out_4411443613772384488[308] = 0; + out_4411443613772384488[309] = 0; + out_4411443613772384488[310] = 0; + out_4411443613772384488[311] = 0; + out_4411443613772384488[312] = 0; + out_4411443613772384488[313] = 0; + out_4411443613772384488[314] = 0; + out_4411443613772384488[315] = 0; + out_4411443613772384488[316] = 0; + out_4411443613772384488[317] = 0; + out_4411443613772384488[318] = 0; + out_4411443613772384488[319] = 0; + out_4411443613772384488[320] = 0; + out_4411443613772384488[321] = 0; + out_4411443613772384488[322] = 0; + out_4411443613772384488[323] = 1; } -void h_4(double *state, double *unused, double *out_711207782401779392) { - out_711207782401779392[0] = state[6] + state[9]; - out_711207782401779392[1] = state[7] + state[10]; - out_711207782401779392[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_8735933422126784440) { + out_8735933422126784440[0] = state[6] + state[9]; + out_8735933422126784440[1] = state[7] + state[10]; + out_8735933422126784440[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_6437019938278623926) { - out_6437019938278623926[0] = 0; - out_6437019938278623926[1] = 0; - out_6437019938278623926[2] = 0; - out_6437019938278623926[3] = 0; - out_6437019938278623926[4] = 0; - out_6437019938278623926[5] = 0; - out_6437019938278623926[6] = 1; - out_6437019938278623926[7] = 0; - out_6437019938278623926[8] = 0; - out_6437019938278623926[9] = 1; - out_6437019938278623926[10] = 0; - out_6437019938278623926[11] = 0; - out_6437019938278623926[12] = 0; - out_6437019938278623926[13] = 0; - out_6437019938278623926[14] = 0; - out_6437019938278623926[15] = 0; - out_6437019938278623926[16] = 0; - out_6437019938278623926[17] = 0; - out_6437019938278623926[18] = 0; - out_6437019938278623926[19] = 0; - out_6437019938278623926[20] = 0; - out_6437019938278623926[21] = 0; - out_6437019938278623926[22] = 0; - out_6437019938278623926[23] = 0; - out_6437019938278623926[24] = 0; - out_6437019938278623926[25] = 1; - out_6437019938278623926[26] = 0; - out_6437019938278623926[27] = 0; - out_6437019938278623926[28] = 1; - out_6437019938278623926[29] = 0; - out_6437019938278623926[30] = 0; - out_6437019938278623926[31] = 0; - out_6437019938278623926[32] = 0; - out_6437019938278623926[33] = 0; - out_6437019938278623926[34] = 0; - out_6437019938278623926[35] = 0; - out_6437019938278623926[36] = 0; - out_6437019938278623926[37] = 0; - out_6437019938278623926[38] = 0; - out_6437019938278623926[39] = 0; - out_6437019938278623926[40] = 0; - out_6437019938278623926[41] = 0; - out_6437019938278623926[42] = 0; - out_6437019938278623926[43] = 0; - out_6437019938278623926[44] = 1; - out_6437019938278623926[45] = 0; - out_6437019938278623926[46] = 0; - out_6437019938278623926[47] = 1; - out_6437019938278623926[48] = 0; - out_6437019938278623926[49] = 0; - out_6437019938278623926[50] = 0; - out_6437019938278623926[51] = 0; - out_6437019938278623926[52] = 0; - out_6437019938278623926[53] = 0; +void H_4(double *state, double *unused, double *out_2621847070472080084) { + out_2621847070472080084[0] = 0; + out_2621847070472080084[1] = 0; + out_2621847070472080084[2] = 0; + out_2621847070472080084[3] = 0; + out_2621847070472080084[4] = 0; + out_2621847070472080084[5] = 0; + out_2621847070472080084[6] = 1; + out_2621847070472080084[7] = 0; + out_2621847070472080084[8] = 0; + out_2621847070472080084[9] = 1; + out_2621847070472080084[10] = 0; + out_2621847070472080084[11] = 0; + out_2621847070472080084[12] = 0; + out_2621847070472080084[13] = 0; + out_2621847070472080084[14] = 0; + out_2621847070472080084[15] = 0; + out_2621847070472080084[16] = 0; + out_2621847070472080084[17] = 0; + out_2621847070472080084[18] = 0; + out_2621847070472080084[19] = 0; + out_2621847070472080084[20] = 0; + out_2621847070472080084[21] = 0; + out_2621847070472080084[22] = 0; + out_2621847070472080084[23] = 0; + out_2621847070472080084[24] = 0; + out_2621847070472080084[25] = 1; + out_2621847070472080084[26] = 0; + out_2621847070472080084[27] = 0; + out_2621847070472080084[28] = 1; + out_2621847070472080084[29] = 0; + out_2621847070472080084[30] = 0; + out_2621847070472080084[31] = 0; + out_2621847070472080084[32] = 0; + out_2621847070472080084[33] = 0; + out_2621847070472080084[34] = 0; + out_2621847070472080084[35] = 0; + out_2621847070472080084[36] = 0; + out_2621847070472080084[37] = 0; + out_2621847070472080084[38] = 0; + out_2621847070472080084[39] = 0; + out_2621847070472080084[40] = 0; + out_2621847070472080084[41] = 0; + out_2621847070472080084[42] = 0; + out_2621847070472080084[43] = 0; + out_2621847070472080084[44] = 1; + out_2621847070472080084[45] = 0; + out_2621847070472080084[46] = 0; + out_2621847070472080084[47] = 1; + out_2621847070472080084[48] = 0; + out_2621847070472080084[49] = 0; + out_2621847070472080084[50] = 0; + out_2621847070472080084[51] = 0; + out_2621847070472080084[52] = 0; + out_2621847070472080084[53] = 0; } -void h_10(double *state, double *unused, double *out_2942660564170460979) { - out_2942660564170460979[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_2942660564170460979[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_2942660564170460979[2] = -9.8100000000000005*cos(state[0])*cos(state[1]) - state[3]*state[7] + state[4]*state[6] + state[14] + state[17]; +void h_10(double *state, double *unused, double *out_6171703416781276782) { + out_6171703416781276782[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_6171703416781276782[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_6171703416781276782[2] = -9.8100000000000005*cos(state[0])*cos(state[1]) - state[3]*state[7] + state[4]*state[6] + state[14] + state[17]; } -void H_10(double *state, double *unused, double *out_7753365329259358525) { - out_7753365329259358525[0] = 0; - out_7753365329259358525[1] = 9.8100000000000005*cos(state[1]); - out_7753365329259358525[2] = 0; - out_7753365329259358525[3] = 0; - out_7753365329259358525[4] = -state[8]; - out_7753365329259358525[5] = state[7]; - out_7753365329259358525[6] = 0; - out_7753365329259358525[7] = state[5]; - out_7753365329259358525[8] = -state[4]; - out_7753365329259358525[9] = 0; - out_7753365329259358525[10] = 0; - out_7753365329259358525[11] = 0; - out_7753365329259358525[12] = 1; - out_7753365329259358525[13] = 0; - out_7753365329259358525[14] = 0; - out_7753365329259358525[15] = 1; - out_7753365329259358525[16] = 0; - out_7753365329259358525[17] = 0; - out_7753365329259358525[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_7753365329259358525[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_7753365329259358525[20] = 0; - out_7753365329259358525[21] = state[8]; - out_7753365329259358525[22] = 0; - out_7753365329259358525[23] = -state[6]; - out_7753365329259358525[24] = -state[5]; - out_7753365329259358525[25] = 0; - out_7753365329259358525[26] = state[3]; - out_7753365329259358525[27] = 0; - out_7753365329259358525[28] = 0; - out_7753365329259358525[29] = 0; - out_7753365329259358525[30] = 0; - out_7753365329259358525[31] = 1; - out_7753365329259358525[32] = 0; - out_7753365329259358525[33] = 0; - out_7753365329259358525[34] = 1; - out_7753365329259358525[35] = 0; - out_7753365329259358525[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_7753365329259358525[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_7753365329259358525[38] = 0; - out_7753365329259358525[39] = -state[7]; - out_7753365329259358525[40] = state[6]; - out_7753365329259358525[41] = 0; - out_7753365329259358525[42] = state[4]; - out_7753365329259358525[43] = -state[3]; - out_7753365329259358525[44] = 0; - out_7753365329259358525[45] = 0; - out_7753365329259358525[46] = 0; - out_7753365329259358525[47] = 0; - out_7753365329259358525[48] = 0; - out_7753365329259358525[49] = 0; - out_7753365329259358525[50] = 1; - out_7753365329259358525[51] = 0; - out_7753365329259358525[52] = 0; - out_7753365329259358525[53] = 1; +void H_10(double *state, double *unused, double *out_8600432860711518676) { + out_8600432860711518676[0] = 0; + out_8600432860711518676[1] = 9.8100000000000005*cos(state[1]); + out_8600432860711518676[2] = 0; + out_8600432860711518676[3] = 0; + out_8600432860711518676[4] = -state[8]; + out_8600432860711518676[5] = state[7]; + out_8600432860711518676[6] = 0; + out_8600432860711518676[7] = state[5]; + out_8600432860711518676[8] = -state[4]; + out_8600432860711518676[9] = 0; + out_8600432860711518676[10] = 0; + out_8600432860711518676[11] = 0; + out_8600432860711518676[12] = 1; + out_8600432860711518676[13] = 0; + out_8600432860711518676[14] = 0; + out_8600432860711518676[15] = 1; + out_8600432860711518676[16] = 0; + out_8600432860711518676[17] = 0; + out_8600432860711518676[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_8600432860711518676[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_8600432860711518676[20] = 0; + out_8600432860711518676[21] = state[8]; + out_8600432860711518676[22] = 0; + out_8600432860711518676[23] = -state[6]; + out_8600432860711518676[24] = -state[5]; + out_8600432860711518676[25] = 0; + out_8600432860711518676[26] = state[3]; + out_8600432860711518676[27] = 0; + out_8600432860711518676[28] = 0; + out_8600432860711518676[29] = 0; + out_8600432860711518676[30] = 0; + out_8600432860711518676[31] = 1; + out_8600432860711518676[32] = 0; + out_8600432860711518676[33] = 0; + out_8600432860711518676[34] = 1; + out_8600432860711518676[35] = 0; + out_8600432860711518676[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_8600432860711518676[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_8600432860711518676[38] = 0; + out_8600432860711518676[39] = -state[7]; + out_8600432860711518676[40] = state[6]; + out_8600432860711518676[41] = 0; + out_8600432860711518676[42] = state[4]; + out_8600432860711518676[43] = -state[3]; + out_8600432860711518676[44] = 0; + out_8600432860711518676[45] = 0; + out_8600432860711518676[46] = 0; + out_8600432860711518676[47] = 0; + out_8600432860711518676[48] = 0; + out_8600432860711518676[49] = 0; + out_8600432860711518676[50] = 1; + out_8600432860711518676[51] = 0; + out_8600432860711518676[52] = 0; + out_8600432860711518676[53] = 1; } -void h_13(double *state, double *unused, double *out_2914132243760424113) { - out_2914132243760424113[0] = state[3]; - out_2914132243760424113[1] = state[4]; - out_2914132243760424113[2] = state[5]; +void h_13(double *state, double *unused, double *out_7399425647141855355) { + out_7399425647141855355[0] = state[3]; + out_7399425647141855355[1] = state[4]; + out_7399425647141855355[2] = state[5]; } -void H_13(double *state, double *unused, double *out_3224746112946291125) { - out_3224746112946291125[0] = 0; - out_3224746112946291125[1] = 0; - out_3224746112946291125[2] = 0; - out_3224746112946291125[3] = 1; - out_3224746112946291125[4] = 0; - out_3224746112946291125[5] = 0; - out_3224746112946291125[6] = 0; - out_3224746112946291125[7] = 0; - out_3224746112946291125[8] = 0; - out_3224746112946291125[9] = 0; - out_3224746112946291125[10] = 0; - out_3224746112946291125[11] = 0; - out_3224746112946291125[12] = 0; - out_3224746112946291125[13] = 0; - out_3224746112946291125[14] = 0; - out_3224746112946291125[15] = 0; - out_3224746112946291125[16] = 0; - out_3224746112946291125[17] = 0; - out_3224746112946291125[18] = 0; - out_3224746112946291125[19] = 0; - out_3224746112946291125[20] = 0; - out_3224746112946291125[21] = 0; - out_3224746112946291125[22] = 1; - out_3224746112946291125[23] = 0; - out_3224746112946291125[24] = 0; - out_3224746112946291125[25] = 0; - out_3224746112946291125[26] = 0; - out_3224746112946291125[27] = 0; - out_3224746112946291125[28] = 0; - out_3224746112946291125[29] = 0; - out_3224746112946291125[30] = 0; - out_3224746112946291125[31] = 0; - out_3224746112946291125[32] = 0; - out_3224746112946291125[33] = 0; - out_3224746112946291125[34] = 0; - out_3224746112946291125[35] = 0; - out_3224746112946291125[36] = 0; - out_3224746112946291125[37] = 0; - out_3224746112946291125[38] = 0; - out_3224746112946291125[39] = 0; - out_3224746112946291125[40] = 0; - out_3224746112946291125[41] = 1; - out_3224746112946291125[42] = 0; - out_3224746112946291125[43] = 0; - out_3224746112946291125[44] = 0; - out_3224746112946291125[45] = 0; - out_3224746112946291125[46] = 0; - out_3224746112946291125[47] = 0; - out_3224746112946291125[48] = 0; - out_3224746112946291125[49] = 0; - out_3224746112946291125[50] = 0; - out_3224746112946291125[51] = 0; - out_3224746112946291125[52] = 0; - out_3224746112946291125[53] = 0; +void H_13(double *state, double *unused, double *out_590426754860252717) { + out_590426754860252717[0] = 0; + out_590426754860252717[1] = 0; + out_590426754860252717[2] = 0; + out_590426754860252717[3] = 1; + out_590426754860252717[4] = 0; + out_590426754860252717[5] = 0; + out_590426754860252717[6] = 0; + out_590426754860252717[7] = 0; + out_590426754860252717[8] = 0; + out_590426754860252717[9] = 0; + out_590426754860252717[10] = 0; + out_590426754860252717[11] = 0; + out_590426754860252717[12] = 0; + out_590426754860252717[13] = 0; + out_590426754860252717[14] = 0; + out_590426754860252717[15] = 0; + out_590426754860252717[16] = 0; + out_590426754860252717[17] = 0; + out_590426754860252717[18] = 0; + out_590426754860252717[19] = 0; + out_590426754860252717[20] = 0; + out_590426754860252717[21] = 0; + out_590426754860252717[22] = 1; + out_590426754860252717[23] = 0; + out_590426754860252717[24] = 0; + out_590426754860252717[25] = 0; + out_590426754860252717[26] = 0; + out_590426754860252717[27] = 0; + out_590426754860252717[28] = 0; + out_590426754860252717[29] = 0; + out_590426754860252717[30] = 0; + out_590426754860252717[31] = 0; + out_590426754860252717[32] = 0; + out_590426754860252717[33] = 0; + out_590426754860252717[34] = 0; + out_590426754860252717[35] = 0; + out_590426754860252717[36] = 0; + out_590426754860252717[37] = 0; + out_590426754860252717[38] = 0; + out_590426754860252717[39] = 0; + out_590426754860252717[40] = 0; + out_590426754860252717[41] = 1; + out_590426754860252717[42] = 0; + out_590426754860252717[43] = 0; + out_590426754860252717[44] = 0; + out_590426754860252717[45] = 0; + out_590426754860252717[46] = 0; + out_590426754860252717[47] = 0; + out_590426754860252717[48] = 0; + out_590426754860252717[49] = 0; + out_590426754860252717[50] = 0; + out_590426754860252717[51] = 0; + out_590426754860252717[52] = 0; + out_590426754860252717[53] = 0; } -void h_14(double *state, double *unused, double *out_677306400367602538) { - out_677306400367602538[0] = state[6]; - out_677306400367602538[1] = state[7]; - out_677306400367602538[2] = state[8]; +void h_14(double *state, double *unused, double *out_7752950954395433666) { + out_7752950954395433666[0] = state[6]; + out_7752950954395433666[1] = state[7]; + out_7752950954395433666[2] = state[8]; } -void H_14(double *state, double *unused, double *out_2473779081939139397) { - out_2473779081939139397[0] = 0; - out_2473779081939139397[1] = 0; - out_2473779081939139397[2] = 0; - out_2473779081939139397[3] = 0; - out_2473779081939139397[4] = 0; - out_2473779081939139397[5] = 0; - out_2473779081939139397[6] = 1; - out_2473779081939139397[7] = 0; - out_2473779081939139397[8] = 0; - out_2473779081939139397[9] = 0; - out_2473779081939139397[10] = 0; - out_2473779081939139397[11] = 0; - out_2473779081939139397[12] = 0; - out_2473779081939139397[13] = 0; - out_2473779081939139397[14] = 0; - out_2473779081939139397[15] = 0; - out_2473779081939139397[16] = 0; - out_2473779081939139397[17] = 0; - out_2473779081939139397[18] = 0; - out_2473779081939139397[19] = 0; - out_2473779081939139397[20] = 0; - out_2473779081939139397[21] = 0; - out_2473779081939139397[22] = 0; - out_2473779081939139397[23] = 0; - out_2473779081939139397[24] = 0; - out_2473779081939139397[25] = 1; - out_2473779081939139397[26] = 0; - out_2473779081939139397[27] = 0; - out_2473779081939139397[28] = 0; - out_2473779081939139397[29] = 0; - out_2473779081939139397[30] = 0; - out_2473779081939139397[31] = 0; - out_2473779081939139397[32] = 0; - out_2473779081939139397[33] = 0; - out_2473779081939139397[34] = 0; - out_2473779081939139397[35] = 0; - out_2473779081939139397[36] = 0; - out_2473779081939139397[37] = 0; - out_2473779081939139397[38] = 0; - out_2473779081939139397[39] = 0; - out_2473779081939139397[40] = 0; - out_2473779081939139397[41] = 0; - out_2473779081939139397[42] = 0; - out_2473779081939139397[43] = 0; - out_2473779081939139397[44] = 1; - out_2473779081939139397[45] = 0; - out_2473779081939139397[46] = 0; - out_2473779081939139397[47] = 0; - out_2473779081939139397[48] = 0; - out_2473779081939139397[49] = 0; - out_2473779081939139397[50] = 0; - out_2473779081939139397[51] = 0; - out_2473779081939139397[52] = 0; - out_2473779081939139397[53] = 0; +void H_14(double *state, double *unused, double *out_1341393785867404445) { + out_1341393785867404445[0] = 0; + out_1341393785867404445[1] = 0; + out_1341393785867404445[2] = 0; + out_1341393785867404445[3] = 0; + out_1341393785867404445[4] = 0; + out_1341393785867404445[5] = 0; + out_1341393785867404445[6] = 1; + out_1341393785867404445[7] = 0; + out_1341393785867404445[8] = 0; + out_1341393785867404445[9] = 0; + out_1341393785867404445[10] = 0; + out_1341393785867404445[11] = 0; + out_1341393785867404445[12] = 0; + out_1341393785867404445[13] = 0; + out_1341393785867404445[14] = 0; + out_1341393785867404445[15] = 0; + out_1341393785867404445[16] = 0; + out_1341393785867404445[17] = 0; + out_1341393785867404445[18] = 0; + out_1341393785867404445[19] = 0; + out_1341393785867404445[20] = 0; + out_1341393785867404445[21] = 0; + out_1341393785867404445[22] = 0; + out_1341393785867404445[23] = 0; + out_1341393785867404445[24] = 0; + out_1341393785867404445[25] = 1; + out_1341393785867404445[26] = 0; + out_1341393785867404445[27] = 0; + out_1341393785867404445[28] = 0; + out_1341393785867404445[29] = 0; + out_1341393785867404445[30] = 0; + out_1341393785867404445[31] = 0; + out_1341393785867404445[32] = 0; + out_1341393785867404445[33] = 0; + out_1341393785867404445[34] = 0; + out_1341393785867404445[35] = 0; + out_1341393785867404445[36] = 0; + out_1341393785867404445[37] = 0; + out_1341393785867404445[38] = 0; + out_1341393785867404445[39] = 0; + out_1341393785867404445[40] = 0; + out_1341393785867404445[41] = 0; + out_1341393785867404445[42] = 0; + out_1341393785867404445[43] = 0; + out_1341393785867404445[44] = 1; + out_1341393785867404445[45] = 0; + out_1341393785867404445[46] = 0; + out_1341393785867404445[47] = 0; + out_1341393785867404445[48] = 0; + out_1341393785867404445[49] = 0; + out_1341393785867404445[50] = 0; + out_1341393785867404445[51] = 0; + out_1341393785867404445[52] = 0; + out_1341393785867404445[53] = 0; } #include #include @@ -1113,44 +1113,44 @@ void pose_update_13(double *in_x, double *in_P, double *in_z, double *in_R, doub void pose_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { update<3, 3, 0>(in_x, in_P, h_14, H_14, NULL, in_z, in_R, in_ea, MAHA_THRESH_14); } -void pose_err_fun(double *nom_x, double *delta_x, double *out_1227579335886564773) { - err_fun(nom_x, delta_x, out_1227579335886564773); +void pose_err_fun(double *nom_x, double *delta_x, double *out_4115870174312830998) { + err_fun(nom_x, delta_x, out_4115870174312830998); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_7708296302708828001) { - inv_err_fun(nom_x, true_x, out_7708296302708828001); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2299838754417090386) { + inv_err_fun(nom_x, true_x, out_2299838754417090386); } -void pose_H_mod_fun(double *state, double *out_145151651698479708) { - H_mod_fun(state, out_145151651698479708); +void pose_H_mod_fun(double *state, double *out_652375008318869169) { + H_mod_fun(state, out_652375008318869169); } -void pose_f_fun(double *state, double dt, double *out_6753877240357292479) { - f_fun(state, dt, out_6753877240357292479); +void pose_f_fun(double *state, double dt, double *out_1837308153381576760) { + f_fun(state, dt, out_1837308153381576760); } -void pose_F_fun(double *state, double dt, double *out_48451545294533288) { - F_fun(state, dt, out_48451545294533288); +void pose_F_fun(double *state, double dt, double *out_4411443613772384488) { + F_fun(state, dt, out_4411443613772384488); } -void pose_h_4(double *state, double *unused, double *out_711207782401779392) { - h_4(state, unused, out_711207782401779392); +void pose_h_4(double *state, double *unused, double *out_8735933422126784440) { + h_4(state, unused, out_8735933422126784440); } -void pose_H_4(double *state, double *unused, double *out_6437019938278623926) { - H_4(state, unused, out_6437019938278623926); +void pose_H_4(double *state, double *unused, double *out_2621847070472080084) { + H_4(state, unused, out_2621847070472080084); } -void pose_h_10(double *state, double *unused, double *out_2942660564170460979) { - h_10(state, unused, out_2942660564170460979); +void pose_h_10(double *state, double *unused, double *out_6171703416781276782) { + h_10(state, unused, out_6171703416781276782); } -void pose_H_10(double *state, double *unused, double *out_7753365329259358525) { - H_10(state, unused, out_7753365329259358525); +void pose_H_10(double *state, double *unused, double *out_8600432860711518676) { + H_10(state, unused, out_8600432860711518676); } -void pose_h_13(double *state, double *unused, double *out_2914132243760424113) { - h_13(state, unused, out_2914132243760424113); +void pose_h_13(double *state, double *unused, double *out_7399425647141855355) { + h_13(state, unused, out_7399425647141855355); } -void pose_H_13(double *state, double *unused, double *out_3224746112946291125) { - H_13(state, unused, out_3224746112946291125); +void pose_H_13(double *state, double *unused, double *out_590426754860252717) { + H_13(state, unused, out_590426754860252717); } -void pose_h_14(double *state, double *unused, double *out_677306400367602538) { - h_14(state, unused, out_677306400367602538); +void pose_h_14(double *state, double *unused, double *out_7752950954395433666) { + h_14(state, unused, out_7752950954395433666); } -void pose_H_14(double *state, double *unused, double *out_2473779081939139397) { - H_14(state, unused, out_2473779081939139397); +void pose_H_14(double *state, double *unused, double *out_1341393785867404445) { + H_14(state, unused, out_1341393785867404445); } void pose_predict(double *in_x, double *in_P, double *in_Q, double dt) { predict(in_x, in_P, in_Q, dt); diff --git a/selfdrive/locationd/models/generated/pose.h b/selfdrive/locationd/models/generated/pose.h index 54b20c0ce6..b36f1ffc91 100644 --- a/selfdrive/locationd/models/generated/pose.h +++ b/selfdrive/locationd/models/generated/pose.h @@ -5,18 +5,18 @@ void pose_update_4(double *in_x, double *in_P, double *in_z, double *in_R, doubl void pose_update_10(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); void pose_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); void pose_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); -void pose_err_fun(double *nom_x, double *delta_x, double *out_1227579335886564773); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_7708296302708828001); -void pose_H_mod_fun(double *state, double *out_145151651698479708); -void pose_f_fun(double *state, double dt, double *out_6753877240357292479); -void pose_F_fun(double *state, double dt, double *out_48451545294533288); -void pose_h_4(double *state, double *unused, double *out_711207782401779392); -void pose_H_4(double *state, double *unused, double *out_6437019938278623926); -void pose_h_10(double *state, double *unused, double *out_2942660564170460979); -void pose_H_10(double *state, double *unused, double *out_7753365329259358525); -void pose_h_13(double *state, double *unused, double *out_2914132243760424113); -void pose_H_13(double *state, double *unused, double *out_3224746112946291125); -void pose_h_14(double *state, double *unused, double *out_677306400367602538); -void pose_H_14(double *state, double *unused, double *out_2473779081939139397); +void pose_err_fun(double *nom_x, double *delta_x, double *out_4115870174312830998); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2299838754417090386); +void pose_H_mod_fun(double *state, double *out_652375008318869169); +void pose_f_fun(double *state, double dt, double *out_1837308153381576760); +void pose_F_fun(double *state, double dt, double *out_4411443613772384488); +void pose_h_4(double *state, double *unused, double *out_8735933422126784440); +void pose_H_4(double *state, double *unused, double *out_2621847070472080084); +void pose_h_10(double *state, double *unused, double *out_6171703416781276782); +void pose_H_10(double *state, double *unused, double *out_8600432860711518676); +void pose_h_13(double *state, double *unused, double *out_7399425647141855355); +void pose_H_13(double *state, double *unused, double *out_590426754860252717); +void pose_h_14(double *state, double *unused, double *out_7752950954395433666); +void pose_H_14(double *state, double *unused, double *out_1341393785867404445); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/pose.os b/selfdrive/locationd/models/generated/pose.os index b9449ef3d7..d32b2d3bba 100644 Binary files a/selfdrive/locationd/models/generated/pose.os and b/selfdrive/locationd/models/generated/pose.os differ diff --git a/selfdrive/pandad/pandad b/selfdrive/pandad/pandad index 6fc41f0af4..0990bc394f 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/system/camerad/camerad b/system/camerad/camerad index d32f2edf4f..8729db7346 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index 24b6b3df58..f0cbc803c2 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index 3f9d5cdf22..8ccf7c3846 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index ef838d5e28..f80721e22d 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ