diff --git a/cereal/libcereal.a b/cereal/libcereal.a index a242c1f0f8..7312448310 100644 Binary files a/cereal/libcereal.a and b/cereal/libcereal.a differ diff --git a/common/libcommon.a b/common/libcommon.a index 8a947bc151..7fe60755ef 100644 Binary files a/common/libcommon.a and b/common/libcommon.a differ diff --git a/common/params_pyx.so b/common/params_pyx.so index 2a4ef1e1b1..81c4a8fef8 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index 10429c9e28..675c353033 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 0314fbaddf..0c4a41606e 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 182b7f0c45..60d2784537 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 c68bee5651..0ea0a7f025 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 5952d24e7b..22290a82ec 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 230de4f868..e5d9fb0a3b 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 230de4f868..e5d9fb0a3b 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 463bb6710a..e65848cfa4 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 463bb6710a..e65848cfa4 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 463bb6710a..e65848cfa4 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 463bb6710a..e65848cfa4 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 463bb6710a..e65848cfa4 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 463bb6710a..e65848cfa4 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 463bb6710a..e65848cfa4 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 463bb6710a..e65848cfa4 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 230de4f868..e5d9fb0a3b 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 230de4f868..e5d9fb0a3b 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 b32960f1d6..c4f732ad2c 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 230de4f868..e5d9fb0a3b 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 230de4f868..e5d9fb0a3b 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 230de4f868..e5d9fb0a3b 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 230de4f868..e5d9fb0a3b 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 059d541f63..06c8c5c1f7 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-96ef704d-DEBUG"; +const uint8_t gitversion[19] = "DEV-73164cc8-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 2afdbb0d58..29d13c695f 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 a523633668..2c4b2dc940 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 f71e1e0d0b..76195153f6 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 29097b6c18..c3610820a9 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 00171271f9..5746d09cd7 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 6b2c9bb95c..63e2d782da 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 112f44769e..3bc24f6542 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 a77e55da4e..f8aa592b0f 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 fae4924e64..549cd9e25c 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 66a25681bf..6fa6f69f7b 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 fc2c6d1c0b..4bb45778cb 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 bd73ff2d69..7746b7d43c 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 7f3e96f7e9..9ad68c5304 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 526c8061a5..e19c2d0f55 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 59d11871fd..fbf9eb0c44 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 737253adbe..33a5911cde 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 9765a14e10..4a183a6ba5 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 a010f1053c..eb11b1a521 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 c1b12d7d37..c419e4858c 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 cb44a2d7c5..609bd2b3ee 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 faf41279cd..d966c23b0b 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 7a78b09159..a0948659bc 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 470c8bb7e6..7c3e66a0e5 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 b1d32ac32c..7078b62fd3 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 89be00c650..28fecd3cec 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 335ea68c8b..bde4801b3f 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 c22682a948..dfc2cf7059 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 274506c3f3..9a4f43ea6e 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 5922d4d662..688b7e6d36 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 a9ab56b22b..5ff6f80aaf 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 ce5338ac27..bbcefd5857 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 a12ab912cf..765ddfd212 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 cb4055a4a5..6ebd3a04a2 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 062572e7be..466765e1e7 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 f0aaecf4b8..717fa0e92b 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 3877466451..f920c2a1c6 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 e15ce05c15..dbe2ec6424 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 e55408be6c..52f0668f07 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 0b4c885e16..572a08ddcc 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 58086ddd18..32332854c6 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 4d3282c173..ce7b02180d 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 fe8b4b98e9..bce3a1194b 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 ea0916fc8f..f3d4d471f3 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 f8b068bb82..7271fa3d01 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 b266a1c307..4f9619d408 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 b118550e5f..68849e8f6d 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 47d2ae9995..a878e88f71 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 4f2e228c37..aaf5928556 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 28a0ff1b20..47aeac22a7 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 20759999a2..5276569aa3 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 7dc1475b99..bb8eabbaf7 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 bf84645c9c..9d0da970de 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 84393cb62d..fa253f944d 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 36f8204e7f..83d9e6a422 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 778c045909..2d79bad23d 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 23771cf93b..493295913f 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 8bc8b18312..2289e98638 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 03f345958d..3400fc47fb 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 39fd177456..48074b9ebe 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 464bf34603..6f7871d698 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 37ecbc5462..afcd8883c1 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 8b8f4537d8..64fd953964 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 c25c2e80ca..0d1c3476d8 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 cf2c43f027..9d321cfd90 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 793c22c49d..9fa15e7172 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 8323b0a0d0..3006b0a5ec 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 064f2cf96b..1699495c3f 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 e54e6e8f8a..b95df9c649 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 9ca48e8900..147ea383ee 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-96ef704d-DEBUG \ No newline at end of file +DEV-73164cc8-DEBUG \ No newline at end of file diff --git a/rednose_repo/rednose/helpers/ekf_sym_pyx.so b/rednose_repo/rednose/helpers/ekf_sym_pyx.so index 985b6c7854..0c2e6104d9 100755 Binary files a/rednose_repo/rednose/helpers/ekf_sym_pyx.so and b/rednose_repo/rednose/helpers/ekf_sym_pyx.so differ diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 1f04a764b6..ccf6f24b79 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_870685266903427678) { - out_870685266903427678[0] = delta_x[0] + nom_x[0]; - out_870685266903427678[1] = delta_x[1] + nom_x[1]; - out_870685266903427678[2] = delta_x[2] + nom_x[2]; - out_870685266903427678[3] = delta_x[3] + nom_x[3]; - out_870685266903427678[4] = delta_x[4] + nom_x[4]; - out_870685266903427678[5] = delta_x[5] + nom_x[5]; - out_870685266903427678[6] = delta_x[6] + nom_x[6]; - out_870685266903427678[7] = delta_x[7] + nom_x[7]; - out_870685266903427678[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_6312187491562868488) { + out_6312187491562868488[0] = delta_x[0] + nom_x[0]; + out_6312187491562868488[1] = delta_x[1] + nom_x[1]; + out_6312187491562868488[2] = delta_x[2] + nom_x[2]; + out_6312187491562868488[3] = delta_x[3] + nom_x[3]; + out_6312187491562868488[4] = delta_x[4] + nom_x[4]; + out_6312187491562868488[5] = delta_x[5] + nom_x[5]; + out_6312187491562868488[6] = delta_x[6] + nom_x[6]; + out_6312187491562868488[7] = delta_x[7] + nom_x[7]; + out_6312187491562868488[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_8535551015639214025) { - out_8535551015639214025[0] = -nom_x[0] + true_x[0]; - out_8535551015639214025[1] = -nom_x[1] + true_x[1]; - out_8535551015639214025[2] = -nom_x[2] + true_x[2]; - out_8535551015639214025[3] = -nom_x[3] + true_x[3]; - out_8535551015639214025[4] = -nom_x[4] + true_x[4]; - out_8535551015639214025[5] = -nom_x[5] + true_x[5]; - out_8535551015639214025[6] = -nom_x[6] + true_x[6]; - out_8535551015639214025[7] = -nom_x[7] + true_x[7]; - out_8535551015639214025[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_8729778864089017210) { + out_8729778864089017210[0] = -nom_x[0] + true_x[0]; + out_8729778864089017210[1] = -nom_x[1] + true_x[1]; + out_8729778864089017210[2] = -nom_x[2] + true_x[2]; + out_8729778864089017210[3] = -nom_x[3] + true_x[3]; + out_8729778864089017210[4] = -nom_x[4] + true_x[4]; + out_8729778864089017210[5] = -nom_x[5] + true_x[5]; + out_8729778864089017210[6] = -nom_x[6] + true_x[6]; + out_8729778864089017210[7] = -nom_x[7] + true_x[7]; + out_8729778864089017210[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_5722367516210042485) { - out_5722367516210042485[0] = 1.0; - out_5722367516210042485[1] = 0.0; - out_5722367516210042485[2] = 0.0; - out_5722367516210042485[3] = 0.0; - out_5722367516210042485[4] = 0.0; - out_5722367516210042485[5] = 0.0; - out_5722367516210042485[6] = 0.0; - out_5722367516210042485[7] = 0.0; - out_5722367516210042485[8] = 0.0; - out_5722367516210042485[9] = 0.0; - out_5722367516210042485[10] = 1.0; - out_5722367516210042485[11] = 0.0; - out_5722367516210042485[12] = 0.0; - out_5722367516210042485[13] = 0.0; - out_5722367516210042485[14] = 0.0; - out_5722367516210042485[15] = 0.0; - out_5722367516210042485[16] = 0.0; - out_5722367516210042485[17] = 0.0; - out_5722367516210042485[18] = 0.0; - out_5722367516210042485[19] = 0.0; - out_5722367516210042485[20] = 1.0; - out_5722367516210042485[21] = 0.0; - out_5722367516210042485[22] = 0.0; - out_5722367516210042485[23] = 0.0; - out_5722367516210042485[24] = 0.0; - out_5722367516210042485[25] = 0.0; - out_5722367516210042485[26] = 0.0; - out_5722367516210042485[27] = 0.0; - out_5722367516210042485[28] = 0.0; - out_5722367516210042485[29] = 0.0; - out_5722367516210042485[30] = 1.0; - out_5722367516210042485[31] = 0.0; - out_5722367516210042485[32] = 0.0; - out_5722367516210042485[33] = 0.0; - out_5722367516210042485[34] = 0.0; - out_5722367516210042485[35] = 0.0; - out_5722367516210042485[36] = 0.0; - out_5722367516210042485[37] = 0.0; - out_5722367516210042485[38] = 0.0; - out_5722367516210042485[39] = 0.0; - out_5722367516210042485[40] = 1.0; - out_5722367516210042485[41] = 0.0; - out_5722367516210042485[42] = 0.0; - out_5722367516210042485[43] = 0.0; - out_5722367516210042485[44] = 0.0; - out_5722367516210042485[45] = 0.0; - out_5722367516210042485[46] = 0.0; - out_5722367516210042485[47] = 0.0; - out_5722367516210042485[48] = 0.0; - out_5722367516210042485[49] = 0.0; - out_5722367516210042485[50] = 1.0; - out_5722367516210042485[51] = 0.0; - out_5722367516210042485[52] = 0.0; - out_5722367516210042485[53] = 0.0; - out_5722367516210042485[54] = 0.0; - out_5722367516210042485[55] = 0.0; - out_5722367516210042485[56] = 0.0; - out_5722367516210042485[57] = 0.0; - out_5722367516210042485[58] = 0.0; - out_5722367516210042485[59] = 0.0; - out_5722367516210042485[60] = 1.0; - out_5722367516210042485[61] = 0.0; - out_5722367516210042485[62] = 0.0; - out_5722367516210042485[63] = 0.0; - out_5722367516210042485[64] = 0.0; - out_5722367516210042485[65] = 0.0; - out_5722367516210042485[66] = 0.0; - out_5722367516210042485[67] = 0.0; - out_5722367516210042485[68] = 0.0; - out_5722367516210042485[69] = 0.0; - out_5722367516210042485[70] = 1.0; - out_5722367516210042485[71] = 0.0; - out_5722367516210042485[72] = 0.0; - out_5722367516210042485[73] = 0.0; - out_5722367516210042485[74] = 0.0; - out_5722367516210042485[75] = 0.0; - out_5722367516210042485[76] = 0.0; - out_5722367516210042485[77] = 0.0; - out_5722367516210042485[78] = 0.0; - out_5722367516210042485[79] = 0.0; - out_5722367516210042485[80] = 1.0; +void H_mod_fun(double *state, double *out_2433884036008471400) { + out_2433884036008471400[0] = 1.0; + out_2433884036008471400[1] = 0.0; + out_2433884036008471400[2] = 0.0; + out_2433884036008471400[3] = 0.0; + out_2433884036008471400[4] = 0.0; + out_2433884036008471400[5] = 0.0; + out_2433884036008471400[6] = 0.0; + out_2433884036008471400[7] = 0.0; + out_2433884036008471400[8] = 0.0; + out_2433884036008471400[9] = 0.0; + out_2433884036008471400[10] = 1.0; + out_2433884036008471400[11] = 0.0; + out_2433884036008471400[12] = 0.0; + out_2433884036008471400[13] = 0.0; + out_2433884036008471400[14] = 0.0; + out_2433884036008471400[15] = 0.0; + out_2433884036008471400[16] = 0.0; + out_2433884036008471400[17] = 0.0; + out_2433884036008471400[18] = 0.0; + out_2433884036008471400[19] = 0.0; + out_2433884036008471400[20] = 1.0; + out_2433884036008471400[21] = 0.0; + out_2433884036008471400[22] = 0.0; + out_2433884036008471400[23] = 0.0; + out_2433884036008471400[24] = 0.0; + out_2433884036008471400[25] = 0.0; + out_2433884036008471400[26] = 0.0; + out_2433884036008471400[27] = 0.0; + out_2433884036008471400[28] = 0.0; + out_2433884036008471400[29] = 0.0; + out_2433884036008471400[30] = 1.0; + out_2433884036008471400[31] = 0.0; + out_2433884036008471400[32] = 0.0; + out_2433884036008471400[33] = 0.0; + out_2433884036008471400[34] = 0.0; + out_2433884036008471400[35] = 0.0; + out_2433884036008471400[36] = 0.0; + out_2433884036008471400[37] = 0.0; + out_2433884036008471400[38] = 0.0; + out_2433884036008471400[39] = 0.0; + out_2433884036008471400[40] = 1.0; + out_2433884036008471400[41] = 0.0; + out_2433884036008471400[42] = 0.0; + out_2433884036008471400[43] = 0.0; + out_2433884036008471400[44] = 0.0; + out_2433884036008471400[45] = 0.0; + out_2433884036008471400[46] = 0.0; + out_2433884036008471400[47] = 0.0; + out_2433884036008471400[48] = 0.0; + out_2433884036008471400[49] = 0.0; + out_2433884036008471400[50] = 1.0; + out_2433884036008471400[51] = 0.0; + out_2433884036008471400[52] = 0.0; + out_2433884036008471400[53] = 0.0; + out_2433884036008471400[54] = 0.0; + out_2433884036008471400[55] = 0.0; + out_2433884036008471400[56] = 0.0; + out_2433884036008471400[57] = 0.0; + out_2433884036008471400[58] = 0.0; + out_2433884036008471400[59] = 0.0; + out_2433884036008471400[60] = 1.0; + out_2433884036008471400[61] = 0.0; + out_2433884036008471400[62] = 0.0; + out_2433884036008471400[63] = 0.0; + out_2433884036008471400[64] = 0.0; + out_2433884036008471400[65] = 0.0; + out_2433884036008471400[66] = 0.0; + out_2433884036008471400[67] = 0.0; + out_2433884036008471400[68] = 0.0; + out_2433884036008471400[69] = 0.0; + out_2433884036008471400[70] = 1.0; + out_2433884036008471400[71] = 0.0; + out_2433884036008471400[72] = 0.0; + out_2433884036008471400[73] = 0.0; + out_2433884036008471400[74] = 0.0; + out_2433884036008471400[75] = 0.0; + out_2433884036008471400[76] = 0.0; + out_2433884036008471400[77] = 0.0; + out_2433884036008471400[78] = 0.0; + out_2433884036008471400[79] = 0.0; + out_2433884036008471400[80] = 1.0; } -void f_fun(double *state, double dt, double *out_6327711019370239094) { - out_6327711019370239094[0] = state[0]; - out_6327711019370239094[1] = state[1]; - out_6327711019370239094[2] = state[2]; - out_6327711019370239094[3] = state[3]; - out_6327711019370239094[4] = state[4]; - out_6327711019370239094[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_6327711019370239094[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_6327711019370239094[7] = state[7]; - out_6327711019370239094[8] = state[8]; +void f_fun(double *state, double dt, double *out_796409112878320158) { + out_796409112878320158[0] = state[0]; + out_796409112878320158[1] = state[1]; + out_796409112878320158[2] = state[2]; + out_796409112878320158[3] = state[3]; + out_796409112878320158[4] = state[4]; + out_796409112878320158[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_796409112878320158[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_796409112878320158[7] = state[7]; + out_796409112878320158[8] = state[8]; } -void F_fun(double *state, double dt, double *out_334732449615445707) { - out_334732449615445707[0] = 1; - out_334732449615445707[1] = 0; - out_334732449615445707[2] = 0; - out_334732449615445707[3] = 0; - out_334732449615445707[4] = 0; - out_334732449615445707[5] = 0; - out_334732449615445707[6] = 0; - out_334732449615445707[7] = 0; - out_334732449615445707[8] = 0; - out_334732449615445707[9] = 0; - out_334732449615445707[10] = 1; - out_334732449615445707[11] = 0; - out_334732449615445707[12] = 0; - out_334732449615445707[13] = 0; - out_334732449615445707[14] = 0; - out_334732449615445707[15] = 0; - out_334732449615445707[16] = 0; - out_334732449615445707[17] = 0; - out_334732449615445707[18] = 0; - out_334732449615445707[19] = 0; - out_334732449615445707[20] = 1; - out_334732449615445707[21] = 0; - out_334732449615445707[22] = 0; - out_334732449615445707[23] = 0; - out_334732449615445707[24] = 0; - out_334732449615445707[25] = 0; - out_334732449615445707[26] = 0; - out_334732449615445707[27] = 0; - out_334732449615445707[28] = 0; - out_334732449615445707[29] = 0; - out_334732449615445707[30] = 1; - out_334732449615445707[31] = 0; - out_334732449615445707[32] = 0; - out_334732449615445707[33] = 0; - out_334732449615445707[34] = 0; - out_334732449615445707[35] = 0; - out_334732449615445707[36] = 0; - out_334732449615445707[37] = 0; - out_334732449615445707[38] = 0; - out_334732449615445707[39] = 0; - out_334732449615445707[40] = 1; - out_334732449615445707[41] = 0; - out_334732449615445707[42] = 0; - out_334732449615445707[43] = 0; - out_334732449615445707[44] = 0; - out_334732449615445707[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_334732449615445707[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_334732449615445707[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_334732449615445707[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_334732449615445707[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_334732449615445707[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_334732449615445707[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_334732449615445707[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_334732449615445707[53] = -9.8100000000000005*dt; - out_334732449615445707[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_334732449615445707[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_334732449615445707[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_334732449615445707[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_334732449615445707[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_334732449615445707[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_334732449615445707[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_334732449615445707[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_334732449615445707[62] = 0; - out_334732449615445707[63] = 0; - out_334732449615445707[64] = 0; - out_334732449615445707[65] = 0; - out_334732449615445707[66] = 0; - out_334732449615445707[67] = 0; - out_334732449615445707[68] = 0; - out_334732449615445707[69] = 0; - out_334732449615445707[70] = 1; - out_334732449615445707[71] = 0; - out_334732449615445707[72] = 0; - out_334732449615445707[73] = 0; - out_334732449615445707[74] = 0; - out_334732449615445707[75] = 0; - out_334732449615445707[76] = 0; - out_334732449615445707[77] = 0; - out_334732449615445707[78] = 0; - out_334732449615445707[79] = 0; - out_334732449615445707[80] = 1; +void F_fun(double *state, double dt, double *out_8757589862379398872) { + out_8757589862379398872[0] = 1; + out_8757589862379398872[1] = 0; + out_8757589862379398872[2] = 0; + out_8757589862379398872[3] = 0; + out_8757589862379398872[4] = 0; + out_8757589862379398872[5] = 0; + out_8757589862379398872[6] = 0; + out_8757589862379398872[7] = 0; + out_8757589862379398872[8] = 0; + out_8757589862379398872[9] = 0; + out_8757589862379398872[10] = 1; + out_8757589862379398872[11] = 0; + out_8757589862379398872[12] = 0; + out_8757589862379398872[13] = 0; + out_8757589862379398872[14] = 0; + out_8757589862379398872[15] = 0; + out_8757589862379398872[16] = 0; + out_8757589862379398872[17] = 0; + out_8757589862379398872[18] = 0; + out_8757589862379398872[19] = 0; + out_8757589862379398872[20] = 1; + out_8757589862379398872[21] = 0; + out_8757589862379398872[22] = 0; + out_8757589862379398872[23] = 0; + out_8757589862379398872[24] = 0; + out_8757589862379398872[25] = 0; + out_8757589862379398872[26] = 0; + out_8757589862379398872[27] = 0; + out_8757589862379398872[28] = 0; + out_8757589862379398872[29] = 0; + out_8757589862379398872[30] = 1; + out_8757589862379398872[31] = 0; + out_8757589862379398872[32] = 0; + out_8757589862379398872[33] = 0; + out_8757589862379398872[34] = 0; + out_8757589862379398872[35] = 0; + out_8757589862379398872[36] = 0; + out_8757589862379398872[37] = 0; + out_8757589862379398872[38] = 0; + out_8757589862379398872[39] = 0; + out_8757589862379398872[40] = 1; + out_8757589862379398872[41] = 0; + out_8757589862379398872[42] = 0; + out_8757589862379398872[43] = 0; + out_8757589862379398872[44] = 0; + out_8757589862379398872[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_8757589862379398872[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_8757589862379398872[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_8757589862379398872[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_8757589862379398872[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_8757589862379398872[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_8757589862379398872[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_8757589862379398872[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_8757589862379398872[53] = -9.8100000000000005*dt; + out_8757589862379398872[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_8757589862379398872[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_8757589862379398872[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_8757589862379398872[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_8757589862379398872[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_8757589862379398872[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_8757589862379398872[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_8757589862379398872[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_8757589862379398872[62] = 0; + out_8757589862379398872[63] = 0; + out_8757589862379398872[64] = 0; + out_8757589862379398872[65] = 0; + out_8757589862379398872[66] = 0; + out_8757589862379398872[67] = 0; + out_8757589862379398872[68] = 0; + out_8757589862379398872[69] = 0; + out_8757589862379398872[70] = 1; + out_8757589862379398872[71] = 0; + out_8757589862379398872[72] = 0; + out_8757589862379398872[73] = 0; + out_8757589862379398872[74] = 0; + out_8757589862379398872[75] = 0; + out_8757589862379398872[76] = 0; + out_8757589862379398872[77] = 0; + out_8757589862379398872[78] = 0; + out_8757589862379398872[79] = 0; + out_8757589862379398872[80] = 1; } -void h_25(double *state, double *unused, double *out_4988354507217303652) { - out_4988354507217303652[0] = state[6]; +void h_25(double *state, double *unused, double *out_7412030721099193028) { + out_7412030721099193028[0] = state[6]; } -void H_25(double *state, double *unused, double *out_3470820709702194752) { - out_3470820709702194752[0] = 0; - out_3470820709702194752[1] = 0; - out_3470820709702194752[2] = 0; - out_3470820709702194752[3] = 0; - out_3470820709702194752[4] = 0; - out_3470820709702194752[5] = 0; - out_3470820709702194752[6] = 1; - out_3470820709702194752[7] = 0; - out_3470820709702194752[8] = 0; +void H_25(double *state, double *unused, double *out_4581042973285851812) { + out_4581042973285851812[0] = 0; + out_4581042973285851812[1] = 0; + out_4581042973285851812[2] = 0; + out_4581042973285851812[3] = 0; + out_4581042973285851812[4] = 0; + out_4581042973285851812[5] = 0; + out_4581042973285851812[6] = 1; + out_4581042973285851812[7] = 0; + out_4581042973285851812[8] = 0; } -void h_24(double *state, double *unused, double *out_8320591788748654668) { - out_8320591788748654668[0] = state[4]; - out_8320591788748654668[1] = state[5]; +void h_24(double *state, double *unused, double *out_36611885517981660) { + out_36611885517981660[0] = state[4]; + out_36611885517981660[1] = state[5]; } -void H_24(double *state, double *unused, double *out_5643470308707694318) { - out_5643470308707694318[0] = 0; - out_5643470308707694318[1] = 0; - out_5643470308707694318[2] = 0; - out_5643470308707694318[3] = 0; - out_5643470308707694318[4] = 1; - out_5643470308707694318[5] = 0; - out_5643470308707694318[6] = 0; - out_5643470308707694318[7] = 0; - out_5643470308707694318[8] = 0; - out_5643470308707694318[9] = 0; - out_5643470308707694318[10] = 0; - out_5643470308707694318[11] = 0; - out_5643470308707694318[12] = 0; - out_5643470308707694318[13] = 0; - out_5643470308707694318[14] = 1; - out_5643470308707694318[15] = 0; - out_5643470308707694318[16] = 0; - out_5643470308707694318[17] = 0; +void H_24(double *state, double *unused, double *out_4647022212783343413) { + out_4647022212783343413[0] = 0; + out_4647022212783343413[1] = 0; + out_4647022212783343413[2] = 0; + out_4647022212783343413[3] = 0; + out_4647022212783343413[4] = 1; + out_4647022212783343413[5] = 0; + out_4647022212783343413[6] = 0; + out_4647022212783343413[7] = 0; + out_4647022212783343413[8] = 0; + out_4647022212783343413[9] = 0; + out_4647022212783343413[10] = 0; + out_4647022212783343413[11] = 0; + out_4647022212783343413[12] = 0; + out_4647022212783343413[13] = 0; + out_4647022212783343413[14] = 1; + out_4647022212783343413[15] = 0; + out_4647022212783343413[16] = 0; + out_4647022212783343413[17] = 0; } -void h_30(double *state, double *unused, double *out_6271436662034189635) { - out_6271436662034189635[0] = state[4]; +void h_30(double *state, double *unused, double *out_7173029876181856936) { + out_7173029876181856936[0] = state[4]; } -void H_30(double *state, double *unused, double *out_952487751194946125) { - out_952487751194946125[0] = 0; - out_952487751194946125[1] = 0; - out_952487751194946125[2] = 0; - out_952487751194946125[3] = 0; - out_952487751194946125[4] = 1; - out_952487751194946125[5] = 0; - out_952487751194946125[6] = 0; - out_952487751194946125[7] = 0; - out_952487751194946125[8] = 0; +void H_30(double *state, double *unused, double *out_9108739303413460010) { + out_9108739303413460010[0] = 0; + out_9108739303413460010[1] = 0; + out_9108739303413460010[2] = 0; + out_9108739303413460010[3] = 0; + out_9108739303413460010[4] = 1; + out_9108739303413460010[5] = 0; + out_9108739303413460010[6] = 0; + out_9108739303413460010[7] = 0; + out_9108739303413460010[8] = 0; } -void h_26(double *state, double *unused, double *out_628706790953232800) { - out_628706790953232800[0] = state[7]; +void h_26(double *state, double *unused, double *out_5482582041446175976) { + out_5482582041446175976[0] = state[7]; } -void H_26(double *state, double *unused, double *out_7212324028576250976) { - out_7212324028576250976[0] = 0; - out_7212324028576250976[1] = 0; - out_7212324028576250976[2] = 0; - out_7212324028576250976[3] = 0; - out_7212324028576250976[4] = 0; - out_7212324028576250976[5] = 0; - out_7212324028576250976[6] = 0; - out_7212324028576250976[7] = 1; - out_7212324028576250976[8] = 0; +void H_26(double *state, double *unused, double *out_8322546292159908036) { + out_8322546292159908036[0] = 0; + out_8322546292159908036[1] = 0; + out_8322546292159908036[2] = 0; + out_8322546292159908036[3] = 0; + out_8322546292159908036[4] = 0; + out_8322546292159908036[5] = 0; + out_8322546292159908036[6] = 0; + out_8322546292159908036[7] = 1; + out_8322546292159908036[8] = 0; } -void h_27(double *state, double *unused, double *out_7592759828026001303) { - out_7592759828026001303[0] = state[3]; +void h_27(double *state, double *unused, double *out_1569938450907647737) { + out_1569938450907647737[0] = state[3]; } -void H_27(double *state, double *unused, double *out_5774922968645859733) { - out_5774922968645859733[0] = 0; - out_5774922968645859733[1] = 0; - out_5774922968645859733[2] = 0; - out_5774922968645859733[3] = 1; - out_5774922968645859733[4] = 0; - out_5774922968645859733[5] = 0; - out_5774922968645859733[6] = 0; - out_5774922968645859733[7] = 0; - out_5774922968645859733[8] = 0; +void H_27(double *state, double *unused, double *out_6885145232229516793) { + out_6885145232229516793[0] = 0; + out_6885145232229516793[1] = 0; + out_6885145232229516793[2] = 0; + out_6885145232229516793[3] = 1; + out_6885145232229516793[4] = 0; + out_6885145232229516793[5] = 0; + out_6885145232229516793[6] = 0; + out_6885145232229516793[7] = 0; + out_6885145232229516793[8] = 0; } -void h_29(double *state, double *unused, double *out_7749946496377130448) { - out_7749946496377130448[0] = state[1]; +void h_29(double *state, double *unused, double *out_5017889475568647426) { + out_5017889475568647426[0] = state[1]; } -void H_29(double *state, double *unused, double *out_7488285695515410766) { - out_7488285695515410766[0] = 0; - out_7488285695515410766[1] = 1; - out_7488285695515410766[2] = 0; - out_7488285695515410766[3] = 0; - out_7488285695515410766[4] = 0; - out_7488285695515410766[5] = 0; - out_7488285695515410766[6] = 0; - out_7488285695515410766[7] = 0; - out_7488285695515410766[8] = 0; +void H_29(double *state, double *unused, double *out_8598507959099067826) { + out_8598507959099067826[0] = 0; + out_8598507959099067826[1] = 1; + out_8598507959099067826[2] = 0; + out_8598507959099067826[3] = 0; + out_8598507959099067826[4] = 0; + out_8598507959099067826[5] = 0; + out_8598507959099067826[6] = 0; + out_8598507959099067826[7] = 0; + out_8598507959099067826[8] = 0; } -void h_28(double *state, double *unused, double *out_1346308770565827835) { - out_1346308770565827835[0] = state[0]; +void h_28(double *state, double *unused, double *out_3431734816753197908) { + out_3431734816753197908[0] = state[0]; } -void H_28(double *state, double *unused, double *out_5524655423950084515) { - out_5524655423950084515[0] = 1; - out_5524655423950084515[1] = 0; - out_5524655423950084515[2] = 0; - out_5524655423950084515[3] = 0; - out_5524655423950084515[4] = 0; - out_5524655423950084515[5] = 0; - out_5524655423950084515[6] = 0; - out_5524655423950084515[7] = 0; - out_5524655423950084515[8] = 0; +void H_28(double *state, double *unused, double *out_4765837097540953216) { + out_4765837097540953216[0] = 1; + out_4765837097540953216[1] = 0; + out_4765837097540953216[2] = 0; + out_4765837097540953216[3] = 0; + out_4765837097540953216[4] = 0; + out_4765837097540953216[5] = 0; + out_4765837097540953216[6] = 0; + out_4765837097540953216[7] = 0; + out_4765837097540953216[8] = 0; } -void h_31(double *state, double *unused, double *out_2214861449768682318) { - out_2214861449768682318[0] = state[8]; +void h_31(double *state, double *unused, double *out_8237682826887035884) { + out_8237682826887035884[0] = state[8]; } -void H_31(double *state, double *unused, double *out_7838532130809602452) { - out_7838532130809602452[0] = 0; - out_7838532130809602452[1] = 0; - out_7838532130809602452[2] = 0; - out_7838532130809602452[3] = 0; - out_7838532130809602452[4] = 0; - out_7838532130809602452[5] = 0; - out_7838532130809602452[6] = 0; - out_7838532130809602452[7] = 0; - out_7838532130809602452[8] = 1; +void H_31(double *state, double *unused, double *out_4550397011408891384) { + out_4550397011408891384[0] = 0; + out_4550397011408891384[1] = 0; + out_4550397011408891384[2] = 0; + out_4550397011408891384[3] = 0; + out_4550397011408891384[4] = 0; + out_4550397011408891384[5] = 0; + out_4550397011408891384[6] = 0; + out_4550397011408891384[7] = 0; + out_4550397011408891384[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_870685266903427678) { - err_fun(nom_x, delta_x, out_870685266903427678); +void car_err_fun(double *nom_x, double *delta_x, double *out_6312187491562868488) { + err_fun(nom_x, delta_x, out_6312187491562868488); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_8535551015639214025) { - inv_err_fun(nom_x, true_x, out_8535551015639214025); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_8729778864089017210) { + inv_err_fun(nom_x, true_x, out_8729778864089017210); } -void car_H_mod_fun(double *state, double *out_5722367516210042485) { - H_mod_fun(state, out_5722367516210042485); +void car_H_mod_fun(double *state, double *out_2433884036008471400) { + H_mod_fun(state, out_2433884036008471400); } -void car_f_fun(double *state, double dt, double *out_6327711019370239094) { - f_fun(state, dt, out_6327711019370239094); +void car_f_fun(double *state, double dt, double *out_796409112878320158) { + f_fun(state, dt, out_796409112878320158); } -void car_F_fun(double *state, double dt, double *out_334732449615445707) { - F_fun(state, dt, out_334732449615445707); +void car_F_fun(double *state, double dt, double *out_8757589862379398872) { + F_fun(state, dt, out_8757589862379398872); } -void car_h_25(double *state, double *unused, double *out_4988354507217303652) { - h_25(state, unused, out_4988354507217303652); +void car_h_25(double *state, double *unused, double *out_7412030721099193028) { + h_25(state, unused, out_7412030721099193028); } -void car_H_25(double *state, double *unused, double *out_3470820709702194752) { - H_25(state, unused, out_3470820709702194752); +void car_H_25(double *state, double *unused, double *out_4581042973285851812) { + H_25(state, unused, out_4581042973285851812); } -void car_h_24(double *state, double *unused, double *out_8320591788748654668) { - h_24(state, unused, out_8320591788748654668); +void car_h_24(double *state, double *unused, double *out_36611885517981660) { + h_24(state, unused, out_36611885517981660); } -void car_H_24(double *state, double *unused, double *out_5643470308707694318) { - H_24(state, unused, out_5643470308707694318); +void car_H_24(double *state, double *unused, double *out_4647022212783343413) { + H_24(state, unused, out_4647022212783343413); } -void car_h_30(double *state, double *unused, double *out_6271436662034189635) { - h_30(state, unused, out_6271436662034189635); +void car_h_30(double *state, double *unused, double *out_7173029876181856936) { + h_30(state, unused, out_7173029876181856936); } -void car_H_30(double *state, double *unused, double *out_952487751194946125) { - H_30(state, unused, out_952487751194946125); +void car_H_30(double *state, double *unused, double *out_9108739303413460010) { + H_30(state, unused, out_9108739303413460010); } -void car_h_26(double *state, double *unused, double *out_628706790953232800) { - h_26(state, unused, out_628706790953232800); +void car_h_26(double *state, double *unused, double *out_5482582041446175976) { + h_26(state, unused, out_5482582041446175976); } -void car_H_26(double *state, double *unused, double *out_7212324028576250976) { - H_26(state, unused, out_7212324028576250976); +void car_H_26(double *state, double *unused, double *out_8322546292159908036) { + H_26(state, unused, out_8322546292159908036); } -void car_h_27(double *state, double *unused, double *out_7592759828026001303) { - h_27(state, unused, out_7592759828026001303); +void car_h_27(double *state, double *unused, double *out_1569938450907647737) { + h_27(state, unused, out_1569938450907647737); } -void car_H_27(double *state, double *unused, double *out_5774922968645859733) { - H_27(state, unused, out_5774922968645859733); +void car_H_27(double *state, double *unused, double *out_6885145232229516793) { + H_27(state, unused, out_6885145232229516793); } -void car_h_29(double *state, double *unused, double *out_7749946496377130448) { - h_29(state, unused, out_7749946496377130448); +void car_h_29(double *state, double *unused, double *out_5017889475568647426) { + h_29(state, unused, out_5017889475568647426); } -void car_H_29(double *state, double *unused, double *out_7488285695515410766) { - H_29(state, unused, out_7488285695515410766); +void car_H_29(double *state, double *unused, double *out_8598507959099067826) { + H_29(state, unused, out_8598507959099067826); } -void car_h_28(double *state, double *unused, double *out_1346308770565827835) { - h_28(state, unused, out_1346308770565827835); +void car_h_28(double *state, double *unused, double *out_3431734816753197908) { + h_28(state, unused, out_3431734816753197908); } -void car_H_28(double *state, double *unused, double *out_5524655423950084515) { - H_28(state, unused, out_5524655423950084515); +void car_H_28(double *state, double *unused, double *out_4765837097540953216) { + H_28(state, unused, out_4765837097540953216); } -void car_h_31(double *state, double *unused, double *out_2214861449768682318) { - h_31(state, unused, out_2214861449768682318); +void car_h_31(double *state, double *unused, double *out_8237682826887035884) { + h_31(state, unused, out_8237682826887035884); } -void car_H_31(double *state, double *unused, double *out_7838532130809602452) { - H_31(state, unused, out_7838532130809602452); +void car_H_31(double *state, double *unused, double *out_4550397011408891384) { + H_31(state, unused, out_4550397011408891384); } 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 18397a5d8b..f55c6a5ce9 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_870685266903427678); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_8535551015639214025); -void car_H_mod_fun(double *state, double *out_5722367516210042485); -void car_f_fun(double *state, double dt, double *out_6327711019370239094); -void car_F_fun(double *state, double dt, double *out_334732449615445707); -void car_h_25(double *state, double *unused, double *out_4988354507217303652); -void car_H_25(double *state, double *unused, double *out_3470820709702194752); -void car_h_24(double *state, double *unused, double *out_8320591788748654668); -void car_H_24(double *state, double *unused, double *out_5643470308707694318); -void car_h_30(double *state, double *unused, double *out_6271436662034189635); -void car_H_30(double *state, double *unused, double *out_952487751194946125); -void car_h_26(double *state, double *unused, double *out_628706790953232800); -void car_H_26(double *state, double *unused, double *out_7212324028576250976); -void car_h_27(double *state, double *unused, double *out_7592759828026001303); -void car_H_27(double *state, double *unused, double *out_5774922968645859733); -void car_h_29(double *state, double *unused, double *out_7749946496377130448); -void car_H_29(double *state, double *unused, double *out_7488285695515410766); -void car_h_28(double *state, double *unused, double *out_1346308770565827835); -void car_H_28(double *state, double *unused, double *out_5524655423950084515); -void car_h_31(double *state, double *unused, double *out_2214861449768682318); -void car_H_31(double *state, double *unused, double *out_7838532130809602452); +void car_err_fun(double *nom_x, double *delta_x, double *out_6312187491562868488); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_8729778864089017210); +void car_H_mod_fun(double *state, double *out_2433884036008471400); +void car_f_fun(double *state, double dt, double *out_796409112878320158); +void car_F_fun(double *state, double dt, double *out_8757589862379398872); +void car_h_25(double *state, double *unused, double *out_7412030721099193028); +void car_H_25(double *state, double *unused, double *out_4581042973285851812); +void car_h_24(double *state, double *unused, double *out_36611885517981660); +void car_H_24(double *state, double *unused, double *out_4647022212783343413); +void car_h_30(double *state, double *unused, double *out_7173029876181856936); +void car_H_30(double *state, double *unused, double *out_9108739303413460010); +void car_h_26(double *state, double *unused, double *out_5482582041446175976); +void car_H_26(double *state, double *unused, double *out_8322546292159908036); +void car_h_27(double *state, double *unused, double *out_1569938450907647737); +void car_H_27(double *state, double *unused, double *out_6885145232229516793); +void car_h_29(double *state, double *unused, double *out_5017889475568647426); +void car_H_29(double *state, double *unused, double *out_8598507959099067826); +void car_h_28(double *state, double *unused, double *out_3431734816753197908); +void car_H_28(double *state, double *unused, double *out_4765837097540953216); +void car_h_31(double *state, double *unused, double *out_8237682826887035884); +void car_H_31(double *state, double *unused, double *out_4550397011408891384); void car_predict(double *in_x, double *in_P, double *in_Q, double dt); void car_set_mass(double x); void car_set_rotational_inertia(double x); diff --git a/selfdrive/locationd/models/generated/car.os b/selfdrive/locationd/models/generated/car.os index 763f6afba2..6d7a588d97 100644 Binary files a/selfdrive/locationd/models/generated/car.os and b/selfdrive/locationd/models/generated/car.os differ diff --git a/selfdrive/locationd/models/generated/libcar.so b/selfdrive/locationd/models/generated/libcar.so index e66b841c4e..ec2c55555b 100755 Binary files a/selfdrive/locationd/models/generated/libcar.so and b/selfdrive/locationd/models/generated/libcar.so differ diff --git a/selfdrive/locationd/models/generated/libpose.so b/selfdrive/locationd/models/generated/libpose.so index 1661647d9a..d89955950b 100755 Binary files a/selfdrive/locationd/models/generated/libpose.so and b/selfdrive/locationd/models/generated/libpose.so differ diff --git a/selfdrive/locationd/models/generated/pose.cpp b/selfdrive/locationd/models/generated/pose.cpp index d8aa44dc8f..f8a13e8b8d 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_731387772078245243) { - out_731387772078245243[0] = delta_x[0] + nom_x[0]; - out_731387772078245243[1] = delta_x[1] + nom_x[1]; - out_731387772078245243[2] = delta_x[2] + nom_x[2]; - out_731387772078245243[3] = delta_x[3] + nom_x[3]; - out_731387772078245243[4] = delta_x[4] + nom_x[4]; - out_731387772078245243[5] = delta_x[5] + nom_x[5]; - out_731387772078245243[6] = delta_x[6] + nom_x[6]; - out_731387772078245243[7] = delta_x[7] + nom_x[7]; - out_731387772078245243[8] = delta_x[8] + nom_x[8]; - out_731387772078245243[9] = delta_x[9] + nom_x[9]; - out_731387772078245243[10] = delta_x[10] + nom_x[10]; - out_731387772078245243[11] = delta_x[11] + nom_x[11]; - out_731387772078245243[12] = delta_x[12] + nom_x[12]; - out_731387772078245243[13] = delta_x[13] + nom_x[13]; - out_731387772078245243[14] = delta_x[14] + nom_x[14]; - out_731387772078245243[15] = delta_x[15] + nom_x[15]; - out_731387772078245243[16] = delta_x[16] + nom_x[16]; - out_731387772078245243[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_2114100039121924448) { + out_2114100039121924448[0] = delta_x[0] + nom_x[0]; + out_2114100039121924448[1] = delta_x[1] + nom_x[1]; + out_2114100039121924448[2] = delta_x[2] + nom_x[2]; + out_2114100039121924448[3] = delta_x[3] + nom_x[3]; + out_2114100039121924448[4] = delta_x[4] + nom_x[4]; + out_2114100039121924448[5] = delta_x[5] + nom_x[5]; + out_2114100039121924448[6] = delta_x[6] + nom_x[6]; + out_2114100039121924448[7] = delta_x[7] + nom_x[7]; + out_2114100039121924448[8] = delta_x[8] + nom_x[8]; + out_2114100039121924448[9] = delta_x[9] + nom_x[9]; + out_2114100039121924448[10] = delta_x[10] + nom_x[10]; + out_2114100039121924448[11] = delta_x[11] + nom_x[11]; + out_2114100039121924448[12] = delta_x[12] + nom_x[12]; + out_2114100039121924448[13] = delta_x[13] + nom_x[13]; + out_2114100039121924448[14] = delta_x[14] + nom_x[14]; + out_2114100039121924448[15] = delta_x[15] + nom_x[15]; + out_2114100039121924448[16] = delta_x[16] + nom_x[16]; + out_2114100039121924448[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_4206130707332622452) { - out_4206130707332622452[0] = -nom_x[0] + true_x[0]; - out_4206130707332622452[1] = -nom_x[1] + true_x[1]; - out_4206130707332622452[2] = -nom_x[2] + true_x[2]; - out_4206130707332622452[3] = -nom_x[3] + true_x[3]; - out_4206130707332622452[4] = -nom_x[4] + true_x[4]; - out_4206130707332622452[5] = -nom_x[5] + true_x[5]; - out_4206130707332622452[6] = -nom_x[6] + true_x[6]; - out_4206130707332622452[7] = -nom_x[7] + true_x[7]; - out_4206130707332622452[8] = -nom_x[8] + true_x[8]; - out_4206130707332622452[9] = -nom_x[9] + true_x[9]; - out_4206130707332622452[10] = -nom_x[10] + true_x[10]; - out_4206130707332622452[11] = -nom_x[11] + true_x[11]; - out_4206130707332622452[12] = -nom_x[12] + true_x[12]; - out_4206130707332622452[13] = -nom_x[13] + true_x[13]; - out_4206130707332622452[14] = -nom_x[14] + true_x[14]; - out_4206130707332622452[15] = -nom_x[15] + true_x[15]; - out_4206130707332622452[16] = -nom_x[16] + true_x[16]; - out_4206130707332622452[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_2618631132718776507) { + out_2618631132718776507[0] = -nom_x[0] + true_x[0]; + out_2618631132718776507[1] = -nom_x[1] + true_x[1]; + out_2618631132718776507[2] = -nom_x[2] + true_x[2]; + out_2618631132718776507[3] = -nom_x[3] + true_x[3]; + out_2618631132718776507[4] = -nom_x[4] + true_x[4]; + out_2618631132718776507[5] = -nom_x[5] + true_x[5]; + out_2618631132718776507[6] = -nom_x[6] + true_x[6]; + out_2618631132718776507[7] = -nom_x[7] + true_x[7]; + out_2618631132718776507[8] = -nom_x[8] + true_x[8]; + out_2618631132718776507[9] = -nom_x[9] + true_x[9]; + out_2618631132718776507[10] = -nom_x[10] + true_x[10]; + out_2618631132718776507[11] = -nom_x[11] + true_x[11]; + out_2618631132718776507[12] = -nom_x[12] + true_x[12]; + out_2618631132718776507[13] = -nom_x[13] + true_x[13]; + out_2618631132718776507[14] = -nom_x[14] + true_x[14]; + out_2618631132718776507[15] = -nom_x[15] + true_x[15]; + out_2618631132718776507[16] = -nom_x[16] + true_x[16]; + out_2618631132718776507[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_2916011499501574438) { - out_2916011499501574438[0] = 1.0; - out_2916011499501574438[1] = 0.0; - out_2916011499501574438[2] = 0.0; - out_2916011499501574438[3] = 0.0; - out_2916011499501574438[4] = 0.0; - out_2916011499501574438[5] = 0.0; - out_2916011499501574438[6] = 0.0; - out_2916011499501574438[7] = 0.0; - out_2916011499501574438[8] = 0.0; - out_2916011499501574438[9] = 0.0; - out_2916011499501574438[10] = 0.0; - out_2916011499501574438[11] = 0.0; - out_2916011499501574438[12] = 0.0; - out_2916011499501574438[13] = 0.0; - out_2916011499501574438[14] = 0.0; - out_2916011499501574438[15] = 0.0; - out_2916011499501574438[16] = 0.0; - out_2916011499501574438[17] = 0.0; - out_2916011499501574438[18] = 0.0; - out_2916011499501574438[19] = 1.0; - out_2916011499501574438[20] = 0.0; - out_2916011499501574438[21] = 0.0; - out_2916011499501574438[22] = 0.0; - out_2916011499501574438[23] = 0.0; - out_2916011499501574438[24] = 0.0; - out_2916011499501574438[25] = 0.0; - out_2916011499501574438[26] = 0.0; - out_2916011499501574438[27] = 0.0; - out_2916011499501574438[28] = 0.0; - out_2916011499501574438[29] = 0.0; - out_2916011499501574438[30] = 0.0; - out_2916011499501574438[31] = 0.0; - out_2916011499501574438[32] = 0.0; - out_2916011499501574438[33] = 0.0; - out_2916011499501574438[34] = 0.0; - out_2916011499501574438[35] = 0.0; - out_2916011499501574438[36] = 0.0; - out_2916011499501574438[37] = 0.0; - out_2916011499501574438[38] = 1.0; - out_2916011499501574438[39] = 0.0; - out_2916011499501574438[40] = 0.0; - out_2916011499501574438[41] = 0.0; - out_2916011499501574438[42] = 0.0; - out_2916011499501574438[43] = 0.0; - out_2916011499501574438[44] = 0.0; - out_2916011499501574438[45] = 0.0; - out_2916011499501574438[46] = 0.0; - out_2916011499501574438[47] = 0.0; - out_2916011499501574438[48] = 0.0; - out_2916011499501574438[49] = 0.0; - out_2916011499501574438[50] = 0.0; - out_2916011499501574438[51] = 0.0; - out_2916011499501574438[52] = 0.0; - out_2916011499501574438[53] = 0.0; - out_2916011499501574438[54] = 0.0; - out_2916011499501574438[55] = 0.0; - out_2916011499501574438[56] = 0.0; - out_2916011499501574438[57] = 1.0; - out_2916011499501574438[58] = 0.0; - out_2916011499501574438[59] = 0.0; - out_2916011499501574438[60] = 0.0; - out_2916011499501574438[61] = 0.0; - out_2916011499501574438[62] = 0.0; - out_2916011499501574438[63] = 0.0; - out_2916011499501574438[64] = 0.0; - out_2916011499501574438[65] = 0.0; - out_2916011499501574438[66] = 0.0; - out_2916011499501574438[67] = 0.0; - out_2916011499501574438[68] = 0.0; - out_2916011499501574438[69] = 0.0; - out_2916011499501574438[70] = 0.0; - out_2916011499501574438[71] = 0.0; - out_2916011499501574438[72] = 0.0; - out_2916011499501574438[73] = 0.0; - out_2916011499501574438[74] = 0.0; - out_2916011499501574438[75] = 0.0; - out_2916011499501574438[76] = 1.0; - out_2916011499501574438[77] = 0.0; - out_2916011499501574438[78] = 0.0; - out_2916011499501574438[79] = 0.0; - out_2916011499501574438[80] = 0.0; - out_2916011499501574438[81] = 0.0; - out_2916011499501574438[82] = 0.0; - out_2916011499501574438[83] = 0.0; - out_2916011499501574438[84] = 0.0; - out_2916011499501574438[85] = 0.0; - out_2916011499501574438[86] = 0.0; - out_2916011499501574438[87] = 0.0; - out_2916011499501574438[88] = 0.0; - out_2916011499501574438[89] = 0.0; - out_2916011499501574438[90] = 0.0; - out_2916011499501574438[91] = 0.0; - out_2916011499501574438[92] = 0.0; - out_2916011499501574438[93] = 0.0; - out_2916011499501574438[94] = 0.0; - out_2916011499501574438[95] = 1.0; - out_2916011499501574438[96] = 0.0; - out_2916011499501574438[97] = 0.0; - out_2916011499501574438[98] = 0.0; - out_2916011499501574438[99] = 0.0; - out_2916011499501574438[100] = 0.0; - out_2916011499501574438[101] = 0.0; - out_2916011499501574438[102] = 0.0; - out_2916011499501574438[103] = 0.0; - out_2916011499501574438[104] = 0.0; - out_2916011499501574438[105] = 0.0; - out_2916011499501574438[106] = 0.0; - out_2916011499501574438[107] = 0.0; - out_2916011499501574438[108] = 0.0; - out_2916011499501574438[109] = 0.0; - out_2916011499501574438[110] = 0.0; - out_2916011499501574438[111] = 0.0; - out_2916011499501574438[112] = 0.0; - out_2916011499501574438[113] = 0.0; - out_2916011499501574438[114] = 1.0; - out_2916011499501574438[115] = 0.0; - out_2916011499501574438[116] = 0.0; - out_2916011499501574438[117] = 0.0; - out_2916011499501574438[118] = 0.0; - out_2916011499501574438[119] = 0.0; - out_2916011499501574438[120] = 0.0; - out_2916011499501574438[121] = 0.0; - out_2916011499501574438[122] = 0.0; - out_2916011499501574438[123] = 0.0; - out_2916011499501574438[124] = 0.0; - out_2916011499501574438[125] = 0.0; - out_2916011499501574438[126] = 0.0; - out_2916011499501574438[127] = 0.0; - out_2916011499501574438[128] = 0.0; - out_2916011499501574438[129] = 0.0; - out_2916011499501574438[130] = 0.0; - out_2916011499501574438[131] = 0.0; - out_2916011499501574438[132] = 0.0; - out_2916011499501574438[133] = 1.0; - out_2916011499501574438[134] = 0.0; - out_2916011499501574438[135] = 0.0; - out_2916011499501574438[136] = 0.0; - out_2916011499501574438[137] = 0.0; - out_2916011499501574438[138] = 0.0; - out_2916011499501574438[139] = 0.0; - out_2916011499501574438[140] = 0.0; - out_2916011499501574438[141] = 0.0; - out_2916011499501574438[142] = 0.0; - out_2916011499501574438[143] = 0.0; - out_2916011499501574438[144] = 0.0; - out_2916011499501574438[145] = 0.0; - out_2916011499501574438[146] = 0.0; - out_2916011499501574438[147] = 0.0; - out_2916011499501574438[148] = 0.0; - out_2916011499501574438[149] = 0.0; - out_2916011499501574438[150] = 0.0; - out_2916011499501574438[151] = 0.0; - out_2916011499501574438[152] = 1.0; - out_2916011499501574438[153] = 0.0; - out_2916011499501574438[154] = 0.0; - out_2916011499501574438[155] = 0.0; - out_2916011499501574438[156] = 0.0; - out_2916011499501574438[157] = 0.0; - out_2916011499501574438[158] = 0.0; - out_2916011499501574438[159] = 0.0; - out_2916011499501574438[160] = 0.0; - out_2916011499501574438[161] = 0.0; - out_2916011499501574438[162] = 0.0; - out_2916011499501574438[163] = 0.0; - out_2916011499501574438[164] = 0.0; - out_2916011499501574438[165] = 0.0; - out_2916011499501574438[166] = 0.0; - out_2916011499501574438[167] = 0.0; - out_2916011499501574438[168] = 0.0; - out_2916011499501574438[169] = 0.0; - out_2916011499501574438[170] = 0.0; - out_2916011499501574438[171] = 1.0; - out_2916011499501574438[172] = 0.0; - out_2916011499501574438[173] = 0.0; - out_2916011499501574438[174] = 0.0; - out_2916011499501574438[175] = 0.0; - out_2916011499501574438[176] = 0.0; - out_2916011499501574438[177] = 0.0; - out_2916011499501574438[178] = 0.0; - out_2916011499501574438[179] = 0.0; - out_2916011499501574438[180] = 0.0; - out_2916011499501574438[181] = 0.0; - out_2916011499501574438[182] = 0.0; - out_2916011499501574438[183] = 0.0; - out_2916011499501574438[184] = 0.0; - out_2916011499501574438[185] = 0.0; - out_2916011499501574438[186] = 0.0; - out_2916011499501574438[187] = 0.0; - out_2916011499501574438[188] = 0.0; - out_2916011499501574438[189] = 0.0; - out_2916011499501574438[190] = 1.0; - out_2916011499501574438[191] = 0.0; - out_2916011499501574438[192] = 0.0; - out_2916011499501574438[193] = 0.0; - out_2916011499501574438[194] = 0.0; - out_2916011499501574438[195] = 0.0; - out_2916011499501574438[196] = 0.0; - out_2916011499501574438[197] = 0.0; - out_2916011499501574438[198] = 0.0; - out_2916011499501574438[199] = 0.0; - out_2916011499501574438[200] = 0.0; - out_2916011499501574438[201] = 0.0; - out_2916011499501574438[202] = 0.0; - out_2916011499501574438[203] = 0.0; - out_2916011499501574438[204] = 0.0; - out_2916011499501574438[205] = 0.0; - out_2916011499501574438[206] = 0.0; - out_2916011499501574438[207] = 0.0; - out_2916011499501574438[208] = 0.0; - out_2916011499501574438[209] = 1.0; - out_2916011499501574438[210] = 0.0; - out_2916011499501574438[211] = 0.0; - out_2916011499501574438[212] = 0.0; - out_2916011499501574438[213] = 0.0; - out_2916011499501574438[214] = 0.0; - out_2916011499501574438[215] = 0.0; - out_2916011499501574438[216] = 0.0; - out_2916011499501574438[217] = 0.0; - out_2916011499501574438[218] = 0.0; - out_2916011499501574438[219] = 0.0; - out_2916011499501574438[220] = 0.0; - out_2916011499501574438[221] = 0.0; - out_2916011499501574438[222] = 0.0; - out_2916011499501574438[223] = 0.0; - out_2916011499501574438[224] = 0.0; - out_2916011499501574438[225] = 0.0; - out_2916011499501574438[226] = 0.0; - out_2916011499501574438[227] = 0.0; - out_2916011499501574438[228] = 1.0; - out_2916011499501574438[229] = 0.0; - out_2916011499501574438[230] = 0.0; - out_2916011499501574438[231] = 0.0; - out_2916011499501574438[232] = 0.0; - out_2916011499501574438[233] = 0.0; - out_2916011499501574438[234] = 0.0; - out_2916011499501574438[235] = 0.0; - out_2916011499501574438[236] = 0.0; - out_2916011499501574438[237] = 0.0; - out_2916011499501574438[238] = 0.0; - out_2916011499501574438[239] = 0.0; - out_2916011499501574438[240] = 0.0; - out_2916011499501574438[241] = 0.0; - out_2916011499501574438[242] = 0.0; - out_2916011499501574438[243] = 0.0; - out_2916011499501574438[244] = 0.0; - out_2916011499501574438[245] = 0.0; - out_2916011499501574438[246] = 0.0; - out_2916011499501574438[247] = 1.0; - out_2916011499501574438[248] = 0.0; - out_2916011499501574438[249] = 0.0; - out_2916011499501574438[250] = 0.0; - out_2916011499501574438[251] = 0.0; - out_2916011499501574438[252] = 0.0; - out_2916011499501574438[253] = 0.0; - out_2916011499501574438[254] = 0.0; - out_2916011499501574438[255] = 0.0; - out_2916011499501574438[256] = 0.0; - out_2916011499501574438[257] = 0.0; - out_2916011499501574438[258] = 0.0; - out_2916011499501574438[259] = 0.0; - out_2916011499501574438[260] = 0.0; - out_2916011499501574438[261] = 0.0; - out_2916011499501574438[262] = 0.0; - out_2916011499501574438[263] = 0.0; - out_2916011499501574438[264] = 0.0; - out_2916011499501574438[265] = 0.0; - out_2916011499501574438[266] = 1.0; - out_2916011499501574438[267] = 0.0; - out_2916011499501574438[268] = 0.0; - out_2916011499501574438[269] = 0.0; - out_2916011499501574438[270] = 0.0; - out_2916011499501574438[271] = 0.0; - out_2916011499501574438[272] = 0.0; - out_2916011499501574438[273] = 0.0; - out_2916011499501574438[274] = 0.0; - out_2916011499501574438[275] = 0.0; - out_2916011499501574438[276] = 0.0; - out_2916011499501574438[277] = 0.0; - out_2916011499501574438[278] = 0.0; - out_2916011499501574438[279] = 0.0; - out_2916011499501574438[280] = 0.0; - out_2916011499501574438[281] = 0.0; - out_2916011499501574438[282] = 0.0; - out_2916011499501574438[283] = 0.0; - out_2916011499501574438[284] = 0.0; - out_2916011499501574438[285] = 1.0; - out_2916011499501574438[286] = 0.0; - out_2916011499501574438[287] = 0.0; - out_2916011499501574438[288] = 0.0; - out_2916011499501574438[289] = 0.0; - out_2916011499501574438[290] = 0.0; - out_2916011499501574438[291] = 0.0; - out_2916011499501574438[292] = 0.0; - out_2916011499501574438[293] = 0.0; - out_2916011499501574438[294] = 0.0; - out_2916011499501574438[295] = 0.0; - out_2916011499501574438[296] = 0.0; - out_2916011499501574438[297] = 0.0; - out_2916011499501574438[298] = 0.0; - out_2916011499501574438[299] = 0.0; - out_2916011499501574438[300] = 0.0; - out_2916011499501574438[301] = 0.0; - out_2916011499501574438[302] = 0.0; - out_2916011499501574438[303] = 0.0; - out_2916011499501574438[304] = 1.0; - out_2916011499501574438[305] = 0.0; - out_2916011499501574438[306] = 0.0; - out_2916011499501574438[307] = 0.0; - out_2916011499501574438[308] = 0.0; - out_2916011499501574438[309] = 0.0; - out_2916011499501574438[310] = 0.0; - out_2916011499501574438[311] = 0.0; - out_2916011499501574438[312] = 0.0; - out_2916011499501574438[313] = 0.0; - out_2916011499501574438[314] = 0.0; - out_2916011499501574438[315] = 0.0; - out_2916011499501574438[316] = 0.0; - out_2916011499501574438[317] = 0.0; - out_2916011499501574438[318] = 0.0; - out_2916011499501574438[319] = 0.0; - out_2916011499501574438[320] = 0.0; - out_2916011499501574438[321] = 0.0; - out_2916011499501574438[322] = 0.0; - out_2916011499501574438[323] = 1.0; +void H_mod_fun(double *state, double *out_4058554015661107700) { + out_4058554015661107700[0] = 1.0; + out_4058554015661107700[1] = 0.0; + out_4058554015661107700[2] = 0.0; + out_4058554015661107700[3] = 0.0; + out_4058554015661107700[4] = 0.0; + out_4058554015661107700[5] = 0.0; + out_4058554015661107700[6] = 0.0; + out_4058554015661107700[7] = 0.0; + out_4058554015661107700[8] = 0.0; + out_4058554015661107700[9] = 0.0; + out_4058554015661107700[10] = 0.0; + out_4058554015661107700[11] = 0.0; + out_4058554015661107700[12] = 0.0; + out_4058554015661107700[13] = 0.0; + out_4058554015661107700[14] = 0.0; + out_4058554015661107700[15] = 0.0; + out_4058554015661107700[16] = 0.0; + out_4058554015661107700[17] = 0.0; + out_4058554015661107700[18] = 0.0; + out_4058554015661107700[19] = 1.0; + out_4058554015661107700[20] = 0.0; + out_4058554015661107700[21] = 0.0; + out_4058554015661107700[22] = 0.0; + out_4058554015661107700[23] = 0.0; + out_4058554015661107700[24] = 0.0; + out_4058554015661107700[25] = 0.0; + out_4058554015661107700[26] = 0.0; + out_4058554015661107700[27] = 0.0; + out_4058554015661107700[28] = 0.0; + out_4058554015661107700[29] = 0.0; + out_4058554015661107700[30] = 0.0; + out_4058554015661107700[31] = 0.0; + out_4058554015661107700[32] = 0.0; + out_4058554015661107700[33] = 0.0; + out_4058554015661107700[34] = 0.0; + out_4058554015661107700[35] = 0.0; + out_4058554015661107700[36] = 0.0; + out_4058554015661107700[37] = 0.0; + out_4058554015661107700[38] = 1.0; + out_4058554015661107700[39] = 0.0; + out_4058554015661107700[40] = 0.0; + out_4058554015661107700[41] = 0.0; + out_4058554015661107700[42] = 0.0; + out_4058554015661107700[43] = 0.0; + out_4058554015661107700[44] = 0.0; + out_4058554015661107700[45] = 0.0; + out_4058554015661107700[46] = 0.0; + out_4058554015661107700[47] = 0.0; + out_4058554015661107700[48] = 0.0; + out_4058554015661107700[49] = 0.0; + out_4058554015661107700[50] = 0.0; + out_4058554015661107700[51] = 0.0; + out_4058554015661107700[52] = 0.0; + out_4058554015661107700[53] = 0.0; + out_4058554015661107700[54] = 0.0; + out_4058554015661107700[55] = 0.0; + out_4058554015661107700[56] = 0.0; + out_4058554015661107700[57] = 1.0; + out_4058554015661107700[58] = 0.0; + out_4058554015661107700[59] = 0.0; + out_4058554015661107700[60] = 0.0; + out_4058554015661107700[61] = 0.0; + out_4058554015661107700[62] = 0.0; + out_4058554015661107700[63] = 0.0; + out_4058554015661107700[64] = 0.0; + out_4058554015661107700[65] = 0.0; + out_4058554015661107700[66] = 0.0; + out_4058554015661107700[67] = 0.0; + out_4058554015661107700[68] = 0.0; + out_4058554015661107700[69] = 0.0; + out_4058554015661107700[70] = 0.0; + out_4058554015661107700[71] = 0.0; + out_4058554015661107700[72] = 0.0; + out_4058554015661107700[73] = 0.0; + out_4058554015661107700[74] = 0.0; + out_4058554015661107700[75] = 0.0; + out_4058554015661107700[76] = 1.0; + out_4058554015661107700[77] = 0.0; + out_4058554015661107700[78] = 0.0; + out_4058554015661107700[79] = 0.0; + out_4058554015661107700[80] = 0.0; + out_4058554015661107700[81] = 0.0; + out_4058554015661107700[82] = 0.0; + out_4058554015661107700[83] = 0.0; + out_4058554015661107700[84] = 0.0; + out_4058554015661107700[85] = 0.0; + out_4058554015661107700[86] = 0.0; + out_4058554015661107700[87] = 0.0; + out_4058554015661107700[88] = 0.0; + out_4058554015661107700[89] = 0.0; + out_4058554015661107700[90] = 0.0; + out_4058554015661107700[91] = 0.0; + out_4058554015661107700[92] = 0.0; + out_4058554015661107700[93] = 0.0; + out_4058554015661107700[94] = 0.0; + out_4058554015661107700[95] = 1.0; + out_4058554015661107700[96] = 0.0; + out_4058554015661107700[97] = 0.0; + out_4058554015661107700[98] = 0.0; + out_4058554015661107700[99] = 0.0; + out_4058554015661107700[100] = 0.0; + out_4058554015661107700[101] = 0.0; + out_4058554015661107700[102] = 0.0; + out_4058554015661107700[103] = 0.0; + out_4058554015661107700[104] = 0.0; + out_4058554015661107700[105] = 0.0; + out_4058554015661107700[106] = 0.0; + out_4058554015661107700[107] = 0.0; + out_4058554015661107700[108] = 0.0; + out_4058554015661107700[109] = 0.0; + out_4058554015661107700[110] = 0.0; + out_4058554015661107700[111] = 0.0; + out_4058554015661107700[112] = 0.0; + out_4058554015661107700[113] = 0.0; + out_4058554015661107700[114] = 1.0; + out_4058554015661107700[115] = 0.0; + out_4058554015661107700[116] = 0.0; + out_4058554015661107700[117] = 0.0; + out_4058554015661107700[118] = 0.0; + out_4058554015661107700[119] = 0.0; + out_4058554015661107700[120] = 0.0; + out_4058554015661107700[121] = 0.0; + out_4058554015661107700[122] = 0.0; + out_4058554015661107700[123] = 0.0; + out_4058554015661107700[124] = 0.0; + out_4058554015661107700[125] = 0.0; + out_4058554015661107700[126] = 0.0; + out_4058554015661107700[127] = 0.0; + out_4058554015661107700[128] = 0.0; + out_4058554015661107700[129] = 0.0; + out_4058554015661107700[130] = 0.0; + out_4058554015661107700[131] = 0.0; + out_4058554015661107700[132] = 0.0; + out_4058554015661107700[133] = 1.0; + out_4058554015661107700[134] = 0.0; + out_4058554015661107700[135] = 0.0; + out_4058554015661107700[136] = 0.0; + out_4058554015661107700[137] = 0.0; + out_4058554015661107700[138] = 0.0; + out_4058554015661107700[139] = 0.0; + out_4058554015661107700[140] = 0.0; + out_4058554015661107700[141] = 0.0; + out_4058554015661107700[142] = 0.0; + out_4058554015661107700[143] = 0.0; + out_4058554015661107700[144] = 0.0; + out_4058554015661107700[145] = 0.0; + out_4058554015661107700[146] = 0.0; + out_4058554015661107700[147] = 0.0; + out_4058554015661107700[148] = 0.0; + out_4058554015661107700[149] = 0.0; + out_4058554015661107700[150] = 0.0; + out_4058554015661107700[151] = 0.0; + out_4058554015661107700[152] = 1.0; + out_4058554015661107700[153] = 0.0; + out_4058554015661107700[154] = 0.0; + out_4058554015661107700[155] = 0.0; + out_4058554015661107700[156] = 0.0; + out_4058554015661107700[157] = 0.0; + out_4058554015661107700[158] = 0.0; + out_4058554015661107700[159] = 0.0; + out_4058554015661107700[160] = 0.0; + out_4058554015661107700[161] = 0.0; + out_4058554015661107700[162] = 0.0; + out_4058554015661107700[163] = 0.0; + out_4058554015661107700[164] = 0.0; + out_4058554015661107700[165] = 0.0; + out_4058554015661107700[166] = 0.0; + out_4058554015661107700[167] = 0.0; + out_4058554015661107700[168] = 0.0; + out_4058554015661107700[169] = 0.0; + out_4058554015661107700[170] = 0.0; + out_4058554015661107700[171] = 1.0; + out_4058554015661107700[172] = 0.0; + out_4058554015661107700[173] = 0.0; + out_4058554015661107700[174] = 0.0; + out_4058554015661107700[175] = 0.0; + out_4058554015661107700[176] = 0.0; + out_4058554015661107700[177] = 0.0; + out_4058554015661107700[178] = 0.0; + out_4058554015661107700[179] = 0.0; + out_4058554015661107700[180] = 0.0; + out_4058554015661107700[181] = 0.0; + out_4058554015661107700[182] = 0.0; + out_4058554015661107700[183] = 0.0; + out_4058554015661107700[184] = 0.0; + out_4058554015661107700[185] = 0.0; + out_4058554015661107700[186] = 0.0; + out_4058554015661107700[187] = 0.0; + out_4058554015661107700[188] = 0.0; + out_4058554015661107700[189] = 0.0; + out_4058554015661107700[190] = 1.0; + out_4058554015661107700[191] = 0.0; + out_4058554015661107700[192] = 0.0; + out_4058554015661107700[193] = 0.0; + out_4058554015661107700[194] = 0.0; + out_4058554015661107700[195] = 0.0; + out_4058554015661107700[196] = 0.0; + out_4058554015661107700[197] = 0.0; + out_4058554015661107700[198] = 0.0; + out_4058554015661107700[199] = 0.0; + out_4058554015661107700[200] = 0.0; + out_4058554015661107700[201] = 0.0; + out_4058554015661107700[202] = 0.0; + out_4058554015661107700[203] = 0.0; + out_4058554015661107700[204] = 0.0; + out_4058554015661107700[205] = 0.0; + out_4058554015661107700[206] = 0.0; + out_4058554015661107700[207] = 0.0; + out_4058554015661107700[208] = 0.0; + out_4058554015661107700[209] = 1.0; + out_4058554015661107700[210] = 0.0; + out_4058554015661107700[211] = 0.0; + out_4058554015661107700[212] = 0.0; + out_4058554015661107700[213] = 0.0; + out_4058554015661107700[214] = 0.0; + out_4058554015661107700[215] = 0.0; + out_4058554015661107700[216] = 0.0; + out_4058554015661107700[217] = 0.0; + out_4058554015661107700[218] = 0.0; + out_4058554015661107700[219] = 0.0; + out_4058554015661107700[220] = 0.0; + out_4058554015661107700[221] = 0.0; + out_4058554015661107700[222] = 0.0; + out_4058554015661107700[223] = 0.0; + out_4058554015661107700[224] = 0.0; + out_4058554015661107700[225] = 0.0; + out_4058554015661107700[226] = 0.0; + out_4058554015661107700[227] = 0.0; + out_4058554015661107700[228] = 1.0; + out_4058554015661107700[229] = 0.0; + out_4058554015661107700[230] = 0.0; + out_4058554015661107700[231] = 0.0; + out_4058554015661107700[232] = 0.0; + out_4058554015661107700[233] = 0.0; + out_4058554015661107700[234] = 0.0; + out_4058554015661107700[235] = 0.0; + out_4058554015661107700[236] = 0.0; + out_4058554015661107700[237] = 0.0; + out_4058554015661107700[238] = 0.0; + out_4058554015661107700[239] = 0.0; + out_4058554015661107700[240] = 0.0; + out_4058554015661107700[241] = 0.0; + out_4058554015661107700[242] = 0.0; + out_4058554015661107700[243] = 0.0; + out_4058554015661107700[244] = 0.0; + out_4058554015661107700[245] = 0.0; + out_4058554015661107700[246] = 0.0; + out_4058554015661107700[247] = 1.0; + out_4058554015661107700[248] = 0.0; + out_4058554015661107700[249] = 0.0; + out_4058554015661107700[250] = 0.0; + out_4058554015661107700[251] = 0.0; + out_4058554015661107700[252] = 0.0; + out_4058554015661107700[253] = 0.0; + out_4058554015661107700[254] = 0.0; + out_4058554015661107700[255] = 0.0; + out_4058554015661107700[256] = 0.0; + out_4058554015661107700[257] = 0.0; + out_4058554015661107700[258] = 0.0; + out_4058554015661107700[259] = 0.0; + out_4058554015661107700[260] = 0.0; + out_4058554015661107700[261] = 0.0; + out_4058554015661107700[262] = 0.0; + out_4058554015661107700[263] = 0.0; + out_4058554015661107700[264] = 0.0; + out_4058554015661107700[265] = 0.0; + out_4058554015661107700[266] = 1.0; + out_4058554015661107700[267] = 0.0; + out_4058554015661107700[268] = 0.0; + out_4058554015661107700[269] = 0.0; + out_4058554015661107700[270] = 0.0; + out_4058554015661107700[271] = 0.0; + out_4058554015661107700[272] = 0.0; + out_4058554015661107700[273] = 0.0; + out_4058554015661107700[274] = 0.0; + out_4058554015661107700[275] = 0.0; + out_4058554015661107700[276] = 0.0; + out_4058554015661107700[277] = 0.0; + out_4058554015661107700[278] = 0.0; + out_4058554015661107700[279] = 0.0; + out_4058554015661107700[280] = 0.0; + out_4058554015661107700[281] = 0.0; + out_4058554015661107700[282] = 0.0; + out_4058554015661107700[283] = 0.0; + out_4058554015661107700[284] = 0.0; + out_4058554015661107700[285] = 1.0; + out_4058554015661107700[286] = 0.0; + out_4058554015661107700[287] = 0.0; + out_4058554015661107700[288] = 0.0; + out_4058554015661107700[289] = 0.0; + out_4058554015661107700[290] = 0.0; + out_4058554015661107700[291] = 0.0; + out_4058554015661107700[292] = 0.0; + out_4058554015661107700[293] = 0.0; + out_4058554015661107700[294] = 0.0; + out_4058554015661107700[295] = 0.0; + out_4058554015661107700[296] = 0.0; + out_4058554015661107700[297] = 0.0; + out_4058554015661107700[298] = 0.0; + out_4058554015661107700[299] = 0.0; + out_4058554015661107700[300] = 0.0; + out_4058554015661107700[301] = 0.0; + out_4058554015661107700[302] = 0.0; + out_4058554015661107700[303] = 0.0; + out_4058554015661107700[304] = 1.0; + out_4058554015661107700[305] = 0.0; + out_4058554015661107700[306] = 0.0; + out_4058554015661107700[307] = 0.0; + out_4058554015661107700[308] = 0.0; + out_4058554015661107700[309] = 0.0; + out_4058554015661107700[310] = 0.0; + out_4058554015661107700[311] = 0.0; + out_4058554015661107700[312] = 0.0; + out_4058554015661107700[313] = 0.0; + out_4058554015661107700[314] = 0.0; + out_4058554015661107700[315] = 0.0; + out_4058554015661107700[316] = 0.0; + out_4058554015661107700[317] = 0.0; + out_4058554015661107700[318] = 0.0; + out_4058554015661107700[319] = 0.0; + out_4058554015661107700[320] = 0.0; + out_4058554015661107700[321] = 0.0; + out_4058554015661107700[322] = 0.0; + out_4058554015661107700[323] = 1.0; } -void f_fun(double *state, double dt, double *out_4420889553799640425) { - out_4420889553799640425[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_4420889553799640425[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_4420889553799640425[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_4420889553799640425[3] = dt*state[12] + state[3]; - out_4420889553799640425[4] = dt*state[13] + state[4]; - out_4420889553799640425[5] = dt*state[14] + state[5]; - out_4420889553799640425[6] = state[6]; - out_4420889553799640425[7] = state[7]; - out_4420889553799640425[8] = state[8]; - out_4420889553799640425[9] = state[9]; - out_4420889553799640425[10] = state[10]; - out_4420889553799640425[11] = state[11]; - out_4420889553799640425[12] = state[12]; - out_4420889553799640425[13] = state[13]; - out_4420889553799640425[14] = state[14]; - out_4420889553799640425[15] = state[15]; - out_4420889553799640425[16] = state[16]; - out_4420889553799640425[17] = state[17]; +void f_fun(double *state, double dt, double *out_5119116025028968719) { + out_5119116025028968719[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_5119116025028968719[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_5119116025028968719[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_5119116025028968719[3] = dt*state[12] + state[3]; + out_5119116025028968719[4] = dt*state[13] + state[4]; + out_5119116025028968719[5] = dt*state[14] + state[5]; + out_5119116025028968719[6] = state[6]; + out_5119116025028968719[7] = state[7]; + out_5119116025028968719[8] = state[8]; + out_5119116025028968719[9] = state[9]; + out_5119116025028968719[10] = state[10]; + out_5119116025028968719[11] = state[11]; + out_5119116025028968719[12] = state[12]; + out_5119116025028968719[13] = state[13]; + out_5119116025028968719[14] = state[14]; + out_5119116025028968719[15] = state[15]; + out_5119116025028968719[16] = state[16]; + out_5119116025028968719[17] = state[17]; } -void F_fun(double *state, double dt, double *out_586752082486093199) { - out_586752082486093199[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_586752082486093199[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_586752082486093199[2] = 0; - out_586752082486093199[3] = 0; - out_586752082486093199[4] = 0; - out_586752082486093199[5] = 0; - out_586752082486093199[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_586752082486093199[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_586752082486093199[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_586752082486093199[9] = 0; - out_586752082486093199[10] = 0; - out_586752082486093199[11] = 0; - out_586752082486093199[12] = 0; - out_586752082486093199[13] = 0; - out_586752082486093199[14] = 0; - out_586752082486093199[15] = 0; - out_586752082486093199[16] = 0; - out_586752082486093199[17] = 0; - out_586752082486093199[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_586752082486093199[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_586752082486093199[20] = 0; - out_586752082486093199[21] = 0; - out_586752082486093199[22] = 0; - out_586752082486093199[23] = 0; - out_586752082486093199[24] = 0; - out_586752082486093199[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_586752082486093199[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_586752082486093199[27] = 0; - out_586752082486093199[28] = 0; - out_586752082486093199[29] = 0; - out_586752082486093199[30] = 0; - out_586752082486093199[31] = 0; - out_586752082486093199[32] = 0; - out_586752082486093199[33] = 0; - out_586752082486093199[34] = 0; - out_586752082486093199[35] = 0; - out_586752082486093199[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_586752082486093199[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_586752082486093199[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_586752082486093199[39] = 0; - out_586752082486093199[40] = 0; - out_586752082486093199[41] = 0; - out_586752082486093199[42] = 0; - out_586752082486093199[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_586752082486093199[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_586752082486093199[45] = 0; - out_586752082486093199[46] = 0; - out_586752082486093199[47] = 0; - out_586752082486093199[48] = 0; - out_586752082486093199[49] = 0; - out_586752082486093199[50] = 0; - out_586752082486093199[51] = 0; - out_586752082486093199[52] = 0; - out_586752082486093199[53] = 0; - out_586752082486093199[54] = 0; - out_586752082486093199[55] = 0; - out_586752082486093199[56] = 0; - out_586752082486093199[57] = 1; - out_586752082486093199[58] = 0; - out_586752082486093199[59] = 0; - out_586752082486093199[60] = 0; - out_586752082486093199[61] = 0; - out_586752082486093199[62] = 0; - out_586752082486093199[63] = 0; - out_586752082486093199[64] = 0; - out_586752082486093199[65] = 0; - out_586752082486093199[66] = dt; - out_586752082486093199[67] = 0; - out_586752082486093199[68] = 0; - out_586752082486093199[69] = 0; - out_586752082486093199[70] = 0; - out_586752082486093199[71] = 0; - out_586752082486093199[72] = 0; - out_586752082486093199[73] = 0; - out_586752082486093199[74] = 0; - out_586752082486093199[75] = 0; - out_586752082486093199[76] = 1; - out_586752082486093199[77] = 0; - out_586752082486093199[78] = 0; - out_586752082486093199[79] = 0; - out_586752082486093199[80] = 0; - out_586752082486093199[81] = 0; - out_586752082486093199[82] = 0; - out_586752082486093199[83] = 0; - out_586752082486093199[84] = 0; - out_586752082486093199[85] = dt; - out_586752082486093199[86] = 0; - out_586752082486093199[87] = 0; - out_586752082486093199[88] = 0; - out_586752082486093199[89] = 0; - out_586752082486093199[90] = 0; - out_586752082486093199[91] = 0; - out_586752082486093199[92] = 0; - out_586752082486093199[93] = 0; - out_586752082486093199[94] = 0; - out_586752082486093199[95] = 1; - out_586752082486093199[96] = 0; - out_586752082486093199[97] = 0; - out_586752082486093199[98] = 0; - out_586752082486093199[99] = 0; - out_586752082486093199[100] = 0; - out_586752082486093199[101] = 0; - out_586752082486093199[102] = 0; - out_586752082486093199[103] = 0; - out_586752082486093199[104] = dt; - out_586752082486093199[105] = 0; - out_586752082486093199[106] = 0; - out_586752082486093199[107] = 0; - out_586752082486093199[108] = 0; - out_586752082486093199[109] = 0; - out_586752082486093199[110] = 0; - out_586752082486093199[111] = 0; - out_586752082486093199[112] = 0; - out_586752082486093199[113] = 0; - out_586752082486093199[114] = 1; - out_586752082486093199[115] = 0; - out_586752082486093199[116] = 0; - out_586752082486093199[117] = 0; - out_586752082486093199[118] = 0; - out_586752082486093199[119] = 0; - out_586752082486093199[120] = 0; - out_586752082486093199[121] = 0; - out_586752082486093199[122] = 0; - out_586752082486093199[123] = 0; - out_586752082486093199[124] = 0; - out_586752082486093199[125] = 0; - out_586752082486093199[126] = 0; - out_586752082486093199[127] = 0; - out_586752082486093199[128] = 0; - out_586752082486093199[129] = 0; - out_586752082486093199[130] = 0; - out_586752082486093199[131] = 0; - out_586752082486093199[132] = 0; - out_586752082486093199[133] = 1; - out_586752082486093199[134] = 0; - out_586752082486093199[135] = 0; - out_586752082486093199[136] = 0; - out_586752082486093199[137] = 0; - out_586752082486093199[138] = 0; - out_586752082486093199[139] = 0; - out_586752082486093199[140] = 0; - out_586752082486093199[141] = 0; - out_586752082486093199[142] = 0; - out_586752082486093199[143] = 0; - out_586752082486093199[144] = 0; - out_586752082486093199[145] = 0; - out_586752082486093199[146] = 0; - out_586752082486093199[147] = 0; - out_586752082486093199[148] = 0; - out_586752082486093199[149] = 0; - out_586752082486093199[150] = 0; - out_586752082486093199[151] = 0; - out_586752082486093199[152] = 1; - out_586752082486093199[153] = 0; - out_586752082486093199[154] = 0; - out_586752082486093199[155] = 0; - out_586752082486093199[156] = 0; - out_586752082486093199[157] = 0; - out_586752082486093199[158] = 0; - out_586752082486093199[159] = 0; - out_586752082486093199[160] = 0; - out_586752082486093199[161] = 0; - out_586752082486093199[162] = 0; - out_586752082486093199[163] = 0; - out_586752082486093199[164] = 0; - out_586752082486093199[165] = 0; - out_586752082486093199[166] = 0; - out_586752082486093199[167] = 0; - out_586752082486093199[168] = 0; - out_586752082486093199[169] = 0; - out_586752082486093199[170] = 0; - out_586752082486093199[171] = 1; - out_586752082486093199[172] = 0; - out_586752082486093199[173] = 0; - out_586752082486093199[174] = 0; - out_586752082486093199[175] = 0; - out_586752082486093199[176] = 0; - out_586752082486093199[177] = 0; - out_586752082486093199[178] = 0; - out_586752082486093199[179] = 0; - out_586752082486093199[180] = 0; - out_586752082486093199[181] = 0; - out_586752082486093199[182] = 0; - out_586752082486093199[183] = 0; - out_586752082486093199[184] = 0; - out_586752082486093199[185] = 0; - out_586752082486093199[186] = 0; - out_586752082486093199[187] = 0; - out_586752082486093199[188] = 0; - out_586752082486093199[189] = 0; - out_586752082486093199[190] = 1; - out_586752082486093199[191] = 0; - out_586752082486093199[192] = 0; - out_586752082486093199[193] = 0; - out_586752082486093199[194] = 0; - out_586752082486093199[195] = 0; - out_586752082486093199[196] = 0; - out_586752082486093199[197] = 0; - out_586752082486093199[198] = 0; - out_586752082486093199[199] = 0; - out_586752082486093199[200] = 0; - out_586752082486093199[201] = 0; - out_586752082486093199[202] = 0; - out_586752082486093199[203] = 0; - out_586752082486093199[204] = 0; - out_586752082486093199[205] = 0; - out_586752082486093199[206] = 0; - out_586752082486093199[207] = 0; - out_586752082486093199[208] = 0; - out_586752082486093199[209] = 1; - out_586752082486093199[210] = 0; - out_586752082486093199[211] = 0; - out_586752082486093199[212] = 0; - out_586752082486093199[213] = 0; - out_586752082486093199[214] = 0; - out_586752082486093199[215] = 0; - out_586752082486093199[216] = 0; - out_586752082486093199[217] = 0; - out_586752082486093199[218] = 0; - out_586752082486093199[219] = 0; - out_586752082486093199[220] = 0; - out_586752082486093199[221] = 0; - out_586752082486093199[222] = 0; - out_586752082486093199[223] = 0; - out_586752082486093199[224] = 0; - out_586752082486093199[225] = 0; - out_586752082486093199[226] = 0; - out_586752082486093199[227] = 0; - out_586752082486093199[228] = 1; - out_586752082486093199[229] = 0; - out_586752082486093199[230] = 0; - out_586752082486093199[231] = 0; - out_586752082486093199[232] = 0; - out_586752082486093199[233] = 0; - out_586752082486093199[234] = 0; - out_586752082486093199[235] = 0; - out_586752082486093199[236] = 0; - out_586752082486093199[237] = 0; - out_586752082486093199[238] = 0; - out_586752082486093199[239] = 0; - out_586752082486093199[240] = 0; - out_586752082486093199[241] = 0; - out_586752082486093199[242] = 0; - out_586752082486093199[243] = 0; - out_586752082486093199[244] = 0; - out_586752082486093199[245] = 0; - out_586752082486093199[246] = 0; - out_586752082486093199[247] = 1; - out_586752082486093199[248] = 0; - out_586752082486093199[249] = 0; - out_586752082486093199[250] = 0; - out_586752082486093199[251] = 0; - out_586752082486093199[252] = 0; - out_586752082486093199[253] = 0; - out_586752082486093199[254] = 0; - out_586752082486093199[255] = 0; - out_586752082486093199[256] = 0; - out_586752082486093199[257] = 0; - out_586752082486093199[258] = 0; - out_586752082486093199[259] = 0; - out_586752082486093199[260] = 0; - out_586752082486093199[261] = 0; - out_586752082486093199[262] = 0; - out_586752082486093199[263] = 0; - out_586752082486093199[264] = 0; - out_586752082486093199[265] = 0; - out_586752082486093199[266] = 1; - out_586752082486093199[267] = 0; - out_586752082486093199[268] = 0; - out_586752082486093199[269] = 0; - out_586752082486093199[270] = 0; - out_586752082486093199[271] = 0; - out_586752082486093199[272] = 0; - out_586752082486093199[273] = 0; - out_586752082486093199[274] = 0; - out_586752082486093199[275] = 0; - out_586752082486093199[276] = 0; - out_586752082486093199[277] = 0; - out_586752082486093199[278] = 0; - out_586752082486093199[279] = 0; - out_586752082486093199[280] = 0; - out_586752082486093199[281] = 0; - out_586752082486093199[282] = 0; - out_586752082486093199[283] = 0; - out_586752082486093199[284] = 0; - out_586752082486093199[285] = 1; - out_586752082486093199[286] = 0; - out_586752082486093199[287] = 0; - out_586752082486093199[288] = 0; - out_586752082486093199[289] = 0; - out_586752082486093199[290] = 0; - out_586752082486093199[291] = 0; - out_586752082486093199[292] = 0; - out_586752082486093199[293] = 0; - out_586752082486093199[294] = 0; - out_586752082486093199[295] = 0; - out_586752082486093199[296] = 0; - out_586752082486093199[297] = 0; - out_586752082486093199[298] = 0; - out_586752082486093199[299] = 0; - out_586752082486093199[300] = 0; - out_586752082486093199[301] = 0; - out_586752082486093199[302] = 0; - out_586752082486093199[303] = 0; - out_586752082486093199[304] = 1; - out_586752082486093199[305] = 0; - out_586752082486093199[306] = 0; - out_586752082486093199[307] = 0; - out_586752082486093199[308] = 0; - out_586752082486093199[309] = 0; - out_586752082486093199[310] = 0; - out_586752082486093199[311] = 0; - out_586752082486093199[312] = 0; - out_586752082486093199[313] = 0; - out_586752082486093199[314] = 0; - out_586752082486093199[315] = 0; - out_586752082486093199[316] = 0; - out_586752082486093199[317] = 0; - out_586752082486093199[318] = 0; - out_586752082486093199[319] = 0; - out_586752082486093199[320] = 0; - out_586752082486093199[321] = 0; - out_586752082486093199[322] = 0; - out_586752082486093199[323] = 1; +void F_fun(double *state, double dt, double *out_6155576186277025652) { + out_6155576186277025652[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_6155576186277025652[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_6155576186277025652[2] = 0; + out_6155576186277025652[3] = 0; + out_6155576186277025652[4] = 0; + out_6155576186277025652[5] = 0; + out_6155576186277025652[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_6155576186277025652[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_6155576186277025652[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_6155576186277025652[9] = 0; + out_6155576186277025652[10] = 0; + out_6155576186277025652[11] = 0; + out_6155576186277025652[12] = 0; + out_6155576186277025652[13] = 0; + out_6155576186277025652[14] = 0; + out_6155576186277025652[15] = 0; + out_6155576186277025652[16] = 0; + out_6155576186277025652[17] = 0; + out_6155576186277025652[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_6155576186277025652[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_6155576186277025652[20] = 0; + out_6155576186277025652[21] = 0; + out_6155576186277025652[22] = 0; + out_6155576186277025652[23] = 0; + out_6155576186277025652[24] = 0; + out_6155576186277025652[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_6155576186277025652[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_6155576186277025652[27] = 0; + out_6155576186277025652[28] = 0; + out_6155576186277025652[29] = 0; + out_6155576186277025652[30] = 0; + out_6155576186277025652[31] = 0; + out_6155576186277025652[32] = 0; + out_6155576186277025652[33] = 0; + out_6155576186277025652[34] = 0; + out_6155576186277025652[35] = 0; + out_6155576186277025652[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_6155576186277025652[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_6155576186277025652[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_6155576186277025652[39] = 0; + out_6155576186277025652[40] = 0; + out_6155576186277025652[41] = 0; + out_6155576186277025652[42] = 0; + out_6155576186277025652[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_6155576186277025652[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_6155576186277025652[45] = 0; + out_6155576186277025652[46] = 0; + out_6155576186277025652[47] = 0; + out_6155576186277025652[48] = 0; + out_6155576186277025652[49] = 0; + out_6155576186277025652[50] = 0; + out_6155576186277025652[51] = 0; + out_6155576186277025652[52] = 0; + out_6155576186277025652[53] = 0; + out_6155576186277025652[54] = 0; + out_6155576186277025652[55] = 0; + out_6155576186277025652[56] = 0; + out_6155576186277025652[57] = 1; + out_6155576186277025652[58] = 0; + out_6155576186277025652[59] = 0; + out_6155576186277025652[60] = 0; + out_6155576186277025652[61] = 0; + out_6155576186277025652[62] = 0; + out_6155576186277025652[63] = 0; + out_6155576186277025652[64] = 0; + out_6155576186277025652[65] = 0; + out_6155576186277025652[66] = dt; + out_6155576186277025652[67] = 0; + out_6155576186277025652[68] = 0; + out_6155576186277025652[69] = 0; + out_6155576186277025652[70] = 0; + out_6155576186277025652[71] = 0; + out_6155576186277025652[72] = 0; + out_6155576186277025652[73] = 0; + out_6155576186277025652[74] = 0; + out_6155576186277025652[75] = 0; + out_6155576186277025652[76] = 1; + out_6155576186277025652[77] = 0; + out_6155576186277025652[78] = 0; + out_6155576186277025652[79] = 0; + out_6155576186277025652[80] = 0; + out_6155576186277025652[81] = 0; + out_6155576186277025652[82] = 0; + out_6155576186277025652[83] = 0; + out_6155576186277025652[84] = 0; + out_6155576186277025652[85] = dt; + out_6155576186277025652[86] = 0; + out_6155576186277025652[87] = 0; + out_6155576186277025652[88] = 0; + out_6155576186277025652[89] = 0; + out_6155576186277025652[90] = 0; + out_6155576186277025652[91] = 0; + out_6155576186277025652[92] = 0; + out_6155576186277025652[93] = 0; + out_6155576186277025652[94] = 0; + out_6155576186277025652[95] = 1; + out_6155576186277025652[96] = 0; + out_6155576186277025652[97] = 0; + out_6155576186277025652[98] = 0; + out_6155576186277025652[99] = 0; + out_6155576186277025652[100] = 0; + out_6155576186277025652[101] = 0; + out_6155576186277025652[102] = 0; + out_6155576186277025652[103] = 0; + out_6155576186277025652[104] = dt; + out_6155576186277025652[105] = 0; + out_6155576186277025652[106] = 0; + out_6155576186277025652[107] = 0; + out_6155576186277025652[108] = 0; + out_6155576186277025652[109] = 0; + out_6155576186277025652[110] = 0; + out_6155576186277025652[111] = 0; + out_6155576186277025652[112] = 0; + out_6155576186277025652[113] = 0; + out_6155576186277025652[114] = 1; + out_6155576186277025652[115] = 0; + out_6155576186277025652[116] = 0; + out_6155576186277025652[117] = 0; + out_6155576186277025652[118] = 0; + out_6155576186277025652[119] = 0; + out_6155576186277025652[120] = 0; + out_6155576186277025652[121] = 0; + out_6155576186277025652[122] = 0; + out_6155576186277025652[123] = 0; + out_6155576186277025652[124] = 0; + out_6155576186277025652[125] = 0; + out_6155576186277025652[126] = 0; + out_6155576186277025652[127] = 0; + out_6155576186277025652[128] = 0; + out_6155576186277025652[129] = 0; + out_6155576186277025652[130] = 0; + out_6155576186277025652[131] = 0; + out_6155576186277025652[132] = 0; + out_6155576186277025652[133] = 1; + out_6155576186277025652[134] = 0; + out_6155576186277025652[135] = 0; + out_6155576186277025652[136] = 0; + out_6155576186277025652[137] = 0; + out_6155576186277025652[138] = 0; + out_6155576186277025652[139] = 0; + out_6155576186277025652[140] = 0; + out_6155576186277025652[141] = 0; + out_6155576186277025652[142] = 0; + out_6155576186277025652[143] = 0; + out_6155576186277025652[144] = 0; + out_6155576186277025652[145] = 0; + out_6155576186277025652[146] = 0; + out_6155576186277025652[147] = 0; + out_6155576186277025652[148] = 0; + out_6155576186277025652[149] = 0; + out_6155576186277025652[150] = 0; + out_6155576186277025652[151] = 0; + out_6155576186277025652[152] = 1; + out_6155576186277025652[153] = 0; + out_6155576186277025652[154] = 0; + out_6155576186277025652[155] = 0; + out_6155576186277025652[156] = 0; + out_6155576186277025652[157] = 0; + out_6155576186277025652[158] = 0; + out_6155576186277025652[159] = 0; + out_6155576186277025652[160] = 0; + out_6155576186277025652[161] = 0; + out_6155576186277025652[162] = 0; + out_6155576186277025652[163] = 0; + out_6155576186277025652[164] = 0; + out_6155576186277025652[165] = 0; + out_6155576186277025652[166] = 0; + out_6155576186277025652[167] = 0; + out_6155576186277025652[168] = 0; + out_6155576186277025652[169] = 0; + out_6155576186277025652[170] = 0; + out_6155576186277025652[171] = 1; + out_6155576186277025652[172] = 0; + out_6155576186277025652[173] = 0; + out_6155576186277025652[174] = 0; + out_6155576186277025652[175] = 0; + out_6155576186277025652[176] = 0; + out_6155576186277025652[177] = 0; + out_6155576186277025652[178] = 0; + out_6155576186277025652[179] = 0; + out_6155576186277025652[180] = 0; + out_6155576186277025652[181] = 0; + out_6155576186277025652[182] = 0; + out_6155576186277025652[183] = 0; + out_6155576186277025652[184] = 0; + out_6155576186277025652[185] = 0; + out_6155576186277025652[186] = 0; + out_6155576186277025652[187] = 0; + out_6155576186277025652[188] = 0; + out_6155576186277025652[189] = 0; + out_6155576186277025652[190] = 1; + out_6155576186277025652[191] = 0; + out_6155576186277025652[192] = 0; + out_6155576186277025652[193] = 0; + out_6155576186277025652[194] = 0; + out_6155576186277025652[195] = 0; + out_6155576186277025652[196] = 0; + out_6155576186277025652[197] = 0; + out_6155576186277025652[198] = 0; + out_6155576186277025652[199] = 0; + out_6155576186277025652[200] = 0; + out_6155576186277025652[201] = 0; + out_6155576186277025652[202] = 0; + out_6155576186277025652[203] = 0; + out_6155576186277025652[204] = 0; + out_6155576186277025652[205] = 0; + out_6155576186277025652[206] = 0; + out_6155576186277025652[207] = 0; + out_6155576186277025652[208] = 0; + out_6155576186277025652[209] = 1; + out_6155576186277025652[210] = 0; + out_6155576186277025652[211] = 0; + out_6155576186277025652[212] = 0; + out_6155576186277025652[213] = 0; + out_6155576186277025652[214] = 0; + out_6155576186277025652[215] = 0; + out_6155576186277025652[216] = 0; + out_6155576186277025652[217] = 0; + out_6155576186277025652[218] = 0; + out_6155576186277025652[219] = 0; + out_6155576186277025652[220] = 0; + out_6155576186277025652[221] = 0; + out_6155576186277025652[222] = 0; + out_6155576186277025652[223] = 0; + out_6155576186277025652[224] = 0; + out_6155576186277025652[225] = 0; + out_6155576186277025652[226] = 0; + out_6155576186277025652[227] = 0; + out_6155576186277025652[228] = 1; + out_6155576186277025652[229] = 0; + out_6155576186277025652[230] = 0; + out_6155576186277025652[231] = 0; + out_6155576186277025652[232] = 0; + out_6155576186277025652[233] = 0; + out_6155576186277025652[234] = 0; + out_6155576186277025652[235] = 0; + out_6155576186277025652[236] = 0; + out_6155576186277025652[237] = 0; + out_6155576186277025652[238] = 0; + out_6155576186277025652[239] = 0; + out_6155576186277025652[240] = 0; + out_6155576186277025652[241] = 0; + out_6155576186277025652[242] = 0; + out_6155576186277025652[243] = 0; + out_6155576186277025652[244] = 0; + out_6155576186277025652[245] = 0; + out_6155576186277025652[246] = 0; + out_6155576186277025652[247] = 1; + out_6155576186277025652[248] = 0; + out_6155576186277025652[249] = 0; + out_6155576186277025652[250] = 0; + out_6155576186277025652[251] = 0; + out_6155576186277025652[252] = 0; + out_6155576186277025652[253] = 0; + out_6155576186277025652[254] = 0; + out_6155576186277025652[255] = 0; + out_6155576186277025652[256] = 0; + out_6155576186277025652[257] = 0; + out_6155576186277025652[258] = 0; + out_6155576186277025652[259] = 0; + out_6155576186277025652[260] = 0; + out_6155576186277025652[261] = 0; + out_6155576186277025652[262] = 0; + out_6155576186277025652[263] = 0; + out_6155576186277025652[264] = 0; + out_6155576186277025652[265] = 0; + out_6155576186277025652[266] = 1; + out_6155576186277025652[267] = 0; + out_6155576186277025652[268] = 0; + out_6155576186277025652[269] = 0; + out_6155576186277025652[270] = 0; + out_6155576186277025652[271] = 0; + out_6155576186277025652[272] = 0; + out_6155576186277025652[273] = 0; + out_6155576186277025652[274] = 0; + out_6155576186277025652[275] = 0; + out_6155576186277025652[276] = 0; + out_6155576186277025652[277] = 0; + out_6155576186277025652[278] = 0; + out_6155576186277025652[279] = 0; + out_6155576186277025652[280] = 0; + out_6155576186277025652[281] = 0; + out_6155576186277025652[282] = 0; + out_6155576186277025652[283] = 0; + out_6155576186277025652[284] = 0; + out_6155576186277025652[285] = 1; + out_6155576186277025652[286] = 0; + out_6155576186277025652[287] = 0; + out_6155576186277025652[288] = 0; + out_6155576186277025652[289] = 0; + out_6155576186277025652[290] = 0; + out_6155576186277025652[291] = 0; + out_6155576186277025652[292] = 0; + out_6155576186277025652[293] = 0; + out_6155576186277025652[294] = 0; + out_6155576186277025652[295] = 0; + out_6155576186277025652[296] = 0; + out_6155576186277025652[297] = 0; + out_6155576186277025652[298] = 0; + out_6155576186277025652[299] = 0; + out_6155576186277025652[300] = 0; + out_6155576186277025652[301] = 0; + out_6155576186277025652[302] = 0; + out_6155576186277025652[303] = 0; + out_6155576186277025652[304] = 1; + out_6155576186277025652[305] = 0; + out_6155576186277025652[306] = 0; + out_6155576186277025652[307] = 0; + out_6155576186277025652[308] = 0; + out_6155576186277025652[309] = 0; + out_6155576186277025652[310] = 0; + out_6155576186277025652[311] = 0; + out_6155576186277025652[312] = 0; + out_6155576186277025652[313] = 0; + out_6155576186277025652[314] = 0; + out_6155576186277025652[315] = 0; + out_6155576186277025652[316] = 0; + out_6155576186277025652[317] = 0; + out_6155576186277025652[318] = 0; + out_6155576186277025652[319] = 0; + out_6155576186277025652[320] = 0; + out_6155576186277025652[321] = 0; + out_6155576186277025652[322] = 0; + out_6155576186277025652[323] = 1; } -void h_4(double *state, double *unused, double *out_1330626957984611772) { - out_1330626957984611772[0] = state[6] + state[9]; - out_1330626957984611772[1] = state[7] + state[10]; - out_1330626957984611772[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_8035983586487788014) { + out_8035983586487788014[0] = state[6] + state[9]; + out_8035983586487788014[1] = state[7] + state[10]; + out_8035983586487788014[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_8068529884540655173) { - out_8068529884540655173[0] = 0; - out_8068529884540655173[1] = 0; - out_8068529884540655173[2] = 0; - out_8068529884540655173[3] = 0; - out_8068529884540655173[4] = 0; - out_8068529884540655173[5] = 0; - out_8068529884540655173[6] = 1; - out_8068529884540655173[7] = 0; - out_8068529884540655173[8] = 0; - out_8068529884540655173[9] = 1; - out_8068529884540655173[10] = 0; - out_8068529884540655173[11] = 0; - out_8068529884540655173[12] = 0; - out_8068529884540655173[13] = 0; - out_8068529884540655173[14] = 0; - out_8068529884540655173[15] = 0; - out_8068529884540655173[16] = 0; - out_8068529884540655173[17] = 0; - out_8068529884540655173[18] = 0; - out_8068529884540655173[19] = 0; - out_8068529884540655173[20] = 0; - out_8068529884540655173[21] = 0; - out_8068529884540655173[22] = 0; - out_8068529884540655173[23] = 0; - out_8068529884540655173[24] = 0; - out_8068529884540655173[25] = 1; - out_8068529884540655173[26] = 0; - out_8068529884540655173[27] = 0; - out_8068529884540655173[28] = 1; - out_8068529884540655173[29] = 0; - out_8068529884540655173[30] = 0; - out_8068529884540655173[31] = 0; - out_8068529884540655173[32] = 0; - out_8068529884540655173[33] = 0; - out_8068529884540655173[34] = 0; - out_8068529884540655173[35] = 0; - out_8068529884540655173[36] = 0; - out_8068529884540655173[37] = 0; - out_8068529884540655173[38] = 0; - out_8068529884540655173[39] = 0; - out_8068529884540655173[40] = 0; - out_8068529884540655173[41] = 0; - out_8068529884540655173[42] = 0; - out_8068529884540655173[43] = 0; - out_8068529884540655173[44] = 1; - out_8068529884540655173[45] = 0; - out_8068529884540655173[46] = 0; - out_8068529884540655173[47] = 1; - out_8068529884540655173[48] = 0; - out_8068529884540655173[49] = 0; - out_8068529884540655173[50] = 0; - out_8068529884540655173[51] = 0; - out_8068529884540655173[52] = 0; - out_8068529884540655173[53] = 0; +void H_4(double *state, double *unused, double *out_9211072400700188435) { + out_9211072400700188435[0] = 0; + out_9211072400700188435[1] = 0; + out_9211072400700188435[2] = 0; + out_9211072400700188435[3] = 0; + out_9211072400700188435[4] = 0; + out_9211072400700188435[5] = 0; + out_9211072400700188435[6] = 1; + out_9211072400700188435[7] = 0; + out_9211072400700188435[8] = 0; + out_9211072400700188435[9] = 1; + out_9211072400700188435[10] = 0; + out_9211072400700188435[11] = 0; + out_9211072400700188435[12] = 0; + out_9211072400700188435[13] = 0; + out_9211072400700188435[14] = 0; + out_9211072400700188435[15] = 0; + out_9211072400700188435[16] = 0; + out_9211072400700188435[17] = 0; + out_9211072400700188435[18] = 0; + out_9211072400700188435[19] = 0; + out_9211072400700188435[20] = 0; + out_9211072400700188435[21] = 0; + out_9211072400700188435[22] = 0; + out_9211072400700188435[23] = 0; + out_9211072400700188435[24] = 0; + out_9211072400700188435[25] = 1; + out_9211072400700188435[26] = 0; + out_9211072400700188435[27] = 0; + out_9211072400700188435[28] = 1; + out_9211072400700188435[29] = 0; + out_9211072400700188435[30] = 0; + out_9211072400700188435[31] = 0; + out_9211072400700188435[32] = 0; + out_9211072400700188435[33] = 0; + out_9211072400700188435[34] = 0; + out_9211072400700188435[35] = 0; + out_9211072400700188435[36] = 0; + out_9211072400700188435[37] = 0; + out_9211072400700188435[38] = 0; + out_9211072400700188435[39] = 0; + out_9211072400700188435[40] = 0; + out_9211072400700188435[41] = 0; + out_9211072400700188435[42] = 0; + out_9211072400700188435[43] = 0; + out_9211072400700188435[44] = 1; + out_9211072400700188435[45] = 0; + out_9211072400700188435[46] = 0; + out_9211072400700188435[47] = 1; + out_9211072400700188435[48] = 0; + out_9211072400700188435[49] = 0; + out_9211072400700188435[50] = 0; + out_9211072400700188435[51] = 0; + out_9211072400700188435[52] = 0; + out_9211072400700188435[53] = 0; } -void h_10(double *state, double *unused, double *out_5440536108172962293) { - out_5440536108172962293[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_5440536108172962293[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_5440536108172962293[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_2517419169749679290) { + out_2517419169749679290[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_2517419169749679290[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_2517419169749679290[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_3777395375513848052) { - out_3777395375513848052[0] = 0; - out_3777395375513848052[1] = 9.8100000000000005*cos(state[1]); - out_3777395375513848052[2] = 0; - out_3777395375513848052[3] = 0; - out_3777395375513848052[4] = -state[8]; - out_3777395375513848052[5] = state[7]; - out_3777395375513848052[6] = 0; - out_3777395375513848052[7] = state[5]; - out_3777395375513848052[8] = -state[4]; - out_3777395375513848052[9] = 0; - out_3777395375513848052[10] = 0; - out_3777395375513848052[11] = 0; - out_3777395375513848052[12] = 1; - out_3777395375513848052[13] = 0; - out_3777395375513848052[14] = 0; - out_3777395375513848052[15] = 1; - out_3777395375513848052[16] = 0; - out_3777395375513848052[17] = 0; - out_3777395375513848052[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_3777395375513848052[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_3777395375513848052[20] = 0; - out_3777395375513848052[21] = state[8]; - out_3777395375513848052[22] = 0; - out_3777395375513848052[23] = -state[6]; - out_3777395375513848052[24] = -state[5]; - out_3777395375513848052[25] = 0; - out_3777395375513848052[26] = state[3]; - out_3777395375513848052[27] = 0; - out_3777395375513848052[28] = 0; - out_3777395375513848052[29] = 0; - out_3777395375513848052[30] = 0; - out_3777395375513848052[31] = 1; - out_3777395375513848052[32] = 0; - out_3777395375513848052[33] = 0; - out_3777395375513848052[34] = 1; - out_3777395375513848052[35] = 0; - out_3777395375513848052[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_3777395375513848052[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_3777395375513848052[38] = 0; - out_3777395375513848052[39] = -state[7]; - out_3777395375513848052[40] = state[6]; - out_3777395375513848052[41] = 0; - out_3777395375513848052[42] = state[4]; - out_3777395375513848052[43] = -state[3]; - out_3777395375513848052[44] = 0; - out_3777395375513848052[45] = 0; - out_3777395375513848052[46] = 0; - out_3777395375513848052[47] = 0; - out_3777395375513848052[48] = 0; - out_3777395375513848052[49] = 0; - out_3777395375513848052[50] = 1; - out_3777395375513848052[51] = 0; - out_3777395375513848052[52] = 0; - out_3777395375513848052[53] = 1; +void H_10(double *state, double *unused, double *out_3554499959659757522) { + out_3554499959659757522[0] = 0; + out_3554499959659757522[1] = 9.8100000000000005*cos(state[1]); + out_3554499959659757522[2] = 0; + out_3554499959659757522[3] = 0; + out_3554499959659757522[4] = -state[8]; + out_3554499959659757522[5] = state[7]; + out_3554499959659757522[6] = 0; + out_3554499959659757522[7] = state[5]; + out_3554499959659757522[8] = -state[4]; + out_3554499959659757522[9] = 0; + out_3554499959659757522[10] = 0; + out_3554499959659757522[11] = 0; + out_3554499959659757522[12] = 1; + out_3554499959659757522[13] = 0; + out_3554499959659757522[14] = 0; + out_3554499959659757522[15] = 1; + out_3554499959659757522[16] = 0; + out_3554499959659757522[17] = 0; + out_3554499959659757522[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_3554499959659757522[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_3554499959659757522[20] = 0; + out_3554499959659757522[21] = state[8]; + out_3554499959659757522[22] = 0; + out_3554499959659757522[23] = -state[6]; + out_3554499959659757522[24] = -state[5]; + out_3554499959659757522[25] = 0; + out_3554499959659757522[26] = state[3]; + out_3554499959659757522[27] = 0; + out_3554499959659757522[28] = 0; + out_3554499959659757522[29] = 0; + out_3554499959659757522[30] = 0; + out_3554499959659757522[31] = 1; + out_3554499959659757522[32] = 0; + out_3554499959659757522[33] = 0; + out_3554499959659757522[34] = 1; + out_3554499959659757522[35] = 0; + out_3554499959659757522[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_3554499959659757522[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_3554499959659757522[38] = 0; + out_3554499959659757522[39] = -state[7]; + out_3554499959659757522[40] = state[6]; + out_3554499959659757522[41] = 0; + out_3554499959659757522[42] = state[4]; + out_3554499959659757522[43] = -state[3]; + out_3554499959659757522[44] = 0; + out_3554499959659757522[45] = 0; + out_3554499959659757522[46] = 0; + out_3554499959659757522[47] = 0; + out_3554499959659757522[48] = 0; + out_3554499959659757522[49] = 0; + out_3554499959659757522[50] = 1; + out_3554499959659757522[51] = 0; + out_3554499959659757522[52] = 0; + out_3554499959659757522[53] = 1; } -void h_13(double *state, double *unused, double *out_7911476404081815631) { - out_7911476404081815631[0] = state[3]; - out_7911476404081815631[1] = state[4]; - out_7911476404081815631[2] = state[5]; +void h_13(double *state, double *unused, double *out_231752859429249092) { + out_231752859429249092[0] = state[3]; + out_231752859429249092[1] = state[4]; + out_231752859429249092[2] = state[5]; } -void H_13(double *state, double *unused, double *out_7165940363836563642) { - out_7165940363836563642[0] = 0; - out_7165940363836563642[1] = 0; - out_7165940363836563642[2] = 0; - out_7165940363836563642[3] = 1; - out_7165940363836563642[4] = 0; - out_7165940363836563642[5] = 0; - out_7165940363836563642[6] = 0; - out_7165940363836563642[7] = 0; - out_7165940363836563642[8] = 0; - out_7165940363836563642[9] = 0; - out_7165940363836563642[10] = 0; - out_7165940363836563642[11] = 0; - out_7165940363836563642[12] = 0; - out_7165940363836563642[13] = 0; - out_7165940363836563642[14] = 0; - out_7165940363836563642[15] = 0; - out_7165940363836563642[16] = 0; - out_7165940363836563642[17] = 0; - out_7165940363836563642[18] = 0; - out_7165940363836563642[19] = 0; - out_7165940363836563642[20] = 0; - out_7165940363836563642[21] = 0; - out_7165940363836563642[22] = 1; - out_7165940363836563642[23] = 0; - out_7165940363836563642[24] = 0; - out_7165940363836563642[25] = 0; - out_7165940363836563642[26] = 0; - out_7165940363836563642[27] = 0; - out_7165940363836563642[28] = 0; - out_7165940363836563642[29] = 0; - out_7165940363836563642[30] = 0; - out_7165940363836563642[31] = 0; - out_7165940363836563642[32] = 0; - out_7165940363836563642[33] = 0; - out_7165940363836563642[34] = 0; - out_7165940363836563642[35] = 0; - out_7165940363836563642[36] = 0; - out_7165940363836563642[37] = 0; - out_7165940363836563642[38] = 0; - out_7165940363836563642[39] = 0; - out_7165940363836563642[40] = 0; - out_7165940363836563642[41] = 1; - out_7165940363836563642[42] = 0; - out_7165940363836563642[43] = 0; - out_7165940363836563642[44] = 0; - out_7165940363836563642[45] = 0; - out_7165940363836563642[46] = 0; - out_7165940363836563642[47] = 0; - out_7165940363836563642[48] = 0; - out_7165940363836563642[49] = 0; - out_7165940363836563642[50] = 0; - out_7165940363836563642[51] = 0; - out_7165940363836563642[52] = 0; - out_7165940363836563642[53] = 0; +void H_13(double *state, double *unused, double *out_6023397847677030380) { + out_6023397847677030380[0] = 0; + out_6023397847677030380[1] = 0; + out_6023397847677030380[2] = 0; + out_6023397847677030380[3] = 1; + out_6023397847677030380[4] = 0; + out_6023397847677030380[5] = 0; + out_6023397847677030380[6] = 0; + out_6023397847677030380[7] = 0; + out_6023397847677030380[8] = 0; + out_6023397847677030380[9] = 0; + out_6023397847677030380[10] = 0; + out_6023397847677030380[11] = 0; + out_6023397847677030380[12] = 0; + out_6023397847677030380[13] = 0; + out_6023397847677030380[14] = 0; + out_6023397847677030380[15] = 0; + out_6023397847677030380[16] = 0; + out_6023397847677030380[17] = 0; + out_6023397847677030380[18] = 0; + out_6023397847677030380[19] = 0; + out_6023397847677030380[20] = 0; + out_6023397847677030380[21] = 0; + out_6023397847677030380[22] = 1; + out_6023397847677030380[23] = 0; + out_6023397847677030380[24] = 0; + out_6023397847677030380[25] = 0; + out_6023397847677030380[26] = 0; + out_6023397847677030380[27] = 0; + out_6023397847677030380[28] = 0; + out_6023397847677030380[29] = 0; + out_6023397847677030380[30] = 0; + out_6023397847677030380[31] = 0; + out_6023397847677030380[32] = 0; + out_6023397847677030380[33] = 0; + out_6023397847677030380[34] = 0; + out_6023397847677030380[35] = 0; + out_6023397847677030380[36] = 0; + out_6023397847677030380[37] = 0; + out_6023397847677030380[38] = 0; + out_6023397847677030380[39] = 0; + out_6023397847677030380[40] = 0; + out_6023397847677030380[41] = 1; + out_6023397847677030380[42] = 0; + out_6023397847677030380[43] = 0; + out_6023397847677030380[44] = 0; + out_6023397847677030380[45] = 0; + out_6023397847677030380[46] = 0; + out_6023397847677030380[47] = 0; + out_6023397847677030380[48] = 0; + out_6023397847677030380[49] = 0; + out_6023397847677030380[50] = 0; + out_6023397847677030380[51] = 0; + out_6023397847677030380[52] = 0; + out_6023397847677030380[53] = 0; } -void h_14(double *state, double *unused, double *out_5129805537420960347) { - out_5129805537420960347[0] = state[6]; - out_5129805537420960347[1] = state[7]; - out_5129805537420960347[2] = state[8]; +void h_14(double *state, double *unused, double *out_4883626546262470581) { + out_4883626546262470581[0] = state[6]; + out_4883626546262470581[1] = state[7]; + out_4883626546262470581[2] = state[8]; } -void H_14(double *state, double *unused, double *out_4985741452245282877) { - out_4985741452245282877[0] = 0; - out_4985741452245282877[1] = 0; - out_4985741452245282877[2] = 0; - out_4985741452245282877[3] = 0; - out_4985741452245282877[4] = 0; - out_4985741452245282877[5] = 0; - out_4985741452245282877[6] = 1; - out_4985741452245282877[7] = 0; - out_4985741452245282877[8] = 0; - out_4985741452245282877[9] = 0; - out_4985741452245282877[10] = 0; - out_4985741452245282877[11] = 0; - out_4985741452245282877[12] = 0; - out_4985741452245282877[13] = 0; - out_4985741452245282877[14] = 0; - out_4985741452245282877[15] = 0; - out_4985741452245282877[16] = 0; - out_4985741452245282877[17] = 0; - out_4985741452245282877[18] = 0; - out_4985741452245282877[19] = 0; - out_4985741452245282877[20] = 0; - out_4985741452245282877[21] = 0; - out_4985741452245282877[22] = 0; - out_4985741452245282877[23] = 0; - out_4985741452245282877[24] = 0; - out_4985741452245282877[25] = 1; - out_4985741452245282877[26] = 0; - out_4985741452245282877[27] = 0; - out_4985741452245282877[28] = 0; - out_4985741452245282877[29] = 0; - out_4985741452245282877[30] = 0; - out_4985741452245282877[31] = 0; - out_4985741452245282877[32] = 0; - out_4985741452245282877[33] = 0; - out_4985741452245282877[34] = 0; - out_4985741452245282877[35] = 0; - out_4985741452245282877[36] = 0; - out_4985741452245282877[37] = 0; - out_4985741452245282877[38] = 0; - out_4985741452245282877[39] = 0; - out_4985741452245282877[40] = 0; - out_4985741452245282877[41] = 0; - out_4985741452245282877[42] = 0; - out_4985741452245282877[43] = 0; - out_4985741452245282877[44] = 1; - out_4985741452245282877[45] = 0; - out_4985741452245282877[46] = 0; - out_4985741452245282877[47] = 0; - out_4985741452245282877[48] = 0; - out_4985741452245282877[49] = 0; - out_4985741452245282877[50] = 0; - out_4985741452245282877[51] = 0; - out_4985741452245282877[52] = 0; - out_4985741452245282877[53] = 0; +void H_14(double *state, double *unused, double *out_6128283968404816139) { + out_6128283968404816139[0] = 0; + out_6128283968404816139[1] = 0; + out_6128283968404816139[2] = 0; + out_6128283968404816139[3] = 0; + out_6128283968404816139[4] = 0; + out_6128283968404816139[5] = 0; + out_6128283968404816139[6] = 1; + out_6128283968404816139[7] = 0; + out_6128283968404816139[8] = 0; + out_6128283968404816139[9] = 0; + out_6128283968404816139[10] = 0; + out_6128283968404816139[11] = 0; + out_6128283968404816139[12] = 0; + out_6128283968404816139[13] = 0; + out_6128283968404816139[14] = 0; + out_6128283968404816139[15] = 0; + out_6128283968404816139[16] = 0; + out_6128283968404816139[17] = 0; + out_6128283968404816139[18] = 0; + out_6128283968404816139[19] = 0; + out_6128283968404816139[20] = 0; + out_6128283968404816139[21] = 0; + out_6128283968404816139[22] = 0; + out_6128283968404816139[23] = 0; + out_6128283968404816139[24] = 0; + out_6128283968404816139[25] = 1; + out_6128283968404816139[26] = 0; + out_6128283968404816139[27] = 0; + out_6128283968404816139[28] = 0; + out_6128283968404816139[29] = 0; + out_6128283968404816139[30] = 0; + out_6128283968404816139[31] = 0; + out_6128283968404816139[32] = 0; + out_6128283968404816139[33] = 0; + out_6128283968404816139[34] = 0; + out_6128283968404816139[35] = 0; + out_6128283968404816139[36] = 0; + out_6128283968404816139[37] = 0; + out_6128283968404816139[38] = 0; + out_6128283968404816139[39] = 0; + out_6128283968404816139[40] = 0; + out_6128283968404816139[41] = 0; + out_6128283968404816139[42] = 0; + out_6128283968404816139[43] = 0; + out_6128283968404816139[44] = 1; + out_6128283968404816139[45] = 0; + out_6128283968404816139[46] = 0; + out_6128283968404816139[47] = 0; + out_6128283968404816139[48] = 0; + out_6128283968404816139[49] = 0; + out_6128283968404816139[50] = 0; + out_6128283968404816139[51] = 0; + out_6128283968404816139[52] = 0; + out_6128283968404816139[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_731387772078245243) { - err_fun(nom_x, delta_x, out_731387772078245243); +void pose_err_fun(double *nom_x, double *delta_x, double *out_2114100039121924448) { + err_fun(nom_x, delta_x, out_2114100039121924448); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_4206130707332622452) { - inv_err_fun(nom_x, true_x, out_4206130707332622452); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2618631132718776507) { + inv_err_fun(nom_x, true_x, out_2618631132718776507); } -void pose_H_mod_fun(double *state, double *out_2916011499501574438) { - H_mod_fun(state, out_2916011499501574438); +void pose_H_mod_fun(double *state, double *out_4058554015661107700) { + H_mod_fun(state, out_4058554015661107700); } -void pose_f_fun(double *state, double dt, double *out_4420889553799640425) { - f_fun(state, dt, out_4420889553799640425); +void pose_f_fun(double *state, double dt, double *out_5119116025028968719) { + f_fun(state, dt, out_5119116025028968719); } -void pose_F_fun(double *state, double dt, double *out_586752082486093199) { - F_fun(state, dt, out_586752082486093199); +void pose_F_fun(double *state, double dt, double *out_6155576186277025652) { + F_fun(state, dt, out_6155576186277025652); } -void pose_h_4(double *state, double *unused, double *out_1330626957984611772) { - h_4(state, unused, out_1330626957984611772); +void pose_h_4(double *state, double *unused, double *out_8035983586487788014) { + h_4(state, unused, out_8035983586487788014); } -void pose_H_4(double *state, double *unused, double *out_8068529884540655173) { - H_4(state, unused, out_8068529884540655173); +void pose_H_4(double *state, double *unused, double *out_9211072400700188435) { + H_4(state, unused, out_9211072400700188435); } -void pose_h_10(double *state, double *unused, double *out_5440536108172962293) { - h_10(state, unused, out_5440536108172962293); +void pose_h_10(double *state, double *unused, double *out_2517419169749679290) { + h_10(state, unused, out_2517419169749679290); } -void pose_H_10(double *state, double *unused, double *out_3777395375513848052) { - H_10(state, unused, out_3777395375513848052); +void pose_H_10(double *state, double *unused, double *out_3554499959659757522) { + H_10(state, unused, out_3554499959659757522); } -void pose_h_13(double *state, double *unused, double *out_7911476404081815631) { - h_13(state, unused, out_7911476404081815631); +void pose_h_13(double *state, double *unused, double *out_231752859429249092) { + h_13(state, unused, out_231752859429249092); } -void pose_H_13(double *state, double *unused, double *out_7165940363836563642) { - H_13(state, unused, out_7165940363836563642); +void pose_H_13(double *state, double *unused, double *out_6023397847677030380) { + H_13(state, unused, out_6023397847677030380); } -void pose_h_14(double *state, double *unused, double *out_5129805537420960347) { - h_14(state, unused, out_5129805537420960347); +void pose_h_14(double *state, double *unused, double *out_4883626546262470581) { + h_14(state, unused, out_4883626546262470581); } -void pose_H_14(double *state, double *unused, double *out_4985741452245282877) { - H_14(state, unused, out_4985741452245282877); +void pose_H_14(double *state, double *unused, double *out_6128283968404816139) { + H_14(state, unused, out_6128283968404816139); } 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 98c89066d0..5262b05948 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_731387772078245243); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_4206130707332622452); -void pose_H_mod_fun(double *state, double *out_2916011499501574438); -void pose_f_fun(double *state, double dt, double *out_4420889553799640425); -void pose_F_fun(double *state, double dt, double *out_586752082486093199); -void pose_h_4(double *state, double *unused, double *out_1330626957984611772); -void pose_H_4(double *state, double *unused, double *out_8068529884540655173); -void pose_h_10(double *state, double *unused, double *out_5440536108172962293); -void pose_H_10(double *state, double *unused, double *out_3777395375513848052); -void pose_h_13(double *state, double *unused, double *out_7911476404081815631); -void pose_H_13(double *state, double *unused, double *out_7165940363836563642); -void pose_h_14(double *state, double *unused, double *out_5129805537420960347); -void pose_H_14(double *state, double *unused, double *out_4985741452245282877); +void pose_err_fun(double *nom_x, double *delta_x, double *out_2114100039121924448); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2618631132718776507); +void pose_H_mod_fun(double *state, double *out_4058554015661107700); +void pose_f_fun(double *state, double dt, double *out_5119116025028968719); +void pose_F_fun(double *state, double dt, double *out_6155576186277025652); +void pose_h_4(double *state, double *unused, double *out_8035983586487788014); +void pose_H_4(double *state, double *unused, double *out_9211072400700188435); +void pose_h_10(double *state, double *unused, double *out_2517419169749679290); +void pose_H_10(double *state, double *unused, double *out_3554499959659757522); +void pose_h_13(double *state, double *unused, double *out_231752859429249092); +void pose_H_13(double *state, double *unused, double *out_6023397847677030380); +void pose_h_14(double *state, double *unused, double *out_4883626546262470581); +void pose_H_14(double *state, double *unused, double *out_6128283968404816139); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/pose.os b/selfdrive/locationd/models/generated/pose.os index b9449ef3d7..d32b2d3bba 100644 Binary files a/selfdrive/locationd/models/generated/pose.os and b/selfdrive/locationd/models/generated/pose.os differ diff --git a/selfdrive/pandad/pandad b/selfdrive/pandad/pandad index a8cd14249c..ab9dc2c82a 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/system/camerad/camerad b/system/camerad/camerad index ca9381f056..70cdb18604 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index b79360c1ed..9b9dbcaa88 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index 3730355887..54294b21e2 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index 372fe1445b..8f9c4e9db6 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ