diff --git a/cereal/libcereal.a b/cereal/libcereal.a index 7ac66de66d..4726683483 100644 Binary files a/cereal/libcereal.a and b/cereal/libcereal.a differ diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index 3df79deca5..6840096205 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 b70c48b8d5..9d8e1041d0 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 58c8e9e478..3b5e413d15 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 aad0b90d29..e844fa727d 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 817c25fe5e..228f886127 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 d63331c9ec..752605cb5d 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 d63331c9ec..752605cb5d 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 a83e9cd1ad..4eff454564 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 a83e9cd1ad..4eff454564 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 a83e9cd1ad..4eff454564 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 a83e9cd1ad..4eff454564 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 a83e9cd1ad..4eff454564 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 a83e9cd1ad..4eff454564 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 d63331c9ec..752605cb5d 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 d63331c9ec..752605cb5d 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 5b6cf54a45..753f7ae5b6 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 d63331c9ec..752605cb5d 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 d63331c9ec..752605cb5d 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 a97ef22dc2..4d7d94c6ff 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-af2e153e-DEBUG"; +const uint8_t gitversion[19] = "DEV-dc62d0e2-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index db84ea214c..0ef65a07c9 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 fb7327595b..ce7fc1300f 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 dca4babeb4..825030e463 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 b297b28dc1..0acaffff0c 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 5df1969f73..a76962f2a9 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 1e66c09286..d00ad22736 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 c157a3b683..cb56910e00 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 6a8a3d9268..da56403c89 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 5acca32e85..605dd28e5c 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 df3d7bb520..b109fdaf39 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 713d268b69..ccff14c737 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 720cc1fbea..11111c4e99 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 6d0e3b3353..e4d388afb4 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 2b1f32952c..77ae7d0147 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 ba92c3f4f3..604c687c35 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 c370936ed0..40607029ff 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 8481df1ceb..9bc570f7ee 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 280cd1ccc5..8e75bab23d 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 cc0a986a7c..88818f216f 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 1e8299a243..685c1b852f 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 8165e3b2cc..7263f94bee 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 8d6a2196cc..655322be7d 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 c3802709c9..c00af76f45 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 81dc419419..dab8d2e09b 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 925c4574dd..7ea677de2c 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 fce4a321ef..c7defb6ba6 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 c332c70c17..d7ea4db571 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 e693f3c580..5769069695 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 d870838638..8033ebb6b8 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 ef027ffbc7..42dca38e41 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 560ac71086..13d87a0124 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 da0880de40..9f3b511a8b 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 cdf0cf0f83..6c0a72ed8a 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 65d7210b87..b2d946fe4a 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 6a1a94a26a..b59b448424 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 079a0314d4..e7fc392e0c 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 d94626cccf..78821c06a7 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 2146657a89..152146d6c2 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 e3efd7643c..ac3a53093e 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 593704b1ad..7ed87caf54 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 ca561fc6ce..48a71ea876 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 50975c0a32..590b32a0d8 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 2c9d068d5d..0cfb6a5581 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 0f674d8487..ef6f5cd558 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 7250237414..6e9aab9fb9 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 4d3f94dd31..fd87572610 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 68aab2680a..4cd83f6564 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 8d9b4ae354..29588baded 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 386c06f5a0..54ab557593 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 0013b5180d..8867fcd77d 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 d68d849d91..869971c911 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 4d9f2d5a6f..8778e56674 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 d65108fb8c..4b8d2bb321 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-af2e153e-DEBUG \ No newline at end of file +DEV-dc62d0e2-DEBUG \ No newline at end of file diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index f8a737bbcc..1006f0fd16 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -4,7 +4,6 @@ import pytest from openpilot.common.constants import CV from openpilot.common.realtime import DT_MDL -from openpilot.starpilot.common.starpilot_variables import PLANNER_TIME from openpilot.starpilot.controls.lib.curve_speed_controller import CSC_MAX_DECEL_RATE, CurveSpeedController, MIN_TRAINING_TIME from openpilot.starpilot.controls.lib.starpilot_vcruise import ( FORCE_STOP_CAP_SLACK_M, @@ -195,7 +194,7 @@ def test_curve_speed_controller_releases_immediately_when_disabled(): def test_curve_speed_controller_does_not_compete_with_force_stop(): - planner, vcruise = make_vcruise(red_light=True, road_curvature=0.02) + planner, vcruise = make_vcruise(red_light=True, road_curvature=0.001) sm = make_sm(standstill=False) toggles = make_toggles() toggles.curve_speed_controller = True @@ -209,6 +208,21 @@ def test_curve_speed_controller_does_not_compete_with_force_stop(): assert not vcruise.csc.target_set +def test_curve_speed_controller_learns_through_a_signaled_curve(): + planner, vcruise = make_vcruise(road_curvature=0.02) + sm = make_sm(standstill=False) + sm["carControl"].longActive = False + sm["carState"].leftBlinker = True + planner.driving_in_curve = True + planner.lateral_acceleration = 2.4 + vcruise.csc.training_timer = MIN_TRAINING_TIME + + vcruise.csc.log_data(20.0, sm) + + assert vcruise.csc.enable_training + assert vcruise.csc.curvature_data["0.02"]["count"] == 1 + + def test_curve_speed_controller_can_be_limited_to_driving_without_a_lead(): planner, vcruise = make_vcruise() diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index f1c3ceacb8..23517eaf33 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_6663383586198040946) { - out_6663383586198040946[0] = delta_x[0] + nom_x[0]; - out_6663383586198040946[1] = delta_x[1] + nom_x[1]; - out_6663383586198040946[2] = delta_x[2] + nom_x[2]; - out_6663383586198040946[3] = delta_x[3] + nom_x[3]; - out_6663383586198040946[4] = delta_x[4] + nom_x[4]; - out_6663383586198040946[5] = delta_x[5] + nom_x[5]; - out_6663383586198040946[6] = delta_x[6] + nom_x[6]; - out_6663383586198040946[7] = delta_x[7] + nom_x[7]; - out_6663383586198040946[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_8986378044421410730) { + out_8986378044421410730[0] = delta_x[0] + nom_x[0]; + out_8986378044421410730[1] = delta_x[1] + nom_x[1]; + out_8986378044421410730[2] = delta_x[2] + nom_x[2]; + out_8986378044421410730[3] = delta_x[3] + nom_x[3]; + out_8986378044421410730[4] = delta_x[4] + nom_x[4]; + out_8986378044421410730[5] = delta_x[5] + nom_x[5]; + out_8986378044421410730[6] = delta_x[6] + nom_x[6]; + out_8986378044421410730[7] = delta_x[7] + nom_x[7]; + out_8986378044421410730[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_8588337980518277825) { - out_8588337980518277825[0] = -nom_x[0] + true_x[0]; - out_8588337980518277825[1] = -nom_x[1] + true_x[1]; - out_8588337980518277825[2] = -nom_x[2] + true_x[2]; - out_8588337980518277825[3] = -nom_x[3] + true_x[3]; - out_8588337980518277825[4] = -nom_x[4] + true_x[4]; - out_8588337980518277825[5] = -nom_x[5] + true_x[5]; - out_8588337980518277825[6] = -nom_x[6] + true_x[6]; - out_8588337980518277825[7] = -nom_x[7] + true_x[7]; - out_8588337980518277825[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_1940910250995899848) { + out_1940910250995899848[0] = -nom_x[0] + true_x[0]; + out_1940910250995899848[1] = -nom_x[1] + true_x[1]; + out_1940910250995899848[2] = -nom_x[2] + true_x[2]; + out_1940910250995899848[3] = -nom_x[3] + true_x[3]; + out_1940910250995899848[4] = -nom_x[4] + true_x[4]; + out_1940910250995899848[5] = -nom_x[5] + true_x[5]; + out_1940910250995899848[6] = -nom_x[6] + true_x[6]; + out_1940910250995899848[7] = -nom_x[7] + true_x[7]; + out_1940910250995899848[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_6555656847717771558) { - out_6555656847717771558[0] = 1.0; - out_6555656847717771558[1] = 0.0; - out_6555656847717771558[2] = 0.0; - out_6555656847717771558[3] = 0.0; - out_6555656847717771558[4] = 0.0; - out_6555656847717771558[5] = 0.0; - out_6555656847717771558[6] = 0.0; - out_6555656847717771558[7] = 0.0; - out_6555656847717771558[8] = 0.0; - out_6555656847717771558[9] = 0.0; - out_6555656847717771558[10] = 1.0; - out_6555656847717771558[11] = 0.0; - out_6555656847717771558[12] = 0.0; - out_6555656847717771558[13] = 0.0; - out_6555656847717771558[14] = 0.0; - out_6555656847717771558[15] = 0.0; - out_6555656847717771558[16] = 0.0; - out_6555656847717771558[17] = 0.0; - out_6555656847717771558[18] = 0.0; - out_6555656847717771558[19] = 0.0; - out_6555656847717771558[20] = 1.0; - out_6555656847717771558[21] = 0.0; - out_6555656847717771558[22] = 0.0; - out_6555656847717771558[23] = 0.0; - out_6555656847717771558[24] = 0.0; - out_6555656847717771558[25] = 0.0; - out_6555656847717771558[26] = 0.0; - out_6555656847717771558[27] = 0.0; - out_6555656847717771558[28] = 0.0; - out_6555656847717771558[29] = 0.0; - out_6555656847717771558[30] = 1.0; - out_6555656847717771558[31] = 0.0; - out_6555656847717771558[32] = 0.0; - out_6555656847717771558[33] = 0.0; - out_6555656847717771558[34] = 0.0; - out_6555656847717771558[35] = 0.0; - out_6555656847717771558[36] = 0.0; - out_6555656847717771558[37] = 0.0; - out_6555656847717771558[38] = 0.0; - out_6555656847717771558[39] = 0.0; - out_6555656847717771558[40] = 1.0; - out_6555656847717771558[41] = 0.0; - out_6555656847717771558[42] = 0.0; - out_6555656847717771558[43] = 0.0; - out_6555656847717771558[44] = 0.0; - out_6555656847717771558[45] = 0.0; - out_6555656847717771558[46] = 0.0; - out_6555656847717771558[47] = 0.0; - out_6555656847717771558[48] = 0.0; - out_6555656847717771558[49] = 0.0; - out_6555656847717771558[50] = 1.0; - out_6555656847717771558[51] = 0.0; - out_6555656847717771558[52] = 0.0; - out_6555656847717771558[53] = 0.0; - out_6555656847717771558[54] = 0.0; - out_6555656847717771558[55] = 0.0; - out_6555656847717771558[56] = 0.0; - out_6555656847717771558[57] = 0.0; - out_6555656847717771558[58] = 0.0; - out_6555656847717771558[59] = 0.0; - out_6555656847717771558[60] = 1.0; - out_6555656847717771558[61] = 0.0; - out_6555656847717771558[62] = 0.0; - out_6555656847717771558[63] = 0.0; - out_6555656847717771558[64] = 0.0; - out_6555656847717771558[65] = 0.0; - out_6555656847717771558[66] = 0.0; - out_6555656847717771558[67] = 0.0; - out_6555656847717771558[68] = 0.0; - out_6555656847717771558[69] = 0.0; - out_6555656847717771558[70] = 1.0; - out_6555656847717771558[71] = 0.0; - out_6555656847717771558[72] = 0.0; - out_6555656847717771558[73] = 0.0; - out_6555656847717771558[74] = 0.0; - out_6555656847717771558[75] = 0.0; - out_6555656847717771558[76] = 0.0; - out_6555656847717771558[77] = 0.0; - out_6555656847717771558[78] = 0.0; - out_6555656847717771558[79] = 0.0; - out_6555656847717771558[80] = 1.0; +void H_mod_fun(double *state, double *out_1327881932680008443) { + out_1327881932680008443[0] = 1.0; + out_1327881932680008443[1] = 0.0; + out_1327881932680008443[2] = 0.0; + out_1327881932680008443[3] = 0.0; + out_1327881932680008443[4] = 0.0; + out_1327881932680008443[5] = 0.0; + out_1327881932680008443[6] = 0.0; + out_1327881932680008443[7] = 0.0; + out_1327881932680008443[8] = 0.0; + out_1327881932680008443[9] = 0.0; + out_1327881932680008443[10] = 1.0; + out_1327881932680008443[11] = 0.0; + out_1327881932680008443[12] = 0.0; + out_1327881932680008443[13] = 0.0; + out_1327881932680008443[14] = 0.0; + out_1327881932680008443[15] = 0.0; + out_1327881932680008443[16] = 0.0; + out_1327881932680008443[17] = 0.0; + out_1327881932680008443[18] = 0.0; + out_1327881932680008443[19] = 0.0; + out_1327881932680008443[20] = 1.0; + out_1327881932680008443[21] = 0.0; + out_1327881932680008443[22] = 0.0; + out_1327881932680008443[23] = 0.0; + out_1327881932680008443[24] = 0.0; + out_1327881932680008443[25] = 0.0; + out_1327881932680008443[26] = 0.0; + out_1327881932680008443[27] = 0.0; + out_1327881932680008443[28] = 0.0; + out_1327881932680008443[29] = 0.0; + out_1327881932680008443[30] = 1.0; + out_1327881932680008443[31] = 0.0; + out_1327881932680008443[32] = 0.0; + out_1327881932680008443[33] = 0.0; + out_1327881932680008443[34] = 0.0; + out_1327881932680008443[35] = 0.0; + out_1327881932680008443[36] = 0.0; + out_1327881932680008443[37] = 0.0; + out_1327881932680008443[38] = 0.0; + out_1327881932680008443[39] = 0.0; + out_1327881932680008443[40] = 1.0; + out_1327881932680008443[41] = 0.0; + out_1327881932680008443[42] = 0.0; + out_1327881932680008443[43] = 0.0; + out_1327881932680008443[44] = 0.0; + out_1327881932680008443[45] = 0.0; + out_1327881932680008443[46] = 0.0; + out_1327881932680008443[47] = 0.0; + out_1327881932680008443[48] = 0.0; + out_1327881932680008443[49] = 0.0; + out_1327881932680008443[50] = 1.0; + out_1327881932680008443[51] = 0.0; + out_1327881932680008443[52] = 0.0; + out_1327881932680008443[53] = 0.0; + out_1327881932680008443[54] = 0.0; + out_1327881932680008443[55] = 0.0; + out_1327881932680008443[56] = 0.0; + out_1327881932680008443[57] = 0.0; + out_1327881932680008443[58] = 0.0; + out_1327881932680008443[59] = 0.0; + out_1327881932680008443[60] = 1.0; + out_1327881932680008443[61] = 0.0; + out_1327881932680008443[62] = 0.0; + out_1327881932680008443[63] = 0.0; + out_1327881932680008443[64] = 0.0; + out_1327881932680008443[65] = 0.0; + out_1327881932680008443[66] = 0.0; + out_1327881932680008443[67] = 0.0; + out_1327881932680008443[68] = 0.0; + out_1327881932680008443[69] = 0.0; + out_1327881932680008443[70] = 1.0; + out_1327881932680008443[71] = 0.0; + out_1327881932680008443[72] = 0.0; + out_1327881932680008443[73] = 0.0; + out_1327881932680008443[74] = 0.0; + out_1327881932680008443[75] = 0.0; + out_1327881932680008443[76] = 0.0; + out_1327881932680008443[77] = 0.0; + out_1327881932680008443[78] = 0.0; + out_1327881932680008443[79] = 0.0; + out_1327881932680008443[80] = 1.0; } -void f_fun(double *state, double dt, double *out_7137582187284124708) { - out_7137582187284124708[0] = state[0]; - out_7137582187284124708[1] = state[1]; - out_7137582187284124708[2] = state[2]; - out_7137582187284124708[3] = state[3]; - out_7137582187284124708[4] = state[4]; - out_7137582187284124708[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_7137582187284124708[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_7137582187284124708[7] = state[7]; - out_7137582187284124708[8] = state[8]; +void f_fun(double *state, double dt, double *out_7832214154397608531) { + out_7832214154397608531[0] = state[0]; + out_7832214154397608531[1] = state[1]; + out_7832214154397608531[2] = state[2]; + out_7832214154397608531[3] = state[3]; + out_7832214154397608531[4] = state[4]; + out_7832214154397608531[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_7832214154397608531[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_7832214154397608531[7] = state[7]; + out_7832214154397608531[8] = state[8]; } -void F_fun(double *state, double dt, double *out_2957883628728215032) { - out_2957883628728215032[0] = 1; - out_2957883628728215032[1] = 0; - out_2957883628728215032[2] = 0; - out_2957883628728215032[3] = 0; - out_2957883628728215032[4] = 0; - out_2957883628728215032[5] = 0; - out_2957883628728215032[6] = 0; - out_2957883628728215032[7] = 0; - out_2957883628728215032[8] = 0; - out_2957883628728215032[9] = 0; - out_2957883628728215032[10] = 1; - out_2957883628728215032[11] = 0; - out_2957883628728215032[12] = 0; - out_2957883628728215032[13] = 0; - out_2957883628728215032[14] = 0; - out_2957883628728215032[15] = 0; - out_2957883628728215032[16] = 0; - out_2957883628728215032[17] = 0; - out_2957883628728215032[18] = 0; - out_2957883628728215032[19] = 0; - out_2957883628728215032[20] = 1; - out_2957883628728215032[21] = 0; - out_2957883628728215032[22] = 0; - out_2957883628728215032[23] = 0; - out_2957883628728215032[24] = 0; - out_2957883628728215032[25] = 0; - out_2957883628728215032[26] = 0; - out_2957883628728215032[27] = 0; - out_2957883628728215032[28] = 0; - out_2957883628728215032[29] = 0; - out_2957883628728215032[30] = 1; - out_2957883628728215032[31] = 0; - out_2957883628728215032[32] = 0; - out_2957883628728215032[33] = 0; - out_2957883628728215032[34] = 0; - out_2957883628728215032[35] = 0; - out_2957883628728215032[36] = 0; - out_2957883628728215032[37] = 0; - out_2957883628728215032[38] = 0; - out_2957883628728215032[39] = 0; - out_2957883628728215032[40] = 1; - out_2957883628728215032[41] = 0; - out_2957883628728215032[42] = 0; - out_2957883628728215032[43] = 0; - out_2957883628728215032[44] = 0; - out_2957883628728215032[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_2957883628728215032[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_2957883628728215032[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_2957883628728215032[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_2957883628728215032[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_2957883628728215032[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_2957883628728215032[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_2957883628728215032[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_2957883628728215032[53] = -9.8100000000000005*dt; - out_2957883628728215032[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_2957883628728215032[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_2957883628728215032[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_2957883628728215032[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_2957883628728215032[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_2957883628728215032[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_2957883628728215032[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_2957883628728215032[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_2957883628728215032[62] = 0; - out_2957883628728215032[63] = 0; - out_2957883628728215032[64] = 0; - out_2957883628728215032[65] = 0; - out_2957883628728215032[66] = 0; - out_2957883628728215032[67] = 0; - out_2957883628728215032[68] = 0; - out_2957883628728215032[69] = 0; - out_2957883628728215032[70] = 1; - out_2957883628728215032[71] = 0; - out_2957883628728215032[72] = 0; - out_2957883628728215032[73] = 0; - out_2957883628728215032[74] = 0; - out_2957883628728215032[75] = 0; - out_2957883628728215032[76] = 0; - out_2957883628728215032[77] = 0; - out_2957883628728215032[78] = 0; - out_2957883628728215032[79] = 0; - out_2957883628728215032[80] = 1; +void F_fun(double *state, double dt, double *out_4114606570946988857) { + out_4114606570946988857[0] = 1; + out_4114606570946988857[1] = 0; + out_4114606570946988857[2] = 0; + out_4114606570946988857[3] = 0; + out_4114606570946988857[4] = 0; + out_4114606570946988857[5] = 0; + out_4114606570946988857[6] = 0; + out_4114606570946988857[7] = 0; + out_4114606570946988857[8] = 0; + out_4114606570946988857[9] = 0; + out_4114606570946988857[10] = 1; + out_4114606570946988857[11] = 0; + out_4114606570946988857[12] = 0; + out_4114606570946988857[13] = 0; + out_4114606570946988857[14] = 0; + out_4114606570946988857[15] = 0; + out_4114606570946988857[16] = 0; + out_4114606570946988857[17] = 0; + out_4114606570946988857[18] = 0; + out_4114606570946988857[19] = 0; + out_4114606570946988857[20] = 1; + out_4114606570946988857[21] = 0; + out_4114606570946988857[22] = 0; + out_4114606570946988857[23] = 0; + out_4114606570946988857[24] = 0; + out_4114606570946988857[25] = 0; + out_4114606570946988857[26] = 0; + out_4114606570946988857[27] = 0; + out_4114606570946988857[28] = 0; + out_4114606570946988857[29] = 0; + out_4114606570946988857[30] = 1; + out_4114606570946988857[31] = 0; + out_4114606570946988857[32] = 0; + out_4114606570946988857[33] = 0; + out_4114606570946988857[34] = 0; + out_4114606570946988857[35] = 0; + out_4114606570946988857[36] = 0; + out_4114606570946988857[37] = 0; + out_4114606570946988857[38] = 0; + out_4114606570946988857[39] = 0; + out_4114606570946988857[40] = 1; + out_4114606570946988857[41] = 0; + out_4114606570946988857[42] = 0; + out_4114606570946988857[43] = 0; + out_4114606570946988857[44] = 0; + out_4114606570946988857[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_4114606570946988857[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_4114606570946988857[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_4114606570946988857[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_4114606570946988857[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_4114606570946988857[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_4114606570946988857[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_4114606570946988857[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_4114606570946988857[53] = -9.8100000000000005*dt; + out_4114606570946988857[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_4114606570946988857[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_4114606570946988857[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_4114606570946988857[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_4114606570946988857[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_4114606570946988857[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_4114606570946988857[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_4114606570946988857[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_4114606570946988857[62] = 0; + out_4114606570946988857[63] = 0; + out_4114606570946988857[64] = 0; + out_4114606570946988857[65] = 0; + out_4114606570946988857[66] = 0; + out_4114606570946988857[67] = 0; + out_4114606570946988857[68] = 0; + out_4114606570946988857[69] = 0; + out_4114606570946988857[70] = 1; + out_4114606570946988857[71] = 0; + out_4114606570946988857[72] = 0; + out_4114606570946988857[73] = 0; + out_4114606570946988857[74] = 0; + out_4114606570946988857[75] = 0; + out_4114606570946988857[76] = 0; + out_4114606570946988857[77] = 0; + out_4114606570946988857[78] = 0; + out_4114606570946988857[79] = 0; + out_4114606570946988857[80] = 1; } -void h_25(double *state, double *unused, double *out_753214482939102632) { - out_753214482939102632[0] = state[6]; +void h_25(double *state, double *unused, double *out_8263447783360605785) { + out_8263447783360605785[0] = state[6]; } -void H_25(double *state, double *unused, double *out_58633887827741607) { - out_58633887827741607[0] = 0; - out_58633887827741607[1] = 0; - out_58633887827741607[2] = 0; - out_58633887827741607[3] = 0; - out_58633887827741607[4] = 0; - out_58633887827741607[5] = 0; - out_58633887827741607[6] = 1; - out_58633887827741607[7] = 0; - out_58633887827741607[8] = 0; +void H_25(double *state, double *unused, double *out_5780776564353391637) { + out_5780776564353391637[0] = 0; + out_5780776564353391637[1] = 0; + out_5780776564353391637[2] = 0; + out_5780776564353391637[3] = 0; + out_5780776564353391637[4] = 0; + out_5780776564353391637[5] = 0; + out_5780776564353391637[6] = 1; + out_5780776564353391637[7] = 0; + out_5780776564353391637[8] = 0; } -void h_24(double *state, double *unused, double *out_163608178505822735) { - out_163608178505822735[0] = state[4]; - out_163608178505822735[1] = state[5]; +void h_24(double *state, double *unused, double *out_4755010794979530940) { + out_4755010794979530940[0] = state[4]; + out_4755010794979530940[1] = state[5]; } -void H_24(double *state, double *unused, double *out_6142398792023419819) { - out_6142398792023419819[0] = 0; - out_6142398792023419819[1] = 0; - out_6142398792023419819[2] = 0; - out_6142398792023419819[3] = 0; - out_6142398792023419819[4] = 1; - out_6142398792023419819[5] = 0; - out_6142398792023419819[6] = 0; - out_6142398792023419819[7] = 0; - out_6142398792023419819[8] = 0; - out_6142398792023419819[9] = 0; - out_6142398792023419819[10] = 0; - out_6142398792023419819[11] = 0; - out_6142398792023419819[12] = 0; - out_6142398792023419819[13] = 0; - out_6142398792023419819[14] = 1; - out_6142398792023419819[15] = 0; - out_6142398792023419819[16] = 0; - out_6142398792023419819[17] = 0; +void H_24(double *state, double *unused, double *out_9108041910553836843) { + out_9108041910553836843[0] = 0; + out_9108041910553836843[1] = 0; + out_9108041910553836843[2] = 0; + out_9108041910553836843[3] = 0; + out_9108041910553836843[4] = 1; + out_9108041910553836843[5] = 0; + out_9108041910553836843[6] = 0; + out_9108041910553836843[7] = 0; + out_9108041910553836843[8] = 0; + out_9108041910553836843[9] = 0; + out_9108041910553836843[10] = 0; + out_9108041910553836843[11] = 0; + out_9108041910553836843[12] = 0; + out_9108041910553836843[13] = 0; + out_9108041910553836843[14] = 1; + out_9108041910553836843[15] = 0; + out_9108041910553836843[16] = 0; + out_9108041910553836843[17] = 0; } -void h_30(double *state, double *unused, double *out_596027814587973487) { - out_596027814587973487[0] = state[4]; +void h_30(double *state, double *unused, double *out_6812816390731722618) { + out_6812816390731722618[0] = state[4]; } -void H_30(double *state, double *unused, double *out_70705059315498463) { - out_70705059315498463[0] = 0; - out_70705059315498463[1] = 0; - out_70705059315498463[2] = 0; - out_70705059315498463[3] = 0; - out_70705059315498463[4] = 1; - out_70705059315498463[5] = 0; - out_70705059315498463[6] = 0; - out_70705059315498463[7] = 0; - out_70705059315498463[8] = 0; +void H_30(double *state, double *unused, double *out_5749277167864543224) { + out_5749277167864543224[0] = 0; + out_5749277167864543224[1] = 0; + out_5749277167864543224[2] = 0; + out_5749277167864543224[3] = 0; + out_5749277167864543224[4] = 1; + out_5749277167864543224[5] = 0; + out_5749277167864543224[6] = 0; + out_5749277167864543224[7] = 0; + out_5749277167864543224[8] = 0; } -void h_26(double *state, double *unused, double *out_6995069332001822633) { - out_6995069332001822633[0] = state[7]; +void h_26(double *state, double *unused, double *out_4631164979184763219) { + out_4631164979184763219[0] = state[7]; } -void H_26(double *state, double *unused, double *out_3682869431046314617) { - out_3682869431046314617[0] = 0; - out_3682869431046314617[1] = 0; - out_3682869431046314617[2] = 0; - out_3682869431046314617[3] = 0; - out_3682869431046314617[4] = 0; - out_3682869431046314617[5] = 0; - out_3682869431046314617[6] = 0; - out_3682869431046314617[7] = 1; - out_3682869431046314617[8] = 0; +void H_26(double *state, double *unused, double *out_2039273245479335413) { + out_2039273245479335413[0] = 0; + out_2039273245479335413[1] = 0; + out_2039273245479335413[2] = 0; + out_2039273245479335413[3] = 0; + out_2039273245479335413[4] = 0; + out_2039273245479335413[5] = 0; + out_2039273245479335413[6] = 0; + out_2039273245479335413[7] = 1; + out_2039273245479335413[8] = 0; } -void h_27(double *state, double *unused, double *out_8228754689067738133) { - out_8228754689067738133[0] = state[3]; +void h_27(double *state, double *unused, double *out_6907448477601489242) { + out_6907448477601489242[0] = state[3]; } -void H_27(double *state, double *unused, double *out_2245468371115923374) { - out_2245468371115923374[0] = 0; - out_2245468371115923374[1] = 0; - out_2245468371115923374[2] = 0; - out_2245468371115923374[3] = 1; - out_2245468371115923374[4] = 0; - out_2245468371115923374[5] = 0; - out_2245468371115923374[6] = 0; - out_2245468371115923374[7] = 0; - out_2245468371115923374[8] = 0; +void H_27(double *state, double *unused, double *out_7924040479664968135) { + out_7924040479664968135[0] = 0; + out_7924040479664968135[1] = 0; + out_7924040479664968135[2] = 0; + out_7924040479664968135[3] = 1; + out_7924040479664968135[4] = 0; + out_7924040479664968135[5] = 0; + out_7924040479664968135[6] = 0; + out_7924040479664968135[7] = 0; + out_7924040479664968135[8] = 0; } -void h_29(double *state, double *unused, double *out_347482038313688948) { - out_347482038313688948[0] = state[1]; +void h_29(double *state, double *unused, double *out_6632254415316983353) { + out_6632254415316983353[0] = state[1]; } -void H_29(double *state, double *unused, double *out_3958831097985474407) { - out_3958831097985474407[0] = 0; - out_3958831097985474407[1] = 1; - out_3958831097985474407[2] = 0; - out_3958831097985474407[3] = 0; - out_3958831097985474407[4] = 0; - out_3958831097985474407[5] = 0; - out_3958831097985474407[6] = 0; - out_3958831097985474407[7] = 0; - out_3958831097985474407[8] = 0; +void H_29(double *state, double *unused, double *out_5239045823550151040) { + out_5239045823550151040[0] = 0; + out_5239045823550151040[1] = 1; + out_5239045823550151040[2] = 0; + out_5239045823550151040[3] = 0; + out_5239045823550151040[4] = 0; + out_5239045823550151040[5] = 0; + out_5239045823550151040[6] = 0; + out_5239045823550151040[7] = 0; + out_5239045823550151040[8] = 0; } -void h_28(double *state, double *unused, double *out_7625438804391260616) { - out_7625438804391260616[0] = state[0]; +void h_28(double *state, double *unused, double *out_4283151879014610665) { + out_4283151879014610665[0] = state[0]; } -void H_28(double *state, double *unused, double *out_9041230115055004981) { - out_9041230115055004981[0] = 1; - out_9041230115055004981[1] = 0; - out_9041230115055004981[2] = 0; - out_9041230115055004981[3] = 0; - out_9041230115055004981[4] = 0; - out_9041230115055004981[5] = 0; - out_9041230115055004981[6] = 0; - out_9041230115055004981[7] = 0; - out_9041230115055004981[8] = 0; +void H_28(double *state, double *unused, double *out_8125299233089870002) { + out_8125299233089870002[0] = 1; + out_8125299233089870002[1] = 0; + out_8125299233089870002[2] = 0; + out_8125299233089870002[3] = 0; + out_8125299233089870002[4] = 0; + out_8125299233089870002[5] = 0; + out_8125299233089870002[6] = 0; + out_8125299233089870002[7] = 0; + out_8125299233089870002[8] = 0; } -void h_31(double *state, double *unused, double *out_5467162699907911337) { - out_5467162699907911337[0] = state[8]; +void h_31(double *state, double *unused, double *out_7988253721076099896) { + out_7988253721076099896[0] = state[8]; } -void H_31(double *state, double *unused, double *out_89279849704702035) { - out_89279849704702035[0] = 0; - out_89279849704702035[1] = 0; - out_89279849704702035[2] = 0; - out_89279849704702035[3] = 0; - out_89279849704702035[4] = 0; - out_89279849704702035[5] = 0; - out_89279849704702035[6] = 0; - out_89279849704702035[7] = 0; - out_89279849704702035[8] = 1; +void H_31(double *state, double *unused, double *out_5811422526230352065) { + out_5811422526230352065[0] = 0; + out_5811422526230352065[1] = 0; + out_5811422526230352065[2] = 0; + out_5811422526230352065[3] = 0; + out_5811422526230352065[4] = 0; + out_5811422526230352065[5] = 0; + out_5811422526230352065[6] = 0; + out_5811422526230352065[7] = 0; + out_5811422526230352065[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_6663383586198040946) { - err_fun(nom_x, delta_x, out_6663383586198040946); +void car_err_fun(double *nom_x, double *delta_x, double *out_8986378044421410730) { + err_fun(nom_x, delta_x, out_8986378044421410730); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_8588337980518277825) { - inv_err_fun(nom_x, true_x, out_8588337980518277825); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_1940910250995899848) { + inv_err_fun(nom_x, true_x, out_1940910250995899848); } -void car_H_mod_fun(double *state, double *out_6555656847717771558) { - H_mod_fun(state, out_6555656847717771558); +void car_H_mod_fun(double *state, double *out_1327881932680008443) { + H_mod_fun(state, out_1327881932680008443); } -void car_f_fun(double *state, double dt, double *out_7137582187284124708) { - f_fun(state, dt, out_7137582187284124708); +void car_f_fun(double *state, double dt, double *out_7832214154397608531) { + f_fun(state, dt, out_7832214154397608531); } -void car_F_fun(double *state, double dt, double *out_2957883628728215032) { - F_fun(state, dt, out_2957883628728215032); +void car_F_fun(double *state, double dt, double *out_4114606570946988857) { + F_fun(state, dt, out_4114606570946988857); } -void car_h_25(double *state, double *unused, double *out_753214482939102632) { - h_25(state, unused, out_753214482939102632); +void car_h_25(double *state, double *unused, double *out_8263447783360605785) { + h_25(state, unused, out_8263447783360605785); } -void car_H_25(double *state, double *unused, double *out_58633887827741607) { - H_25(state, unused, out_58633887827741607); +void car_H_25(double *state, double *unused, double *out_5780776564353391637) { + H_25(state, unused, out_5780776564353391637); } -void car_h_24(double *state, double *unused, double *out_163608178505822735) { - h_24(state, unused, out_163608178505822735); +void car_h_24(double *state, double *unused, double *out_4755010794979530940) { + h_24(state, unused, out_4755010794979530940); } -void car_H_24(double *state, double *unused, double *out_6142398792023419819) { - H_24(state, unused, out_6142398792023419819); +void car_H_24(double *state, double *unused, double *out_9108041910553836843) { + H_24(state, unused, out_9108041910553836843); } -void car_h_30(double *state, double *unused, double *out_596027814587973487) { - h_30(state, unused, out_596027814587973487); +void car_h_30(double *state, double *unused, double *out_6812816390731722618) { + h_30(state, unused, out_6812816390731722618); } -void car_H_30(double *state, double *unused, double *out_70705059315498463) { - H_30(state, unused, out_70705059315498463); +void car_H_30(double *state, double *unused, double *out_5749277167864543224) { + H_30(state, unused, out_5749277167864543224); } -void car_h_26(double *state, double *unused, double *out_6995069332001822633) { - h_26(state, unused, out_6995069332001822633); +void car_h_26(double *state, double *unused, double *out_4631164979184763219) { + h_26(state, unused, out_4631164979184763219); } -void car_H_26(double *state, double *unused, double *out_3682869431046314617) { - H_26(state, unused, out_3682869431046314617); +void car_H_26(double *state, double *unused, double *out_2039273245479335413) { + H_26(state, unused, out_2039273245479335413); } -void car_h_27(double *state, double *unused, double *out_8228754689067738133) { - h_27(state, unused, out_8228754689067738133); +void car_h_27(double *state, double *unused, double *out_6907448477601489242) { + h_27(state, unused, out_6907448477601489242); } -void car_H_27(double *state, double *unused, double *out_2245468371115923374) { - H_27(state, unused, out_2245468371115923374); +void car_H_27(double *state, double *unused, double *out_7924040479664968135) { + H_27(state, unused, out_7924040479664968135); } -void car_h_29(double *state, double *unused, double *out_347482038313688948) { - h_29(state, unused, out_347482038313688948); +void car_h_29(double *state, double *unused, double *out_6632254415316983353) { + h_29(state, unused, out_6632254415316983353); } -void car_H_29(double *state, double *unused, double *out_3958831097985474407) { - H_29(state, unused, out_3958831097985474407); +void car_H_29(double *state, double *unused, double *out_5239045823550151040) { + H_29(state, unused, out_5239045823550151040); } -void car_h_28(double *state, double *unused, double *out_7625438804391260616) { - h_28(state, unused, out_7625438804391260616); +void car_h_28(double *state, double *unused, double *out_4283151879014610665) { + h_28(state, unused, out_4283151879014610665); } -void car_H_28(double *state, double *unused, double *out_9041230115055004981) { - H_28(state, unused, out_9041230115055004981); +void car_H_28(double *state, double *unused, double *out_8125299233089870002) { + H_28(state, unused, out_8125299233089870002); } -void car_h_31(double *state, double *unused, double *out_5467162699907911337) { - h_31(state, unused, out_5467162699907911337); +void car_h_31(double *state, double *unused, double *out_7988253721076099896) { + h_31(state, unused, out_7988253721076099896); } -void car_H_31(double *state, double *unused, double *out_89279849704702035) { - H_31(state, unused, out_89279849704702035); +void car_H_31(double *state, double *unused, double *out_5811422526230352065) { + H_31(state, unused, out_5811422526230352065); } 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 5b14d92535..82df6e62d3 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_6663383586198040946); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_8588337980518277825); -void car_H_mod_fun(double *state, double *out_6555656847717771558); -void car_f_fun(double *state, double dt, double *out_7137582187284124708); -void car_F_fun(double *state, double dt, double *out_2957883628728215032); -void car_h_25(double *state, double *unused, double *out_753214482939102632); -void car_H_25(double *state, double *unused, double *out_58633887827741607); -void car_h_24(double *state, double *unused, double *out_163608178505822735); -void car_H_24(double *state, double *unused, double *out_6142398792023419819); -void car_h_30(double *state, double *unused, double *out_596027814587973487); -void car_H_30(double *state, double *unused, double *out_70705059315498463); -void car_h_26(double *state, double *unused, double *out_6995069332001822633); -void car_H_26(double *state, double *unused, double *out_3682869431046314617); -void car_h_27(double *state, double *unused, double *out_8228754689067738133); -void car_H_27(double *state, double *unused, double *out_2245468371115923374); -void car_h_29(double *state, double *unused, double *out_347482038313688948); -void car_H_29(double *state, double *unused, double *out_3958831097985474407); -void car_h_28(double *state, double *unused, double *out_7625438804391260616); -void car_H_28(double *state, double *unused, double *out_9041230115055004981); -void car_h_31(double *state, double *unused, double *out_5467162699907911337); -void car_H_31(double *state, double *unused, double *out_89279849704702035); +void car_err_fun(double *nom_x, double *delta_x, double *out_8986378044421410730); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_1940910250995899848); +void car_H_mod_fun(double *state, double *out_1327881932680008443); +void car_f_fun(double *state, double dt, double *out_7832214154397608531); +void car_F_fun(double *state, double dt, double *out_4114606570946988857); +void car_h_25(double *state, double *unused, double *out_8263447783360605785); +void car_H_25(double *state, double *unused, double *out_5780776564353391637); +void car_h_24(double *state, double *unused, double *out_4755010794979530940); +void car_H_24(double *state, double *unused, double *out_9108041910553836843); +void car_h_30(double *state, double *unused, double *out_6812816390731722618); +void car_H_30(double *state, double *unused, double *out_5749277167864543224); +void car_h_26(double *state, double *unused, double *out_4631164979184763219); +void car_H_26(double *state, double *unused, double *out_2039273245479335413); +void car_h_27(double *state, double *unused, double *out_6907448477601489242); +void car_H_27(double *state, double *unused, double *out_7924040479664968135); +void car_h_29(double *state, double *unused, double *out_6632254415316983353); +void car_H_29(double *state, double *unused, double *out_5239045823550151040); +void car_h_28(double *state, double *unused, double *out_4283151879014610665); +void car_H_28(double *state, double *unused, double *out_8125299233089870002); +void car_h_31(double *state, double *unused, double *out_7988253721076099896); +void car_H_31(double *state, double *unused, double *out_5811422526230352065); 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 b88403e348..5dfedfda0a 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_309314752274053340) { - out_309314752274053340[0] = delta_x[0] + nom_x[0]; - out_309314752274053340[1] = delta_x[1] + nom_x[1]; - out_309314752274053340[2] = delta_x[2] + nom_x[2]; - out_309314752274053340[3] = delta_x[3] + nom_x[3]; - out_309314752274053340[4] = delta_x[4] + nom_x[4]; - out_309314752274053340[5] = delta_x[5] + nom_x[5]; - out_309314752274053340[6] = delta_x[6] + nom_x[6]; - out_309314752274053340[7] = delta_x[7] + nom_x[7]; - out_309314752274053340[8] = delta_x[8] + nom_x[8]; - out_309314752274053340[9] = delta_x[9] + nom_x[9]; - out_309314752274053340[10] = delta_x[10] + nom_x[10]; - out_309314752274053340[11] = delta_x[11] + nom_x[11]; - out_309314752274053340[12] = delta_x[12] + nom_x[12]; - out_309314752274053340[13] = delta_x[13] + nom_x[13]; - out_309314752274053340[14] = delta_x[14] + nom_x[14]; - out_309314752274053340[15] = delta_x[15] + nom_x[15]; - out_309314752274053340[16] = delta_x[16] + nom_x[16]; - out_309314752274053340[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_1282506072975981321) { + out_1282506072975981321[0] = delta_x[0] + nom_x[0]; + out_1282506072975981321[1] = delta_x[1] + nom_x[1]; + out_1282506072975981321[2] = delta_x[2] + nom_x[2]; + out_1282506072975981321[3] = delta_x[3] + nom_x[3]; + out_1282506072975981321[4] = delta_x[4] + nom_x[4]; + out_1282506072975981321[5] = delta_x[5] + nom_x[5]; + out_1282506072975981321[6] = delta_x[6] + nom_x[6]; + out_1282506072975981321[7] = delta_x[7] + nom_x[7]; + out_1282506072975981321[8] = delta_x[8] + nom_x[8]; + out_1282506072975981321[9] = delta_x[9] + nom_x[9]; + out_1282506072975981321[10] = delta_x[10] + nom_x[10]; + out_1282506072975981321[11] = delta_x[11] + nom_x[11]; + out_1282506072975981321[12] = delta_x[12] + nom_x[12]; + out_1282506072975981321[13] = delta_x[13] + nom_x[13]; + out_1282506072975981321[14] = delta_x[14] + nom_x[14]; + out_1282506072975981321[15] = delta_x[15] + nom_x[15]; + out_1282506072975981321[16] = delta_x[16] + nom_x[16]; + out_1282506072975981321[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_651338713617187314) { - out_651338713617187314[0] = -nom_x[0] + true_x[0]; - out_651338713617187314[1] = -nom_x[1] + true_x[1]; - out_651338713617187314[2] = -nom_x[2] + true_x[2]; - out_651338713617187314[3] = -nom_x[3] + true_x[3]; - out_651338713617187314[4] = -nom_x[4] + true_x[4]; - out_651338713617187314[5] = -nom_x[5] + true_x[5]; - out_651338713617187314[6] = -nom_x[6] + true_x[6]; - out_651338713617187314[7] = -nom_x[7] + true_x[7]; - out_651338713617187314[8] = -nom_x[8] + true_x[8]; - out_651338713617187314[9] = -nom_x[9] + true_x[9]; - out_651338713617187314[10] = -nom_x[10] + true_x[10]; - out_651338713617187314[11] = -nom_x[11] + true_x[11]; - out_651338713617187314[12] = -nom_x[12] + true_x[12]; - out_651338713617187314[13] = -nom_x[13] + true_x[13]; - out_651338713617187314[14] = -nom_x[14] + true_x[14]; - out_651338713617187314[15] = -nom_x[15] + true_x[15]; - out_651338713617187314[16] = -nom_x[16] + true_x[16]; - out_651338713617187314[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_8512738075508223963) { + out_8512738075508223963[0] = -nom_x[0] + true_x[0]; + out_8512738075508223963[1] = -nom_x[1] + true_x[1]; + out_8512738075508223963[2] = -nom_x[2] + true_x[2]; + out_8512738075508223963[3] = -nom_x[3] + true_x[3]; + out_8512738075508223963[4] = -nom_x[4] + true_x[4]; + out_8512738075508223963[5] = -nom_x[5] + true_x[5]; + out_8512738075508223963[6] = -nom_x[6] + true_x[6]; + out_8512738075508223963[7] = -nom_x[7] + true_x[7]; + out_8512738075508223963[8] = -nom_x[8] + true_x[8]; + out_8512738075508223963[9] = -nom_x[9] + true_x[9]; + out_8512738075508223963[10] = -nom_x[10] + true_x[10]; + out_8512738075508223963[11] = -nom_x[11] + true_x[11]; + out_8512738075508223963[12] = -nom_x[12] + true_x[12]; + out_8512738075508223963[13] = -nom_x[13] + true_x[13]; + out_8512738075508223963[14] = -nom_x[14] + true_x[14]; + out_8512738075508223963[15] = -nom_x[15] + true_x[15]; + out_8512738075508223963[16] = -nom_x[16] + true_x[16]; + out_8512738075508223963[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_1638652939169850596) { - out_1638652939169850596[0] = 1.0; - out_1638652939169850596[1] = 0.0; - out_1638652939169850596[2] = 0.0; - out_1638652939169850596[3] = 0.0; - out_1638652939169850596[4] = 0.0; - out_1638652939169850596[5] = 0.0; - out_1638652939169850596[6] = 0.0; - out_1638652939169850596[7] = 0.0; - out_1638652939169850596[8] = 0.0; - out_1638652939169850596[9] = 0.0; - out_1638652939169850596[10] = 0.0; - out_1638652939169850596[11] = 0.0; - out_1638652939169850596[12] = 0.0; - out_1638652939169850596[13] = 0.0; - out_1638652939169850596[14] = 0.0; - out_1638652939169850596[15] = 0.0; - out_1638652939169850596[16] = 0.0; - out_1638652939169850596[17] = 0.0; - out_1638652939169850596[18] = 0.0; - out_1638652939169850596[19] = 1.0; - out_1638652939169850596[20] = 0.0; - out_1638652939169850596[21] = 0.0; - out_1638652939169850596[22] = 0.0; - out_1638652939169850596[23] = 0.0; - out_1638652939169850596[24] = 0.0; - out_1638652939169850596[25] = 0.0; - out_1638652939169850596[26] = 0.0; - out_1638652939169850596[27] = 0.0; - out_1638652939169850596[28] = 0.0; - out_1638652939169850596[29] = 0.0; - out_1638652939169850596[30] = 0.0; - out_1638652939169850596[31] = 0.0; - out_1638652939169850596[32] = 0.0; - out_1638652939169850596[33] = 0.0; - out_1638652939169850596[34] = 0.0; - out_1638652939169850596[35] = 0.0; - out_1638652939169850596[36] = 0.0; - out_1638652939169850596[37] = 0.0; - out_1638652939169850596[38] = 1.0; - out_1638652939169850596[39] = 0.0; - out_1638652939169850596[40] = 0.0; - out_1638652939169850596[41] = 0.0; - out_1638652939169850596[42] = 0.0; - out_1638652939169850596[43] = 0.0; - out_1638652939169850596[44] = 0.0; - out_1638652939169850596[45] = 0.0; - out_1638652939169850596[46] = 0.0; - out_1638652939169850596[47] = 0.0; - out_1638652939169850596[48] = 0.0; - out_1638652939169850596[49] = 0.0; - out_1638652939169850596[50] = 0.0; - out_1638652939169850596[51] = 0.0; - out_1638652939169850596[52] = 0.0; - out_1638652939169850596[53] = 0.0; - out_1638652939169850596[54] = 0.0; - out_1638652939169850596[55] = 0.0; - out_1638652939169850596[56] = 0.0; - out_1638652939169850596[57] = 1.0; - out_1638652939169850596[58] = 0.0; - out_1638652939169850596[59] = 0.0; - out_1638652939169850596[60] = 0.0; - out_1638652939169850596[61] = 0.0; - out_1638652939169850596[62] = 0.0; - out_1638652939169850596[63] = 0.0; - out_1638652939169850596[64] = 0.0; - out_1638652939169850596[65] = 0.0; - out_1638652939169850596[66] = 0.0; - out_1638652939169850596[67] = 0.0; - out_1638652939169850596[68] = 0.0; - out_1638652939169850596[69] = 0.0; - out_1638652939169850596[70] = 0.0; - out_1638652939169850596[71] = 0.0; - out_1638652939169850596[72] = 0.0; - out_1638652939169850596[73] = 0.0; - out_1638652939169850596[74] = 0.0; - out_1638652939169850596[75] = 0.0; - out_1638652939169850596[76] = 1.0; - out_1638652939169850596[77] = 0.0; - out_1638652939169850596[78] = 0.0; - out_1638652939169850596[79] = 0.0; - out_1638652939169850596[80] = 0.0; - out_1638652939169850596[81] = 0.0; - out_1638652939169850596[82] = 0.0; - out_1638652939169850596[83] = 0.0; - out_1638652939169850596[84] = 0.0; - out_1638652939169850596[85] = 0.0; - out_1638652939169850596[86] = 0.0; - out_1638652939169850596[87] = 0.0; - out_1638652939169850596[88] = 0.0; - out_1638652939169850596[89] = 0.0; - out_1638652939169850596[90] = 0.0; - out_1638652939169850596[91] = 0.0; - out_1638652939169850596[92] = 0.0; - out_1638652939169850596[93] = 0.0; - out_1638652939169850596[94] = 0.0; - out_1638652939169850596[95] = 1.0; - out_1638652939169850596[96] = 0.0; - out_1638652939169850596[97] = 0.0; - out_1638652939169850596[98] = 0.0; - out_1638652939169850596[99] = 0.0; - out_1638652939169850596[100] = 0.0; - out_1638652939169850596[101] = 0.0; - out_1638652939169850596[102] = 0.0; - out_1638652939169850596[103] = 0.0; - out_1638652939169850596[104] = 0.0; - out_1638652939169850596[105] = 0.0; - out_1638652939169850596[106] = 0.0; - out_1638652939169850596[107] = 0.0; - out_1638652939169850596[108] = 0.0; - out_1638652939169850596[109] = 0.0; - out_1638652939169850596[110] = 0.0; - out_1638652939169850596[111] = 0.0; - out_1638652939169850596[112] = 0.0; - out_1638652939169850596[113] = 0.0; - out_1638652939169850596[114] = 1.0; - out_1638652939169850596[115] = 0.0; - out_1638652939169850596[116] = 0.0; - out_1638652939169850596[117] = 0.0; - out_1638652939169850596[118] = 0.0; - out_1638652939169850596[119] = 0.0; - out_1638652939169850596[120] = 0.0; - out_1638652939169850596[121] = 0.0; - out_1638652939169850596[122] = 0.0; - out_1638652939169850596[123] = 0.0; - out_1638652939169850596[124] = 0.0; - out_1638652939169850596[125] = 0.0; - out_1638652939169850596[126] = 0.0; - out_1638652939169850596[127] = 0.0; - out_1638652939169850596[128] = 0.0; - out_1638652939169850596[129] = 0.0; - out_1638652939169850596[130] = 0.0; - out_1638652939169850596[131] = 0.0; - out_1638652939169850596[132] = 0.0; - out_1638652939169850596[133] = 1.0; - out_1638652939169850596[134] = 0.0; - out_1638652939169850596[135] = 0.0; - out_1638652939169850596[136] = 0.0; - out_1638652939169850596[137] = 0.0; - out_1638652939169850596[138] = 0.0; - out_1638652939169850596[139] = 0.0; - out_1638652939169850596[140] = 0.0; - out_1638652939169850596[141] = 0.0; - out_1638652939169850596[142] = 0.0; - out_1638652939169850596[143] = 0.0; - out_1638652939169850596[144] = 0.0; - out_1638652939169850596[145] = 0.0; - out_1638652939169850596[146] = 0.0; - out_1638652939169850596[147] = 0.0; - out_1638652939169850596[148] = 0.0; - out_1638652939169850596[149] = 0.0; - out_1638652939169850596[150] = 0.0; - out_1638652939169850596[151] = 0.0; - out_1638652939169850596[152] = 1.0; - out_1638652939169850596[153] = 0.0; - out_1638652939169850596[154] = 0.0; - out_1638652939169850596[155] = 0.0; - out_1638652939169850596[156] = 0.0; - out_1638652939169850596[157] = 0.0; - out_1638652939169850596[158] = 0.0; - out_1638652939169850596[159] = 0.0; - out_1638652939169850596[160] = 0.0; - out_1638652939169850596[161] = 0.0; - out_1638652939169850596[162] = 0.0; - out_1638652939169850596[163] = 0.0; - out_1638652939169850596[164] = 0.0; - out_1638652939169850596[165] = 0.0; - out_1638652939169850596[166] = 0.0; - out_1638652939169850596[167] = 0.0; - out_1638652939169850596[168] = 0.0; - out_1638652939169850596[169] = 0.0; - out_1638652939169850596[170] = 0.0; - out_1638652939169850596[171] = 1.0; - out_1638652939169850596[172] = 0.0; - out_1638652939169850596[173] = 0.0; - out_1638652939169850596[174] = 0.0; - out_1638652939169850596[175] = 0.0; - out_1638652939169850596[176] = 0.0; - out_1638652939169850596[177] = 0.0; - out_1638652939169850596[178] = 0.0; - out_1638652939169850596[179] = 0.0; - out_1638652939169850596[180] = 0.0; - out_1638652939169850596[181] = 0.0; - out_1638652939169850596[182] = 0.0; - out_1638652939169850596[183] = 0.0; - out_1638652939169850596[184] = 0.0; - out_1638652939169850596[185] = 0.0; - out_1638652939169850596[186] = 0.0; - out_1638652939169850596[187] = 0.0; - out_1638652939169850596[188] = 0.0; - out_1638652939169850596[189] = 0.0; - out_1638652939169850596[190] = 1.0; - out_1638652939169850596[191] = 0.0; - out_1638652939169850596[192] = 0.0; - out_1638652939169850596[193] = 0.0; - out_1638652939169850596[194] = 0.0; - out_1638652939169850596[195] = 0.0; - out_1638652939169850596[196] = 0.0; - out_1638652939169850596[197] = 0.0; - out_1638652939169850596[198] = 0.0; - out_1638652939169850596[199] = 0.0; - out_1638652939169850596[200] = 0.0; - out_1638652939169850596[201] = 0.0; - out_1638652939169850596[202] = 0.0; - out_1638652939169850596[203] = 0.0; - out_1638652939169850596[204] = 0.0; - out_1638652939169850596[205] = 0.0; - out_1638652939169850596[206] = 0.0; - out_1638652939169850596[207] = 0.0; - out_1638652939169850596[208] = 0.0; - out_1638652939169850596[209] = 1.0; - out_1638652939169850596[210] = 0.0; - out_1638652939169850596[211] = 0.0; - out_1638652939169850596[212] = 0.0; - out_1638652939169850596[213] = 0.0; - out_1638652939169850596[214] = 0.0; - out_1638652939169850596[215] = 0.0; - out_1638652939169850596[216] = 0.0; - out_1638652939169850596[217] = 0.0; - out_1638652939169850596[218] = 0.0; - out_1638652939169850596[219] = 0.0; - out_1638652939169850596[220] = 0.0; - out_1638652939169850596[221] = 0.0; - out_1638652939169850596[222] = 0.0; - out_1638652939169850596[223] = 0.0; - out_1638652939169850596[224] = 0.0; - out_1638652939169850596[225] = 0.0; - out_1638652939169850596[226] = 0.0; - out_1638652939169850596[227] = 0.0; - out_1638652939169850596[228] = 1.0; - out_1638652939169850596[229] = 0.0; - out_1638652939169850596[230] = 0.0; - out_1638652939169850596[231] = 0.0; - out_1638652939169850596[232] = 0.0; - out_1638652939169850596[233] = 0.0; - out_1638652939169850596[234] = 0.0; - out_1638652939169850596[235] = 0.0; - out_1638652939169850596[236] = 0.0; - out_1638652939169850596[237] = 0.0; - out_1638652939169850596[238] = 0.0; - out_1638652939169850596[239] = 0.0; - out_1638652939169850596[240] = 0.0; - out_1638652939169850596[241] = 0.0; - out_1638652939169850596[242] = 0.0; - out_1638652939169850596[243] = 0.0; - out_1638652939169850596[244] = 0.0; - out_1638652939169850596[245] = 0.0; - out_1638652939169850596[246] = 0.0; - out_1638652939169850596[247] = 1.0; - out_1638652939169850596[248] = 0.0; - out_1638652939169850596[249] = 0.0; - out_1638652939169850596[250] = 0.0; - out_1638652939169850596[251] = 0.0; - out_1638652939169850596[252] = 0.0; - out_1638652939169850596[253] = 0.0; - out_1638652939169850596[254] = 0.0; - out_1638652939169850596[255] = 0.0; - out_1638652939169850596[256] = 0.0; - out_1638652939169850596[257] = 0.0; - out_1638652939169850596[258] = 0.0; - out_1638652939169850596[259] = 0.0; - out_1638652939169850596[260] = 0.0; - out_1638652939169850596[261] = 0.0; - out_1638652939169850596[262] = 0.0; - out_1638652939169850596[263] = 0.0; - out_1638652939169850596[264] = 0.0; - out_1638652939169850596[265] = 0.0; - out_1638652939169850596[266] = 1.0; - out_1638652939169850596[267] = 0.0; - out_1638652939169850596[268] = 0.0; - out_1638652939169850596[269] = 0.0; - out_1638652939169850596[270] = 0.0; - out_1638652939169850596[271] = 0.0; - out_1638652939169850596[272] = 0.0; - out_1638652939169850596[273] = 0.0; - out_1638652939169850596[274] = 0.0; - out_1638652939169850596[275] = 0.0; - out_1638652939169850596[276] = 0.0; - out_1638652939169850596[277] = 0.0; - out_1638652939169850596[278] = 0.0; - out_1638652939169850596[279] = 0.0; - out_1638652939169850596[280] = 0.0; - out_1638652939169850596[281] = 0.0; - out_1638652939169850596[282] = 0.0; - out_1638652939169850596[283] = 0.0; - out_1638652939169850596[284] = 0.0; - out_1638652939169850596[285] = 1.0; - out_1638652939169850596[286] = 0.0; - out_1638652939169850596[287] = 0.0; - out_1638652939169850596[288] = 0.0; - out_1638652939169850596[289] = 0.0; - out_1638652939169850596[290] = 0.0; - out_1638652939169850596[291] = 0.0; - out_1638652939169850596[292] = 0.0; - out_1638652939169850596[293] = 0.0; - out_1638652939169850596[294] = 0.0; - out_1638652939169850596[295] = 0.0; - out_1638652939169850596[296] = 0.0; - out_1638652939169850596[297] = 0.0; - out_1638652939169850596[298] = 0.0; - out_1638652939169850596[299] = 0.0; - out_1638652939169850596[300] = 0.0; - out_1638652939169850596[301] = 0.0; - out_1638652939169850596[302] = 0.0; - out_1638652939169850596[303] = 0.0; - out_1638652939169850596[304] = 1.0; - out_1638652939169850596[305] = 0.0; - out_1638652939169850596[306] = 0.0; - out_1638652939169850596[307] = 0.0; - out_1638652939169850596[308] = 0.0; - out_1638652939169850596[309] = 0.0; - out_1638652939169850596[310] = 0.0; - out_1638652939169850596[311] = 0.0; - out_1638652939169850596[312] = 0.0; - out_1638652939169850596[313] = 0.0; - out_1638652939169850596[314] = 0.0; - out_1638652939169850596[315] = 0.0; - out_1638652939169850596[316] = 0.0; - out_1638652939169850596[317] = 0.0; - out_1638652939169850596[318] = 0.0; - out_1638652939169850596[319] = 0.0; - out_1638652939169850596[320] = 0.0; - out_1638652939169850596[321] = 0.0; - out_1638652939169850596[322] = 0.0; - out_1638652939169850596[323] = 1.0; +void H_mod_fun(double *state, double *out_3135048301285292021) { + out_3135048301285292021[0] = 1.0; + out_3135048301285292021[1] = 0.0; + out_3135048301285292021[2] = 0.0; + out_3135048301285292021[3] = 0.0; + out_3135048301285292021[4] = 0.0; + out_3135048301285292021[5] = 0.0; + out_3135048301285292021[6] = 0.0; + out_3135048301285292021[7] = 0.0; + out_3135048301285292021[8] = 0.0; + out_3135048301285292021[9] = 0.0; + out_3135048301285292021[10] = 0.0; + out_3135048301285292021[11] = 0.0; + out_3135048301285292021[12] = 0.0; + out_3135048301285292021[13] = 0.0; + out_3135048301285292021[14] = 0.0; + out_3135048301285292021[15] = 0.0; + out_3135048301285292021[16] = 0.0; + out_3135048301285292021[17] = 0.0; + out_3135048301285292021[18] = 0.0; + out_3135048301285292021[19] = 1.0; + out_3135048301285292021[20] = 0.0; + out_3135048301285292021[21] = 0.0; + out_3135048301285292021[22] = 0.0; + out_3135048301285292021[23] = 0.0; + out_3135048301285292021[24] = 0.0; + out_3135048301285292021[25] = 0.0; + out_3135048301285292021[26] = 0.0; + out_3135048301285292021[27] = 0.0; + out_3135048301285292021[28] = 0.0; + out_3135048301285292021[29] = 0.0; + out_3135048301285292021[30] = 0.0; + out_3135048301285292021[31] = 0.0; + out_3135048301285292021[32] = 0.0; + out_3135048301285292021[33] = 0.0; + out_3135048301285292021[34] = 0.0; + out_3135048301285292021[35] = 0.0; + out_3135048301285292021[36] = 0.0; + out_3135048301285292021[37] = 0.0; + out_3135048301285292021[38] = 1.0; + out_3135048301285292021[39] = 0.0; + out_3135048301285292021[40] = 0.0; + out_3135048301285292021[41] = 0.0; + out_3135048301285292021[42] = 0.0; + out_3135048301285292021[43] = 0.0; + out_3135048301285292021[44] = 0.0; + out_3135048301285292021[45] = 0.0; + out_3135048301285292021[46] = 0.0; + out_3135048301285292021[47] = 0.0; + out_3135048301285292021[48] = 0.0; + out_3135048301285292021[49] = 0.0; + out_3135048301285292021[50] = 0.0; + out_3135048301285292021[51] = 0.0; + out_3135048301285292021[52] = 0.0; + out_3135048301285292021[53] = 0.0; + out_3135048301285292021[54] = 0.0; + out_3135048301285292021[55] = 0.0; + out_3135048301285292021[56] = 0.0; + out_3135048301285292021[57] = 1.0; + out_3135048301285292021[58] = 0.0; + out_3135048301285292021[59] = 0.0; + out_3135048301285292021[60] = 0.0; + out_3135048301285292021[61] = 0.0; + out_3135048301285292021[62] = 0.0; + out_3135048301285292021[63] = 0.0; + out_3135048301285292021[64] = 0.0; + out_3135048301285292021[65] = 0.0; + out_3135048301285292021[66] = 0.0; + out_3135048301285292021[67] = 0.0; + out_3135048301285292021[68] = 0.0; + out_3135048301285292021[69] = 0.0; + out_3135048301285292021[70] = 0.0; + out_3135048301285292021[71] = 0.0; + out_3135048301285292021[72] = 0.0; + out_3135048301285292021[73] = 0.0; + out_3135048301285292021[74] = 0.0; + out_3135048301285292021[75] = 0.0; + out_3135048301285292021[76] = 1.0; + out_3135048301285292021[77] = 0.0; + out_3135048301285292021[78] = 0.0; + out_3135048301285292021[79] = 0.0; + out_3135048301285292021[80] = 0.0; + out_3135048301285292021[81] = 0.0; + out_3135048301285292021[82] = 0.0; + out_3135048301285292021[83] = 0.0; + out_3135048301285292021[84] = 0.0; + out_3135048301285292021[85] = 0.0; + out_3135048301285292021[86] = 0.0; + out_3135048301285292021[87] = 0.0; + out_3135048301285292021[88] = 0.0; + out_3135048301285292021[89] = 0.0; + out_3135048301285292021[90] = 0.0; + out_3135048301285292021[91] = 0.0; + out_3135048301285292021[92] = 0.0; + out_3135048301285292021[93] = 0.0; + out_3135048301285292021[94] = 0.0; + out_3135048301285292021[95] = 1.0; + out_3135048301285292021[96] = 0.0; + out_3135048301285292021[97] = 0.0; + out_3135048301285292021[98] = 0.0; + out_3135048301285292021[99] = 0.0; + out_3135048301285292021[100] = 0.0; + out_3135048301285292021[101] = 0.0; + out_3135048301285292021[102] = 0.0; + out_3135048301285292021[103] = 0.0; + out_3135048301285292021[104] = 0.0; + out_3135048301285292021[105] = 0.0; + out_3135048301285292021[106] = 0.0; + out_3135048301285292021[107] = 0.0; + out_3135048301285292021[108] = 0.0; + out_3135048301285292021[109] = 0.0; + out_3135048301285292021[110] = 0.0; + out_3135048301285292021[111] = 0.0; + out_3135048301285292021[112] = 0.0; + out_3135048301285292021[113] = 0.0; + out_3135048301285292021[114] = 1.0; + out_3135048301285292021[115] = 0.0; + out_3135048301285292021[116] = 0.0; + out_3135048301285292021[117] = 0.0; + out_3135048301285292021[118] = 0.0; + out_3135048301285292021[119] = 0.0; + out_3135048301285292021[120] = 0.0; + out_3135048301285292021[121] = 0.0; + out_3135048301285292021[122] = 0.0; + out_3135048301285292021[123] = 0.0; + out_3135048301285292021[124] = 0.0; + out_3135048301285292021[125] = 0.0; + out_3135048301285292021[126] = 0.0; + out_3135048301285292021[127] = 0.0; + out_3135048301285292021[128] = 0.0; + out_3135048301285292021[129] = 0.0; + out_3135048301285292021[130] = 0.0; + out_3135048301285292021[131] = 0.0; + out_3135048301285292021[132] = 0.0; + out_3135048301285292021[133] = 1.0; + out_3135048301285292021[134] = 0.0; + out_3135048301285292021[135] = 0.0; + out_3135048301285292021[136] = 0.0; + out_3135048301285292021[137] = 0.0; + out_3135048301285292021[138] = 0.0; + out_3135048301285292021[139] = 0.0; + out_3135048301285292021[140] = 0.0; + out_3135048301285292021[141] = 0.0; + out_3135048301285292021[142] = 0.0; + out_3135048301285292021[143] = 0.0; + out_3135048301285292021[144] = 0.0; + out_3135048301285292021[145] = 0.0; + out_3135048301285292021[146] = 0.0; + out_3135048301285292021[147] = 0.0; + out_3135048301285292021[148] = 0.0; + out_3135048301285292021[149] = 0.0; + out_3135048301285292021[150] = 0.0; + out_3135048301285292021[151] = 0.0; + out_3135048301285292021[152] = 1.0; + out_3135048301285292021[153] = 0.0; + out_3135048301285292021[154] = 0.0; + out_3135048301285292021[155] = 0.0; + out_3135048301285292021[156] = 0.0; + out_3135048301285292021[157] = 0.0; + out_3135048301285292021[158] = 0.0; + out_3135048301285292021[159] = 0.0; + out_3135048301285292021[160] = 0.0; + out_3135048301285292021[161] = 0.0; + out_3135048301285292021[162] = 0.0; + out_3135048301285292021[163] = 0.0; + out_3135048301285292021[164] = 0.0; + out_3135048301285292021[165] = 0.0; + out_3135048301285292021[166] = 0.0; + out_3135048301285292021[167] = 0.0; + out_3135048301285292021[168] = 0.0; + out_3135048301285292021[169] = 0.0; + out_3135048301285292021[170] = 0.0; + out_3135048301285292021[171] = 1.0; + out_3135048301285292021[172] = 0.0; + out_3135048301285292021[173] = 0.0; + out_3135048301285292021[174] = 0.0; + out_3135048301285292021[175] = 0.0; + out_3135048301285292021[176] = 0.0; + out_3135048301285292021[177] = 0.0; + out_3135048301285292021[178] = 0.0; + out_3135048301285292021[179] = 0.0; + out_3135048301285292021[180] = 0.0; + out_3135048301285292021[181] = 0.0; + out_3135048301285292021[182] = 0.0; + out_3135048301285292021[183] = 0.0; + out_3135048301285292021[184] = 0.0; + out_3135048301285292021[185] = 0.0; + out_3135048301285292021[186] = 0.0; + out_3135048301285292021[187] = 0.0; + out_3135048301285292021[188] = 0.0; + out_3135048301285292021[189] = 0.0; + out_3135048301285292021[190] = 1.0; + out_3135048301285292021[191] = 0.0; + out_3135048301285292021[192] = 0.0; + out_3135048301285292021[193] = 0.0; + out_3135048301285292021[194] = 0.0; + out_3135048301285292021[195] = 0.0; + out_3135048301285292021[196] = 0.0; + out_3135048301285292021[197] = 0.0; + out_3135048301285292021[198] = 0.0; + out_3135048301285292021[199] = 0.0; + out_3135048301285292021[200] = 0.0; + out_3135048301285292021[201] = 0.0; + out_3135048301285292021[202] = 0.0; + out_3135048301285292021[203] = 0.0; + out_3135048301285292021[204] = 0.0; + out_3135048301285292021[205] = 0.0; + out_3135048301285292021[206] = 0.0; + out_3135048301285292021[207] = 0.0; + out_3135048301285292021[208] = 0.0; + out_3135048301285292021[209] = 1.0; + out_3135048301285292021[210] = 0.0; + out_3135048301285292021[211] = 0.0; + out_3135048301285292021[212] = 0.0; + out_3135048301285292021[213] = 0.0; + out_3135048301285292021[214] = 0.0; + out_3135048301285292021[215] = 0.0; + out_3135048301285292021[216] = 0.0; + out_3135048301285292021[217] = 0.0; + out_3135048301285292021[218] = 0.0; + out_3135048301285292021[219] = 0.0; + out_3135048301285292021[220] = 0.0; + out_3135048301285292021[221] = 0.0; + out_3135048301285292021[222] = 0.0; + out_3135048301285292021[223] = 0.0; + out_3135048301285292021[224] = 0.0; + out_3135048301285292021[225] = 0.0; + out_3135048301285292021[226] = 0.0; + out_3135048301285292021[227] = 0.0; + out_3135048301285292021[228] = 1.0; + out_3135048301285292021[229] = 0.0; + out_3135048301285292021[230] = 0.0; + out_3135048301285292021[231] = 0.0; + out_3135048301285292021[232] = 0.0; + out_3135048301285292021[233] = 0.0; + out_3135048301285292021[234] = 0.0; + out_3135048301285292021[235] = 0.0; + out_3135048301285292021[236] = 0.0; + out_3135048301285292021[237] = 0.0; + out_3135048301285292021[238] = 0.0; + out_3135048301285292021[239] = 0.0; + out_3135048301285292021[240] = 0.0; + out_3135048301285292021[241] = 0.0; + out_3135048301285292021[242] = 0.0; + out_3135048301285292021[243] = 0.0; + out_3135048301285292021[244] = 0.0; + out_3135048301285292021[245] = 0.0; + out_3135048301285292021[246] = 0.0; + out_3135048301285292021[247] = 1.0; + out_3135048301285292021[248] = 0.0; + out_3135048301285292021[249] = 0.0; + out_3135048301285292021[250] = 0.0; + out_3135048301285292021[251] = 0.0; + out_3135048301285292021[252] = 0.0; + out_3135048301285292021[253] = 0.0; + out_3135048301285292021[254] = 0.0; + out_3135048301285292021[255] = 0.0; + out_3135048301285292021[256] = 0.0; + out_3135048301285292021[257] = 0.0; + out_3135048301285292021[258] = 0.0; + out_3135048301285292021[259] = 0.0; + out_3135048301285292021[260] = 0.0; + out_3135048301285292021[261] = 0.0; + out_3135048301285292021[262] = 0.0; + out_3135048301285292021[263] = 0.0; + out_3135048301285292021[264] = 0.0; + out_3135048301285292021[265] = 0.0; + out_3135048301285292021[266] = 1.0; + out_3135048301285292021[267] = 0.0; + out_3135048301285292021[268] = 0.0; + out_3135048301285292021[269] = 0.0; + out_3135048301285292021[270] = 0.0; + out_3135048301285292021[271] = 0.0; + out_3135048301285292021[272] = 0.0; + out_3135048301285292021[273] = 0.0; + out_3135048301285292021[274] = 0.0; + out_3135048301285292021[275] = 0.0; + out_3135048301285292021[276] = 0.0; + out_3135048301285292021[277] = 0.0; + out_3135048301285292021[278] = 0.0; + out_3135048301285292021[279] = 0.0; + out_3135048301285292021[280] = 0.0; + out_3135048301285292021[281] = 0.0; + out_3135048301285292021[282] = 0.0; + out_3135048301285292021[283] = 0.0; + out_3135048301285292021[284] = 0.0; + out_3135048301285292021[285] = 1.0; + out_3135048301285292021[286] = 0.0; + out_3135048301285292021[287] = 0.0; + out_3135048301285292021[288] = 0.0; + out_3135048301285292021[289] = 0.0; + out_3135048301285292021[290] = 0.0; + out_3135048301285292021[291] = 0.0; + out_3135048301285292021[292] = 0.0; + out_3135048301285292021[293] = 0.0; + out_3135048301285292021[294] = 0.0; + out_3135048301285292021[295] = 0.0; + out_3135048301285292021[296] = 0.0; + out_3135048301285292021[297] = 0.0; + out_3135048301285292021[298] = 0.0; + out_3135048301285292021[299] = 0.0; + out_3135048301285292021[300] = 0.0; + out_3135048301285292021[301] = 0.0; + out_3135048301285292021[302] = 0.0; + out_3135048301285292021[303] = 0.0; + out_3135048301285292021[304] = 1.0; + out_3135048301285292021[305] = 0.0; + out_3135048301285292021[306] = 0.0; + out_3135048301285292021[307] = 0.0; + out_3135048301285292021[308] = 0.0; + out_3135048301285292021[309] = 0.0; + out_3135048301285292021[310] = 0.0; + out_3135048301285292021[311] = 0.0; + out_3135048301285292021[312] = 0.0; + out_3135048301285292021[313] = 0.0; + out_3135048301285292021[314] = 0.0; + out_3135048301285292021[315] = 0.0; + out_3135048301285292021[316] = 0.0; + out_3135048301285292021[317] = 0.0; + out_3135048301285292021[318] = 0.0; + out_3135048301285292021[319] = 0.0; + out_3135048301285292021[320] = 0.0; + out_3135048301285292021[321] = 0.0; + out_3135048301285292021[322] = 0.0; + out_3135048301285292021[323] = 1.0; } -void f_fun(double *state, double dt, double *out_2831397313375340905) { - out_2831397313375340905[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_2831397313375340905[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_2831397313375340905[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_2831397313375340905[3] = dt*state[12] + state[3]; - out_2831397313375340905[4] = dt*state[13] + state[4]; - out_2831397313375340905[5] = dt*state[14] + state[5]; - out_2831397313375340905[6] = state[6]; - out_2831397313375340905[7] = state[7]; - out_2831397313375340905[8] = state[8]; - out_2831397313375340905[9] = state[9]; - out_2831397313375340905[10] = state[10]; - out_2831397313375340905[11] = state[11]; - out_2831397313375340905[12] = state[12]; - out_2831397313375340905[13] = state[13]; - out_2831397313375340905[14] = state[14]; - out_2831397313375340905[15] = state[15]; - out_2831397313375340905[16] = state[16]; - out_2831397313375340905[17] = state[17]; +void f_fun(double *state, double dt, double *out_8178969782644284561) { + out_8178969782644284561[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_8178969782644284561[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_8178969782644284561[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_8178969782644284561[3] = dt*state[12] + state[3]; + out_8178969782644284561[4] = dt*state[13] + state[4]; + out_8178969782644284561[5] = dt*state[14] + state[5]; + out_8178969782644284561[6] = state[6]; + out_8178969782644284561[7] = state[7]; + out_8178969782644284561[8] = state[8]; + out_8178969782644284561[9] = state[9]; + out_8178969782644284561[10] = state[10]; + out_8178969782644284561[11] = state[11]; + out_8178969782644284561[12] = state[12]; + out_8178969782644284561[13] = state[13]; + out_8178969782644284561[14] = state[14]; + out_8178969782644284561[15] = state[15]; + out_8178969782644284561[16] = state[16]; + out_8178969782644284561[17] = state[17]; } -void F_fun(double *state, double dt, double *out_852487606375420018) { - out_852487606375420018[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_852487606375420018[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_852487606375420018[2] = 0; - out_852487606375420018[3] = 0; - out_852487606375420018[4] = 0; - out_852487606375420018[5] = 0; - out_852487606375420018[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_852487606375420018[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_852487606375420018[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_852487606375420018[9] = 0; - out_852487606375420018[10] = 0; - out_852487606375420018[11] = 0; - out_852487606375420018[12] = 0; - out_852487606375420018[13] = 0; - out_852487606375420018[14] = 0; - out_852487606375420018[15] = 0; - out_852487606375420018[16] = 0; - out_852487606375420018[17] = 0; - out_852487606375420018[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_852487606375420018[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_852487606375420018[20] = 0; - out_852487606375420018[21] = 0; - out_852487606375420018[22] = 0; - out_852487606375420018[23] = 0; - out_852487606375420018[24] = 0; - out_852487606375420018[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_852487606375420018[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_852487606375420018[27] = 0; - out_852487606375420018[28] = 0; - out_852487606375420018[29] = 0; - out_852487606375420018[30] = 0; - out_852487606375420018[31] = 0; - out_852487606375420018[32] = 0; - out_852487606375420018[33] = 0; - out_852487606375420018[34] = 0; - out_852487606375420018[35] = 0; - out_852487606375420018[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_852487606375420018[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_852487606375420018[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_852487606375420018[39] = 0; - out_852487606375420018[40] = 0; - out_852487606375420018[41] = 0; - out_852487606375420018[42] = 0; - out_852487606375420018[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_852487606375420018[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_852487606375420018[45] = 0; - out_852487606375420018[46] = 0; - out_852487606375420018[47] = 0; - out_852487606375420018[48] = 0; - out_852487606375420018[49] = 0; - out_852487606375420018[50] = 0; - out_852487606375420018[51] = 0; - out_852487606375420018[52] = 0; - out_852487606375420018[53] = 0; - out_852487606375420018[54] = 0; - out_852487606375420018[55] = 0; - out_852487606375420018[56] = 0; - out_852487606375420018[57] = 1; - out_852487606375420018[58] = 0; - out_852487606375420018[59] = 0; - out_852487606375420018[60] = 0; - out_852487606375420018[61] = 0; - out_852487606375420018[62] = 0; - out_852487606375420018[63] = 0; - out_852487606375420018[64] = 0; - out_852487606375420018[65] = 0; - out_852487606375420018[66] = dt; - out_852487606375420018[67] = 0; - out_852487606375420018[68] = 0; - out_852487606375420018[69] = 0; - out_852487606375420018[70] = 0; - out_852487606375420018[71] = 0; - out_852487606375420018[72] = 0; - out_852487606375420018[73] = 0; - out_852487606375420018[74] = 0; - out_852487606375420018[75] = 0; - out_852487606375420018[76] = 1; - out_852487606375420018[77] = 0; - out_852487606375420018[78] = 0; - out_852487606375420018[79] = 0; - out_852487606375420018[80] = 0; - out_852487606375420018[81] = 0; - out_852487606375420018[82] = 0; - out_852487606375420018[83] = 0; - out_852487606375420018[84] = 0; - out_852487606375420018[85] = dt; - out_852487606375420018[86] = 0; - out_852487606375420018[87] = 0; - out_852487606375420018[88] = 0; - out_852487606375420018[89] = 0; - out_852487606375420018[90] = 0; - out_852487606375420018[91] = 0; - out_852487606375420018[92] = 0; - out_852487606375420018[93] = 0; - out_852487606375420018[94] = 0; - out_852487606375420018[95] = 1; - out_852487606375420018[96] = 0; - out_852487606375420018[97] = 0; - out_852487606375420018[98] = 0; - out_852487606375420018[99] = 0; - out_852487606375420018[100] = 0; - out_852487606375420018[101] = 0; - out_852487606375420018[102] = 0; - out_852487606375420018[103] = 0; - out_852487606375420018[104] = dt; - out_852487606375420018[105] = 0; - out_852487606375420018[106] = 0; - out_852487606375420018[107] = 0; - out_852487606375420018[108] = 0; - out_852487606375420018[109] = 0; - out_852487606375420018[110] = 0; - out_852487606375420018[111] = 0; - out_852487606375420018[112] = 0; - out_852487606375420018[113] = 0; - out_852487606375420018[114] = 1; - out_852487606375420018[115] = 0; - out_852487606375420018[116] = 0; - out_852487606375420018[117] = 0; - out_852487606375420018[118] = 0; - out_852487606375420018[119] = 0; - out_852487606375420018[120] = 0; - out_852487606375420018[121] = 0; - out_852487606375420018[122] = 0; - out_852487606375420018[123] = 0; - out_852487606375420018[124] = 0; - out_852487606375420018[125] = 0; - out_852487606375420018[126] = 0; - out_852487606375420018[127] = 0; - out_852487606375420018[128] = 0; - out_852487606375420018[129] = 0; - out_852487606375420018[130] = 0; - out_852487606375420018[131] = 0; - out_852487606375420018[132] = 0; - out_852487606375420018[133] = 1; - out_852487606375420018[134] = 0; - out_852487606375420018[135] = 0; - out_852487606375420018[136] = 0; - out_852487606375420018[137] = 0; - out_852487606375420018[138] = 0; - out_852487606375420018[139] = 0; - out_852487606375420018[140] = 0; - out_852487606375420018[141] = 0; - out_852487606375420018[142] = 0; - out_852487606375420018[143] = 0; - out_852487606375420018[144] = 0; - out_852487606375420018[145] = 0; - out_852487606375420018[146] = 0; - out_852487606375420018[147] = 0; - out_852487606375420018[148] = 0; - out_852487606375420018[149] = 0; - out_852487606375420018[150] = 0; - out_852487606375420018[151] = 0; - out_852487606375420018[152] = 1; - out_852487606375420018[153] = 0; - out_852487606375420018[154] = 0; - out_852487606375420018[155] = 0; - out_852487606375420018[156] = 0; - out_852487606375420018[157] = 0; - out_852487606375420018[158] = 0; - out_852487606375420018[159] = 0; - out_852487606375420018[160] = 0; - out_852487606375420018[161] = 0; - out_852487606375420018[162] = 0; - out_852487606375420018[163] = 0; - out_852487606375420018[164] = 0; - out_852487606375420018[165] = 0; - out_852487606375420018[166] = 0; - out_852487606375420018[167] = 0; - out_852487606375420018[168] = 0; - out_852487606375420018[169] = 0; - out_852487606375420018[170] = 0; - out_852487606375420018[171] = 1; - out_852487606375420018[172] = 0; - out_852487606375420018[173] = 0; - out_852487606375420018[174] = 0; - out_852487606375420018[175] = 0; - out_852487606375420018[176] = 0; - out_852487606375420018[177] = 0; - out_852487606375420018[178] = 0; - out_852487606375420018[179] = 0; - out_852487606375420018[180] = 0; - out_852487606375420018[181] = 0; - out_852487606375420018[182] = 0; - out_852487606375420018[183] = 0; - out_852487606375420018[184] = 0; - out_852487606375420018[185] = 0; - out_852487606375420018[186] = 0; - out_852487606375420018[187] = 0; - out_852487606375420018[188] = 0; - out_852487606375420018[189] = 0; - out_852487606375420018[190] = 1; - out_852487606375420018[191] = 0; - out_852487606375420018[192] = 0; - out_852487606375420018[193] = 0; - out_852487606375420018[194] = 0; - out_852487606375420018[195] = 0; - out_852487606375420018[196] = 0; - out_852487606375420018[197] = 0; - out_852487606375420018[198] = 0; - out_852487606375420018[199] = 0; - out_852487606375420018[200] = 0; - out_852487606375420018[201] = 0; - out_852487606375420018[202] = 0; - out_852487606375420018[203] = 0; - out_852487606375420018[204] = 0; - out_852487606375420018[205] = 0; - out_852487606375420018[206] = 0; - out_852487606375420018[207] = 0; - out_852487606375420018[208] = 0; - out_852487606375420018[209] = 1; - out_852487606375420018[210] = 0; - out_852487606375420018[211] = 0; - out_852487606375420018[212] = 0; - out_852487606375420018[213] = 0; - out_852487606375420018[214] = 0; - out_852487606375420018[215] = 0; - out_852487606375420018[216] = 0; - out_852487606375420018[217] = 0; - out_852487606375420018[218] = 0; - out_852487606375420018[219] = 0; - out_852487606375420018[220] = 0; - out_852487606375420018[221] = 0; - out_852487606375420018[222] = 0; - out_852487606375420018[223] = 0; - out_852487606375420018[224] = 0; - out_852487606375420018[225] = 0; - out_852487606375420018[226] = 0; - out_852487606375420018[227] = 0; - out_852487606375420018[228] = 1; - out_852487606375420018[229] = 0; - out_852487606375420018[230] = 0; - out_852487606375420018[231] = 0; - out_852487606375420018[232] = 0; - out_852487606375420018[233] = 0; - out_852487606375420018[234] = 0; - out_852487606375420018[235] = 0; - out_852487606375420018[236] = 0; - out_852487606375420018[237] = 0; - out_852487606375420018[238] = 0; - out_852487606375420018[239] = 0; - out_852487606375420018[240] = 0; - out_852487606375420018[241] = 0; - out_852487606375420018[242] = 0; - out_852487606375420018[243] = 0; - out_852487606375420018[244] = 0; - out_852487606375420018[245] = 0; - out_852487606375420018[246] = 0; - out_852487606375420018[247] = 1; - out_852487606375420018[248] = 0; - out_852487606375420018[249] = 0; - out_852487606375420018[250] = 0; - out_852487606375420018[251] = 0; - out_852487606375420018[252] = 0; - out_852487606375420018[253] = 0; - out_852487606375420018[254] = 0; - out_852487606375420018[255] = 0; - out_852487606375420018[256] = 0; - out_852487606375420018[257] = 0; - out_852487606375420018[258] = 0; - out_852487606375420018[259] = 0; - out_852487606375420018[260] = 0; - out_852487606375420018[261] = 0; - out_852487606375420018[262] = 0; - out_852487606375420018[263] = 0; - out_852487606375420018[264] = 0; - out_852487606375420018[265] = 0; - out_852487606375420018[266] = 1; - out_852487606375420018[267] = 0; - out_852487606375420018[268] = 0; - out_852487606375420018[269] = 0; - out_852487606375420018[270] = 0; - out_852487606375420018[271] = 0; - out_852487606375420018[272] = 0; - out_852487606375420018[273] = 0; - out_852487606375420018[274] = 0; - out_852487606375420018[275] = 0; - out_852487606375420018[276] = 0; - out_852487606375420018[277] = 0; - out_852487606375420018[278] = 0; - out_852487606375420018[279] = 0; - out_852487606375420018[280] = 0; - out_852487606375420018[281] = 0; - out_852487606375420018[282] = 0; - out_852487606375420018[283] = 0; - out_852487606375420018[284] = 0; - out_852487606375420018[285] = 1; - out_852487606375420018[286] = 0; - out_852487606375420018[287] = 0; - out_852487606375420018[288] = 0; - out_852487606375420018[289] = 0; - out_852487606375420018[290] = 0; - out_852487606375420018[291] = 0; - out_852487606375420018[292] = 0; - out_852487606375420018[293] = 0; - out_852487606375420018[294] = 0; - out_852487606375420018[295] = 0; - out_852487606375420018[296] = 0; - out_852487606375420018[297] = 0; - out_852487606375420018[298] = 0; - out_852487606375420018[299] = 0; - out_852487606375420018[300] = 0; - out_852487606375420018[301] = 0; - out_852487606375420018[302] = 0; - out_852487606375420018[303] = 0; - out_852487606375420018[304] = 1; - out_852487606375420018[305] = 0; - out_852487606375420018[306] = 0; - out_852487606375420018[307] = 0; - out_852487606375420018[308] = 0; - out_852487606375420018[309] = 0; - out_852487606375420018[310] = 0; - out_852487606375420018[311] = 0; - out_852487606375420018[312] = 0; - out_852487606375420018[313] = 0; - out_852487606375420018[314] = 0; - out_852487606375420018[315] = 0; - out_852487606375420018[316] = 0; - out_852487606375420018[317] = 0; - out_852487606375420018[318] = 0; - out_852487606375420018[319] = 0; - out_852487606375420018[320] = 0; - out_852487606375420018[321] = 0; - out_852487606375420018[322] = 0; - out_852487606375420018[323] = 1; +void F_fun(double *state, double dt, double *out_7659025943100145673) { + out_7659025943100145673[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_7659025943100145673[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_7659025943100145673[2] = 0; + out_7659025943100145673[3] = 0; + out_7659025943100145673[4] = 0; + out_7659025943100145673[5] = 0; + out_7659025943100145673[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_7659025943100145673[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_7659025943100145673[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_7659025943100145673[9] = 0; + out_7659025943100145673[10] = 0; + out_7659025943100145673[11] = 0; + out_7659025943100145673[12] = 0; + out_7659025943100145673[13] = 0; + out_7659025943100145673[14] = 0; + out_7659025943100145673[15] = 0; + out_7659025943100145673[16] = 0; + out_7659025943100145673[17] = 0; + out_7659025943100145673[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_7659025943100145673[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_7659025943100145673[20] = 0; + out_7659025943100145673[21] = 0; + out_7659025943100145673[22] = 0; + out_7659025943100145673[23] = 0; + out_7659025943100145673[24] = 0; + out_7659025943100145673[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_7659025943100145673[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_7659025943100145673[27] = 0; + out_7659025943100145673[28] = 0; + out_7659025943100145673[29] = 0; + out_7659025943100145673[30] = 0; + out_7659025943100145673[31] = 0; + out_7659025943100145673[32] = 0; + out_7659025943100145673[33] = 0; + out_7659025943100145673[34] = 0; + out_7659025943100145673[35] = 0; + out_7659025943100145673[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_7659025943100145673[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_7659025943100145673[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_7659025943100145673[39] = 0; + out_7659025943100145673[40] = 0; + out_7659025943100145673[41] = 0; + out_7659025943100145673[42] = 0; + out_7659025943100145673[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_7659025943100145673[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_7659025943100145673[45] = 0; + out_7659025943100145673[46] = 0; + out_7659025943100145673[47] = 0; + out_7659025943100145673[48] = 0; + out_7659025943100145673[49] = 0; + out_7659025943100145673[50] = 0; + out_7659025943100145673[51] = 0; + out_7659025943100145673[52] = 0; + out_7659025943100145673[53] = 0; + out_7659025943100145673[54] = 0; + out_7659025943100145673[55] = 0; + out_7659025943100145673[56] = 0; + out_7659025943100145673[57] = 1; + out_7659025943100145673[58] = 0; + out_7659025943100145673[59] = 0; + out_7659025943100145673[60] = 0; + out_7659025943100145673[61] = 0; + out_7659025943100145673[62] = 0; + out_7659025943100145673[63] = 0; + out_7659025943100145673[64] = 0; + out_7659025943100145673[65] = 0; + out_7659025943100145673[66] = dt; + out_7659025943100145673[67] = 0; + out_7659025943100145673[68] = 0; + out_7659025943100145673[69] = 0; + out_7659025943100145673[70] = 0; + out_7659025943100145673[71] = 0; + out_7659025943100145673[72] = 0; + out_7659025943100145673[73] = 0; + out_7659025943100145673[74] = 0; + out_7659025943100145673[75] = 0; + out_7659025943100145673[76] = 1; + out_7659025943100145673[77] = 0; + out_7659025943100145673[78] = 0; + out_7659025943100145673[79] = 0; + out_7659025943100145673[80] = 0; + out_7659025943100145673[81] = 0; + out_7659025943100145673[82] = 0; + out_7659025943100145673[83] = 0; + out_7659025943100145673[84] = 0; + out_7659025943100145673[85] = dt; + out_7659025943100145673[86] = 0; + out_7659025943100145673[87] = 0; + out_7659025943100145673[88] = 0; + out_7659025943100145673[89] = 0; + out_7659025943100145673[90] = 0; + out_7659025943100145673[91] = 0; + out_7659025943100145673[92] = 0; + out_7659025943100145673[93] = 0; + out_7659025943100145673[94] = 0; + out_7659025943100145673[95] = 1; + out_7659025943100145673[96] = 0; + out_7659025943100145673[97] = 0; + out_7659025943100145673[98] = 0; + out_7659025943100145673[99] = 0; + out_7659025943100145673[100] = 0; + out_7659025943100145673[101] = 0; + out_7659025943100145673[102] = 0; + out_7659025943100145673[103] = 0; + out_7659025943100145673[104] = dt; + out_7659025943100145673[105] = 0; + out_7659025943100145673[106] = 0; + out_7659025943100145673[107] = 0; + out_7659025943100145673[108] = 0; + out_7659025943100145673[109] = 0; + out_7659025943100145673[110] = 0; + out_7659025943100145673[111] = 0; + out_7659025943100145673[112] = 0; + out_7659025943100145673[113] = 0; + out_7659025943100145673[114] = 1; + out_7659025943100145673[115] = 0; + out_7659025943100145673[116] = 0; + out_7659025943100145673[117] = 0; + out_7659025943100145673[118] = 0; + out_7659025943100145673[119] = 0; + out_7659025943100145673[120] = 0; + out_7659025943100145673[121] = 0; + out_7659025943100145673[122] = 0; + out_7659025943100145673[123] = 0; + out_7659025943100145673[124] = 0; + out_7659025943100145673[125] = 0; + out_7659025943100145673[126] = 0; + out_7659025943100145673[127] = 0; + out_7659025943100145673[128] = 0; + out_7659025943100145673[129] = 0; + out_7659025943100145673[130] = 0; + out_7659025943100145673[131] = 0; + out_7659025943100145673[132] = 0; + out_7659025943100145673[133] = 1; + out_7659025943100145673[134] = 0; + out_7659025943100145673[135] = 0; + out_7659025943100145673[136] = 0; + out_7659025943100145673[137] = 0; + out_7659025943100145673[138] = 0; + out_7659025943100145673[139] = 0; + out_7659025943100145673[140] = 0; + out_7659025943100145673[141] = 0; + out_7659025943100145673[142] = 0; + out_7659025943100145673[143] = 0; + out_7659025943100145673[144] = 0; + out_7659025943100145673[145] = 0; + out_7659025943100145673[146] = 0; + out_7659025943100145673[147] = 0; + out_7659025943100145673[148] = 0; + out_7659025943100145673[149] = 0; + out_7659025943100145673[150] = 0; + out_7659025943100145673[151] = 0; + out_7659025943100145673[152] = 1; + out_7659025943100145673[153] = 0; + out_7659025943100145673[154] = 0; + out_7659025943100145673[155] = 0; + out_7659025943100145673[156] = 0; + out_7659025943100145673[157] = 0; + out_7659025943100145673[158] = 0; + out_7659025943100145673[159] = 0; + out_7659025943100145673[160] = 0; + out_7659025943100145673[161] = 0; + out_7659025943100145673[162] = 0; + out_7659025943100145673[163] = 0; + out_7659025943100145673[164] = 0; + out_7659025943100145673[165] = 0; + out_7659025943100145673[166] = 0; + out_7659025943100145673[167] = 0; + out_7659025943100145673[168] = 0; + out_7659025943100145673[169] = 0; + out_7659025943100145673[170] = 0; + out_7659025943100145673[171] = 1; + out_7659025943100145673[172] = 0; + out_7659025943100145673[173] = 0; + out_7659025943100145673[174] = 0; + out_7659025943100145673[175] = 0; + out_7659025943100145673[176] = 0; + out_7659025943100145673[177] = 0; + out_7659025943100145673[178] = 0; + out_7659025943100145673[179] = 0; + out_7659025943100145673[180] = 0; + out_7659025943100145673[181] = 0; + out_7659025943100145673[182] = 0; + out_7659025943100145673[183] = 0; + out_7659025943100145673[184] = 0; + out_7659025943100145673[185] = 0; + out_7659025943100145673[186] = 0; + out_7659025943100145673[187] = 0; + out_7659025943100145673[188] = 0; + out_7659025943100145673[189] = 0; + out_7659025943100145673[190] = 1; + out_7659025943100145673[191] = 0; + out_7659025943100145673[192] = 0; + out_7659025943100145673[193] = 0; + out_7659025943100145673[194] = 0; + out_7659025943100145673[195] = 0; + out_7659025943100145673[196] = 0; + out_7659025943100145673[197] = 0; + out_7659025943100145673[198] = 0; + out_7659025943100145673[199] = 0; + out_7659025943100145673[200] = 0; + out_7659025943100145673[201] = 0; + out_7659025943100145673[202] = 0; + out_7659025943100145673[203] = 0; + out_7659025943100145673[204] = 0; + out_7659025943100145673[205] = 0; + out_7659025943100145673[206] = 0; + out_7659025943100145673[207] = 0; + out_7659025943100145673[208] = 0; + out_7659025943100145673[209] = 1; + out_7659025943100145673[210] = 0; + out_7659025943100145673[211] = 0; + out_7659025943100145673[212] = 0; + out_7659025943100145673[213] = 0; + out_7659025943100145673[214] = 0; + out_7659025943100145673[215] = 0; + out_7659025943100145673[216] = 0; + out_7659025943100145673[217] = 0; + out_7659025943100145673[218] = 0; + out_7659025943100145673[219] = 0; + out_7659025943100145673[220] = 0; + out_7659025943100145673[221] = 0; + out_7659025943100145673[222] = 0; + out_7659025943100145673[223] = 0; + out_7659025943100145673[224] = 0; + out_7659025943100145673[225] = 0; + out_7659025943100145673[226] = 0; + out_7659025943100145673[227] = 0; + out_7659025943100145673[228] = 1; + out_7659025943100145673[229] = 0; + out_7659025943100145673[230] = 0; + out_7659025943100145673[231] = 0; + out_7659025943100145673[232] = 0; + out_7659025943100145673[233] = 0; + out_7659025943100145673[234] = 0; + out_7659025943100145673[235] = 0; + out_7659025943100145673[236] = 0; + out_7659025943100145673[237] = 0; + out_7659025943100145673[238] = 0; + out_7659025943100145673[239] = 0; + out_7659025943100145673[240] = 0; + out_7659025943100145673[241] = 0; + out_7659025943100145673[242] = 0; + out_7659025943100145673[243] = 0; + out_7659025943100145673[244] = 0; + out_7659025943100145673[245] = 0; + out_7659025943100145673[246] = 0; + out_7659025943100145673[247] = 1; + out_7659025943100145673[248] = 0; + out_7659025943100145673[249] = 0; + out_7659025943100145673[250] = 0; + out_7659025943100145673[251] = 0; + out_7659025943100145673[252] = 0; + out_7659025943100145673[253] = 0; + out_7659025943100145673[254] = 0; + out_7659025943100145673[255] = 0; + out_7659025943100145673[256] = 0; + out_7659025943100145673[257] = 0; + out_7659025943100145673[258] = 0; + out_7659025943100145673[259] = 0; + out_7659025943100145673[260] = 0; + out_7659025943100145673[261] = 0; + out_7659025943100145673[262] = 0; + out_7659025943100145673[263] = 0; + out_7659025943100145673[264] = 0; + out_7659025943100145673[265] = 0; + out_7659025943100145673[266] = 1; + out_7659025943100145673[267] = 0; + out_7659025943100145673[268] = 0; + out_7659025943100145673[269] = 0; + out_7659025943100145673[270] = 0; + out_7659025943100145673[271] = 0; + out_7659025943100145673[272] = 0; + out_7659025943100145673[273] = 0; + out_7659025943100145673[274] = 0; + out_7659025943100145673[275] = 0; + out_7659025943100145673[276] = 0; + out_7659025943100145673[277] = 0; + out_7659025943100145673[278] = 0; + out_7659025943100145673[279] = 0; + out_7659025943100145673[280] = 0; + out_7659025943100145673[281] = 0; + out_7659025943100145673[282] = 0; + out_7659025943100145673[283] = 0; + out_7659025943100145673[284] = 0; + out_7659025943100145673[285] = 1; + out_7659025943100145673[286] = 0; + out_7659025943100145673[287] = 0; + out_7659025943100145673[288] = 0; + out_7659025943100145673[289] = 0; + out_7659025943100145673[290] = 0; + out_7659025943100145673[291] = 0; + out_7659025943100145673[292] = 0; + out_7659025943100145673[293] = 0; + out_7659025943100145673[294] = 0; + out_7659025943100145673[295] = 0; + out_7659025943100145673[296] = 0; + out_7659025943100145673[297] = 0; + out_7659025943100145673[298] = 0; + out_7659025943100145673[299] = 0; + out_7659025943100145673[300] = 0; + out_7659025943100145673[301] = 0; + out_7659025943100145673[302] = 0; + out_7659025943100145673[303] = 0; + out_7659025943100145673[304] = 1; + out_7659025943100145673[305] = 0; + out_7659025943100145673[306] = 0; + out_7659025943100145673[307] = 0; + out_7659025943100145673[308] = 0; + out_7659025943100145673[309] = 0; + out_7659025943100145673[310] = 0; + out_7659025943100145673[311] = 0; + out_7659025943100145673[312] = 0; + out_7659025943100145673[313] = 0; + out_7659025943100145673[314] = 0; + out_7659025943100145673[315] = 0; + out_7659025943100145673[316] = 0; + out_7659025943100145673[317] = 0; + out_7659025943100145673[318] = 0; + out_7659025943100145673[319] = 0; + out_7659025943100145673[320] = 0; + out_7659025943100145673[321] = 0; + out_7659025943100145673[322] = 0; + out_7659025943100145673[323] = 1; } -void h_4(double *state, double *unused, double *out_6027168379751608829) { - out_6027168379751608829[0] = state[6] + state[9]; - out_6027168379751608829[1] = state[7] + state[10]; - out_6027168379751608829[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_6376674584644624571) { + out_6376674584644624571[0] = state[6] + state[9]; + out_6376674584644624571[1] = state[7] + state[10]; + out_6376674584644624571[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_4209428867582108125) { - out_4209428867582108125[0] = 0; - out_4209428867582108125[1] = 0; - out_4209428867582108125[2] = 0; - out_4209428867582108125[3] = 0; - out_4209428867582108125[4] = 0; - out_4209428867582108125[5] = 0; - out_4209428867582108125[6] = 1; - out_4209428867582108125[7] = 0; - out_4209428867582108125[8] = 0; - out_4209428867582108125[9] = 1; - out_4209428867582108125[10] = 0; - out_4209428867582108125[11] = 0; - out_4209428867582108125[12] = 0; - out_4209428867582108125[13] = 0; - out_4209428867582108125[14] = 0; - out_4209428867582108125[15] = 0; - out_4209428867582108125[16] = 0; - out_4209428867582108125[17] = 0; - out_4209428867582108125[18] = 0; - out_4209428867582108125[19] = 0; - out_4209428867582108125[20] = 0; - out_4209428867582108125[21] = 0; - out_4209428867582108125[22] = 0; - out_4209428867582108125[23] = 0; - out_4209428867582108125[24] = 0; - out_4209428867582108125[25] = 1; - out_4209428867582108125[26] = 0; - out_4209428867582108125[27] = 0; - out_4209428867582108125[28] = 1; - out_4209428867582108125[29] = 0; - out_4209428867582108125[30] = 0; - out_4209428867582108125[31] = 0; - out_4209428867582108125[32] = 0; - out_4209428867582108125[33] = 0; - out_4209428867582108125[34] = 0; - out_4209428867582108125[35] = 0; - out_4209428867582108125[36] = 0; - out_4209428867582108125[37] = 0; - out_4209428867582108125[38] = 0; - out_4209428867582108125[39] = 0; - out_4209428867582108125[40] = 0; - out_4209428867582108125[41] = 0; - out_4209428867582108125[42] = 0; - out_4209428867582108125[43] = 0; - out_4209428867582108125[44] = 1; - out_4209428867582108125[45] = 0; - out_4209428867582108125[46] = 0; - out_4209428867582108125[47] = 1; - out_4209428867582108125[48] = 0; - out_4209428867582108125[49] = 0; - out_4209428867582108125[50] = 0; - out_4209428867582108125[51] = 0; - out_4209428867582108125[52] = 0; - out_4209428867582108125[53] = 0; +void H_4(double *state, double *unused, double *out_1315349575378995160) { + out_1315349575378995160[0] = 0; + out_1315349575378995160[1] = 0; + out_1315349575378995160[2] = 0; + out_1315349575378995160[3] = 0; + out_1315349575378995160[4] = 0; + out_1315349575378995160[5] = 0; + out_1315349575378995160[6] = 1; + out_1315349575378995160[7] = 0; + out_1315349575378995160[8] = 0; + out_1315349575378995160[9] = 1; + out_1315349575378995160[10] = 0; + out_1315349575378995160[11] = 0; + out_1315349575378995160[12] = 0; + out_1315349575378995160[13] = 0; + out_1315349575378995160[14] = 0; + out_1315349575378995160[15] = 0; + out_1315349575378995160[16] = 0; + out_1315349575378995160[17] = 0; + out_1315349575378995160[18] = 0; + out_1315349575378995160[19] = 0; + out_1315349575378995160[20] = 0; + out_1315349575378995160[21] = 0; + out_1315349575378995160[22] = 0; + out_1315349575378995160[23] = 0; + out_1315349575378995160[24] = 0; + out_1315349575378995160[25] = 1; + out_1315349575378995160[26] = 0; + out_1315349575378995160[27] = 0; + out_1315349575378995160[28] = 1; + out_1315349575378995160[29] = 0; + out_1315349575378995160[30] = 0; + out_1315349575378995160[31] = 0; + out_1315349575378995160[32] = 0; + out_1315349575378995160[33] = 0; + out_1315349575378995160[34] = 0; + out_1315349575378995160[35] = 0; + out_1315349575378995160[36] = 0; + out_1315349575378995160[37] = 0; + out_1315349575378995160[38] = 0; + out_1315349575378995160[39] = 0; + out_1315349575378995160[40] = 0; + out_1315349575378995160[41] = 0; + out_1315349575378995160[42] = 0; + out_1315349575378995160[43] = 0; + out_1315349575378995160[44] = 1; + out_1315349575378995160[45] = 0; + out_1315349575378995160[46] = 0; + out_1315349575378995160[47] = 1; + out_1315349575378995160[48] = 0; + out_1315349575378995160[49] = 0; + out_1315349575378995160[50] = 0; + out_1315349575378995160[51] = 0; + out_1315349575378995160[52] = 0; + out_1315349575378995160[53] = 0; } -void h_10(double *state, double *unused, double *out_744954405419605369) { - out_744954405419605369[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_744954405419605369[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_744954405419605369[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_7386007572913415364) { + out_7386007572913415364[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_7386007572913415364[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_7386007572913415364[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_4888049829332208881) { - out_4888049829332208881[0] = 0; - out_4888049829332208881[1] = 9.8100000000000005*cos(state[1]); - out_4888049829332208881[2] = 0; - out_4888049829332208881[3] = 0; - out_4888049829332208881[4] = -state[8]; - out_4888049829332208881[5] = state[7]; - out_4888049829332208881[6] = 0; - out_4888049829332208881[7] = state[5]; - out_4888049829332208881[8] = -state[4]; - out_4888049829332208881[9] = 0; - out_4888049829332208881[10] = 0; - out_4888049829332208881[11] = 0; - out_4888049829332208881[12] = 1; - out_4888049829332208881[13] = 0; - out_4888049829332208881[14] = 0; - out_4888049829332208881[15] = 1; - out_4888049829332208881[16] = 0; - out_4888049829332208881[17] = 0; - out_4888049829332208881[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_4888049829332208881[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_4888049829332208881[20] = 0; - out_4888049829332208881[21] = state[8]; - out_4888049829332208881[22] = 0; - out_4888049829332208881[23] = -state[6]; - out_4888049829332208881[24] = -state[5]; - out_4888049829332208881[25] = 0; - out_4888049829332208881[26] = state[3]; - out_4888049829332208881[27] = 0; - out_4888049829332208881[28] = 0; - out_4888049829332208881[29] = 0; - out_4888049829332208881[30] = 0; - out_4888049829332208881[31] = 1; - out_4888049829332208881[32] = 0; - out_4888049829332208881[33] = 0; - out_4888049829332208881[34] = 1; - out_4888049829332208881[35] = 0; - out_4888049829332208881[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_4888049829332208881[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_4888049829332208881[38] = 0; - out_4888049829332208881[39] = -state[7]; - out_4888049829332208881[40] = state[6]; - out_4888049829332208881[41] = 0; - out_4888049829332208881[42] = state[4]; - out_4888049829332208881[43] = -state[3]; - out_4888049829332208881[44] = 0; - out_4888049829332208881[45] = 0; - out_4888049829332208881[46] = 0; - out_4888049829332208881[47] = 0; - out_4888049829332208881[48] = 0; - out_4888049829332208881[49] = 0; - out_4888049829332208881[50] = 1; - out_4888049829332208881[51] = 0; - out_4888049829332208881[52] = 0; - out_4888049829332208881[53] = 1; +void H_10(double *state, double *unused, double *out_2432083188517445378) { + out_2432083188517445378[0] = 0; + out_2432083188517445378[1] = 9.8100000000000005*cos(state[1]); + out_2432083188517445378[2] = 0; + out_2432083188517445378[3] = 0; + out_2432083188517445378[4] = -state[8]; + out_2432083188517445378[5] = state[7]; + out_2432083188517445378[6] = 0; + out_2432083188517445378[7] = state[5]; + out_2432083188517445378[8] = -state[4]; + out_2432083188517445378[9] = 0; + out_2432083188517445378[10] = 0; + out_2432083188517445378[11] = 0; + out_2432083188517445378[12] = 1; + out_2432083188517445378[13] = 0; + out_2432083188517445378[14] = 0; + out_2432083188517445378[15] = 1; + out_2432083188517445378[16] = 0; + out_2432083188517445378[17] = 0; + out_2432083188517445378[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_2432083188517445378[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_2432083188517445378[20] = 0; + out_2432083188517445378[21] = state[8]; + out_2432083188517445378[22] = 0; + out_2432083188517445378[23] = -state[6]; + out_2432083188517445378[24] = -state[5]; + out_2432083188517445378[25] = 0; + out_2432083188517445378[26] = state[3]; + out_2432083188517445378[27] = 0; + out_2432083188517445378[28] = 0; + out_2432083188517445378[29] = 0; + out_2432083188517445378[30] = 0; + out_2432083188517445378[31] = 1; + out_2432083188517445378[32] = 0; + out_2432083188517445378[33] = 0; + out_2432083188517445378[34] = 1; + out_2432083188517445378[35] = 0; + out_2432083188517445378[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_2432083188517445378[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_2432083188517445378[38] = 0; + out_2432083188517445378[39] = -state[7]; + out_2432083188517445378[40] = state[6]; + out_2432083188517445378[41] = 0; + out_2432083188517445378[42] = state[4]; + out_2432083188517445378[43] = -state[3]; + out_2432083188517445378[44] = 0; + out_2432083188517445378[45] = 0; + out_2432083188517445378[46] = 0; + out_2432083188517445378[47] = 0; + out_2432083188517445378[48] = 0; + out_2432083188517445378[49] = 0; + out_2432083188517445378[50] = 1; + out_2432083188517445378[51] = 0; + out_2432083188517445378[52] = 0; + out_2432083188517445378[53] = 1; } -void h_13(double *state, double *unused, double *out_6652718215234119768) { - out_6652718215234119768[0] = state[3]; - out_6652718215234119768[1] = state[4]; - out_6652718215234119768[2] = state[5]; +void h_13(double *state, double *unused, double *out_2876319704878199880) { + out_2876319704878199880[0] = state[3]; + out_2876319704878199880[1] = state[4]; + out_2876319704878199880[2] = state[5]; } -void H_13(double *state, double *unused, double *out_997155042249775324) { - out_997155042249775324[0] = 0; - out_997155042249775324[1] = 0; - out_997155042249775324[2] = 0; - out_997155042249775324[3] = 1; - out_997155042249775324[4] = 0; - out_997155042249775324[5] = 0; - out_997155042249775324[6] = 0; - out_997155042249775324[7] = 0; - out_997155042249775324[8] = 0; - out_997155042249775324[9] = 0; - out_997155042249775324[10] = 0; - out_997155042249775324[11] = 0; - out_997155042249775324[12] = 0; - out_997155042249775324[13] = 0; - out_997155042249775324[14] = 0; - out_997155042249775324[15] = 0; - out_997155042249775324[16] = 0; - out_997155042249775324[17] = 0; - out_997155042249775324[18] = 0; - out_997155042249775324[19] = 0; - out_997155042249775324[20] = 0; - out_997155042249775324[21] = 0; - out_997155042249775324[22] = 1; - out_997155042249775324[23] = 0; - out_997155042249775324[24] = 0; - out_997155042249775324[25] = 0; - out_997155042249775324[26] = 0; - out_997155042249775324[27] = 0; - out_997155042249775324[28] = 0; - out_997155042249775324[29] = 0; - out_997155042249775324[30] = 0; - out_997155042249775324[31] = 0; - out_997155042249775324[32] = 0; - out_997155042249775324[33] = 0; - out_997155042249775324[34] = 0; - out_997155042249775324[35] = 0; - out_997155042249775324[36] = 0; - out_997155042249775324[37] = 0; - out_997155042249775324[38] = 0; - out_997155042249775324[39] = 0; - out_997155042249775324[40] = 0; - out_997155042249775324[41] = 1; - out_997155042249775324[42] = 0; - out_997155042249775324[43] = 0; - out_997155042249775324[44] = 0; - out_997155042249775324[45] = 0; - out_997155042249775324[46] = 0; - out_997155042249775324[47] = 0; - out_997155042249775324[48] = 0; - out_997155042249775324[49] = 0; - out_997155042249775324[50] = 0; - out_997155042249775324[51] = 0; - out_997155042249775324[52] = 0; - out_997155042249775324[53] = 0; +void H_13(double *state, double *unused, double *out_8925980783695696089) { + out_8925980783695696089[0] = 0; + out_8925980783695696089[1] = 0; + out_8925980783695696089[2] = 0; + out_8925980783695696089[3] = 1; + out_8925980783695696089[4] = 0; + out_8925980783695696089[5] = 0; + out_8925980783695696089[6] = 0; + out_8925980783695696089[7] = 0; + out_8925980783695696089[8] = 0; + out_8925980783695696089[9] = 0; + out_8925980783695696089[10] = 0; + out_8925980783695696089[11] = 0; + out_8925980783695696089[12] = 0; + out_8925980783695696089[13] = 0; + out_8925980783695696089[14] = 0; + out_8925980783695696089[15] = 0; + out_8925980783695696089[16] = 0; + out_8925980783695696089[17] = 0; + out_8925980783695696089[18] = 0; + out_8925980783695696089[19] = 0; + out_8925980783695696089[20] = 0; + out_8925980783695696089[21] = 0; + out_8925980783695696089[22] = 1; + out_8925980783695696089[23] = 0; + out_8925980783695696089[24] = 0; + out_8925980783695696089[25] = 0; + out_8925980783695696089[26] = 0; + out_8925980783695696089[27] = 0; + out_8925980783695696089[28] = 0; + out_8925980783695696089[29] = 0; + out_8925980783695696089[30] = 0; + out_8925980783695696089[31] = 0; + out_8925980783695696089[32] = 0; + out_8925980783695696089[33] = 0; + out_8925980783695696089[34] = 0; + out_8925980783695696089[35] = 0; + out_8925980783695696089[36] = 0; + out_8925980783695696089[37] = 0; + out_8925980783695696089[38] = 0; + out_8925980783695696089[39] = 0; + out_8925980783695696089[40] = 0; + out_8925980783695696089[41] = 1; + out_8925980783695696089[42] = 0; + out_8925980783695696089[43] = 0; + out_8925980783695696089[44] = 0; + out_8925980783695696089[45] = 0; + out_8925980783695696089[46] = 0; + out_8925980783695696089[47] = 0; + out_8925980783695696089[48] = 0; + out_8925980783695696089[49] = 0; + out_8925980783695696089[50] = 0; + out_8925980783695696089[51] = 0; + out_8925980783695696089[52] = 0; + out_8925980783695696089[53] = 0; } -void h_14(double *state, double *unused, double *out_7748768270616969343) { - out_7748768270616969343[0] = state[6]; - out_7748768270616969343[1] = state[7]; - out_7748768270616969343[2] = state[8]; +void h_14(double *state, double *unused, double *out_1255542748321681251) { + out_1255542748321681251[0] = state[6]; + out_1255542748321681251[1] = state[7]; + out_1255542748321681251[2] = state[8]; } -void H_14(double *state, double *unused, double *out_4644545394226991724) { - out_4644545394226991724[0] = 0; - out_4644545394226991724[1] = 0; - out_4644545394226991724[2] = 0; - out_4644545394226991724[3] = 0; - out_4644545394226991724[4] = 0; - out_4644545394226991724[5] = 0; - out_4644545394226991724[6] = 1; - out_4644545394226991724[7] = 0; - out_4644545394226991724[8] = 0; - out_4644545394226991724[9] = 0; - out_4644545394226991724[10] = 0; - out_4644545394226991724[11] = 0; - out_4644545394226991724[12] = 0; - out_4644545394226991724[13] = 0; - out_4644545394226991724[14] = 0; - out_4644545394226991724[15] = 0; - out_4644545394226991724[16] = 0; - out_4644545394226991724[17] = 0; - out_4644545394226991724[18] = 0; - out_4644545394226991724[19] = 0; - out_4644545394226991724[20] = 0; - out_4644545394226991724[21] = 0; - out_4644545394226991724[22] = 0; - out_4644545394226991724[23] = 0; - out_4644545394226991724[24] = 0; - out_4644545394226991724[25] = 1; - out_4644545394226991724[26] = 0; - out_4644545394226991724[27] = 0; - out_4644545394226991724[28] = 0; - out_4644545394226991724[29] = 0; - out_4644545394226991724[30] = 0; - out_4644545394226991724[31] = 0; - out_4644545394226991724[32] = 0; - out_4644545394226991724[33] = 0; - out_4644545394226991724[34] = 0; - out_4644545394226991724[35] = 0; - out_4644545394226991724[36] = 0; - out_4644545394226991724[37] = 0; - out_4644545394226991724[38] = 0; - out_4644545394226991724[39] = 0; - out_4644545394226991724[40] = 0; - out_4644545394226991724[41] = 0; - out_4644545394226991724[42] = 0; - out_4644545394226991724[43] = 0; - out_4644545394226991724[44] = 1; - out_4644545394226991724[45] = 0; - out_4644545394226991724[46] = 0; - out_4644545394226991724[47] = 0; - out_4644545394226991724[48] = 0; - out_4644545394226991724[49] = 0; - out_4644545394226991724[50] = 0; - out_4644545394226991724[51] = 0; - out_4644545394226991724[52] = 0; - out_4644545394226991724[53] = 0; +void H_14(double *state, double *unused, double *out_5278590431718479689) { + out_5278590431718479689[0] = 0; + out_5278590431718479689[1] = 0; + out_5278590431718479689[2] = 0; + out_5278590431718479689[3] = 0; + out_5278590431718479689[4] = 0; + out_5278590431718479689[5] = 0; + out_5278590431718479689[6] = 1; + out_5278590431718479689[7] = 0; + out_5278590431718479689[8] = 0; + out_5278590431718479689[9] = 0; + out_5278590431718479689[10] = 0; + out_5278590431718479689[11] = 0; + out_5278590431718479689[12] = 0; + out_5278590431718479689[13] = 0; + out_5278590431718479689[14] = 0; + out_5278590431718479689[15] = 0; + out_5278590431718479689[16] = 0; + out_5278590431718479689[17] = 0; + out_5278590431718479689[18] = 0; + out_5278590431718479689[19] = 0; + out_5278590431718479689[20] = 0; + out_5278590431718479689[21] = 0; + out_5278590431718479689[22] = 0; + out_5278590431718479689[23] = 0; + out_5278590431718479689[24] = 0; + out_5278590431718479689[25] = 1; + out_5278590431718479689[26] = 0; + out_5278590431718479689[27] = 0; + out_5278590431718479689[28] = 0; + out_5278590431718479689[29] = 0; + out_5278590431718479689[30] = 0; + out_5278590431718479689[31] = 0; + out_5278590431718479689[32] = 0; + out_5278590431718479689[33] = 0; + out_5278590431718479689[34] = 0; + out_5278590431718479689[35] = 0; + out_5278590431718479689[36] = 0; + out_5278590431718479689[37] = 0; + out_5278590431718479689[38] = 0; + out_5278590431718479689[39] = 0; + out_5278590431718479689[40] = 0; + out_5278590431718479689[41] = 0; + out_5278590431718479689[42] = 0; + out_5278590431718479689[43] = 0; + out_5278590431718479689[44] = 1; + out_5278590431718479689[45] = 0; + out_5278590431718479689[46] = 0; + out_5278590431718479689[47] = 0; + out_5278590431718479689[48] = 0; + out_5278590431718479689[49] = 0; + out_5278590431718479689[50] = 0; + out_5278590431718479689[51] = 0; + out_5278590431718479689[52] = 0; + out_5278590431718479689[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_309314752274053340) { - err_fun(nom_x, delta_x, out_309314752274053340); +void pose_err_fun(double *nom_x, double *delta_x, double *out_1282506072975981321) { + err_fun(nom_x, delta_x, out_1282506072975981321); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_651338713617187314) { - inv_err_fun(nom_x, true_x, out_651338713617187314); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_8512738075508223963) { + inv_err_fun(nom_x, true_x, out_8512738075508223963); } -void pose_H_mod_fun(double *state, double *out_1638652939169850596) { - H_mod_fun(state, out_1638652939169850596); +void pose_H_mod_fun(double *state, double *out_3135048301285292021) { + H_mod_fun(state, out_3135048301285292021); } -void pose_f_fun(double *state, double dt, double *out_2831397313375340905) { - f_fun(state, dt, out_2831397313375340905); +void pose_f_fun(double *state, double dt, double *out_8178969782644284561) { + f_fun(state, dt, out_8178969782644284561); } -void pose_F_fun(double *state, double dt, double *out_852487606375420018) { - F_fun(state, dt, out_852487606375420018); +void pose_F_fun(double *state, double dt, double *out_7659025943100145673) { + F_fun(state, dt, out_7659025943100145673); } -void pose_h_4(double *state, double *unused, double *out_6027168379751608829) { - h_4(state, unused, out_6027168379751608829); +void pose_h_4(double *state, double *unused, double *out_6376674584644624571) { + h_4(state, unused, out_6376674584644624571); } -void pose_H_4(double *state, double *unused, double *out_4209428867582108125) { - H_4(state, unused, out_4209428867582108125); +void pose_H_4(double *state, double *unused, double *out_1315349575378995160) { + H_4(state, unused, out_1315349575378995160); } -void pose_h_10(double *state, double *unused, double *out_744954405419605369) { - h_10(state, unused, out_744954405419605369); +void pose_h_10(double *state, double *unused, double *out_7386007572913415364) { + h_10(state, unused, out_7386007572913415364); } -void pose_H_10(double *state, double *unused, double *out_4888049829332208881) { - H_10(state, unused, out_4888049829332208881); +void pose_H_10(double *state, double *unused, double *out_2432083188517445378) { + H_10(state, unused, out_2432083188517445378); } -void pose_h_13(double *state, double *unused, double *out_6652718215234119768) { - h_13(state, unused, out_6652718215234119768); +void pose_h_13(double *state, double *unused, double *out_2876319704878199880) { + h_13(state, unused, out_2876319704878199880); } -void pose_H_13(double *state, double *unused, double *out_997155042249775324) { - H_13(state, unused, out_997155042249775324); +void pose_H_13(double *state, double *unused, double *out_8925980783695696089) { + H_13(state, unused, out_8925980783695696089); } -void pose_h_14(double *state, double *unused, double *out_7748768270616969343) { - h_14(state, unused, out_7748768270616969343); +void pose_h_14(double *state, double *unused, double *out_1255542748321681251) { + h_14(state, unused, out_1255542748321681251); } -void pose_H_14(double *state, double *unused, double *out_4644545394226991724) { - H_14(state, unused, out_4644545394226991724); +void pose_H_14(double *state, double *unused, double *out_5278590431718479689) { + H_14(state, unused, out_5278590431718479689); } 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 0b8b93d676..938ab6723e 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_309314752274053340); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_651338713617187314); -void pose_H_mod_fun(double *state, double *out_1638652939169850596); -void pose_f_fun(double *state, double dt, double *out_2831397313375340905); -void pose_F_fun(double *state, double dt, double *out_852487606375420018); -void pose_h_4(double *state, double *unused, double *out_6027168379751608829); -void pose_H_4(double *state, double *unused, double *out_4209428867582108125); -void pose_h_10(double *state, double *unused, double *out_744954405419605369); -void pose_H_10(double *state, double *unused, double *out_4888049829332208881); -void pose_h_13(double *state, double *unused, double *out_6652718215234119768); -void pose_H_13(double *state, double *unused, double *out_997155042249775324); -void pose_h_14(double *state, double *unused, double *out_7748768270616969343); -void pose_H_14(double *state, double *unused, double *out_4644545394226991724); +void pose_err_fun(double *nom_x, double *delta_x, double *out_1282506072975981321); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_8512738075508223963); +void pose_H_mod_fun(double *state, double *out_3135048301285292021); +void pose_f_fun(double *state, double dt, double *out_8178969782644284561); +void pose_F_fun(double *state, double dt, double *out_7659025943100145673); +void pose_h_4(double *state, double *unused, double *out_6376674584644624571); +void pose_H_4(double *state, double *unused, double *out_1315349575378995160); +void pose_h_10(double *state, double *unused, double *out_7386007572913415364); +void pose_H_10(double *state, double *unused, double *out_2432083188517445378); +void pose_h_13(double *state, double *unused, double *out_2876319704878199880); +void pose_H_13(double *state, double *unused, double *out_8925980783695696089); +void pose_h_14(double *state, double *unused, double *out_1255542748321681251); +void pose_H_14(double *state, double *unused, double *out_5278590431718479689); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file