diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index b86b53110..4f873f768 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 372ef37f7..a3889203f 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 b34aa61c7..c91b8db62 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 ef05b637e..40801a843 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 37fea4a2e..ba76d0463 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 43f0bcd3d..0d6451dde 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 43f0bcd3d..0d6451dde 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 cdb2f9bd4..1f6f82cf9 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 cdb2f9bd4..1f6f82cf9 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 cdb2f9bd4..1f6f82cf9 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 cdb2f9bd4..1f6f82cf9 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 cdb2f9bd4..1f6f82cf9 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 cdb2f9bd4..1f6f82cf9 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_hkg_remote.bin b/panda/board/obj/bootstub.panda_hkg_remote.bin index 43f0bcd3d..0d6451dde 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 43f0bcd3d..0d6451dde 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 87e2a5733..5860b1074 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 43f0bcd3d..0d6451dde 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 43f0bcd3d..0d6451dde 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/gitversion.h b/panda/board/obj/gitversion.h index bdc8c59bd..3424696b8 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-77db3e50-DEBUG"; +const uint8_t gitversion[19] = "DEV-d4cfd342-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index b1b55f4ef..8ba01ae55 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 9fd497d13..0574ad9bc 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 7783d4293..b2a56e640 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 aa50436e6..1a933a55c 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 e6679a4ea..1a0e88762 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 542282947..8e1a7c90b 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 98af2e333..f668628fe 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 3ec917cb1..142c372fc 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 abf105704..d732551a8 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 84e46f95d..f4c6d2dc2 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 1fb807a20..2d750cd66 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 093ea8a2d..7468bbee2 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 39a4096a6..093392f92 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 4766b699c..64675c91b 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 b7738d668..d0105fe5b 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 855247925..aafaf7d16 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 8abbd0af2..8772f2004 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 221448917..8327db042 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 6813b75e8..2e09b8f39 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 c3202b305..f89677e5a 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 f16eed804..288dd63f2 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 3367da9ed..3c23f34b9 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 9f1d071dc..6ff593ae0 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 4d309b94c..079f45032 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 b2c821d7c..9d02ff1c1 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 563af0207..edc88c89a 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 da4ef8baf..20e1c9d74 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 369bb80a7..07c9d11ca 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 3b202412c..74085e9bc 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 c51a42f74..3bd45e9b2 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 fcb3f290b..191ef4d67 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 ac2f4530b..8208ab7be 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_hkg_remote.bin.signed b/panda/board/obj/panda_hkg_remote.bin.signed index 325079bd4..1e4c9ae4d 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 142d54e59..c668f5169 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 bad0b7576..efcd6d353 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 947bc8e4c..7d4b94005 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 49b7222b9..82afaa2af 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 6ec690799..9535227de 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 1cf6d6f6c..d84c19497 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 6f27b0e59..405880557 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 534c83a50..61c67dcc1 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 787402e7c..a03cea7c5 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 d72d8aa94..d6cfe2b59 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 aa674e8f6..372ddfd6e 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 660f76cd3..856b02fa7 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 04c6a8c1c..4793bee24 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 b22a2d724..b20fca952 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 13b2b27e2..be5c4335f 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 a94b88970..3433c30be 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 1e649db93..000050db2 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 cab76df5e..1f8bcd067 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 a4e07768a..36983295f 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/version b/panda/board/obj/version index 8a0b05cfd..bb8d818d6 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-77db3e50-DEBUG \ No newline at end of file +DEV-d4cfd342-DEBUG \ No newline at end of file diff --git a/scripts/speed_limit_vision/import_bookmark_leadins.py b/scripts/speed_limit_vision/import_bookmark_leadins.py index 147d90163..469b429fd 100644 --- a/scripts/speed_limit_vision/import_bookmark_leadins.py +++ b/scripts/speed_limit_vision/import_bookmark_leadins.py @@ -62,17 +62,36 @@ def load_existing_rows(manifest_path: Path) -> dict[str, dict[str, str]]: return {row["record_key"]: row for row in reader if row.get("record_key")} -def read_frame_at(video_path: Path, target_time_s: float): +def read_frames_at(video_path: Path, target_times_s: list[float]): capture = cv2.VideoCapture(str(video_path)) fps = capture.get(cv2.CAP_PROP_FPS) or 20.0 - frame_index = max(int(round(target_time_s * fps)), 0) - capture.set(cv2.CAP_PROP_POS_FRAMES, frame_index) - ok, frame_bgr = capture.read() + targets = sorted((max(int(round(target_time_s * fps)), 0), target_time_s) for target_time_s in target_times_s) + results = {} + frame_index = 0 + last_frame_index = -1 + last_frame_bgr = None + + for target_frame_index, target_time_s in targets: + if target_frame_index == last_frame_index and last_frame_bgr is not None: + results[target_time_s] = (last_frame_index / fps, last_frame_bgr.copy()) + continue + + while frame_index < target_frame_index: + if not capture.grab(): + capture.release() + return results + frame_index += 1 + + ok, frame_bgr = capture.read() + if not ok or frame_bgr is None: + break + last_frame_index = frame_index + last_frame_bgr = frame_bgr + results[target_time_s] = (frame_index / fps, frame_bgr) + frame_index += 1 + capture.release() - if not ok or frame_bgr is None: - return None - actual_time_s = frame_index / fps - return actual_time_s, frame_bgr + return results def sample_offsets(leadin_start_s: float, segment_offset_s: float, sample_every: float, max_samples: int): @@ -104,7 +123,7 @@ def extract_window_frames(row: dict, clip_root: Path, sample_every: float, max_s leadin_start_s = float(row["leadinStartS"]) spans_previous_segment = bool(row.get("spansPreviousSegment")) - sampled = [] + requests = [] previous_clip = clip_root / f"{route}--{segment - 1}" / "fcamera.hevc" current_clip = clip_root / f"{route}--{segment}" / "fcamera.hevc" @@ -119,10 +138,18 @@ def extract_window_frames(row: dict, clip_root: Path, sample_every: float, max_s if not source_video.is_file(): continue - frame_info = read_frame_at(source_video, source_time_s) + requests.append((relative_offset_s, source_video, source_time_s)) + + decoded_by_video = {} + for source_video in {request[1] for request in requests}: + target_times_s = [request[2] for request in requests if request[1] == source_video] + decoded_by_video[source_video] = read_frames_at(source_video, target_times_s) + + sampled = [] + for relative_offset_s, source_video, source_time_s in requests: + frame_info = decoded_by_video[source_video].get(source_time_s) if frame_info is None: continue - actual_time_s, frame_bgr = frame_info sampled.append({ "relative_offset_s": relative_offset_s, diff --git a/scripts/speed_limit_vision/mine_connect_route_bookmarks.py b/scripts/speed_limit_vision/mine_connect_route_bookmarks.py index b1362464f..29b2940d0 100644 --- a/scripts/speed_limit_vision/mine_connect_route_bookmarks.py +++ b/scripts/speed_limit_vision/mine_connect_route_bookmarks.py @@ -101,6 +101,8 @@ def load_route_bookmarks(clip_root: Path, log_id: str) -> list[dict]: if event_type == "userBookmark": deduped[-1]["event_type"] = event_type deduped[-1]["route_time_s"] = route_time_s + deduped[-1]["segment"] = max(int(route_time_s // 60.0), 0) + deduped[-1]["segment_offset_s"] = route_time_s - deduped[-1]["segment"] * 60.0 continue segment = max(int(route_time_s // 60.0), 0) diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 29965aba3..894ebc8a9 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_9086440322595224646) { - out_9086440322595224646[0] = delta_x[0] + nom_x[0]; - out_9086440322595224646[1] = delta_x[1] + nom_x[1]; - out_9086440322595224646[2] = delta_x[2] + nom_x[2]; - out_9086440322595224646[3] = delta_x[3] + nom_x[3]; - out_9086440322595224646[4] = delta_x[4] + nom_x[4]; - out_9086440322595224646[5] = delta_x[5] + nom_x[5]; - out_9086440322595224646[6] = delta_x[6] + nom_x[6]; - out_9086440322595224646[7] = delta_x[7] + nom_x[7]; - out_9086440322595224646[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_7550976477549507356) { + out_7550976477549507356[0] = delta_x[0] + nom_x[0]; + out_7550976477549507356[1] = delta_x[1] + nom_x[1]; + out_7550976477549507356[2] = delta_x[2] + nom_x[2]; + out_7550976477549507356[3] = delta_x[3] + nom_x[3]; + out_7550976477549507356[4] = delta_x[4] + nom_x[4]; + out_7550976477549507356[5] = delta_x[5] + nom_x[5]; + out_7550976477549507356[6] = delta_x[6] + nom_x[6]; + out_7550976477549507356[7] = delta_x[7] + nom_x[7]; + out_7550976477549507356[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_7805211099664095660) { - out_7805211099664095660[0] = -nom_x[0] + true_x[0]; - out_7805211099664095660[1] = -nom_x[1] + true_x[1]; - out_7805211099664095660[2] = -nom_x[2] + true_x[2]; - out_7805211099664095660[3] = -nom_x[3] + true_x[3]; - out_7805211099664095660[4] = -nom_x[4] + true_x[4]; - out_7805211099664095660[5] = -nom_x[5] + true_x[5]; - out_7805211099664095660[6] = -nom_x[6] + true_x[6]; - out_7805211099664095660[7] = -nom_x[7] + true_x[7]; - out_7805211099664095660[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_3781853638498010) { + out_3781853638498010[0] = -nom_x[0] + true_x[0]; + out_3781853638498010[1] = -nom_x[1] + true_x[1]; + out_3781853638498010[2] = -nom_x[2] + true_x[2]; + out_3781853638498010[3] = -nom_x[3] + true_x[3]; + out_3781853638498010[4] = -nom_x[4] + true_x[4]; + out_3781853638498010[5] = -nom_x[5] + true_x[5]; + out_3781853638498010[6] = -nom_x[6] + true_x[6]; + out_3781853638498010[7] = -nom_x[7] + true_x[7]; + out_3781853638498010[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_2822454019109077104) { - out_2822454019109077104[0] = 1.0; - out_2822454019109077104[1] = 0.0; - out_2822454019109077104[2] = 0.0; - out_2822454019109077104[3] = 0.0; - out_2822454019109077104[4] = 0.0; - out_2822454019109077104[5] = 0.0; - out_2822454019109077104[6] = 0.0; - out_2822454019109077104[7] = 0.0; - out_2822454019109077104[8] = 0.0; - out_2822454019109077104[9] = 0.0; - out_2822454019109077104[10] = 1.0; - out_2822454019109077104[11] = 0.0; - out_2822454019109077104[12] = 0.0; - out_2822454019109077104[13] = 0.0; - out_2822454019109077104[14] = 0.0; - out_2822454019109077104[15] = 0.0; - out_2822454019109077104[16] = 0.0; - out_2822454019109077104[17] = 0.0; - out_2822454019109077104[18] = 0.0; - out_2822454019109077104[19] = 0.0; - out_2822454019109077104[20] = 1.0; - out_2822454019109077104[21] = 0.0; - out_2822454019109077104[22] = 0.0; - out_2822454019109077104[23] = 0.0; - out_2822454019109077104[24] = 0.0; - out_2822454019109077104[25] = 0.0; - out_2822454019109077104[26] = 0.0; - out_2822454019109077104[27] = 0.0; - out_2822454019109077104[28] = 0.0; - out_2822454019109077104[29] = 0.0; - out_2822454019109077104[30] = 1.0; - out_2822454019109077104[31] = 0.0; - out_2822454019109077104[32] = 0.0; - out_2822454019109077104[33] = 0.0; - out_2822454019109077104[34] = 0.0; - out_2822454019109077104[35] = 0.0; - out_2822454019109077104[36] = 0.0; - out_2822454019109077104[37] = 0.0; - out_2822454019109077104[38] = 0.0; - out_2822454019109077104[39] = 0.0; - out_2822454019109077104[40] = 1.0; - out_2822454019109077104[41] = 0.0; - out_2822454019109077104[42] = 0.0; - out_2822454019109077104[43] = 0.0; - out_2822454019109077104[44] = 0.0; - out_2822454019109077104[45] = 0.0; - out_2822454019109077104[46] = 0.0; - out_2822454019109077104[47] = 0.0; - out_2822454019109077104[48] = 0.0; - out_2822454019109077104[49] = 0.0; - out_2822454019109077104[50] = 1.0; - out_2822454019109077104[51] = 0.0; - out_2822454019109077104[52] = 0.0; - out_2822454019109077104[53] = 0.0; - out_2822454019109077104[54] = 0.0; - out_2822454019109077104[55] = 0.0; - out_2822454019109077104[56] = 0.0; - out_2822454019109077104[57] = 0.0; - out_2822454019109077104[58] = 0.0; - out_2822454019109077104[59] = 0.0; - out_2822454019109077104[60] = 1.0; - out_2822454019109077104[61] = 0.0; - out_2822454019109077104[62] = 0.0; - out_2822454019109077104[63] = 0.0; - out_2822454019109077104[64] = 0.0; - out_2822454019109077104[65] = 0.0; - out_2822454019109077104[66] = 0.0; - out_2822454019109077104[67] = 0.0; - out_2822454019109077104[68] = 0.0; - out_2822454019109077104[69] = 0.0; - out_2822454019109077104[70] = 1.0; - out_2822454019109077104[71] = 0.0; - out_2822454019109077104[72] = 0.0; - out_2822454019109077104[73] = 0.0; - out_2822454019109077104[74] = 0.0; - out_2822454019109077104[75] = 0.0; - out_2822454019109077104[76] = 0.0; - out_2822454019109077104[77] = 0.0; - out_2822454019109077104[78] = 0.0; - out_2822454019109077104[79] = 0.0; - out_2822454019109077104[80] = 1.0; +void H_mod_fun(double *state, double *out_6037661802737362721) { + out_6037661802737362721[0] = 1.0; + out_6037661802737362721[1] = 0.0; + out_6037661802737362721[2] = 0.0; + out_6037661802737362721[3] = 0.0; + out_6037661802737362721[4] = 0.0; + out_6037661802737362721[5] = 0.0; + out_6037661802737362721[6] = 0.0; + out_6037661802737362721[7] = 0.0; + out_6037661802737362721[8] = 0.0; + out_6037661802737362721[9] = 0.0; + out_6037661802737362721[10] = 1.0; + out_6037661802737362721[11] = 0.0; + out_6037661802737362721[12] = 0.0; + out_6037661802737362721[13] = 0.0; + out_6037661802737362721[14] = 0.0; + out_6037661802737362721[15] = 0.0; + out_6037661802737362721[16] = 0.0; + out_6037661802737362721[17] = 0.0; + out_6037661802737362721[18] = 0.0; + out_6037661802737362721[19] = 0.0; + out_6037661802737362721[20] = 1.0; + out_6037661802737362721[21] = 0.0; + out_6037661802737362721[22] = 0.0; + out_6037661802737362721[23] = 0.0; + out_6037661802737362721[24] = 0.0; + out_6037661802737362721[25] = 0.0; + out_6037661802737362721[26] = 0.0; + out_6037661802737362721[27] = 0.0; + out_6037661802737362721[28] = 0.0; + out_6037661802737362721[29] = 0.0; + out_6037661802737362721[30] = 1.0; + out_6037661802737362721[31] = 0.0; + out_6037661802737362721[32] = 0.0; + out_6037661802737362721[33] = 0.0; + out_6037661802737362721[34] = 0.0; + out_6037661802737362721[35] = 0.0; + out_6037661802737362721[36] = 0.0; + out_6037661802737362721[37] = 0.0; + out_6037661802737362721[38] = 0.0; + out_6037661802737362721[39] = 0.0; + out_6037661802737362721[40] = 1.0; + out_6037661802737362721[41] = 0.0; + out_6037661802737362721[42] = 0.0; + out_6037661802737362721[43] = 0.0; + out_6037661802737362721[44] = 0.0; + out_6037661802737362721[45] = 0.0; + out_6037661802737362721[46] = 0.0; + out_6037661802737362721[47] = 0.0; + out_6037661802737362721[48] = 0.0; + out_6037661802737362721[49] = 0.0; + out_6037661802737362721[50] = 1.0; + out_6037661802737362721[51] = 0.0; + out_6037661802737362721[52] = 0.0; + out_6037661802737362721[53] = 0.0; + out_6037661802737362721[54] = 0.0; + out_6037661802737362721[55] = 0.0; + out_6037661802737362721[56] = 0.0; + out_6037661802737362721[57] = 0.0; + out_6037661802737362721[58] = 0.0; + out_6037661802737362721[59] = 0.0; + out_6037661802737362721[60] = 1.0; + out_6037661802737362721[61] = 0.0; + out_6037661802737362721[62] = 0.0; + out_6037661802737362721[63] = 0.0; + out_6037661802737362721[64] = 0.0; + out_6037661802737362721[65] = 0.0; + out_6037661802737362721[66] = 0.0; + out_6037661802737362721[67] = 0.0; + out_6037661802737362721[68] = 0.0; + out_6037661802737362721[69] = 0.0; + out_6037661802737362721[70] = 1.0; + out_6037661802737362721[71] = 0.0; + out_6037661802737362721[72] = 0.0; + out_6037661802737362721[73] = 0.0; + out_6037661802737362721[74] = 0.0; + out_6037661802737362721[75] = 0.0; + out_6037661802737362721[76] = 0.0; + out_6037661802737362721[77] = 0.0; + out_6037661802737362721[78] = 0.0; + out_6037661802737362721[79] = 0.0; + out_6037661802737362721[80] = 1.0; } -void f_fun(double *state, double dt, double *out_1333075080878653623) { - out_1333075080878653623[0] = state[0]; - out_1333075080878653623[1] = state[1]; - out_1333075080878653623[2] = state[2]; - out_1333075080878653623[3] = state[3]; - out_1333075080878653623[4] = state[4]; - out_1333075080878653623[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_1333075080878653623[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_1333075080878653623[7] = state[7]; - out_1333075080878653623[8] = state[8]; +void f_fun(double *state, double dt, double *out_8701591355184024691) { + out_8701591355184024691[0] = state[0]; + out_8701591355184024691[1] = state[1]; + out_8701591355184024691[2] = state[2]; + out_8701591355184024691[3] = state[3]; + out_8701591355184024691[4] = state[4]; + out_8701591355184024691[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_8701591355184024691[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_8701591355184024691[7] = state[7]; + out_8701591355184024691[8] = state[8]; } -void F_fun(double *state, double dt, double *out_8557949136162757558) { - out_8557949136162757558[0] = 1; - out_8557949136162757558[1] = 0; - out_8557949136162757558[2] = 0; - out_8557949136162757558[3] = 0; - out_8557949136162757558[4] = 0; - out_8557949136162757558[5] = 0; - out_8557949136162757558[6] = 0; - out_8557949136162757558[7] = 0; - out_8557949136162757558[8] = 0; - out_8557949136162757558[9] = 0; - out_8557949136162757558[10] = 1; - out_8557949136162757558[11] = 0; - out_8557949136162757558[12] = 0; - out_8557949136162757558[13] = 0; - out_8557949136162757558[14] = 0; - out_8557949136162757558[15] = 0; - out_8557949136162757558[16] = 0; - out_8557949136162757558[17] = 0; - out_8557949136162757558[18] = 0; - out_8557949136162757558[19] = 0; - out_8557949136162757558[20] = 1; - out_8557949136162757558[21] = 0; - out_8557949136162757558[22] = 0; - out_8557949136162757558[23] = 0; - out_8557949136162757558[24] = 0; - out_8557949136162757558[25] = 0; - out_8557949136162757558[26] = 0; - out_8557949136162757558[27] = 0; - out_8557949136162757558[28] = 0; - out_8557949136162757558[29] = 0; - out_8557949136162757558[30] = 1; - out_8557949136162757558[31] = 0; - out_8557949136162757558[32] = 0; - out_8557949136162757558[33] = 0; - out_8557949136162757558[34] = 0; - out_8557949136162757558[35] = 0; - out_8557949136162757558[36] = 0; - out_8557949136162757558[37] = 0; - out_8557949136162757558[38] = 0; - out_8557949136162757558[39] = 0; - out_8557949136162757558[40] = 1; - out_8557949136162757558[41] = 0; - out_8557949136162757558[42] = 0; - out_8557949136162757558[43] = 0; - out_8557949136162757558[44] = 0; - out_8557949136162757558[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_8557949136162757558[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_8557949136162757558[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_8557949136162757558[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_8557949136162757558[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_8557949136162757558[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_8557949136162757558[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_8557949136162757558[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_8557949136162757558[53] = -9.8100000000000005*dt; - out_8557949136162757558[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_8557949136162757558[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_8557949136162757558[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_8557949136162757558[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_8557949136162757558[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_8557949136162757558[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_8557949136162757558[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_8557949136162757558[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_8557949136162757558[62] = 0; - out_8557949136162757558[63] = 0; - out_8557949136162757558[64] = 0; - out_8557949136162757558[65] = 0; - out_8557949136162757558[66] = 0; - out_8557949136162757558[67] = 0; - out_8557949136162757558[68] = 0; - out_8557949136162757558[69] = 0; - out_8557949136162757558[70] = 1; - out_8557949136162757558[71] = 0; - out_8557949136162757558[72] = 0; - out_8557949136162757558[73] = 0; - out_8557949136162757558[74] = 0; - out_8557949136162757558[75] = 0; - out_8557949136162757558[76] = 0; - out_8557949136162757558[77] = 0; - out_8557949136162757558[78] = 0; - out_8557949136162757558[79] = 0; - out_8557949136162757558[80] = 1; +void F_fun(double *state, double dt, double *out_4180724957458131151) { + out_4180724957458131151[0] = 1; + out_4180724957458131151[1] = 0; + out_4180724957458131151[2] = 0; + out_4180724957458131151[3] = 0; + out_4180724957458131151[4] = 0; + out_4180724957458131151[5] = 0; + out_4180724957458131151[6] = 0; + out_4180724957458131151[7] = 0; + out_4180724957458131151[8] = 0; + out_4180724957458131151[9] = 0; + out_4180724957458131151[10] = 1; + out_4180724957458131151[11] = 0; + out_4180724957458131151[12] = 0; + out_4180724957458131151[13] = 0; + out_4180724957458131151[14] = 0; + out_4180724957458131151[15] = 0; + out_4180724957458131151[16] = 0; + out_4180724957458131151[17] = 0; + out_4180724957458131151[18] = 0; + out_4180724957458131151[19] = 0; + out_4180724957458131151[20] = 1; + out_4180724957458131151[21] = 0; + out_4180724957458131151[22] = 0; + out_4180724957458131151[23] = 0; + out_4180724957458131151[24] = 0; + out_4180724957458131151[25] = 0; + out_4180724957458131151[26] = 0; + out_4180724957458131151[27] = 0; + out_4180724957458131151[28] = 0; + out_4180724957458131151[29] = 0; + out_4180724957458131151[30] = 1; + out_4180724957458131151[31] = 0; + out_4180724957458131151[32] = 0; + out_4180724957458131151[33] = 0; + out_4180724957458131151[34] = 0; + out_4180724957458131151[35] = 0; + out_4180724957458131151[36] = 0; + out_4180724957458131151[37] = 0; + out_4180724957458131151[38] = 0; + out_4180724957458131151[39] = 0; + out_4180724957458131151[40] = 1; + out_4180724957458131151[41] = 0; + out_4180724957458131151[42] = 0; + out_4180724957458131151[43] = 0; + out_4180724957458131151[44] = 0; + out_4180724957458131151[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_4180724957458131151[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_4180724957458131151[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_4180724957458131151[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_4180724957458131151[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_4180724957458131151[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_4180724957458131151[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_4180724957458131151[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_4180724957458131151[53] = -9.8100000000000005*dt; + out_4180724957458131151[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_4180724957458131151[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_4180724957458131151[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_4180724957458131151[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_4180724957458131151[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_4180724957458131151[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_4180724957458131151[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_4180724957458131151[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_4180724957458131151[62] = 0; + out_4180724957458131151[63] = 0; + out_4180724957458131151[64] = 0; + out_4180724957458131151[65] = 0; + out_4180724957458131151[66] = 0; + out_4180724957458131151[67] = 0; + out_4180724957458131151[68] = 0; + out_4180724957458131151[69] = 0; + out_4180724957458131151[70] = 1; + out_4180724957458131151[71] = 0; + out_4180724957458131151[72] = 0; + out_4180724957458131151[73] = 0; + out_4180724957458131151[74] = 0; + out_4180724957458131151[75] = 0; + out_4180724957458131151[76] = 0; + out_4180724957458131151[77] = 0; + out_4180724957458131151[78] = 0; + out_4180724957458131151[79] = 0; + out_4180724957458131151[80] = 1; } -void h_25(double *state, double *unused, double *out_5666050874031952366) { - out_5666050874031952366[0] = state[6]; +void h_25(double *state, double *unused, double *out_76571637109634956) { + out_76571637109634956[0] = state[6]; } -void H_25(double *state, double *unused, double *out_3274454347497297512) { - out_3274454347497297512[0] = 0; - out_3274454347497297512[1] = 0; - out_3274454347497297512[2] = 0; - out_3274454347497297512[3] = 0; - out_3274454347497297512[4] = 0; - out_3274454347497297512[5] = 0; - out_3274454347497297512[6] = 1; - out_3274454347497297512[7] = 0; - out_3274454347497297512[8] = 0; +void H_25(double *state, double *unused, double *out_7505390445787524055) { + out_7505390445787524055[0] = 0; + out_7505390445787524055[1] = 0; + out_7505390445787524055[2] = 0; + out_7505390445787524055[3] = 0; + out_7505390445787524055[4] = 0; + out_7505390445787524055[5] = 0; + out_7505390445787524055[6] = 1; + out_7505390445787524055[7] = 0; + out_7505390445787524055[8] = 0; } -void h_24(double *state, double *unused, double *out_7890691293906506169) { - out_7890691293906506169[0] = state[4]; - out_7890691293906506169[1] = state[5]; +void h_24(double *state, double *unused, double *out_6249911379334495626) { + out_6249911379334495626[0] = state[4]; + out_6249911379334495626[1] = state[5]; } -void H_24(double *state, double *unused, double *out_5447103946502797078) { - out_5447103946502797078[0] = 0; - out_5447103946502797078[1] = 0; - out_5447103946502797078[2] = 0; - out_5447103946502797078[3] = 0; - out_5447103946502797078[4] = 1; - out_5447103946502797078[5] = 0; - out_5447103946502797078[6] = 0; - out_5447103946502797078[7] = 0; - out_5447103946502797078[8] = 0; - out_5447103946502797078[9] = 0; - out_5447103946502797078[10] = 0; - out_5447103946502797078[11] = 0; - out_5447103946502797078[12] = 0; - out_5447103946502797078[13] = 0; - out_5447103946502797078[14] = 1; - out_5447103946502797078[15] = 0; - out_5447103946502797078[16] = 0; - out_5447103946502797078[17] = 0; +void H_24(double *state, double *unused, double *out_2231896162874511461) { + out_2231896162874511461[0] = 0; + out_2231896162874511461[1] = 0; + out_2231896162874511461[2] = 0; + out_2231896162874511461[3] = 0; + out_2231896162874511461[4] = 1; + out_2231896162874511461[5] = 0; + out_2231896162874511461[6] = 0; + out_2231896162874511461[7] = 0; + out_2231896162874511461[8] = 0; + out_2231896162874511461[9] = 0; + out_2231896162874511461[10] = 0; + out_2231896162874511461[11] = 0; + out_2231896162874511461[12] = 0; + out_2231896162874511461[13] = 0; + out_2231896162874511461[14] = 1; + out_2231896162874511461[15] = 0; + out_2231896162874511461[16] = 0; + out_2231896162874511461[17] = 0; } -void h_30(double *state, double *unused, double *out_5390856811747446477) { - out_5390856811747446477[0] = state[4]; +void h_30(double *state, double *unused, double *out_198622425174870933) { + out_198622425174870933[0] = state[4]; } -void H_30(double *state, double *unused, double *out_3642235993994319243) { - out_3642235993994319243[0] = 0; - out_3642235993994319243[1] = 0; - out_3642235993994319243[2] = 0; - out_3642235993994319243[3] = 0; - out_3642235993994319243[4] = 1; - out_3642235993994319243[5] = 0; - out_3642235993994319243[6] = 0; - out_3642235993994319243[7] = 0; - out_3642235993994319243[8] = 0; +void H_30(double *state, double *unused, double *out_6413657297794419363) { + out_6413657297794419363[0] = 0; + out_6413657297794419363[1] = 0; + out_6413657297794419363[2] = 0; + out_6413657297794419363[3] = 0; + out_6413657297794419363[4] = 1; + out_6413657297794419363[5] = 0; + out_6413657297794419363[6] = 0; + out_6413657297794419363[7] = 0; + out_6413657297794419363[8] = 0; } -void h_26(double *state, double *unused, double *out_4304130163390982150) { - out_4304130163390982150[0] = state[7]; +void h_26(double *state, double *unused, double *out_5679663062383844155) { + out_5679663062383844155[0] = state[7]; } -void H_26(double *state, double *unused, double *out_7015957666371353736) { - out_7015957666371353736[0] = 0; - out_7015957666371353736[1] = 0; - out_7015957666371353736[2] = 0; - out_7015957666371353736[3] = 0; - out_7015957666371353736[4] = 0; - out_7015957666371353736[5] = 0; - out_7015957666371353736[6] = 0; - out_7015957666371353736[7] = 1; - out_7015957666371353736[8] = 0; +void H_26(double *state, double *unused, double *out_7199850309047971337) { + out_7199850309047971337[0] = 0; + out_7199850309047971337[1] = 0; + out_7199850309047971337[2] = 0; + out_7199850309047971337[3] = 0; + out_7199850309047971337[4] = 0; + out_7199850309047971337[5] = 0; + out_7199850309047971337[6] = 0; + out_7199850309047971337[7] = 1; + out_7199850309047971337[8] = 0; } -void h_27(double *state, double *unused, double *out_5777930495884533471) { - out_5777930495884533471[0] = state[3]; +void h_27(double *state, double *unused, double *out_4925101630551210689) { + out_4925101630551210689[0] = state[3]; } -void H_27(double *state, double *unused, double *out_1467472682193894332) { - out_1467472682193894332[0] = 0; - out_1467472682193894332[1] = 0; - out_1467472682193894332[2] = 0; - out_1467472682193894332[3] = 1; - out_1467472682193894332[4] = 0; - out_1467472682193894332[5] = 0; - out_1467472682193894332[6] = 0; - out_1467472682193894332[7] = 0; - out_1467472682193894332[8] = 0; +void H_27(double *state, double *unused, double *out_8637251368978362580) { + out_8637251368978362580[0] = 0; + out_8637251368978362580[1] = 0; + out_8637251368978362580[2] = 0; + out_8637251368978362580[3] = 1; + out_8637251368978362580[4] = 0; + out_8637251368978362580[5] = 0; + out_8637251368978362580[6] = 0; + out_8637251368978362580[7] = 0; + out_8637251368978362580[8] = 0; } -void h_29(double *state, double *unused, double *out_4787540927070968960) { - out_4787540927070968960[0] = state[1]; +void h_29(double *state, double *unused, double *out_5200295692835716578) { + out_5200295692835716578[0] = state[1]; } -void H_29(double *state, double *unused, double *out_245890044675656701) { - out_245890044675656701[0] = 0; - out_245890044675656701[1] = 1; - out_245890044675656701[2] = 0; - out_245890044675656701[3] = 0; - out_245890044675656701[4] = 0; - out_245890044675656701[5] = 0; - out_245890044675656701[6] = 0; - out_245890044675656701[7] = 0; - out_245890044675656701[8] = 0; +void H_29(double *state, double *unused, double *out_6923888642108811547) { + out_6923888642108811547[0] = 0; + out_6923888642108811547[1] = 1; + out_6923888642108811547[2] = 0; + out_6923888642108811547[3] = 0; + out_6923888642108811547[4] = 0; + out_6923888642108811547[5] = 0; + out_6923888642108811547[6] = 0; + out_6923888642108811547[7] = 0; + out_6923888642108811547[8] = 0; } -void h_28(double *state, double *unused, double *out_6006244842257137284) { - out_6006244842257137284[0] = state[0]; +void h_28(double *state, double *unused, double *out_8899456413702232613) { + out_8899456413702232613[0] = state[0]; } -void H_28(double *state, double *unused, double *out_5328289061745187275) { - out_5328289061745187275[0] = 1; - out_5328289061745187275[1] = 0; - out_5328289061745187275[2] = 0; - out_5328289061745187275[3] = 0; - out_5328289061745187275[4] = 0; - out_5328289061745187275[5] = 0; - out_5328289061745187275[6] = 0; - out_5328289061745187275[7] = 0; - out_5328289061745187275[8] = 0; +void H_28(double *state, double *unused, double *out_8887518913674137798) { + out_8887518913674137798[0] = 1; + out_8887518913674137798[1] = 0; + out_8887518913674137798[2] = 0; + out_8887518913674137798[3] = 0; + out_8887518913674137798[4] = 0; + out_8887518913674137798[5] = 0; + out_8887518913674137798[6] = 0; + out_8887518913674137798[7] = 0; + out_8887518913674137798[8] = 0; } -void h_31(double *state, double *unused, double *out_5508864205680823221) { - out_5508864205680823221[0] = state[8]; +void h_31(double *state, double *unused, double *out_3714021165363657306) { + out_3714021165363657306[0] = state[8]; } -void H_31(double *state, double *unused, double *out_3243808385620337084) { - out_3243808385620337084[0] = 0; - out_3243808385620337084[1] = 0; - out_3243808385620337084[2] = 0; - out_3243808385620337084[3] = 0; - out_3243808385620337084[4] = 0; - out_3243808385620337084[5] = 0; - out_3243808385620337084[6] = 0; - out_3243808385620337084[7] = 0; - out_3243808385620337084[8] = 1; +void H_31(double *state, double *unused, double *out_7474744483910563627) { + out_7474744483910563627[0] = 0; + out_7474744483910563627[1] = 0; + out_7474744483910563627[2] = 0; + out_7474744483910563627[3] = 0; + out_7474744483910563627[4] = 0; + out_7474744483910563627[5] = 0; + out_7474744483910563627[6] = 0; + out_7474744483910563627[7] = 0; + out_7474744483910563627[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_9086440322595224646) { - err_fun(nom_x, delta_x, out_9086440322595224646); +void car_err_fun(double *nom_x, double *delta_x, double *out_7550976477549507356) { + err_fun(nom_x, delta_x, out_7550976477549507356); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_7805211099664095660) { - inv_err_fun(nom_x, true_x, out_7805211099664095660); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_3781853638498010) { + inv_err_fun(nom_x, true_x, out_3781853638498010); } -void car_H_mod_fun(double *state, double *out_2822454019109077104) { - H_mod_fun(state, out_2822454019109077104); +void car_H_mod_fun(double *state, double *out_6037661802737362721) { + H_mod_fun(state, out_6037661802737362721); } -void car_f_fun(double *state, double dt, double *out_1333075080878653623) { - f_fun(state, dt, out_1333075080878653623); +void car_f_fun(double *state, double dt, double *out_8701591355184024691) { + f_fun(state, dt, out_8701591355184024691); } -void car_F_fun(double *state, double dt, double *out_8557949136162757558) { - F_fun(state, dt, out_8557949136162757558); +void car_F_fun(double *state, double dt, double *out_4180724957458131151) { + F_fun(state, dt, out_4180724957458131151); } -void car_h_25(double *state, double *unused, double *out_5666050874031952366) { - h_25(state, unused, out_5666050874031952366); +void car_h_25(double *state, double *unused, double *out_76571637109634956) { + h_25(state, unused, out_76571637109634956); } -void car_H_25(double *state, double *unused, double *out_3274454347497297512) { - H_25(state, unused, out_3274454347497297512); +void car_H_25(double *state, double *unused, double *out_7505390445787524055) { + H_25(state, unused, out_7505390445787524055); } -void car_h_24(double *state, double *unused, double *out_7890691293906506169) { - h_24(state, unused, out_7890691293906506169); +void car_h_24(double *state, double *unused, double *out_6249911379334495626) { + h_24(state, unused, out_6249911379334495626); } -void car_H_24(double *state, double *unused, double *out_5447103946502797078) { - H_24(state, unused, out_5447103946502797078); +void car_H_24(double *state, double *unused, double *out_2231896162874511461) { + H_24(state, unused, out_2231896162874511461); } -void car_h_30(double *state, double *unused, double *out_5390856811747446477) { - h_30(state, unused, out_5390856811747446477); +void car_h_30(double *state, double *unused, double *out_198622425174870933) { + h_30(state, unused, out_198622425174870933); } -void car_H_30(double *state, double *unused, double *out_3642235993994319243) { - H_30(state, unused, out_3642235993994319243); +void car_H_30(double *state, double *unused, double *out_6413657297794419363) { + H_30(state, unused, out_6413657297794419363); } -void car_h_26(double *state, double *unused, double *out_4304130163390982150) { - h_26(state, unused, out_4304130163390982150); +void car_h_26(double *state, double *unused, double *out_5679663062383844155) { + h_26(state, unused, out_5679663062383844155); } -void car_H_26(double *state, double *unused, double *out_7015957666371353736) { - H_26(state, unused, out_7015957666371353736); +void car_H_26(double *state, double *unused, double *out_7199850309047971337) { + H_26(state, unused, out_7199850309047971337); } -void car_h_27(double *state, double *unused, double *out_5777930495884533471) { - h_27(state, unused, out_5777930495884533471); +void car_h_27(double *state, double *unused, double *out_4925101630551210689) { + h_27(state, unused, out_4925101630551210689); } -void car_H_27(double *state, double *unused, double *out_1467472682193894332) { - H_27(state, unused, out_1467472682193894332); +void car_H_27(double *state, double *unused, double *out_8637251368978362580) { + H_27(state, unused, out_8637251368978362580); } -void car_h_29(double *state, double *unused, double *out_4787540927070968960) { - h_29(state, unused, out_4787540927070968960); +void car_h_29(double *state, double *unused, double *out_5200295692835716578) { + h_29(state, unused, out_5200295692835716578); } -void car_H_29(double *state, double *unused, double *out_245890044675656701) { - H_29(state, unused, out_245890044675656701); +void car_H_29(double *state, double *unused, double *out_6923888642108811547) { + H_29(state, unused, out_6923888642108811547); } -void car_h_28(double *state, double *unused, double *out_6006244842257137284) { - h_28(state, unused, out_6006244842257137284); +void car_h_28(double *state, double *unused, double *out_8899456413702232613) { + h_28(state, unused, out_8899456413702232613); } -void car_H_28(double *state, double *unused, double *out_5328289061745187275) { - H_28(state, unused, out_5328289061745187275); +void car_H_28(double *state, double *unused, double *out_8887518913674137798) { + H_28(state, unused, out_8887518913674137798); } -void car_h_31(double *state, double *unused, double *out_5508864205680823221) { - h_31(state, unused, out_5508864205680823221); +void car_h_31(double *state, double *unused, double *out_3714021165363657306) { + h_31(state, unused, out_3714021165363657306); } -void car_H_31(double *state, double *unused, double *out_3243808385620337084) { - H_31(state, unused, out_3243808385620337084); +void car_H_31(double *state, double *unused, double *out_7474744483910563627) { + H_31(state, unused, out_7474744483910563627); } 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 899655ae0..cee536c17 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_9086440322595224646); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_7805211099664095660); -void car_H_mod_fun(double *state, double *out_2822454019109077104); -void car_f_fun(double *state, double dt, double *out_1333075080878653623); -void car_F_fun(double *state, double dt, double *out_8557949136162757558); -void car_h_25(double *state, double *unused, double *out_5666050874031952366); -void car_H_25(double *state, double *unused, double *out_3274454347497297512); -void car_h_24(double *state, double *unused, double *out_7890691293906506169); -void car_H_24(double *state, double *unused, double *out_5447103946502797078); -void car_h_30(double *state, double *unused, double *out_5390856811747446477); -void car_H_30(double *state, double *unused, double *out_3642235993994319243); -void car_h_26(double *state, double *unused, double *out_4304130163390982150); -void car_H_26(double *state, double *unused, double *out_7015957666371353736); -void car_h_27(double *state, double *unused, double *out_5777930495884533471); -void car_H_27(double *state, double *unused, double *out_1467472682193894332); -void car_h_29(double *state, double *unused, double *out_4787540927070968960); -void car_H_29(double *state, double *unused, double *out_245890044675656701); -void car_h_28(double *state, double *unused, double *out_6006244842257137284); -void car_H_28(double *state, double *unused, double *out_5328289061745187275); -void car_h_31(double *state, double *unused, double *out_5508864205680823221); -void car_H_31(double *state, double *unused, double *out_3243808385620337084); +void car_err_fun(double *nom_x, double *delta_x, double *out_7550976477549507356); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_3781853638498010); +void car_H_mod_fun(double *state, double *out_6037661802737362721); +void car_f_fun(double *state, double dt, double *out_8701591355184024691); +void car_F_fun(double *state, double dt, double *out_4180724957458131151); +void car_h_25(double *state, double *unused, double *out_76571637109634956); +void car_H_25(double *state, double *unused, double *out_7505390445787524055); +void car_h_24(double *state, double *unused, double *out_6249911379334495626); +void car_H_24(double *state, double *unused, double *out_2231896162874511461); +void car_h_30(double *state, double *unused, double *out_198622425174870933); +void car_H_30(double *state, double *unused, double *out_6413657297794419363); +void car_h_26(double *state, double *unused, double *out_5679663062383844155); +void car_H_26(double *state, double *unused, double *out_7199850309047971337); +void car_h_27(double *state, double *unused, double *out_4925101630551210689); +void car_H_27(double *state, double *unused, double *out_8637251368978362580); +void car_h_29(double *state, double *unused, double *out_5200295692835716578); +void car_H_29(double *state, double *unused, double *out_6923888642108811547); +void car_h_28(double *state, double *unused, double *out_8899456413702232613); +void car_H_28(double *state, double *unused, double *out_8887518913674137798); +void car_h_31(double *state, double *unused, double *out_3714021165363657306); +void car_H_31(double *state, double *unused, double *out_7474744483910563627); void car_predict(double *in_x, double *in_P, double *in_Q, double dt); void car_set_mass(double x); void car_set_rotational_inertia(double x); diff --git a/selfdrive/locationd/models/generated/pose.cpp b/selfdrive/locationd/models/generated/pose.cpp index 686de27ae..f7cd80b26 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_7322678164781620542) { - out_7322678164781620542[0] = delta_x[0] + nom_x[0]; - out_7322678164781620542[1] = delta_x[1] + nom_x[1]; - out_7322678164781620542[2] = delta_x[2] + nom_x[2]; - out_7322678164781620542[3] = delta_x[3] + nom_x[3]; - out_7322678164781620542[4] = delta_x[4] + nom_x[4]; - out_7322678164781620542[5] = delta_x[5] + nom_x[5]; - out_7322678164781620542[6] = delta_x[6] + nom_x[6]; - out_7322678164781620542[7] = delta_x[7] + nom_x[7]; - out_7322678164781620542[8] = delta_x[8] + nom_x[8]; - out_7322678164781620542[9] = delta_x[9] + nom_x[9]; - out_7322678164781620542[10] = delta_x[10] + nom_x[10]; - out_7322678164781620542[11] = delta_x[11] + nom_x[11]; - out_7322678164781620542[12] = delta_x[12] + nom_x[12]; - out_7322678164781620542[13] = delta_x[13] + nom_x[13]; - out_7322678164781620542[14] = delta_x[14] + nom_x[14]; - out_7322678164781620542[15] = delta_x[15] + nom_x[15]; - out_7322678164781620542[16] = delta_x[16] + nom_x[16]; - out_7322678164781620542[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_1527400177482295711) { + out_1527400177482295711[0] = delta_x[0] + nom_x[0]; + out_1527400177482295711[1] = delta_x[1] + nom_x[1]; + out_1527400177482295711[2] = delta_x[2] + nom_x[2]; + out_1527400177482295711[3] = delta_x[3] + nom_x[3]; + out_1527400177482295711[4] = delta_x[4] + nom_x[4]; + out_1527400177482295711[5] = delta_x[5] + nom_x[5]; + out_1527400177482295711[6] = delta_x[6] + nom_x[6]; + out_1527400177482295711[7] = delta_x[7] + nom_x[7]; + out_1527400177482295711[8] = delta_x[8] + nom_x[8]; + out_1527400177482295711[9] = delta_x[9] + nom_x[9]; + out_1527400177482295711[10] = delta_x[10] + nom_x[10]; + out_1527400177482295711[11] = delta_x[11] + nom_x[11]; + out_1527400177482295711[12] = delta_x[12] + nom_x[12]; + out_1527400177482295711[13] = delta_x[13] + nom_x[13]; + out_1527400177482295711[14] = delta_x[14] + nom_x[14]; + out_1527400177482295711[15] = delta_x[15] + nom_x[15]; + out_1527400177482295711[16] = delta_x[16] + nom_x[16]; + out_1527400177482295711[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_6351713798129038522) { - out_6351713798129038522[0] = -nom_x[0] + true_x[0]; - out_6351713798129038522[1] = -nom_x[1] + true_x[1]; - out_6351713798129038522[2] = -nom_x[2] + true_x[2]; - out_6351713798129038522[3] = -nom_x[3] + true_x[3]; - out_6351713798129038522[4] = -nom_x[4] + true_x[4]; - out_6351713798129038522[5] = -nom_x[5] + true_x[5]; - out_6351713798129038522[6] = -nom_x[6] + true_x[6]; - out_6351713798129038522[7] = -nom_x[7] + true_x[7]; - out_6351713798129038522[8] = -nom_x[8] + true_x[8]; - out_6351713798129038522[9] = -nom_x[9] + true_x[9]; - out_6351713798129038522[10] = -nom_x[10] + true_x[10]; - out_6351713798129038522[11] = -nom_x[11] + true_x[11]; - out_6351713798129038522[12] = -nom_x[12] + true_x[12]; - out_6351713798129038522[13] = -nom_x[13] + true_x[13]; - out_6351713798129038522[14] = -nom_x[14] + true_x[14]; - out_6351713798129038522[15] = -nom_x[15] + true_x[15]; - out_6351713798129038522[16] = -nom_x[16] + true_x[16]; - out_6351713798129038522[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_7620805951199866590) { + out_7620805951199866590[0] = -nom_x[0] + true_x[0]; + out_7620805951199866590[1] = -nom_x[1] + true_x[1]; + out_7620805951199866590[2] = -nom_x[2] + true_x[2]; + out_7620805951199866590[3] = -nom_x[3] + true_x[3]; + out_7620805951199866590[4] = -nom_x[4] + true_x[4]; + out_7620805951199866590[5] = -nom_x[5] + true_x[5]; + out_7620805951199866590[6] = -nom_x[6] + true_x[6]; + out_7620805951199866590[7] = -nom_x[7] + true_x[7]; + out_7620805951199866590[8] = -nom_x[8] + true_x[8]; + out_7620805951199866590[9] = -nom_x[9] + true_x[9]; + out_7620805951199866590[10] = -nom_x[10] + true_x[10]; + out_7620805951199866590[11] = -nom_x[11] + true_x[11]; + out_7620805951199866590[12] = -nom_x[12] + true_x[12]; + out_7620805951199866590[13] = -nom_x[13] + true_x[13]; + out_7620805951199866590[14] = -nom_x[14] + true_x[14]; + out_7620805951199866590[15] = -nom_x[15] + true_x[15]; + out_7620805951199866590[16] = -nom_x[16] + true_x[16]; + out_7620805951199866590[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_7181373993335857162) { - out_7181373993335857162[0] = 1.0; - out_7181373993335857162[1] = 0.0; - out_7181373993335857162[2] = 0.0; - out_7181373993335857162[3] = 0.0; - out_7181373993335857162[4] = 0.0; - out_7181373993335857162[5] = 0.0; - out_7181373993335857162[6] = 0.0; - out_7181373993335857162[7] = 0.0; - out_7181373993335857162[8] = 0.0; - out_7181373993335857162[9] = 0.0; - out_7181373993335857162[10] = 0.0; - out_7181373993335857162[11] = 0.0; - out_7181373993335857162[12] = 0.0; - out_7181373993335857162[13] = 0.0; - out_7181373993335857162[14] = 0.0; - out_7181373993335857162[15] = 0.0; - out_7181373993335857162[16] = 0.0; - out_7181373993335857162[17] = 0.0; - out_7181373993335857162[18] = 0.0; - out_7181373993335857162[19] = 1.0; - out_7181373993335857162[20] = 0.0; - out_7181373993335857162[21] = 0.0; - out_7181373993335857162[22] = 0.0; - out_7181373993335857162[23] = 0.0; - out_7181373993335857162[24] = 0.0; - out_7181373993335857162[25] = 0.0; - out_7181373993335857162[26] = 0.0; - out_7181373993335857162[27] = 0.0; - out_7181373993335857162[28] = 0.0; - out_7181373993335857162[29] = 0.0; - out_7181373993335857162[30] = 0.0; - out_7181373993335857162[31] = 0.0; - out_7181373993335857162[32] = 0.0; - out_7181373993335857162[33] = 0.0; - out_7181373993335857162[34] = 0.0; - out_7181373993335857162[35] = 0.0; - out_7181373993335857162[36] = 0.0; - out_7181373993335857162[37] = 0.0; - out_7181373993335857162[38] = 1.0; - out_7181373993335857162[39] = 0.0; - out_7181373993335857162[40] = 0.0; - out_7181373993335857162[41] = 0.0; - out_7181373993335857162[42] = 0.0; - out_7181373993335857162[43] = 0.0; - out_7181373993335857162[44] = 0.0; - out_7181373993335857162[45] = 0.0; - out_7181373993335857162[46] = 0.0; - out_7181373993335857162[47] = 0.0; - out_7181373993335857162[48] = 0.0; - out_7181373993335857162[49] = 0.0; - out_7181373993335857162[50] = 0.0; - out_7181373993335857162[51] = 0.0; - out_7181373993335857162[52] = 0.0; - out_7181373993335857162[53] = 0.0; - out_7181373993335857162[54] = 0.0; - out_7181373993335857162[55] = 0.0; - out_7181373993335857162[56] = 0.0; - out_7181373993335857162[57] = 1.0; - out_7181373993335857162[58] = 0.0; - out_7181373993335857162[59] = 0.0; - out_7181373993335857162[60] = 0.0; - out_7181373993335857162[61] = 0.0; - out_7181373993335857162[62] = 0.0; - out_7181373993335857162[63] = 0.0; - out_7181373993335857162[64] = 0.0; - out_7181373993335857162[65] = 0.0; - out_7181373993335857162[66] = 0.0; - out_7181373993335857162[67] = 0.0; - out_7181373993335857162[68] = 0.0; - out_7181373993335857162[69] = 0.0; - out_7181373993335857162[70] = 0.0; - out_7181373993335857162[71] = 0.0; - out_7181373993335857162[72] = 0.0; - out_7181373993335857162[73] = 0.0; - out_7181373993335857162[74] = 0.0; - out_7181373993335857162[75] = 0.0; - out_7181373993335857162[76] = 1.0; - out_7181373993335857162[77] = 0.0; - out_7181373993335857162[78] = 0.0; - out_7181373993335857162[79] = 0.0; - out_7181373993335857162[80] = 0.0; - out_7181373993335857162[81] = 0.0; - out_7181373993335857162[82] = 0.0; - out_7181373993335857162[83] = 0.0; - out_7181373993335857162[84] = 0.0; - out_7181373993335857162[85] = 0.0; - out_7181373993335857162[86] = 0.0; - out_7181373993335857162[87] = 0.0; - out_7181373993335857162[88] = 0.0; - out_7181373993335857162[89] = 0.0; - out_7181373993335857162[90] = 0.0; - out_7181373993335857162[91] = 0.0; - out_7181373993335857162[92] = 0.0; - out_7181373993335857162[93] = 0.0; - out_7181373993335857162[94] = 0.0; - out_7181373993335857162[95] = 1.0; - out_7181373993335857162[96] = 0.0; - out_7181373993335857162[97] = 0.0; - out_7181373993335857162[98] = 0.0; - out_7181373993335857162[99] = 0.0; - out_7181373993335857162[100] = 0.0; - out_7181373993335857162[101] = 0.0; - out_7181373993335857162[102] = 0.0; - out_7181373993335857162[103] = 0.0; - out_7181373993335857162[104] = 0.0; - out_7181373993335857162[105] = 0.0; - out_7181373993335857162[106] = 0.0; - out_7181373993335857162[107] = 0.0; - out_7181373993335857162[108] = 0.0; - out_7181373993335857162[109] = 0.0; - out_7181373993335857162[110] = 0.0; - out_7181373993335857162[111] = 0.0; - out_7181373993335857162[112] = 0.0; - out_7181373993335857162[113] = 0.0; - out_7181373993335857162[114] = 1.0; - out_7181373993335857162[115] = 0.0; - out_7181373993335857162[116] = 0.0; - out_7181373993335857162[117] = 0.0; - out_7181373993335857162[118] = 0.0; - out_7181373993335857162[119] = 0.0; - out_7181373993335857162[120] = 0.0; - out_7181373993335857162[121] = 0.0; - out_7181373993335857162[122] = 0.0; - out_7181373993335857162[123] = 0.0; - out_7181373993335857162[124] = 0.0; - out_7181373993335857162[125] = 0.0; - out_7181373993335857162[126] = 0.0; - out_7181373993335857162[127] = 0.0; - out_7181373993335857162[128] = 0.0; - out_7181373993335857162[129] = 0.0; - out_7181373993335857162[130] = 0.0; - out_7181373993335857162[131] = 0.0; - out_7181373993335857162[132] = 0.0; - out_7181373993335857162[133] = 1.0; - out_7181373993335857162[134] = 0.0; - out_7181373993335857162[135] = 0.0; - out_7181373993335857162[136] = 0.0; - out_7181373993335857162[137] = 0.0; - out_7181373993335857162[138] = 0.0; - out_7181373993335857162[139] = 0.0; - out_7181373993335857162[140] = 0.0; - out_7181373993335857162[141] = 0.0; - out_7181373993335857162[142] = 0.0; - out_7181373993335857162[143] = 0.0; - out_7181373993335857162[144] = 0.0; - out_7181373993335857162[145] = 0.0; - out_7181373993335857162[146] = 0.0; - out_7181373993335857162[147] = 0.0; - out_7181373993335857162[148] = 0.0; - out_7181373993335857162[149] = 0.0; - out_7181373993335857162[150] = 0.0; - out_7181373993335857162[151] = 0.0; - out_7181373993335857162[152] = 1.0; - out_7181373993335857162[153] = 0.0; - out_7181373993335857162[154] = 0.0; - out_7181373993335857162[155] = 0.0; - out_7181373993335857162[156] = 0.0; - out_7181373993335857162[157] = 0.0; - out_7181373993335857162[158] = 0.0; - out_7181373993335857162[159] = 0.0; - out_7181373993335857162[160] = 0.0; - out_7181373993335857162[161] = 0.0; - out_7181373993335857162[162] = 0.0; - out_7181373993335857162[163] = 0.0; - out_7181373993335857162[164] = 0.0; - out_7181373993335857162[165] = 0.0; - out_7181373993335857162[166] = 0.0; - out_7181373993335857162[167] = 0.0; - out_7181373993335857162[168] = 0.0; - out_7181373993335857162[169] = 0.0; - out_7181373993335857162[170] = 0.0; - out_7181373993335857162[171] = 1.0; - out_7181373993335857162[172] = 0.0; - out_7181373993335857162[173] = 0.0; - out_7181373993335857162[174] = 0.0; - out_7181373993335857162[175] = 0.0; - out_7181373993335857162[176] = 0.0; - out_7181373993335857162[177] = 0.0; - out_7181373993335857162[178] = 0.0; - out_7181373993335857162[179] = 0.0; - out_7181373993335857162[180] = 0.0; - out_7181373993335857162[181] = 0.0; - out_7181373993335857162[182] = 0.0; - out_7181373993335857162[183] = 0.0; - out_7181373993335857162[184] = 0.0; - out_7181373993335857162[185] = 0.0; - out_7181373993335857162[186] = 0.0; - out_7181373993335857162[187] = 0.0; - out_7181373993335857162[188] = 0.0; - out_7181373993335857162[189] = 0.0; - out_7181373993335857162[190] = 1.0; - out_7181373993335857162[191] = 0.0; - out_7181373993335857162[192] = 0.0; - out_7181373993335857162[193] = 0.0; - out_7181373993335857162[194] = 0.0; - out_7181373993335857162[195] = 0.0; - out_7181373993335857162[196] = 0.0; - out_7181373993335857162[197] = 0.0; - out_7181373993335857162[198] = 0.0; - out_7181373993335857162[199] = 0.0; - out_7181373993335857162[200] = 0.0; - out_7181373993335857162[201] = 0.0; - out_7181373993335857162[202] = 0.0; - out_7181373993335857162[203] = 0.0; - out_7181373993335857162[204] = 0.0; - out_7181373993335857162[205] = 0.0; - out_7181373993335857162[206] = 0.0; - out_7181373993335857162[207] = 0.0; - out_7181373993335857162[208] = 0.0; - out_7181373993335857162[209] = 1.0; - out_7181373993335857162[210] = 0.0; - out_7181373993335857162[211] = 0.0; - out_7181373993335857162[212] = 0.0; - out_7181373993335857162[213] = 0.0; - out_7181373993335857162[214] = 0.0; - out_7181373993335857162[215] = 0.0; - out_7181373993335857162[216] = 0.0; - out_7181373993335857162[217] = 0.0; - out_7181373993335857162[218] = 0.0; - out_7181373993335857162[219] = 0.0; - out_7181373993335857162[220] = 0.0; - out_7181373993335857162[221] = 0.0; - out_7181373993335857162[222] = 0.0; - out_7181373993335857162[223] = 0.0; - out_7181373993335857162[224] = 0.0; - out_7181373993335857162[225] = 0.0; - out_7181373993335857162[226] = 0.0; - out_7181373993335857162[227] = 0.0; - out_7181373993335857162[228] = 1.0; - out_7181373993335857162[229] = 0.0; - out_7181373993335857162[230] = 0.0; - out_7181373993335857162[231] = 0.0; - out_7181373993335857162[232] = 0.0; - out_7181373993335857162[233] = 0.0; - out_7181373993335857162[234] = 0.0; - out_7181373993335857162[235] = 0.0; - out_7181373993335857162[236] = 0.0; - out_7181373993335857162[237] = 0.0; - out_7181373993335857162[238] = 0.0; - out_7181373993335857162[239] = 0.0; - out_7181373993335857162[240] = 0.0; - out_7181373993335857162[241] = 0.0; - out_7181373993335857162[242] = 0.0; - out_7181373993335857162[243] = 0.0; - out_7181373993335857162[244] = 0.0; - out_7181373993335857162[245] = 0.0; - out_7181373993335857162[246] = 0.0; - out_7181373993335857162[247] = 1.0; - out_7181373993335857162[248] = 0.0; - out_7181373993335857162[249] = 0.0; - out_7181373993335857162[250] = 0.0; - out_7181373993335857162[251] = 0.0; - out_7181373993335857162[252] = 0.0; - out_7181373993335857162[253] = 0.0; - out_7181373993335857162[254] = 0.0; - out_7181373993335857162[255] = 0.0; - out_7181373993335857162[256] = 0.0; - out_7181373993335857162[257] = 0.0; - out_7181373993335857162[258] = 0.0; - out_7181373993335857162[259] = 0.0; - out_7181373993335857162[260] = 0.0; - out_7181373993335857162[261] = 0.0; - out_7181373993335857162[262] = 0.0; - out_7181373993335857162[263] = 0.0; - out_7181373993335857162[264] = 0.0; - out_7181373993335857162[265] = 0.0; - out_7181373993335857162[266] = 1.0; - out_7181373993335857162[267] = 0.0; - out_7181373993335857162[268] = 0.0; - out_7181373993335857162[269] = 0.0; - out_7181373993335857162[270] = 0.0; - out_7181373993335857162[271] = 0.0; - out_7181373993335857162[272] = 0.0; - out_7181373993335857162[273] = 0.0; - out_7181373993335857162[274] = 0.0; - out_7181373993335857162[275] = 0.0; - out_7181373993335857162[276] = 0.0; - out_7181373993335857162[277] = 0.0; - out_7181373993335857162[278] = 0.0; - out_7181373993335857162[279] = 0.0; - out_7181373993335857162[280] = 0.0; - out_7181373993335857162[281] = 0.0; - out_7181373993335857162[282] = 0.0; - out_7181373993335857162[283] = 0.0; - out_7181373993335857162[284] = 0.0; - out_7181373993335857162[285] = 1.0; - out_7181373993335857162[286] = 0.0; - out_7181373993335857162[287] = 0.0; - out_7181373993335857162[288] = 0.0; - out_7181373993335857162[289] = 0.0; - out_7181373993335857162[290] = 0.0; - out_7181373993335857162[291] = 0.0; - out_7181373993335857162[292] = 0.0; - out_7181373993335857162[293] = 0.0; - out_7181373993335857162[294] = 0.0; - out_7181373993335857162[295] = 0.0; - out_7181373993335857162[296] = 0.0; - out_7181373993335857162[297] = 0.0; - out_7181373993335857162[298] = 0.0; - out_7181373993335857162[299] = 0.0; - out_7181373993335857162[300] = 0.0; - out_7181373993335857162[301] = 0.0; - out_7181373993335857162[302] = 0.0; - out_7181373993335857162[303] = 0.0; - out_7181373993335857162[304] = 1.0; - out_7181373993335857162[305] = 0.0; - out_7181373993335857162[306] = 0.0; - out_7181373993335857162[307] = 0.0; - out_7181373993335857162[308] = 0.0; - out_7181373993335857162[309] = 0.0; - out_7181373993335857162[310] = 0.0; - out_7181373993335857162[311] = 0.0; - out_7181373993335857162[312] = 0.0; - out_7181373993335857162[313] = 0.0; - out_7181373993335857162[314] = 0.0; - out_7181373993335857162[315] = 0.0; - out_7181373993335857162[316] = 0.0; - out_7181373993335857162[317] = 0.0; - out_7181373993335857162[318] = 0.0; - out_7181373993335857162[319] = 0.0; - out_7181373993335857162[320] = 0.0; - out_7181373993335857162[321] = 0.0; - out_7181373993335857162[322] = 0.0; - out_7181373993335857162[323] = 1.0; +void H_mod_fun(double *state, double *out_2041474721312435071) { + out_2041474721312435071[0] = 1.0; + out_2041474721312435071[1] = 0.0; + out_2041474721312435071[2] = 0.0; + out_2041474721312435071[3] = 0.0; + out_2041474721312435071[4] = 0.0; + out_2041474721312435071[5] = 0.0; + out_2041474721312435071[6] = 0.0; + out_2041474721312435071[7] = 0.0; + out_2041474721312435071[8] = 0.0; + out_2041474721312435071[9] = 0.0; + out_2041474721312435071[10] = 0.0; + out_2041474721312435071[11] = 0.0; + out_2041474721312435071[12] = 0.0; + out_2041474721312435071[13] = 0.0; + out_2041474721312435071[14] = 0.0; + out_2041474721312435071[15] = 0.0; + out_2041474721312435071[16] = 0.0; + out_2041474721312435071[17] = 0.0; + out_2041474721312435071[18] = 0.0; + out_2041474721312435071[19] = 1.0; + out_2041474721312435071[20] = 0.0; + out_2041474721312435071[21] = 0.0; + out_2041474721312435071[22] = 0.0; + out_2041474721312435071[23] = 0.0; + out_2041474721312435071[24] = 0.0; + out_2041474721312435071[25] = 0.0; + out_2041474721312435071[26] = 0.0; + out_2041474721312435071[27] = 0.0; + out_2041474721312435071[28] = 0.0; + out_2041474721312435071[29] = 0.0; + out_2041474721312435071[30] = 0.0; + out_2041474721312435071[31] = 0.0; + out_2041474721312435071[32] = 0.0; + out_2041474721312435071[33] = 0.0; + out_2041474721312435071[34] = 0.0; + out_2041474721312435071[35] = 0.0; + out_2041474721312435071[36] = 0.0; + out_2041474721312435071[37] = 0.0; + out_2041474721312435071[38] = 1.0; + out_2041474721312435071[39] = 0.0; + out_2041474721312435071[40] = 0.0; + out_2041474721312435071[41] = 0.0; + out_2041474721312435071[42] = 0.0; + out_2041474721312435071[43] = 0.0; + out_2041474721312435071[44] = 0.0; + out_2041474721312435071[45] = 0.0; + out_2041474721312435071[46] = 0.0; + out_2041474721312435071[47] = 0.0; + out_2041474721312435071[48] = 0.0; + out_2041474721312435071[49] = 0.0; + out_2041474721312435071[50] = 0.0; + out_2041474721312435071[51] = 0.0; + out_2041474721312435071[52] = 0.0; + out_2041474721312435071[53] = 0.0; + out_2041474721312435071[54] = 0.0; + out_2041474721312435071[55] = 0.0; + out_2041474721312435071[56] = 0.0; + out_2041474721312435071[57] = 1.0; + out_2041474721312435071[58] = 0.0; + out_2041474721312435071[59] = 0.0; + out_2041474721312435071[60] = 0.0; + out_2041474721312435071[61] = 0.0; + out_2041474721312435071[62] = 0.0; + out_2041474721312435071[63] = 0.0; + out_2041474721312435071[64] = 0.0; + out_2041474721312435071[65] = 0.0; + out_2041474721312435071[66] = 0.0; + out_2041474721312435071[67] = 0.0; + out_2041474721312435071[68] = 0.0; + out_2041474721312435071[69] = 0.0; + out_2041474721312435071[70] = 0.0; + out_2041474721312435071[71] = 0.0; + out_2041474721312435071[72] = 0.0; + out_2041474721312435071[73] = 0.0; + out_2041474721312435071[74] = 0.0; + out_2041474721312435071[75] = 0.0; + out_2041474721312435071[76] = 1.0; + out_2041474721312435071[77] = 0.0; + out_2041474721312435071[78] = 0.0; + out_2041474721312435071[79] = 0.0; + out_2041474721312435071[80] = 0.0; + out_2041474721312435071[81] = 0.0; + out_2041474721312435071[82] = 0.0; + out_2041474721312435071[83] = 0.0; + out_2041474721312435071[84] = 0.0; + out_2041474721312435071[85] = 0.0; + out_2041474721312435071[86] = 0.0; + out_2041474721312435071[87] = 0.0; + out_2041474721312435071[88] = 0.0; + out_2041474721312435071[89] = 0.0; + out_2041474721312435071[90] = 0.0; + out_2041474721312435071[91] = 0.0; + out_2041474721312435071[92] = 0.0; + out_2041474721312435071[93] = 0.0; + out_2041474721312435071[94] = 0.0; + out_2041474721312435071[95] = 1.0; + out_2041474721312435071[96] = 0.0; + out_2041474721312435071[97] = 0.0; + out_2041474721312435071[98] = 0.0; + out_2041474721312435071[99] = 0.0; + out_2041474721312435071[100] = 0.0; + out_2041474721312435071[101] = 0.0; + out_2041474721312435071[102] = 0.0; + out_2041474721312435071[103] = 0.0; + out_2041474721312435071[104] = 0.0; + out_2041474721312435071[105] = 0.0; + out_2041474721312435071[106] = 0.0; + out_2041474721312435071[107] = 0.0; + out_2041474721312435071[108] = 0.0; + out_2041474721312435071[109] = 0.0; + out_2041474721312435071[110] = 0.0; + out_2041474721312435071[111] = 0.0; + out_2041474721312435071[112] = 0.0; + out_2041474721312435071[113] = 0.0; + out_2041474721312435071[114] = 1.0; + out_2041474721312435071[115] = 0.0; + out_2041474721312435071[116] = 0.0; + out_2041474721312435071[117] = 0.0; + out_2041474721312435071[118] = 0.0; + out_2041474721312435071[119] = 0.0; + out_2041474721312435071[120] = 0.0; + out_2041474721312435071[121] = 0.0; + out_2041474721312435071[122] = 0.0; + out_2041474721312435071[123] = 0.0; + out_2041474721312435071[124] = 0.0; + out_2041474721312435071[125] = 0.0; + out_2041474721312435071[126] = 0.0; + out_2041474721312435071[127] = 0.0; + out_2041474721312435071[128] = 0.0; + out_2041474721312435071[129] = 0.0; + out_2041474721312435071[130] = 0.0; + out_2041474721312435071[131] = 0.0; + out_2041474721312435071[132] = 0.0; + out_2041474721312435071[133] = 1.0; + out_2041474721312435071[134] = 0.0; + out_2041474721312435071[135] = 0.0; + out_2041474721312435071[136] = 0.0; + out_2041474721312435071[137] = 0.0; + out_2041474721312435071[138] = 0.0; + out_2041474721312435071[139] = 0.0; + out_2041474721312435071[140] = 0.0; + out_2041474721312435071[141] = 0.0; + out_2041474721312435071[142] = 0.0; + out_2041474721312435071[143] = 0.0; + out_2041474721312435071[144] = 0.0; + out_2041474721312435071[145] = 0.0; + out_2041474721312435071[146] = 0.0; + out_2041474721312435071[147] = 0.0; + out_2041474721312435071[148] = 0.0; + out_2041474721312435071[149] = 0.0; + out_2041474721312435071[150] = 0.0; + out_2041474721312435071[151] = 0.0; + out_2041474721312435071[152] = 1.0; + out_2041474721312435071[153] = 0.0; + out_2041474721312435071[154] = 0.0; + out_2041474721312435071[155] = 0.0; + out_2041474721312435071[156] = 0.0; + out_2041474721312435071[157] = 0.0; + out_2041474721312435071[158] = 0.0; + out_2041474721312435071[159] = 0.0; + out_2041474721312435071[160] = 0.0; + out_2041474721312435071[161] = 0.0; + out_2041474721312435071[162] = 0.0; + out_2041474721312435071[163] = 0.0; + out_2041474721312435071[164] = 0.0; + out_2041474721312435071[165] = 0.0; + out_2041474721312435071[166] = 0.0; + out_2041474721312435071[167] = 0.0; + out_2041474721312435071[168] = 0.0; + out_2041474721312435071[169] = 0.0; + out_2041474721312435071[170] = 0.0; + out_2041474721312435071[171] = 1.0; + out_2041474721312435071[172] = 0.0; + out_2041474721312435071[173] = 0.0; + out_2041474721312435071[174] = 0.0; + out_2041474721312435071[175] = 0.0; + out_2041474721312435071[176] = 0.0; + out_2041474721312435071[177] = 0.0; + out_2041474721312435071[178] = 0.0; + out_2041474721312435071[179] = 0.0; + out_2041474721312435071[180] = 0.0; + out_2041474721312435071[181] = 0.0; + out_2041474721312435071[182] = 0.0; + out_2041474721312435071[183] = 0.0; + out_2041474721312435071[184] = 0.0; + out_2041474721312435071[185] = 0.0; + out_2041474721312435071[186] = 0.0; + out_2041474721312435071[187] = 0.0; + out_2041474721312435071[188] = 0.0; + out_2041474721312435071[189] = 0.0; + out_2041474721312435071[190] = 1.0; + out_2041474721312435071[191] = 0.0; + out_2041474721312435071[192] = 0.0; + out_2041474721312435071[193] = 0.0; + out_2041474721312435071[194] = 0.0; + out_2041474721312435071[195] = 0.0; + out_2041474721312435071[196] = 0.0; + out_2041474721312435071[197] = 0.0; + out_2041474721312435071[198] = 0.0; + out_2041474721312435071[199] = 0.0; + out_2041474721312435071[200] = 0.0; + out_2041474721312435071[201] = 0.0; + out_2041474721312435071[202] = 0.0; + out_2041474721312435071[203] = 0.0; + out_2041474721312435071[204] = 0.0; + out_2041474721312435071[205] = 0.0; + out_2041474721312435071[206] = 0.0; + out_2041474721312435071[207] = 0.0; + out_2041474721312435071[208] = 0.0; + out_2041474721312435071[209] = 1.0; + out_2041474721312435071[210] = 0.0; + out_2041474721312435071[211] = 0.0; + out_2041474721312435071[212] = 0.0; + out_2041474721312435071[213] = 0.0; + out_2041474721312435071[214] = 0.0; + out_2041474721312435071[215] = 0.0; + out_2041474721312435071[216] = 0.0; + out_2041474721312435071[217] = 0.0; + out_2041474721312435071[218] = 0.0; + out_2041474721312435071[219] = 0.0; + out_2041474721312435071[220] = 0.0; + out_2041474721312435071[221] = 0.0; + out_2041474721312435071[222] = 0.0; + out_2041474721312435071[223] = 0.0; + out_2041474721312435071[224] = 0.0; + out_2041474721312435071[225] = 0.0; + out_2041474721312435071[226] = 0.0; + out_2041474721312435071[227] = 0.0; + out_2041474721312435071[228] = 1.0; + out_2041474721312435071[229] = 0.0; + out_2041474721312435071[230] = 0.0; + out_2041474721312435071[231] = 0.0; + out_2041474721312435071[232] = 0.0; + out_2041474721312435071[233] = 0.0; + out_2041474721312435071[234] = 0.0; + out_2041474721312435071[235] = 0.0; + out_2041474721312435071[236] = 0.0; + out_2041474721312435071[237] = 0.0; + out_2041474721312435071[238] = 0.0; + out_2041474721312435071[239] = 0.0; + out_2041474721312435071[240] = 0.0; + out_2041474721312435071[241] = 0.0; + out_2041474721312435071[242] = 0.0; + out_2041474721312435071[243] = 0.0; + out_2041474721312435071[244] = 0.0; + out_2041474721312435071[245] = 0.0; + out_2041474721312435071[246] = 0.0; + out_2041474721312435071[247] = 1.0; + out_2041474721312435071[248] = 0.0; + out_2041474721312435071[249] = 0.0; + out_2041474721312435071[250] = 0.0; + out_2041474721312435071[251] = 0.0; + out_2041474721312435071[252] = 0.0; + out_2041474721312435071[253] = 0.0; + out_2041474721312435071[254] = 0.0; + out_2041474721312435071[255] = 0.0; + out_2041474721312435071[256] = 0.0; + out_2041474721312435071[257] = 0.0; + out_2041474721312435071[258] = 0.0; + out_2041474721312435071[259] = 0.0; + out_2041474721312435071[260] = 0.0; + out_2041474721312435071[261] = 0.0; + out_2041474721312435071[262] = 0.0; + out_2041474721312435071[263] = 0.0; + out_2041474721312435071[264] = 0.0; + out_2041474721312435071[265] = 0.0; + out_2041474721312435071[266] = 1.0; + out_2041474721312435071[267] = 0.0; + out_2041474721312435071[268] = 0.0; + out_2041474721312435071[269] = 0.0; + out_2041474721312435071[270] = 0.0; + out_2041474721312435071[271] = 0.0; + out_2041474721312435071[272] = 0.0; + out_2041474721312435071[273] = 0.0; + out_2041474721312435071[274] = 0.0; + out_2041474721312435071[275] = 0.0; + out_2041474721312435071[276] = 0.0; + out_2041474721312435071[277] = 0.0; + out_2041474721312435071[278] = 0.0; + out_2041474721312435071[279] = 0.0; + out_2041474721312435071[280] = 0.0; + out_2041474721312435071[281] = 0.0; + out_2041474721312435071[282] = 0.0; + out_2041474721312435071[283] = 0.0; + out_2041474721312435071[284] = 0.0; + out_2041474721312435071[285] = 1.0; + out_2041474721312435071[286] = 0.0; + out_2041474721312435071[287] = 0.0; + out_2041474721312435071[288] = 0.0; + out_2041474721312435071[289] = 0.0; + out_2041474721312435071[290] = 0.0; + out_2041474721312435071[291] = 0.0; + out_2041474721312435071[292] = 0.0; + out_2041474721312435071[293] = 0.0; + out_2041474721312435071[294] = 0.0; + out_2041474721312435071[295] = 0.0; + out_2041474721312435071[296] = 0.0; + out_2041474721312435071[297] = 0.0; + out_2041474721312435071[298] = 0.0; + out_2041474721312435071[299] = 0.0; + out_2041474721312435071[300] = 0.0; + out_2041474721312435071[301] = 0.0; + out_2041474721312435071[302] = 0.0; + out_2041474721312435071[303] = 0.0; + out_2041474721312435071[304] = 1.0; + out_2041474721312435071[305] = 0.0; + out_2041474721312435071[306] = 0.0; + out_2041474721312435071[307] = 0.0; + out_2041474721312435071[308] = 0.0; + out_2041474721312435071[309] = 0.0; + out_2041474721312435071[310] = 0.0; + out_2041474721312435071[311] = 0.0; + out_2041474721312435071[312] = 0.0; + out_2041474721312435071[313] = 0.0; + out_2041474721312435071[314] = 0.0; + out_2041474721312435071[315] = 0.0; + out_2041474721312435071[316] = 0.0; + out_2041474721312435071[317] = 0.0; + out_2041474721312435071[318] = 0.0; + out_2041474721312435071[319] = 0.0; + out_2041474721312435071[320] = 0.0; + out_2041474721312435071[321] = 0.0; + out_2041474721312435071[322] = 0.0; + out_2041474721312435071[323] = 1.0; } -void f_fun(double *state, double dt, double *out_1544031639553813882) { - out_1544031639553813882[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_1544031639553813882[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_1544031639553813882[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_1544031639553813882[3] = dt*state[12] + state[3]; - out_1544031639553813882[4] = dt*state[13] + state[4]; - out_1544031639553813882[5] = dt*state[14] + state[5]; - out_1544031639553813882[6] = state[6]; - out_1544031639553813882[7] = state[7]; - out_1544031639553813882[8] = state[8]; - out_1544031639553813882[9] = state[9]; - out_1544031639553813882[10] = state[10]; - out_1544031639553813882[11] = state[11]; - out_1544031639553813882[12] = state[12]; - out_1544031639553813882[13] = state[13]; - out_1544031639553813882[14] = state[14]; - out_1544031639553813882[15] = state[15]; - out_1544031639553813882[16] = state[16]; - out_1544031639553813882[17] = state[17]; +void f_fun(double *state, double dt, double *out_3319995548118437592) { + out_3319995548118437592[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_3319995548118437592[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_3319995548118437592[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_3319995548118437592[3] = dt*state[12] + state[3]; + out_3319995548118437592[4] = dt*state[13] + state[4]; + out_3319995548118437592[5] = dt*state[14] + state[5]; + out_3319995548118437592[6] = state[6]; + out_3319995548118437592[7] = state[7]; + out_3319995548118437592[8] = state[8]; + out_3319995548118437592[9] = state[9]; + out_3319995548118437592[10] = state[10]; + out_3319995548118437592[11] = state[11]; + out_3319995548118437592[12] = state[12]; + out_3319995548118437592[13] = state[13]; + out_3319995548118437592[14] = state[14]; + out_3319995548118437592[15] = state[15]; + out_3319995548118437592[16] = state[16]; + out_3319995548118437592[17] = state[17]; } -void F_fun(double *state, double dt, double *out_4307246870374060335) { - out_4307246870374060335[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_4307246870374060335[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_4307246870374060335[2] = 0; - out_4307246870374060335[3] = 0; - out_4307246870374060335[4] = 0; - out_4307246870374060335[5] = 0; - out_4307246870374060335[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_4307246870374060335[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_4307246870374060335[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_4307246870374060335[9] = 0; - out_4307246870374060335[10] = 0; - out_4307246870374060335[11] = 0; - out_4307246870374060335[12] = 0; - out_4307246870374060335[13] = 0; - out_4307246870374060335[14] = 0; - out_4307246870374060335[15] = 0; - out_4307246870374060335[16] = 0; - out_4307246870374060335[17] = 0; - out_4307246870374060335[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_4307246870374060335[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_4307246870374060335[20] = 0; - out_4307246870374060335[21] = 0; - out_4307246870374060335[22] = 0; - out_4307246870374060335[23] = 0; - out_4307246870374060335[24] = 0; - out_4307246870374060335[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_4307246870374060335[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_4307246870374060335[27] = 0; - out_4307246870374060335[28] = 0; - out_4307246870374060335[29] = 0; - out_4307246870374060335[30] = 0; - out_4307246870374060335[31] = 0; - out_4307246870374060335[32] = 0; - out_4307246870374060335[33] = 0; - out_4307246870374060335[34] = 0; - out_4307246870374060335[35] = 0; - out_4307246870374060335[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_4307246870374060335[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_4307246870374060335[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_4307246870374060335[39] = 0; - out_4307246870374060335[40] = 0; - out_4307246870374060335[41] = 0; - out_4307246870374060335[42] = 0; - out_4307246870374060335[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_4307246870374060335[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_4307246870374060335[45] = 0; - out_4307246870374060335[46] = 0; - out_4307246870374060335[47] = 0; - out_4307246870374060335[48] = 0; - out_4307246870374060335[49] = 0; - out_4307246870374060335[50] = 0; - out_4307246870374060335[51] = 0; - out_4307246870374060335[52] = 0; - out_4307246870374060335[53] = 0; - out_4307246870374060335[54] = 0; - out_4307246870374060335[55] = 0; - out_4307246870374060335[56] = 0; - out_4307246870374060335[57] = 1; - out_4307246870374060335[58] = 0; - out_4307246870374060335[59] = 0; - out_4307246870374060335[60] = 0; - out_4307246870374060335[61] = 0; - out_4307246870374060335[62] = 0; - out_4307246870374060335[63] = 0; - out_4307246870374060335[64] = 0; - out_4307246870374060335[65] = 0; - out_4307246870374060335[66] = dt; - out_4307246870374060335[67] = 0; - out_4307246870374060335[68] = 0; - out_4307246870374060335[69] = 0; - out_4307246870374060335[70] = 0; - out_4307246870374060335[71] = 0; - out_4307246870374060335[72] = 0; - out_4307246870374060335[73] = 0; - out_4307246870374060335[74] = 0; - out_4307246870374060335[75] = 0; - out_4307246870374060335[76] = 1; - out_4307246870374060335[77] = 0; - out_4307246870374060335[78] = 0; - out_4307246870374060335[79] = 0; - out_4307246870374060335[80] = 0; - out_4307246870374060335[81] = 0; - out_4307246870374060335[82] = 0; - out_4307246870374060335[83] = 0; - out_4307246870374060335[84] = 0; - out_4307246870374060335[85] = dt; - out_4307246870374060335[86] = 0; - out_4307246870374060335[87] = 0; - out_4307246870374060335[88] = 0; - out_4307246870374060335[89] = 0; - out_4307246870374060335[90] = 0; - out_4307246870374060335[91] = 0; - out_4307246870374060335[92] = 0; - out_4307246870374060335[93] = 0; - out_4307246870374060335[94] = 0; - out_4307246870374060335[95] = 1; - out_4307246870374060335[96] = 0; - out_4307246870374060335[97] = 0; - out_4307246870374060335[98] = 0; - out_4307246870374060335[99] = 0; - out_4307246870374060335[100] = 0; - out_4307246870374060335[101] = 0; - out_4307246870374060335[102] = 0; - out_4307246870374060335[103] = 0; - out_4307246870374060335[104] = dt; - out_4307246870374060335[105] = 0; - out_4307246870374060335[106] = 0; - out_4307246870374060335[107] = 0; - out_4307246870374060335[108] = 0; - out_4307246870374060335[109] = 0; - out_4307246870374060335[110] = 0; - out_4307246870374060335[111] = 0; - out_4307246870374060335[112] = 0; - out_4307246870374060335[113] = 0; - out_4307246870374060335[114] = 1; - out_4307246870374060335[115] = 0; - out_4307246870374060335[116] = 0; - out_4307246870374060335[117] = 0; - out_4307246870374060335[118] = 0; - out_4307246870374060335[119] = 0; - out_4307246870374060335[120] = 0; - out_4307246870374060335[121] = 0; - out_4307246870374060335[122] = 0; - out_4307246870374060335[123] = 0; - out_4307246870374060335[124] = 0; - out_4307246870374060335[125] = 0; - out_4307246870374060335[126] = 0; - out_4307246870374060335[127] = 0; - out_4307246870374060335[128] = 0; - out_4307246870374060335[129] = 0; - out_4307246870374060335[130] = 0; - out_4307246870374060335[131] = 0; - out_4307246870374060335[132] = 0; - out_4307246870374060335[133] = 1; - out_4307246870374060335[134] = 0; - out_4307246870374060335[135] = 0; - out_4307246870374060335[136] = 0; - out_4307246870374060335[137] = 0; - out_4307246870374060335[138] = 0; - out_4307246870374060335[139] = 0; - out_4307246870374060335[140] = 0; - out_4307246870374060335[141] = 0; - out_4307246870374060335[142] = 0; - out_4307246870374060335[143] = 0; - out_4307246870374060335[144] = 0; - out_4307246870374060335[145] = 0; - out_4307246870374060335[146] = 0; - out_4307246870374060335[147] = 0; - out_4307246870374060335[148] = 0; - out_4307246870374060335[149] = 0; - out_4307246870374060335[150] = 0; - out_4307246870374060335[151] = 0; - out_4307246870374060335[152] = 1; - out_4307246870374060335[153] = 0; - out_4307246870374060335[154] = 0; - out_4307246870374060335[155] = 0; - out_4307246870374060335[156] = 0; - out_4307246870374060335[157] = 0; - out_4307246870374060335[158] = 0; - out_4307246870374060335[159] = 0; - out_4307246870374060335[160] = 0; - out_4307246870374060335[161] = 0; - out_4307246870374060335[162] = 0; - out_4307246870374060335[163] = 0; - out_4307246870374060335[164] = 0; - out_4307246870374060335[165] = 0; - out_4307246870374060335[166] = 0; - out_4307246870374060335[167] = 0; - out_4307246870374060335[168] = 0; - out_4307246870374060335[169] = 0; - out_4307246870374060335[170] = 0; - out_4307246870374060335[171] = 1; - out_4307246870374060335[172] = 0; - out_4307246870374060335[173] = 0; - out_4307246870374060335[174] = 0; - out_4307246870374060335[175] = 0; - out_4307246870374060335[176] = 0; - out_4307246870374060335[177] = 0; - out_4307246870374060335[178] = 0; - out_4307246870374060335[179] = 0; - out_4307246870374060335[180] = 0; - out_4307246870374060335[181] = 0; - out_4307246870374060335[182] = 0; - out_4307246870374060335[183] = 0; - out_4307246870374060335[184] = 0; - out_4307246870374060335[185] = 0; - out_4307246870374060335[186] = 0; - out_4307246870374060335[187] = 0; - out_4307246870374060335[188] = 0; - out_4307246870374060335[189] = 0; - out_4307246870374060335[190] = 1; - out_4307246870374060335[191] = 0; - out_4307246870374060335[192] = 0; - out_4307246870374060335[193] = 0; - out_4307246870374060335[194] = 0; - out_4307246870374060335[195] = 0; - out_4307246870374060335[196] = 0; - out_4307246870374060335[197] = 0; - out_4307246870374060335[198] = 0; - out_4307246870374060335[199] = 0; - out_4307246870374060335[200] = 0; - out_4307246870374060335[201] = 0; - out_4307246870374060335[202] = 0; - out_4307246870374060335[203] = 0; - out_4307246870374060335[204] = 0; - out_4307246870374060335[205] = 0; - out_4307246870374060335[206] = 0; - out_4307246870374060335[207] = 0; - out_4307246870374060335[208] = 0; - out_4307246870374060335[209] = 1; - out_4307246870374060335[210] = 0; - out_4307246870374060335[211] = 0; - out_4307246870374060335[212] = 0; - out_4307246870374060335[213] = 0; - out_4307246870374060335[214] = 0; - out_4307246870374060335[215] = 0; - out_4307246870374060335[216] = 0; - out_4307246870374060335[217] = 0; - out_4307246870374060335[218] = 0; - out_4307246870374060335[219] = 0; - out_4307246870374060335[220] = 0; - out_4307246870374060335[221] = 0; - out_4307246870374060335[222] = 0; - out_4307246870374060335[223] = 0; - out_4307246870374060335[224] = 0; - out_4307246870374060335[225] = 0; - out_4307246870374060335[226] = 0; - out_4307246870374060335[227] = 0; - out_4307246870374060335[228] = 1; - out_4307246870374060335[229] = 0; - out_4307246870374060335[230] = 0; - out_4307246870374060335[231] = 0; - out_4307246870374060335[232] = 0; - out_4307246870374060335[233] = 0; - out_4307246870374060335[234] = 0; - out_4307246870374060335[235] = 0; - out_4307246870374060335[236] = 0; - out_4307246870374060335[237] = 0; - out_4307246870374060335[238] = 0; - out_4307246870374060335[239] = 0; - out_4307246870374060335[240] = 0; - out_4307246870374060335[241] = 0; - out_4307246870374060335[242] = 0; - out_4307246870374060335[243] = 0; - out_4307246870374060335[244] = 0; - out_4307246870374060335[245] = 0; - out_4307246870374060335[246] = 0; - out_4307246870374060335[247] = 1; - out_4307246870374060335[248] = 0; - out_4307246870374060335[249] = 0; - out_4307246870374060335[250] = 0; - out_4307246870374060335[251] = 0; - out_4307246870374060335[252] = 0; - out_4307246870374060335[253] = 0; - out_4307246870374060335[254] = 0; - out_4307246870374060335[255] = 0; - out_4307246870374060335[256] = 0; - out_4307246870374060335[257] = 0; - out_4307246870374060335[258] = 0; - out_4307246870374060335[259] = 0; - out_4307246870374060335[260] = 0; - out_4307246870374060335[261] = 0; - out_4307246870374060335[262] = 0; - out_4307246870374060335[263] = 0; - out_4307246870374060335[264] = 0; - out_4307246870374060335[265] = 0; - out_4307246870374060335[266] = 1; - out_4307246870374060335[267] = 0; - out_4307246870374060335[268] = 0; - out_4307246870374060335[269] = 0; - out_4307246870374060335[270] = 0; - out_4307246870374060335[271] = 0; - out_4307246870374060335[272] = 0; - out_4307246870374060335[273] = 0; - out_4307246870374060335[274] = 0; - out_4307246870374060335[275] = 0; - out_4307246870374060335[276] = 0; - out_4307246870374060335[277] = 0; - out_4307246870374060335[278] = 0; - out_4307246870374060335[279] = 0; - out_4307246870374060335[280] = 0; - out_4307246870374060335[281] = 0; - out_4307246870374060335[282] = 0; - out_4307246870374060335[283] = 0; - out_4307246870374060335[284] = 0; - out_4307246870374060335[285] = 1; - out_4307246870374060335[286] = 0; - out_4307246870374060335[287] = 0; - out_4307246870374060335[288] = 0; - out_4307246870374060335[289] = 0; - out_4307246870374060335[290] = 0; - out_4307246870374060335[291] = 0; - out_4307246870374060335[292] = 0; - out_4307246870374060335[293] = 0; - out_4307246870374060335[294] = 0; - out_4307246870374060335[295] = 0; - out_4307246870374060335[296] = 0; - out_4307246870374060335[297] = 0; - out_4307246870374060335[298] = 0; - out_4307246870374060335[299] = 0; - out_4307246870374060335[300] = 0; - out_4307246870374060335[301] = 0; - out_4307246870374060335[302] = 0; - out_4307246870374060335[303] = 0; - out_4307246870374060335[304] = 1; - out_4307246870374060335[305] = 0; - out_4307246870374060335[306] = 0; - out_4307246870374060335[307] = 0; - out_4307246870374060335[308] = 0; - out_4307246870374060335[309] = 0; - out_4307246870374060335[310] = 0; - out_4307246870374060335[311] = 0; - out_4307246870374060335[312] = 0; - out_4307246870374060335[313] = 0; - out_4307246870374060335[314] = 0; - out_4307246870374060335[315] = 0; - out_4307246870374060335[316] = 0; - out_4307246870374060335[317] = 0; - out_4307246870374060335[318] = 0; - out_4307246870374060335[319] = 0; - out_4307246870374060335[320] = 0; - out_4307246870374060335[321] = 0; - out_4307246870374060335[322] = 0; - out_4307246870374060335[323] = 1; +void F_fun(double *state, double dt, double *out_1297884238983223646) { + out_1297884238983223646[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_1297884238983223646[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_1297884238983223646[2] = 0; + out_1297884238983223646[3] = 0; + out_1297884238983223646[4] = 0; + out_1297884238983223646[5] = 0; + out_1297884238983223646[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_1297884238983223646[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_1297884238983223646[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_1297884238983223646[9] = 0; + out_1297884238983223646[10] = 0; + out_1297884238983223646[11] = 0; + out_1297884238983223646[12] = 0; + out_1297884238983223646[13] = 0; + out_1297884238983223646[14] = 0; + out_1297884238983223646[15] = 0; + out_1297884238983223646[16] = 0; + out_1297884238983223646[17] = 0; + out_1297884238983223646[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_1297884238983223646[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_1297884238983223646[20] = 0; + out_1297884238983223646[21] = 0; + out_1297884238983223646[22] = 0; + out_1297884238983223646[23] = 0; + out_1297884238983223646[24] = 0; + out_1297884238983223646[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_1297884238983223646[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_1297884238983223646[27] = 0; + out_1297884238983223646[28] = 0; + out_1297884238983223646[29] = 0; + out_1297884238983223646[30] = 0; + out_1297884238983223646[31] = 0; + out_1297884238983223646[32] = 0; + out_1297884238983223646[33] = 0; + out_1297884238983223646[34] = 0; + out_1297884238983223646[35] = 0; + out_1297884238983223646[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_1297884238983223646[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_1297884238983223646[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_1297884238983223646[39] = 0; + out_1297884238983223646[40] = 0; + out_1297884238983223646[41] = 0; + out_1297884238983223646[42] = 0; + out_1297884238983223646[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_1297884238983223646[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_1297884238983223646[45] = 0; + out_1297884238983223646[46] = 0; + out_1297884238983223646[47] = 0; + out_1297884238983223646[48] = 0; + out_1297884238983223646[49] = 0; + out_1297884238983223646[50] = 0; + out_1297884238983223646[51] = 0; + out_1297884238983223646[52] = 0; + out_1297884238983223646[53] = 0; + out_1297884238983223646[54] = 0; + out_1297884238983223646[55] = 0; + out_1297884238983223646[56] = 0; + out_1297884238983223646[57] = 1; + out_1297884238983223646[58] = 0; + out_1297884238983223646[59] = 0; + out_1297884238983223646[60] = 0; + out_1297884238983223646[61] = 0; + out_1297884238983223646[62] = 0; + out_1297884238983223646[63] = 0; + out_1297884238983223646[64] = 0; + out_1297884238983223646[65] = 0; + out_1297884238983223646[66] = dt; + out_1297884238983223646[67] = 0; + out_1297884238983223646[68] = 0; + out_1297884238983223646[69] = 0; + out_1297884238983223646[70] = 0; + out_1297884238983223646[71] = 0; + out_1297884238983223646[72] = 0; + out_1297884238983223646[73] = 0; + out_1297884238983223646[74] = 0; + out_1297884238983223646[75] = 0; + out_1297884238983223646[76] = 1; + out_1297884238983223646[77] = 0; + out_1297884238983223646[78] = 0; + out_1297884238983223646[79] = 0; + out_1297884238983223646[80] = 0; + out_1297884238983223646[81] = 0; + out_1297884238983223646[82] = 0; + out_1297884238983223646[83] = 0; + out_1297884238983223646[84] = 0; + out_1297884238983223646[85] = dt; + out_1297884238983223646[86] = 0; + out_1297884238983223646[87] = 0; + out_1297884238983223646[88] = 0; + out_1297884238983223646[89] = 0; + out_1297884238983223646[90] = 0; + out_1297884238983223646[91] = 0; + out_1297884238983223646[92] = 0; + out_1297884238983223646[93] = 0; + out_1297884238983223646[94] = 0; + out_1297884238983223646[95] = 1; + out_1297884238983223646[96] = 0; + out_1297884238983223646[97] = 0; + out_1297884238983223646[98] = 0; + out_1297884238983223646[99] = 0; + out_1297884238983223646[100] = 0; + out_1297884238983223646[101] = 0; + out_1297884238983223646[102] = 0; + out_1297884238983223646[103] = 0; + out_1297884238983223646[104] = dt; + out_1297884238983223646[105] = 0; + out_1297884238983223646[106] = 0; + out_1297884238983223646[107] = 0; + out_1297884238983223646[108] = 0; + out_1297884238983223646[109] = 0; + out_1297884238983223646[110] = 0; + out_1297884238983223646[111] = 0; + out_1297884238983223646[112] = 0; + out_1297884238983223646[113] = 0; + out_1297884238983223646[114] = 1; + out_1297884238983223646[115] = 0; + out_1297884238983223646[116] = 0; + out_1297884238983223646[117] = 0; + out_1297884238983223646[118] = 0; + out_1297884238983223646[119] = 0; + out_1297884238983223646[120] = 0; + out_1297884238983223646[121] = 0; + out_1297884238983223646[122] = 0; + out_1297884238983223646[123] = 0; + out_1297884238983223646[124] = 0; + out_1297884238983223646[125] = 0; + out_1297884238983223646[126] = 0; + out_1297884238983223646[127] = 0; + out_1297884238983223646[128] = 0; + out_1297884238983223646[129] = 0; + out_1297884238983223646[130] = 0; + out_1297884238983223646[131] = 0; + out_1297884238983223646[132] = 0; + out_1297884238983223646[133] = 1; + out_1297884238983223646[134] = 0; + out_1297884238983223646[135] = 0; + out_1297884238983223646[136] = 0; + out_1297884238983223646[137] = 0; + out_1297884238983223646[138] = 0; + out_1297884238983223646[139] = 0; + out_1297884238983223646[140] = 0; + out_1297884238983223646[141] = 0; + out_1297884238983223646[142] = 0; + out_1297884238983223646[143] = 0; + out_1297884238983223646[144] = 0; + out_1297884238983223646[145] = 0; + out_1297884238983223646[146] = 0; + out_1297884238983223646[147] = 0; + out_1297884238983223646[148] = 0; + out_1297884238983223646[149] = 0; + out_1297884238983223646[150] = 0; + out_1297884238983223646[151] = 0; + out_1297884238983223646[152] = 1; + out_1297884238983223646[153] = 0; + out_1297884238983223646[154] = 0; + out_1297884238983223646[155] = 0; + out_1297884238983223646[156] = 0; + out_1297884238983223646[157] = 0; + out_1297884238983223646[158] = 0; + out_1297884238983223646[159] = 0; + out_1297884238983223646[160] = 0; + out_1297884238983223646[161] = 0; + out_1297884238983223646[162] = 0; + out_1297884238983223646[163] = 0; + out_1297884238983223646[164] = 0; + out_1297884238983223646[165] = 0; + out_1297884238983223646[166] = 0; + out_1297884238983223646[167] = 0; + out_1297884238983223646[168] = 0; + out_1297884238983223646[169] = 0; + out_1297884238983223646[170] = 0; + out_1297884238983223646[171] = 1; + out_1297884238983223646[172] = 0; + out_1297884238983223646[173] = 0; + out_1297884238983223646[174] = 0; + out_1297884238983223646[175] = 0; + out_1297884238983223646[176] = 0; + out_1297884238983223646[177] = 0; + out_1297884238983223646[178] = 0; + out_1297884238983223646[179] = 0; + out_1297884238983223646[180] = 0; + out_1297884238983223646[181] = 0; + out_1297884238983223646[182] = 0; + out_1297884238983223646[183] = 0; + out_1297884238983223646[184] = 0; + out_1297884238983223646[185] = 0; + out_1297884238983223646[186] = 0; + out_1297884238983223646[187] = 0; + out_1297884238983223646[188] = 0; + out_1297884238983223646[189] = 0; + out_1297884238983223646[190] = 1; + out_1297884238983223646[191] = 0; + out_1297884238983223646[192] = 0; + out_1297884238983223646[193] = 0; + out_1297884238983223646[194] = 0; + out_1297884238983223646[195] = 0; + out_1297884238983223646[196] = 0; + out_1297884238983223646[197] = 0; + out_1297884238983223646[198] = 0; + out_1297884238983223646[199] = 0; + out_1297884238983223646[200] = 0; + out_1297884238983223646[201] = 0; + out_1297884238983223646[202] = 0; + out_1297884238983223646[203] = 0; + out_1297884238983223646[204] = 0; + out_1297884238983223646[205] = 0; + out_1297884238983223646[206] = 0; + out_1297884238983223646[207] = 0; + out_1297884238983223646[208] = 0; + out_1297884238983223646[209] = 1; + out_1297884238983223646[210] = 0; + out_1297884238983223646[211] = 0; + out_1297884238983223646[212] = 0; + out_1297884238983223646[213] = 0; + out_1297884238983223646[214] = 0; + out_1297884238983223646[215] = 0; + out_1297884238983223646[216] = 0; + out_1297884238983223646[217] = 0; + out_1297884238983223646[218] = 0; + out_1297884238983223646[219] = 0; + out_1297884238983223646[220] = 0; + out_1297884238983223646[221] = 0; + out_1297884238983223646[222] = 0; + out_1297884238983223646[223] = 0; + out_1297884238983223646[224] = 0; + out_1297884238983223646[225] = 0; + out_1297884238983223646[226] = 0; + out_1297884238983223646[227] = 0; + out_1297884238983223646[228] = 1; + out_1297884238983223646[229] = 0; + out_1297884238983223646[230] = 0; + out_1297884238983223646[231] = 0; + out_1297884238983223646[232] = 0; + out_1297884238983223646[233] = 0; + out_1297884238983223646[234] = 0; + out_1297884238983223646[235] = 0; + out_1297884238983223646[236] = 0; + out_1297884238983223646[237] = 0; + out_1297884238983223646[238] = 0; + out_1297884238983223646[239] = 0; + out_1297884238983223646[240] = 0; + out_1297884238983223646[241] = 0; + out_1297884238983223646[242] = 0; + out_1297884238983223646[243] = 0; + out_1297884238983223646[244] = 0; + out_1297884238983223646[245] = 0; + out_1297884238983223646[246] = 0; + out_1297884238983223646[247] = 1; + out_1297884238983223646[248] = 0; + out_1297884238983223646[249] = 0; + out_1297884238983223646[250] = 0; + out_1297884238983223646[251] = 0; + out_1297884238983223646[252] = 0; + out_1297884238983223646[253] = 0; + out_1297884238983223646[254] = 0; + out_1297884238983223646[255] = 0; + out_1297884238983223646[256] = 0; + out_1297884238983223646[257] = 0; + out_1297884238983223646[258] = 0; + out_1297884238983223646[259] = 0; + out_1297884238983223646[260] = 0; + out_1297884238983223646[261] = 0; + out_1297884238983223646[262] = 0; + out_1297884238983223646[263] = 0; + out_1297884238983223646[264] = 0; + out_1297884238983223646[265] = 0; + out_1297884238983223646[266] = 1; + out_1297884238983223646[267] = 0; + out_1297884238983223646[268] = 0; + out_1297884238983223646[269] = 0; + out_1297884238983223646[270] = 0; + out_1297884238983223646[271] = 0; + out_1297884238983223646[272] = 0; + out_1297884238983223646[273] = 0; + out_1297884238983223646[274] = 0; + out_1297884238983223646[275] = 0; + out_1297884238983223646[276] = 0; + out_1297884238983223646[277] = 0; + out_1297884238983223646[278] = 0; + out_1297884238983223646[279] = 0; + out_1297884238983223646[280] = 0; + out_1297884238983223646[281] = 0; + out_1297884238983223646[282] = 0; + out_1297884238983223646[283] = 0; + out_1297884238983223646[284] = 0; + out_1297884238983223646[285] = 1; + out_1297884238983223646[286] = 0; + out_1297884238983223646[287] = 0; + out_1297884238983223646[288] = 0; + out_1297884238983223646[289] = 0; + out_1297884238983223646[290] = 0; + out_1297884238983223646[291] = 0; + out_1297884238983223646[292] = 0; + out_1297884238983223646[293] = 0; + out_1297884238983223646[294] = 0; + out_1297884238983223646[295] = 0; + out_1297884238983223646[296] = 0; + out_1297884238983223646[297] = 0; + out_1297884238983223646[298] = 0; + out_1297884238983223646[299] = 0; + out_1297884238983223646[300] = 0; + out_1297884238983223646[301] = 0; + out_1297884238983223646[302] = 0; + out_1297884238983223646[303] = 0; + out_1297884238983223646[304] = 1; + out_1297884238983223646[305] = 0; + out_1297884238983223646[306] = 0; + out_1297884238983223646[307] = 0; + out_1297884238983223646[308] = 0; + out_1297884238983223646[309] = 0; + out_1297884238983223646[310] = 0; + out_1297884238983223646[311] = 0; + out_1297884238983223646[312] = 0; + out_1297884238983223646[313] = 0; + out_1297884238983223646[314] = 0; + out_1297884238983223646[315] = 0; + out_1297884238983223646[316] = 0; + out_1297884238983223646[317] = 0; + out_1297884238983223646[318] = 0; + out_1297884238983223646[319] = 0; + out_1297884238983223646[320] = 0; + out_1297884238983223646[321] = 0; + out_1297884238983223646[322] = 0; + out_1297884238983223646[323] = 1; } -void h_4(double *state, double *unused, double *out_2329964328807098264) { - out_2329964328807098264[0] = state[6] + state[9]; - out_2329964328807098264[1] = state[7] + state[10]; - out_2329964328807098264[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_8728651807226544018) { + out_8728651807226544018[0] = state[6] + state[9]; + out_8728651807226544018[1] = state[7] + state[10]; + out_8728651807226544018[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_3409566783491949590) { - out_3409566783491949590[0] = 0; - out_3409566783491949590[1] = 0; - out_3409566783491949590[2] = 0; - out_3409566783491949590[3] = 0; - out_3409566783491949590[4] = 0; - out_3409566783491949590[5] = 0; - out_3409566783491949590[6] = 1; - out_3409566783491949590[7] = 0; - out_3409566783491949590[8] = 0; - out_3409566783491949590[9] = 1; - out_3409566783491949590[10] = 0; - out_3409566783491949590[11] = 0; - out_3409566783491949590[12] = 0; - out_3409566783491949590[13] = 0; - out_3409566783491949590[14] = 0; - out_3409566783491949590[15] = 0; - out_3409566783491949590[16] = 0; - out_3409566783491949590[17] = 0; - out_3409566783491949590[18] = 0; - out_3409566783491949590[19] = 0; - out_3409566783491949590[20] = 0; - out_3409566783491949590[21] = 0; - out_3409566783491949590[22] = 0; - out_3409566783491949590[23] = 0; - out_3409566783491949590[24] = 0; - out_3409566783491949590[25] = 1; - out_3409566783491949590[26] = 0; - out_3409566783491949590[27] = 0; - out_3409566783491949590[28] = 1; - out_3409566783491949590[29] = 0; - out_3409566783491949590[30] = 0; - out_3409566783491949590[31] = 0; - out_3409566783491949590[32] = 0; - out_3409566783491949590[33] = 0; - out_3409566783491949590[34] = 0; - out_3409566783491949590[35] = 0; - out_3409566783491949590[36] = 0; - out_3409566783491949590[37] = 0; - out_3409566783491949590[38] = 0; - out_3409566783491949590[39] = 0; - out_3409566783491949590[40] = 0; - out_3409566783491949590[41] = 0; - out_3409566783491949590[42] = 0; - out_3409566783491949590[43] = 0; - out_3409566783491949590[44] = 1; - out_3409566783491949590[45] = 0; - out_3409566783491949590[46] = 0; - out_3409566783491949590[47] = 1; - out_3409566783491949590[48] = 0; - out_3409566783491949590[49] = 0; - out_3409566783491949590[50] = 0; - out_3409566783491949590[51] = 0; - out_3409566783491949590[52] = 0; - out_3409566783491949590[53] = 0; +void H_4(double *state, double *unused, double *out_5315696800103384324) { + out_5315696800103384324[0] = 0; + out_5315696800103384324[1] = 0; + out_5315696800103384324[2] = 0; + out_5315696800103384324[3] = 0; + out_5315696800103384324[4] = 0; + out_5315696800103384324[5] = 0; + out_5315696800103384324[6] = 1; + out_5315696800103384324[7] = 0; + out_5315696800103384324[8] = 0; + out_5315696800103384324[9] = 1; + out_5315696800103384324[10] = 0; + out_5315696800103384324[11] = 0; + out_5315696800103384324[12] = 0; + out_5315696800103384324[13] = 0; + out_5315696800103384324[14] = 0; + out_5315696800103384324[15] = 0; + out_5315696800103384324[16] = 0; + out_5315696800103384324[17] = 0; + out_5315696800103384324[18] = 0; + out_5315696800103384324[19] = 0; + out_5315696800103384324[20] = 0; + out_5315696800103384324[21] = 0; + out_5315696800103384324[22] = 0; + out_5315696800103384324[23] = 0; + out_5315696800103384324[24] = 0; + out_5315696800103384324[25] = 1; + out_5315696800103384324[26] = 0; + out_5315696800103384324[27] = 0; + out_5315696800103384324[28] = 1; + out_5315696800103384324[29] = 0; + out_5315696800103384324[30] = 0; + out_5315696800103384324[31] = 0; + out_5315696800103384324[32] = 0; + out_5315696800103384324[33] = 0; + out_5315696800103384324[34] = 0; + out_5315696800103384324[35] = 0; + out_5315696800103384324[36] = 0; + out_5315696800103384324[37] = 0; + out_5315696800103384324[38] = 0; + out_5315696800103384324[39] = 0; + out_5315696800103384324[40] = 0; + out_5315696800103384324[41] = 0; + out_5315696800103384324[42] = 0; + out_5315696800103384324[43] = 0; + out_5315696800103384324[44] = 1; + out_5315696800103384324[45] = 0; + out_5315696800103384324[46] = 0; + out_5315696800103384324[47] = 1; + out_5315696800103384324[48] = 0; + out_5315696800103384324[49] = 0; + out_5315696800103384324[50] = 0; + out_5315696800103384324[51] = 0; + out_5315696800103384324[52] = 0; + out_5315696800103384324[53] = 0; } -void h_10(double *state, double *unused, double *out_1764029327220956455) { - out_1764029327220956455[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_1764029327220956455[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_1764029327220956455[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_3679201747496490736) { + out_3679201747496490736[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_3679201747496490736[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_3679201747496490736[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_5084421046690710562) { - out_5084421046690710562[0] = 0; - out_5084421046690710562[1] = 9.8100000000000005*cos(state[1]); - out_5084421046690710562[2] = 0; - out_5084421046690710562[3] = 0; - out_5084421046690710562[4] = -state[8]; - out_5084421046690710562[5] = state[7]; - out_5084421046690710562[6] = 0; - out_5084421046690710562[7] = state[5]; - out_5084421046690710562[8] = -state[4]; - out_5084421046690710562[9] = 0; - out_5084421046690710562[10] = 0; - out_5084421046690710562[11] = 0; - out_5084421046690710562[12] = 1; - out_5084421046690710562[13] = 0; - out_5084421046690710562[14] = 0; - out_5084421046690710562[15] = 1; - out_5084421046690710562[16] = 0; - out_5084421046690710562[17] = 0; - out_5084421046690710562[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_5084421046690710562[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_5084421046690710562[20] = 0; - out_5084421046690710562[21] = state[8]; - out_5084421046690710562[22] = 0; - out_5084421046690710562[23] = -state[6]; - out_5084421046690710562[24] = -state[5]; - out_5084421046690710562[25] = 0; - out_5084421046690710562[26] = state[3]; - out_5084421046690710562[27] = 0; - out_5084421046690710562[28] = 0; - out_5084421046690710562[29] = 0; - out_5084421046690710562[30] = 0; - out_5084421046690710562[31] = 1; - out_5084421046690710562[32] = 0; - out_5084421046690710562[33] = 0; - out_5084421046690710562[34] = 1; - out_5084421046690710562[35] = 0; - out_5084421046690710562[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_5084421046690710562[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_5084421046690710562[38] = 0; - out_5084421046690710562[39] = -state[7]; - out_5084421046690710562[40] = state[6]; - out_5084421046690710562[41] = 0; - out_5084421046690710562[42] = state[4]; - out_5084421046690710562[43] = -state[3]; - out_5084421046690710562[44] = 0; - out_5084421046690710562[45] = 0; - out_5084421046690710562[46] = 0; - out_5084421046690710562[47] = 0; - out_5084421046690710562[48] = 0; - out_5084421046690710562[49] = 0; - out_5084421046690710562[50] = 1; - out_5084421046690710562[51] = 0; - out_5084421046690710562[52] = 0; - out_5084421046690710562[53] = 1; +void H_10(double *state, double *unused, double *out_2420796569664890088) { + out_2420796569664890088[0] = 0; + out_2420796569664890088[1] = 9.8100000000000005*cos(state[1]); + out_2420796569664890088[2] = 0; + out_2420796569664890088[3] = 0; + out_2420796569664890088[4] = -state[8]; + out_2420796569664890088[5] = state[7]; + out_2420796569664890088[6] = 0; + out_2420796569664890088[7] = state[5]; + out_2420796569664890088[8] = -state[4]; + out_2420796569664890088[9] = 0; + out_2420796569664890088[10] = 0; + out_2420796569664890088[11] = 0; + out_2420796569664890088[12] = 1; + out_2420796569664890088[13] = 0; + out_2420796569664890088[14] = 0; + out_2420796569664890088[15] = 1; + out_2420796569664890088[16] = 0; + out_2420796569664890088[17] = 0; + out_2420796569664890088[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_2420796569664890088[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_2420796569664890088[20] = 0; + out_2420796569664890088[21] = state[8]; + out_2420796569664890088[22] = 0; + out_2420796569664890088[23] = -state[6]; + out_2420796569664890088[24] = -state[5]; + out_2420796569664890088[25] = 0; + out_2420796569664890088[26] = state[3]; + out_2420796569664890088[27] = 0; + out_2420796569664890088[28] = 0; + out_2420796569664890088[29] = 0; + out_2420796569664890088[30] = 0; + out_2420796569664890088[31] = 1; + out_2420796569664890088[32] = 0; + out_2420796569664890088[33] = 0; + out_2420796569664890088[34] = 1; + out_2420796569664890088[35] = 0; + out_2420796569664890088[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_2420796569664890088[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_2420796569664890088[38] = 0; + out_2420796569664890088[39] = -state[7]; + out_2420796569664890088[40] = state[6]; + out_2420796569664890088[41] = 0; + out_2420796569664890088[42] = state[4]; + out_2420796569664890088[43] = -state[3]; + out_2420796569664890088[44] = 0; + out_2420796569664890088[45] = 0; + out_2420796569664890088[46] = 0; + out_2420796569664890088[47] = 0; + out_2420796569664890088[48] = 0; + out_2420796569664890088[49] = 0; + out_2420796569664890088[50] = 1; + out_2420796569664890088[51] = 0; + out_2420796569664890088[52] = 0; + out_2420796569664890088[53] = 1; } -void h_13(double *state, double *unused, double *out_7799867727264054991) { - out_7799867727264054991[0] = state[3]; - out_7799867727264054991[1] = state[4]; - out_7799867727264054991[2] = state[5]; +void h_13(double *state, double *unused, double *out_4772104686591378688) { + out_4772104686591378688[0] = state[3]; + out_4772104686591378688[1] = state[4]; + out_4772104686591378688[2] = state[5]; } -void H_13(double *state, double *unused, double *out_2844964863810105486) { - out_2844964863810105486[0] = 0; - out_2844964863810105486[1] = 0; - out_2844964863810105486[2] = 0; - out_2844964863810105486[3] = 1; - out_2844964863810105486[4] = 0; - out_2844964863810105486[5] = 0; - out_2844964863810105486[6] = 0; - out_2844964863810105486[7] = 0; - out_2844964863810105486[8] = 0; - out_2844964863810105486[9] = 0; - out_2844964863810105486[10] = 0; - out_2844964863810105486[11] = 0; - out_2844964863810105486[12] = 0; - out_2844964863810105486[13] = 0; - out_2844964863810105486[14] = 0; - out_2844964863810105486[15] = 0; - out_2844964863810105486[16] = 0; - out_2844964863810105486[17] = 0; - out_2844964863810105486[18] = 0; - out_2844964863810105486[19] = 0; - out_2844964863810105486[20] = 0; - out_2844964863810105486[21] = 0; - out_2844964863810105486[22] = 1; - out_2844964863810105486[23] = 0; - out_2844964863810105486[24] = 0; - out_2844964863810105486[25] = 0; - out_2844964863810105486[26] = 0; - out_2844964863810105486[27] = 0; - out_2844964863810105486[28] = 0; - out_2844964863810105486[29] = 0; - out_2844964863810105486[30] = 0; - out_2844964863810105486[31] = 0; - out_2844964863810105486[32] = 0; - out_2844964863810105486[33] = 0; - out_2844964863810105486[34] = 0; - out_2844964863810105486[35] = 0; - out_2844964863810105486[36] = 0; - out_2844964863810105486[37] = 0; - out_2844964863810105486[38] = 0; - out_2844964863810105486[39] = 0; - out_2844964863810105486[40] = 0; - out_2844964863810105486[41] = 1; - out_2844964863810105486[42] = 0; - out_2844964863810105486[43] = 0; - out_2844964863810105486[44] = 0; - out_2844964863810105486[45] = 0; - out_2844964863810105486[46] = 0; - out_2844964863810105486[47] = 0; - out_2844964863810105486[48] = 0; - out_2844964863810105486[49] = 0; - out_2844964863810105486[50] = 0; - out_2844964863810105486[51] = 0; - out_2844964863810105486[52] = 0; - out_2844964863810105486[53] = 0; +void H_13(double *state, double *unused, double *out_2103422974771051523) { + out_2103422974771051523[0] = 0; + out_2103422974771051523[1] = 0; + out_2103422974771051523[2] = 0; + out_2103422974771051523[3] = 1; + out_2103422974771051523[4] = 0; + out_2103422974771051523[5] = 0; + out_2103422974771051523[6] = 0; + out_2103422974771051523[7] = 0; + out_2103422974771051523[8] = 0; + out_2103422974771051523[9] = 0; + out_2103422974771051523[10] = 0; + out_2103422974771051523[11] = 0; + out_2103422974771051523[12] = 0; + out_2103422974771051523[13] = 0; + out_2103422974771051523[14] = 0; + out_2103422974771051523[15] = 0; + out_2103422974771051523[16] = 0; + out_2103422974771051523[17] = 0; + out_2103422974771051523[18] = 0; + out_2103422974771051523[19] = 0; + out_2103422974771051523[20] = 0; + out_2103422974771051523[21] = 0; + out_2103422974771051523[22] = 1; + out_2103422974771051523[23] = 0; + out_2103422974771051523[24] = 0; + out_2103422974771051523[25] = 0; + out_2103422974771051523[26] = 0; + out_2103422974771051523[27] = 0; + out_2103422974771051523[28] = 0; + out_2103422974771051523[29] = 0; + out_2103422974771051523[30] = 0; + out_2103422974771051523[31] = 0; + out_2103422974771051523[32] = 0; + out_2103422974771051523[33] = 0; + out_2103422974771051523[34] = 0; + out_2103422974771051523[35] = 0; + out_2103422974771051523[36] = 0; + out_2103422974771051523[37] = 0; + out_2103422974771051523[38] = 0; + out_2103422974771051523[39] = 0; + out_2103422974771051523[40] = 0; + out_2103422974771051523[41] = 1; + out_2103422974771051523[42] = 0; + out_2103422974771051523[43] = 0; + out_2103422974771051523[44] = 0; + out_2103422974771051523[45] = 0; + out_2103422974771051523[46] = 0; + out_2103422974771051523[47] = 0; + out_2103422974771051523[48] = 0; + out_2103422974771051523[49] = 0; + out_2103422974771051523[50] = 0; + out_2103422974771051523[51] = 0; + out_2103422974771051523[52] = 0; + out_2103422974771051523[53] = 0; } -void h_14(double *state, double *unused, double *out_3740143774591228770) { - out_3740143774591228770[0] = state[6]; - out_3740143774591228770[1] = state[7]; - out_3740143774591228770[2] = state[8]; +void h_14(double *state, double *unused, double *out_3663326241180753804) { + out_3663326241180753804[0] = state[6]; + out_3663326241180753804[1] = state[7]; + out_3663326241180753804[2] = state[8]; } -void H_14(double *state, double *unused, double *out_6492355215787321886) { - out_6492355215787321886[0] = 0; - out_6492355215787321886[1] = 0; - out_6492355215787321886[2] = 0; - out_6492355215787321886[3] = 0; - out_6492355215787321886[4] = 0; - out_6492355215787321886[5] = 0; - out_6492355215787321886[6] = 1; - out_6492355215787321886[7] = 0; - out_6492355215787321886[8] = 0; - out_6492355215787321886[9] = 0; - out_6492355215787321886[10] = 0; - out_6492355215787321886[11] = 0; - out_6492355215787321886[12] = 0; - out_6492355215787321886[13] = 0; - out_6492355215787321886[14] = 0; - out_6492355215787321886[15] = 0; - out_6492355215787321886[16] = 0; - out_6492355215787321886[17] = 0; - out_6492355215787321886[18] = 0; - out_6492355215787321886[19] = 0; - out_6492355215787321886[20] = 0; - out_6492355215787321886[21] = 0; - out_6492355215787321886[22] = 0; - out_6492355215787321886[23] = 0; - out_6492355215787321886[24] = 0; - out_6492355215787321886[25] = 1; - out_6492355215787321886[26] = 0; - out_6492355215787321886[27] = 0; - out_6492355215787321886[28] = 0; - out_6492355215787321886[29] = 0; - out_6492355215787321886[30] = 0; - out_6492355215787321886[31] = 0; - out_6492355215787321886[32] = 0; - out_6492355215787321886[33] = 0; - out_6492355215787321886[34] = 0; - out_6492355215787321886[35] = 0; - out_6492355215787321886[36] = 0; - out_6492355215787321886[37] = 0; - out_6492355215787321886[38] = 0; - out_6492355215787321886[39] = 0; - out_6492355215787321886[40] = 0; - out_6492355215787321886[41] = 0; - out_6492355215787321886[42] = 0; - out_6492355215787321886[43] = 0; - out_6492355215787321886[44] = 1; - out_6492355215787321886[45] = 0; - out_6492355215787321886[46] = 0; - out_6492355215787321886[47] = 0; - out_6492355215787321886[48] = 0; - out_6492355215787321886[49] = 0; - out_6492355215787321886[50] = 0; - out_6492355215787321886[51] = 0; - out_6492355215787321886[52] = 0; - out_6492355215787321886[53] = 0; +void H_14(double *state, double *unused, double *out_1352455943763899795) { + out_1352455943763899795[0] = 0; + out_1352455943763899795[1] = 0; + out_1352455943763899795[2] = 0; + out_1352455943763899795[3] = 0; + out_1352455943763899795[4] = 0; + out_1352455943763899795[5] = 0; + out_1352455943763899795[6] = 1; + out_1352455943763899795[7] = 0; + out_1352455943763899795[8] = 0; + out_1352455943763899795[9] = 0; + out_1352455943763899795[10] = 0; + out_1352455943763899795[11] = 0; + out_1352455943763899795[12] = 0; + out_1352455943763899795[13] = 0; + out_1352455943763899795[14] = 0; + out_1352455943763899795[15] = 0; + out_1352455943763899795[16] = 0; + out_1352455943763899795[17] = 0; + out_1352455943763899795[18] = 0; + out_1352455943763899795[19] = 0; + out_1352455943763899795[20] = 0; + out_1352455943763899795[21] = 0; + out_1352455943763899795[22] = 0; + out_1352455943763899795[23] = 0; + out_1352455943763899795[24] = 0; + out_1352455943763899795[25] = 1; + out_1352455943763899795[26] = 0; + out_1352455943763899795[27] = 0; + out_1352455943763899795[28] = 0; + out_1352455943763899795[29] = 0; + out_1352455943763899795[30] = 0; + out_1352455943763899795[31] = 0; + out_1352455943763899795[32] = 0; + out_1352455943763899795[33] = 0; + out_1352455943763899795[34] = 0; + out_1352455943763899795[35] = 0; + out_1352455943763899795[36] = 0; + out_1352455943763899795[37] = 0; + out_1352455943763899795[38] = 0; + out_1352455943763899795[39] = 0; + out_1352455943763899795[40] = 0; + out_1352455943763899795[41] = 0; + out_1352455943763899795[42] = 0; + out_1352455943763899795[43] = 0; + out_1352455943763899795[44] = 1; + out_1352455943763899795[45] = 0; + out_1352455943763899795[46] = 0; + out_1352455943763899795[47] = 0; + out_1352455943763899795[48] = 0; + out_1352455943763899795[49] = 0; + out_1352455943763899795[50] = 0; + out_1352455943763899795[51] = 0; + out_1352455943763899795[52] = 0; + out_1352455943763899795[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_7322678164781620542) { - err_fun(nom_x, delta_x, out_7322678164781620542); +void pose_err_fun(double *nom_x, double *delta_x, double *out_1527400177482295711) { + err_fun(nom_x, delta_x, out_1527400177482295711); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6351713798129038522) { - inv_err_fun(nom_x, true_x, out_6351713798129038522); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_7620805951199866590) { + inv_err_fun(nom_x, true_x, out_7620805951199866590); } -void pose_H_mod_fun(double *state, double *out_7181373993335857162) { - H_mod_fun(state, out_7181373993335857162); +void pose_H_mod_fun(double *state, double *out_2041474721312435071) { + H_mod_fun(state, out_2041474721312435071); } -void pose_f_fun(double *state, double dt, double *out_1544031639553813882) { - f_fun(state, dt, out_1544031639553813882); +void pose_f_fun(double *state, double dt, double *out_3319995548118437592) { + f_fun(state, dt, out_3319995548118437592); } -void pose_F_fun(double *state, double dt, double *out_4307246870374060335) { - F_fun(state, dt, out_4307246870374060335); +void pose_F_fun(double *state, double dt, double *out_1297884238983223646) { + F_fun(state, dt, out_1297884238983223646); } -void pose_h_4(double *state, double *unused, double *out_2329964328807098264) { - h_4(state, unused, out_2329964328807098264); +void pose_h_4(double *state, double *unused, double *out_8728651807226544018) { + h_4(state, unused, out_8728651807226544018); } -void pose_H_4(double *state, double *unused, double *out_3409566783491949590) { - H_4(state, unused, out_3409566783491949590); +void pose_H_4(double *state, double *unused, double *out_5315696800103384324) { + H_4(state, unused, out_5315696800103384324); } -void pose_h_10(double *state, double *unused, double *out_1764029327220956455) { - h_10(state, unused, out_1764029327220956455); +void pose_h_10(double *state, double *unused, double *out_3679201747496490736) { + h_10(state, unused, out_3679201747496490736); } -void pose_H_10(double *state, double *unused, double *out_5084421046690710562) { - H_10(state, unused, out_5084421046690710562); +void pose_H_10(double *state, double *unused, double *out_2420796569664890088) { + H_10(state, unused, out_2420796569664890088); } -void pose_h_13(double *state, double *unused, double *out_7799867727264054991) { - h_13(state, unused, out_7799867727264054991); +void pose_h_13(double *state, double *unused, double *out_4772104686591378688) { + h_13(state, unused, out_4772104686591378688); } -void pose_H_13(double *state, double *unused, double *out_2844964863810105486) { - H_13(state, unused, out_2844964863810105486); +void pose_H_13(double *state, double *unused, double *out_2103422974771051523) { + H_13(state, unused, out_2103422974771051523); } -void pose_h_14(double *state, double *unused, double *out_3740143774591228770) { - h_14(state, unused, out_3740143774591228770); +void pose_h_14(double *state, double *unused, double *out_3663326241180753804) { + h_14(state, unused, out_3663326241180753804); } -void pose_H_14(double *state, double *unused, double *out_6492355215787321886) { - H_14(state, unused, out_6492355215787321886); +void pose_H_14(double *state, double *unused, double *out_1352455943763899795) { + H_14(state, unused, out_1352455943763899795); } 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 19d91e0ec..f862c8930 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_7322678164781620542); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6351713798129038522); -void pose_H_mod_fun(double *state, double *out_7181373993335857162); -void pose_f_fun(double *state, double dt, double *out_1544031639553813882); -void pose_F_fun(double *state, double dt, double *out_4307246870374060335); -void pose_h_4(double *state, double *unused, double *out_2329964328807098264); -void pose_H_4(double *state, double *unused, double *out_3409566783491949590); -void pose_h_10(double *state, double *unused, double *out_1764029327220956455); -void pose_H_10(double *state, double *unused, double *out_5084421046690710562); -void pose_h_13(double *state, double *unused, double *out_7799867727264054991); -void pose_H_13(double *state, double *unused, double *out_2844964863810105486); -void pose_h_14(double *state, double *unused, double *out_3740143774591228770); -void pose_H_14(double *state, double *unused, double *out_6492355215787321886); +void pose_err_fun(double *nom_x, double *delta_x, double *out_1527400177482295711); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_7620805951199866590); +void pose_H_mod_fun(double *state, double *out_2041474721312435071); +void pose_f_fun(double *state, double dt, double *out_3319995548118437592); +void pose_F_fun(double *state, double dt, double *out_1297884238983223646); +void pose_h_4(double *state, double *unused, double *out_8728651807226544018); +void pose_H_4(double *state, double *unused, double *out_5315696800103384324); +void pose_h_10(double *state, double *unused, double *out_3679201747496490736); +void pose_H_10(double *state, double *unused, double *out_2420796569664890088); +void pose_h_13(double *state, double *unused, double *out_4772104686591378688); +void pose_H_13(double *state, double *unused, double *out_2103422974771051523); +void pose_h_14(double *state, double *unused, double *out_3663326241180753804); +void pose_H_14(double *state, double *unused, double *out_1352455943763899795); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/pandad/pandad b/selfdrive/pandad/pandad index 02d9c7678..eec7292c2 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index eba89f3ca..7ea12608b 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/system/camerad/camerad b/system/camerad/camerad index f9d6eff2b..a4bd1efa7 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index 1882dfdde..941c4e0b9 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index 9b2618ffb..879e6cf18 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index 73a23cd97..36d84b9d2 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ