diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index bf8d60f24a..47bd3b8918 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 b75c65f8ea..e7082ae941 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 224d2963dd..6e908f00eb 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 e2c80679a1..45c9c9bb6e 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 3e8551ea7d..f362bdb8fb 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 8399cfb926..8205bf5c3b 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 8399cfb926..8205bf5c3b 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 1968ae0108..695050979d 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 1968ae0108..695050979d 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 1968ae0108..695050979d 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 1968ae0108..695050979d 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 1968ae0108..695050979d 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 1968ae0108..695050979d 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 1968ae0108..695050979d 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 1968ae0108..695050979d 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 8399cfb926..8205bf5c3b 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 8399cfb926..8205bf5c3b 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 999041cce5..0bcb84dd01 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 8399cfb926..8205bf5c3b 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 8399cfb926..8205bf5c3b 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 8399cfb926..8205bf5c3b 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 8399cfb926..8205bf5c3b 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 b3b2d01c58..2823b31224 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-02446532-DEBUG"; +const uint8_t gitversion[19] = "DEV-5bead815-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 56622f6f3f..c3d5aa89d1 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 3ddde010d3..ce629417e8 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 0e1fe2757f..a57cbb97c9 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 58816babde..df14e6a6b2 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 847a8666a7..a7246799f9 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 bbcb538a58..88f15af55e 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 62dafa7bf8..96d05c5a7c 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 64062ba1b3..6da9bdf243 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 bcd793076a..c87087796c 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 a87490a7cc..5dcd145ba9 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 1e3dd85964..c506c011a8 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 246a3dde8e..6a98cdeed9 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 7e58795398..da6c9460c6 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 4e9bdf0e8f..c5f7bb8354 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 e7fc76dde9..ed3b4e02d2 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 7442cdc304..88a0e8b8bb 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 fcdf9e6b79..2e35914148 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 c775760e7c..f6d2396824 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 91f5aa6196..9a8c672d01 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 ad3810a7e9..d38886cf7d 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 d9d485971e..d91713dd7b 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 e033f50908..ed4b8fb484 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 be77975b7e..6fe7013a74 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 f1ea9e99c7..b1c65dbb2b 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 b86a24ebe4..7f28f3ee65 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 ff0a8526e3..d08d7f6d18 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 549f3b7afa..5315e7c373 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 8cbd1c316b..90261e1584 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 e474262d9a..7ab36dbb0e 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 55a4b44f13..dfac49673b 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 f2aa1e1bf2..1f52ce8efa 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 f8ab99161b..681cb48a69 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 915ca5766e..d5b1f14323 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 cfbd21bf89..4acd07937a 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 01f7b10c41..8cc1c20b52 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 a98bffded2..d418373495 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 e2fecb8ce3..75108fde86 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 1f5a64c29b..e4bc65e6cf 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 c381bf9606..5d05c4fb9d 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 653e6c4583..ce8a6c82d6 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 70fb0631b3..11bb75e6f6 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 d51a1301ba..d8e47ff1a4 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 e06e2cce63..a9b4975d34 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 8a1780c45b..b3fc65f4a3 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 36540dc998..b8e23b5355 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 a10ef182b0..f52ffaa809 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 96e91a1893..3743a1f801 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 d029d1a69b..f96eb9285a 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 48a8cea6ee..d829f3e4f2 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 f9a19f73cc..e546ff631e 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 22f04b0170..f41a75772d 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 908d5151a5..2b2287dedb 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 e46a39277d..5ca9f99a5d 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 2da2e16109..50799f9b12 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 96f22be746..f44f14a878 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 be5c84620d..6bd6972e3c 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 eba8cb8cca..3dc5c4085f 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 f279297a3e..4354526003 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 45c75b41ef..ab54def12f 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 9b727e9d95..e85898273f 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 932537c157..4b8e1b8e06 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 ca9e066ed8..586504dbb6 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 07065c602f..0648479321 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 1e7bb1e98b..6f754a33ec 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 a9e171afb5..d234f3a354 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 6b49822be3..a649d34a1e 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 2ce07e993a..8465f5415d 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 578b6a11b1..974a58d525 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 38e199e830..d3b9e72c76 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-02446532-DEBUG \ No newline at end of file +DEV-5bead815-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 4bd56ce546..088ba3656c 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_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 err_fun(double *nom_x, double *delta_x, double *out_6455770263676897448) { + out_6455770263676897448[0] = delta_x[0] + nom_x[0]; + out_6455770263676897448[1] = delta_x[1] + nom_x[1]; + out_6455770263676897448[2] = delta_x[2] + nom_x[2]; + out_6455770263676897448[3] = delta_x[3] + nom_x[3]; + out_6455770263676897448[4] = delta_x[4] + nom_x[4]; + out_6455770263676897448[5] = delta_x[5] + nom_x[5]; + out_6455770263676897448[6] = delta_x[6] + nom_x[6]; + out_6455770263676897448[7] = delta_x[7] + nom_x[7]; + out_6455770263676897448[8] = delta_x[8] + nom_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 inv_err_fun(double *nom_x, double *true_x, double *out_3264339508163218326) { + out_3264339508163218326[0] = -nom_x[0] + true_x[0]; + out_3264339508163218326[1] = -nom_x[1] + true_x[1]; + out_3264339508163218326[2] = -nom_x[2] + true_x[2]; + out_3264339508163218326[3] = -nom_x[3] + true_x[3]; + out_3264339508163218326[4] = -nom_x[4] + true_x[4]; + out_3264339508163218326[5] = -nom_x[5] + true_x[5]; + out_3264339508163218326[6] = -nom_x[6] + true_x[6]; + out_3264339508163218326[7] = -nom_x[7] + true_x[7]; + out_3264339508163218326[8] = -nom_x[8] + true_x[8]; } -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 H_mod_fun(double *state, double *out_2314116705851516155) { + out_2314116705851516155[0] = 1.0; + out_2314116705851516155[1] = 0.0; + out_2314116705851516155[2] = 0.0; + out_2314116705851516155[3] = 0.0; + out_2314116705851516155[4] = 0.0; + out_2314116705851516155[5] = 0.0; + out_2314116705851516155[6] = 0.0; + out_2314116705851516155[7] = 0.0; + out_2314116705851516155[8] = 0.0; + out_2314116705851516155[9] = 0.0; + out_2314116705851516155[10] = 1.0; + out_2314116705851516155[11] = 0.0; + out_2314116705851516155[12] = 0.0; + out_2314116705851516155[13] = 0.0; + out_2314116705851516155[14] = 0.0; + out_2314116705851516155[15] = 0.0; + out_2314116705851516155[16] = 0.0; + out_2314116705851516155[17] = 0.0; + out_2314116705851516155[18] = 0.0; + out_2314116705851516155[19] = 0.0; + out_2314116705851516155[20] = 1.0; + out_2314116705851516155[21] = 0.0; + out_2314116705851516155[22] = 0.0; + out_2314116705851516155[23] = 0.0; + out_2314116705851516155[24] = 0.0; + out_2314116705851516155[25] = 0.0; + out_2314116705851516155[26] = 0.0; + out_2314116705851516155[27] = 0.0; + out_2314116705851516155[28] = 0.0; + out_2314116705851516155[29] = 0.0; + out_2314116705851516155[30] = 1.0; + out_2314116705851516155[31] = 0.0; + out_2314116705851516155[32] = 0.0; + out_2314116705851516155[33] = 0.0; + out_2314116705851516155[34] = 0.0; + out_2314116705851516155[35] = 0.0; + out_2314116705851516155[36] = 0.0; + out_2314116705851516155[37] = 0.0; + out_2314116705851516155[38] = 0.0; + out_2314116705851516155[39] = 0.0; + out_2314116705851516155[40] = 1.0; + out_2314116705851516155[41] = 0.0; + out_2314116705851516155[42] = 0.0; + out_2314116705851516155[43] = 0.0; + out_2314116705851516155[44] = 0.0; + out_2314116705851516155[45] = 0.0; + out_2314116705851516155[46] = 0.0; + out_2314116705851516155[47] = 0.0; + out_2314116705851516155[48] = 0.0; + out_2314116705851516155[49] = 0.0; + out_2314116705851516155[50] = 1.0; + out_2314116705851516155[51] = 0.0; + out_2314116705851516155[52] = 0.0; + out_2314116705851516155[53] = 0.0; + out_2314116705851516155[54] = 0.0; + out_2314116705851516155[55] = 0.0; + out_2314116705851516155[56] = 0.0; + out_2314116705851516155[57] = 0.0; + out_2314116705851516155[58] = 0.0; + out_2314116705851516155[59] = 0.0; + out_2314116705851516155[60] = 1.0; + out_2314116705851516155[61] = 0.0; + out_2314116705851516155[62] = 0.0; + out_2314116705851516155[63] = 0.0; + out_2314116705851516155[64] = 0.0; + out_2314116705851516155[65] = 0.0; + out_2314116705851516155[66] = 0.0; + out_2314116705851516155[67] = 0.0; + out_2314116705851516155[68] = 0.0; + out_2314116705851516155[69] = 0.0; + out_2314116705851516155[70] = 1.0; + out_2314116705851516155[71] = 0.0; + out_2314116705851516155[72] = 0.0; + out_2314116705851516155[73] = 0.0; + out_2314116705851516155[74] = 0.0; + out_2314116705851516155[75] = 0.0; + out_2314116705851516155[76] = 0.0; + out_2314116705851516155[77] = 0.0; + out_2314116705851516155[78] = 0.0; + out_2314116705851516155[79] = 0.0; + out_2314116705851516155[80] = 1.0; } -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_3179221506286947540) { + out_3179221506286947540[0] = state[0]; + out_3179221506286947540[1] = state[1]; + out_3179221506286947540[2] = state[2]; + out_3179221506286947540[3] = state[3]; + out_3179221506286947540[4] = state[4]; + out_3179221506286947540[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_3179221506286947540[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_3179221506286947540[7] = state[7]; + out_3179221506286947540[8] = state[8]; } -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 F_fun(double *state, double dt, double *out_2603402554270443885) { + out_2603402554270443885[0] = 1; + out_2603402554270443885[1] = 0; + out_2603402554270443885[2] = 0; + out_2603402554270443885[3] = 0; + out_2603402554270443885[4] = 0; + out_2603402554270443885[5] = 0; + out_2603402554270443885[6] = 0; + out_2603402554270443885[7] = 0; + out_2603402554270443885[8] = 0; + out_2603402554270443885[9] = 0; + out_2603402554270443885[10] = 1; + out_2603402554270443885[11] = 0; + out_2603402554270443885[12] = 0; + out_2603402554270443885[13] = 0; + out_2603402554270443885[14] = 0; + out_2603402554270443885[15] = 0; + out_2603402554270443885[16] = 0; + out_2603402554270443885[17] = 0; + out_2603402554270443885[18] = 0; + out_2603402554270443885[19] = 0; + out_2603402554270443885[20] = 1; + out_2603402554270443885[21] = 0; + out_2603402554270443885[22] = 0; + out_2603402554270443885[23] = 0; + out_2603402554270443885[24] = 0; + out_2603402554270443885[25] = 0; + out_2603402554270443885[26] = 0; + out_2603402554270443885[27] = 0; + out_2603402554270443885[28] = 0; + out_2603402554270443885[29] = 0; + out_2603402554270443885[30] = 1; + out_2603402554270443885[31] = 0; + out_2603402554270443885[32] = 0; + out_2603402554270443885[33] = 0; + out_2603402554270443885[34] = 0; + out_2603402554270443885[35] = 0; + out_2603402554270443885[36] = 0; + out_2603402554270443885[37] = 0; + out_2603402554270443885[38] = 0; + out_2603402554270443885[39] = 0; + out_2603402554270443885[40] = 1; + out_2603402554270443885[41] = 0; + out_2603402554270443885[42] = 0; + out_2603402554270443885[43] = 0; + out_2603402554270443885[44] = 0; + out_2603402554270443885[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_2603402554270443885[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_2603402554270443885[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_2603402554270443885[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_2603402554270443885[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_2603402554270443885[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_2603402554270443885[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_2603402554270443885[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_2603402554270443885[53] = -9.8100000000000005*dt; + out_2603402554270443885[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_2603402554270443885[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_2603402554270443885[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2603402554270443885[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2603402554270443885[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_2603402554270443885[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_2603402554270443885[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_2603402554270443885[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2603402554270443885[62] = 0; + out_2603402554270443885[63] = 0; + out_2603402554270443885[64] = 0; + out_2603402554270443885[65] = 0; + out_2603402554270443885[66] = 0; + out_2603402554270443885[67] = 0; + out_2603402554270443885[68] = 0; + out_2603402554270443885[69] = 0; + out_2603402554270443885[70] = 1; + out_2603402554270443885[71] = 0; + out_2603402554270443885[72] = 0; + out_2603402554270443885[73] = 0; + out_2603402554270443885[74] = 0; + out_2603402554270443885[75] = 0; + out_2603402554270443885[76] = 0; + out_2603402554270443885[77] = 0; + out_2603402554270443885[78] = 0; + out_2603402554270443885[79] = 0; + out_2603402554270443885[80] = 1; } -void h_25(double *state, double *unused, double *out_7956251239264178232) { - out_7956251239264178232[0] = state[6]; +void h_25(double *state, double *unused, double *out_7483142871930603684) { + out_7483142871930603684[0] = state[6]; } -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_25(double *state, double *unused, double *out_1208863944137975522) { + out_1208863944137975522[0] = 0; + out_1208863944137975522[1] = 0; + out_1208863944137975522[2] = 0; + out_1208863944137975522[3] = 0; + out_1208863944137975522[4] = 0; + out_1208863944137975522[5] = 0; + out_1208863944137975522[6] = 1; + out_1208863944137975522[7] = 0; + out_1208863944137975522[8] = 0; } -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_5607942348347630047) { + out_5607942348347630047[0] = state[4]; + out_5607942348347630047[1] = state[5]; } -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_24(double *state, double *unused, double *out_25779713474104338) { + out_25779713474104338[0] = 0; + out_25779713474104338[1] = 0; + out_25779713474104338[2] = 0; + out_25779713474104338[3] = 0; + out_25779713474104338[4] = 1; + out_25779713474104338[5] = 0; + out_25779713474104338[6] = 0; + out_25779713474104338[7] = 0; + out_25779713474104338[8] = 0; + out_25779713474104338[9] = 0; + out_25779713474104338[10] = 0; + out_25779713474104338[11] = 0; + out_25779713474104338[12] = 0; + out_25779713474104338[13] = 0; + out_25779713474104338[14] = 1; + out_25779713474104338[15] = 0; + out_25779713474104338[16] = 0; + out_25779713474104338[17] = 0; } -void h_30(double *state, double *unused, double *out_4094567762835676580) { - out_4094567762835676580[0] = state[4]; +void h_30(double *state, double *unused, double *out_3845693343676581334) { + out_3845693343676581334[0] = state[4]; } -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_30(double *state, double *unused, double *out_1079524996994735452) { + out_1079524996994735452[0] = 0; + out_1079524996994735452[1] = 0; + out_1079524996994735452[2] = 0; + out_1079524996994735452[3] = 0; + out_1079524996994735452[4] = 1; + out_1079524996994735452[5] = 0; + out_1079524996994735452[6] = 0; + out_1079524996994735452[7] = 0; + out_1079524996994735452[8] = 0; } -void h_26(double *state, double *unused, double *out_4086855108634070771) { - out_4086855108634070771[0] = state[7]; +void h_26(double *state, double *unused, double *out_8847366611337426979) { + out_8847366611337426979[0] = state[7]; } -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_26(double *state, double *unused, double *out_2532639374736080702) { + out_2532639374736080702[0] = 0; + out_2532639374736080702[1] = 0; + out_2532639374736080702[2] = 0; + out_2532639374736080702[3] = 0; + out_2532639374736080702[4] = 0; + out_2532639374736080702[5] = 0; + out_2532639374736080702[6] = 0; + out_2532639374736080702[7] = 1; + out_2532639374736080702[8] = 0; } -void h_27(double *state, double *unused, double *out_3279583348191151873) { - out_3279583348191151873[0] = state[3]; +void h_27(double *state, double *unused, double *out_7323093963955398614) { + out_7323093963955398614[0] = state[3]; } -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_27(double *state, double *unused, double *out_1095238314805689459) { + out_1095238314805689459[0] = 0; + out_1095238314805689459[1] = 0; + out_1095238314805689459[2] = 0; + out_1095238314805689459[3] = 1; + out_1095238314805689459[4] = 0; + out_1095238314805689459[5] = 0; + out_1095238314805689459[6] = 0; + out_1095238314805689459[7] = 0; + out_1095238314805689459[8] = 0; } -void h_29(double *state, double *unused, double *out_3491251878159199063) { - out_3491251878159199063[0] = state[1]; +void h_29(double *state, double *unused, double *out_1155979923984264311) { + out_1155979923984264311[0] = state[1]; } -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_29(double *state, double *unused, double *out_1589756341309127636) { + out_1589756341309127636[0] = 0; + out_1589756341309127636[1] = 1; + out_1589756341309127636[2] = 0; + out_1589756341309127636[3] = 0; + out_1589756341309127636[4] = 0; + out_1589756341309127636[5] = 0; + out_1589756341309127636[6] = 0; + out_1589756341309127636[7] = 0; + out_1589756341309127636[8] = 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_5021130674020958612) { + out_5021130674020958612[0] = state[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_28(double *state, double *unused, double *out_7891000058744771066) { + out_7891000058744771066[0] = 1; + out_7891000058744771066[1] = 0; + out_7891000058744771066[2] = 0; + out_7891000058744771066[3] = 0; + out_7891000058744771066[4] = 0; + out_7891000058744771066[5] = 0; + out_7891000058744771066[6] = 0; + out_7891000058744771066[7] = 0; + out_7891000058744771066[8] = 0; } -void h_31(double *state, double *unused, double *out_271458526840810605) { - out_271458526840810605[0] = state[8]; +void h_31(double *state, double *unused, double *out_3082328551024898747) { + out_3082328551024898747[0] = state[8]; } -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; +void H_31(double *state, double *unused, double *out_1239509906014935950) { + out_1239509906014935950[0] = 0; + out_1239509906014935950[1] = 0; + out_1239509906014935950[2] = 0; + out_1239509906014935950[3] = 0; + out_1239509906014935950[4] = 0; + out_1239509906014935950[5] = 0; + out_1239509906014935950[6] = 0; + out_1239509906014935950[7] = 0; + out_1239509906014935950[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_8548372190097700555) { - err_fun(nom_x, delta_x, out_8548372190097700555); +void car_err_fun(double *nom_x, double *delta_x, double *out_6455770263676897448) { + err_fun(nom_x, delta_x, out_6455770263676897448); } -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_inv_err_fun(double *nom_x, double *true_x, double *out_3264339508163218326) { + inv_err_fun(nom_x, true_x, out_3264339508163218326); } -void car_H_mod_fun(double *state, double *out_7008429191596952258) { - H_mod_fun(state, out_7008429191596952258); +void car_H_mod_fun(double *state, double *out_2314116705851516155) { + H_mod_fun(state, out_2314116705851516155); } -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_3179221506286947540) { + f_fun(state, dt, out_3179221506286947540); } -void car_F_fun(double *state, double dt, double *out_6789911951967766610) { - F_fun(state, dt, out_6789911951967766610); +void car_F_fun(double *state, double dt, double *out_2603402554270443885) { + F_fun(state, dt, out_2603402554270443885); } -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_7483142871930603684) { + h_25(state, unused, out_7483142871930603684); } -void car_H_25(double *state, double *unused, double *out_912908841376907547) { - H_25(state, unused, out_912908841376907547); +void car_H_25(double *state, double *unused, double *out_1208863944137975522) { + H_25(state, unused, out_1208863944137975522); } -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_5607942348347630047) { + h_24(state, unused, out_5607942348347630047); } -void car_H_24(double *state, double *unused, double *out_2501382670766146913) { - H_24(state, unused, out_2501382670766146913); +void car_H_24(double *state, double *unused, double *out_25779713474104338) { + H_24(state, unused, out_25779713474104338); } -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_3845693343676581334) { + h_30(state, unused, out_3845693343676581334); } -void car_H_30(double *state, double *unused, double *out_3614787488750700651) { - H_30(state, unused, out_3614787488750700651); +void car_H_30(double *state, double *unused, double *out_1079524996994735452) { + H_30(state, unused, out_1079524996994735452); } -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_8847366611337426979) { + h_26(state, unused, out_8847366611337426979); } -void car_H_26(double *state, double *unused, double *out_2828594477497148677) { - H_26(state, unused, out_2828594477497148677); +void car_H_26(double *state, double *unused, double *out_2532639374736080702) { + H_26(state, unused, out_2532639374736080702); } -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_7323093963955398614) { + h_27(state, unused, out_7323093963955398614); } -void car_H_27(double *state, double *unused, double *out_5789550800551125562) { - H_27(state, unused, out_5789550800551125562); +void car_H_27(double *state, double *unused, double *out_1095238314805689459) { + H_27(state, unused, out_1095238314805689459); } -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_1155979923984264311) { + h_29(state, unused, out_1155979923984264311); } -void car_H_29(double *state, double *unused, double *out_3104556144436308467) { - H_29(state, unused, out_3104556144436308467); +void car_H_29(double *state, double *unused, double *out_1589756341309127636) { + H_29(state, unused, out_1589756341309127636); } -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_5021130674020958612) { + h_28(state, unused, out_5021130674020958612); } -void car_H_28(double *state, double *unused, double *out_1140925872870982216) { - H_28(state, unused, out_1140925872870982216); +void car_H_28(double *state, double *unused, double *out_7891000058744771066) { + H_28(state, unused, out_7891000058744771066); } -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_3082328551024898747) { + h_31(state, unused, out_3082328551024898747); } -void car_H_31(double *state, double *unused, double *out_3454802579730500153) { - H_31(state, unused, out_3454802579730500153); +void car_H_31(double *state, double *unused, double *out_1239509906014935950) { + H_31(state, unused, out_1239509906014935950); } 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 d04432b09b..7a0bf85bf2 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_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_err_fun(double *nom_x, double *delta_x, double *out_6455770263676897448); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_3264339508163218326); +void car_H_mod_fun(double *state, double *out_2314116705851516155); +void car_f_fun(double *state, double dt, double *out_3179221506286947540); +void car_F_fun(double *state, double dt, double *out_2603402554270443885); +void car_h_25(double *state, double *unused, double *out_7483142871930603684); +void car_H_25(double *state, double *unused, double *out_1208863944137975522); +void car_h_24(double *state, double *unused, double *out_5607942348347630047); +void car_H_24(double *state, double *unused, double *out_25779713474104338); +void car_h_30(double *state, double *unused, double *out_3845693343676581334); +void car_H_30(double *state, double *unused, double *out_1079524996994735452); +void car_h_26(double *state, double *unused, double *out_8847366611337426979); +void car_H_26(double *state, double *unused, double *out_2532639374736080702); +void car_h_27(double *state, double *unused, double *out_7323093963955398614); +void car_H_27(double *state, double *unused, double *out_1095238314805689459); +void car_h_29(double *state, double *unused, double *out_1155979923984264311); +void car_H_29(double *state, double *unused, double *out_1589756341309127636); +void car_h_28(double *state, double *unused, double *out_5021130674020958612); +void car_H_28(double *state, double *unused, double *out_7891000058744771066); +void car_h_31(double *state, double *unused, double *out_3082328551024898747); +void car_H_31(double *state, double *unused, double *out_1239509906014935950); 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 d1c2edfaad..3f79da4ecc 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_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 err_fun(double *nom_x, double *delta_x, double *out_3211789106073125283) { + out_3211789106073125283[0] = delta_x[0] + nom_x[0]; + out_3211789106073125283[1] = delta_x[1] + nom_x[1]; + out_3211789106073125283[2] = delta_x[2] + nom_x[2]; + out_3211789106073125283[3] = delta_x[3] + nom_x[3]; + out_3211789106073125283[4] = delta_x[4] + nom_x[4]; + out_3211789106073125283[5] = delta_x[5] + nom_x[5]; + out_3211789106073125283[6] = delta_x[6] + nom_x[6]; + out_3211789106073125283[7] = delta_x[7] + nom_x[7]; + out_3211789106073125283[8] = delta_x[8] + nom_x[8]; + out_3211789106073125283[9] = delta_x[9] + nom_x[9]; + out_3211789106073125283[10] = delta_x[10] + nom_x[10]; + out_3211789106073125283[11] = delta_x[11] + nom_x[11]; + out_3211789106073125283[12] = delta_x[12] + nom_x[12]; + out_3211789106073125283[13] = delta_x[13] + nom_x[13]; + out_3211789106073125283[14] = delta_x[14] + nom_x[14]; + out_3211789106073125283[15] = delta_x[15] + nom_x[15]; + out_3211789106073125283[16] = delta_x[16] + nom_x[16]; + out_3211789106073125283[17] = delta_x[17] + nom_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 inv_err_fun(double *nom_x, double *true_x, double *out_6226563961506943969) { + out_6226563961506943969[0] = -nom_x[0] + true_x[0]; + out_6226563961506943969[1] = -nom_x[1] + true_x[1]; + out_6226563961506943969[2] = -nom_x[2] + true_x[2]; + out_6226563961506943969[3] = -nom_x[3] + true_x[3]; + out_6226563961506943969[4] = -nom_x[4] + true_x[4]; + out_6226563961506943969[5] = -nom_x[5] + true_x[5]; + out_6226563961506943969[6] = -nom_x[6] + true_x[6]; + out_6226563961506943969[7] = -nom_x[7] + true_x[7]; + out_6226563961506943969[8] = -nom_x[8] + true_x[8]; + out_6226563961506943969[9] = -nom_x[9] + true_x[9]; + out_6226563961506943969[10] = -nom_x[10] + true_x[10]; + out_6226563961506943969[11] = -nom_x[11] + true_x[11]; + out_6226563961506943969[12] = -nom_x[12] + true_x[12]; + out_6226563961506943969[13] = -nom_x[13] + true_x[13]; + out_6226563961506943969[14] = -nom_x[14] + true_x[14]; + out_6226563961506943969[15] = -nom_x[15] + true_x[15]; + out_6226563961506943969[16] = -nom_x[16] + true_x[16]; + out_6226563961506943969[17] = -nom_x[17] + true_x[17]; } -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 H_mod_fun(double *state, double *out_5161461913541420634) { + out_5161461913541420634[0] = 1.0; + out_5161461913541420634[1] = 0.0; + out_5161461913541420634[2] = 0.0; + out_5161461913541420634[3] = 0.0; + out_5161461913541420634[4] = 0.0; + out_5161461913541420634[5] = 0.0; + out_5161461913541420634[6] = 0.0; + out_5161461913541420634[7] = 0.0; + out_5161461913541420634[8] = 0.0; + out_5161461913541420634[9] = 0.0; + out_5161461913541420634[10] = 0.0; + out_5161461913541420634[11] = 0.0; + out_5161461913541420634[12] = 0.0; + out_5161461913541420634[13] = 0.0; + out_5161461913541420634[14] = 0.0; + out_5161461913541420634[15] = 0.0; + out_5161461913541420634[16] = 0.0; + out_5161461913541420634[17] = 0.0; + out_5161461913541420634[18] = 0.0; + out_5161461913541420634[19] = 1.0; + out_5161461913541420634[20] = 0.0; + out_5161461913541420634[21] = 0.0; + out_5161461913541420634[22] = 0.0; + out_5161461913541420634[23] = 0.0; + out_5161461913541420634[24] = 0.0; + out_5161461913541420634[25] = 0.0; + out_5161461913541420634[26] = 0.0; + out_5161461913541420634[27] = 0.0; + out_5161461913541420634[28] = 0.0; + out_5161461913541420634[29] = 0.0; + out_5161461913541420634[30] = 0.0; + out_5161461913541420634[31] = 0.0; + out_5161461913541420634[32] = 0.0; + out_5161461913541420634[33] = 0.0; + out_5161461913541420634[34] = 0.0; + out_5161461913541420634[35] = 0.0; + out_5161461913541420634[36] = 0.0; + out_5161461913541420634[37] = 0.0; + out_5161461913541420634[38] = 1.0; + out_5161461913541420634[39] = 0.0; + out_5161461913541420634[40] = 0.0; + out_5161461913541420634[41] = 0.0; + out_5161461913541420634[42] = 0.0; + out_5161461913541420634[43] = 0.0; + out_5161461913541420634[44] = 0.0; + out_5161461913541420634[45] = 0.0; + out_5161461913541420634[46] = 0.0; + out_5161461913541420634[47] = 0.0; + out_5161461913541420634[48] = 0.0; + out_5161461913541420634[49] = 0.0; + out_5161461913541420634[50] = 0.0; + out_5161461913541420634[51] = 0.0; + out_5161461913541420634[52] = 0.0; + out_5161461913541420634[53] = 0.0; + out_5161461913541420634[54] = 0.0; + out_5161461913541420634[55] = 0.0; + out_5161461913541420634[56] = 0.0; + out_5161461913541420634[57] = 1.0; + out_5161461913541420634[58] = 0.0; + out_5161461913541420634[59] = 0.0; + out_5161461913541420634[60] = 0.0; + out_5161461913541420634[61] = 0.0; + out_5161461913541420634[62] = 0.0; + out_5161461913541420634[63] = 0.0; + out_5161461913541420634[64] = 0.0; + out_5161461913541420634[65] = 0.0; + out_5161461913541420634[66] = 0.0; + out_5161461913541420634[67] = 0.0; + out_5161461913541420634[68] = 0.0; + out_5161461913541420634[69] = 0.0; + out_5161461913541420634[70] = 0.0; + out_5161461913541420634[71] = 0.0; + out_5161461913541420634[72] = 0.0; + out_5161461913541420634[73] = 0.0; + out_5161461913541420634[74] = 0.0; + out_5161461913541420634[75] = 0.0; + out_5161461913541420634[76] = 1.0; + out_5161461913541420634[77] = 0.0; + out_5161461913541420634[78] = 0.0; + out_5161461913541420634[79] = 0.0; + out_5161461913541420634[80] = 0.0; + out_5161461913541420634[81] = 0.0; + out_5161461913541420634[82] = 0.0; + out_5161461913541420634[83] = 0.0; + out_5161461913541420634[84] = 0.0; + out_5161461913541420634[85] = 0.0; + out_5161461913541420634[86] = 0.0; + out_5161461913541420634[87] = 0.0; + out_5161461913541420634[88] = 0.0; + out_5161461913541420634[89] = 0.0; + out_5161461913541420634[90] = 0.0; + out_5161461913541420634[91] = 0.0; + out_5161461913541420634[92] = 0.0; + out_5161461913541420634[93] = 0.0; + out_5161461913541420634[94] = 0.0; + out_5161461913541420634[95] = 1.0; + out_5161461913541420634[96] = 0.0; + out_5161461913541420634[97] = 0.0; + out_5161461913541420634[98] = 0.0; + out_5161461913541420634[99] = 0.0; + out_5161461913541420634[100] = 0.0; + out_5161461913541420634[101] = 0.0; + out_5161461913541420634[102] = 0.0; + out_5161461913541420634[103] = 0.0; + out_5161461913541420634[104] = 0.0; + out_5161461913541420634[105] = 0.0; + out_5161461913541420634[106] = 0.0; + out_5161461913541420634[107] = 0.0; + out_5161461913541420634[108] = 0.0; + out_5161461913541420634[109] = 0.0; + out_5161461913541420634[110] = 0.0; + out_5161461913541420634[111] = 0.0; + out_5161461913541420634[112] = 0.0; + out_5161461913541420634[113] = 0.0; + out_5161461913541420634[114] = 1.0; + out_5161461913541420634[115] = 0.0; + out_5161461913541420634[116] = 0.0; + out_5161461913541420634[117] = 0.0; + out_5161461913541420634[118] = 0.0; + out_5161461913541420634[119] = 0.0; + out_5161461913541420634[120] = 0.0; + out_5161461913541420634[121] = 0.0; + out_5161461913541420634[122] = 0.0; + out_5161461913541420634[123] = 0.0; + out_5161461913541420634[124] = 0.0; + out_5161461913541420634[125] = 0.0; + out_5161461913541420634[126] = 0.0; + out_5161461913541420634[127] = 0.0; + out_5161461913541420634[128] = 0.0; + out_5161461913541420634[129] = 0.0; + out_5161461913541420634[130] = 0.0; + out_5161461913541420634[131] = 0.0; + out_5161461913541420634[132] = 0.0; + out_5161461913541420634[133] = 1.0; + out_5161461913541420634[134] = 0.0; + out_5161461913541420634[135] = 0.0; + out_5161461913541420634[136] = 0.0; + out_5161461913541420634[137] = 0.0; + out_5161461913541420634[138] = 0.0; + out_5161461913541420634[139] = 0.0; + out_5161461913541420634[140] = 0.0; + out_5161461913541420634[141] = 0.0; + out_5161461913541420634[142] = 0.0; + out_5161461913541420634[143] = 0.0; + out_5161461913541420634[144] = 0.0; + out_5161461913541420634[145] = 0.0; + out_5161461913541420634[146] = 0.0; + out_5161461913541420634[147] = 0.0; + out_5161461913541420634[148] = 0.0; + out_5161461913541420634[149] = 0.0; + out_5161461913541420634[150] = 0.0; + out_5161461913541420634[151] = 0.0; + out_5161461913541420634[152] = 1.0; + out_5161461913541420634[153] = 0.0; + out_5161461913541420634[154] = 0.0; + out_5161461913541420634[155] = 0.0; + out_5161461913541420634[156] = 0.0; + out_5161461913541420634[157] = 0.0; + out_5161461913541420634[158] = 0.0; + out_5161461913541420634[159] = 0.0; + out_5161461913541420634[160] = 0.0; + out_5161461913541420634[161] = 0.0; + out_5161461913541420634[162] = 0.0; + out_5161461913541420634[163] = 0.0; + out_5161461913541420634[164] = 0.0; + out_5161461913541420634[165] = 0.0; + out_5161461913541420634[166] = 0.0; + out_5161461913541420634[167] = 0.0; + out_5161461913541420634[168] = 0.0; + out_5161461913541420634[169] = 0.0; + out_5161461913541420634[170] = 0.0; + out_5161461913541420634[171] = 1.0; + out_5161461913541420634[172] = 0.0; + out_5161461913541420634[173] = 0.0; + out_5161461913541420634[174] = 0.0; + out_5161461913541420634[175] = 0.0; + out_5161461913541420634[176] = 0.0; + out_5161461913541420634[177] = 0.0; + out_5161461913541420634[178] = 0.0; + out_5161461913541420634[179] = 0.0; + out_5161461913541420634[180] = 0.0; + out_5161461913541420634[181] = 0.0; + out_5161461913541420634[182] = 0.0; + out_5161461913541420634[183] = 0.0; + out_5161461913541420634[184] = 0.0; + out_5161461913541420634[185] = 0.0; + out_5161461913541420634[186] = 0.0; + out_5161461913541420634[187] = 0.0; + out_5161461913541420634[188] = 0.0; + out_5161461913541420634[189] = 0.0; + out_5161461913541420634[190] = 1.0; + out_5161461913541420634[191] = 0.0; + out_5161461913541420634[192] = 0.0; + out_5161461913541420634[193] = 0.0; + out_5161461913541420634[194] = 0.0; + out_5161461913541420634[195] = 0.0; + out_5161461913541420634[196] = 0.0; + out_5161461913541420634[197] = 0.0; + out_5161461913541420634[198] = 0.0; + out_5161461913541420634[199] = 0.0; + out_5161461913541420634[200] = 0.0; + out_5161461913541420634[201] = 0.0; + out_5161461913541420634[202] = 0.0; + out_5161461913541420634[203] = 0.0; + out_5161461913541420634[204] = 0.0; + out_5161461913541420634[205] = 0.0; + out_5161461913541420634[206] = 0.0; + out_5161461913541420634[207] = 0.0; + out_5161461913541420634[208] = 0.0; + out_5161461913541420634[209] = 1.0; + out_5161461913541420634[210] = 0.0; + out_5161461913541420634[211] = 0.0; + out_5161461913541420634[212] = 0.0; + out_5161461913541420634[213] = 0.0; + out_5161461913541420634[214] = 0.0; + out_5161461913541420634[215] = 0.0; + out_5161461913541420634[216] = 0.0; + out_5161461913541420634[217] = 0.0; + out_5161461913541420634[218] = 0.0; + out_5161461913541420634[219] = 0.0; + out_5161461913541420634[220] = 0.0; + out_5161461913541420634[221] = 0.0; + out_5161461913541420634[222] = 0.0; + out_5161461913541420634[223] = 0.0; + out_5161461913541420634[224] = 0.0; + out_5161461913541420634[225] = 0.0; + out_5161461913541420634[226] = 0.0; + out_5161461913541420634[227] = 0.0; + out_5161461913541420634[228] = 1.0; + out_5161461913541420634[229] = 0.0; + out_5161461913541420634[230] = 0.0; + out_5161461913541420634[231] = 0.0; + out_5161461913541420634[232] = 0.0; + out_5161461913541420634[233] = 0.0; + out_5161461913541420634[234] = 0.0; + out_5161461913541420634[235] = 0.0; + out_5161461913541420634[236] = 0.0; + out_5161461913541420634[237] = 0.0; + out_5161461913541420634[238] = 0.0; + out_5161461913541420634[239] = 0.0; + out_5161461913541420634[240] = 0.0; + out_5161461913541420634[241] = 0.0; + out_5161461913541420634[242] = 0.0; + out_5161461913541420634[243] = 0.0; + out_5161461913541420634[244] = 0.0; + out_5161461913541420634[245] = 0.0; + out_5161461913541420634[246] = 0.0; + out_5161461913541420634[247] = 1.0; + out_5161461913541420634[248] = 0.0; + out_5161461913541420634[249] = 0.0; + out_5161461913541420634[250] = 0.0; + out_5161461913541420634[251] = 0.0; + out_5161461913541420634[252] = 0.0; + out_5161461913541420634[253] = 0.0; + out_5161461913541420634[254] = 0.0; + out_5161461913541420634[255] = 0.0; + out_5161461913541420634[256] = 0.0; + out_5161461913541420634[257] = 0.0; + out_5161461913541420634[258] = 0.0; + out_5161461913541420634[259] = 0.0; + out_5161461913541420634[260] = 0.0; + out_5161461913541420634[261] = 0.0; + out_5161461913541420634[262] = 0.0; + out_5161461913541420634[263] = 0.0; + out_5161461913541420634[264] = 0.0; + out_5161461913541420634[265] = 0.0; + out_5161461913541420634[266] = 1.0; + out_5161461913541420634[267] = 0.0; + out_5161461913541420634[268] = 0.0; + out_5161461913541420634[269] = 0.0; + out_5161461913541420634[270] = 0.0; + out_5161461913541420634[271] = 0.0; + out_5161461913541420634[272] = 0.0; + out_5161461913541420634[273] = 0.0; + out_5161461913541420634[274] = 0.0; + out_5161461913541420634[275] = 0.0; + out_5161461913541420634[276] = 0.0; + out_5161461913541420634[277] = 0.0; + out_5161461913541420634[278] = 0.0; + out_5161461913541420634[279] = 0.0; + out_5161461913541420634[280] = 0.0; + out_5161461913541420634[281] = 0.0; + out_5161461913541420634[282] = 0.0; + out_5161461913541420634[283] = 0.0; + out_5161461913541420634[284] = 0.0; + out_5161461913541420634[285] = 1.0; + out_5161461913541420634[286] = 0.0; + out_5161461913541420634[287] = 0.0; + out_5161461913541420634[288] = 0.0; + out_5161461913541420634[289] = 0.0; + out_5161461913541420634[290] = 0.0; + out_5161461913541420634[291] = 0.0; + out_5161461913541420634[292] = 0.0; + out_5161461913541420634[293] = 0.0; + out_5161461913541420634[294] = 0.0; + out_5161461913541420634[295] = 0.0; + out_5161461913541420634[296] = 0.0; + out_5161461913541420634[297] = 0.0; + out_5161461913541420634[298] = 0.0; + out_5161461913541420634[299] = 0.0; + out_5161461913541420634[300] = 0.0; + out_5161461913541420634[301] = 0.0; + out_5161461913541420634[302] = 0.0; + out_5161461913541420634[303] = 0.0; + out_5161461913541420634[304] = 1.0; + out_5161461913541420634[305] = 0.0; + out_5161461913541420634[306] = 0.0; + out_5161461913541420634[307] = 0.0; + out_5161461913541420634[308] = 0.0; + out_5161461913541420634[309] = 0.0; + out_5161461913541420634[310] = 0.0; + out_5161461913541420634[311] = 0.0; + out_5161461913541420634[312] = 0.0; + out_5161461913541420634[313] = 0.0; + out_5161461913541420634[314] = 0.0; + out_5161461913541420634[315] = 0.0; + out_5161461913541420634[316] = 0.0; + out_5161461913541420634[317] = 0.0; + out_5161461913541420634[318] = 0.0; + out_5161461913541420634[319] = 0.0; + out_5161461913541420634[320] = 0.0; + out_5161461913541420634[321] = 0.0; + out_5161461913541420634[322] = 0.0; + out_5161461913541420634[323] = 1.0; } -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_3057528827902898560) { + out_3057528827902898560[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_3057528827902898560[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_3057528827902898560[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_3057528827902898560[3] = dt*state[12] + state[3]; + out_3057528827902898560[4] = dt*state[13] + state[4]; + out_3057528827902898560[5] = dt*state[14] + state[5]; + out_3057528827902898560[6] = state[6]; + out_3057528827902898560[7] = state[7]; + out_3057528827902898560[8] = state[8]; + out_3057528827902898560[9] = state[9]; + out_3057528827902898560[10] = state[10]; + out_3057528827902898560[11] = state[11]; + out_3057528827902898560[12] = state[12]; + out_3057528827902898560[13] = state[13]; + out_3057528827902898560[14] = state[14]; + out_3057528827902898560[15] = state[15]; + out_3057528827902898560[16] = state[16]; + out_3057528827902898560[17] = state[17]; } -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 F_fun(double *state, double dt, double *out_2410705301866754520) { + out_2410705301866754520[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_2410705301866754520[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_2410705301866754520[2] = 0; + out_2410705301866754520[3] = 0; + out_2410705301866754520[4] = 0; + out_2410705301866754520[5] = 0; + out_2410705301866754520[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_2410705301866754520[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_2410705301866754520[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_2410705301866754520[9] = 0; + out_2410705301866754520[10] = 0; + out_2410705301866754520[11] = 0; + out_2410705301866754520[12] = 0; + out_2410705301866754520[13] = 0; + out_2410705301866754520[14] = 0; + out_2410705301866754520[15] = 0; + out_2410705301866754520[16] = 0; + out_2410705301866754520[17] = 0; + out_2410705301866754520[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_2410705301866754520[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_2410705301866754520[20] = 0; + out_2410705301866754520[21] = 0; + out_2410705301866754520[22] = 0; + out_2410705301866754520[23] = 0; + out_2410705301866754520[24] = 0; + out_2410705301866754520[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_2410705301866754520[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_2410705301866754520[27] = 0; + out_2410705301866754520[28] = 0; + out_2410705301866754520[29] = 0; + out_2410705301866754520[30] = 0; + out_2410705301866754520[31] = 0; + out_2410705301866754520[32] = 0; + out_2410705301866754520[33] = 0; + out_2410705301866754520[34] = 0; + out_2410705301866754520[35] = 0; + out_2410705301866754520[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_2410705301866754520[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_2410705301866754520[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_2410705301866754520[39] = 0; + out_2410705301866754520[40] = 0; + out_2410705301866754520[41] = 0; + out_2410705301866754520[42] = 0; + out_2410705301866754520[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_2410705301866754520[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_2410705301866754520[45] = 0; + out_2410705301866754520[46] = 0; + out_2410705301866754520[47] = 0; + out_2410705301866754520[48] = 0; + out_2410705301866754520[49] = 0; + out_2410705301866754520[50] = 0; + out_2410705301866754520[51] = 0; + out_2410705301866754520[52] = 0; + out_2410705301866754520[53] = 0; + out_2410705301866754520[54] = 0; + out_2410705301866754520[55] = 0; + out_2410705301866754520[56] = 0; + out_2410705301866754520[57] = 1; + out_2410705301866754520[58] = 0; + out_2410705301866754520[59] = 0; + out_2410705301866754520[60] = 0; + out_2410705301866754520[61] = 0; + out_2410705301866754520[62] = 0; + out_2410705301866754520[63] = 0; + out_2410705301866754520[64] = 0; + out_2410705301866754520[65] = 0; + out_2410705301866754520[66] = dt; + out_2410705301866754520[67] = 0; + out_2410705301866754520[68] = 0; + out_2410705301866754520[69] = 0; + out_2410705301866754520[70] = 0; + out_2410705301866754520[71] = 0; + out_2410705301866754520[72] = 0; + out_2410705301866754520[73] = 0; + out_2410705301866754520[74] = 0; + out_2410705301866754520[75] = 0; + out_2410705301866754520[76] = 1; + out_2410705301866754520[77] = 0; + out_2410705301866754520[78] = 0; + out_2410705301866754520[79] = 0; + out_2410705301866754520[80] = 0; + out_2410705301866754520[81] = 0; + out_2410705301866754520[82] = 0; + out_2410705301866754520[83] = 0; + out_2410705301866754520[84] = 0; + out_2410705301866754520[85] = dt; + out_2410705301866754520[86] = 0; + out_2410705301866754520[87] = 0; + out_2410705301866754520[88] = 0; + out_2410705301866754520[89] = 0; + out_2410705301866754520[90] = 0; + out_2410705301866754520[91] = 0; + out_2410705301866754520[92] = 0; + out_2410705301866754520[93] = 0; + out_2410705301866754520[94] = 0; + out_2410705301866754520[95] = 1; + out_2410705301866754520[96] = 0; + out_2410705301866754520[97] = 0; + out_2410705301866754520[98] = 0; + out_2410705301866754520[99] = 0; + out_2410705301866754520[100] = 0; + out_2410705301866754520[101] = 0; + out_2410705301866754520[102] = 0; + out_2410705301866754520[103] = 0; + out_2410705301866754520[104] = dt; + out_2410705301866754520[105] = 0; + out_2410705301866754520[106] = 0; + out_2410705301866754520[107] = 0; + out_2410705301866754520[108] = 0; + out_2410705301866754520[109] = 0; + out_2410705301866754520[110] = 0; + out_2410705301866754520[111] = 0; + out_2410705301866754520[112] = 0; + out_2410705301866754520[113] = 0; + out_2410705301866754520[114] = 1; + out_2410705301866754520[115] = 0; + out_2410705301866754520[116] = 0; + out_2410705301866754520[117] = 0; + out_2410705301866754520[118] = 0; + out_2410705301866754520[119] = 0; + out_2410705301866754520[120] = 0; + out_2410705301866754520[121] = 0; + out_2410705301866754520[122] = 0; + out_2410705301866754520[123] = 0; + out_2410705301866754520[124] = 0; + out_2410705301866754520[125] = 0; + out_2410705301866754520[126] = 0; + out_2410705301866754520[127] = 0; + out_2410705301866754520[128] = 0; + out_2410705301866754520[129] = 0; + out_2410705301866754520[130] = 0; + out_2410705301866754520[131] = 0; + out_2410705301866754520[132] = 0; + out_2410705301866754520[133] = 1; + out_2410705301866754520[134] = 0; + out_2410705301866754520[135] = 0; + out_2410705301866754520[136] = 0; + out_2410705301866754520[137] = 0; + out_2410705301866754520[138] = 0; + out_2410705301866754520[139] = 0; + out_2410705301866754520[140] = 0; + out_2410705301866754520[141] = 0; + out_2410705301866754520[142] = 0; + out_2410705301866754520[143] = 0; + out_2410705301866754520[144] = 0; + out_2410705301866754520[145] = 0; + out_2410705301866754520[146] = 0; + out_2410705301866754520[147] = 0; + out_2410705301866754520[148] = 0; + out_2410705301866754520[149] = 0; + out_2410705301866754520[150] = 0; + out_2410705301866754520[151] = 0; + out_2410705301866754520[152] = 1; + out_2410705301866754520[153] = 0; + out_2410705301866754520[154] = 0; + out_2410705301866754520[155] = 0; + out_2410705301866754520[156] = 0; + out_2410705301866754520[157] = 0; + out_2410705301866754520[158] = 0; + out_2410705301866754520[159] = 0; + out_2410705301866754520[160] = 0; + out_2410705301866754520[161] = 0; + out_2410705301866754520[162] = 0; + out_2410705301866754520[163] = 0; + out_2410705301866754520[164] = 0; + out_2410705301866754520[165] = 0; + out_2410705301866754520[166] = 0; + out_2410705301866754520[167] = 0; + out_2410705301866754520[168] = 0; + out_2410705301866754520[169] = 0; + out_2410705301866754520[170] = 0; + out_2410705301866754520[171] = 1; + out_2410705301866754520[172] = 0; + out_2410705301866754520[173] = 0; + out_2410705301866754520[174] = 0; + out_2410705301866754520[175] = 0; + out_2410705301866754520[176] = 0; + out_2410705301866754520[177] = 0; + out_2410705301866754520[178] = 0; + out_2410705301866754520[179] = 0; + out_2410705301866754520[180] = 0; + out_2410705301866754520[181] = 0; + out_2410705301866754520[182] = 0; + out_2410705301866754520[183] = 0; + out_2410705301866754520[184] = 0; + out_2410705301866754520[185] = 0; + out_2410705301866754520[186] = 0; + out_2410705301866754520[187] = 0; + out_2410705301866754520[188] = 0; + out_2410705301866754520[189] = 0; + out_2410705301866754520[190] = 1; + out_2410705301866754520[191] = 0; + out_2410705301866754520[192] = 0; + out_2410705301866754520[193] = 0; + out_2410705301866754520[194] = 0; + out_2410705301866754520[195] = 0; + out_2410705301866754520[196] = 0; + out_2410705301866754520[197] = 0; + out_2410705301866754520[198] = 0; + out_2410705301866754520[199] = 0; + out_2410705301866754520[200] = 0; + out_2410705301866754520[201] = 0; + out_2410705301866754520[202] = 0; + out_2410705301866754520[203] = 0; + out_2410705301866754520[204] = 0; + out_2410705301866754520[205] = 0; + out_2410705301866754520[206] = 0; + out_2410705301866754520[207] = 0; + out_2410705301866754520[208] = 0; + out_2410705301866754520[209] = 1; + out_2410705301866754520[210] = 0; + out_2410705301866754520[211] = 0; + out_2410705301866754520[212] = 0; + out_2410705301866754520[213] = 0; + out_2410705301866754520[214] = 0; + out_2410705301866754520[215] = 0; + out_2410705301866754520[216] = 0; + out_2410705301866754520[217] = 0; + out_2410705301866754520[218] = 0; + out_2410705301866754520[219] = 0; + out_2410705301866754520[220] = 0; + out_2410705301866754520[221] = 0; + out_2410705301866754520[222] = 0; + out_2410705301866754520[223] = 0; + out_2410705301866754520[224] = 0; + out_2410705301866754520[225] = 0; + out_2410705301866754520[226] = 0; + out_2410705301866754520[227] = 0; + out_2410705301866754520[228] = 1; + out_2410705301866754520[229] = 0; + out_2410705301866754520[230] = 0; + out_2410705301866754520[231] = 0; + out_2410705301866754520[232] = 0; + out_2410705301866754520[233] = 0; + out_2410705301866754520[234] = 0; + out_2410705301866754520[235] = 0; + out_2410705301866754520[236] = 0; + out_2410705301866754520[237] = 0; + out_2410705301866754520[238] = 0; + out_2410705301866754520[239] = 0; + out_2410705301866754520[240] = 0; + out_2410705301866754520[241] = 0; + out_2410705301866754520[242] = 0; + out_2410705301866754520[243] = 0; + out_2410705301866754520[244] = 0; + out_2410705301866754520[245] = 0; + out_2410705301866754520[246] = 0; + out_2410705301866754520[247] = 1; + out_2410705301866754520[248] = 0; + out_2410705301866754520[249] = 0; + out_2410705301866754520[250] = 0; + out_2410705301866754520[251] = 0; + out_2410705301866754520[252] = 0; + out_2410705301866754520[253] = 0; + out_2410705301866754520[254] = 0; + out_2410705301866754520[255] = 0; + out_2410705301866754520[256] = 0; + out_2410705301866754520[257] = 0; + out_2410705301866754520[258] = 0; + out_2410705301866754520[259] = 0; + out_2410705301866754520[260] = 0; + out_2410705301866754520[261] = 0; + out_2410705301866754520[262] = 0; + out_2410705301866754520[263] = 0; + out_2410705301866754520[264] = 0; + out_2410705301866754520[265] = 0; + out_2410705301866754520[266] = 1; + out_2410705301866754520[267] = 0; + out_2410705301866754520[268] = 0; + out_2410705301866754520[269] = 0; + out_2410705301866754520[270] = 0; + out_2410705301866754520[271] = 0; + out_2410705301866754520[272] = 0; + out_2410705301866754520[273] = 0; + out_2410705301866754520[274] = 0; + out_2410705301866754520[275] = 0; + out_2410705301866754520[276] = 0; + out_2410705301866754520[277] = 0; + out_2410705301866754520[278] = 0; + out_2410705301866754520[279] = 0; + out_2410705301866754520[280] = 0; + out_2410705301866754520[281] = 0; + out_2410705301866754520[282] = 0; + out_2410705301866754520[283] = 0; + out_2410705301866754520[284] = 0; + out_2410705301866754520[285] = 1; + out_2410705301866754520[286] = 0; + out_2410705301866754520[287] = 0; + out_2410705301866754520[288] = 0; + out_2410705301866754520[289] = 0; + out_2410705301866754520[290] = 0; + out_2410705301866754520[291] = 0; + out_2410705301866754520[292] = 0; + out_2410705301866754520[293] = 0; + out_2410705301866754520[294] = 0; + out_2410705301866754520[295] = 0; + out_2410705301866754520[296] = 0; + out_2410705301866754520[297] = 0; + out_2410705301866754520[298] = 0; + out_2410705301866754520[299] = 0; + out_2410705301866754520[300] = 0; + out_2410705301866754520[301] = 0; + out_2410705301866754520[302] = 0; + out_2410705301866754520[303] = 0; + out_2410705301866754520[304] = 1; + out_2410705301866754520[305] = 0; + out_2410705301866754520[306] = 0; + out_2410705301866754520[307] = 0; + out_2410705301866754520[308] = 0; + out_2410705301866754520[309] = 0; + out_2410705301866754520[310] = 0; + out_2410705301866754520[311] = 0; + out_2410705301866754520[312] = 0; + out_2410705301866754520[313] = 0; + out_2410705301866754520[314] = 0; + out_2410705301866754520[315] = 0; + out_2410705301866754520[316] = 0; + out_2410705301866754520[317] = 0; + out_2410705301866754520[318] = 0; + out_2410705301866754520[319] = 0; + out_2410705301866754520[320] = 0; + out_2410705301866754520[321] = 0; + out_2410705301866754520[322] = 0; + out_2410705301866754520[323] = 1; } -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_9112018432623595899) { + out_9112018432623595899[0] = state[6] + state[9]; + out_9112018432623595899[1] = state[7] + state[10]; + out_9112018432623595899[2] = state[8] + state[11]; } -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_4(double *state, double *unused, double *out_5072803088510724449) { + out_5072803088510724449[0] = 0; + out_5072803088510724449[1] = 0; + out_5072803088510724449[2] = 0; + out_5072803088510724449[3] = 0; + out_5072803088510724449[4] = 0; + out_5072803088510724449[5] = 0; + out_5072803088510724449[6] = 1; + out_5072803088510724449[7] = 0; + out_5072803088510724449[8] = 0; + out_5072803088510724449[9] = 1; + out_5072803088510724449[10] = 0; + out_5072803088510724449[11] = 0; + out_5072803088510724449[12] = 0; + out_5072803088510724449[13] = 0; + out_5072803088510724449[14] = 0; + out_5072803088510724449[15] = 0; + out_5072803088510724449[16] = 0; + out_5072803088510724449[17] = 0; + out_5072803088510724449[18] = 0; + out_5072803088510724449[19] = 0; + out_5072803088510724449[20] = 0; + out_5072803088510724449[21] = 0; + out_5072803088510724449[22] = 0; + out_5072803088510724449[23] = 0; + out_5072803088510724449[24] = 0; + out_5072803088510724449[25] = 1; + out_5072803088510724449[26] = 0; + out_5072803088510724449[27] = 0; + out_5072803088510724449[28] = 1; + out_5072803088510724449[29] = 0; + out_5072803088510724449[30] = 0; + out_5072803088510724449[31] = 0; + out_5072803088510724449[32] = 0; + out_5072803088510724449[33] = 0; + out_5072803088510724449[34] = 0; + out_5072803088510724449[35] = 0; + out_5072803088510724449[36] = 0; + out_5072803088510724449[37] = 0; + out_5072803088510724449[38] = 0; + out_5072803088510724449[39] = 0; + out_5072803088510724449[40] = 0; + out_5072803088510724449[41] = 0; + out_5072803088510724449[42] = 0; + out_5072803088510724449[43] = 0; + out_5072803088510724449[44] = 1; + out_5072803088510724449[45] = 0; + out_5072803088510724449[46] = 0; + out_5072803088510724449[47] = 1; + out_5072803088510724449[48] = 0; + out_5072803088510724449[49] = 0; + out_5072803088510724449[50] = 0; + out_5072803088510724449[51] = 0; + out_5072803088510724449[52] = 0; + out_5072803088510724449[53] = 0; } -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_1222625076235428622) { + out_1222625076235428622[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_1222625076235428622[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_1222625076235428622[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_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_10(double *state, double *unused, double *out_6913994360612087924) { + out_6913994360612087924[0] = 0; + out_6913994360612087924[1] = 9.8100000000000005*cos(state[1]); + out_6913994360612087924[2] = 0; + out_6913994360612087924[3] = 0; + out_6913994360612087924[4] = -state[8]; + out_6913994360612087924[5] = state[7]; + out_6913994360612087924[6] = 0; + out_6913994360612087924[7] = state[5]; + out_6913994360612087924[8] = -state[4]; + out_6913994360612087924[9] = 0; + out_6913994360612087924[10] = 0; + out_6913994360612087924[11] = 0; + out_6913994360612087924[12] = 1; + out_6913994360612087924[13] = 0; + out_6913994360612087924[14] = 0; + out_6913994360612087924[15] = 1; + out_6913994360612087924[16] = 0; + out_6913994360612087924[17] = 0; + out_6913994360612087924[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_6913994360612087924[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_6913994360612087924[20] = 0; + out_6913994360612087924[21] = state[8]; + out_6913994360612087924[22] = 0; + out_6913994360612087924[23] = -state[6]; + out_6913994360612087924[24] = -state[5]; + out_6913994360612087924[25] = 0; + out_6913994360612087924[26] = state[3]; + out_6913994360612087924[27] = 0; + out_6913994360612087924[28] = 0; + out_6913994360612087924[29] = 0; + out_6913994360612087924[30] = 0; + out_6913994360612087924[31] = 1; + out_6913994360612087924[32] = 0; + out_6913994360612087924[33] = 0; + out_6913994360612087924[34] = 1; + out_6913994360612087924[35] = 0; + out_6913994360612087924[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_6913994360612087924[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_6913994360612087924[38] = 0; + out_6913994360612087924[39] = -state[7]; + out_6913994360612087924[40] = state[6]; + out_6913994360612087924[41] = 0; + out_6913994360612087924[42] = state[4]; + out_6913994360612087924[43] = -state[3]; + out_6913994360612087924[44] = 0; + out_6913994360612087924[45] = 0; + out_6913994360612087924[46] = 0; + out_6913994360612087924[47] = 0; + out_6913994360612087924[48] = 0; + out_6913994360612087924[49] = 0; + out_6913994360612087924[50] = 1; + out_6913994360612087924[51] = 0; + out_6913994360612087924[52] = 0; + out_6913994360612087924[53] = 1; } -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_5539054079846767747) { + out_5539054079846767747[0] = state[3]; + out_5539054079846767747[1] = state[4]; + out_5539054079846767747[2] = state[5]; } -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_13(double *state, double *unused, double *out_5637405008192568553) { + out_5637405008192568553[0] = 0; + out_5637405008192568553[1] = 0; + out_5637405008192568553[2] = 0; + out_5637405008192568553[3] = 1; + out_5637405008192568553[4] = 0; + out_5637405008192568553[5] = 0; + out_5637405008192568553[6] = 0; + out_5637405008192568553[7] = 0; + out_5637405008192568553[8] = 0; + out_5637405008192568553[9] = 0; + out_5637405008192568553[10] = 0; + out_5637405008192568553[11] = 0; + out_5637405008192568553[12] = 0; + out_5637405008192568553[13] = 0; + out_5637405008192568553[14] = 0; + out_5637405008192568553[15] = 0; + out_5637405008192568553[16] = 0; + out_5637405008192568553[17] = 0; + out_5637405008192568553[18] = 0; + out_5637405008192568553[19] = 0; + out_5637405008192568553[20] = 0; + out_5637405008192568553[21] = 0; + out_5637405008192568553[22] = 1; + out_5637405008192568553[23] = 0; + out_5637405008192568553[24] = 0; + out_5637405008192568553[25] = 0; + out_5637405008192568553[26] = 0; + out_5637405008192568553[27] = 0; + out_5637405008192568553[28] = 0; + out_5637405008192568553[29] = 0; + out_5637405008192568553[30] = 0; + out_5637405008192568553[31] = 0; + out_5637405008192568553[32] = 0; + out_5637405008192568553[33] = 0; + out_5637405008192568553[34] = 0; + out_5637405008192568553[35] = 0; + out_5637405008192568553[36] = 0; + out_5637405008192568553[37] = 0; + out_5637405008192568553[38] = 0; + out_5637405008192568553[39] = 0; + out_5637405008192568553[40] = 0; + out_5637405008192568553[41] = 1; + out_5637405008192568553[42] = 0; + out_5637405008192568553[43] = 0; + out_5637405008192568553[44] = 0; + out_5637405008192568553[45] = 0; + out_5637405008192568553[46] = 0; + out_5637405008192568553[47] = 0; + out_5637405008192568553[48] = 0; + out_5637405008192568553[49] = 0; + out_5637405008192568553[50] = 0; + out_5637405008192568553[51] = 0; + out_5637405008192568553[52] = 0; + out_5637405008192568553[53] = 0; } -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_4189770429909065638) { + out_4189770429909065638[0] = state[6]; + out_4189770429909065638[1] = state[7]; + out_4189770429909065638[2] = state[8]; } -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; +void H_14(double *state, double *unused, double *out_1990014656215352153) { + out_1990014656215352153[0] = 0; + out_1990014656215352153[1] = 0; + out_1990014656215352153[2] = 0; + out_1990014656215352153[3] = 0; + out_1990014656215352153[4] = 0; + out_1990014656215352153[5] = 0; + out_1990014656215352153[6] = 1; + out_1990014656215352153[7] = 0; + out_1990014656215352153[8] = 0; + out_1990014656215352153[9] = 0; + out_1990014656215352153[10] = 0; + out_1990014656215352153[11] = 0; + out_1990014656215352153[12] = 0; + out_1990014656215352153[13] = 0; + out_1990014656215352153[14] = 0; + out_1990014656215352153[15] = 0; + out_1990014656215352153[16] = 0; + out_1990014656215352153[17] = 0; + out_1990014656215352153[18] = 0; + out_1990014656215352153[19] = 0; + out_1990014656215352153[20] = 0; + out_1990014656215352153[21] = 0; + out_1990014656215352153[22] = 0; + out_1990014656215352153[23] = 0; + out_1990014656215352153[24] = 0; + out_1990014656215352153[25] = 1; + out_1990014656215352153[26] = 0; + out_1990014656215352153[27] = 0; + out_1990014656215352153[28] = 0; + out_1990014656215352153[29] = 0; + out_1990014656215352153[30] = 0; + out_1990014656215352153[31] = 0; + out_1990014656215352153[32] = 0; + out_1990014656215352153[33] = 0; + out_1990014656215352153[34] = 0; + out_1990014656215352153[35] = 0; + out_1990014656215352153[36] = 0; + out_1990014656215352153[37] = 0; + out_1990014656215352153[38] = 0; + out_1990014656215352153[39] = 0; + out_1990014656215352153[40] = 0; + out_1990014656215352153[41] = 0; + out_1990014656215352153[42] = 0; + out_1990014656215352153[43] = 0; + out_1990014656215352153[44] = 1; + out_1990014656215352153[45] = 0; + out_1990014656215352153[46] = 0; + out_1990014656215352153[47] = 0; + out_1990014656215352153[48] = 0; + out_1990014656215352153[49] = 0; + out_1990014656215352153[50] = 0; + out_1990014656215352153[51] = 0; + out_1990014656215352153[52] = 0; + out_1990014656215352153[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_7911616955151800495) { - err_fun(nom_x, delta_x, out_7911616955151800495); +void pose_err_fun(double *nom_x, double *delta_x, double *out_3211789106073125283) { + err_fun(nom_x, delta_x, out_3211789106073125283); } -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_inv_err_fun(double *nom_x, double *true_x, double *out_6226563961506943969) { + inv_err_fun(nom_x, true_x, out_6226563961506943969); } -void pose_H_mod_fun(double *state, double *out_8208145951121679600) { - H_mod_fun(state, out_8208145951121679600); +void pose_H_mod_fun(double *state, double *out_5161461913541420634) { + H_mod_fun(state, out_5161461913541420634); } -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_3057528827902898560) { + f_fun(state, dt, out_3057528827902898560); } -void pose_F_fun(double *state, double dt, double *out_4299830908048765183) { - F_fun(state, dt, out_4299830908048765183); +void pose_F_fun(double *state, double dt, double *out_2410705301866754520) { + F_fun(state, dt, out_2410705301866754520); } -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_9112018432623595899) { + h_4(state, unused, out_9112018432623595899); } -void pose_H_4(double *state, double *unused, double *out_6964376043796922763) { - H_4(state, unused, out_6964376043796922763); +void pose_H_4(double *state, double *unused, double *out_5072803088510724449) { + H_4(state, unused, out_5072803088510724449); } -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_1222625076235428622) { + h_10(state, unused, out_1222625076235428622); } -void pose_H_10(double *state, double *unused, double *out_8208658119783990230) { - H_10(state, unused, out_8208658119783990230); +void pose_H_10(double *state, double *unused, double *out_6913994360612087924) { + H_10(state, unused, out_6913994360612087924); } -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_5539054079846767747) { + h_13(state, unused, out_5539054079846767747); } -void pose_H_13(double *state, double *unused, double *out_8270094204580296052) { - H_13(state, unused, out_8270094204580296052); +void pose_H_13(double *state, double *unused, double *out_5637405008192568553) { + H_13(state, unused, out_5637405008192568553); } -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_4189770429909065638) { + h_14(state, unused, out_4189770429909065638); } -void pose_H_14(double *state, double *unused, double *out_7519127173573144324) { - H_14(state, unused, out_7519127173573144324); +void pose_H_14(double *state, double *unused, double *out_1990014656215352153) { + H_14(state, unused, out_1990014656215352153); } 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 a61af5935a..93d02334ac 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_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_err_fun(double *nom_x, double *delta_x, double *out_3211789106073125283); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6226563961506943969); +void pose_H_mod_fun(double *state, double *out_5161461913541420634); +void pose_f_fun(double *state, double dt, double *out_3057528827902898560); +void pose_F_fun(double *state, double dt, double *out_2410705301866754520); +void pose_h_4(double *state, double *unused, double *out_9112018432623595899); +void pose_H_4(double *state, double *unused, double *out_5072803088510724449); +void pose_h_10(double *state, double *unused, double *out_1222625076235428622); +void pose_H_10(double *state, double *unused, double *out_6913994360612087924); +void pose_h_13(double *state, double *unused, double *out_5539054079846767747); +void pose_H_13(double *state, double *unused, double *out_5637405008192568553); +void pose_h_14(double *state, double *unused, double *out_4189770429909065638); +void pose_H_14(double *state, double *unused, double *out_1990014656215352153); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file