diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index 79dc7f8032..bf8d60f24a 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 eb82dea0b1..b75c65f8ea 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 d84d2580bf..224d2963dd 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 9bc4f360f3..e2c80679a1 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 5b0db97e65..3e8551ea7d 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 02a3e958d2..8399cfb926 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 02a3e958d2..8399cfb926 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 74a6a9781a..1968ae0108 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 74a6a9781a..1968ae0108 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 74a6a9781a..1968ae0108 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 74a6a9781a..1968ae0108 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 74a6a9781a..1968ae0108 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 74a6a9781a..1968ae0108 100755 Binary files a/panda/board/obj/bootstub.panda_h7_remote_can_ignition_only.bin and b/panda/board/obj/bootstub.panda_h7_remote_can_ignition_only.bin differ diff --git a/panda/board/obj/bootstub.panda_h7_tesla_wake.bin b/panda/board/obj/bootstub.panda_h7_tesla_wake.bin index 74a6a9781a..1968ae0108 100755 Binary files a/panda/board/obj/bootstub.panda_h7_tesla_wake.bin and b/panda/board/obj/bootstub.panda_h7_tesla_wake.bin differ diff --git a/panda/board/obj/bootstub.panda_h7_tesla_wake_can_ignition_only.bin b/panda/board/obj/bootstub.panda_h7_tesla_wake_can_ignition_only.bin index 74a6a9781a..1968ae0108 100755 Binary files a/panda/board/obj/bootstub.panda_h7_tesla_wake_can_ignition_only.bin and b/panda/board/obj/bootstub.panda_h7_tesla_wake_can_ignition_only.bin differ diff --git a/panda/board/obj/bootstub.panda_hkg_remote.bin b/panda/board/obj/bootstub.panda_hkg_remote.bin index 02a3e958d2..8399cfb926 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 02a3e958d2..8399cfb926 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 f7f1219a37..999041cce5 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 02a3e958d2..8399cfb926 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 02a3e958d2..8399cfb926 100755 Binary files a/panda/board/obj/bootstub.panda_remote_can_ignition_only.bin and b/panda/board/obj/bootstub.panda_remote_can_ignition_only.bin differ diff --git a/panda/board/obj/bootstub.panda_tesla_wake.bin b/panda/board/obj/bootstub.panda_tesla_wake.bin index 02a3e958d2..8399cfb926 100755 Binary files a/panda/board/obj/bootstub.panda_tesla_wake.bin and b/panda/board/obj/bootstub.panda_tesla_wake.bin differ diff --git a/panda/board/obj/bootstub.panda_tesla_wake_can_ignition_only.bin b/panda/board/obj/bootstub.panda_tesla_wake_can_ignition_only.bin index 02a3e958d2..8399cfb926 100755 Binary files a/panda/board/obj/bootstub.panda_tesla_wake_can_ignition_only.bin and b/panda/board/obj/bootstub.panda_tesla_wake_can_ignition_only.bin differ diff --git a/panda/board/obj/gitversion.h b/panda/board/obj/gitversion.h index 295b0c610e..b3b2d01c58 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-bf00f88b-DEBUG"; +const uint8_t gitversion[19] = "DEV-02446532-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 9bacd1d525..56622f6f3f 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 6a3de3ab51..3ddde010d3 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 2bfb1a0794..0e1fe2757f 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 29eb150bda..58816babde 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 8fea6d9abd..847a8666a7 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 7921c5a2a1..bbcb538a58 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 d303ab85b2..62dafa7bf8 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 66d0484015..64062ba1b3 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 462ad1ba28..bcd793076a 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 5d750d840e..a87490a7cc 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 0c7fdd553f..1e3dd85964 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 133f01beec..246a3dde8e 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 2949839ca9..7e58795398 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 9ecb8a3f81..4e9bdf0e8f 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 ac59b6ad23..e7fc76dde9 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 fc4d4386aa..7442cdc304 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 eb679ef747..fcdf9e6b79 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 8352c46faf..c775760e7c 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 4b98cc48d9..91f5aa6196 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 65258255cf..ad3810a7e9 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 7a20d8c1e7..d9d485971e 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 bde10404b2..e033f50908 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 ec7adea6fb..be77975b7e 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 ceec401a21..f1ea9e99c7 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 68e98941c7..b86a24ebe4 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 55b2fec7c4..ff0a8526e3 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 faa806b7b6..549f3b7afa 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 75109ddf76..8cbd1c316b 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 2e12f210bb..e474262d9a 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 1285d5bdab..55a4b44f13 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 c2292a14be..f2aa1e1bf2 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 27d131b48c..f8ab99161b 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 485e6f2560..915ca5766e 100644 Binary files a/panda/board/obj/panda_h7_tesla_wake.bin.signed and b/panda/board/obj/panda_h7_tesla_wake.bin.signed differ diff --git a/panda/board/obj/panda_h7_tesla_wake/bootstub.elf b/panda/board/obj/panda_h7_tesla_wake/bootstub.elf index c855e75ce4..cfbd21bf89 100755 Binary files a/panda/board/obj/panda_h7_tesla_wake/bootstub.elf and b/panda/board/obj/panda_h7_tesla_wake/bootstub.elf differ diff --git a/panda/board/obj/panda_h7_tesla_wake/main.bin b/panda/board/obj/panda_h7_tesla_wake/main.bin index db06065cb6..01f7b10c41 100755 Binary files a/panda/board/obj/panda_h7_tesla_wake/main.bin and b/panda/board/obj/panda_h7_tesla_wake/main.bin differ diff --git a/panda/board/obj/panda_h7_tesla_wake/main.elf b/panda/board/obj/panda_h7_tesla_wake/main.elf index cf9fbfc017..a98bffded2 100755 Binary files a/panda/board/obj/panda_h7_tesla_wake/main.elf and b/panda/board/obj/panda_h7_tesla_wake/main.elf differ diff --git a/panda/board/obj/panda_h7_tesla_wake_can_ignition_only.bin.signed b/panda/board/obj/panda_h7_tesla_wake_can_ignition_only.bin.signed index 2ae7797478..e2fecb8ce3 100644 Binary files a/panda/board/obj/panda_h7_tesla_wake_can_ignition_only.bin.signed and b/panda/board/obj/panda_h7_tesla_wake_can_ignition_only.bin.signed differ diff --git a/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/bootstub.elf b/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/bootstub.elf index dbd039e5a2..1f5a64c29b 100755 Binary files a/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/bootstub.elf and b/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/bootstub.elf differ diff --git a/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/main.bin b/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/main.bin index a7fb3ff752..c381bf9606 100755 Binary files a/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/main.bin and b/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/main.bin differ diff --git a/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/main.elf b/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/main.elf index 793c115a0f..653e6c4583 100755 Binary files a/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/main.elf and b/panda/board/obj/panda_h7_tesla_wake_can_ignition_only/main.elf differ diff --git a/panda/board/obj/panda_hkg_remote.bin.signed b/panda/board/obj/panda_hkg_remote.bin.signed index ee0b842c46..70fb0631b3 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 7fbb7f81dd..d51a1301ba 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 92397bed07..e06e2cce63 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 f85dedfa6f..8a1780c45b 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 5e3b36ad51..36540dc998 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 54103c68a2..a10ef182b0 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 982cca33cc..96e91a1893 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 e3b0d10582..d029d1a69b 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 d030ec6705..48a8cea6ee 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 a4623ef233..f9a19f73cc 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 6633227269..22f04b0170 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 b2a650d925..908d5151a5 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 8649076673..e46a39277d 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 8b96911113..2da2e16109 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 e145359590..96f22be746 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 acb46f6dbe..be5c84620d 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 8cfdb4acfc..eba8cb8cca 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 f25a015aa4..f279297a3e 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 b10778b7de..45c75b41ef 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 e22b61e585..9b727e9d95 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 b60f7a4b89..932537c157 100644 Binary files a/panda/board/obj/panda_tesla_wake.bin.signed and b/panda/board/obj/panda_tesla_wake.bin.signed differ diff --git a/panda/board/obj/panda_tesla_wake/bootstub.elf b/panda/board/obj/panda_tesla_wake/bootstub.elf index f8afbbbaf9..ca9e066ed8 100755 Binary files a/panda/board/obj/panda_tesla_wake/bootstub.elf and b/panda/board/obj/panda_tesla_wake/bootstub.elf differ diff --git a/panda/board/obj/panda_tesla_wake/main.bin b/panda/board/obj/panda_tesla_wake/main.bin index 9516e637af..07065c602f 100755 Binary files a/panda/board/obj/panda_tesla_wake/main.bin and b/panda/board/obj/panda_tesla_wake/main.bin differ diff --git a/panda/board/obj/panda_tesla_wake/main.elf b/panda/board/obj/panda_tesla_wake/main.elf index 839821faee..1e7bb1e98b 100755 Binary files a/panda/board/obj/panda_tesla_wake/main.elf and b/panda/board/obj/panda_tesla_wake/main.elf differ diff --git a/panda/board/obj/panda_tesla_wake_can_ignition_only.bin.signed b/panda/board/obj/panda_tesla_wake_can_ignition_only.bin.signed index ebfea0fd05..a9e171afb5 100644 Binary files a/panda/board/obj/panda_tesla_wake_can_ignition_only.bin.signed and b/panda/board/obj/panda_tesla_wake_can_ignition_only.bin.signed differ diff --git a/panda/board/obj/panda_tesla_wake_can_ignition_only/bootstub.elf b/panda/board/obj/panda_tesla_wake_can_ignition_only/bootstub.elf index fbf8850721..6b49822be3 100755 Binary files a/panda/board/obj/panda_tesla_wake_can_ignition_only/bootstub.elf and b/panda/board/obj/panda_tesla_wake_can_ignition_only/bootstub.elf differ diff --git a/panda/board/obj/panda_tesla_wake_can_ignition_only/main.bin b/panda/board/obj/panda_tesla_wake_can_ignition_only/main.bin index 81b595c13e..2ce07e993a 100755 Binary files a/panda/board/obj/panda_tesla_wake_can_ignition_only/main.bin and b/panda/board/obj/panda_tesla_wake_can_ignition_only/main.bin differ diff --git a/panda/board/obj/panda_tesla_wake_can_ignition_only/main.elf b/panda/board/obj/panda_tesla_wake_can_ignition_only/main.elf index fde67ed8fe..578b6a11b1 100755 Binary files a/panda/board/obj/panda_tesla_wake_can_ignition_only/main.elf and b/panda/board/obj/panda_tesla_wake_can_ignition_only/main.elf differ diff --git a/panda/board/obj/version b/panda/board/obj/version index e180d0f830..38e199e830 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-bf00f88b-DEBUG \ No newline at end of file +DEV-02446532-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index abf9f4c7ee..4bd56ce546 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_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 err_fun(double *nom_x, double *delta_x, double *out_8548372190097700555) { + out_8548372190097700555[0] = delta_x[0] + nom_x[0]; + out_8548372190097700555[1] = delta_x[1] + nom_x[1]; + out_8548372190097700555[2] = delta_x[2] + nom_x[2]; + out_8548372190097700555[3] = delta_x[3] + nom_x[3]; + out_8548372190097700555[4] = delta_x[4] + nom_x[4]; + out_8548372190097700555[5] = delta_x[5] + nom_x[5]; + out_8548372190097700555[6] = delta_x[6] + nom_x[6]; + out_8548372190097700555[7] = delta_x[7] + nom_x[7]; + out_8548372190097700555[8] = delta_x[8] + nom_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 inv_err_fun(double *nom_x, double *true_x, double *out_6257373597812199292) { + out_6257373597812199292[0] = -nom_x[0] + true_x[0]; + out_6257373597812199292[1] = -nom_x[1] + true_x[1]; + out_6257373597812199292[2] = -nom_x[2] + true_x[2]; + out_6257373597812199292[3] = -nom_x[3] + true_x[3]; + out_6257373597812199292[4] = -nom_x[4] + true_x[4]; + out_6257373597812199292[5] = -nom_x[5] + true_x[5]; + out_6257373597812199292[6] = -nom_x[6] + true_x[6]; + out_6257373597812199292[7] = -nom_x[7] + true_x[7]; + out_6257373597812199292[8] = -nom_x[8] + true_x[8]; } -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 H_mod_fun(double *state, double *out_7008429191596952258) { + out_7008429191596952258[0] = 1.0; + out_7008429191596952258[1] = 0.0; + out_7008429191596952258[2] = 0.0; + out_7008429191596952258[3] = 0.0; + out_7008429191596952258[4] = 0.0; + out_7008429191596952258[5] = 0.0; + out_7008429191596952258[6] = 0.0; + out_7008429191596952258[7] = 0.0; + out_7008429191596952258[8] = 0.0; + out_7008429191596952258[9] = 0.0; + out_7008429191596952258[10] = 1.0; + out_7008429191596952258[11] = 0.0; + out_7008429191596952258[12] = 0.0; + out_7008429191596952258[13] = 0.0; + out_7008429191596952258[14] = 0.0; + out_7008429191596952258[15] = 0.0; + out_7008429191596952258[16] = 0.0; + out_7008429191596952258[17] = 0.0; + out_7008429191596952258[18] = 0.0; + out_7008429191596952258[19] = 0.0; + out_7008429191596952258[20] = 1.0; + out_7008429191596952258[21] = 0.0; + out_7008429191596952258[22] = 0.0; + out_7008429191596952258[23] = 0.0; + out_7008429191596952258[24] = 0.0; + out_7008429191596952258[25] = 0.0; + out_7008429191596952258[26] = 0.0; + out_7008429191596952258[27] = 0.0; + out_7008429191596952258[28] = 0.0; + out_7008429191596952258[29] = 0.0; + out_7008429191596952258[30] = 1.0; + out_7008429191596952258[31] = 0.0; + out_7008429191596952258[32] = 0.0; + out_7008429191596952258[33] = 0.0; + out_7008429191596952258[34] = 0.0; + out_7008429191596952258[35] = 0.0; + out_7008429191596952258[36] = 0.0; + out_7008429191596952258[37] = 0.0; + out_7008429191596952258[38] = 0.0; + out_7008429191596952258[39] = 0.0; + out_7008429191596952258[40] = 1.0; + out_7008429191596952258[41] = 0.0; + out_7008429191596952258[42] = 0.0; + out_7008429191596952258[43] = 0.0; + out_7008429191596952258[44] = 0.0; + out_7008429191596952258[45] = 0.0; + out_7008429191596952258[46] = 0.0; + out_7008429191596952258[47] = 0.0; + out_7008429191596952258[48] = 0.0; + out_7008429191596952258[49] = 0.0; + out_7008429191596952258[50] = 1.0; + out_7008429191596952258[51] = 0.0; + out_7008429191596952258[52] = 0.0; + out_7008429191596952258[53] = 0.0; + out_7008429191596952258[54] = 0.0; + out_7008429191596952258[55] = 0.0; + out_7008429191596952258[56] = 0.0; + out_7008429191596952258[57] = 0.0; + out_7008429191596952258[58] = 0.0; + out_7008429191596952258[59] = 0.0; + out_7008429191596952258[60] = 1.0; + out_7008429191596952258[61] = 0.0; + out_7008429191596952258[62] = 0.0; + out_7008429191596952258[63] = 0.0; + out_7008429191596952258[64] = 0.0; + out_7008429191596952258[65] = 0.0; + out_7008429191596952258[66] = 0.0; + out_7008429191596952258[67] = 0.0; + out_7008429191596952258[68] = 0.0; + out_7008429191596952258[69] = 0.0; + out_7008429191596952258[70] = 1.0; + out_7008429191596952258[71] = 0.0; + out_7008429191596952258[72] = 0.0; + out_7008429191596952258[73] = 0.0; + out_7008429191596952258[74] = 0.0; + out_7008429191596952258[75] = 0.0; + out_7008429191596952258[76] = 0.0; + out_7008429191596952258[77] = 0.0; + out_7008429191596952258[78] = 0.0; + out_7008429191596952258[79] = 0.0; + out_7008429191596952258[80] = 1.0; } -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_211764458208077354) { + out_211764458208077354[0] = state[0]; + out_211764458208077354[1] = state[1]; + out_211764458208077354[2] = state[2]; + out_211764458208077354[3] = state[3]; + out_211764458208077354[4] = state[4]; + out_211764458208077354[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_211764458208077354[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_211764458208077354[7] = state[7]; + out_211764458208077354[8] = state[8]; } -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 F_fun(double *state, double dt, double *out_6789911951967766610) { + out_6789911951967766610[0] = 1; + out_6789911951967766610[1] = 0; + out_6789911951967766610[2] = 0; + out_6789911951967766610[3] = 0; + out_6789911951967766610[4] = 0; + out_6789911951967766610[5] = 0; + out_6789911951967766610[6] = 0; + out_6789911951967766610[7] = 0; + out_6789911951967766610[8] = 0; + out_6789911951967766610[9] = 0; + out_6789911951967766610[10] = 1; + out_6789911951967766610[11] = 0; + out_6789911951967766610[12] = 0; + out_6789911951967766610[13] = 0; + out_6789911951967766610[14] = 0; + out_6789911951967766610[15] = 0; + out_6789911951967766610[16] = 0; + out_6789911951967766610[17] = 0; + out_6789911951967766610[18] = 0; + out_6789911951967766610[19] = 0; + out_6789911951967766610[20] = 1; + out_6789911951967766610[21] = 0; + out_6789911951967766610[22] = 0; + out_6789911951967766610[23] = 0; + out_6789911951967766610[24] = 0; + out_6789911951967766610[25] = 0; + out_6789911951967766610[26] = 0; + out_6789911951967766610[27] = 0; + out_6789911951967766610[28] = 0; + out_6789911951967766610[29] = 0; + out_6789911951967766610[30] = 1; + out_6789911951967766610[31] = 0; + out_6789911951967766610[32] = 0; + out_6789911951967766610[33] = 0; + out_6789911951967766610[34] = 0; + out_6789911951967766610[35] = 0; + out_6789911951967766610[36] = 0; + out_6789911951967766610[37] = 0; + out_6789911951967766610[38] = 0; + out_6789911951967766610[39] = 0; + out_6789911951967766610[40] = 1; + out_6789911951967766610[41] = 0; + out_6789911951967766610[42] = 0; + out_6789911951967766610[43] = 0; + out_6789911951967766610[44] = 0; + out_6789911951967766610[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_6789911951967766610[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_6789911951967766610[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_6789911951967766610[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_6789911951967766610[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_6789911951967766610[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_6789911951967766610[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_6789911951967766610[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_6789911951967766610[53] = -9.8100000000000005*dt; + out_6789911951967766610[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_6789911951967766610[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_6789911951967766610[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_6789911951967766610[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_6789911951967766610[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_6789911951967766610[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_6789911951967766610[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_6789911951967766610[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_6789911951967766610[62] = 0; + out_6789911951967766610[63] = 0; + out_6789911951967766610[64] = 0; + out_6789911951967766610[65] = 0; + out_6789911951967766610[66] = 0; + out_6789911951967766610[67] = 0; + out_6789911951967766610[68] = 0; + out_6789911951967766610[69] = 0; + out_6789911951967766610[70] = 1; + out_6789911951967766610[71] = 0; + out_6789911951967766610[72] = 0; + out_6789911951967766610[73] = 0; + out_6789911951967766610[74] = 0; + out_6789911951967766610[75] = 0; + out_6789911951967766610[76] = 0; + out_6789911951967766610[77] = 0; + out_6789911951967766610[78] = 0; + out_6789911951967766610[79] = 0; + out_6789911951967766610[80] = 1; } -void h_25(double *state, double *unused, double *out_7662070615539319427) { - out_7662070615539319427[0] = state[6]; +void h_25(double *state, double *unused, double *out_7956251239264178232) { + out_7956251239264178232[0] = state[6]; } -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_25(double *state, double *unused, double *out_912908841376907547) { + out_912908841376907547[0] = 0; + out_912908841376907547[1] = 0; + out_912908841376907547[2] = 0; + out_912908841376907547[3] = 0; + out_912908841376907547[4] = 0; + out_912908841376907547[5] = 0; + out_912908841376907547[6] = 1; + out_912908841376907547[7] = 0; + out_912908841376907547[8] = 0; } -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_7723154835838931558) { + out_7723154835838931558[0] = state[4]; + out_7723154835838931558[1] = state[5]; } -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_24(double *state, double *unused, double *out_2501382670766146913) { + out_2501382670766146913[0] = 0; + out_2501382670766146913[1] = 0; + out_2501382670766146913[2] = 0; + out_2501382670766146913[3] = 0; + out_2501382670766146913[4] = 1; + out_2501382670766146913[5] = 0; + out_2501382670766146913[6] = 0; + out_2501382670766146913[7] = 0; + out_2501382670766146913[8] = 0; + out_2501382670766146913[9] = 0; + out_2501382670766146913[10] = 0; + out_2501382670766146913[11] = 0; + out_2501382670766146913[12] = 0; + out_2501382670766146913[13] = 0; + out_2501382670766146913[14] = 1; + out_2501382670766146913[15] = 0; + out_2501382670766146913[16] = 0; + out_2501382670766146913[17] = 0; } -void h_30(double *state, double *unused, double *out_9064035073689776395) { - out_9064035073689776395[0] = state[4]; +void h_30(double *state, double *unused, double *out_4094567762835676580) { + out_4094567762835676580[0] = state[4]; } -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_30(double *state, double *unused, double *out_3614787488750700651) { + out_3614787488750700651[0] = 0; + out_3614787488750700651[1] = 0; + out_3614787488750700651[2] = 0; + out_3614787488750700651[3] = 0; + out_3614787488750700651[4] = 1; + out_3614787488750700651[5] = 0; + out_3614787488750700651[6] = 0; + out_3614787488750700651[7] = 0; + out_3614787488750700651[8] = 0; } -void h_26(double *state, double *unused, double *out_1162711888116392193) { - out_1162711888116392193[0] = state[7]; +void h_26(double *state, double *unused, double *out_4086855108634070771) { + out_4086855108634070771[0] = state[7]; } -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_26(double *state, double *unused, double *out_2828594477497148677) { + out_2828594477497148677[0] = 0; + out_2828594477497148677[1] = 0; + out_2828594477497148677[2] = 0; + out_2828594477497148677[3] = 0; + out_2828594477497148677[4] = 0; + out_2828594477497148677[5] = 0; + out_2828594477497148677[6] = 0; + out_2828594477497148677[7] = 1; + out_2828594477497148677[8] = 0; } -void h_27(double *state, double *unused, double *out_4113321920138957337) { - out_4113321920138957337[0] = state[3]; +void h_27(double *state, double *unused, double *out_3279583348191151873) { + out_3279583348191151873[0] = state[3]; } -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_27(double *state, double *unused, double *out_5789550800551125562) { + out_5789550800551125562[0] = 0; + out_5789550800551125562[1] = 0; + out_5789550800551125562[2] = 0; + out_5789550800551125562[3] = 1; + out_5789550800551125562[4] = 0; + out_5789550800551125562[5] = 0; + out_5789550800551125562[6] = 0; + out_5789550800551125562[7] = 0; + out_5789550800551125562[8] = 0; } -void h_29(double *state, double *unused, double *out_7215941399213971055) { - out_7215941399213971055[0] = state[1]; +void h_29(double *state, double *unused, double *out_3491251878159199063) { + out_3491251878159199063[0] = state[1]; } -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_29(double *state, double *unused, double *out_3104556144436308467) { + out_3104556144436308467[0] = 0; + out_3104556144436308467[1] = 1; + out_3104556144436308467[2] = 0; + out_3104556144436308467[3] = 0; + out_3104556144436308467[4] = 0; + out_3104556144436308467[5] = 0; + out_3104556144436308467[6] = 0; + out_3104556144436308467[7] = 0; + out_3104556144436308467[8] = 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_2111839547115010136) { + out_2111839547115010136[0] = state[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_28(double *state, double *unused, double *out_1140925872870982216) { + out_1140925872870982216[0] = 1; + out_1140925872870982216[1] = 0; + out_1140925872870982216[2] = 0; + out_1140925872870982216[3] = 0; + out_1140925872870982216[4] = 0; + out_1140925872870982216[5] = 0; + out_1140925872870982216[6] = 0; + out_1140925872870982216[7] = 0; + out_1140925872870982216[8] = 0; } -void h_31(double *state, double *unused, double *out_5470065409481246982) { - out_5470065409481246982[0] = state[8]; +void h_31(double *state, double *unused, double *out_271458526840810605) { + out_271458526840810605[0] = state[8]; } -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; +void H_31(double *state, double *unused, double *out_3454802579730500153) { + out_3454802579730500153[0] = 0; + out_3454802579730500153[1] = 0; + out_3454802579730500153[2] = 0; + out_3454802579730500153[3] = 0; + out_3454802579730500153[4] = 0; + out_3454802579730500153[5] = 0; + out_3454802579730500153[6] = 0; + out_3454802579730500153[7] = 0; + out_3454802579730500153[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_1326346818289980211) { - err_fun(nom_x, delta_x, out_1326346818289980211); +void car_err_fun(double *nom_x, double *delta_x, double *out_8548372190097700555) { + err_fun(nom_x, delta_x, out_8548372190097700555); } -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_inv_err_fun(double *nom_x, double *true_x, double *out_6257373597812199292) { + inv_err_fun(nom_x, true_x, out_6257373597812199292); } -void car_H_mod_fun(double *state, double *out_8221305666287256291) { - H_mod_fun(state, out_8221305666287256291); +void car_H_mod_fun(double *state, double *out_7008429191596952258) { + H_mod_fun(state, out_7008429191596952258); } -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_211764458208077354) { + f_fun(state, dt, out_211764458208077354); } -void car_F_fun(double *state, double dt, double *out_8878866990102607558) { - F_fun(state, dt, out_8878866990102607558); +void car_F_fun(double *state, double dt, double *out_6789911951967766610) { + F_fun(state, dt, out_6789911951967766610); } -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_7956251239264178232) { + h_25(state, unused, out_7956251239264178232); } -void car_H_25(double *state, double *unused, double *out_6916966556095284671) { - H_25(state, unused, out_6916966556095284671); +void car_H_25(double *state, double *unused, double *out_912908841376907547) { + H_25(state, unused, out_912908841376907547); } -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_7723154835838931558) { + h_24(state, unused, out_7723154835838931558); } -void car_H_24(double *state, double *unused, double *out_5718391886591489980) { - H_24(state, unused, out_5718391886591489980); +void car_H_24(double *state, double *unused, double *out_2501382670766146913) { + H_24(state, unused, out_2501382670766146913); } -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_4094567762835676580) { + h_30(state, unused, out_4094567762835676580); } -void car_H_30(double *state, double *unused, double *out_2389270225967676473) { - H_30(state, unused, out_2389270225967676473); +void car_H_30(double *state, double *unused, double *out_3614787488750700651) { + H_30(state, unused, out_3614787488750700651); } -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_4086855108634070771) { + h_26(state, unused, out_4086855108634070771); } -void car_H_26(double *state, double *unused, double *out_3175463237221228447) { - H_26(state, unused, out_3175463237221228447); +void car_H_26(double *state, double *unused, double *out_2828594477497148677) { + H_26(state, unused, out_2828594477497148677); } -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_3279583348191151873) { + h_27(state, unused, out_3279583348191151873); } -void car_H_27(double *state, double *unused, double *out_4612864297151619690) { - H_27(state, unused, out_4612864297151619690); +void car_H_27(double *state, double *unused, double *out_5789550800551125562) { + H_27(state, unused, out_5789550800551125562); } -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_3491251878159199063) { + h_29(state, unused, out_3491251878159199063); } -void car_H_29(double *state, double *unused, double *out_2899501570282068657) { - H_29(state, unused, out_2899501570282068657); +void car_H_29(double *state, double *unused, double *out_3104556144436308467) { + H_29(state, unused, out_3104556144436308467); } -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_2111839547115010136) { + h_28(state, unused, out_2111839547115010136); } -void car_H_28(double *state, double *unused, double *out_2182897446787461917) { - H_28(state, unused, out_2182897446787461917); +void car_H_28(double *state, double *unused, double *out_1140925872870982216) { + H_28(state, unused, out_1140925872870982216); } -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_271458526840810605) { + h_31(state, unused, out_271458526840810605); } -void car_H_31(double *state, double *unused, double *out_6947612517972245099) { - H_31(state, unused, out_6947612517972245099); +void car_H_31(double *state, double *unused, double *out_3454802579730500153) { + H_31(state, unused, out_3454802579730500153); } 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 07655929ea..d04432b09b 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_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_err_fun(double *nom_x, double *delta_x, double *out_8548372190097700555); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_6257373597812199292); +void car_H_mod_fun(double *state, double *out_7008429191596952258); +void car_f_fun(double *state, double dt, double *out_211764458208077354); +void car_F_fun(double *state, double dt, double *out_6789911951967766610); +void car_h_25(double *state, double *unused, double *out_7956251239264178232); +void car_H_25(double *state, double *unused, double *out_912908841376907547); +void car_h_24(double *state, double *unused, double *out_7723154835838931558); +void car_H_24(double *state, double *unused, double *out_2501382670766146913); +void car_h_30(double *state, double *unused, double *out_4094567762835676580); +void car_H_30(double *state, double *unused, double *out_3614787488750700651); +void car_h_26(double *state, double *unused, double *out_4086855108634070771); +void car_H_26(double *state, double *unused, double *out_2828594477497148677); +void car_h_27(double *state, double *unused, double *out_3279583348191151873); +void car_H_27(double *state, double *unused, double *out_5789550800551125562); +void car_h_29(double *state, double *unused, double *out_3491251878159199063); +void car_H_29(double *state, double *unused, double *out_3104556144436308467); +void car_h_28(double *state, double *unused, double *out_2111839547115010136); +void car_H_28(double *state, double *unused, double *out_1140925872870982216); +void car_h_31(double *state, double *unused, double *out_271458526840810605); +void car_H_31(double *state, double *unused, double *out_3454802579730500153); 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 0d814de584..d1c2edfaad 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_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 err_fun(double *nom_x, double *delta_x, double *out_7911616955151800495) { + out_7911616955151800495[0] = delta_x[0] + nom_x[0]; + out_7911616955151800495[1] = delta_x[1] + nom_x[1]; + out_7911616955151800495[2] = delta_x[2] + nom_x[2]; + out_7911616955151800495[3] = delta_x[3] + nom_x[3]; + out_7911616955151800495[4] = delta_x[4] + nom_x[4]; + out_7911616955151800495[5] = delta_x[5] + nom_x[5]; + out_7911616955151800495[6] = delta_x[6] + nom_x[6]; + out_7911616955151800495[7] = delta_x[7] + nom_x[7]; + out_7911616955151800495[8] = delta_x[8] + nom_x[8]; + out_7911616955151800495[9] = delta_x[9] + nom_x[9]; + out_7911616955151800495[10] = delta_x[10] + nom_x[10]; + out_7911616955151800495[11] = delta_x[11] + nom_x[11]; + out_7911616955151800495[12] = delta_x[12] + nom_x[12]; + out_7911616955151800495[13] = delta_x[13] + nom_x[13]; + out_7911616955151800495[14] = delta_x[14] + nom_x[14]; + out_7911616955151800495[15] = delta_x[15] + nom_x[15]; + out_7911616955151800495[16] = delta_x[16] + nom_x[16]; + out_7911616955151800495[17] = delta_x[17] + nom_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 inv_err_fun(double *nom_x, double *true_x, double *out_7584171002491943517) { + out_7584171002491943517[0] = -nom_x[0] + true_x[0]; + out_7584171002491943517[1] = -nom_x[1] + true_x[1]; + out_7584171002491943517[2] = -nom_x[2] + true_x[2]; + out_7584171002491943517[3] = -nom_x[3] + true_x[3]; + out_7584171002491943517[4] = -nom_x[4] + true_x[4]; + out_7584171002491943517[5] = -nom_x[5] + true_x[5]; + out_7584171002491943517[6] = -nom_x[6] + true_x[6]; + out_7584171002491943517[7] = -nom_x[7] + true_x[7]; + out_7584171002491943517[8] = -nom_x[8] + true_x[8]; + out_7584171002491943517[9] = -nom_x[9] + true_x[9]; + out_7584171002491943517[10] = -nom_x[10] + true_x[10]; + out_7584171002491943517[11] = -nom_x[11] + true_x[11]; + out_7584171002491943517[12] = -nom_x[12] + true_x[12]; + out_7584171002491943517[13] = -nom_x[13] + true_x[13]; + out_7584171002491943517[14] = -nom_x[14] + true_x[14]; + out_7584171002491943517[15] = -nom_x[15] + true_x[15]; + out_7584171002491943517[16] = -nom_x[16] + true_x[16]; + out_7584171002491943517[17] = -nom_x[17] + true_x[17]; } -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 H_mod_fun(double *state, double *out_8208145951121679600) { + out_8208145951121679600[0] = 1.0; + out_8208145951121679600[1] = 0.0; + out_8208145951121679600[2] = 0.0; + out_8208145951121679600[3] = 0.0; + out_8208145951121679600[4] = 0.0; + out_8208145951121679600[5] = 0.0; + out_8208145951121679600[6] = 0.0; + out_8208145951121679600[7] = 0.0; + out_8208145951121679600[8] = 0.0; + out_8208145951121679600[9] = 0.0; + out_8208145951121679600[10] = 0.0; + out_8208145951121679600[11] = 0.0; + out_8208145951121679600[12] = 0.0; + out_8208145951121679600[13] = 0.0; + out_8208145951121679600[14] = 0.0; + out_8208145951121679600[15] = 0.0; + out_8208145951121679600[16] = 0.0; + out_8208145951121679600[17] = 0.0; + out_8208145951121679600[18] = 0.0; + out_8208145951121679600[19] = 1.0; + out_8208145951121679600[20] = 0.0; + out_8208145951121679600[21] = 0.0; + out_8208145951121679600[22] = 0.0; + out_8208145951121679600[23] = 0.0; + out_8208145951121679600[24] = 0.0; + out_8208145951121679600[25] = 0.0; + out_8208145951121679600[26] = 0.0; + out_8208145951121679600[27] = 0.0; + out_8208145951121679600[28] = 0.0; + out_8208145951121679600[29] = 0.0; + out_8208145951121679600[30] = 0.0; + out_8208145951121679600[31] = 0.0; + out_8208145951121679600[32] = 0.0; + out_8208145951121679600[33] = 0.0; + out_8208145951121679600[34] = 0.0; + out_8208145951121679600[35] = 0.0; + out_8208145951121679600[36] = 0.0; + out_8208145951121679600[37] = 0.0; + out_8208145951121679600[38] = 1.0; + out_8208145951121679600[39] = 0.0; + out_8208145951121679600[40] = 0.0; + out_8208145951121679600[41] = 0.0; + out_8208145951121679600[42] = 0.0; + out_8208145951121679600[43] = 0.0; + out_8208145951121679600[44] = 0.0; + out_8208145951121679600[45] = 0.0; + out_8208145951121679600[46] = 0.0; + out_8208145951121679600[47] = 0.0; + out_8208145951121679600[48] = 0.0; + out_8208145951121679600[49] = 0.0; + out_8208145951121679600[50] = 0.0; + out_8208145951121679600[51] = 0.0; + out_8208145951121679600[52] = 0.0; + out_8208145951121679600[53] = 0.0; + out_8208145951121679600[54] = 0.0; + out_8208145951121679600[55] = 0.0; + out_8208145951121679600[56] = 0.0; + out_8208145951121679600[57] = 1.0; + out_8208145951121679600[58] = 0.0; + out_8208145951121679600[59] = 0.0; + out_8208145951121679600[60] = 0.0; + out_8208145951121679600[61] = 0.0; + out_8208145951121679600[62] = 0.0; + out_8208145951121679600[63] = 0.0; + out_8208145951121679600[64] = 0.0; + out_8208145951121679600[65] = 0.0; + out_8208145951121679600[66] = 0.0; + out_8208145951121679600[67] = 0.0; + out_8208145951121679600[68] = 0.0; + out_8208145951121679600[69] = 0.0; + out_8208145951121679600[70] = 0.0; + out_8208145951121679600[71] = 0.0; + out_8208145951121679600[72] = 0.0; + out_8208145951121679600[73] = 0.0; + out_8208145951121679600[74] = 0.0; + out_8208145951121679600[75] = 0.0; + out_8208145951121679600[76] = 1.0; + out_8208145951121679600[77] = 0.0; + out_8208145951121679600[78] = 0.0; + out_8208145951121679600[79] = 0.0; + out_8208145951121679600[80] = 0.0; + out_8208145951121679600[81] = 0.0; + out_8208145951121679600[82] = 0.0; + out_8208145951121679600[83] = 0.0; + out_8208145951121679600[84] = 0.0; + out_8208145951121679600[85] = 0.0; + out_8208145951121679600[86] = 0.0; + out_8208145951121679600[87] = 0.0; + out_8208145951121679600[88] = 0.0; + out_8208145951121679600[89] = 0.0; + out_8208145951121679600[90] = 0.0; + out_8208145951121679600[91] = 0.0; + out_8208145951121679600[92] = 0.0; + out_8208145951121679600[93] = 0.0; + out_8208145951121679600[94] = 0.0; + out_8208145951121679600[95] = 1.0; + out_8208145951121679600[96] = 0.0; + out_8208145951121679600[97] = 0.0; + out_8208145951121679600[98] = 0.0; + out_8208145951121679600[99] = 0.0; + out_8208145951121679600[100] = 0.0; + out_8208145951121679600[101] = 0.0; + out_8208145951121679600[102] = 0.0; + out_8208145951121679600[103] = 0.0; + out_8208145951121679600[104] = 0.0; + out_8208145951121679600[105] = 0.0; + out_8208145951121679600[106] = 0.0; + out_8208145951121679600[107] = 0.0; + out_8208145951121679600[108] = 0.0; + out_8208145951121679600[109] = 0.0; + out_8208145951121679600[110] = 0.0; + out_8208145951121679600[111] = 0.0; + out_8208145951121679600[112] = 0.0; + out_8208145951121679600[113] = 0.0; + out_8208145951121679600[114] = 1.0; + out_8208145951121679600[115] = 0.0; + out_8208145951121679600[116] = 0.0; + out_8208145951121679600[117] = 0.0; + out_8208145951121679600[118] = 0.0; + out_8208145951121679600[119] = 0.0; + out_8208145951121679600[120] = 0.0; + out_8208145951121679600[121] = 0.0; + out_8208145951121679600[122] = 0.0; + out_8208145951121679600[123] = 0.0; + out_8208145951121679600[124] = 0.0; + out_8208145951121679600[125] = 0.0; + out_8208145951121679600[126] = 0.0; + out_8208145951121679600[127] = 0.0; + out_8208145951121679600[128] = 0.0; + out_8208145951121679600[129] = 0.0; + out_8208145951121679600[130] = 0.0; + out_8208145951121679600[131] = 0.0; + out_8208145951121679600[132] = 0.0; + out_8208145951121679600[133] = 1.0; + out_8208145951121679600[134] = 0.0; + out_8208145951121679600[135] = 0.0; + out_8208145951121679600[136] = 0.0; + out_8208145951121679600[137] = 0.0; + out_8208145951121679600[138] = 0.0; + out_8208145951121679600[139] = 0.0; + out_8208145951121679600[140] = 0.0; + out_8208145951121679600[141] = 0.0; + out_8208145951121679600[142] = 0.0; + out_8208145951121679600[143] = 0.0; + out_8208145951121679600[144] = 0.0; + out_8208145951121679600[145] = 0.0; + out_8208145951121679600[146] = 0.0; + out_8208145951121679600[147] = 0.0; + out_8208145951121679600[148] = 0.0; + out_8208145951121679600[149] = 0.0; + out_8208145951121679600[150] = 0.0; + out_8208145951121679600[151] = 0.0; + out_8208145951121679600[152] = 1.0; + out_8208145951121679600[153] = 0.0; + out_8208145951121679600[154] = 0.0; + out_8208145951121679600[155] = 0.0; + out_8208145951121679600[156] = 0.0; + out_8208145951121679600[157] = 0.0; + out_8208145951121679600[158] = 0.0; + out_8208145951121679600[159] = 0.0; + out_8208145951121679600[160] = 0.0; + out_8208145951121679600[161] = 0.0; + out_8208145951121679600[162] = 0.0; + out_8208145951121679600[163] = 0.0; + out_8208145951121679600[164] = 0.0; + out_8208145951121679600[165] = 0.0; + out_8208145951121679600[166] = 0.0; + out_8208145951121679600[167] = 0.0; + out_8208145951121679600[168] = 0.0; + out_8208145951121679600[169] = 0.0; + out_8208145951121679600[170] = 0.0; + out_8208145951121679600[171] = 1.0; + out_8208145951121679600[172] = 0.0; + out_8208145951121679600[173] = 0.0; + out_8208145951121679600[174] = 0.0; + out_8208145951121679600[175] = 0.0; + out_8208145951121679600[176] = 0.0; + out_8208145951121679600[177] = 0.0; + out_8208145951121679600[178] = 0.0; + out_8208145951121679600[179] = 0.0; + out_8208145951121679600[180] = 0.0; + out_8208145951121679600[181] = 0.0; + out_8208145951121679600[182] = 0.0; + out_8208145951121679600[183] = 0.0; + out_8208145951121679600[184] = 0.0; + out_8208145951121679600[185] = 0.0; + out_8208145951121679600[186] = 0.0; + out_8208145951121679600[187] = 0.0; + out_8208145951121679600[188] = 0.0; + out_8208145951121679600[189] = 0.0; + out_8208145951121679600[190] = 1.0; + out_8208145951121679600[191] = 0.0; + out_8208145951121679600[192] = 0.0; + out_8208145951121679600[193] = 0.0; + out_8208145951121679600[194] = 0.0; + out_8208145951121679600[195] = 0.0; + out_8208145951121679600[196] = 0.0; + out_8208145951121679600[197] = 0.0; + out_8208145951121679600[198] = 0.0; + out_8208145951121679600[199] = 0.0; + out_8208145951121679600[200] = 0.0; + out_8208145951121679600[201] = 0.0; + out_8208145951121679600[202] = 0.0; + out_8208145951121679600[203] = 0.0; + out_8208145951121679600[204] = 0.0; + out_8208145951121679600[205] = 0.0; + out_8208145951121679600[206] = 0.0; + out_8208145951121679600[207] = 0.0; + out_8208145951121679600[208] = 0.0; + out_8208145951121679600[209] = 1.0; + out_8208145951121679600[210] = 0.0; + out_8208145951121679600[211] = 0.0; + out_8208145951121679600[212] = 0.0; + out_8208145951121679600[213] = 0.0; + out_8208145951121679600[214] = 0.0; + out_8208145951121679600[215] = 0.0; + out_8208145951121679600[216] = 0.0; + out_8208145951121679600[217] = 0.0; + out_8208145951121679600[218] = 0.0; + out_8208145951121679600[219] = 0.0; + out_8208145951121679600[220] = 0.0; + out_8208145951121679600[221] = 0.0; + out_8208145951121679600[222] = 0.0; + out_8208145951121679600[223] = 0.0; + out_8208145951121679600[224] = 0.0; + out_8208145951121679600[225] = 0.0; + out_8208145951121679600[226] = 0.0; + out_8208145951121679600[227] = 0.0; + out_8208145951121679600[228] = 1.0; + out_8208145951121679600[229] = 0.0; + out_8208145951121679600[230] = 0.0; + out_8208145951121679600[231] = 0.0; + out_8208145951121679600[232] = 0.0; + out_8208145951121679600[233] = 0.0; + out_8208145951121679600[234] = 0.0; + out_8208145951121679600[235] = 0.0; + out_8208145951121679600[236] = 0.0; + out_8208145951121679600[237] = 0.0; + out_8208145951121679600[238] = 0.0; + out_8208145951121679600[239] = 0.0; + out_8208145951121679600[240] = 0.0; + out_8208145951121679600[241] = 0.0; + out_8208145951121679600[242] = 0.0; + out_8208145951121679600[243] = 0.0; + out_8208145951121679600[244] = 0.0; + out_8208145951121679600[245] = 0.0; + out_8208145951121679600[246] = 0.0; + out_8208145951121679600[247] = 1.0; + out_8208145951121679600[248] = 0.0; + out_8208145951121679600[249] = 0.0; + out_8208145951121679600[250] = 0.0; + out_8208145951121679600[251] = 0.0; + out_8208145951121679600[252] = 0.0; + out_8208145951121679600[253] = 0.0; + out_8208145951121679600[254] = 0.0; + out_8208145951121679600[255] = 0.0; + out_8208145951121679600[256] = 0.0; + out_8208145951121679600[257] = 0.0; + out_8208145951121679600[258] = 0.0; + out_8208145951121679600[259] = 0.0; + out_8208145951121679600[260] = 0.0; + out_8208145951121679600[261] = 0.0; + out_8208145951121679600[262] = 0.0; + out_8208145951121679600[263] = 0.0; + out_8208145951121679600[264] = 0.0; + out_8208145951121679600[265] = 0.0; + out_8208145951121679600[266] = 1.0; + out_8208145951121679600[267] = 0.0; + out_8208145951121679600[268] = 0.0; + out_8208145951121679600[269] = 0.0; + out_8208145951121679600[270] = 0.0; + out_8208145951121679600[271] = 0.0; + out_8208145951121679600[272] = 0.0; + out_8208145951121679600[273] = 0.0; + out_8208145951121679600[274] = 0.0; + out_8208145951121679600[275] = 0.0; + out_8208145951121679600[276] = 0.0; + out_8208145951121679600[277] = 0.0; + out_8208145951121679600[278] = 0.0; + out_8208145951121679600[279] = 0.0; + out_8208145951121679600[280] = 0.0; + out_8208145951121679600[281] = 0.0; + out_8208145951121679600[282] = 0.0; + out_8208145951121679600[283] = 0.0; + out_8208145951121679600[284] = 0.0; + out_8208145951121679600[285] = 1.0; + out_8208145951121679600[286] = 0.0; + out_8208145951121679600[287] = 0.0; + out_8208145951121679600[288] = 0.0; + out_8208145951121679600[289] = 0.0; + out_8208145951121679600[290] = 0.0; + out_8208145951121679600[291] = 0.0; + out_8208145951121679600[292] = 0.0; + out_8208145951121679600[293] = 0.0; + out_8208145951121679600[294] = 0.0; + out_8208145951121679600[295] = 0.0; + out_8208145951121679600[296] = 0.0; + out_8208145951121679600[297] = 0.0; + out_8208145951121679600[298] = 0.0; + out_8208145951121679600[299] = 0.0; + out_8208145951121679600[300] = 0.0; + out_8208145951121679600[301] = 0.0; + out_8208145951121679600[302] = 0.0; + out_8208145951121679600[303] = 0.0; + out_8208145951121679600[304] = 1.0; + out_8208145951121679600[305] = 0.0; + out_8208145951121679600[306] = 0.0; + out_8208145951121679600[307] = 0.0; + out_8208145951121679600[308] = 0.0; + out_8208145951121679600[309] = 0.0; + out_8208145951121679600[310] = 0.0; + out_8208145951121679600[311] = 0.0; + out_8208145951121679600[312] = 0.0; + out_8208145951121679600[313] = 0.0; + out_8208145951121679600[314] = 0.0; + out_8208145951121679600[315] = 0.0; + out_8208145951121679600[316] = 0.0; + out_8208145951121679600[317] = 0.0; + out_8208145951121679600[318] = 0.0; + out_8208145951121679600[319] = 0.0; + out_8208145951121679600[320] = 0.0; + out_8208145951121679600[321] = 0.0; + out_8208145951121679600[322] = 0.0; + out_8208145951121679600[323] = 1.0; } -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_7339808689618378881) { + out_7339808689618378881[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_7339808689618378881[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_7339808689618378881[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_7339808689618378881[3] = dt*state[12] + state[3]; + out_7339808689618378881[4] = dt*state[13] + state[4]; + out_7339808689618378881[5] = dt*state[14] + state[5]; + out_7339808689618378881[6] = state[6]; + out_7339808689618378881[7] = state[7]; + out_7339808689618378881[8] = state[8]; + out_7339808689618378881[9] = state[9]; + out_7339808689618378881[10] = state[10]; + out_7339808689618378881[11] = state[11]; + out_7339808689618378881[12] = state[12]; + out_7339808689618378881[13] = state[13]; + out_7339808689618378881[14] = state[14]; + out_7339808689618378881[15] = state[15]; + out_7339808689618378881[16] = state[16]; + out_7339808689618378881[17] = state[17]; } -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 F_fun(double *state, double dt, double *out_4299830908048765183) { + out_4299830908048765183[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_4299830908048765183[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_4299830908048765183[2] = 0; + out_4299830908048765183[3] = 0; + out_4299830908048765183[4] = 0; + out_4299830908048765183[5] = 0; + out_4299830908048765183[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_4299830908048765183[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_4299830908048765183[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_4299830908048765183[9] = 0; + out_4299830908048765183[10] = 0; + out_4299830908048765183[11] = 0; + out_4299830908048765183[12] = 0; + out_4299830908048765183[13] = 0; + out_4299830908048765183[14] = 0; + out_4299830908048765183[15] = 0; + out_4299830908048765183[16] = 0; + out_4299830908048765183[17] = 0; + out_4299830908048765183[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_4299830908048765183[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_4299830908048765183[20] = 0; + out_4299830908048765183[21] = 0; + out_4299830908048765183[22] = 0; + out_4299830908048765183[23] = 0; + out_4299830908048765183[24] = 0; + out_4299830908048765183[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_4299830908048765183[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_4299830908048765183[27] = 0; + out_4299830908048765183[28] = 0; + out_4299830908048765183[29] = 0; + out_4299830908048765183[30] = 0; + out_4299830908048765183[31] = 0; + out_4299830908048765183[32] = 0; + out_4299830908048765183[33] = 0; + out_4299830908048765183[34] = 0; + out_4299830908048765183[35] = 0; + out_4299830908048765183[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_4299830908048765183[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_4299830908048765183[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_4299830908048765183[39] = 0; + out_4299830908048765183[40] = 0; + out_4299830908048765183[41] = 0; + out_4299830908048765183[42] = 0; + out_4299830908048765183[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_4299830908048765183[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_4299830908048765183[45] = 0; + out_4299830908048765183[46] = 0; + out_4299830908048765183[47] = 0; + out_4299830908048765183[48] = 0; + out_4299830908048765183[49] = 0; + out_4299830908048765183[50] = 0; + out_4299830908048765183[51] = 0; + out_4299830908048765183[52] = 0; + out_4299830908048765183[53] = 0; + out_4299830908048765183[54] = 0; + out_4299830908048765183[55] = 0; + out_4299830908048765183[56] = 0; + out_4299830908048765183[57] = 1; + out_4299830908048765183[58] = 0; + out_4299830908048765183[59] = 0; + out_4299830908048765183[60] = 0; + out_4299830908048765183[61] = 0; + out_4299830908048765183[62] = 0; + out_4299830908048765183[63] = 0; + out_4299830908048765183[64] = 0; + out_4299830908048765183[65] = 0; + out_4299830908048765183[66] = dt; + out_4299830908048765183[67] = 0; + out_4299830908048765183[68] = 0; + out_4299830908048765183[69] = 0; + out_4299830908048765183[70] = 0; + out_4299830908048765183[71] = 0; + out_4299830908048765183[72] = 0; + out_4299830908048765183[73] = 0; + out_4299830908048765183[74] = 0; + out_4299830908048765183[75] = 0; + out_4299830908048765183[76] = 1; + out_4299830908048765183[77] = 0; + out_4299830908048765183[78] = 0; + out_4299830908048765183[79] = 0; + out_4299830908048765183[80] = 0; + out_4299830908048765183[81] = 0; + out_4299830908048765183[82] = 0; + out_4299830908048765183[83] = 0; + out_4299830908048765183[84] = 0; + out_4299830908048765183[85] = dt; + out_4299830908048765183[86] = 0; + out_4299830908048765183[87] = 0; + out_4299830908048765183[88] = 0; + out_4299830908048765183[89] = 0; + out_4299830908048765183[90] = 0; + out_4299830908048765183[91] = 0; + out_4299830908048765183[92] = 0; + out_4299830908048765183[93] = 0; + out_4299830908048765183[94] = 0; + out_4299830908048765183[95] = 1; + out_4299830908048765183[96] = 0; + out_4299830908048765183[97] = 0; + out_4299830908048765183[98] = 0; + out_4299830908048765183[99] = 0; + out_4299830908048765183[100] = 0; + out_4299830908048765183[101] = 0; + out_4299830908048765183[102] = 0; + out_4299830908048765183[103] = 0; + out_4299830908048765183[104] = dt; + out_4299830908048765183[105] = 0; + out_4299830908048765183[106] = 0; + out_4299830908048765183[107] = 0; + out_4299830908048765183[108] = 0; + out_4299830908048765183[109] = 0; + out_4299830908048765183[110] = 0; + out_4299830908048765183[111] = 0; + out_4299830908048765183[112] = 0; + out_4299830908048765183[113] = 0; + out_4299830908048765183[114] = 1; + out_4299830908048765183[115] = 0; + out_4299830908048765183[116] = 0; + out_4299830908048765183[117] = 0; + out_4299830908048765183[118] = 0; + out_4299830908048765183[119] = 0; + out_4299830908048765183[120] = 0; + out_4299830908048765183[121] = 0; + out_4299830908048765183[122] = 0; + out_4299830908048765183[123] = 0; + out_4299830908048765183[124] = 0; + out_4299830908048765183[125] = 0; + out_4299830908048765183[126] = 0; + out_4299830908048765183[127] = 0; + out_4299830908048765183[128] = 0; + out_4299830908048765183[129] = 0; + out_4299830908048765183[130] = 0; + out_4299830908048765183[131] = 0; + out_4299830908048765183[132] = 0; + out_4299830908048765183[133] = 1; + out_4299830908048765183[134] = 0; + out_4299830908048765183[135] = 0; + out_4299830908048765183[136] = 0; + out_4299830908048765183[137] = 0; + out_4299830908048765183[138] = 0; + out_4299830908048765183[139] = 0; + out_4299830908048765183[140] = 0; + out_4299830908048765183[141] = 0; + out_4299830908048765183[142] = 0; + out_4299830908048765183[143] = 0; + out_4299830908048765183[144] = 0; + out_4299830908048765183[145] = 0; + out_4299830908048765183[146] = 0; + out_4299830908048765183[147] = 0; + out_4299830908048765183[148] = 0; + out_4299830908048765183[149] = 0; + out_4299830908048765183[150] = 0; + out_4299830908048765183[151] = 0; + out_4299830908048765183[152] = 1; + out_4299830908048765183[153] = 0; + out_4299830908048765183[154] = 0; + out_4299830908048765183[155] = 0; + out_4299830908048765183[156] = 0; + out_4299830908048765183[157] = 0; + out_4299830908048765183[158] = 0; + out_4299830908048765183[159] = 0; + out_4299830908048765183[160] = 0; + out_4299830908048765183[161] = 0; + out_4299830908048765183[162] = 0; + out_4299830908048765183[163] = 0; + out_4299830908048765183[164] = 0; + out_4299830908048765183[165] = 0; + out_4299830908048765183[166] = 0; + out_4299830908048765183[167] = 0; + out_4299830908048765183[168] = 0; + out_4299830908048765183[169] = 0; + out_4299830908048765183[170] = 0; + out_4299830908048765183[171] = 1; + out_4299830908048765183[172] = 0; + out_4299830908048765183[173] = 0; + out_4299830908048765183[174] = 0; + out_4299830908048765183[175] = 0; + out_4299830908048765183[176] = 0; + out_4299830908048765183[177] = 0; + out_4299830908048765183[178] = 0; + out_4299830908048765183[179] = 0; + out_4299830908048765183[180] = 0; + out_4299830908048765183[181] = 0; + out_4299830908048765183[182] = 0; + out_4299830908048765183[183] = 0; + out_4299830908048765183[184] = 0; + out_4299830908048765183[185] = 0; + out_4299830908048765183[186] = 0; + out_4299830908048765183[187] = 0; + out_4299830908048765183[188] = 0; + out_4299830908048765183[189] = 0; + out_4299830908048765183[190] = 1; + out_4299830908048765183[191] = 0; + out_4299830908048765183[192] = 0; + out_4299830908048765183[193] = 0; + out_4299830908048765183[194] = 0; + out_4299830908048765183[195] = 0; + out_4299830908048765183[196] = 0; + out_4299830908048765183[197] = 0; + out_4299830908048765183[198] = 0; + out_4299830908048765183[199] = 0; + out_4299830908048765183[200] = 0; + out_4299830908048765183[201] = 0; + out_4299830908048765183[202] = 0; + out_4299830908048765183[203] = 0; + out_4299830908048765183[204] = 0; + out_4299830908048765183[205] = 0; + out_4299830908048765183[206] = 0; + out_4299830908048765183[207] = 0; + out_4299830908048765183[208] = 0; + out_4299830908048765183[209] = 1; + out_4299830908048765183[210] = 0; + out_4299830908048765183[211] = 0; + out_4299830908048765183[212] = 0; + out_4299830908048765183[213] = 0; + out_4299830908048765183[214] = 0; + out_4299830908048765183[215] = 0; + out_4299830908048765183[216] = 0; + out_4299830908048765183[217] = 0; + out_4299830908048765183[218] = 0; + out_4299830908048765183[219] = 0; + out_4299830908048765183[220] = 0; + out_4299830908048765183[221] = 0; + out_4299830908048765183[222] = 0; + out_4299830908048765183[223] = 0; + out_4299830908048765183[224] = 0; + out_4299830908048765183[225] = 0; + out_4299830908048765183[226] = 0; + out_4299830908048765183[227] = 0; + out_4299830908048765183[228] = 1; + out_4299830908048765183[229] = 0; + out_4299830908048765183[230] = 0; + out_4299830908048765183[231] = 0; + out_4299830908048765183[232] = 0; + out_4299830908048765183[233] = 0; + out_4299830908048765183[234] = 0; + out_4299830908048765183[235] = 0; + out_4299830908048765183[236] = 0; + out_4299830908048765183[237] = 0; + out_4299830908048765183[238] = 0; + out_4299830908048765183[239] = 0; + out_4299830908048765183[240] = 0; + out_4299830908048765183[241] = 0; + out_4299830908048765183[242] = 0; + out_4299830908048765183[243] = 0; + out_4299830908048765183[244] = 0; + out_4299830908048765183[245] = 0; + out_4299830908048765183[246] = 0; + out_4299830908048765183[247] = 1; + out_4299830908048765183[248] = 0; + out_4299830908048765183[249] = 0; + out_4299830908048765183[250] = 0; + out_4299830908048765183[251] = 0; + out_4299830908048765183[252] = 0; + out_4299830908048765183[253] = 0; + out_4299830908048765183[254] = 0; + out_4299830908048765183[255] = 0; + out_4299830908048765183[256] = 0; + out_4299830908048765183[257] = 0; + out_4299830908048765183[258] = 0; + out_4299830908048765183[259] = 0; + out_4299830908048765183[260] = 0; + out_4299830908048765183[261] = 0; + out_4299830908048765183[262] = 0; + out_4299830908048765183[263] = 0; + out_4299830908048765183[264] = 0; + out_4299830908048765183[265] = 0; + out_4299830908048765183[266] = 1; + out_4299830908048765183[267] = 0; + out_4299830908048765183[268] = 0; + out_4299830908048765183[269] = 0; + out_4299830908048765183[270] = 0; + out_4299830908048765183[271] = 0; + out_4299830908048765183[272] = 0; + out_4299830908048765183[273] = 0; + out_4299830908048765183[274] = 0; + out_4299830908048765183[275] = 0; + out_4299830908048765183[276] = 0; + out_4299830908048765183[277] = 0; + out_4299830908048765183[278] = 0; + out_4299830908048765183[279] = 0; + out_4299830908048765183[280] = 0; + out_4299830908048765183[281] = 0; + out_4299830908048765183[282] = 0; + out_4299830908048765183[283] = 0; + out_4299830908048765183[284] = 0; + out_4299830908048765183[285] = 1; + out_4299830908048765183[286] = 0; + out_4299830908048765183[287] = 0; + out_4299830908048765183[288] = 0; + out_4299830908048765183[289] = 0; + out_4299830908048765183[290] = 0; + out_4299830908048765183[291] = 0; + out_4299830908048765183[292] = 0; + out_4299830908048765183[293] = 0; + out_4299830908048765183[294] = 0; + out_4299830908048765183[295] = 0; + out_4299830908048765183[296] = 0; + out_4299830908048765183[297] = 0; + out_4299830908048765183[298] = 0; + out_4299830908048765183[299] = 0; + out_4299830908048765183[300] = 0; + out_4299830908048765183[301] = 0; + out_4299830908048765183[302] = 0; + out_4299830908048765183[303] = 0; + out_4299830908048765183[304] = 1; + out_4299830908048765183[305] = 0; + out_4299830908048765183[306] = 0; + out_4299830908048765183[307] = 0; + out_4299830908048765183[308] = 0; + out_4299830908048765183[309] = 0; + out_4299830908048765183[310] = 0; + out_4299830908048765183[311] = 0; + out_4299830908048765183[312] = 0; + out_4299830908048765183[313] = 0; + out_4299830908048765183[314] = 0; + out_4299830908048765183[315] = 0; + out_4299830908048765183[316] = 0; + out_4299830908048765183[317] = 0; + out_4299830908048765183[318] = 0; + out_4299830908048765183[319] = 0; + out_4299830908048765183[320] = 0; + out_4299830908048765183[321] = 0; + out_4299830908048765183[322] = 0; + out_4299830908048765183[323] = 1; } -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_3838924425177329508) { + out_3838924425177329508[0] = state[6] + state[9]; + out_3838924425177329508[1] = state[7] + state[10]; + out_3838924425177329508[2] = state[8] + state[11]; } -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_4(double *state, double *unused, double *out_6964376043796922763) { + out_6964376043796922763[0] = 0; + out_6964376043796922763[1] = 0; + out_6964376043796922763[2] = 0; + out_6964376043796922763[3] = 0; + out_6964376043796922763[4] = 0; + out_6964376043796922763[5] = 0; + out_6964376043796922763[6] = 1; + out_6964376043796922763[7] = 0; + out_6964376043796922763[8] = 0; + out_6964376043796922763[9] = 1; + out_6964376043796922763[10] = 0; + out_6964376043796922763[11] = 0; + out_6964376043796922763[12] = 0; + out_6964376043796922763[13] = 0; + out_6964376043796922763[14] = 0; + out_6964376043796922763[15] = 0; + out_6964376043796922763[16] = 0; + out_6964376043796922763[17] = 0; + out_6964376043796922763[18] = 0; + out_6964376043796922763[19] = 0; + out_6964376043796922763[20] = 0; + out_6964376043796922763[21] = 0; + out_6964376043796922763[22] = 0; + out_6964376043796922763[23] = 0; + out_6964376043796922763[24] = 0; + out_6964376043796922763[25] = 1; + out_6964376043796922763[26] = 0; + out_6964376043796922763[27] = 0; + out_6964376043796922763[28] = 1; + out_6964376043796922763[29] = 0; + out_6964376043796922763[30] = 0; + out_6964376043796922763[31] = 0; + out_6964376043796922763[32] = 0; + out_6964376043796922763[33] = 0; + out_6964376043796922763[34] = 0; + out_6964376043796922763[35] = 0; + out_6964376043796922763[36] = 0; + out_6964376043796922763[37] = 0; + out_6964376043796922763[38] = 0; + out_6964376043796922763[39] = 0; + out_6964376043796922763[40] = 0; + out_6964376043796922763[41] = 0; + out_6964376043796922763[42] = 0; + out_6964376043796922763[43] = 0; + out_6964376043796922763[44] = 1; + out_6964376043796922763[45] = 0; + out_6964376043796922763[46] = 0; + out_6964376043796922763[47] = 1; + out_6964376043796922763[48] = 0; + out_6964376043796922763[49] = 0; + out_6964376043796922763[50] = 0; + out_6964376043796922763[51] = 0; + out_6964376043796922763[52] = 0; + out_6964376043796922763[53] = 0; } -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_4478443536349725461) { + out_4478443536349725461[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_4478443536349725461[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_4478443536349725461[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_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_10(double *state, double *unused, double *out_8208658119783990230) { + out_8208658119783990230[0] = 0; + out_8208658119783990230[1] = 9.8100000000000005*cos(state[1]); + out_8208658119783990230[2] = 0; + out_8208658119783990230[3] = 0; + out_8208658119783990230[4] = -state[8]; + out_8208658119783990230[5] = state[7]; + out_8208658119783990230[6] = 0; + out_8208658119783990230[7] = state[5]; + out_8208658119783990230[8] = -state[4]; + out_8208658119783990230[9] = 0; + out_8208658119783990230[10] = 0; + out_8208658119783990230[11] = 0; + out_8208658119783990230[12] = 1; + out_8208658119783990230[13] = 0; + out_8208658119783990230[14] = 0; + out_8208658119783990230[15] = 1; + out_8208658119783990230[16] = 0; + out_8208658119783990230[17] = 0; + out_8208658119783990230[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_8208658119783990230[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_8208658119783990230[20] = 0; + out_8208658119783990230[21] = state[8]; + out_8208658119783990230[22] = 0; + out_8208658119783990230[23] = -state[6]; + out_8208658119783990230[24] = -state[5]; + out_8208658119783990230[25] = 0; + out_8208658119783990230[26] = state[3]; + out_8208658119783990230[27] = 0; + out_8208658119783990230[28] = 0; + out_8208658119783990230[29] = 0; + out_8208658119783990230[30] = 0; + out_8208658119783990230[31] = 1; + out_8208658119783990230[32] = 0; + out_8208658119783990230[33] = 0; + out_8208658119783990230[34] = 1; + out_8208658119783990230[35] = 0; + out_8208658119783990230[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_8208658119783990230[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_8208658119783990230[38] = 0; + out_8208658119783990230[39] = -state[7]; + out_8208658119783990230[40] = state[6]; + out_8208658119783990230[41] = 0; + out_8208658119783990230[42] = state[4]; + out_8208658119783990230[43] = -state[3]; + out_8208658119783990230[44] = 0; + out_8208658119783990230[45] = 0; + out_8208658119783990230[46] = 0; + out_8208658119783990230[47] = 0; + out_8208658119783990230[48] = 0; + out_8208658119783990230[49] = 0; + out_8208658119783990230[50] = 1; + out_8208658119783990230[51] = 0; + out_8208658119783990230[52] = 0; + out_8208658119783990230[53] = 1; } -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_349204714200215709) { + out_349204714200215709[0] = state[3]; + out_349204714200215709[1] = state[4]; + out_349204714200215709[2] = state[5]; } -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_13(double *state, double *unused, double *out_8270094204580296052) { + out_8270094204580296052[0] = 0; + out_8270094204580296052[1] = 0; + out_8270094204580296052[2] = 0; + out_8270094204580296052[3] = 1; + out_8270094204580296052[4] = 0; + out_8270094204580296052[5] = 0; + out_8270094204580296052[6] = 0; + out_8270094204580296052[7] = 0; + out_8270094204580296052[8] = 0; + out_8270094204580296052[9] = 0; + out_8270094204580296052[10] = 0; + out_8270094204580296052[11] = 0; + out_8270094204580296052[12] = 0; + out_8270094204580296052[13] = 0; + out_8270094204580296052[14] = 0; + out_8270094204580296052[15] = 0; + out_8270094204580296052[16] = 0; + out_8270094204580296052[17] = 0; + out_8270094204580296052[18] = 0; + out_8270094204580296052[19] = 0; + out_8270094204580296052[20] = 0; + out_8270094204580296052[21] = 0; + out_8270094204580296052[22] = 1; + out_8270094204580296052[23] = 0; + out_8270094204580296052[24] = 0; + out_8270094204580296052[25] = 0; + out_8270094204580296052[26] = 0; + out_8270094204580296052[27] = 0; + out_8270094204580296052[28] = 0; + out_8270094204580296052[29] = 0; + out_8270094204580296052[30] = 0; + out_8270094204580296052[31] = 0; + out_8270094204580296052[32] = 0; + out_8270094204580296052[33] = 0; + out_8270094204580296052[34] = 0; + out_8270094204580296052[35] = 0; + out_8270094204580296052[36] = 0; + out_8270094204580296052[37] = 0; + out_8270094204580296052[38] = 0; + out_8270094204580296052[39] = 0; + out_8270094204580296052[40] = 0; + out_8270094204580296052[41] = 1; + out_8270094204580296052[42] = 0; + out_8270094204580296052[43] = 0; + out_8270094204580296052[44] = 0; + out_8270094204580296052[45] = 0; + out_8270094204580296052[46] = 0; + out_8270094204580296052[47] = 0; + out_8270094204580296052[48] = 0; + out_8270094204580296052[49] = 0; + out_8270094204580296052[50] = 0; + out_8270094204580296052[51] = 0; + out_8270094204580296052[52] = 0; + out_8270094204580296052[53] = 0; } -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_5535558880524166264) { + out_5535558880524166264[0] = state[6]; + out_5535558880524166264[1] = state[7]; + out_5535558880524166264[2] = state[8]; } -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; +void H_14(double *state, double *unused, double *out_7519127173573144324) { + out_7519127173573144324[0] = 0; + out_7519127173573144324[1] = 0; + out_7519127173573144324[2] = 0; + out_7519127173573144324[3] = 0; + out_7519127173573144324[4] = 0; + out_7519127173573144324[5] = 0; + out_7519127173573144324[6] = 1; + out_7519127173573144324[7] = 0; + out_7519127173573144324[8] = 0; + out_7519127173573144324[9] = 0; + out_7519127173573144324[10] = 0; + out_7519127173573144324[11] = 0; + out_7519127173573144324[12] = 0; + out_7519127173573144324[13] = 0; + out_7519127173573144324[14] = 0; + out_7519127173573144324[15] = 0; + out_7519127173573144324[16] = 0; + out_7519127173573144324[17] = 0; + out_7519127173573144324[18] = 0; + out_7519127173573144324[19] = 0; + out_7519127173573144324[20] = 0; + out_7519127173573144324[21] = 0; + out_7519127173573144324[22] = 0; + out_7519127173573144324[23] = 0; + out_7519127173573144324[24] = 0; + out_7519127173573144324[25] = 1; + out_7519127173573144324[26] = 0; + out_7519127173573144324[27] = 0; + out_7519127173573144324[28] = 0; + out_7519127173573144324[29] = 0; + out_7519127173573144324[30] = 0; + out_7519127173573144324[31] = 0; + out_7519127173573144324[32] = 0; + out_7519127173573144324[33] = 0; + out_7519127173573144324[34] = 0; + out_7519127173573144324[35] = 0; + out_7519127173573144324[36] = 0; + out_7519127173573144324[37] = 0; + out_7519127173573144324[38] = 0; + out_7519127173573144324[39] = 0; + out_7519127173573144324[40] = 0; + out_7519127173573144324[41] = 0; + out_7519127173573144324[42] = 0; + out_7519127173573144324[43] = 0; + out_7519127173573144324[44] = 1; + out_7519127173573144324[45] = 0; + out_7519127173573144324[46] = 0; + out_7519127173573144324[47] = 0; + out_7519127173573144324[48] = 0; + out_7519127173573144324[49] = 0; + out_7519127173573144324[50] = 0; + out_7519127173573144324[51] = 0; + out_7519127173573144324[52] = 0; + out_7519127173573144324[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_4497386892446686963) { - err_fun(nom_x, delta_x, out_4497386892446686963); +void pose_err_fun(double *nom_x, double *delta_x, double *out_7911616955151800495) { + err_fun(nom_x, delta_x, out_7911616955151800495); } -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_inv_err_fun(double *nom_x, double *true_x, double *out_7584171002491943517) { + inv_err_fun(nom_x, true_x, out_7584171002491943517); } -void pose_H_mod_fun(double *state, double *out_163641472803010022) { - H_mod_fun(state, out_163641472803010022); +void pose_H_mod_fun(double *state, double *out_8208145951121679600) { + H_mod_fun(state, out_8208145951121679600); } -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_7339808689618378881) { + f_fun(state, dt, out_7339808689618378881); } -void pose_F_fun(double *state, double dt, double *out_4806322824000620331) { - F_fun(state, dt, out_4806322824000620331); +void pose_F_fun(double *state, double dt, double *out_4299830908048765183) { + F_fun(state, dt, out_4299830908048765183); } -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_3838924425177329508) { + h_4(state, unused, out_3838924425177329508); } -void pose_H_4(double *state, double *unused, double *out_9176625892716694514) { - H_4(state, unused, out_9176625892716694514); +void pose_H_4(double *state, double *unused, double *out_6964376043796922763) { + H_4(state, unused, out_6964376043796922763); } -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_4478443536349725461) { + h_10(state, unused, out_4478443536349725461); } -void pose_H_10(double *state, double *unused, double *out_6720236421065597274) { - H_10(state, unused, out_6720236421065597274); +void pose_H_10(double *state, double *unused, double *out_8208658119783990230) { + H_10(state, unused, out_8208658119783990230); } -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_349204714200215709) { + h_13(state, unused, out_349204714200215709); } -void pose_H_13(double *state, double *unused, double *out_6057844355660524301) { - H_13(state, unused, out_6057844355660524301); +void pose_H_13(double *state, double *unused, double *out_8270094204580296052) { + H_13(state, unused, out_8270094204580296052); } -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_5535558880524166264) { + h_14(state, unused, out_5535558880524166264); } -void pose_H_14(double *state, double *unused, double *out_5306877324653372573) { - H_14(state, unused, out_5306877324653372573); +void pose_H_14(double *state, double *unused, double *out_7519127173573144324) { + H_14(state, unused, out_7519127173573144324); } 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 6296363cfd..a61af5935a 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_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_err_fun(double *nom_x, double *delta_x, double *out_7911616955151800495); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_7584171002491943517); +void pose_H_mod_fun(double *state, double *out_8208145951121679600); +void pose_f_fun(double *state, double dt, double *out_7339808689618378881); +void pose_F_fun(double *state, double dt, double *out_4299830908048765183); +void pose_h_4(double *state, double *unused, double *out_3838924425177329508); +void pose_H_4(double *state, double *unused, double *out_6964376043796922763); +void pose_h_10(double *state, double *unused, double *out_4478443536349725461); +void pose_H_10(double *state, double *unused, double *out_8208658119783990230); +void pose_h_13(double *state, double *unused, double *out_349204714200215709); +void pose_H_13(double *state, double *unused, double *out_8270094204580296052); +void pose_h_14(double *state, double *unused, double *out_5535558880524166264); +void pose_H_14(double *state, double *unused, double *out_7519127173573144324); 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 a183220b4f..f57f27c547 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/system/camerad/camerad b/system/camerad/camerad index 84fec71d91..f6899bf972 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index 9bae73a4e0..f3c8985653 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index a63970579f..0f9d6a9040 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index aae84be89f..8c5d8a3739 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ