diff --git a/common/libcommon.a b/common/libcommon.a index 28e8aeaa94..8bd6bef5a3 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 2bce85cc71..20b7da4032 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 4f18b5eac4..79dc7f8032 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 d6aabc19bc..eb82dea0b1 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 2f2a301c2b..d84d2580bf 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 8e9cf159c1..9bc4f360f3 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 587fbe8a9e..5b0db97e65 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 04e85e3c95..02a3e958d2 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 04e85e3c95..02a3e958d2 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 494a87db7e..74a6a9781a 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 494a87db7e..74a6a9781a 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 494a87db7e..74a6a9781a 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 494a87db7e..74a6a9781a 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 494a87db7e..74a6a9781a 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 494a87db7e..74a6a9781a 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 new file mode 100755 index 0000000000..74a6a9781a Binary files /dev/null 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 new file mode 100755 index 0000000000..74a6a9781a Binary files /dev/null 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 04e85e3c95..02a3e958d2 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 04e85e3c95..02a3e958d2 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 23bd983506..f7f1219a37 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 04e85e3c95..02a3e958d2 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 04e85e3c95..02a3e958d2 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 new file mode 100755 index 0000000000..02a3e958d2 Binary files /dev/null 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 new file mode 100755 index 0000000000..02a3e958d2 Binary files /dev/null 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 e9513dcbd7..295b0c610e 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-24b8789c-DEBUG"; +const uint8_t gitversion[19] = "DEV-bf00f88b-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 45f482beeb..9bacd1d525 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 8078504135..6a3de3ab51 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 3387248041..2bfb1a0794 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 26c820d8eb..29eb150bda 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 18d60274bc..8fea6d9abd 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 efebefd9f1..7921c5a2a1 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 dacb937c9c..d303ab85b2 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 16153420fe..66d0484015 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 069d89dd1f..462ad1ba28 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 c60f95e841..5d750d840e 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 6e5c3606b1..0c7fdd553f 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 1891dc848a..133f01beec 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 bfee8475f9..2949839ca9 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 104c1e471b..9ecb8a3f81 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 eb86dec710..ac59b6ad23 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 fa0b4ebffa..fc4d4386aa 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 cd312c9cfd..eb679ef747 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 2ab2ac104b..8352c46faf 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 cd05629831..4b98cc48d9 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 2aebf60673..65258255cf 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 520407178e..7a20d8c1e7 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 579cc7ec11..bde10404b2 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 8b05679179..ec7adea6fb 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 6ab5da0301..ceec401a21 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 f4495b19e3..68e98941c7 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 f404eb6d97..55b2fec7c4 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 b609066af7..faa806b7b6 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 4c7f6125b8..75109ddf76 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 88d56a6a14..2e12f210bb 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 079fdab3e1..1285d5bdab 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 64140d7afe..c2292a14be 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 1bf6e4ca1b..27d131b48c 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 95bfd5142a..485e6f2560 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 new file mode 100755 index 0000000000..c855e75ce4 Binary files /dev/null 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 new file mode 100755 index 0000000000..db06065cb6 Binary files /dev/null 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 new file mode 100755 index 0000000000..cf9fbfc017 Binary files /dev/null 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 5c4c135811..2ae7797478 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 new file mode 100755 index 0000000000..dbd039e5a2 Binary files /dev/null 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 new file mode 100755 index 0000000000..a7fb3ff752 Binary files /dev/null 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 new file mode 100755 index 0000000000..793c115a0f Binary files /dev/null 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 db6968f070..ee0b842c46 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 286356b6a5..7fbb7f81dd 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 d51b76803f..92397bed07 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 000f59b0bc..f85dedfa6f 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 8e3abfefdb..5e3b36ad51 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 efc624dbf4..54103c68a2 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 de03f64be1..982cca33cc 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 09a3c3951c..e3b0d10582 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 23cdd3d1d7..d030ec6705 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 52293178f6..a4623ef233 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 a202e86c1e..6633227269 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 f8e7afffa9..b2a650d925 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 f31a4800a7..8649076673 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 b89bde5364..8b96911113 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 6dec5f4cb4..e145359590 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 7fa8049062..acb46f6dbe 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 5bbddd52a3..8cfdb4acfc 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 dad4ecf815..f25a015aa4 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 301f8048b6..b10778b7de 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 1c57ebfc72..e22b61e585 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 e34c26643b..b60f7a4b89 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 new file mode 100755 index 0000000000..f8afbbbaf9 Binary files /dev/null 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 new file mode 100755 index 0000000000..9516e637af Binary files /dev/null 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 new file mode 100755 index 0000000000..839821faee Binary files /dev/null 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 517c26d571..ebfea0fd05 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 new file mode 100755 index 0000000000..fbf8850721 Binary files /dev/null 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 new file mode 100755 index 0000000000..81b595c13e Binary files /dev/null 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 new file mode 100755 index 0000000000..fde67ed8fe Binary files /dev/null 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 a50dcc36e8..e180d0f830 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-24b8789c-DEBUG \ No newline at end of file +DEV-bf00f88b-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 99250cec3b..abf9f4c7ee 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_1264104166079454940) { - out_1264104166079454940[0] = delta_x[0] + nom_x[0]; - out_1264104166079454940[1] = delta_x[1] + nom_x[1]; - out_1264104166079454940[2] = delta_x[2] + nom_x[2]; - out_1264104166079454940[3] = delta_x[3] + nom_x[3]; - out_1264104166079454940[4] = delta_x[4] + nom_x[4]; - out_1264104166079454940[5] = delta_x[5] + nom_x[5]; - out_1264104166079454940[6] = delta_x[6] + nom_x[6]; - out_1264104166079454940[7] = delta_x[7] + nom_x[7]; - out_1264104166079454940[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_1326346818289980211) { + out_1326346818289980211[0] = delta_x[0] + nom_x[0]; + out_1326346818289980211[1] = delta_x[1] + nom_x[1]; + out_1326346818289980211[2] = delta_x[2] + nom_x[2]; + out_1326346818289980211[3] = delta_x[3] + nom_x[3]; + out_1326346818289980211[4] = delta_x[4] + nom_x[4]; + out_1326346818289980211[5] = delta_x[5] + nom_x[5]; + out_1326346818289980211[6] = delta_x[6] + nom_x[6]; + out_1326346818289980211[7] = delta_x[7] + nom_x[7]; + out_1326346818289980211[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_3182169158929336617) { - out_3182169158929336617[0] = -nom_x[0] + true_x[0]; - out_3182169158929336617[1] = -nom_x[1] + true_x[1]; - out_3182169158929336617[2] = -nom_x[2] + true_x[2]; - out_3182169158929336617[3] = -nom_x[3] + true_x[3]; - out_3182169158929336617[4] = -nom_x[4] + true_x[4]; - out_3182169158929336617[5] = -nom_x[5] + true_x[5]; - out_3182169158929336617[6] = -nom_x[6] + true_x[6]; - out_3182169158929336617[7] = -nom_x[7] + true_x[7]; - out_3182169158929336617[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_7721308480372758585) { + out_7721308480372758585[0] = -nom_x[0] + true_x[0]; + out_7721308480372758585[1] = -nom_x[1] + true_x[1]; + out_7721308480372758585[2] = -nom_x[2] + true_x[2]; + out_7721308480372758585[3] = -nom_x[3] + true_x[3]; + out_7721308480372758585[4] = -nom_x[4] + true_x[4]; + out_7721308480372758585[5] = -nom_x[5] + true_x[5]; + out_7721308480372758585[6] = -nom_x[6] + true_x[6]; + out_7721308480372758585[7] = -nom_x[7] + true_x[7]; + out_7721308480372758585[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_3410660055109284948) { - out_3410660055109284948[0] = 1.0; - out_3410660055109284948[1] = 0.0; - out_3410660055109284948[2] = 0.0; - out_3410660055109284948[3] = 0.0; - out_3410660055109284948[4] = 0.0; - out_3410660055109284948[5] = 0.0; - out_3410660055109284948[6] = 0.0; - out_3410660055109284948[7] = 0.0; - out_3410660055109284948[8] = 0.0; - out_3410660055109284948[9] = 0.0; - out_3410660055109284948[10] = 1.0; - out_3410660055109284948[11] = 0.0; - out_3410660055109284948[12] = 0.0; - out_3410660055109284948[13] = 0.0; - out_3410660055109284948[14] = 0.0; - out_3410660055109284948[15] = 0.0; - out_3410660055109284948[16] = 0.0; - out_3410660055109284948[17] = 0.0; - out_3410660055109284948[18] = 0.0; - out_3410660055109284948[19] = 0.0; - out_3410660055109284948[20] = 1.0; - out_3410660055109284948[21] = 0.0; - out_3410660055109284948[22] = 0.0; - out_3410660055109284948[23] = 0.0; - out_3410660055109284948[24] = 0.0; - out_3410660055109284948[25] = 0.0; - out_3410660055109284948[26] = 0.0; - out_3410660055109284948[27] = 0.0; - out_3410660055109284948[28] = 0.0; - out_3410660055109284948[29] = 0.0; - out_3410660055109284948[30] = 1.0; - out_3410660055109284948[31] = 0.0; - out_3410660055109284948[32] = 0.0; - out_3410660055109284948[33] = 0.0; - out_3410660055109284948[34] = 0.0; - out_3410660055109284948[35] = 0.0; - out_3410660055109284948[36] = 0.0; - out_3410660055109284948[37] = 0.0; - out_3410660055109284948[38] = 0.0; - out_3410660055109284948[39] = 0.0; - out_3410660055109284948[40] = 1.0; - out_3410660055109284948[41] = 0.0; - out_3410660055109284948[42] = 0.0; - out_3410660055109284948[43] = 0.0; - out_3410660055109284948[44] = 0.0; - out_3410660055109284948[45] = 0.0; - out_3410660055109284948[46] = 0.0; - out_3410660055109284948[47] = 0.0; - out_3410660055109284948[48] = 0.0; - out_3410660055109284948[49] = 0.0; - out_3410660055109284948[50] = 1.0; - out_3410660055109284948[51] = 0.0; - out_3410660055109284948[52] = 0.0; - out_3410660055109284948[53] = 0.0; - out_3410660055109284948[54] = 0.0; - out_3410660055109284948[55] = 0.0; - out_3410660055109284948[56] = 0.0; - out_3410660055109284948[57] = 0.0; - out_3410660055109284948[58] = 0.0; - out_3410660055109284948[59] = 0.0; - out_3410660055109284948[60] = 1.0; - out_3410660055109284948[61] = 0.0; - out_3410660055109284948[62] = 0.0; - out_3410660055109284948[63] = 0.0; - out_3410660055109284948[64] = 0.0; - out_3410660055109284948[65] = 0.0; - out_3410660055109284948[66] = 0.0; - out_3410660055109284948[67] = 0.0; - out_3410660055109284948[68] = 0.0; - out_3410660055109284948[69] = 0.0; - out_3410660055109284948[70] = 1.0; - out_3410660055109284948[71] = 0.0; - out_3410660055109284948[72] = 0.0; - out_3410660055109284948[73] = 0.0; - out_3410660055109284948[74] = 0.0; - out_3410660055109284948[75] = 0.0; - out_3410660055109284948[76] = 0.0; - out_3410660055109284948[77] = 0.0; - out_3410660055109284948[78] = 0.0; - out_3410660055109284948[79] = 0.0; - out_3410660055109284948[80] = 1.0; +void H_mod_fun(double *state, double *out_8221305666287256291) { + out_8221305666287256291[0] = 1.0; + out_8221305666287256291[1] = 0.0; + out_8221305666287256291[2] = 0.0; + out_8221305666287256291[3] = 0.0; + out_8221305666287256291[4] = 0.0; + out_8221305666287256291[5] = 0.0; + out_8221305666287256291[6] = 0.0; + out_8221305666287256291[7] = 0.0; + out_8221305666287256291[8] = 0.0; + out_8221305666287256291[9] = 0.0; + out_8221305666287256291[10] = 1.0; + out_8221305666287256291[11] = 0.0; + out_8221305666287256291[12] = 0.0; + out_8221305666287256291[13] = 0.0; + out_8221305666287256291[14] = 0.0; + out_8221305666287256291[15] = 0.0; + out_8221305666287256291[16] = 0.0; + out_8221305666287256291[17] = 0.0; + out_8221305666287256291[18] = 0.0; + out_8221305666287256291[19] = 0.0; + out_8221305666287256291[20] = 1.0; + out_8221305666287256291[21] = 0.0; + out_8221305666287256291[22] = 0.0; + out_8221305666287256291[23] = 0.0; + out_8221305666287256291[24] = 0.0; + out_8221305666287256291[25] = 0.0; + out_8221305666287256291[26] = 0.0; + out_8221305666287256291[27] = 0.0; + out_8221305666287256291[28] = 0.0; + out_8221305666287256291[29] = 0.0; + out_8221305666287256291[30] = 1.0; + out_8221305666287256291[31] = 0.0; + out_8221305666287256291[32] = 0.0; + out_8221305666287256291[33] = 0.0; + out_8221305666287256291[34] = 0.0; + out_8221305666287256291[35] = 0.0; + out_8221305666287256291[36] = 0.0; + out_8221305666287256291[37] = 0.0; + out_8221305666287256291[38] = 0.0; + out_8221305666287256291[39] = 0.0; + out_8221305666287256291[40] = 1.0; + out_8221305666287256291[41] = 0.0; + out_8221305666287256291[42] = 0.0; + out_8221305666287256291[43] = 0.0; + out_8221305666287256291[44] = 0.0; + out_8221305666287256291[45] = 0.0; + out_8221305666287256291[46] = 0.0; + out_8221305666287256291[47] = 0.0; + out_8221305666287256291[48] = 0.0; + out_8221305666287256291[49] = 0.0; + out_8221305666287256291[50] = 1.0; + out_8221305666287256291[51] = 0.0; + out_8221305666287256291[52] = 0.0; + out_8221305666287256291[53] = 0.0; + out_8221305666287256291[54] = 0.0; + out_8221305666287256291[55] = 0.0; + out_8221305666287256291[56] = 0.0; + out_8221305666287256291[57] = 0.0; + out_8221305666287256291[58] = 0.0; + out_8221305666287256291[59] = 0.0; + out_8221305666287256291[60] = 1.0; + out_8221305666287256291[61] = 0.0; + out_8221305666287256291[62] = 0.0; + out_8221305666287256291[63] = 0.0; + out_8221305666287256291[64] = 0.0; + out_8221305666287256291[65] = 0.0; + out_8221305666287256291[66] = 0.0; + out_8221305666287256291[67] = 0.0; + out_8221305666287256291[68] = 0.0; + out_8221305666287256291[69] = 0.0; + out_8221305666287256291[70] = 1.0; + out_8221305666287256291[71] = 0.0; + out_8221305666287256291[72] = 0.0; + out_8221305666287256291[73] = 0.0; + out_8221305666287256291[74] = 0.0; + out_8221305666287256291[75] = 0.0; + out_8221305666287256291[76] = 0.0; + out_8221305666287256291[77] = 0.0; + out_8221305666287256291[78] = 0.0; + out_8221305666287256291[79] = 0.0; + out_8221305666287256291[80] = 1.0; } -void f_fun(double *state, double dt, double *out_6946480961019455904) { - out_6946480961019455904[0] = state[0]; - out_6946480961019455904[1] = state[1]; - out_6946480961019455904[2] = state[2]; - out_6946480961019455904[3] = state[3]; - out_6946480961019455904[4] = state[4]; - out_6946480961019455904[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_6946480961019455904[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_6946480961019455904[7] = state[7]; - out_6946480961019455904[8] = state[8]; +void f_fun(double *state, double dt, double *out_4355494038563606728) { + out_4355494038563606728[0] = state[0]; + out_4355494038563606728[1] = state[1]; + out_4355494038563606728[2] = state[2]; + out_4355494038563606728[3] = state[3]; + out_4355494038563606728[4] = state[4]; + out_4355494038563606728[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_4355494038563606728[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_4355494038563606728[7] = state[7]; + out_4355494038563606728[8] = state[8]; } -void F_fun(double *state, double dt, double *out_2425315094814185050) { - out_2425315094814185050[0] = 1; - out_2425315094814185050[1] = 0; - out_2425315094814185050[2] = 0; - out_2425315094814185050[3] = 0; - out_2425315094814185050[4] = 0; - out_2425315094814185050[5] = 0; - out_2425315094814185050[6] = 0; - out_2425315094814185050[7] = 0; - out_2425315094814185050[8] = 0; - out_2425315094814185050[9] = 0; - out_2425315094814185050[10] = 1; - out_2425315094814185050[11] = 0; - out_2425315094814185050[12] = 0; - out_2425315094814185050[13] = 0; - out_2425315094814185050[14] = 0; - out_2425315094814185050[15] = 0; - out_2425315094814185050[16] = 0; - out_2425315094814185050[17] = 0; - out_2425315094814185050[18] = 0; - out_2425315094814185050[19] = 0; - out_2425315094814185050[20] = 1; - out_2425315094814185050[21] = 0; - out_2425315094814185050[22] = 0; - out_2425315094814185050[23] = 0; - out_2425315094814185050[24] = 0; - out_2425315094814185050[25] = 0; - out_2425315094814185050[26] = 0; - out_2425315094814185050[27] = 0; - out_2425315094814185050[28] = 0; - out_2425315094814185050[29] = 0; - out_2425315094814185050[30] = 1; - out_2425315094814185050[31] = 0; - out_2425315094814185050[32] = 0; - out_2425315094814185050[33] = 0; - out_2425315094814185050[34] = 0; - out_2425315094814185050[35] = 0; - out_2425315094814185050[36] = 0; - out_2425315094814185050[37] = 0; - out_2425315094814185050[38] = 0; - out_2425315094814185050[39] = 0; - out_2425315094814185050[40] = 1; - out_2425315094814185050[41] = 0; - out_2425315094814185050[42] = 0; - out_2425315094814185050[43] = 0; - out_2425315094814185050[44] = 0; - out_2425315094814185050[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_2425315094814185050[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_2425315094814185050[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_2425315094814185050[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_2425315094814185050[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_2425315094814185050[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_2425315094814185050[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_2425315094814185050[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_2425315094814185050[53] = -9.8100000000000005*dt; - out_2425315094814185050[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_2425315094814185050[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_2425315094814185050[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_2425315094814185050[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_2425315094814185050[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_2425315094814185050[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_2425315094814185050[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_2425315094814185050[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_2425315094814185050[62] = 0; - out_2425315094814185050[63] = 0; - out_2425315094814185050[64] = 0; - out_2425315094814185050[65] = 0; - out_2425315094814185050[66] = 0; - out_2425315094814185050[67] = 0; - out_2425315094814185050[68] = 0; - out_2425315094814185050[69] = 0; - out_2425315094814185050[70] = 1; - out_2425315094814185050[71] = 0; - out_2425315094814185050[72] = 0; - out_2425315094814185050[73] = 0; - out_2425315094814185050[74] = 0; - out_2425315094814185050[75] = 0; - out_2425315094814185050[76] = 0; - out_2425315094814185050[77] = 0; - out_2425315094814185050[78] = 0; - out_2425315094814185050[79] = 0; - out_2425315094814185050[80] = 1; +void F_fun(double *state, double dt, double *out_8878866990102607558) { + out_8878866990102607558[0] = 1; + out_8878866990102607558[1] = 0; + out_8878866990102607558[2] = 0; + out_8878866990102607558[3] = 0; + out_8878866990102607558[4] = 0; + out_8878866990102607558[5] = 0; + out_8878866990102607558[6] = 0; + out_8878866990102607558[7] = 0; + out_8878866990102607558[8] = 0; + out_8878866990102607558[9] = 0; + out_8878866990102607558[10] = 1; + out_8878866990102607558[11] = 0; + out_8878866990102607558[12] = 0; + out_8878866990102607558[13] = 0; + out_8878866990102607558[14] = 0; + out_8878866990102607558[15] = 0; + out_8878866990102607558[16] = 0; + out_8878866990102607558[17] = 0; + out_8878866990102607558[18] = 0; + out_8878866990102607558[19] = 0; + out_8878866990102607558[20] = 1; + out_8878866990102607558[21] = 0; + out_8878866990102607558[22] = 0; + out_8878866990102607558[23] = 0; + out_8878866990102607558[24] = 0; + out_8878866990102607558[25] = 0; + out_8878866990102607558[26] = 0; + out_8878866990102607558[27] = 0; + out_8878866990102607558[28] = 0; + out_8878866990102607558[29] = 0; + out_8878866990102607558[30] = 1; + out_8878866990102607558[31] = 0; + out_8878866990102607558[32] = 0; + out_8878866990102607558[33] = 0; + out_8878866990102607558[34] = 0; + out_8878866990102607558[35] = 0; + out_8878866990102607558[36] = 0; + out_8878866990102607558[37] = 0; + out_8878866990102607558[38] = 0; + out_8878866990102607558[39] = 0; + out_8878866990102607558[40] = 1; + out_8878866990102607558[41] = 0; + out_8878866990102607558[42] = 0; + out_8878866990102607558[43] = 0; + out_8878866990102607558[44] = 0; + out_8878866990102607558[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_8878866990102607558[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_8878866990102607558[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_8878866990102607558[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_8878866990102607558[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_8878866990102607558[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_8878866990102607558[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_8878866990102607558[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_8878866990102607558[53] = -9.8100000000000005*dt; + out_8878866990102607558[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_8878866990102607558[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_8878866990102607558[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_8878866990102607558[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_8878866990102607558[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_8878866990102607558[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_8878866990102607558[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_8878866990102607558[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_8878866990102607558[62] = 0; + out_8878866990102607558[63] = 0; + out_8878866990102607558[64] = 0; + out_8878866990102607558[65] = 0; + out_8878866990102607558[66] = 0; + out_8878866990102607558[67] = 0; + out_8878866990102607558[68] = 0; + out_8878866990102607558[69] = 0; + out_8878866990102607558[70] = 1; + out_8878866990102607558[71] = 0; + out_8878866990102607558[72] = 0; + out_8878866990102607558[73] = 0; + out_8878866990102607558[74] = 0; + out_8878866990102607558[75] = 0; + out_8878866990102607558[76] = 0; + out_8878866990102607558[77] = 0; + out_8878866990102607558[78] = 0; + out_8878866990102607558[79] = 0; + out_8878866990102607558[80] = 1; } -void h_25(double *state, double *unused, double *out_3083351420745802205) { - out_3083351420745802205[0] = state[6]; +void h_25(double *state, double *unused, double *out_7662070615539319427) { + out_7662070615539319427[0] = state[6]; } -void H_25(double *state, double *unused, double *out_1342134769348572192) { - out_1342134769348572192[0] = 0; - out_1342134769348572192[1] = 0; - out_1342134769348572192[2] = 0; - out_1342134769348572192[3] = 0; - out_1342134769348572192[4] = 0; - out_1342134769348572192[5] = 0; - out_1342134769348572192[6] = 1; - out_1342134769348572192[7] = 0; - out_1342134769348572192[8] = 0; +void H_25(double *state, double *unused, double *out_6916966556095284671) { + out_6916966556095284671[0] = 0; + out_6916966556095284671[1] = 0; + out_6916966556095284671[2] = 0; + out_6916966556095284671[3] = 0; + out_6916966556095284671[4] = 0; + out_6916966556095284671[5] = 0; + out_6916966556095284671[6] = 1; + out_6916966556095284671[7] = 0; + out_6916966556095284671[8] = 0; } -void h_24(double *state, double *unused, double *out_7968288456028556638) { - out_7968288456028556638[0] = state[4]; - out_7968288456028556638[1] = state[5]; +void h_24(double *state, double *unused, double *out_552573547376297234) { + out_552573547376297234[0] = state[4]; + out_552573547376297234[1] = state[5]; } -void H_24(double *state, double *unused, double *out_7917706575940090293) { - out_7917706575940090293[0] = 0; - out_7917706575940090293[1] = 0; - out_7917706575940090293[2] = 0; - out_7917706575940090293[3] = 0; - out_7917706575940090293[4] = 1; - out_7917706575940090293[5] = 0; - out_7917706575940090293[6] = 0; - out_7917706575940090293[7] = 0; - out_7917706575940090293[8] = 0; - out_7917706575940090293[9] = 0; - out_7917706575940090293[10] = 0; - out_7917706575940090293[11] = 0; - out_7917706575940090293[12] = 0; - out_7917706575940090293[13] = 0; - out_7917706575940090293[14] = 1; - out_7917706575940090293[15] = 0; - out_7917706575940090293[16] = 0; - out_7917706575940090293[17] = 0; +void H_24(double *state, double *unused, double *out_5718391886591489980) { + out_5718391886591489980[0] = 0; + out_5718391886591489980[1] = 0; + out_5718391886591489980[2] = 0; + out_5718391886591489980[3] = 0; + out_5718391886591489980[4] = 1; + out_5718391886591489980[5] = 0; + out_5718391886591489980[6] = 0; + out_5718391886591489980[7] = 0; + out_5718391886591489980[8] = 0; + out_5718391886591489980[9] = 0; + out_5718391886591489980[10] = 0; + out_5718391886591489980[11] = 0; + out_5718391886591489980[12] = 0; + out_5718391886591489980[13] = 0; + out_5718391886591489980[14] = 1; + out_5718391886591489980[15] = 0; + out_5718391886591489980[16] = 0; + out_5718391886591489980[17] = 0; } -void h_30(double *state, double *unused, double *out_8592557426613398475) { - out_8592557426613398475[0] = state[4]; +void h_30(double *state, double *unused, double *out_9064035073689776395) { + out_9064035073689776395[0] = state[4]; } -void H_30(double *state, double *unused, double *out_8258825110840188947) { - out_8258825110840188947[0] = 0; - out_8258825110840188947[1] = 0; - out_8258825110840188947[2] = 0; - out_8258825110840188947[3] = 0; - out_8258825110840188947[4] = 1; - out_8258825110840188947[5] = 0; - out_8258825110840188947[6] = 0; - out_8258825110840188947[7] = 0; - out_8258825110840188947[8] = 0; +void H_30(double *state, double *unused, double *out_2389270225967676473) { + out_2389270225967676473[0] = 0; + out_2389270225967676473[1] = 0; + out_2389270225967676473[2] = 0; + out_2389270225967676473[3] = 0; + out_2389270225967676473[4] = 1; + out_2389270225967676473[5] = 0; + out_2389270225967676473[6] = 0; + out_2389270225967676473[7] = 0; + out_2389270225967676473[8] = 0; } -void h_26(double *state, double *unused, double *out_3590884158952552830) { - out_3590884158952552830[0] = state[7]; +void h_26(double *state, double *unused, double *out_1162711888116392193) { + out_1162711888116392193[0] = state[7]; } -void H_26(double *state, double *unused, double *out_2399368549525484032) { - out_2399368549525484032[0] = 0; - out_2399368549525484032[1] = 0; - out_2399368549525484032[2] = 0; - out_2399368549525484032[3] = 0; - out_2399368549525484032[4] = 0; - out_2399368549525484032[5] = 0; - out_2399368549525484032[6] = 0; - out_2399368549525484032[7] = 1; - out_2399368549525484032[8] = 0; +void H_26(double *state, double *unused, double *out_3175463237221228447) { + out_3175463237221228447[0] = 0; + out_3175463237221228447[1] = 0; + out_3175463237221228447[2] = 0; + out_3175463237221228447[3] = 0; + out_3175463237221228447[4] = 0; + out_3175463237221228447[5] = 0; + out_3175463237221228447[6] = 0; + out_3175463237221228447[7] = 1; + out_3175463237221228447[8] = 0; } -void h_27(double *state, double *unused, double *out_5937191991694674237) { - out_5937191991694674237[0] = state[3]; +void h_27(double *state, double *unused, double *out_4113321920138957337) { + out_4113321920138957337[0] = state[3]; } -void H_27(double *state, double *unused, double *out_961967489595092789) { - out_961967489595092789[0] = 0; - out_961967489595092789[1] = 0; - out_961967489595092789[2] = 0; - out_961967489595092789[3] = 1; - out_961967489595092789[4] = 0; - out_961967489595092789[5] = 0; - out_961967489595092789[6] = 0; - out_961967489595092789[7] = 0; - out_961967489595092789[8] = 0; +void H_27(double *state, double *unused, double *out_4612864297151619690) { + out_4612864297151619690[0] = 0; + out_4612864297151619690[1] = 0; + out_4612864297151619690[2] = 0; + out_4612864297151619690[3] = 1; + out_4612864297151619690[4] = 0; + out_4612864297151619690[5] = 0; + out_4612864297151619690[6] = 0; + out_4612864297151619690[7] = 0; + out_4612864297151619690[8] = 0; } -void h_29(double *state, double *unused, double *out_5780005323343545092) { - out_5780005323343545092[0] = state[1]; +void h_29(double *state, double *unused, double *out_7215941399213971055) { + out_7215941399213971055[0] = state[1]; } -void H_29(double *state, double *unused, double *out_1723027166519724306) { - out_1723027166519724306[0] = 0; - out_1723027166519724306[1] = 1; - out_1723027166519724306[2] = 0; - out_1723027166519724306[3] = 0; - out_1723027166519724306[4] = 0; - out_1723027166519724306[5] = 0; - out_1723027166519724306[6] = 0; - out_1723027166519724306[7] = 0; - out_1723027166519724306[8] = 0; +void H_29(double *state, double *unused, double *out_2899501570282068657) { + out_2899501570282068657[0] = 0; + out_2899501570282068657[1] = 1; + out_2899501570282068657[2] = 0; + out_2899501570282068657[3] = 0; + out_2899501570282068657[4] = 0; + out_2899501570282068657[5] = 0; + out_2899501570282068657[6] = 0; + out_2899501570282068657[7] = 0; + out_2899501570282068657[8] = 0; } -void h_28(double *state, double *unused, double *out_974811994013381005) { - out_974811994013381005[0] = state[0]; +void h_28(double *state, double *unused, double *out_1529407950941999498) { + out_1529407950941999498[0] = state[0]; } -void H_28(double *state, double *unused, double *out_3686657438085050557) { - out_3686657438085050557[0] = 1; - out_3686657438085050557[1] = 0; - out_3686657438085050557[2] = 0; - out_3686657438085050557[3] = 0; - out_3686657438085050557[4] = 0; - out_3686657438085050557[5] = 0; - out_3686657438085050557[6] = 0; - out_3686657438085050557[7] = 0; - out_3686657438085050557[8] = 0; +void H_28(double *state, double *unused, double *out_2182897446787461917) { + out_2182897446787461917[0] = 1; + out_2182897446787461917[1] = 0; + out_2182897446787461917[2] = 0; + out_2182897446787461917[3] = 0; + out_2182897446787461917[4] = 0; + out_2182897446787461917[5] = 0; + out_2182897446787461917[6] = 0; + out_2182897446787461917[7] = 0; + out_2182897446787461917[8] = 0; } -void h_31(double *state, double *unused, double *out_2926164752394673060) { - out_2926164752394673060[0] = state[8]; +void h_31(double *state, double *unused, double *out_5470065409481246982) { + out_5470065409481246982[0] = state[8]; } -void H_31(double *state, double *unused, double *out_1372780731225532620) { - out_1372780731225532620[0] = 0; - out_1372780731225532620[1] = 0; - out_1372780731225532620[2] = 0; - out_1372780731225532620[3] = 0; - out_1372780731225532620[4] = 0; - out_1372780731225532620[5] = 0; - out_1372780731225532620[6] = 0; - out_1372780731225532620[7] = 0; - out_1372780731225532620[8] = 1; +void H_31(double *state, double *unused, double *out_6947612517972245099) { + out_6947612517972245099[0] = 0; + out_6947612517972245099[1] = 0; + out_6947612517972245099[2] = 0; + out_6947612517972245099[3] = 0; + out_6947612517972245099[4] = 0; + out_6947612517972245099[5] = 0; + out_6947612517972245099[6] = 0; + out_6947612517972245099[7] = 0; + out_6947612517972245099[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_1264104166079454940) { - err_fun(nom_x, delta_x, out_1264104166079454940); +void car_err_fun(double *nom_x, double *delta_x, double *out_1326346818289980211) { + err_fun(nom_x, delta_x, out_1326346818289980211); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_3182169158929336617) { - inv_err_fun(nom_x, true_x, out_3182169158929336617); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_7721308480372758585) { + inv_err_fun(nom_x, true_x, out_7721308480372758585); } -void car_H_mod_fun(double *state, double *out_3410660055109284948) { - H_mod_fun(state, out_3410660055109284948); +void car_H_mod_fun(double *state, double *out_8221305666287256291) { + H_mod_fun(state, out_8221305666287256291); } -void car_f_fun(double *state, double dt, double *out_6946480961019455904) { - f_fun(state, dt, out_6946480961019455904); +void car_f_fun(double *state, double dt, double *out_4355494038563606728) { + f_fun(state, dt, out_4355494038563606728); } -void car_F_fun(double *state, double dt, double *out_2425315094814185050) { - F_fun(state, dt, out_2425315094814185050); +void car_F_fun(double *state, double dt, double *out_8878866990102607558) { + F_fun(state, dt, out_8878866990102607558); } -void car_h_25(double *state, double *unused, double *out_3083351420745802205) { - h_25(state, unused, out_3083351420745802205); +void car_h_25(double *state, double *unused, double *out_7662070615539319427) { + h_25(state, unused, out_7662070615539319427); } -void car_H_25(double *state, double *unused, double *out_1342134769348572192) { - H_25(state, unused, out_1342134769348572192); +void car_H_25(double *state, double *unused, double *out_6916966556095284671) { + H_25(state, unused, out_6916966556095284671); } -void car_h_24(double *state, double *unused, double *out_7968288456028556638) { - h_24(state, unused, out_7968288456028556638); +void car_h_24(double *state, double *unused, double *out_552573547376297234) { + h_24(state, unused, out_552573547376297234); } -void car_H_24(double *state, double *unused, double *out_7917706575940090293) { - H_24(state, unused, out_7917706575940090293); +void car_H_24(double *state, double *unused, double *out_5718391886591489980) { + H_24(state, unused, out_5718391886591489980); } -void car_h_30(double *state, double *unused, double *out_8592557426613398475) { - h_30(state, unused, out_8592557426613398475); +void car_h_30(double *state, double *unused, double *out_9064035073689776395) { + h_30(state, unused, out_9064035073689776395); } -void car_H_30(double *state, double *unused, double *out_8258825110840188947) { - H_30(state, unused, out_8258825110840188947); +void car_H_30(double *state, double *unused, double *out_2389270225967676473) { + H_30(state, unused, out_2389270225967676473); } -void car_h_26(double *state, double *unused, double *out_3590884158952552830) { - h_26(state, unused, out_3590884158952552830); +void car_h_26(double *state, double *unused, double *out_1162711888116392193) { + h_26(state, unused, out_1162711888116392193); } -void car_H_26(double *state, double *unused, double *out_2399368549525484032) { - H_26(state, unused, out_2399368549525484032); +void car_H_26(double *state, double *unused, double *out_3175463237221228447) { + H_26(state, unused, out_3175463237221228447); } -void car_h_27(double *state, double *unused, double *out_5937191991694674237) { - h_27(state, unused, out_5937191991694674237); +void car_h_27(double *state, double *unused, double *out_4113321920138957337) { + h_27(state, unused, out_4113321920138957337); } -void car_H_27(double *state, double *unused, double *out_961967489595092789) { - H_27(state, unused, out_961967489595092789); +void car_H_27(double *state, double *unused, double *out_4612864297151619690) { + H_27(state, unused, out_4612864297151619690); } -void car_h_29(double *state, double *unused, double *out_5780005323343545092) { - h_29(state, unused, out_5780005323343545092); +void car_h_29(double *state, double *unused, double *out_7215941399213971055) { + h_29(state, unused, out_7215941399213971055); } -void car_H_29(double *state, double *unused, double *out_1723027166519724306) { - H_29(state, unused, out_1723027166519724306); +void car_H_29(double *state, double *unused, double *out_2899501570282068657) { + H_29(state, unused, out_2899501570282068657); } -void car_h_28(double *state, double *unused, double *out_974811994013381005) { - h_28(state, unused, out_974811994013381005); +void car_h_28(double *state, double *unused, double *out_1529407950941999498) { + h_28(state, unused, out_1529407950941999498); } -void car_H_28(double *state, double *unused, double *out_3686657438085050557) { - H_28(state, unused, out_3686657438085050557); +void car_H_28(double *state, double *unused, double *out_2182897446787461917) { + H_28(state, unused, out_2182897446787461917); } -void car_h_31(double *state, double *unused, double *out_2926164752394673060) { - h_31(state, unused, out_2926164752394673060); +void car_h_31(double *state, double *unused, double *out_5470065409481246982) { + h_31(state, unused, out_5470065409481246982); } -void car_H_31(double *state, double *unused, double *out_1372780731225532620) { - H_31(state, unused, out_1372780731225532620); +void car_H_31(double *state, double *unused, double *out_6947612517972245099) { + H_31(state, unused, out_6947612517972245099); } 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 da84444f67..07655929ea 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_1264104166079454940); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_3182169158929336617); -void car_H_mod_fun(double *state, double *out_3410660055109284948); -void car_f_fun(double *state, double dt, double *out_6946480961019455904); -void car_F_fun(double *state, double dt, double *out_2425315094814185050); -void car_h_25(double *state, double *unused, double *out_3083351420745802205); -void car_H_25(double *state, double *unused, double *out_1342134769348572192); -void car_h_24(double *state, double *unused, double *out_7968288456028556638); -void car_H_24(double *state, double *unused, double *out_7917706575940090293); -void car_h_30(double *state, double *unused, double *out_8592557426613398475); -void car_H_30(double *state, double *unused, double *out_8258825110840188947); -void car_h_26(double *state, double *unused, double *out_3590884158952552830); -void car_H_26(double *state, double *unused, double *out_2399368549525484032); -void car_h_27(double *state, double *unused, double *out_5937191991694674237); -void car_H_27(double *state, double *unused, double *out_961967489595092789); -void car_h_29(double *state, double *unused, double *out_5780005323343545092); -void car_H_29(double *state, double *unused, double *out_1723027166519724306); -void car_h_28(double *state, double *unused, double *out_974811994013381005); -void car_H_28(double *state, double *unused, double *out_3686657438085050557); -void car_h_31(double *state, double *unused, double *out_2926164752394673060); -void car_H_31(double *state, double *unused, double *out_1372780731225532620); +void car_err_fun(double *nom_x, double *delta_x, double *out_1326346818289980211); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_7721308480372758585); +void car_H_mod_fun(double *state, double *out_8221305666287256291); +void car_f_fun(double *state, double dt, double *out_4355494038563606728); +void car_F_fun(double *state, double dt, double *out_8878866990102607558); +void car_h_25(double *state, double *unused, double *out_7662070615539319427); +void car_H_25(double *state, double *unused, double *out_6916966556095284671); +void car_h_24(double *state, double *unused, double *out_552573547376297234); +void car_H_24(double *state, double *unused, double *out_5718391886591489980); +void car_h_30(double *state, double *unused, double *out_9064035073689776395); +void car_H_30(double *state, double *unused, double *out_2389270225967676473); +void car_h_26(double *state, double *unused, double *out_1162711888116392193); +void car_H_26(double *state, double *unused, double *out_3175463237221228447); +void car_h_27(double *state, double *unused, double *out_4113321920138957337); +void car_H_27(double *state, double *unused, double *out_4612864297151619690); +void car_h_29(double *state, double *unused, double *out_7215941399213971055); +void car_H_29(double *state, double *unused, double *out_2899501570282068657); +void car_h_28(double *state, double *unused, double *out_1529407950941999498); +void car_H_28(double *state, double *unused, double *out_2182897446787461917); +void car_h_31(double *state, double *unused, double *out_5470065409481246982); +void car_H_31(double *state, double *unused, double *out_6947612517972245099); void car_predict(double *in_x, double *in_P, double *in_Q, double dt); void car_set_mass(double x); void car_set_rotational_inertia(double x); diff --git a/selfdrive/locationd/models/generated/pose.cpp b/selfdrive/locationd/models/generated/pose.cpp index 760b2a37ed..0d814de584 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_908811316603708218) { - out_908811316603708218[0] = delta_x[0] + nom_x[0]; - out_908811316603708218[1] = delta_x[1] + nom_x[1]; - out_908811316603708218[2] = delta_x[2] + nom_x[2]; - out_908811316603708218[3] = delta_x[3] + nom_x[3]; - out_908811316603708218[4] = delta_x[4] + nom_x[4]; - out_908811316603708218[5] = delta_x[5] + nom_x[5]; - out_908811316603708218[6] = delta_x[6] + nom_x[6]; - out_908811316603708218[7] = delta_x[7] + nom_x[7]; - out_908811316603708218[8] = delta_x[8] + nom_x[8]; - out_908811316603708218[9] = delta_x[9] + nom_x[9]; - out_908811316603708218[10] = delta_x[10] + nom_x[10]; - out_908811316603708218[11] = delta_x[11] + nom_x[11]; - out_908811316603708218[12] = delta_x[12] + nom_x[12]; - out_908811316603708218[13] = delta_x[13] + nom_x[13]; - out_908811316603708218[14] = delta_x[14] + nom_x[14]; - out_908811316603708218[15] = delta_x[15] + nom_x[15]; - out_908811316603708218[16] = delta_x[16] + nom_x[16]; - out_908811316603708218[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_4497386892446686963) { + out_4497386892446686963[0] = delta_x[0] + nom_x[0]; + out_4497386892446686963[1] = delta_x[1] + nom_x[1]; + out_4497386892446686963[2] = delta_x[2] + nom_x[2]; + out_4497386892446686963[3] = delta_x[3] + nom_x[3]; + out_4497386892446686963[4] = delta_x[4] + nom_x[4]; + out_4497386892446686963[5] = delta_x[5] + nom_x[5]; + out_4497386892446686963[6] = delta_x[6] + nom_x[6]; + out_4497386892446686963[7] = delta_x[7] + nom_x[7]; + out_4497386892446686963[8] = delta_x[8] + nom_x[8]; + out_4497386892446686963[9] = delta_x[9] + nom_x[9]; + out_4497386892446686963[10] = delta_x[10] + nom_x[10]; + out_4497386892446686963[11] = delta_x[11] + nom_x[11]; + out_4497386892446686963[12] = delta_x[12] + nom_x[12]; + out_4497386892446686963[13] = delta_x[13] + nom_x[13]; + out_4497386892446686963[14] = delta_x[14] + nom_x[14]; + out_4497386892446686963[15] = delta_x[15] + nom_x[15]; + out_4497386892446686963[16] = delta_x[16] + nom_x[16]; + out_4497386892446686963[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_6097135032344727242) { - out_6097135032344727242[0] = -nom_x[0] + true_x[0]; - out_6097135032344727242[1] = -nom_x[1] + true_x[1]; - out_6097135032344727242[2] = -nom_x[2] + true_x[2]; - out_6097135032344727242[3] = -nom_x[3] + true_x[3]; - out_6097135032344727242[4] = -nom_x[4] + true_x[4]; - out_6097135032344727242[5] = -nom_x[5] + true_x[5]; - out_6097135032344727242[6] = -nom_x[6] + true_x[6]; - out_6097135032344727242[7] = -nom_x[7] + true_x[7]; - out_6097135032344727242[8] = -nom_x[8] + true_x[8]; - out_6097135032344727242[9] = -nom_x[9] + true_x[9]; - out_6097135032344727242[10] = -nom_x[10] + true_x[10]; - out_6097135032344727242[11] = -nom_x[11] + true_x[11]; - out_6097135032344727242[12] = -nom_x[12] + true_x[12]; - out_6097135032344727242[13] = -nom_x[13] + true_x[13]; - out_6097135032344727242[14] = -nom_x[14] + true_x[14]; - out_6097135032344727242[15] = -nom_x[15] + true_x[15]; - out_6097135032344727242[16] = -nom_x[16] + true_x[16]; - out_6097135032344727242[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_4020535164465364860) { + out_4020535164465364860[0] = -nom_x[0] + true_x[0]; + out_4020535164465364860[1] = -nom_x[1] + true_x[1]; + out_4020535164465364860[2] = -nom_x[2] + true_x[2]; + out_4020535164465364860[3] = -nom_x[3] + true_x[3]; + out_4020535164465364860[4] = -nom_x[4] + true_x[4]; + out_4020535164465364860[5] = -nom_x[5] + true_x[5]; + out_4020535164465364860[6] = -nom_x[6] + true_x[6]; + out_4020535164465364860[7] = -nom_x[7] + true_x[7]; + out_4020535164465364860[8] = -nom_x[8] + true_x[8]; + out_4020535164465364860[9] = -nom_x[9] + true_x[9]; + out_4020535164465364860[10] = -nom_x[10] + true_x[10]; + out_4020535164465364860[11] = -nom_x[11] + true_x[11]; + out_4020535164465364860[12] = -nom_x[12] + true_x[12]; + out_4020535164465364860[13] = -nom_x[13] + true_x[13]; + out_4020535164465364860[14] = -nom_x[14] + true_x[14]; + out_4020535164465364860[15] = -nom_x[15] + true_x[15]; + out_4020535164465364860[16] = -nom_x[16] + true_x[16]; + out_4020535164465364860[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_5840907614347766849) { - out_5840907614347766849[0] = 1.0; - out_5840907614347766849[1] = 0.0; - out_5840907614347766849[2] = 0.0; - out_5840907614347766849[3] = 0.0; - out_5840907614347766849[4] = 0.0; - out_5840907614347766849[5] = 0.0; - out_5840907614347766849[6] = 0.0; - out_5840907614347766849[7] = 0.0; - out_5840907614347766849[8] = 0.0; - out_5840907614347766849[9] = 0.0; - out_5840907614347766849[10] = 0.0; - out_5840907614347766849[11] = 0.0; - out_5840907614347766849[12] = 0.0; - out_5840907614347766849[13] = 0.0; - out_5840907614347766849[14] = 0.0; - out_5840907614347766849[15] = 0.0; - out_5840907614347766849[16] = 0.0; - out_5840907614347766849[17] = 0.0; - out_5840907614347766849[18] = 0.0; - out_5840907614347766849[19] = 1.0; - out_5840907614347766849[20] = 0.0; - out_5840907614347766849[21] = 0.0; - out_5840907614347766849[22] = 0.0; - out_5840907614347766849[23] = 0.0; - out_5840907614347766849[24] = 0.0; - out_5840907614347766849[25] = 0.0; - out_5840907614347766849[26] = 0.0; - out_5840907614347766849[27] = 0.0; - out_5840907614347766849[28] = 0.0; - out_5840907614347766849[29] = 0.0; - out_5840907614347766849[30] = 0.0; - out_5840907614347766849[31] = 0.0; - out_5840907614347766849[32] = 0.0; - out_5840907614347766849[33] = 0.0; - out_5840907614347766849[34] = 0.0; - out_5840907614347766849[35] = 0.0; - out_5840907614347766849[36] = 0.0; - out_5840907614347766849[37] = 0.0; - out_5840907614347766849[38] = 1.0; - out_5840907614347766849[39] = 0.0; - out_5840907614347766849[40] = 0.0; - out_5840907614347766849[41] = 0.0; - out_5840907614347766849[42] = 0.0; - out_5840907614347766849[43] = 0.0; - out_5840907614347766849[44] = 0.0; - out_5840907614347766849[45] = 0.0; - out_5840907614347766849[46] = 0.0; - out_5840907614347766849[47] = 0.0; - out_5840907614347766849[48] = 0.0; - out_5840907614347766849[49] = 0.0; - out_5840907614347766849[50] = 0.0; - out_5840907614347766849[51] = 0.0; - out_5840907614347766849[52] = 0.0; - out_5840907614347766849[53] = 0.0; - out_5840907614347766849[54] = 0.0; - out_5840907614347766849[55] = 0.0; - out_5840907614347766849[56] = 0.0; - out_5840907614347766849[57] = 1.0; - out_5840907614347766849[58] = 0.0; - out_5840907614347766849[59] = 0.0; - out_5840907614347766849[60] = 0.0; - out_5840907614347766849[61] = 0.0; - out_5840907614347766849[62] = 0.0; - out_5840907614347766849[63] = 0.0; - out_5840907614347766849[64] = 0.0; - out_5840907614347766849[65] = 0.0; - out_5840907614347766849[66] = 0.0; - out_5840907614347766849[67] = 0.0; - out_5840907614347766849[68] = 0.0; - out_5840907614347766849[69] = 0.0; - out_5840907614347766849[70] = 0.0; - out_5840907614347766849[71] = 0.0; - out_5840907614347766849[72] = 0.0; - out_5840907614347766849[73] = 0.0; - out_5840907614347766849[74] = 0.0; - out_5840907614347766849[75] = 0.0; - out_5840907614347766849[76] = 1.0; - out_5840907614347766849[77] = 0.0; - out_5840907614347766849[78] = 0.0; - out_5840907614347766849[79] = 0.0; - out_5840907614347766849[80] = 0.0; - out_5840907614347766849[81] = 0.0; - out_5840907614347766849[82] = 0.0; - out_5840907614347766849[83] = 0.0; - out_5840907614347766849[84] = 0.0; - out_5840907614347766849[85] = 0.0; - out_5840907614347766849[86] = 0.0; - out_5840907614347766849[87] = 0.0; - out_5840907614347766849[88] = 0.0; - out_5840907614347766849[89] = 0.0; - out_5840907614347766849[90] = 0.0; - out_5840907614347766849[91] = 0.0; - out_5840907614347766849[92] = 0.0; - out_5840907614347766849[93] = 0.0; - out_5840907614347766849[94] = 0.0; - out_5840907614347766849[95] = 1.0; - out_5840907614347766849[96] = 0.0; - out_5840907614347766849[97] = 0.0; - out_5840907614347766849[98] = 0.0; - out_5840907614347766849[99] = 0.0; - out_5840907614347766849[100] = 0.0; - out_5840907614347766849[101] = 0.0; - out_5840907614347766849[102] = 0.0; - out_5840907614347766849[103] = 0.0; - out_5840907614347766849[104] = 0.0; - out_5840907614347766849[105] = 0.0; - out_5840907614347766849[106] = 0.0; - out_5840907614347766849[107] = 0.0; - out_5840907614347766849[108] = 0.0; - out_5840907614347766849[109] = 0.0; - out_5840907614347766849[110] = 0.0; - out_5840907614347766849[111] = 0.0; - out_5840907614347766849[112] = 0.0; - out_5840907614347766849[113] = 0.0; - out_5840907614347766849[114] = 1.0; - out_5840907614347766849[115] = 0.0; - out_5840907614347766849[116] = 0.0; - out_5840907614347766849[117] = 0.0; - out_5840907614347766849[118] = 0.0; - out_5840907614347766849[119] = 0.0; - out_5840907614347766849[120] = 0.0; - out_5840907614347766849[121] = 0.0; - out_5840907614347766849[122] = 0.0; - out_5840907614347766849[123] = 0.0; - out_5840907614347766849[124] = 0.0; - out_5840907614347766849[125] = 0.0; - out_5840907614347766849[126] = 0.0; - out_5840907614347766849[127] = 0.0; - out_5840907614347766849[128] = 0.0; - out_5840907614347766849[129] = 0.0; - out_5840907614347766849[130] = 0.0; - out_5840907614347766849[131] = 0.0; - out_5840907614347766849[132] = 0.0; - out_5840907614347766849[133] = 1.0; - out_5840907614347766849[134] = 0.0; - out_5840907614347766849[135] = 0.0; - out_5840907614347766849[136] = 0.0; - out_5840907614347766849[137] = 0.0; - out_5840907614347766849[138] = 0.0; - out_5840907614347766849[139] = 0.0; - out_5840907614347766849[140] = 0.0; - out_5840907614347766849[141] = 0.0; - out_5840907614347766849[142] = 0.0; - out_5840907614347766849[143] = 0.0; - out_5840907614347766849[144] = 0.0; - out_5840907614347766849[145] = 0.0; - out_5840907614347766849[146] = 0.0; - out_5840907614347766849[147] = 0.0; - out_5840907614347766849[148] = 0.0; - out_5840907614347766849[149] = 0.0; - out_5840907614347766849[150] = 0.0; - out_5840907614347766849[151] = 0.0; - out_5840907614347766849[152] = 1.0; - out_5840907614347766849[153] = 0.0; - out_5840907614347766849[154] = 0.0; - out_5840907614347766849[155] = 0.0; - out_5840907614347766849[156] = 0.0; - out_5840907614347766849[157] = 0.0; - out_5840907614347766849[158] = 0.0; - out_5840907614347766849[159] = 0.0; - out_5840907614347766849[160] = 0.0; - out_5840907614347766849[161] = 0.0; - out_5840907614347766849[162] = 0.0; - out_5840907614347766849[163] = 0.0; - out_5840907614347766849[164] = 0.0; - out_5840907614347766849[165] = 0.0; - out_5840907614347766849[166] = 0.0; - out_5840907614347766849[167] = 0.0; - out_5840907614347766849[168] = 0.0; - out_5840907614347766849[169] = 0.0; - out_5840907614347766849[170] = 0.0; - out_5840907614347766849[171] = 1.0; - out_5840907614347766849[172] = 0.0; - out_5840907614347766849[173] = 0.0; - out_5840907614347766849[174] = 0.0; - out_5840907614347766849[175] = 0.0; - out_5840907614347766849[176] = 0.0; - out_5840907614347766849[177] = 0.0; - out_5840907614347766849[178] = 0.0; - out_5840907614347766849[179] = 0.0; - out_5840907614347766849[180] = 0.0; - out_5840907614347766849[181] = 0.0; - out_5840907614347766849[182] = 0.0; - out_5840907614347766849[183] = 0.0; - out_5840907614347766849[184] = 0.0; - out_5840907614347766849[185] = 0.0; - out_5840907614347766849[186] = 0.0; - out_5840907614347766849[187] = 0.0; - out_5840907614347766849[188] = 0.0; - out_5840907614347766849[189] = 0.0; - out_5840907614347766849[190] = 1.0; - out_5840907614347766849[191] = 0.0; - out_5840907614347766849[192] = 0.0; - out_5840907614347766849[193] = 0.0; - out_5840907614347766849[194] = 0.0; - out_5840907614347766849[195] = 0.0; - out_5840907614347766849[196] = 0.0; - out_5840907614347766849[197] = 0.0; - out_5840907614347766849[198] = 0.0; - out_5840907614347766849[199] = 0.0; - out_5840907614347766849[200] = 0.0; - out_5840907614347766849[201] = 0.0; - out_5840907614347766849[202] = 0.0; - out_5840907614347766849[203] = 0.0; - out_5840907614347766849[204] = 0.0; - out_5840907614347766849[205] = 0.0; - out_5840907614347766849[206] = 0.0; - out_5840907614347766849[207] = 0.0; - out_5840907614347766849[208] = 0.0; - out_5840907614347766849[209] = 1.0; - out_5840907614347766849[210] = 0.0; - out_5840907614347766849[211] = 0.0; - out_5840907614347766849[212] = 0.0; - out_5840907614347766849[213] = 0.0; - out_5840907614347766849[214] = 0.0; - out_5840907614347766849[215] = 0.0; - out_5840907614347766849[216] = 0.0; - out_5840907614347766849[217] = 0.0; - out_5840907614347766849[218] = 0.0; - out_5840907614347766849[219] = 0.0; - out_5840907614347766849[220] = 0.0; - out_5840907614347766849[221] = 0.0; - out_5840907614347766849[222] = 0.0; - out_5840907614347766849[223] = 0.0; - out_5840907614347766849[224] = 0.0; - out_5840907614347766849[225] = 0.0; - out_5840907614347766849[226] = 0.0; - out_5840907614347766849[227] = 0.0; - out_5840907614347766849[228] = 1.0; - out_5840907614347766849[229] = 0.0; - out_5840907614347766849[230] = 0.0; - out_5840907614347766849[231] = 0.0; - out_5840907614347766849[232] = 0.0; - out_5840907614347766849[233] = 0.0; - out_5840907614347766849[234] = 0.0; - out_5840907614347766849[235] = 0.0; - out_5840907614347766849[236] = 0.0; - out_5840907614347766849[237] = 0.0; - out_5840907614347766849[238] = 0.0; - out_5840907614347766849[239] = 0.0; - out_5840907614347766849[240] = 0.0; - out_5840907614347766849[241] = 0.0; - out_5840907614347766849[242] = 0.0; - out_5840907614347766849[243] = 0.0; - out_5840907614347766849[244] = 0.0; - out_5840907614347766849[245] = 0.0; - out_5840907614347766849[246] = 0.0; - out_5840907614347766849[247] = 1.0; - out_5840907614347766849[248] = 0.0; - out_5840907614347766849[249] = 0.0; - out_5840907614347766849[250] = 0.0; - out_5840907614347766849[251] = 0.0; - out_5840907614347766849[252] = 0.0; - out_5840907614347766849[253] = 0.0; - out_5840907614347766849[254] = 0.0; - out_5840907614347766849[255] = 0.0; - out_5840907614347766849[256] = 0.0; - out_5840907614347766849[257] = 0.0; - out_5840907614347766849[258] = 0.0; - out_5840907614347766849[259] = 0.0; - out_5840907614347766849[260] = 0.0; - out_5840907614347766849[261] = 0.0; - out_5840907614347766849[262] = 0.0; - out_5840907614347766849[263] = 0.0; - out_5840907614347766849[264] = 0.0; - out_5840907614347766849[265] = 0.0; - out_5840907614347766849[266] = 1.0; - out_5840907614347766849[267] = 0.0; - out_5840907614347766849[268] = 0.0; - out_5840907614347766849[269] = 0.0; - out_5840907614347766849[270] = 0.0; - out_5840907614347766849[271] = 0.0; - out_5840907614347766849[272] = 0.0; - out_5840907614347766849[273] = 0.0; - out_5840907614347766849[274] = 0.0; - out_5840907614347766849[275] = 0.0; - out_5840907614347766849[276] = 0.0; - out_5840907614347766849[277] = 0.0; - out_5840907614347766849[278] = 0.0; - out_5840907614347766849[279] = 0.0; - out_5840907614347766849[280] = 0.0; - out_5840907614347766849[281] = 0.0; - out_5840907614347766849[282] = 0.0; - out_5840907614347766849[283] = 0.0; - out_5840907614347766849[284] = 0.0; - out_5840907614347766849[285] = 1.0; - out_5840907614347766849[286] = 0.0; - out_5840907614347766849[287] = 0.0; - out_5840907614347766849[288] = 0.0; - out_5840907614347766849[289] = 0.0; - out_5840907614347766849[290] = 0.0; - out_5840907614347766849[291] = 0.0; - out_5840907614347766849[292] = 0.0; - out_5840907614347766849[293] = 0.0; - out_5840907614347766849[294] = 0.0; - out_5840907614347766849[295] = 0.0; - out_5840907614347766849[296] = 0.0; - out_5840907614347766849[297] = 0.0; - out_5840907614347766849[298] = 0.0; - out_5840907614347766849[299] = 0.0; - out_5840907614347766849[300] = 0.0; - out_5840907614347766849[301] = 0.0; - out_5840907614347766849[302] = 0.0; - out_5840907614347766849[303] = 0.0; - out_5840907614347766849[304] = 1.0; - out_5840907614347766849[305] = 0.0; - out_5840907614347766849[306] = 0.0; - out_5840907614347766849[307] = 0.0; - out_5840907614347766849[308] = 0.0; - out_5840907614347766849[309] = 0.0; - out_5840907614347766849[310] = 0.0; - out_5840907614347766849[311] = 0.0; - out_5840907614347766849[312] = 0.0; - out_5840907614347766849[313] = 0.0; - out_5840907614347766849[314] = 0.0; - out_5840907614347766849[315] = 0.0; - out_5840907614347766849[316] = 0.0; - out_5840907614347766849[317] = 0.0; - out_5840907614347766849[318] = 0.0; - out_5840907614347766849[319] = 0.0; - out_5840907614347766849[320] = 0.0; - out_5840907614347766849[321] = 0.0; - out_5840907614347766849[322] = 0.0; - out_5840907614347766849[323] = 1.0; +void H_mod_fun(double *state, double *out_163641472803010022) { + out_163641472803010022[0] = 1.0; + out_163641472803010022[1] = 0.0; + out_163641472803010022[2] = 0.0; + out_163641472803010022[3] = 0.0; + out_163641472803010022[4] = 0.0; + out_163641472803010022[5] = 0.0; + out_163641472803010022[6] = 0.0; + out_163641472803010022[7] = 0.0; + out_163641472803010022[8] = 0.0; + out_163641472803010022[9] = 0.0; + out_163641472803010022[10] = 0.0; + out_163641472803010022[11] = 0.0; + out_163641472803010022[12] = 0.0; + out_163641472803010022[13] = 0.0; + out_163641472803010022[14] = 0.0; + out_163641472803010022[15] = 0.0; + out_163641472803010022[16] = 0.0; + out_163641472803010022[17] = 0.0; + out_163641472803010022[18] = 0.0; + out_163641472803010022[19] = 1.0; + out_163641472803010022[20] = 0.0; + out_163641472803010022[21] = 0.0; + out_163641472803010022[22] = 0.0; + out_163641472803010022[23] = 0.0; + out_163641472803010022[24] = 0.0; + out_163641472803010022[25] = 0.0; + out_163641472803010022[26] = 0.0; + out_163641472803010022[27] = 0.0; + out_163641472803010022[28] = 0.0; + out_163641472803010022[29] = 0.0; + out_163641472803010022[30] = 0.0; + out_163641472803010022[31] = 0.0; + out_163641472803010022[32] = 0.0; + out_163641472803010022[33] = 0.0; + out_163641472803010022[34] = 0.0; + out_163641472803010022[35] = 0.0; + out_163641472803010022[36] = 0.0; + out_163641472803010022[37] = 0.0; + out_163641472803010022[38] = 1.0; + out_163641472803010022[39] = 0.0; + out_163641472803010022[40] = 0.0; + out_163641472803010022[41] = 0.0; + out_163641472803010022[42] = 0.0; + out_163641472803010022[43] = 0.0; + out_163641472803010022[44] = 0.0; + out_163641472803010022[45] = 0.0; + out_163641472803010022[46] = 0.0; + out_163641472803010022[47] = 0.0; + out_163641472803010022[48] = 0.0; + out_163641472803010022[49] = 0.0; + out_163641472803010022[50] = 0.0; + out_163641472803010022[51] = 0.0; + out_163641472803010022[52] = 0.0; + out_163641472803010022[53] = 0.0; + out_163641472803010022[54] = 0.0; + out_163641472803010022[55] = 0.0; + out_163641472803010022[56] = 0.0; + out_163641472803010022[57] = 1.0; + out_163641472803010022[58] = 0.0; + out_163641472803010022[59] = 0.0; + out_163641472803010022[60] = 0.0; + out_163641472803010022[61] = 0.0; + out_163641472803010022[62] = 0.0; + out_163641472803010022[63] = 0.0; + out_163641472803010022[64] = 0.0; + out_163641472803010022[65] = 0.0; + out_163641472803010022[66] = 0.0; + out_163641472803010022[67] = 0.0; + out_163641472803010022[68] = 0.0; + out_163641472803010022[69] = 0.0; + out_163641472803010022[70] = 0.0; + out_163641472803010022[71] = 0.0; + out_163641472803010022[72] = 0.0; + out_163641472803010022[73] = 0.0; + out_163641472803010022[74] = 0.0; + out_163641472803010022[75] = 0.0; + out_163641472803010022[76] = 1.0; + out_163641472803010022[77] = 0.0; + out_163641472803010022[78] = 0.0; + out_163641472803010022[79] = 0.0; + out_163641472803010022[80] = 0.0; + out_163641472803010022[81] = 0.0; + out_163641472803010022[82] = 0.0; + out_163641472803010022[83] = 0.0; + out_163641472803010022[84] = 0.0; + out_163641472803010022[85] = 0.0; + out_163641472803010022[86] = 0.0; + out_163641472803010022[87] = 0.0; + out_163641472803010022[88] = 0.0; + out_163641472803010022[89] = 0.0; + out_163641472803010022[90] = 0.0; + out_163641472803010022[91] = 0.0; + out_163641472803010022[92] = 0.0; + out_163641472803010022[93] = 0.0; + out_163641472803010022[94] = 0.0; + out_163641472803010022[95] = 1.0; + out_163641472803010022[96] = 0.0; + out_163641472803010022[97] = 0.0; + out_163641472803010022[98] = 0.0; + out_163641472803010022[99] = 0.0; + out_163641472803010022[100] = 0.0; + out_163641472803010022[101] = 0.0; + out_163641472803010022[102] = 0.0; + out_163641472803010022[103] = 0.0; + out_163641472803010022[104] = 0.0; + out_163641472803010022[105] = 0.0; + out_163641472803010022[106] = 0.0; + out_163641472803010022[107] = 0.0; + out_163641472803010022[108] = 0.0; + out_163641472803010022[109] = 0.0; + out_163641472803010022[110] = 0.0; + out_163641472803010022[111] = 0.0; + out_163641472803010022[112] = 0.0; + out_163641472803010022[113] = 0.0; + out_163641472803010022[114] = 1.0; + out_163641472803010022[115] = 0.0; + out_163641472803010022[116] = 0.0; + out_163641472803010022[117] = 0.0; + out_163641472803010022[118] = 0.0; + out_163641472803010022[119] = 0.0; + out_163641472803010022[120] = 0.0; + out_163641472803010022[121] = 0.0; + out_163641472803010022[122] = 0.0; + out_163641472803010022[123] = 0.0; + out_163641472803010022[124] = 0.0; + out_163641472803010022[125] = 0.0; + out_163641472803010022[126] = 0.0; + out_163641472803010022[127] = 0.0; + out_163641472803010022[128] = 0.0; + out_163641472803010022[129] = 0.0; + out_163641472803010022[130] = 0.0; + out_163641472803010022[131] = 0.0; + out_163641472803010022[132] = 0.0; + out_163641472803010022[133] = 1.0; + out_163641472803010022[134] = 0.0; + out_163641472803010022[135] = 0.0; + out_163641472803010022[136] = 0.0; + out_163641472803010022[137] = 0.0; + out_163641472803010022[138] = 0.0; + out_163641472803010022[139] = 0.0; + out_163641472803010022[140] = 0.0; + out_163641472803010022[141] = 0.0; + out_163641472803010022[142] = 0.0; + out_163641472803010022[143] = 0.0; + out_163641472803010022[144] = 0.0; + out_163641472803010022[145] = 0.0; + out_163641472803010022[146] = 0.0; + out_163641472803010022[147] = 0.0; + out_163641472803010022[148] = 0.0; + out_163641472803010022[149] = 0.0; + out_163641472803010022[150] = 0.0; + out_163641472803010022[151] = 0.0; + out_163641472803010022[152] = 1.0; + out_163641472803010022[153] = 0.0; + out_163641472803010022[154] = 0.0; + out_163641472803010022[155] = 0.0; + out_163641472803010022[156] = 0.0; + out_163641472803010022[157] = 0.0; + out_163641472803010022[158] = 0.0; + out_163641472803010022[159] = 0.0; + out_163641472803010022[160] = 0.0; + out_163641472803010022[161] = 0.0; + out_163641472803010022[162] = 0.0; + out_163641472803010022[163] = 0.0; + out_163641472803010022[164] = 0.0; + out_163641472803010022[165] = 0.0; + out_163641472803010022[166] = 0.0; + out_163641472803010022[167] = 0.0; + out_163641472803010022[168] = 0.0; + out_163641472803010022[169] = 0.0; + out_163641472803010022[170] = 0.0; + out_163641472803010022[171] = 1.0; + out_163641472803010022[172] = 0.0; + out_163641472803010022[173] = 0.0; + out_163641472803010022[174] = 0.0; + out_163641472803010022[175] = 0.0; + out_163641472803010022[176] = 0.0; + out_163641472803010022[177] = 0.0; + out_163641472803010022[178] = 0.0; + out_163641472803010022[179] = 0.0; + out_163641472803010022[180] = 0.0; + out_163641472803010022[181] = 0.0; + out_163641472803010022[182] = 0.0; + out_163641472803010022[183] = 0.0; + out_163641472803010022[184] = 0.0; + out_163641472803010022[185] = 0.0; + out_163641472803010022[186] = 0.0; + out_163641472803010022[187] = 0.0; + out_163641472803010022[188] = 0.0; + out_163641472803010022[189] = 0.0; + out_163641472803010022[190] = 1.0; + out_163641472803010022[191] = 0.0; + out_163641472803010022[192] = 0.0; + out_163641472803010022[193] = 0.0; + out_163641472803010022[194] = 0.0; + out_163641472803010022[195] = 0.0; + out_163641472803010022[196] = 0.0; + out_163641472803010022[197] = 0.0; + out_163641472803010022[198] = 0.0; + out_163641472803010022[199] = 0.0; + out_163641472803010022[200] = 0.0; + out_163641472803010022[201] = 0.0; + out_163641472803010022[202] = 0.0; + out_163641472803010022[203] = 0.0; + out_163641472803010022[204] = 0.0; + out_163641472803010022[205] = 0.0; + out_163641472803010022[206] = 0.0; + out_163641472803010022[207] = 0.0; + out_163641472803010022[208] = 0.0; + out_163641472803010022[209] = 1.0; + out_163641472803010022[210] = 0.0; + out_163641472803010022[211] = 0.0; + out_163641472803010022[212] = 0.0; + out_163641472803010022[213] = 0.0; + out_163641472803010022[214] = 0.0; + out_163641472803010022[215] = 0.0; + out_163641472803010022[216] = 0.0; + out_163641472803010022[217] = 0.0; + out_163641472803010022[218] = 0.0; + out_163641472803010022[219] = 0.0; + out_163641472803010022[220] = 0.0; + out_163641472803010022[221] = 0.0; + out_163641472803010022[222] = 0.0; + out_163641472803010022[223] = 0.0; + out_163641472803010022[224] = 0.0; + out_163641472803010022[225] = 0.0; + out_163641472803010022[226] = 0.0; + out_163641472803010022[227] = 0.0; + out_163641472803010022[228] = 1.0; + out_163641472803010022[229] = 0.0; + out_163641472803010022[230] = 0.0; + out_163641472803010022[231] = 0.0; + out_163641472803010022[232] = 0.0; + out_163641472803010022[233] = 0.0; + out_163641472803010022[234] = 0.0; + out_163641472803010022[235] = 0.0; + out_163641472803010022[236] = 0.0; + out_163641472803010022[237] = 0.0; + out_163641472803010022[238] = 0.0; + out_163641472803010022[239] = 0.0; + out_163641472803010022[240] = 0.0; + out_163641472803010022[241] = 0.0; + out_163641472803010022[242] = 0.0; + out_163641472803010022[243] = 0.0; + out_163641472803010022[244] = 0.0; + out_163641472803010022[245] = 0.0; + out_163641472803010022[246] = 0.0; + out_163641472803010022[247] = 1.0; + out_163641472803010022[248] = 0.0; + out_163641472803010022[249] = 0.0; + out_163641472803010022[250] = 0.0; + out_163641472803010022[251] = 0.0; + out_163641472803010022[252] = 0.0; + out_163641472803010022[253] = 0.0; + out_163641472803010022[254] = 0.0; + out_163641472803010022[255] = 0.0; + out_163641472803010022[256] = 0.0; + out_163641472803010022[257] = 0.0; + out_163641472803010022[258] = 0.0; + out_163641472803010022[259] = 0.0; + out_163641472803010022[260] = 0.0; + out_163641472803010022[261] = 0.0; + out_163641472803010022[262] = 0.0; + out_163641472803010022[263] = 0.0; + out_163641472803010022[264] = 0.0; + out_163641472803010022[265] = 0.0; + out_163641472803010022[266] = 1.0; + out_163641472803010022[267] = 0.0; + out_163641472803010022[268] = 0.0; + out_163641472803010022[269] = 0.0; + out_163641472803010022[270] = 0.0; + out_163641472803010022[271] = 0.0; + out_163641472803010022[272] = 0.0; + out_163641472803010022[273] = 0.0; + out_163641472803010022[274] = 0.0; + out_163641472803010022[275] = 0.0; + out_163641472803010022[276] = 0.0; + out_163641472803010022[277] = 0.0; + out_163641472803010022[278] = 0.0; + out_163641472803010022[279] = 0.0; + out_163641472803010022[280] = 0.0; + out_163641472803010022[281] = 0.0; + out_163641472803010022[282] = 0.0; + out_163641472803010022[283] = 0.0; + out_163641472803010022[284] = 0.0; + out_163641472803010022[285] = 1.0; + out_163641472803010022[286] = 0.0; + out_163641472803010022[287] = 0.0; + out_163641472803010022[288] = 0.0; + out_163641472803010022[289] = 0.0; + out_163641472803010022[290] = 0.0; + out_163641472803010022[291] = 0.0; + out_163641472803010022[292] = 0.0; + out_163641472803010022[293] = 0.0; + out_163641472803010022[294] = 0.0; + out_163641472803010022[295] = 0.0; + out_163641472803010022[296] = 0.0; + out_163641472803010022[297] = 0.0; + out_163641472803010022[298] = 0.0; + out_163641472803010022[299] = 0.0; + out_163641472803010022[300] = 0.0; + out_163641472803010022[301] = 0.0; + out_163641472803010022[302] = 0.0; + out_163641472803010022[303] = 0.0; + out_163641472803010022[304] = 1.0; + out_163641472803010022[305] = 0.0; + out_163641472803010022[306] = 0.0; + out_163641472803010022[307] = 0.0; + out_163641472803010022[308] = 0.0; + out_163641472803010022[309] = 0.0; + out_163641472803010022[310] = 0.0; + out_163641472803010022[311] = 0.0; + out_163641472803010022[312] = 0.0; + out_163641472803010022[313] = 0.0; + out_163641472803010022[314] = 0.0; + out_163641472803010022[315] = 0.0; + out_163641472803010022[316] = 0.0; + out_163641472803010022[317] = 0.0; + out_163641472803010022[318] = 0.0; + out_163641472803010022[319] = 0.0; + out_163641472803010022[320] = 0.0; + out_163641472803010022[321] = 0.0; + out_163641472803010022[322] = 0.0; + out_163641472803010022[323] = 1.0; } -void f_fun(double *state, double dt, double *out_6742419823108127014) { - out_6742419823108127014[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_6742419823108127014[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_6742419823108127014[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_6742419823108127014[3] = dt*state[12] + state[3]; - out_6742419823108127014[4] = dt*state[13] + state[4]; - out_6742419823108127014[5] = dt*state[14] + state[5]; - out_6742419823108127014[6] = state[6]; - out_6742419823108127014[7] = state[7]; - out_6742419823108127014[8] = state[8]; - out_6742419823108127014[9] = state[9]; - out_6742419823108127014[10] = state[10]; - out_6742419823108127014[11] = state[11]; - out_6742419823108127014[12] = state[12]; - out_6742419823108127014[13] = state[13]; - out_6742419823108127014[14] = state[14]; - out_6742419823108127014[15] = state[15]; - out_6742419823108127014[16] = state[16]; - out_6742419823108127014[17] = state[17]; +void f_fun(double *state, double dt, double *out_7113476067971581844) { + out_7113476067971581844[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_7113476067971581844[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_7113476067971581844[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_7113476067971581844[3] = dt*state[12] + state[3]; + out_7113476067971581844[4] = dt*state[13] + state[4]; + out_7113476067971581844[5] = dt*state[14] + state[5]; + out_7113476067971581844[6] = state[6]; + out_7113476067971581844[7] = state[7]; + out_7113476067971581844[8] = state[8]; + out_7113476067971581844[9] = state[9]; + out_7113476067971581844[10] = state[10]; + out_7113476067971581844[11] = state[11]; + out_7113476067971581844[12] = state[12]; + out_7113476067971581844[13] = state[13]; + out_7113476067971581844[14] = state[14]; + out_7113476067971581844[15] = state[15]; + out_7113476067971581844[16] = state[16]; + out_7113476067971581844[17] = state[17]; } -void F_fun(double *state, double dt, double *out_7663122633241286867) { - out_7663122633241286867[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_7663122633241286867[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_7663122633241286867[2] = 0; - out_7663122633241286867[3] = 0; - out_7663122633241286867[4] = 0; - out_7663122633241286867[5] = 0; - out_7663122633241286867[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_7663122633241286867[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_7663122633241286867[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_7663122633241286867[9] = 0; - out_7663122633241286867[10] = 0; - out_7663122633241286867[11] = 0; - out_7663122633241286867[12] = 0; - out_7663122633241286867[13] = 0; - out_7663122633241286867[14] = 0; - out_7663122633241286867[15] = 0; - out_7663122633241286867[16] = 0; - out_7663122633241286867[17] = 0; - out_7663122633241286867[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_7663122633241286867[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_7663122633241286867[20] = 0; - out_7663122633241286867[21] = 0; - out_7663122633241286867[22] = 0; - out_7663122633241286867[23] = 0; - out_7663122633241286867[24] = 0; - out_7663122633241286867[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_7663122633241286867[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_7663122633241286867[27] = 0; - out_7663122633241286867[28] = 0; - out_7663122633241286867[29] = 0; - out_7663122633241286867[30] = 0; - out_7663122633241286867[31] = 0; - out_7663122633241286867[32] = 0; - out_7663122633241286867[33] = 0; - out_7663122633241286867[34] = 0; - out_7663122633241286867[35] = 0; - out_7663122633241286867[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_7663122633241286867[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_7663122633241286867[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_7663122633241286867[39] = 0; - out_7663122633241286867[40] = 0; - out_7663122633241286867[41] = 0; - out_7663122633241286867[42] = 0; - out_7663122633241286867[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_7663122633241286867[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_7663122633241286867[45] = 0; - out_7663122633241286867[46] = 0; - out_7663122633241286867[47] = 0; - out_7663122633241286867[48] = 0; - out_7663122633241286867[49] = 0; - out_7663122633241286867[50] = 0; - out_7663122633241286867[51] = 0; - out_7663122633241286867[52] = 0; - out_7663122633241286867[53] = 0; - out_7663122633241286867[54] = 0; - out_7663122633241286867[55] = 0; - out_7663122633241286867[56] = 0; - out_7663122633241286867[57] = 1; - out_7663122633241286867[58] = 0; - out_7663122633241286867[59] = 0; - out_7663122633241286867[60] = 0; - out_7663122633241286867[61] = 0; - out_7663122633241286867[62] = 0; - out_7663122633241286867[63] = 0; - out_7663122633241286867[64] = 0; - out_7663122633241286867[65] = 0; - out_7663122633241286867[66] = dt; - out_7663122633241286867[67] = 0; - out_7663122633241286867[68] = 0; - out_7663122633241286867[69] = 0; - out_7663122633241286867[70] = 0; - out_7663122633241286867[71] = 0; - out_7663122633241286867[72] = 0; - out_7663122633241286867[73] = 0; - out_7663122633241286867[74] = 0; - out_7663122633241286867[75] = 0; - out_7663122633241286867[76] = 1; - out_7663122633241286867[77] = 0; - out_7663122633241286867[78] = 0; - out_7663122633241286867[79] = 0; - out_7663122633241286867[80] = 0; - out_7663122633241286867[81] = 0; - out_7663122633241286867[82] = 0; - out_7663122633241286867[83] = 0; - out_7663122633241286867[84] = 0; - out_7663122633241286867[85] = dt; - out_7663122633241286867[86] = 0; - out_7663122633241286867[87] = 0; - out_7663122633241286867[88] = 0; - out_7663122633241286867[89] = 0; - out_7663122633241286867[90] = 0; - out_7663122633241286867[91] = 0; - out_7663122633241286867[92] = 0; - out_7663122633241286867[93] = 0; - out_7663122633241286867[94] = 0; - out_7663122633241286867[95] = 1; - out_7663122633241286867[96] = 0; - out_7663122633241286867[97] = 0; - out_7663122633241286867[98] = 0; - out_7663122633241286867[99] = 0; - out_7663122633241286867[100] = 0; - out_7663122633241286867[101] = 0; - out_7663122633241286867[102] = 0; - out_7663122633241286867[103] = 0; - out_7663122633241286867[104] = dt; - out_7663122633241286867[105] = 0; - out_7663122633241286867[106] = 0; - out_7663122633241286867[107] = 0; - out_7663122633241286867[108] = 0; - out_7663122633241286867[109] = 0; - out_7663122633241286867[110] = 0; - out_7663122633241286867[111] = 0; - out_7663122633241286867[112] = 0; - out_7663122633241286867[113] = 0; - out_7663122633241286867[114] = 1; - out_7663122633241286867[115] = 0; - out_7663122633241286867[116] = 0; - out_7663122633241286867[117] = 0; - out_7663122633241286867[118] = 0; - out_7663122633241286867[119] = 0; - out_7663122633241286867[120] = 0; - out_7663122633241286867[121] = 0; - out_7663122633241286867[122] = 0; - out_7663122633241286867[123] = 0; - out_7663122633241286867[124] = 0; - out_7663122633241286867[125] = 0; - out_7663122633241286867[126] = 0; - out_7663122633241286867[127] = 0; - out_7663122633241286867[128] = 0; - out_7663122633241286867[129] = 0; - out_7663122633241286867[130] = 0; - out_7663122633241286867[131] = 0; - out_7663122633241286867[132] = 0; - out_7663122633241286867[133] = 1; - out_7663122633241286867[134] = 0; - out_7663122633241286867[135] = 0; - out_7663122633241286867[136] = 0; - out_7663122633241286867[137] = 0; - out_7663122633241286867[138] = 0; - out_7663122633241286867[139] = 0; - out_7663122633241286867[140] = 0; - out_7663122633241286867[141] = 0; - out_7663122633241286867[142] = 0; - out_7663122633241286867[143] = 0; - out_7663122633241286867[144] = 0; - out_7663122633241286867[145] = 0; - out_7663122633241286867[146] = 0; - out_7663122633241286867[147] = 0; - out_7663122633241286867[148] = 0; - out_7663122633241286867[149] = 0; - out_7663122633241286867[150] = 0; - out_7663122633241286867[151] = 0; - out_7663122633241286867[152] = 1; - out_7663122633241286867[153] = 0; - out_7663122633241286867[154] = 0; - out_7663122633241286867[155] = 0; - out_7663122633241286867[156] = 0; - out_7663122633241286867[157] = 0; - out_7663122633241286867[158] = 0; - out_7663122633241286867[159] = 0; - out_7663122633241286867[160] = 0; - out_7663122633241286867[161] = 0; - out_7663122633241286867[162] = 0; - out_7663122633241286867[163] = 0; - out_7663122633241286867[164] = 0; - out_7663122633241286867[165] = 0; - out_7663122633241286867[166] = 0; - out_7663122633241286867[167] = 0; - out_7663122633241286867[168] = 0; - out_7663122633241286867[169] = 0; - out_7663122633241286867[170] = 0; - out_7663122633241286867[171] = 1; - out_7663122633241286867[172] = 0; - out_7663122633241286867[173] = 0; - out_7663122633241286867[174] = 0; - out_7663122633241286867[175] = 0; - out_7663122633241286867[176] = 0; - out_7663122633241286867[177] = 0; - out_7663122633241286867[178] = 0; - out_7663122633241286867[179] = 0; - out_7663122633241286867[180] = 0; - out_7663122633241286867[181] = 0; - out_7663122633241286867[182] = 0; - out_7663122633241286867[183] = 0; - out_7663122633241286867[184] = 0; - out_7663122633241286867[185] = 0; - out_7663122633241286867[186] = 0; - out_7663122633241286867[187] = 0; - out_7663122633241286867[188] = 0; - out_7663122633241286867[189] = 0; - out_7663122633241286867[190] = 1; - out_7663122633241286867[191] = 0; - out_7663122633241286867[192] = 0; - out_7663122633241286867[193] = 0; - out_7663122633241286867[194] = 0; - out_7663122633241286867[195] = 0; - out_7663122633241286867[196] = 0; - out_7663122633241286867[197] = 0; - out_7663122633241286867[198] = 0; - out_7663122633241286867[199] = 0; - out_7663122633241286867[200] = 0; - out_7663122633241286867[201] = 0; - out_7663122633241286867[202] = 0; - out_7663122633241286867[203] = 0; - out_7663122633241286867[204] = 0; - out_7663122633241286867[205] = 0; - out_7663122633241286867[206] = 0; - out_7663122633241286867[207] = 0; - out_7663122633241286867[208] = 0; - out_7663122633241286867[209] = 1; - out_7663122633241286867[210] = 0; - out_7663122633241286867[211] = 0; - out_7663122633241286867[212] = 0; - out_7663122633241286867[213] = 0; - out_7663122633241286867[214] = 0; - out_7663122633241286867[215] = 0; - out_7663122633241286867[216] = 0; - out_7663122633241286867[217] = 0; - out_7663122633241286867[218] = 0; - out_7663122633241286867[219] = 0; - out_7663122633241286867[220] = 0; - out_7663122633241286867[221] = 0; - out_7663122633241286867[222] = 0; - out_7663122633241286867[223] = 0; - out_7663122633241286867[224] = 0; - out_7663122633241286867[225] = 0; - out_7663122633241286867[226] = 0; - out_7663122633241286867[227] = 0; - out_7663122633241286867[228] = 1; - out_7663122633241286867[229] = 0; - out_7663122633241286867[230] = 0; - out_7663122633241286867[231] = 0; - out_7663122633241286867[232] = 0; - out_7663122633241286867[233] = 0; - out_7663122633241286867[234] = 0; - out_7663122633241286867[235] = 0; - out_7663122633241286867[236] = 0; - out_7663122633241286867[237] = 0; - out_7663122633241286867[238] = 0; - out_7663122633241286867[239] = 0; - out_7663122633241286867[240] = 0; - out_7663122633241286867[241] = 0; - out_7663122633241286867[242] = 0; - out_7663122633241286867[243] = 0; - out_7663122633241286867[244] = 0; - out_7663122633241286867[245] = 0; - out_7663122633241286867[246] = 0; - out_7663122633241286867[247] = 1; - out_7663122633241286867[248] = 0; - out_7663122633241286867[249] = 0; - out_7663122633241286867[250] = 0; - out_7663122633241286867[251] = 0; - out_7663122633241286867[252] = 0; - out_7663122633241286867[253] = 0; - out_7663122633241286867[254] = 0; - out_7663122633241286867[255] = 0; - out_7663122633241286867[256] = 0; - out_7663122633241286867[257] = 0; - out_7663122633241286867[258] = 0; - out_7663122633241286867[259] = 0; - out_7663122633241286867[260] = 0; - out_7663122633241286867[261] = 0; - out_7663122633241286867[262] = 0; - out_7663122633241286867[263] = 0; - out_7663122633241286867[264] = 0; - out_7663122633241286867[265] = 0; - out_7663122633241286867[266] = 1; - out_7663122633241286867[267] = 0; - out_7663122633241286867[268] = 0; - out_7663122633241286867[269] = 0; - out_7663122633241286867[270] = 0; - out_7663122633241286867[271] = 0; - out_7663122633241286867[272] = 0; - out_7663122633241286867[273] = 0; - out_7663122633241286867[274] = 0; - out_7663122633241286867[275] = 0; - out_7663122633241286867[276] = 0; - out_7663122633241286867[277] = 0; - out_7663122633241286867[278] = 0; - out_7663122633241286867[279] = 0; - out_7663122633241286867[280] = 0; - out_7663122633241286867[281] = 0; - out_7663122633241286867[282] = 0; - out_7663122633241286867[283] = 0; - out_7663122633241286867[284] = 0; - out_7663122633241286867[285] = 1; - out_7663122633241286867[286] = 0; - out_7663122633241286867[287] = 0; - out_7663122633241286867[288] = 0; - out_7663122633241286867[289] = 0; - out_7663122633241286867[290] = 0; - out_7663122633241286867[291] = 0; - out_7663122633241286867[292] = 0; - out_7663122633241286867[293] = 0; - out_7663122633241286867[294] = 0; - out_7663122633241286867[295] = 0; - out_7663122633241286867[296] = 0; - out_7663122633241286867[297] = 0; - out_7663122633241286867[298] = 0; - out_7663122633241286867[299] = 0; - out_7663122633241286867[300] = 0; - out_7663122633241286867[301] = 0; - out_7663122633241286867[302] = 0; - out_7663122633241286867[303] = 0; - out_7663122633241286867[304] = 1; - out_7663122633241286867[305] = 0; - out_7663122633241286867[306] = 0; - out_7663122633241286867[307] = 0; - out_7663122633241286867[308] = 0; - out_7663122633241286867[309] = 0; - out_7663122633241286867[310] = 0; - out_7663122633241286867[311] = 0; - out_7663122633241286867[312] = 0; - out_7663122633241286867[313] = 0; - out_7663122633241286867[314] = 0; - out_7663122633241286867[315] = 0; - out_7663122633241286867[316] = 0; - out_7663122633241286867[317] = 0; - out_7663122633241286867[318] = 0; - out_7663122633241286867[319] = 0; - out_7663122633241286867[320] = 0; - out_7663122633241286867[321] = 0; - out_7663122633241286867[322] = 0; - out_7663122633241286867[323] = 1; +void F_fun(double *state, double dt, double *out_4806322824000620331) { + out_4806322824000620331[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_4806322824000620331[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_4806322824000620331[2] = 0; + out_4806322824000620331[3] = 0; + out_4806322824000620331[4] = 0; + out_4806322824000620331[5] = 0; + out_4806322824000620331[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_4806322824000620331[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_4806322824000620331[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_4806322824000620331[9] = 0; + out_4806322824000620331[10] = 0; + out_4806322824000620331[11] = 0; + out_4806322824000620331[12] = 0; + out_4806322824000620331[13] = 0; + out_4806322824000620331[14] = 0; + out_4806322824000620331[15] = 0; + out_4806322824000620331[16] = 0; + out_4806322824000620331[17] = 0; + out_4806322824000620331[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_4806322824000620331[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_4806322824000620331[20] = 0; + out_4806322824000620331[21] = 0; + out_4806322824000620331[22] = 0; + out_4806322824000620331[23] = 0; + out_4806322824000620331[24] = 0; + out_4806322824000620331[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_4806322824000620331[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_4806322824000620331[27] = 0; + out_4806322824000620331[28] = 0; + out_4806322824000620331[29] = 0; + out_4806322824000620331[30] = 0; + out_4806322824000620331[31] = 0; + out_4806322824000620331[32] = 0; + out_4806322824000620331[33] = 0; + out_4806322824000620331[34] = 0; + out_4806322824000620331[35] = 0; + out_4806322824000620331[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_4806322824000620331[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_4806322824000620331[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_4806322824000620331[39] = 0; + out_4806322824000620331[40] = 0; + out_4806322824000620331[41] = 0; + out_4806322824000620331[42] = 0; + out_4806322824000620331[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_4806322824000620331[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_4806322824000620331[45] = 0; + out_4806322824000620331[46] = 0; + out_4806322824000620331[47] = 0; + out_4806322824000620331[48] = 0; + out_4806322824000620331[49] = 0; + out_4806322824000620331[50] = 0; + out_4806322824000620331[51] = 0; + out_4806322824000620331[52] = 0; + out_4806322824000620331[53] = 0; + out_4806322824000620331[54] = 0; + out_4806322824000620331[55] = 0; + out_4806322824000620331[56] = 0; + out_4806322824000620331[57] = 1; + out_4806322824000620331[58] = 0; + out_4806322824000620331[59] = 0; + out_4806322824000620331[60] = 0; + out_4806322824000620331[61] = 0; + out_4806322824000620331[62] = 0; + out_4806322824000620331[63] = 0; + out_4806322824000620331[64] = 0; + out_4806322824000620331[65] = 0; + out_4806322824000620331[66] = dt; + out_4806322824000620331[67] = 0; + out_4806322824000620331[68] = 0; + out_4806322824000620331[69] = 0; + out_4806322824000620331[70] = 0; + out_4806322824000620331[71] = 0; + out_4806322824000620331[72] = 0; + out_4806322824000620331[73] = 0; + out_4806322824000620331[74] = 0; + out_4806322824000620331[75] = 0; + out_4806322824000620331[76] = 1; + out_4806322824000620331[77] = 0; + out_4806322824000620331[78] = 0; + out_4806322824000620331[79] = 0; + out_4806322824000620331[80] = 0; + out_4806322824000620331[81] = 0; + out_4806322824000620331[82] = 0; + out_4806322824000620331[83] = 0; + out_4806322824000620331[84] = 0; + out_4806322824000620331[85] = dt; + out_4806322824000620331[86] = 0; + out_4806322824000620331[87] = 0; + out_4806322824000620331[88] = 0; + out_4806322824000620331[89] = 0; + out_4806322824000620331[90] = 0; + out_4806322824000620331[91] = 0; + out_4806322824000620331[92] = 0; + out_4806322824000620331[93] = 0; + out_4806322824000620331[94] = 0; + out_4806322824000620331[95] = 1; + out_4806322824000620331[96] = 0; + out_4806322824000620331[97] = 0; + out_4806322824000620331[98] = 0; + out_4806322824000620331[99] = 0; + out_4806322824000620331[100] = 0; + out_4806322824000620331[101] = 0; + out_4806322824000620331[102] = 0; + out_4806322824000620331[103] = 0; + out_4806322824000620331[104] = dt; + out_4806322824000620331[105] = 0; + out_4806322824000620331[106] = 0; + out_4806322824000620331[107] = 0; + out_4806322824000620331[108] = 0; + out_4806322824000620331[109] = 0; + out_4806322824000620331[110] = 0; + out_4806322824000620331[111] = 0; + out_4806322824000620331[112] = 0; + out_4806322824000620331[113] = 0; + out_4806322824000620331[114] = 1; + out_4806322824000620331[115] = 0; + out_4806322824000620331[116] = 0; + out_4806322824000620331[117] = 0; + out_4806322824000620331[118] = 0; + out_4806322824000620331[119] = 0; + out_4806322824000620331[120] = 0; + out_4806322824000620331[121] = 0; + out_4806322824000620331[122] = 0; + out_4806322824000620331[123] = 0; + out_4806322824000620331[124] = 0; + out_4806322824000620331[125] = 0; + out_4806322824000620331[126] = 0; + out_4806322824000620331[127] = 0; + out_4806322824000620331[128] = 0; + out_4806322824000620331[129] = 0; + out_4806322824000620331[130] = 0; + out_4806322824000620331[131] = 0; + out_4806322824000620331[132] = 0; + out_4806322824000620331[133] = 1; + out_4806322824000620331[134] = 0; + out_4806322824000620331[135] = 0; + out_4806322824000620331[136] = 0; + out_4806322824000620331[137] = 0; + out_4806322824000620331[138] = 0; + out_4806322824000620331[139] = 0; + out_4806322824000620331[140] = 0; + out_4806322824000620331[141] = 0; + out_4806322824000620331[142] = 0; + out_4806322824000620331[143] = 0; + out_4806322824000620331[144] = 0; + out_4806322824000620331[145] = 0; + out_4806322824000620331[146] = 0; + out_4806322824000620331[147] = 0; + out_4806322824000620331[148] = 0; + out_4806322824000620331[149] = 0; + out_4806322824000620331[150] = 0; + out_4806322824000620331[151] = 0; + out_4806322824000620331[152] = 1; + out_4806322824000620331[153] = 0; + out_4806322824000620331[154] = 0; + out_4806322824000620331[155] = 0; + out_4806322824000620331[156] = 0; + out_4806322824000620331[157] = 0; + out_4806322824000620331[158] = 0; + out_4806322824000620331[159] = 0; + out_4806322824000620331[160] = 0; + out_4806322824000620331[161] = 0; + out_4806322824000620331[162] = 0; + out_4806322824000620331[163] = 0; + out_4806322824000620331[164] = 0; + out_4806322824000620331[165] = 0; + out_4806322824000620331[166] = 0; + out_4806322824000620331[167] = 0; + out_4806322824000620331[168] = 0; + out_4806322824000620331[169] = 0; + out_4806322824000620331[170] = 0; + out_4806322824000620331[171] = 1; + out_4806322824000620331[172] = 0; + out_4806322824000620331[173] = 0; + out_4806322824000620331[174] = 0; + out_4806322824000620331[175] = 0; + out_4806322824000620331[176] = 0; + out_4806322824000620331[177] = 0; + out_4806322824000620331[178] = 0; + out_4806322824000620331[179] = 0; + out_4806322824000620331[180] = 0; + out_4806322824000620331[181] = 0; + out_4806322824000620331[182] = 0; + out_4806322824000620331[183] = 0; + out_4806322824000620331[184] = 0; + out_4806322824000620331[185] = 0; + out_4806322824000620331[186] = 0; + out_4806322824000620331[187] = 0; + out_4806322824000620331[188] = 0; + out_4806322824000620331[189] = 0; + out_4806322824000620331[190] = 1; + out_4806322824000620331[191] = 0; + out_4806322824000620331[192] = 0; + out_4806322824000620331[193] = 0; + out_4806322824000620331[194] = 0; + out_4806322824000620331[195] = 0; + out_4806322824000620331[196] = 0; + out_4806322824000620331[197] = 0; + out_4806322824000620331[198] = 0; + out_4806322824000620331[199] = 0; + out_4806322824000620331[200] = 0; + out_4806322824000620331[201] = 0; + out_4806322824000620331[202] = 0; + out_4806322824000620331[203] = 0; + out_4806322824000620331[204] = 0; + out_4806322824000620331[205] = 0; + out_4806322824000620331[206] = 0; + out_4806322824000620331[207] = 0; + out_4806322824000620331[208] = 0; + out_4806322824000620331[209] = 1; + out_4806322824000620331[210] = 0; + out_4806322824000620331[211] = 0; + out_4806322824000620331[212] = 0; + out_4806322824000620331[213] = 0; + out_4806322824000620331[214] = 0; + out_4806322824000620331[215] = 0; + out_4806322824000620331[216] = 0; + out_4806322824000620331[217] = 0; + out_4806322824000620331[218] = 0; + out_4806322824000620331[219] = 0; + out_4806322824000620331[220] = 0; + out_4806322824000620331[221] = 0; + out_4806322824000620331[222] = 0; + out_4806322824000620331[223] = 0; + out_4806322824000620331[224] = 0; + out_4806322824000620331[225] = 0; + out_4806322824000620331[226] = 0; + out_4806322824000620331[227] = 0; + out_4806322824000620331[228] = 1; + out_4806322824000620331[229] = 0; + out_4806322824000620331[230] = 0; + out_4806322824000620331[231] = 0; + out_4806322824000620331[232] = 0; + out_4806322824000620331[233] = 0; + out_4806322824000620331[234] = 0; + out_4806322824000620331[235] = 0; + out_4806322824000620331[236] = 0; + out_4806322824000620331[237] = 0; + out_4806322824000620331[238] = 0; + out_4806322824000620331[239] = 0; + out_4806322824000620331[240] = 0; + out_4806322824000620331[241] = 0; + out_4806322824000620331[242] = 0; + out_4806322824000620331[243] = 0; + out_4806322824000620331[244] = 0; + out_4806322824000620331[245] = 0; + out_4806322824000620331[246] = 0; + out_4806322824000620331[247] = 1; + out_4806322824000620331[248] = 0; + out_4806322824000620331[249] = 0; + out_4806322824000620331[250] = 0; + out_4806322824000620331[251] = 0; + out_4806322824000620331[252] = 0; + out_4806322824000620331[253] = 0; + out_4806322824000620331[254] = 0; + out_4806322824000620331[255] = 0; + out_4806322824000620331[256] = 0; + out_4806322824000620331[257] = 0; + out_4806322824000620331[258] = 0; + out_4806322824000620331[259] = 0; + out_4806322824000620331[260] = 0; + out_4806322824000620331[261] = 0; + out_4806322824000620331[262] = 0; + out_4806322824000620331[263] = 0; + out_4806322824000620331[264] = 0; + out_4806322824000620331[265] = 0; + out_4806322824000620331[266] = 1; + out_4806322824000620331[267] = 0; + out_4806322824000620331[268] = 0; + out_4806322824000620331[269] = 0; + out_4806322824000620331[270] = 0; + out_4806322824000620331[271] = 0; + out_4806322824000620331[272] = 0; + out_4806322824000620331[273] = 0; + out_4806322824000620331[274] = 0; + out_4806322824000620331[275] = 0; + out_4806322824000620331[276] = 0; + out_4806322824000620331[277] = 0; + out_4806322824000620331[278] = 0; + out_4806322824000620331[279] = 0; + out_4806322824000620331[280] = 0; + out_4806322824000620331[281] = 0; + out_4806322824000620331[282] = 0; + out_4806322824000620331[283] = 0; + out_4806322824000620331[284] = 0; + out_4806322824000620331[285] = 1; + out_4806322824000620331[286] = 0; + out_4806322824000620331[287] = 0; + out_4806322824000620331[288] = 0; + out_4806322824000620331[289] = 0; + out_4806322824000620331[290] = 0; + out_4806322824000620331[291] = 0; + out_4806322824000620331[292] = 0; + out_4806322824000620331[293] = 0; + out_4806322824000620331[294] = 0; + out_4806322824000620331[295] = 0; + out_4806322824000620331[296] = 0; + out_4806322824000620331[297] = 0; + out_4806322824000620331[298] = 0; + out_4806322824000620331[299] = 0; + out_4806322824000620331[300] = 0; + out_4806322824000620331[301] = 0; + out_4806322824000620331[302] = 0; + out_4806322824000620331[303] = 0; + out_4806322824000620331[304] = 1; + out_4806322824000620331[305] = 0; + out_4806322824000620331[306] = 0; + out_4806322824000620331[307] = 0; + out_4806322824000620331[308] = 0; + out_4806322824000620331[309] = 0; + out_4806322824000620331[310] = 0; + out_4806322824000620331[311] = 0; + out_4806322824000620331[312] = 0; + out_4806322824000620331[313] = 0; + out_4806322824000620331[314] = 0; + out_4806322824000620331[315] = 0; + out_4806322824000620331[316] = 0; + out_4806322824000620331[317] = 0; + out_4806322824000620331[318] = 0; + out_4806322824000620331[319] = 0; + out_4806322824000620331[320] = 0; + out_4806322824000620331[321] = 0; + out_4806322824000620331[322] = 0; + out_4806322824000620331[323] = 1; } -void h_4(double *state, double *unused, double *out_7389052167092190980) { - out_7389052167092190980[0] = state[6] + state[9]; - out_7389052167092190980[1] = state[7] + state[10]; - out_7389052167092190980[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_2146936515699344985) { + out_2146936515699344985[0] = state[6] + state[9]; + out_2146936515699344985[1] = state[7] + state[10]; + out_2146936515699344985[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_7038855096230664953) { - out_7038855096230664953[0] = 0; - out_7038855096230664953[1] = 0; - out_7038855096230664953[2] = 0; - out_7038855096230664953[3] = 0; - out_7038855096230664953[4] = 0; - out_7038855096230664953[5] = 0; - out_7038855096230664953[6] = 1; - out_7038855096230664953[7] = 0; - out_7038855096230664953[8] = 0; - out_7038855096230664953[9] = 1; - out_7038855096230664953[10] = 0; - out_7038855096230664953[11] = 0; - out_7038855096230664953[12] = 0; - out_7038855096230664953[13] = 0; - out_7038855096230664953[14] = 0; - out_7038855096230664953[15] = 0; - out_7038855096230664953[16] = 0; - out_7038855096230664953[17] = 0; - out_7038855096230664953[18] = 0; - out_7038855096230664953[19] = 0; - out_7038855096230664953[20] = 0; - out_7038855096230664953[21] = 0; - out_7038855096230664953[22] = 0; - out_7038855096230664953[23] = 0; - out_7038855096230664953[24] = 0; - out_7038855096230664953[25] = 1; - out_7038855096230664953[26] = 0; - out_7038855096230664953[27] = 0; - out_7038855096230664953[28] = 1; - out_7038855096230664953[29] = 0; - out_7038855096230664953[30] = 0; - out_7038855096230664953[31] = 0; - out_7038855096230664953[32] = 0; - out_7038855096230664953[33] = 0; - out_7038855096230664953[34] = 0; - out_7038855096230664953[35] = 0; - out_7038855096230664953[36] = 0; - out_7038855096230664953[37] = 0; - out_7038855096230664953[38] = 0; - out_7038855096230664953[39] = 0; - out_7038855096230664953[40] = 0; - out_7038855096230664953[41] = 0; - out_7038855096230664953[42] = 0; - out_7038855096230664953[43] = 0; - out_7038855096230664953[44] = 1; - out_7038855096230664953[45] = 0; - out_7038855096230664953[46] = 0; - out_7038855096230664953[47] = 1; - out_7038855096230664953[48] = 0; - out_7038855096230664953[49] = 0; - out_7038855096230664953[50] = 0; - out_7038855096230664953[51] = 0; - out_7038855096230664953[52] = 0; - out_7038855096230664953[53] = 0; +void H_4(double *state, double *unused, double *out_9176625892716694514) { + out_9176625892716694514[0] = 0; + out_9176625892716694514[1] = 0; + out_9176625892716694514[2] = 0; + out_9176625892716694514[3] = 0; + out_9176625892716694514[4] = 0; + out_9176625892716694514[5] = 0; + out_9176625892716694514[6] = 1; + out_9176625892716694514[7] = 0; + out_9176625892716694514[8] = 0; + out_9176625892716694514[9] = 1; + out_9176625892716694514[10] = 0; + out_9176625892716694514[11] = 0; + out_9176625892716694514[12] = 0; + out_9176625892716694514[13] = 0; + out_9176625892716694514[14] = 0; + out_9176625892716694514[15] = 0; + out_9176625892716694514[16] = 0; + out_9176625892716694514[17] = 0; + out_9176625892716694514[18] = 0; + out_9176625892716694514[19] = 0; + out_9176625892716694514[20] = 0; + out_9176625892716694514[21] = 0; + out_9176625892716694514[22] = 0; + out_9176625892716694514[23] = 0; + out_9176625892716694514[24] = 0; + out_9176625892716694514[25] = 1; + out_9176625892716694514[26] = 0; + out_9176625892716694514[27] = 0; + out_9176625892716694514[28] = 1; + out_9176625892716694514[29] = 0; + out_9176625892716694514[30] = 0; + out_9176625892716694514[31] = 0; + out_9176625892716694514[32] = 0; + out_9176625892716694514[33] = 0; + out_9176625892716694514[34] = 0; + out_9176625892716694514[35] = 0; + out_9176625892716694514[36] = 0; + out_9176625892716694514[37] = 0; + out_9176625892716694514[38] = 0; + out_9176625892716694514[39] = 0; + out_9176625892716694514[40] = 0; + out_9176625892716694514[41] = 0; + out_9176625892716694514[42] = 0; + out_9176625892716694514[43] = 0; + out_9176625892716694514[44] = 1; + out_9176625892716694514[45] = 0; + out_9176625892716694514[46] = 0; + out_9176625892716694514[47] = 1; + out_9176625892716694514[48] = 0; + out_9176625892716694514[49] = 0; + out_9176625892716694514[50] = 0; + out_9176625892716694514[51] = 0; + out_9176625892716694514[52] = 0; + out_9176625892716694514[53] = 0; } -void h_10(double *state, double *unused, double *out_8071536081821918077) { - out_8071536081821918077[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_8071536081821918077[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_8071536081821918077[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_7324595289411116285) { + out_7324595289411116285[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_7324595289411116285[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_7324595289411116285[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_4101630430643064156) { - out_4101630430643064156[0] = 0; - out_4101630430643064156[1] = 9.8100000000000005*cos(state[1]); - out_4101630430643064156[2] = 0; - out_4101630430643064156[3] = 0; - out_4101630430643064156[4] = -state[8]; - out_4101630430643064156[5] = state[7]; - out_4101630430643064156[6] = 0; - out_4101630430643064156[7] = state[5]; - out_4101630430643064156[8] = -state[4]; - out_4101630430643064156[9] = 0; - out_4101630430643064156[10] = 0; - out_4101630430643064156[11] = 0; - out_4101630430643064156[12] = 1; - out_4101630430643064156[13] = 0; - out_4101630430643064156[14] = 0; - out_4101630430643064156[15] = 1; - out_4101630430643064156[16] = 0; - out_4101630430643064156[17] = 0; - out_4101630430643064156[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_4101630430643064156[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_4101630430643064156[20] = 0; - out_4101630430643064156[21] = state[8]; - out_4101630430643064156[22] = 0; - out_4101630430643064156[23] = -state[6]; - out_4101630430643064156[24] = -state[5]; - out_4101630430643064156[25] = 0; - out_4101630430643064156[26] = state[3]; - out_4101630430643064156[27] = 0; - out_4101630430643064156[28] = 0; - out_4101630430643064156[29] = 0; - out_4101630430643064156[30] = 0; - out_4101630430643064156[31] = 1; - out_4101630430643064156[32] = 0; - out_4101630430643064156[33] = 0; - out_4101630430643064156[34] = 1; - out_4101630430643064156[35] = 0; - out_4101630430643064156[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_4101630430643064156[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_4101630430643064156[38] = 0; - out_4101630430643064156[39] = -state[7]; - out_4101630430643064156[40] = state[6]; - out_4101630430643064156[41] = 0; - out_4101630430643064156[42] = state[4]; - out_4101630430643064156[43] = -state[3]; - out_4101630430643064156[44] = 0; - out_4101630430643064156[45] = 0; - out_4101630430643064156[46] = 0; - out_4101630430643064156[47] = 0; - out_4101630430643064156[48] = 0; - out_4101630430643064156[49] = 0; - out_4101630430643064156[50] = 1; - out_4101630430643064156[51] = 0; - out_4101630430643064156[52] = 0; - out_4101630430643064156[53] = 1; +void H_10(double *state, double *unused, double *out_6720236421065597274) { + out_6720236421065597274[0] = 0; + out_6720236421065597274[1] = 9.8100000000000005*cos(state[1]); + out_6720236421065597274[2] = 0; + out_6720236421065597274[3] = 0; + out_6720236421065597274[4] = -state[8]; + out_6720236421065597274[5] = state[7]; + out_6720236421065597274[6] = 0; + out_6720236421065597274[7] = state[5]; + out_6720236421065597274[8] = -state[4]; + out_6720236421065597274[9] = 0; + out_6720236421065597274[10] = 0; + out_6720236421065597274[11] = 0; + out_6720236421065597274[12] = 1; + out_6720236421065597274[13] = 0; + out_6720236421065597274[14] = 0; + out_6720236421065597274[15] = 1; + out_6720236421065597274[16] = 0; + out_6720236421065597274[17] = 0; + out_6720236421065597274[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_6720236421065597274[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_6720236421065597274[20] = 0; + out_6720236421065597274[21] = state[8]; + out_6720236421065597274[22] = 0; + out_6720236421065597274[23] = -state[6]; + out_6720236421065597274[24] = -state[5]; + out_6720236421065597274[25] = 0; + out_6720236421065597274[26] = state[3]; + out_6720236421065597274[27] = 0; + out_6720236421065597274[28] = 0; + out_6720236421065597274[29] = 0; + out_6720236421065597274[30] = 0; + out_6720236421065597274[31] = 1; + out_6720236421065597274[32] = 0; + out_6720236421065597274[33] = 0; + out_6720236421065597274[34] = 1; + out_6720236421065597274[35] = 0; + out_6720236421065597274[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_6720236421065597274[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_6720236421065597274[38] = 0; + out_6720236421065597274[39] = -state[7]; + out_6720236421065597274[40] = state[6]; + out_6720236421065597274[41] = 0; + out_6720236421065597274[42] = state[4]; + out_6720236421065597274[43] = -state[3]; + out_6720236421065597274[44] = 0; + out_6720236421065597274[45] = 0; + out_6720236421065597274[46] = 0; + out_6720236421065597274[47] = 0; + out_6720236421065597274[48] = 0; + out_6720236421065597274[49] = 0; + out_6720236421065597274[50] = 1; + out_6720236421065597274[51] = 0; + out_6720236421065597274[52] = 0; + out_6720236421065597274[53] = 1; } -void h_13(double *state, double *unused, double *out_4611633119501949975) { - out_4611633119501949975[0] = state[3]; - out_4611633119501949975[1] = state[4]; - out_4611633119501949975[2] = state[5]; +void h_13(double *state, double *unused, double *out_3077593966437383014) { + out_3077593966437383014[0] = state[3]; + out_3077593966437383014[1] = state[4]; + out_3077593966437383014[2] = state[5]; } -void H_13(double *state, double *unused, double *out_8195615152146553862) { - out_8195615152146553862[0] = 0; - out_8195615152146553862[1] = 0; - out_8195615152146553862[2] = 0; - out_8195615152146553862[3] = 1; - out_8195615152146553862[4] = 0; - out_8195615152146553862[5] = 0; - out_8195615152146553862[6] = 0; - out_8195615152146553862[7] = 0; - out_8195615152146553862[8] = 0; - out_8195615152146553862[9] = 0; - out_8195615152146553862[10] = 0; - out_8195615152146553862[11] = 0; - out_8195615152146553862[12] = 0; - out_8195615152146553862[13] = 0; - out_8195615152146553862[14] = 0; - out_8195615152146553862[15] = 0; - out_8195615152146553862[16] = 0; - out_8195615152146553862[17] = 0; - out_8195615152146553862[18] = 0; - out_8195615152146553862[19] = 0; - out_8195615152146553862[20] = 0; - out_8195615152146553862[21] = 0; - out_8195615152146553862[22] = 1; - out_8195615152146553862[23] = 0; - out_8195615152146553862[24] = 0; - out_8195615152146553862[25] = 0; - out_8195615152146553862[26] = 0; - out_8195615152146553862[27] = 0; - out_8195615152146553862[28] = 0; - out_8195615152146553862[29] = 0; - out_8195615152146553862[30] = 0; - out_8195615152146553862[31] = 0; - out_8195615152146553862[32] = 0; - out_8195615152146553862[33] = 0; - out_8195615152146553862[34] = 0; - out_8195615152146553862[35] = 0; - out_8195615152146553862[36] = 0; - out_8195615152146553862[37] = 0; - out_8195615152146553862[38] = 0; - out_8195615152146553862[39] = 0; - out_8195615152146553862[40] = 0; - out_8195615152146553862[41] = 1; - out_8195615152146553862[42] = 0; - out_8195615152146553862[43] = 0; - out_8195615152146553862[44] = 0; - out_8195615152146553862[45] = 0; - out_8195615152146553862[46] = 0; - out_8195615152146553862[47] = 0; - out_8195615152146553862[48] = 0; - out_8195615152146553862[49] = 0; - out_8195615152146553862[50] = 0; - out_8195615152146553862[51] = 0; - out_8195615152146553862[52] = 0; - out_8195615152146553862[53] = 0; +void H_13(double *state, double *unused, double *out_6057844355660524301) { + out_6057844355660524301[0] = 0; + out_6057844355660524301[1] = 0; + out_6057844355660524301[2] = 0; + out_6057844355660524301[3] = 1; + out_6057844355660524301[4] = 0; + out_6057844355660524301[5] = 0; + out_6057844355660524301[6] = 0; + out_6057844355660524301[7] = 0; + out_6057844355660524301[8] = 0; + out_6057844355660524301[9] = 0; + out_6057844355660524301[10] = 0; + out_6057844355660524301[11] = 0; + out_6057844355660524301[12] = 0; + out_6057844355660524301[13] = 0; + out_6057844355660524301[14] = 0; + out_6057844355660524301[15] = 0; + out_6057844355660524301[16] = 0; + out_6057844355660524301[17] = 0; + out_6057844355660524301[18] = 0; + out_6057844355660524301[19] = 0; + out_6057844355660524301[20] = 0; + out_6057844355660524301[21] = 0; + out_6057844355660524301[22] = 1; + out_6057844355660524301[23] = 0; + out_6057844355660524301[24] = 0; + out_6057844355660524301[25] = 0; + out_6057844355660524301[26] = 0; + out_6057844355660524301[27] = 0; + out_6057844355660524301[28] = 0; + out_6057844355660524301[29] = 0; + out_6057844355660524301[30] = 0; + out_6057844355660524301[31] = 0; + out_6057844355660524301[32] = 0; + out_6057844355660524301[33] = 0; + out_6057844355660524301[34] = 0; + out_6057844355660524301[35] = 0; + out_6057844355660524301[36] = 0; + out_6057844355660524301[37] = 0; + out_6057844355660524301[38] = 0; + out_6057844355660524301[39] = 0; + out_6057844355660524301[40] = 0; + out_6057844355660524301[41] = 1; + out_6057844355660524301[42] = 0; + out_6057844355660524301[43] = 0; + out_6057844355660524301[44] = 0; + out_6057844355660524301[45] = 0; + out_6057844355660524301[46] = 0; + out_6057844355660524301[47] = 0; + out_6057844355660524301[48] = 0; + out_6057844355660524301[49] = 0; + out_6057844355660524301[50] = 0; + out_6057844355660524301[51] = 0; + out_6057844355660524301[52] = 0; + out_6057844355660524301[53] = 0; } -void h_14(double *state, double *unused, double *out_519391538095944063) { - out_519391538095944063[0] = state[6]; - out_519391538095944063[1] = state[7]; - out_519391538095944063[2] = state[8]; +void h_14(double *state, double *unused, double *out_5838993491781915029) { + out_5838993491781915029[0] = state[6]; + out_5838993491781915029[1] = state[7]; + out_5838993491781915029[2] = state[8]; } -void H_14(double *state, double *unused, double *out_3956066663935292657) { - out_3956066663935292657[0] = 0; - out_3956066663935292657[1] = 0; - out_3956066663935292657[2] = 0; - out_3956066663935292657[3] = 0; - out_3956066663935292657[4] = 0; - out_3956066663935292657[5] = 0; - out_3956066663935292657[6] = 1; - out_3956066663935292657[7] = 0; - out_3956066663935292657[8] = 0; - out_3956066663935292657[9] = 0; - out_3956066663935292657[10] = 0; - out_3956066663935292657[11] = 0; - out_3956066663935292657[12] = 0; - out_3956066663935292657[13] = 0; - out_3956066663935292657[14] = 0; - out_3956066663935292657[15] = 0; - out_3956066663935292657[16] = 0; - out_3956066663935292657[17] = 0; - out_3956066663935292657[18] = 0; - out_3956066663935292657[19] = 0; - out_3956066663935292657[20] = 0; - out_3956066663935292657[21] = 0; - out_3956066663935292657[22] = 0; - out_3956066663935292657[23] = 0; - out_3956066663935292657[24] = 0; - out_3956066663935292657[25] = 1; - out_3956066663935292657[26] = 0; - out_3956066663935292657[27] = 0; - out_3956066663935292657[28] = 0; - out_3956066663935292657[29] = 0; - out_3956066663935292657[30] = 0; - out_3956066663935292657[31] = 0; - out_3956066663935292657[32] = 0; - out_3956066663935292657[33] = 0; - out_3956066663935292657[34] = 0; - out_3956066663935292657[35] = 0; - out_3956066663935292657[36] = 0; - out_3956066663935292657[37] = 0; - out_3956066663935292657[38] = 0; - out_3956066663935292657[39] = 0; - out_3956066663935292657[40] = 0; - out_3956066663935292657[41] = 0; - out_3956066663935292657[42] = 0; - out_3956066663935292657[43] = 0; - out_3956066663935292657[44] = 1; - out_3956066663935292657[45] = 0; - out_3956066663935292657[46] = 0; - out_3956066663935292657[47] = 0; - out_3956066663935292657[48] = 0; - out_3956066663935292657[49] = 0; - out_3956066663935292657[50] = 0; - out_3956066663935292657[51] = 0; - out_3956066663935292657[52] = 0; - out_3956066663935292657[53] = 0; +void H_14(double *state, double *unused, double *out_5306877324653372573) { + out_5306877324653372573[0] = 0; + out_5306877324653372573[1] = 0; + out_5306877324653372573[2] = 0; + out_5306877324653372573[3] = 0; + out_5306877324653372573[4] = 0; + out_5306877324653372573[5] = 0; + out_5306877324653372573[6] = 1; + out_5306877324653372573[7] = 0; + out_5306877324653372573[8] = 0; + out_5306877324653372573[9] = 0; + out_5306877324653372573[10] = 0; + out_5306877324653372573[11] = 0; + out_5306877324653372573[12] = 0; + out_5306877324653372573[13] = 0; + out_5306877324653372573[14] = 0; + out_5306877324653372573[15] = 0; + out_5306877324653372573[16] = 0; + out_5306877324653372573[17] = 0; + out_5306877324653372573[18] = 0; + out_5306877324653372573[19] = 0; + out_5306877324653372573[20] = 0; + out_5306877324653372573[21] = 0; + out_5306877324653372573[22] = 0; + out_5306877324653372573[23] = 0; + out_5306877324653372573[24] = 0; + out_5306877324653372573[25] = 1; + out_5306877324653372573[26] = 0; + out_5306877324653372573[27] = 0; + out_5306877324653372573[28] = 0; + out_5306877324653372573[29] = 0; + out_5306877324653372573[30] = 0; + out_5306877324653372573[31] = 0; + out_5306877324653372573[32] = 0; + out_5306877324653372573[33] = 0; + out_5306877324653372573[34] = 0; + out_5306877324653372573[35] = 0; + out_5306877324653372573[36] = 0; + out_5306877324653372573[37] = 0; + out_5306877324653372573[38] = 0; + out_5306877324653372573[39] = 0; + out_5306877324653372573[40] = 0; + out_5306877324653372573[41] = 0; + out_5306877324653372573[42] = 0; + out_5306877324653372573[43] = 0; + out_5306877324653372573[44] = 1; + out_5306877324653372573[45] = 0; + out_5306877324653372573[46] = 0; + out_5306877324653372573[47] = 0; + out_5306877324653372573[48] = 0; + out_5306877324653372573[49] = 0; + out_5306877324653372573[50] = 0; + out_5306877324653372573[51] = 0; + out_5306877324653372573[52] = 0; + out_5306877324653372573[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_908811316603708218) { - err_fun(nom_x, delta_x, out_908811316603708218); +void pose_err_fun(double *nom_x, double *delta_x, double *out_4497386892446686963) { + err_fun(nom_x, delta_x, out_4497386892446686963); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6097135032344727242) { - inv_err_fun(nom_x, true_x, out_6097135032344727242); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_4020535164465364860) { + inv_err_fun(nom_x, true_x, out_4020535164465364860); } -void pose_H_mod_fun(double *state, double *out_5840907614347766849) { - H_mod_fun(state, out_5840907614347766849); +void pose_H_mod_fun(double *state, double *out_163641472803010022) { + H_mod_fun(state, out_163641472803010022); } -void pose_f_fun(double *state, double dt, double *out_6742419823108127014) { - f_fun(state, dt, out_6742419823108127014); +void pose_f_fun(double *state, double dt, double *out_7113476067971581844) { + f_fun(state, dt, out_7113476067971581844); } -void pose_F_fun(double *state, double dt, double *out_7663122633241286867) { - F_fun(state, dt, out_7663122633241286867); +void pose_F_fun(double *state, double dt, double *out_4806322824000620331) { + F_fun(state, dt, out_4806322824000620331); } -void pose_h_4(double *state, double *unused, double *out_7389052167092190980) { - h_4(state, unused, out_7389052167092190980); +void pose_h_4(double *state, double *unused, double *out_2146936515699344985) { + h_4(state, unused, out_2146936515699344985); } -void pose_H_4(double *state, double *unused, double *out_7038855096230664953) { - H_4(state, unused, out_7038855096230664953); +void pose_H_4(double *state, double *unused, double *out_9176625892716694514) { + H_4(state, unused, out_9176625892716694514); } -void pose_h_10(double *state, double *unused, double *out_8071536081821918077) { - h_10(state, unused, out_8071536081821918077); +void pose_h_10(double *state, double *unused, double *out_7324595289411116285) { + h_10(state, unused, out_7324595289411116285); } -void pose_H_10(double *state, double *unused, double *out_4101630430643064156) { - H_10(state, unused, out_4101630430643064156); +void pose_H_10(double *state, double *unused, double *out_6720236421065597274) { + H_10(state, unused, out_6720236421065597274); } -void pose_h_13(double *state, double *unused, double *out_4611633119501949975) { - h_13(state, unused, out_4611633119501949975); +void pose_h_13(double *state, double *unused, double *out_3077593966437383014) { + h_13(state, unused, out_3077593966437383014); } -void pose_H_13(double *state, double *unused, double *out_8195615152146553862) { - H_13(state, unused, out_8195615152146553862); +void pose_H_13(double *state, double *unused, double *out_6057844355660524301) { + H_13(state, unused, out_6057844355660524301); } -void pose_h_14(double *state, double *unused, double *out_519391538095944063) { - h_14(state, unused, out_519391538095944063); +void pose_h_14(double *state, double *unused, double *out_5838993491781915029) { + h_14(state, unused, out_5838993491781915029); } -void pose_H_14(double *state, double *unused, double *out_3956066663935292657) { - H_14(state, unused, out_3956066663935292657); +void pose_H_14(double *state, double *unused, double *out_5306877324653372573) { + H_14(state, unused, out_5306877324653372573); } 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 193755c25f..6296363cfd 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_908811316603708218); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6097135032344727242); -void pose_H_mod_fun(double *state, double *out_5840907614347766849); -void pose_f_fun(double *state, double dt, double *out_6742419823108127014); -void pose_F_fun(double *state, double dt, double *out_7663122633241286867); -void pose_h_4(double *state, double *unused, double *out_7389052167092190980); -void pose_H_4(double *state, double *unused, double *out_7038855096230664953); -void pose_h_10(double *state, double *unused, double *out_8071536081821918077); -void pose_H_10(double *state, double *unused, double *out_4101630430643064156); -void pose_h_13(double *state, double *unused, double *out_4611633119501949975); -void pose_H_13(double *state, double *unused, double *out_8195615152146553862); -void pose_h_14(double *state, double *unused, double *out_519391538095944063); -void pose_H_14(double *state, double *unused, double *out_3956066663935292657); +void pose_err_fun(double *nom_x, double *delta_x, double *out_4497386892446686963); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_4020535164465364860); +void pose_H_mod_fun(double *state, double *out_163641472803010022); +void pose_f_fun(double *state, double dt, double *out_7113476067971581844); +void pose_F_fun(double *state, double dt, double *out_4806322824000620331); +void pose_h_4(double *state, double *unused, double *out_2146936515699344985); +void pose_H_4(double *state, double *unused, double *out_9176625892716694514); +void pose_h_10(double *state, double *unused, double *out_7324595289411116285); +void pose_H_10(double *state, double *unused, double *out_6720236421065597274); +void pose_h_13(double *state, double *unused, double *out_3077593966437383014); +void pose_H_13(double *state, double *unused, double *out_6057844355660524301); +void pose_h_14(double *state, double *unused, double *out_5838993491781915029); +void pose_H_14(double *state, double *unused, double *out_5306877324653372573); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/pandad/pandad b/selfdrive/pandad/pandad index 04bfd41ad7..a183220b4f 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/system/camerad/camerad b/system/camerad/camerad index 7e9c638718..84fec71d91 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index 40861ad13e..9bae73a4e0 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index dd7545173a..a63970579f 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index af0cedb636..aae84be89f 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ