diff --git a/cereal/messaging/bridge b/cereal/messaging/bridge index 3b87a0245..07cf908ae 100755 Binary files a/cereal/messaging/bridge and b/cereal/messaging/bridge differ diff --git a/common/params_pyx.so b/common/params_pyx.so index 837e8e4dc..3ceec96b2 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/common/transformations/transformations.so b/common/transformations/transformations.so index 614ed6ece..9146f53fa 100755 Binary files a/common/transformations/transformations.so and b/common/transformations/transformations.so differ diff --git a/msgq_repo/msgq/ipc_pyx.so b/msgq_repo/msgq/ipc_pyx.so index 86451bfd9..444bbdcc2 100755 Binary files a/msgq_repo/msgq/ipc_pyx.so and b/msgq_repo/msgq/ipc_pyx.so differ diff --git a/msgq_repo/msgq/visionipc/visionipc_pyx.so b/msgq_repo/msgq/visionipc/visionipc_pyx.so index 1dbc75e2e..6043be25b 100755 Binary files a/msgq_repo/msgq/visionipc/visionipc_pyx.so and b/msgq_repo/msgq/visionipc/visionipc_pyx.so differ diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index f009bea2a..98fa0c1ed 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 96f00992e..415f51a46 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 839528b6b..12edd2dd9 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 87f400c40..2aad31917 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 902bc4eff..7eb4f37ca 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 e5fe63554..f1ef0f3fa 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 e5fe63554..f1ef0f3fa 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 c209a86fb..e543af938 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 c209a86fb..e543af938 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 c209a86fb..e543af938 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 c209a86fb..e543af938 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 c209a86fb..e543af938 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 c209a86fb..e543af938 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 e5fe63554..f1ef0f3fa 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 e5fe63554..f1ef0f3fa 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 64427c184..8bb2df3af 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 e5fe63554..f1ef0f3fa 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 e5fe63554..f1ef0f3fa 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 062dbf9d5..94b6c8c61 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-284dbddd-DEBUG"; +const uint8_t gitversion[19] = "DEV-9263fd7c-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 00f7d3ec9..a8873c24a 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 8dedbe8cb..eb4df5227 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 5b9317882..85be10ad3 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 b54eb4466..b68d5e953 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 56820d15d..f489470ef 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 2ff71a4c2..96584fb6e 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 aa61bf778..dd9abd9fb 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 549dfcdf6..fea24de4e 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 1b4a93944..56309938e 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 379a90e4a..9b8044fbb 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 96578f1fe..75d67c3dc 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 fbbeaf339..2b7b28eba 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 146710a12..6b8fab128 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 d9921151c..4e97ce559 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 c125d4fba..0c464b668 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 e3ab8f496..632ac4665 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 f1cedc6a4..4394c412c 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 3900cd0bd..5f9bca708 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 29a5eb26c..27c60edaa 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 dadbec2e1..38a77e409 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 d216d1025..4e9716996 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 2f35bd7b7..b12cf4194 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 61e9b189b..36ad38dff 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 3153ed400..1bdaaccee 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 4bc4e74fb..1d364d8b1 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 c567eb16c..22bcb5057 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 2aafa51f3..cf5f2c690 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 98d942f50..addcf291e 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 53498bc8e..2f8d687d8 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 6028fe1f5..b1fbdc784 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 ad3759f56..fd928ee33 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 86b7e267d..2358d8d68 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 37efe2699..150c3b193 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 6d0f5fcce..bd44e4e84 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 11e4d2f67..d83066782 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 8ec6c3bf1..e8307bc41 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 8220a95f0..92b4e7aee 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 2ca32a469..64646aa6e 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 a2957d1d5..60c4b8f47 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 df0945e05..44aa92011 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 122939494..c817a6442 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 aaff27468..508b19abd 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 49d631f9c..74142d456 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 811af6800..304298807 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 765a4426f..0029735a4 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 742f6b93e..59034b30c 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 439b1ce15..6df2830ac 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 a9fdb4f49..ea09b5975 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 dc9033161..4d24dca62 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 fc8ea0561..330616d7a 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 bf5ab96de..5ff472533 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 0b79b2798..31a791e72 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 02a395bbb..672ff5c60 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-284dbddd-DEBUG \ No newline at end of file +DEV-9263fd7c-DEBUG \ No newline at end of file diff --git a/rednose_repo/rednose/helpers/ekf_sym_pyx.so b/rednose_repo/rednose/helpers/ekf_sym_pyx.so index 1105909f9..48c93417d 100755 Binary files a/rednose_repo/rednose/helpers/ekf_sym_pyx.so and b/rednose_repo/rednose/helpers/ekf_sym_pyx.so differ diff --git a/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_ocp_solver_pyx.so b/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_ocp_solver_pyx.so index 4755bd4a6..4044c7db4 100755 Binary files a/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_ocp_solver_pyx.so and b/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_ocp_solver_pyx.so differ diff --git a/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/libacados_ocp_solver_lat.so b/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/libacados_ocp_solver_lat.so index 8365b34c7..37e55488f 100755 Binary files a/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/libacados_ocp_solver_lat.so and b/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/libacados_ocp_solver_lat.so differ diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_ocp_solver_pyx.so b/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_ocp_solver_pyx.so index feb0708f7..0262c1dc5 100755 Binary files a/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_ocp_solver_pyx.so and b/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_ocp_solver_pyx.so differ diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/libacados_ocp_solver_long.so b/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/libacados_ocp_solver_long.so index 1d19fbe89..50b7ffba2 100755 Binary files a/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/libacados_ocp_solver_long.so and b/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/libacados_ocp_solver_long.so differ diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index aa401375a..3bee0965c 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_4436222918597397805) { - out_4436222918597397805[0] = delta_x[0] + nom_x[0]; - out_4436222918597397805[1] = delta_x[1] + nom_x[1]; - out_4436222918597397805[2] = delta_x[2] + nom_x[2]; - out_4436222918597397805[3] = delta_x[3] + nom_x[3]; - out_4436222918597397805[4] = delta_x[4] + nom_x[4]; - out_4436222918597397805[5] = delta_x[5] + nom_x[5]; - out_4436222918597397805[6] = delta_x[6] + nom_x[6]; - out_4436222918597397805[7] = delta_x[7] + nom_x[7]; - out_4436222918597397805[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_9105893539066631470) { + out_9105893539066631470[0] = delta_x[0] + nom_x[0]; + out_9105893539066631470[1] = delta_x[1] + nom_x[1]; + out_9105893539066631470[2] = delta_x[2] + nom_x[2]; + out_9105893539066631470[3] = delta_x[3] + nom_x[3]; + out_9105893539066631470[4] = delta_x[4] + nom_x[4]; + out_9105893539066631470[5] = delta_x[5] + nom_x[5]; + out_9105893539066631470[6] = delta_x[6] + nom_x[6]; + out_9105893539066631470[7] = delta_x[7] + nom_x[7]; + out_9105893539066631470[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_5455437002668972894) { - out_5455437002668972894[0] = -nom_x[0] + true_x[0]; - out_5455437002668972894[1] = -nom_x[1] + true_x[1]; - out_5455437002668972894[2] = -nom_x[2] + true_x[2]; - out_5455437002668972894[3] = -nom_x[3] + true_x[3]; - out_5455437002668972894[4] = -nom_x[4] + true_x[4]; - out_5455437002668972894[5] = -nom_x[5] + true_x[5]; - out_5455437002668972894[6] = -nom_x[6] + true_x[6]; - out_5455437002668972894[7] = -nom_x[7] + true_x[7]; - out_5455437002668972894[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_2201885575610208115) { + out_2201885575610208115[0] = -nom_x[0] + true_x[0]; + out_2201885575610208115[1] = -nom_x[1] + true_x[1]; + out_2201885575610208115[2] = -nom_x[2] + true_x[2]; + out_2201885575610208115[3] = -nom_x[3] + true_x[3]; + out_2201885575610208115[4] = -nom_x[4] + true_x[4]; + out_2201885575610208115[5] = -nom_x[5] + true_x[5]; + out_2201885575610208115[6] = -nom_x[6] + true_x[6]; + out_2201885575610208115[7] = -nom_x[7] + true_x[7]; + out_2201885575610208115[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_210375879520347438) { - out_210375879520347438[0] = 1.0; - out_210375879520347438[1] = 0.0; - out_210375879520347438[2] = 0.0; - out_210375879520347438[3] = 0.0; - out_210375879520347438[4] = 0.0; - out_210375879520347438[5] = 0.0; - out_210375879520347438[6] = 0.0; - out_210375879520347438[7] = 0.0; - out_210375879520347438[8] = 0.0; - out_210375879520347438[9] = 0.0; - out_210375879520347438[10] = 1.0; - out_210375879520347438[11] = 0.0; - out_210375879520347438[12] = 0.0; - out_210375879520347438[13] = 0.0; - out_210375879520347438[14] = 0.0; - out_210375879520347438[15] = 0.0; - out_210375879520347438[16] = 0.0; - out_210375879520347438[17] = 0.0; - out_210375879520347438[18] = 0.0; - out_210375879520347438[19] = 0.0; - out_210375879520347438[20] = 1.0; - out_210375879520347438[21] = 0.0; - out_210375879520347438[22] = 0.0; - out_210375879520347438[23] = 0.0; - out_210375879520347438[24] = 0.0; - out_210375879520347438[25] = 0.0; - out_210375879520347438[26] = 0.0; - out_210375879520347438[27] = 0.0; - out_210375879520347438[28] = 0.0; - out_210375879520347438[29] = 0.0; - out_210375879520347438[30] = 1.0; - out_210375879520347438[31] = 0.0; - out_210375879520347438[32] = 0.0; - out_210375879520347438[33] = 0.0; - out_210375879520347438[34] = 0.0; - out_210375879520347438[35] = 0.0; - out_210375879520347438[36] = 0.0; - out_210375879520347438[37] = 0.0; - out_210375879520347438[38] = 0.0; - out_210375879520347438[39] = 0.0; - out_210375879520347438[40] = 1.0; - out_210375879520347438[41] = 0.0; - out_210375879520347438[42] = 0.0; - out_210375879520347438[43] = 0.0; - out_210375879520347438[44] = 0.0; - out_210375879520347438[45] = 0.0; - out_210375879520347438[46] = 0.0; - out_210375879520347438[47] = 0.0; - out_210375879520347438[48] = 0.0; - out_210375879520347438[49] = 0.0; - out_210375879520347438[50] = 1.0; - out_210375879520347438[51] = 0.0; - out_210375879520347438[52] = 0.0; - out_210375879520347438[53] = 0.0; - out_210375879520347438[54] = 0.0; - out_210375879520347438[55] = 0.0; - out_210375879520347438[56] = 0.0; - out_210375879520347438[57] = 0.0; - out_210375879520347438[58] = 0.0; - out_210375879520347438[59] = 0.0; - out_210375879520347438[60] = 1.0; - out_210375879520347438[61] = 0.0; - out_210375879520347438[62] = 0.0; - out_210375879520347438[63] = 0.0; - out_210375879520347438[64] = 0.0; - out_210375879520347438[65] = 0.0; - out_210375879520347438[66] = 0.0; - out_210375879520347438[67] = 0.0; - out_210375879520347438[68] = 0.0; - out_210375879520347438[69] = 0.0; - out_210375879520347438[70] = 1.0; - out_210375879520347438[71] = 0.0; - out_210375879520347438[72] = 0.0; - out_210375879520347438[73] = 0.0; - out_210375879520347438[74] = 0.0; - out_210375879520347438[75] = 0.0; - out_210375879520347438[76] = 0.0; - out_210375879520347438[77] = 0.0; - out_210375879520347438[78] = 0.0; - out_210375879520347438[79] = 0.0; - out_210375879520347438[80] = 1.0; +void H_mod_fun(double *state, double *out_8823093076762317206) { + out_8823093076762317206[0] = 1.0; + out_8823093076762317206[1] = 0.0; + out_8823093076762317206[2] = 0.0; + out_8823093076762317206[3] = 0.0; + out_8823093076762317206[4] = 0.0; + out_8823093076762317206[5] = 0.0; + out_8823093076762317206[6] = 0.0; + out_8823093076762317206[7] = 0.0; + out_8823093076762317206[8] = 0.0; + out_8823093076762317206[9] = 0.0; + out_8823093076762317206[10] = 1.0; + out_8823093076762317206[11] = 0.0; + out_8823093076762317206[12] = 0.0; + out_8823093076762317206[13] = 0.0; + out_8823093076762317206[14] = 0.0; + out_8823093076762317206[15] = 0.0; + out_8823093076762317206[16] = 0.0; + out_8823093076762317206[17] = 0.0; + out_8823093076762317206[18] = 0.0; + out_8823093076762317206[19] = 0.0; + out_8823093076762317206[20] = 1.0; + out_8823093076762317206[21] = 0.0; + out_8823093076762317206[22] = 0.0; + out_8823093076762317206[23] = 0.0; + out_8823093076762317206[24] = 0.0; + out_8823093076762317206[25] = 0.0; + out_8823093076762317206[26] = 0.0; + out_8823093076762317206[27] = 0.0; + out_8823093076762317206[28] = 0.0; + out_8823093076762317206[29] = 0.0; + out_8823093076762317206[30] = 1.0; + out_8823093076762317206[31] = 0.0; + out_8823093076762317206[32] = 0.0; + out_8823093076762317206[33] = 0.0; + out_8823093076762317206[34] = 0.0; + out_8823093076762317206[35] = 0.0; + out_8823093076762317206[36] = 0.0; + out_8823093076762317206[37] = 0.0; + out_8823093076762317206[38] = 0.0; + out_8823093076762317206[39] = 0.0; + out_8823093076762317206[40] = 1.0; + out_8823093076762317206[41] = 0.0; + out_8823093076762317206[42] = 0.0; + out_8823093076762317206[43] = 0.0; + out_8823093076762317206[44] = 0.0; + out_8823093076762317206[45] = 0.0; + out_8823093076762317206[46] = 0.0; + out_8823093076762317206[47] = 0.0; + out_8823093076762317206[48] = 0.0; + out_8823093076762317206[49] = 0.0; + out_8823093076762317206[50] = 1.0; + out_8823093076762317206[51] = 0.0; + out_8823093076762317206[52] = 0.0; + out_8823093076762317206[53] = 0.0; + out_8823093076762317206[54] = 0.0; + out_8823093076762317206[55] = 0.0; + out_8823093076762317206[56] = 0.0; + out_8823093076762317206[57] = 0.0; + out_8823093076762317206[58] = 0.0; + out_8823093076762317206[59] = 0.0; + out_8823093076762317206[60] = 1.0; + out_8823093076762317206[61] = 0.0; + out_8823093076762317206[62] = 0.0; + out_8823093076762317206[63] = 0.0; + out_8823093076762317206[64] = 0.0; + out_8823093076762317206[65] = 0.0; + out_8823093076762317206[66] = 0.0; + out_8823093076762317206[67] = 0.0; + out_8823093076762317206[68] = 0.0; + out_8823093076762317206[69] = 0.0; + out_8823093076762317206[70] = 1.0; + out_8823093076762317206[71] = 0.0; + out_8823093076762317206[72] = 0.0; + out_8823093076762317206[73] = 0.0; + out_8823093076762317206[74] = 0.0; + out_8823093076762317206[75] = 0.0; + out_8823093076762317206[76] = 0.0; + out_8823093076762317206[77] = 0.0; + out_8823093076762317206[78] = 0.0; + out_8823093076762317206[79] = 0.0; + out_8823093076762317206[80] = 1.0; } -void f_fun(double *state, double dt, double *out_467846126367206141) { - out_467846126367206141[0] = state[0]; - out_467846126367206141[1] = state[1]; - out_467846126367206141[2] = state[2]; - out_467846126367206141[3] = state[3]; - out_467846126367206141[4] = state[4]; - out_467846126367206141[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_467846126367206141[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_467846126367206141[7] = state[7]; - out_467846126367206141[8] = state[8]; +void f_fun(double *state, double dt, double *out_8757684582265239992) { + out_8757684582265239992[0] = state[0]; + out_8757684582265239992[1] = state[1]; + out_8757684582265239992[2] = state[2]; + out_8757684582265239992[3] = state[3]; + out_8757684582265239992[4] = state[4]; + out_8757684582265239992[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_8757684582265239992[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_8757684582265239992[7] = state[7]; + out_8757684582265239992[8] = state[8]; } -void F_fun(double *state, double dt, double *out_3146439140780538523) { - out_3146439140780538523[0] = 1; - out_3146439140780538523[1] = 0; - out_3146439140780538523[2] = 0; - out_3146439140780538523[3] = 0; - out_3146439140780538523[4] = 0; - out_3146439140780538523[5] = 0; - out_3146439140780538523[6] = 0; - out_3146439140780538523[7] = 0; - out_3146439140780538523[8] = 0; - out_3146439140780538523[9] = 0; - out_3146439140780538523[10] = 1; - out_3146439140780538523[11] = 0; - out_3146439140780538523[12] = 0; - out_3146439140780538523[13] = 0; - out_3146439140780538523[14] = 0; - out_3146439140780538523[15] = 0; - out_3146439140780538523[16] = 0; - out_3146439140780538523[17] = 0; - out_3146439140780538523[18] = 0; - out_3146439140780538523[19] = 0; - out_3146439140780538523[20] = 1; - out_3146439140780538523[21] = 0; - out_3146439140780538523[22] = 0; - out_3146439140780538523[23] = 0; - out_3146439140780538523[24] = 0; - out_3146439140780538523[25] = 0; - out_3146439140780538523[26] = 0; - out_3146439140780538523[27] = 0; - out_3146439140780538523[28] = 0; - out_3146439140780538523[29] = 0; - out_3146439140780538523[30] = 1; - out_3146439140780538523[31] = 0; - out_3146439140780538523[32] = 0; - out_3146439140780538523[33] = 0; - out_3146439140780538523[34] = 0; - out_3146439140780538523[35] = 0; - out_3146439140780538523[36] = 0; - out_3146439140780538523[37] = 0; - out_3146439140780538523[38] = 0; - out_3146439140780538523[39] = 0; - out_3146439140780538523[40] = 1; - out_3146439140780538523[41] = 0; - out_3146439140780538523[42] = 0; - out_3146439140780538523[43] = 0; - out_3146439140780538523[44] = 0; - out_3146439140780538523[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_3146439140780538523[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_3146439140780538523[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_3146439140780538523[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_3146439140780538523[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_3146439140780538523[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_3146439140780538523[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_3146439140780538523[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_3146439140780538523[53] = -9.8100000000000005*dt; - out_3146439140780538523[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_3146439140780538523[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_3146439140780538523[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_3146439140780538523[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_3146439140780538523[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_3146439140780538523[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_3146439140780538523[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_3146439140780538523[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_3146439140780538523[62] = 0; - out_3146439140780538523[63] = 0; - out_3146439140780538523[64] = 0; - out_3146439140780538523[65] = 0; - out_3146439140780538523[66] = 0; - out_3146439140780538523[67] = 0; - out_3146439140780538523[68] = 0; - out_3146439140780538523[69] = 0; - out_3146439140780538523[70] = 1; - out_3146439140780538523[71] = 0; - out_3146439140780538523[72] = 0; - out_3146439140780538523[73] = 0; - out_3146439140780538523[74] = 0; - out_3146439140780538523[75] = 0; - out_3146439140780538523[76] = 0; - out_3146439140780538523[77] = 0; - out_3146439140780538523[78] = 0; - out_3146439140780538523[79] = 0; - out_3146439140780538523[80] = 1; +void F_fun(double *state, double dt, double *out_2903520854157127208) { + out_2903520854157127208[0] = 1; + out_2903520854157127208[1] = 0; + out_2903520854157127208[2] = 0; + out_2903520854157127208[3] = 0; + out_2903520854157127208[4] = 0; + out_2903520854157127208[5] = 0; + out_2903520854157127208[6] = 0; + out_2903520854157127208[7] = 0; + out_2903520854157127208[8] = 0; + out_2903520854157127208[9] = 0; + out_2903520854157127208[10] = 1; + out_2903520854157127208[11] = 0; + out_2903520854157127208[12] = 0; + out_2903520854157127208[13] = 0; + out_2903520854157127208[14] = 0; + out_2903520854157127208[15] = 0; + out_2903520854157127208[16] = 0; + out_2903520854157127208[17] = 0; + out_2903520854157127208[18] = 0; + out_2903520854157127208[19] = 0; + out_2903520854157127208[20] = 1; + out_2903520854157127208[21] = 0; + out_2903520854157127208[22] = 0; + out_2903520854157127208[23] = 0; + out_2903520854157127208[24] = 0; + out_2903520854157127208[25] = 0; + out_2903520854157127208[26] = 0; + out_2903520854157127208[27] = 0; + out_2903520854157127208[28] = 0; + out_2903520854157127208[29] = 0; + out_2903520854157127208[30] = 1; + out_2903520854157127208[31] = 0; + out_2903520854157127208[32] = 0; + out_2903520854157127208[33] = 0; + out_2903520854157127208[34] = 0; + out_2903520854157127208[35] = 0; + out_2903520854157127208[36] = 0; + out_2903520854157127208[37] = 0; + out_2903520854157127208[38] = 0; + out_2903520854157127208[39] = 0; + out_2903520854157127208[40] = 1; + out_2903520854157127208[41] = 0; + out_2903520854157127208[42] = 0; + out_2903520854157127208[43] = 0; + out_2903520854157127208[44] = 0; + out_2903520854157127208[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_2903520854157127208[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_2903520854157127208[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_2903520854157127208[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_2903520854157127208[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_2903520854157127208[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_2903520854157127208[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_2903520854157127208[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_2903520854157127208[53] = -9.8100000000000005*dt; + out_2903520854157127208[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_2903520854157127208[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_2903520854157127208[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2903520854157127208[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2903520854157127208[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_2903520854157127208[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_2903520854157127208[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_2903520854157127208[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2903520854157127208[62] = 0; + out_2903520854157127208[63] = 0; + out_2903520854157127208[64] = 0; + out_2903520854157127208[65] = 0; + out_2903520854157127208[66] = 0; + out_2903520854157127208[67] = 0; + out_2903520854157127208[68] = 0; + out_2903520854157127208[69] = 0; + out_2903520854157127208[70] = 1; + out_2903520854157127208[71] = 0; + out_2903520854157127208[72] = 0; + out_2903520854157127208[73] = 0; + out_2903520854157127208[74] = 0; + out_2903520854157127208[75] = 0; + out_2903520854157127208[76] = 0; + out_2903520854157127208[77] = 0; + out_2903520854157127208[78] = 0; + out_2903520854157127208[79] = 0; + out_2903520854157127208[80] = 1; } -void h_25(double *state, double *unused, double *out_3524127915259881655) { - out_3524127915259881655[0] = state[6]; +void h_25(double *state, double *unused, double *out_6353487808218647336) { + out_6353487808218647336[0] = state[6]; } -void H_25(double *state, double *unused, double *out_3733424518165712586) { - out_3733424518165712586[0] = 0; - out_3733424518165712586[1] = 0; - out_3733424518165712586[2] = 0; - out_3733424518165712586[3] = 0; - out_3733424518165712586[4] = 0; - out_3733424518165712586[5] = 0; - out_3733424518165712586[6] = 1; - out_3733424518165712586[7] = 0; - out_3733424518165712586[8] = 0; +void H_25(double *state, double *unused, double *out_430462771034997173) { + out_430462771034997173[0] = 0; + out_430462771034997173[1] = 0; + out_430462771034997173[2] = 0; + out_430462771034997173[3] = 0; + out_430462771034997173[4] = 0; + out_430462771034997173[5] = 0; + out_430462771034997173[6] = 1; + out_430462771034997173[7] = 0; + out_430462771034997173[8] = 0; } -void h_24(double *state, double *unused, double *out_5258226883159248845) { - out_5258226883159248845[0] = state[4]; - out_5258226883159248845[1] = state[5]; +void h_24(double *state, double *unused, double *out_276973526775545327) { + out_276973526775545327[0] = state[4]; + out_276973526775545327[1] = state[5]; } -void H_24(double *state, double *unused, double *out_2842147288425805515) { - out_2842147288425805515[0] = 0; - out_2842147288425805515[1] = 0; - out_2842147288425805515[2] = 0; - out_2842147288425805515[3] = 0; - out_2842147288425805515[4] = 1; - out_2842147288425805515[5] = 0; - out_2842147288425805515[6] = 0; - out_2842147288425805515[7] = 0; - out_2842147288425805515[8] = 0; - out_2842147288425805515[9] = 0; - out_2842147288425805515[10] = 0; - out_2842147288425805515[11] = 0; - out_2842147288425805515[12] = 0; - out_2842147288425805515[13] = 0; - out_2842147288425805515[14] = 1; - out_2842147288425805515[15] = 0; - out_2842147288425805515[16] = 0; - out_2842147288425805515[17] = 0; +void H_24(double *state, double *unused, double *out_1742186827970502393) { + out_1742186827970502393[0] = 0; + out_1742186827970502393[1] = 0; + out_1742186827970502393[2] = 0; + out_1742186827970502393[3] = 0; + out_1742186827970502393[4] = 1; + out_1742186827970502393[5] = 0; + out_1742186827970502393[6] = 0; + out_1742186827970502393[7] = 0; + out_1742186827970502393[8] = 0; + out_1742186827970502393[9] = 0; + out_1742186827970502393[10] = 0; + out_1742186827970502393[11] = 0; + out_1742186827970502393[12] = 0; + out_1742186827970502393[13] = 0; + out_1742186827970502393[14] = 1; + out_1742186827970502393[15] = 0; + out_1742186827970502393[16] = 0; + out_1742186827970502393[17] = 0; } -void h_30(double *state, double *unused, double *out_2922709757646518101) { - out_2922709757646518101[0] = state[4]; +void h_30(double *state, double *unused, double *out_2255184509939275472) { + out_2255184509939275472[0] = state[4]; } -void H_30(double *state, double *unused, double *out_3183265823325904169) { - out_3183265823325904169[0] = 0; - out_3183265823325904169[1] = 0; - out_3183265823325904169[2] = 0; - out_3183265823325904169[3] = 0; - out_3183265823325904169[4] = 1; - out_3183265823325904169[5] = 0; - out_3183265823325904169[6] = 0; - out_3183265823325904169[7] = 0; - out_3183265823325904169[8] = 0; +void H_30(double *state, double *unused, double *out_2948795729542245800) { + out_2948795729542245800[0] = 0; + out_2948795729542245800[1] = 0; + out_2948795729542245800[2] = 0; + out_2948795729542245800[3] = 0; + out_2948795729542245800[4] = 1; + out_2948795729542245800[5] = 0; + out_2948795729542245800[6] = 0; + out_2948795729542245800[7] = 0; + out_2948795729542245800[8] = 0; } -void h_26(double *state, double *unused, double *out_2078963510014327544) { - out_2078963510014327544[0] = state[7]; +void h_26(double *state, double *unused, double *out_1273100416603977360) { + out_1273100416603977360[0] = state[7]; } -void H_26(double *state, double *unused, double *out_7474927837039768810) { - out_7474927837039768810[0] = 0; - out_7474927837039768810[1] = 0; - out_7474927837039768810[2] = 0; - out_7474927837039768810[3] = 0; - out_7474927837039768810[4] = 0; - out_7474927837039768810[5] = 0; - out_7474927837039768810[6] = 0; - out_7474927837039768810[7] = 1; - out_7474927837039768810[8] = 0; +void H_26(double *state, double *unused, double *out_3734988740795797774) { + out_3734988740795797774[0] = 0; + out_3734988740795797774[1] = 0; + out_3734988740795797774[2] = 0; + out_3734988740795797774[3] = 0; + out_3734988740795797774[4] = 0; + out_3734988740795797774[5] = 0; + out_3734988740795797774[6] = 0; + out_3734988740795797774[7] = 1; + out_3734988740795797774[8] = 0; } -void h_27(double *state, double *unused, double *out_9006985404715807625) { - out_9006985404715807625[0] = state[3]; +void h_27(double *state, double *unused, double *out_2052169011425579317) { + out_2052169011425579317[0] = state[3]; } -void H_27(double *state, double *unused, double *out_1008502511525479258) { - out_1008502511525479258[0] = 0; - out_1008502511525479258[1] = 0; - out_1008502511525479258[2] = 0; - out_1008502511525479258[3] = 1; - out_1008502511525479258[4] = 0; - out_1008502511525479258[5] = 0; - out_1008502511525479258[6] = 0; - out_1008502511525479258[7] = 0; - out_1008502511525479258[8] = 0; +void H_27(double *state, double *unused, double *out_774032417741820889) { + out_774032417741820889[0] = 0; + out_774032417741820889[1] = 0; + out_774032417741820889[2] = 0; + out_774032417741820889[3] = 1; + out_774032417741820889[4] = 0; + out_774032417741820889[5] = 0; + out_774032417741820889[6] = 0; + out_774032417741820889[7] = 0; + out_774032417741820889[8] = 0; } -void h_29(double *state, double *unused, double *out_1979199904744855666) { - out_1979199904744855666[0] = state[1]; +void h_29(double *state, double *unused, double *out_7256857777600121117) { + out_7256857777600121117[0] = state[1]; } -void H_29(double *state, double *unused, double *out_704860215344071775) { - out_704860215344071775[0] = 0; - out_704860215344071775[1] = 1; - out_704860215344071775[2] = 0; - out_704860215344071775[3] = 0; - out_704860215344071775[4] = 0; - out_704860215344071775[5] = 0; - out_704860215344071775[6] = 0; - out_704860215344071775[7] = 0; - out_704860215344071775[8] = 0; +void H_29(double *state, double *unused, double *out_3459027073856637984) { + out_3459027073856637984[0] = 0; + out_3459027073856637984[1] = 1; + out_3459027073856637984[2] = 0; + out_3459027073856637984[3] = 0; + out_3459027073856637984[4] = 0; + out_3459027073856637984[5] = 0; + out_3459027073856637984[6] = 0; + out_3459027073856637984[7] = 0; + out_3459027073856637984[8] = 0; } -void h_28(double *state, double *unused, double *out_6835353348185046340) { - out_6835353348185046340[0] = state[0]; +void h_28(double *state, double *unused, double *out_2373191903872652216) { + out_2373191903872652216[0] = state[0]; } -void H_28(double *state, double *unused, double *out_5787259232413602349) { - out_5787259232413602349[0] = 1; - out_5787259232413602349[1] = 0; - out_5787259232413602349[2] = 0; - out_5787259232413602349[3] = 0; - out_5787259232413602349[4] = 0; - out_5787259232413602349[5] = 0; - out_5787259232413602349[6] = 0; - out_5787259232413602349[7] = 0; - out_5787259232413602349[8] = 0; +void H_28(double *state, double *unused, double *out_1623371943212892590) { + out_1623371943212892590[0] = 1; + out_1623371943212892590[1] = 0; + out_1623371943212892590[2] = 0; + out_1623371943212892590[3] = 0; + out_1623371943212892590[4] = 0; + out_1623371943212892590[5] = 0; + out_1623371943212892590[6] = 0; + out_1623371943212892590[7] = 0; + out_1623371943212892590[8] = 0; } -void h_31(double *state, double *unused, double *out_4974759307888764822) { - out_4974759307888764822[0] = state[8]; +void h_31(double *state, double *unused, double *out_6078293745934141447) { + out_6078293745934141447[0] = state[8]; } -void H_31(double *state, double *unused, double *out_3702778556288752158) { - out_3702778556288752158[0] = 0; - out_3702778556288752158[1] = 0; - out_3702778556288752158[2] = 0; - out_3702778556288752158[3] = 0; - out_3702778556288752158[4] = 0; - out_3702778556288752158[5] = 0; - out_3702778556288752158[6] = 0; - out_3702778556288752158[7] = 0; - out_3702778556288752158[8] = 1; +void H_31(double *state, double *unused, double *out_3108780638562446298) { + out_3108780638562446298[0] = 0; + out_3108780638562446298[1] = 0; + out_3108780638562446298[2] = 0; + out_3108780638562446298[3] = 0; + out_3108780638562446298[4] = 0; + out_3108780638562446298[5] = 0; + out_3108780638562446298[6] = 0; + out_3108780638562446298[7] = 0; + out_3108780638562446298[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_4436222918597397805) { - err_fun(nom_x, delta_x, out_4436222918597397805); +void car_err_fun(double *nom_x, double *delta_x, double *out_9105893539066631470) { + err_fun(nom_x, delta_x, out_9105893539066631470); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_5455437002668972894) { - inv_err_fun(nom_x, true_x, out_5455437002668972894); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_2201885575610208115) { + inv_err_fun(nom_x, true_x, out_2201885575610208115); } -void car_H_mod_fun(double *state, double *out_210375879520347438) { - H_mod_fun(state, out_210375879520347438); +void car_H_mod_fun(double *state, double *out_8823093076762317206) { + H_mod_fun(state, out_8823093076762317206); } -void car_f_fun(double *state, double dt, double *out_467846126367206141) { - f_fun(state, dt, out_467846126367206141); +void car_f_fun(double *state, double dt, double *out_8757684582265239992) { + f_fun(state, dt, out_8757684582265239992); } -void car_F_fun(double *state, double dt, double *out_3146439140780538523) { - F_fun(state, dt, out_3146439140780538523); +void car_F_fun(double *state, double dt, double *out_2903520854157127208) { + F_fun(state, dt, out_2903520854157127208); } -void car_h_25(double *state, double *unused, double *out_3524127915259881655) { - h_25(state, unused, out_3524127915259881655); +void car_h_25(double *state, double *unused, double *out_6353487808218647336) { + h_25(state, unused, out_6353487808218647336); } -void car_H_25(double *state, double *unused, double *out_3733424518165712586) { - H_25(state, unused, out_3733424518165712586); +void car_H_25(double *state, double *unused, double *out_430462771034997173) { + H_25(state, unused, out_430462771034997173); } -void car_h_24(double *state, double *unused, double *out_5258226883159248845) { - h_24(state, unused, out_5258226883159248845); +void car_h_24(double *state, double *unused, double *out_276973526775545327) { + h_24(state, unused, out_276973526775545327); } -void car_H_24(double *state, double *unused, double *out_2842147288425805515) { - H_24(state, unused, out_2842147288425805515); +void car_H_24(double *state, double *unused, double *out_1742186827970502393) { + H_24(state, unused, out_1742186827970502393); } -void car_h_30(double *state, double *unused, double *out_2922709757646518101) { - h_30(state, unused, out_2922709757646518101); +void car_h_30(double *state, double *unused, double *out_2255184509939275472) { + h_30(state, unused, out_2255184509939275472); } -void car_H_30(double *state, double *unused, double *out_3183265823325904169) { - H_30(state, unused, out_3183265823325904169); +void car_H_30(double *state, double *unused, double *out_2948795729542245800) { + H_30(state, unused, out_2948795729542245800); } -void car_h_26(double *state, double *unused, double *out_2078963510014327544) { - h_26(state, unused, out_2078963510014327544); +void car_h_26(double *state, double *unused, double *out_1273100416603977360) { + h_26(state, unused, out_1273100416603977360); } -void car_H_26(double *state, double *unused, double *out_7474927837039768810) { - H_26(state, unused, out_7474927837039768810); +void car_H_26(double *state, double *unused, double *out_3734988740795797774) { + H_26(state, unused, out_3734988740795797774); } -void car_h_27(double *state, double *unused, double *out_9006985404715807625) { - h_27(state, unused, out_9006985404715807625); +void car_h_27(double *state, double *unused, double *out_2052169011425579317) { + h_27(state, unused, out_2052169011425579317); } -void car_H_27(double *state, double *unused, double *out_1008502511525479258) { - H_27(state, unused, out_1008502511525479258); +void car_H_27(double *state, double *unused, double *out_774032417741820889) { + H_27(state, unused, out_774032417741820889); } -void car_h_29(double *state, double *unused, double *out_1979199904744855666) { - h_29(state, unused, out_1979199904744855666); +void car_h_29(double *state, double *unused, double *out_7256857777600121117) { + h_29(state, unused, out_7256857777600121117); } -void car_H_29(double *state, double *unused, double *out_704860215344071775) { - H_29(state, unused, out_704860215344071775); +void car_H_29(double *state, double *unused, double *out_3459027073856637984) { + H_29(state, unused, out_3459027073856637984); } -void car_h_28(double *state, double *unused, double *out_6835353348185046340) { - h_28(state, unused, out_6835353348185046340); +void car_h_28(double *state, double *unused, double *out_2373191903872652216) { + h_28(state, unused, out_2373191903872652216); } -void car_H_28(double *state, double *unused, double *out_5787259232413602349) { - H_28(state, unused, out_5787259232413602349); +void car_H_28(double *state, double *unused, double *out_1623371943212892590) { + H_28(state, unused, out_1623371943212892590); } -void car_h_31(double *state, double *unused, double *out_4974759307888764822) { - h_31(state, unused, out_4974759307888764822); +void car_h_31(double *state, double *unused, double *out_6078293745934141447) { + h_31(state, unused, out_6078293745934141447); } -void car_H_31(double *state, double *unused, double *out_3702778556288752158) { - H_31(state, unused, out_3702778556288752158); +void car_H_31(double *state, double *unused, double *out_3108780638562446298) { + H_31(state, unused, out_3108780638562446298); } 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 e3972d4d2..e12c02907 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_4436222918597397805); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_5455437002668972894); -void car_H_mod_fun(double *state, double *out_210375879520347438); -void car_f_fun(double *state, double dt, double *out_467846126367206141); -void car_F_fun(double *state, double dt, double *out_3146439140780538523); -void car_h_25(double *state, double *unused, double *out_3524127915259881655); -void car_H_25(double *state, double *unused, double *out_3733424518165712586); -void car_h_24(double *state, double *unused, double *out_5258226883159248845); -void car_H_24(double *state, double *unused, double *out_2842147288425805515); -void car_h_30(double *state, double *unused, double *out_2922709757646518101); -void car_H_30(double *state, double *unused, double *out_3183265823325904169); -void car_h_26(double *state, double *unused, double *out_2078963510014327544); -void car_H_26(double *state, double *unused, double *out_7474927837039768810); -void car_h_27(double *state, double *unused, double *out_9006985404715807625); -void car_H_27(double *state, double *unused, double *out_1008502511525479258); -void car_h_29(double *state, double *unused, double *out_1979199904744855666); -void car_H_29(double *state, double *unused, double *out_704860215344071775); -void car_h_28(double *state, double *unused, double *out_6835353348185046340); -void car_H_28(double *state, double *unused, double *out_5787259232413602349); -void car_h_31(double *state, double *unused, double *out_4974759307888764822); -void car_H_31(double *state, double *unused, double *out_3702778556288752158); +void car_err_fun(double *nom_x, double *delta_x, double *out_9105893539066631470); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_2201885575610208115); +void car_H_mod_fun(double *state, double *out_8823093076762317206); +void car_f_fun(double *state, double dt, double *out_8757684582265239992); +void car_F_fun(double *state, double dt, double *out_2903520854157127208); +void car_h_25(double *state, double *unused, double *out_6353487808218647336); +void car_H_25(double *state, double *unused, double *out_430462771034997173); +void car_h_24(double *state, double *unused, double *out_276973526775545327); +void car_H_24(double *state, double *unused, double *out_1742186827970502393); +void car_h_30(double *state, double *unused, double *out_2255184509939275472); +void car_H_30(double *state, double *unused, double *out_2948795729542245800); +void car_h_26(double *state, double *unused, double *out_1273100416603977360); +void car_H_26(double *state, double *unused, double *out_3734988740795797774); +void car_h_27(double *state, double *unused, double *out_2052169011425579317); +void car_H_27(double *state, double *unused, double *out_774032417741820889); +void car_h_29(double *state, double *unused, double *out_7256857777600121117); +void car_H_29(double *state, double *unused, double *out_3459027073856637984); +void car_h_28(double *state, double *unused, double *out_2373191903872652216); +void car_H_28(double *state, double *unused, double *out_1623371943212892590); +void car_h_31(double *state, double *unused, double *out_6078293745934141447); +void car_H_31(double *state, double *unused, double *out_3108780638562446298); 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/libcar.so b/selfdrive/locationd/models/generated/libcar.so index 419ca2147..2e2224212 100755 Binary files a/selfdrive/locationd/models/generated/libcar.so and b/selfdrive/locationd/models/generated/libcar.so differ diff --git a/selfdrive/locationd/models/generated/libpose.so b/selfdrive/locationd/models/generated/libpose.so index 91488edfc..dff9b15f1 100755 Binary files a/selfdrive/locationd/models/generated/libpose.so and b/selfdrive/locationd/models/generated/libpose.so differ diff --git a/selfdrive/locationd/models/generated/pose.cpp b/selfdrive/locationd/models/generated/pose.cpp index 4544dec05..d3999fa2a 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_4409321269442810496) { - out_4409321269442810496[0] = delta_x[0] + nom_x[0]; - out_4409321269442810496[1] = delta_x[1] + nom_x[1]; - out_4409321269442810496[2] = delta_x[2] + nom_x[2]; - out_4409321269442810496[3] = delta_x[3] + nom_x[3]; - out_4409321269442810496[4] = delta_x[4] + nom_x[4]; - out_4409321269442810496[5] = delta_x[5] + nom_x[5]; - out_4409321269442810496[6] = delta_x[6] + nom_x[6]; - out_4409321269442810496[7] = delta_x[7] + nom_x[7]; - out_4409321269442810496[8] = delta_x[8] + nom_x[8]; - out_4409321269442810496[9] = delta_x[9] + nom_x[9]; - out_4409321269442810496[10] = delta_x[10] + nom_x[10]; - out_4409321269442810496[11] = delta_x[11] + nom_x[11]; - out_4409321269442810496[12] = delta_x[12] + nom_x[12]; - out_4409321269442810496[13] = delta_x[13] + nom_x[13]; - out_4409321269442810496[14] = delta_x[14] + nom_x[14]; - out_4409321269442810496[15] = delta_x[15] + nom_x[15]; - out_4409321269442810496[16] = delta_x[16] + nom_x[16]; - out_4409321269442810496[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_7992688260990922493) { + out_7992688260990922493[0] = delta_x[0] + nom_x[0]; + out_7992688260990922493[1] = delta_x[1] + nom_x[1]; + out_7992688260990922493[2] = delta_x[2] + nom_x[2]; + out_7992688260990922493[3] = delta_x[3] + nom_x[3]; + out_7992688260990922493[4] = delta_x[4] + nom_x[4]; + out_7992688260990922493[5] = delta_x[5] + nom_x[5]; + out_7992688260990922493[6] = delta_x[6] + nom_x[6]; + out_7992688260990922493[7] = delta_x[7] + nom_x[7]; + out_7992688260990922493[8] = delta_x[8] + nom_x[8]; + out_7992688260990922493[9] = delta_x[9] + nom_x[9]; + out_7992688260990922493[10] = delta_x[10] + nom_x[10]; + out_7992688260990922493[11] = delta_x[11] + nom_x[11]; + out_7992688260990922493[12] = delta_x[12] + nom_x[12]; + out_7992688260990922493[13] = delta_x[13] + nom_x[13]; + out_7992688260990922493[14] = delta_x[14] + nom_x[14]; + out_7992688260990922493[15] = delta_x[15] + nom_x[15]; + out_7992688260990922493[16] = delta_x[16] + nom_x[16]; + out_7992688260990922493[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_3789556254182165506) { - out_3789556254182165506[0] = -nom_x[0] + true_x[0]; - out_3789556254182165506[1] = -nom_x[1] + true_x[1]; - out_3789556254182165506[2] = -nom_x[2] + true_x[2]; - out_3789556254182165506[3] = -nom_x[3] + true_x[3]; - out_3789556254182165506[4] = -nom_x[4] + true_x[4]; - out_3789556254182165506[5] = -nom_x[5] + true_x[5]; - out_3789556254182165506[6] = -nom_x[6] + true_x[6]; - out_3789556254182165506[7] = -nom_x[7] + true_x[7]; - out_3789556254182165506[8] = -nom_x[8] + true_x[8]; - out_3789556254182165506[9] = -nom_x[9] + true_x[9]; - out_3789556254182165506[10] = -nom_x[10] + true_x[10]; - out_3789556254182165506[11] = -nom_x[11] + true_x[11]; - out_3789556254182165506[12] = -nom_x[12] + true_x[12]; - out_3789556254182165506[13] = -nom_x[13] + true_x[13]; - out_3789556254182165506[14] = -nom_x[14] + true_x[14]; - out_3789556254182165506[15] = -nom_x[15] + true_x[15]; - out_3789556254182165506[16] = -nom_x[16] + true_x[16]; - out_3789556254182165506[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_5809721741941332225) { + out_5809721741941332225[0] = -nom_x[0] + true_x[0]; + out_5809721741941332225[1] = -nom_x[1] + true_x[1]; + out_5809721741941332225[2] = -nom_x[2] + true_x[2]; + out_5809721741941332225[3] = -nom_x[3] + true_x[3]; + out_5809721741941332225[4] = -nom_x[4] + true_x[4]; + out_5809721741941332225[5] = -nom_x[5] + true_x[5]; + out_5809721741941332225[6] = -nom_x[6] + true_x[6]; + out_5809721741941332225[7] = -nom_x[7] + true_x[7]; + out_5809721741941332225[8] = -nom_x[8] + true_x[8]; + out_5809721741941332225[9] = -nom_x[9] + true_x[9]; + out_5809721741941332225[10] = -nom_x[10] + true_x[10]; + out_5809721741941332225[11] = -nom_x[11] + true_x[11]; + out_5809721741941332225[12] = -nom_x[12] + true_x[12]; + out_5809721741941332225[13] = -nom_x[13] + true_x[13]; + out_5809721741941332225[14] = -nom_x[14] + true_x[14]; + out_5809721741941332225[15] = -nom_x[15] + true_x[15]; + out_5809721741941332225[16] = -nom_x[16] + true_x[16]; + out_5809721741941332225[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_3068405914716205428) { - out_3068405914716205428[0] = 1.0; - out_3068405914716205428[1] = 0.0; - out_3068405914716205428[2] = 0.0; - out_3068405914716205428[3] = 0.0; - out_3068405914716205428[4] = 0.0; - out_3068405914716205428[5] = 0.0; - out_3068405914716205428[6] = 0.0; - out_3068405914716205428[7] = 0.0; - out_3068405914716205428[8] = 0.0; - out_3068405914716205428[9] = 0.0; - out_3068405914716205428[10] = 0.0; - out_3068405914716205428[11] = 0.0; - out_3068405914716205428[12] = 0.0; - out_3068405914716205428[13] = 0.0; - out_3068405914716205428[14] = 0.0; - out_3068405914716205428[15] = 0.0; - out_3068405914716205428[16] = 0.0; - out_3068405914716205428[17] = 0.0; - out_3068405914716205428[18] = 0.0; - out_3068405914716205428[19] = 1.0; - out_3068405914716205428[20] = 0.0; - out_3068405914716205428[21] = 0.0; - out_3068405914716205428[22] = 0.0; - out_3068405914716205428[23] = 0.0; - out_3068405914716205428[24] = 0.0; - out_3068405914716205428[25] = 0.0; - out_3068405914716205428[26] = 0.0; - out_3068405914716205428[27] = 0.0; - out_3068405914716205428[28] = 0.0; - out_3068405914716205428[29] = 0.0; - out_3068405914716205428[30] = 0.0; - out_3068405914716205428[31] = 0.0; - out_3068405914716205428[32] = 0.0; - out_3068405914716205428[33] = 0.0; - out_3068405914716205428[34] = 0.0; - out_3068405914716205428[35] = 0.0; - out_3068405914716205428[36] = 0.0; - out_3068405914716205428[37] = 0.0; - out_3068405914716205428[38] = 1.0; - out_3068405914716205428[39] = 0.0; - out_3068405914716205428[40] = 0.0; - out_3068405914716205428[41] = 0.0; - out_3068405914716205428[42] = 0.0; - out_3068405914716205428[43] = 0.0; - out_3068405914716205428[44] = 0.0; - out_3068405914716205428[45] = 0.0; - out_3068405914716205428[46] = 0.0; - out_3068405914716205428[47] = 0.0; - out_3068405914716205428[48] = 0.0; - out_3068405914716205428[49] = 0.0; - out_3068405914716205428[50] = 0.0; - out_3068405914716205428[51] = 0.0; - out_3068405914716205428[52] = 0.0; - out_3068405914716205428[53] = 0.0; - out_3068405914716205428[54] = 0.0; - out_3068405914716205428[55] = 0.0; - out_3068405914716205428[56] = 0.0; - out_3068405914716205428[57] = 1.0; - out_3068405914716205428[58] = 0.0; - out_3068405914716205428[59] = 0.0; - out_3068405914716205428[60] = 0.0; - out_3068405914716205428[61] = 0.0; - out_3068405914716205428[62] = 0.0; - out_3068405914716205428[63] = 0.0; - out_3068405914716205428[64] = 0.0; - out_3068405914716205428[65] = 0.0; - out_3068405914716205428[66] = 0.0; - out_3068405914716205428[67] = 0.0; - out_3068405914716205428[68] = 0.0; - out_3068405914716205428[69] = 0.0; - out_3068405914716205428[70] = 0.0; - out_3068405914716205428[71] = 0.0; - out_3068405914716205428[72] = 0.0; - out_3068405914716205428[73] = 0.0; - out_3068405914716205428[74] = 0.0; - out_3068405914716205428[75] = 0.0; - out_3068405914716205428[76] = 1.0; - out_3068405914716205428[77] = 0.0; - out_3068405914716205428[78] = 0.0; - out_3068405914716205428[79] = 0.0; - out_3068405914716205428[80] = 0.0; - out_3068405914716205428[81] = 0.0; - out_3068405914716205428[82] = 0.0; - out_3068405914716205428[83] = 0.0; - out_3068405914716205428[84] = 0.0; - out_3068405914716205428[85] = 0.0; - out_3068405914716205428[86] = 0.0; - out_3068405914716205428[87] = 0.0; - out_3068405914716205428[88] = 0.0; - out_3068405914716205428[89] = 0.0; - out_3068405914716205428[90] = 0.0; - out_3068405914716205428[91] = 0.0; - out_3068405914716205428[92] = 0.0; - out_3068405914716205428[93] = 0.0; - out_3068405914716205428[94] = 0.0; - out_3068405914716205428[95] = 1.0; - out_3068405914716205428[96] = 0.0; - out_3068405914716205428[97] = 0.0; - out_3068405914716205428[98] = 0.0; - out_3068405914716205428[99] = 0.0; - out_3068405914716205428[100] = 0.0; - out_3068405914716205428[101] = 0.0; - out_3068405914716205428[102] = 0.0; - out_3068405914716205428[103] = 0.0; - out_3068405914716205428[104] = 0.0; - out_3068405914716205428[105] = 0.0; - out_3068405914716205428[106] = 0.0; - out_3068405914716205428[107] = 0.0; - out_3068405914716205428[108] = 0.0; - out_3068405914716205428[109] = 0.0; - out_3068405914716205428[110] = 0.0; - out_3068405914716205428[111] = 0.0; - out_3068405914716205428[112] = 0.0; - out_3068405914716205428[113] = 0.0; - out_3068405914716205428[114] = 1.0; - out_3068405914716205428[115] = 0.0; - out_3068405914716205428[116] = 0.0; - out_3068405914716205428[117] = 0.0; - out_3068405914716205428[118] = 0.0; - out_3068405914716205428[119] = 0.0; - out_3068405914716205428[120] = 0.0; - out_3068405914716205428[121] = 0.0; - out_3068405914716205428[122] = 0.0; - out_3068405914716205428[123] = 0.0; - out_3068405914716205428[124] = 0.0; - out_3068405914716205428[125] = 0.0; - out_3068405914716205428[126] = 0.0; - out_3068405914716205428[127] = 0.0; - out_3068405914716205428[128] = 0.0; - out_3068405914716205428[129] = 0.0; - out_3068405914716205428[130] = 0.0; - out_3068405914716205428[131] = 0.0; - out_3068405914716205428[132] = 0.0; - out_3068405914716205428[133] = 1.0; - out_3068405914716205428[134] = 0.0; - out_3068405914716205428[135] = 0.0; - out_3068405914716205428[136] = 0.0; - out_3068405914716205428[137] = 0.0; - out_3068405914716205428[138] = 0.0; - out_3068405914716205428[139] = 0.0; - out_3068405914716205428[140] = 0.0; - out_3068405914716205428[141] = 0.0; - out_3068405914716205428[142] = 0.0; - out_3068405914716205428[143] = 0.0; - out_3068405914716205428[144] = 0.0; - out_3068405914716205428[145] = 0.0; - out_3068405914716205428[146] = 0.0; - out_3068405914716205428[147] = 0.0; - out_3068405914716205428[148] = 0.0; - out_3068405914716205428[149] = 0.0; - out_3068405914716205428[150] = 0.0; - out_3068405914716205428[151] = 0.0; - out_3068405914716205428[152] = 1.0; - out_3068405914716205428[153] = 0.0; - out_3068405914716205428[154] = 0.0; - out_3068405914716205428[155] = 0.0; - out_3068405914716205428[156] = 0.0; - out_3068405914716205428[157] = 0.0; - out_3068405914716205428[158] = 0.0; - out_3068405914716205428[159] = 0.0; - out_3068405914716205428[160] = 0.0; - out_3068405914716205428[161] = 0.0; - out_3068405914716205428[162] = 0.0; - out_3068405914716205428[163] = 0.0; - out_3068405914716205428[164] = 0.0; - out_3068405914716205428[165] = 0.0; - out_3068405914716205428[166] = 0.0; - out_3068405914716205428[167] = 0.0; - out_3068405914716205428[168] = 0.0; - out_3068405914716205428[169] = 0.0; - out_3068405914716205428[170] = 0.0; - out_3068405914716205428[171] = 1.0; - out_3068405914716205428[172] = 0.0; - out_3068405914716205428[173] = 0.0; - out_3068405914716205428[174] = 0.0; - out_3068405914716205428[175] = 0.0; - out_3068405914716205428[176] = 0.0; - out_3068405914716205428[177] = 0.0; - out_3068405914716205428[178] = 0.0; - out_3068405914716205428[179] = 0.0; - out_3068405914716205428[180] = 0.0; - out_3068405914716205428[181] = 0.0; - out_3068405914716205428[182] = 0.0; - out_3068405914716205428[183] = 0.0; - out_3068405914716205428[184] = 0.0; - out_3068405914716205428[185] = 0.0; - out_3068405914716205428[186] = 0.0; - out_3068405914716205428[187] = 0.0; - out_3068405914716205428[188] = 0.0; - out_3068405914716205428[189] = 0.0; - out_3068405914716205428[190] = 1.0; - out_3068405914716205428[191] = 0.0; - out_3068405914716205428[192] = 0.0; - out_3068405914716205428[193] = 0.0; - out_3068405914716205428[194] = 0.0; - out_3068405914716205428[195] = 0.0; - out_3068405914716205428[196] = 0.0; - out_3068405914716205428[197] = 0.0; - out_3068405914716205428[198] = 0.0; - out_3068405914716205428[199] = 0.0; - out_3068405914716205428[200] = 0.0; - out_3068405914716205428[201] = 0.0; - out_3068405914716205428[202] = 0.0; - out_3068405914716205428[203] = 0.0; - out_3068405914716205428[204] = 0.0; - out_3068405914716205428[205] = 0.0; - out_3068405914716205428[206] = 0.0; - out_3068405914716205428[207] = 0.0; - out_3068405914716205428[208] = 0.0; - out_3068405914716205428[209] = 1.0; - out_3068405914716205428[210] = 0.0; - out_3068405914716205428[211] = 0.0; - out_3068405914716205428[212] = 0.0; - out_3068405914716205428[213] = 0.0; - out_3068405914716205428[214] = 0.0; - out_3068405914716205428[215] = 0.0; - out_3068405914716205428[216] = 0.0; - out_3068405914716205428[217] = 0.0; - out_3068405914716205428[218] = 0.0; - out_3068405914716205428[219] = 0.0; - out_3068405914716205428[220] = 0.0; - out_3068405914716205428[221] = 0.0; - out_3068405914716205428[222] = 0.0; - out_3068405914716205428[223] = 0.0; - out_3068405914716205428[224] = 0.0; - out_3068405914716205428[225] = 0.0; - out_3068405914716205428[226] = 0.0; - out_3068405914716205428[227] = 0.0; - out_3068405914716205428[228] = 1.0; - out_3068405914716205428[229] = 0.0; - out_3068405914716205428[230] = 0.0; - out_3068405914716205428[231] = 0.0; - out_3068405914716205428[232] = 0.0; - out_3068405914716205428[233] = 0.0; - out_3068405914716205428[234] = 0.0; - out_3068405914716205428[235] = 0.0; - out_3068405914716205428[236] = 0.0; - out_3068405914716205428[237] = 0.0; - out_3068405914716205428[238] = 0.0; - out_3068405914716205428[239] = 0.0; - out_3068405914716205428[240] = 0.0; - out_3068405914716205428[241] = 0.0; - out_3068405914716205428[242] = 0.0; - out_3068405914716205428[243] = 0.0; - out_3068405914716205428[244] = 0.0; - out_3068405914716205428[245] = 0.0; - out_3068405914716205428[246] = 0.0; - out_3068405914716205428[247] = 1.0; - out_3068405914716205428[248] = 0.0; - out_3068405914716205428[249] = 0.0; - out_3068405914716205428[250] = 0.0; - out_3068405914716205428[251] = 0.0; - out_3068405914716205428[252] = 0.0; - out_3068405914716205428[253] = 0.0; - out_3068405914716205428[254] = 0.0; - out_3068405914716205428[255] = 0.0; - out_3068405914716205428[256] = 0.0; - out_3068405914716205428[257] = 0.0; - out_3068405914716205428[258] = 0.0; - out_3068405914716205428[259] = 0.0; - out_3068405914716205428[260] = 0.0; - out_3068405914716205428[261] = 0.0; - out_3068405914716205428[262] = 0.0; - out_3068405914716205428[263] = 0.0; - out_3068405914716205428[264] = 0.0; - out_3068405914716205428[265] = 0.0; - out_3068405914716205428[266] = 1.0; - out_3068405914716205428[267] = 0.0; - out_3068405914716205428[268] = 0.0; - out_3068405914716205428[269] = 0.0; - out_3068405914716205428[270] = 0.0; - out_3068405914716205428[271] = 0.0; - out_3068405914716205428[272] = 0.0; - out_3068405914716205428[273] = 0.0; - out_3068405914716205428[274] = 0.0; - out_3068405914716205428[275] = 0.0; - out_3068405914716205428[276] = 0.0; - out_3068405914716205428[277] = 0.0; - out_3068405914716205428[278] = 0.0; - out_3068405914716205428[279] = 0.0; - out_3068405914716205428[280] = 0.0; - out_3068405914716205428[281] = 0.0; - out_3068405914716205428[282] = 0.0; - out_3068405914716205428[283] = 0.0; - out_3068405914716205428[284] = 0.0; - out_3068405914716205428[285] = 1.0; - out_3068405914716205428[286] = 0.0; - out_3068405914716205428[287] = 0.0; - out_3068405914716205428[288] = 0.0; - out_3068405914716205428[289] = 0.0; - out_3068405914716205428[290] = 0.0; - out_3068405914716205428[291] = 0.0; - out_3068405914716205428[292] = 0.0; - out_3068405914716205428[293] = 0.0; - out_3068405914716205428[294] = 0.0; - out_3068405914716205428[295] = 0.0; - out_3068405914716205428[296] = 0.0; - out_3068405914716205428[297] = 0.0; - out_3068405914716205428[298] = 0.0; - out_3068405914716205428[299] = 0.0; - out_3068405914716205428[300] = 0.0; - out_3068405914716205428[301] = 0.0; - out_3068405914716205428[302] = 0.0; - out_3068405914716205428[303] = 0.0; - out_3068405914716205428[304] = 1.0; - out_3068405914716205428[305] = 0.0; - out_3068405914716205428[306] = 0.0; - out_3068405914716205428[307] = 0.0; - out_3068405914716205428[308] = 0.0; - out_3068405914716205428[309] = 0.0; - out_3068405914716205428[310] = 0.0; - out_3068405914716205428[311] = 0.0; - out_3068405914716205428[312] = 0.0; - out_3068405914716205428[313] = 0.0; - out_3068405914716205428[314] = 0.0; - out_3068405914716205428[315] = 0.0; - out_3068405914716205428[316] = 0.0; - out_3068405914716205428[317] = 0.0; - out_3068405914716205428[318] = 0.0; - out_3068405914716205428[319] = 0.0; - out_3068405914716205428[320] = 0.0; - out_3068405914716205428[321] = 0.0; - out_3068405914716205428[322] = 0.0; - out_3068405914716205428[323] = 1.0; +void H_mod_fun(double *state, double *out_653094641834631323) { + out_653094641834631323[0] = 1.0; + out_653094641834631323[1] = 0.0; + out_653094641834631323[2] = 0.0; + out_653094641834631323[3] = 0.0; + out_653094641834631323[4] = 0.0; + out_653094641834631323[5] = 0.0; + out_653094641834631323[6] = 0.0; + out_653094641834631323[7] = 0.0; + out_653094641834631323[8] = 0.0; + out_653094641834631323[9] = 0.0; + out_653094641834631323[10] = 0.0; + out_653094641834631323[11] = 0.0; + out_653094641834631323[12] = 0.0; + out_653094641834631323[13] = 0.0; + out_653094641834631323[14] = 0.0; + out_653094641834631323[15] = 0.0; + out_653094641834631323[16] = 0.0; + out_653094641834631323[17] = 0.0; + out_653094641834631323[18] = 0.0; + out_653094641834631323[19] = 1.0; + out_653094641834631323[20] = 0.0; + out_653094641834631323[21] = 0.0; + out_653094641834631323[22] = 0.0; + out_653094641834631323[23] = 0.0; + out_653094641834631323[24] = 0.0; + out_653094641834631323[25] = 0.0; + out_653094641834631323[26] = 0.0; + out_653094641834631323[27] = 0.0; + out_653094641834631323[28] = 0.0; + out_653094641834631323[29] = 0.0; + out_653094641834631323[30] = 0.0; + out_653094641834631323[31] = 0.0; + out_653094641834631323[32] = 0.0; + out_653094641834631323[33] = 0.0; + out_653094641834631323[34] = 0.0; + out_653094641834631323[35] = 0.0; + out_653094641834631323[36] = 0.0; + out_653094641834631323[37] = 0.0; + out_653094641834631323[38] = 1.0; + out_653094641834631323[39] = 0.0; + out_653094641834631323[40] = 0.0; + out_653094641834631323[41] = 0.0; + out_653094641834631323[42] = 0.0; + out_653094641834631323[43] = 0.0; + out_653094641834631323[44] = 0.0; + out_653094641834631323[45] = 0.0; + out_653094641834631323[46] = 0.0; + out_653094641834631323[47] = 0.0; + out_653094641834631323[48] = 0.0; + out_653094641834631323[49] = 0.0; + out_653094641834631323[50] = 0.0; + out_653094641834631323[51] = 0.0; + out_653094641834631323[52] = 0.0; + out_653094641834631323[53] = 0.0; + out_653094641834631323[54] = 0.0; + out_653094641834631323[55] = 0.0; + out_653094641834631323[56] = 0.0; + out_653094641834631323[57] = 1.0; + out_653094641834631323[58] = 0.0; + out_653094641834631323[59] = 0.0; + out_653094641834631323[60] = 0.0; + out_653094641834631323[61] = 0.0; + out_653094641834631323[62] = 0.0; + out_653094641834631323[63] = 0.0; + out_653094641834631323[64] = 0.0; + out_653094641834631323[65] = 0.0; + out_653094641834631323[66] = 0.0; + out_653094641834631323[67] = 0.0; + out_653094641834631323[68] = 0.0; + out_653094641834631323[69] = 0.0; + out_653094641834631323[70] = 0.0; + out_653094641834631323[71] = 0.0; + out_653094641834631323[72] = 0.0; + out_653094641834631323[73] = 0.0; + out_653094641834631323[74] = 0.0; + out_653094641834631323[75] = 0.0; + out_653094641834631323[76] = 1.0; + out_653094641834631323[77] = 0.0; + out_653094641834631323[78] = 0.0; + out_653094641834631323[79] = 0.0; + out_653094641834631323[80] = 0.0; + out_653094641834631323[81] = 0.0; + out_653094641834631323[82] = 0.0; + out_653094641834631323[83] = 0.0; + out_653094641834631323[84] = 0.0; + out_653094641834631323[85] = 0.0; + out_653094641834631323[86] = 0.0; + out_653094641834631323[87] = 0.0; + out_653094641834631323[88] = 0.0; + out_653094641834631323[89] = 0.0; + out_653094641834631323[90] = 0.0; + out_653094641834631323[91] = 0.0; + out_653094641834631323[92] = 0.0; + out_653094641834631323[93] = 0.0; + out_653094641834631323[94] = 0.0; + out_653094641834631323[95] = 1.0; + out_653094641834631323[96] = 0.0; + out_653094641834631323[97] = 0.0; + out_653094641834631323[98] = 0.0; + out_653094641834631323[99] = 0.0; + out_653094641834631323[100] = 0.0; + out_653094641834631323[101] = 0.0; + out_653094641834631323[102] = 0.0; + out_653094641834631323[103] = 0.0; + out_653094641834631323[104] = 0.0; + out_653094641834631323[105] = 0.0; + out_653094641834631323[106] = 0.0; + out_653094641834631323[107] = 0.0; + out_653094641834631323[108] = 0.0; + out_653094641834631323[109] = 0.0; + out_653094641834631323[110] = 0.0; + out_653094641834631323[111] = 0.0; + out_653094641834631323[112] = 0.0; + out_653094641834631323[113] = 0.0; + out_653094641834631323[114] = 1.0; + out_653094641834631323[115] = 0.0; + out_653094641834631323[116] = 0.0; + out_653094641834631323[117] = 0.0; + out_653094641834631323[118] = 0.0; + out_653094641834631323[119] = 0.0; + out_653094641834631323[120] = 0.0; + out_653094641834631323[121] = 0.0; + out_653094641834631323[122] = 0.0; + out_653094641834631323[123] = 0.0; + out_653094641834631323[124] = 0.0; + out_653094641834631323[125] = 0.0; + out_653094641834631323[126] = 0.0; + out_653094641834631323[127] = 0.0; + out_653094641834631323[128] = 0.0; + out_653094641834631323[129] = 0.0; + out_653094641834631323[130] = 0.0; + out_653094641834631323[131] = 0.0; + out_653094641834631323[132] = 0.0; + out_653094641834631323[133] = 1.0; + out_653094641834631323[134] = 0.0; + out_653094641834631323[135] = 0.0; + out_653094641834631323[136] = 0.0; + out_653094641834631323[137] = 0.0; + out_653094641834631323[138] = 0.0; + out_653094641834631323[139] = 0.0; + out_653094641834631323[140] = 0.0; + out_653094641834631323[141] = 0.0; + out_653094641834631323[142] = 0.0; + out_653094641834631323[143] = 0.0; + out_653094641834631323[144] = 0.0; + out_653094641834631323[145] = 0.0; + out_653094641834631323[146] = 0.0; + out_653094641834631323[147] = 0.0; + out_653094641834631323[148] = 0.0; + out_653094641834631323[149] = 0.0; + out_653094641834631323[150] = 0.0; + out_653094641834631323[151] = 0.0; + out_653094641834631323[152] = 1.0; + out_653094641834631323[153] = 0.0; + out_653094641834631323[154] = 0.0; + out_653094641834631323[155] = 0.0; + out_653094641834631323[156] = 0.0; + out_653094641834631323[157] = 0.0; + out_653094641834631323[158] = 0.0; + out_653094641834631323[159] = 0.0; + out_653094641834631323[160] = 0.0; + out_653094641834631323[161] = 0.0; + out_653094641834631323[162] = 0.0; + out_653094641834631323[163] = 0.0; + out_653094641834631323[164] = 0.0; + out_653094641834631323[165] = 0.0; + out_653094641834631323[166] = 0.0; + out_653094641834631323[167] = 0.0; + out_653094641834631323[168] = 0.0; + out_653094641834631323[169] = 0.0; + out_653094641834631323[170] = 0.0; + out_653094641834631323[171] = 1.0; + out_653094641834631323[172] = 0.0; + out_653094641834631323[173] = 0.0; + out_653094641834631323[174] = 0.0; + out_653094641834631323[175] = 0.0; + out_653094641834631323[176] = 0.0; + out_653094641834631323[177] = 0.0; + out_653094641834631323[178] = 0.0; + out_653094641834631323[179] = 0.0; + out_653094641834631323[180] = 0.0; + out_653094641834631323[181] = 0.0; + out_653094641834631323[182] = 0.0; + out_653094641834631323[183] = 0.0; + out_653094641834631323[184] = 0.0; + out_653094641834631323[185] = 0.0; + out_653094641834631323[186] = 0.0; + out_653094641834631323[187] = 0.0; + out_653094641834631323[188] = 0.0; + out_653094641834631323[189] = 0.0; + out_653094641834631323[190] = 1.0; + out_653094641834631323[191] = 0.0; + out_653094641834631323[192] = 0.0; + out_653094641834631323[193] = 0.0; + out_653094641834631323[194] = 0.0; + out_653094641834631323[195] = 0.0; + out_653094641834631323[196] = 0.0; + out_653094641834631323[197] = 0.0; + out_653094641834631323[198] = 0.0; + out_653094641834631323[199] = 0.0; + out_653094641834631323[200] = 0.0; + out_653094641834631323[201] = 0.0; + out_653094641834631323[202] = 0.0; + out_653094641834631323[203] = 0.0; + out_653094641834631323[204] = 0.0; + out_653094641834631323[205] = 0.0; + out_653094641834631323[206] = 0.0; + out_653094641834631323[207] = 0.0; + out_653094641834631323[208] = 0.0; + out_653094641834631323[209] = 1.0; + out_653094641834631323[210] = 0.0; + out_653094641834631323[211] = 0.0; + out_653094641834631323[212] = 0.0; + out_653094641834631323[213] = 0.0; + out_653094641834631323[214] = 0.0; + out_653094641834631323[215] = 0.0; + out_653094641834631323[216] = 0.0; + out_653094641834631323[217] = 0.0; + out_653094641834631323[218] = 0.0; + out_653094641834631323[219] = 0.0; + out_653094641834631323[220] = 0.0; + out_653094641834631323[221] = 0.0; + out_653094641834631323[222] = 0.0; + out_653094641834631323[223] = 0.0; + out_653094641834631323[224] = 0.0; + out_653094641834631323[225] = 0.0; + out_653094641834631323[226] = 0.0; + out_653094641834631323[227] = 0.0; + out_653094641834631323[228] = 1.0; + out_653094641834631323[229] = 0.0; + out_653094641834631323[230] = 0.0; + out_653094641834631323[231] = 0.0; + out_653094641834631323[232] = 0.0; + out_653094641834631323[233] = 0.0; + out_653094641834631323[234] = 0.0; + out_653094641834631323[235] = 0.0; + out_653094641834631323[236] = 0.0; + out_653094641834631323[237] = 0.0; + out_653094641834631323[238] = 0.0; + out_653094641834631323[239] = 0.0; + out_653094641834631323[240] = 0.0; + out_653094641834631323[241] = 0.0; + out_653094641834631323[242] = 0.0; + out_653094641834631323[243] = 0.0; + out_653094641834631323[244] = 0.0; + out_653094641834631323[245] = 0.0; + out_653094641834631323[246] = 0.0; + out_653094641834631323[247] = 1.0; + out_653094641834631323[248] = 0.0; + out_653094641834631323[249] = 0.0; + out_653094641834631323[250] = 0.0; + out_653094641834631323[251] = 0.0; + out_653094641834631323[252] = 0.0; + out_653094641834631323[253] = 0.0; + out_653094641834631323[254] = 0.0; + out_653094641834631323[255] = 0.0; + out_653094641834631323[256] = 0.0; + out_653094641834631323[257] = 0.0; + out_653094641834631323[258] = 0.0; + out_653094641834631323[259] = 0.0; + out_653094641834631323[260] = 0.0; + out_653094641834631323[261] = 0.0; + out_653094641834631323[262] = 0.0; + out_653094641834631323[263] = 0.0; + out_653094641834631323[264] = 0.0; + out_653094641834631323[265] = 0.0; + out_653094641834631323[266] = 1.0; + out_653094641834631323[267] = 0.0; + out_653094641834631323[268] = 0.0; + out_653094641834631323[269] = 0.0; + out_653094641834631323[270] = 0.0; + out_653094641834631323[271] = 0.0; + out_653094641834631323[272] = 0.0; + out_653094641834631323[273] = 0.0; + out_653094641834631323[274] = 0.0; + out_653094641834631323[275] = 0.0; + out_653094641834631323[276] = 0.0; + out_653094641834631323[277] = 0.0; + out_653094641834631323[278] = 0.0; + out_653094641834631323[279] = 0.0; + out_653094641834631323[280] = 0.0; + out_653094641834631323[281] = 0.0; + out_653094641834631323[282] = 0.0; + out_653094641834631323[283] = 0.0; + out_653094641834631323[284] = 0.0; + out_653094641834631323[285] = 1.0; + out_653094641834631323[286] = 0.0; + out_653094641834631323[287] = 0.0; + out_653094641834631323[288] = 0.0; + out_653094641834631323[289] = 0.0; + out_653094641834631323[290] = 0.0; + out_653094641834631323[291] = 0.0; + out_653094641834631323[292] = 0.0; + out_653094641834631323[293] = 0.0; + out_653094641834631323[294] = 0.0; + out_653094641834631323[295] = 0.0; + out_653094641834631323[296] = 0.0; + out_653094641834631323[297] = 0.0; + out_653094641834631323[298] = 0.0; + out_653094641834631323[299] = 0.0; + out_653094641834631323[300] = 0.0; + out_653094641834631323[301] = 0.0; + out_653094641834631323[302] = 0.0; + out_653094641834631323[303] = 0.0; + out_653094641834631323[304] = 1.0; + out_653094641834631323[305] = 0.0; + out_653094641834631323[306] = 0.0; + out_653094641834631323[307] = 0.0; + out_653094641834631323[308] = 0.0; + out_653094641834631323[309] = 0.0; + out_653094641834631323[310] = 0.0; + out_653094641834631323[311] = 0.0; + out_653094641834631323[312] = 0.0; + out_653094641834631323[313] = 0.0; + out_653094641834631323[314] = 0.0; + out_653094641834631323[315] = 0.0; + out_653094641834631323[316] = 0.0; + out_653094641834631323[317] = 0.0; + out_653094641834631323[318] = 0.0; + out_653094641834631323[319] = 0.0; + out_653094641834631323[320] = 0.0; + out_653094641834631323[321] = 0.0; + out_653094641834631323[322] = 0.0; + out_653094641834631323[323] = 1.0; } -void f_fun(double *state, double dt, double *out_5515990809264108996) { - out_5515990809264108996[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_5515990809264108996[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_5515990809264108996[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_5515990809264108996[3] = dt*state[12] + state[3]; - out_5515990809264108996[4] = dt*state[13] + state[4]; - out_5515990809264108996[5] = dt*state[14] + state[5]; - out_5515990809264108996[6] = state[6]; - out_5515990809264108996[7] = state[7]; - out_5515990809264108996[8] = state[8]; - out_5515990809264108996[9] = state[9]; - out_5515990809264108996[10] = state[10]; - out_5515990809264108996[11] = state[11]; - out_5515990809264108996[12] = state[12]; - out_5515990809264108996[13] = state[13]; - out_5515990809264108996[14] = state[14]; - out_5515990809264108996[15] = state[15]; - out_5515990809264108996[16] = state[16]; - out_5515990809264108996[17] = state[17]; +void f_fun(double *state, double dt, double *out_5955110383788875610) { + out_5955110383788875610[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_5955110383788875610[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_5955110383788875610[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_5955110383788875610[3] = dt*state[12] + state[3]; + out_5955110383788875610[4] = dt*state[13] + state[4]; + out_5955110383788875610[5] = dt*state[14] + state[5]; + out_5955110383788875610[6] = state[6]; + out_5955110383788875610[7] = state[7]; + out_5955110383788875610[8] = state[8]; + out_5955110383788875610[9] = state[9]; + out_5955110383788875610[10] = state[10]; + out_5955110383788875610[11] = state[11]; + out_5955110383788875610[12] = state[12]; + out_5955110383788875610[13] = state[13]; + out_5955110383788875610[14] = state[14]; + out_5955110383788875610[15] = state[15]; + out_5955110383788875610[16] = state[16]; + out_5955110383788875610[17] = state[17]; } -void F_fun(double *state, double dt, double *out_1412435369220653896) { - out_1412435369220653896[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_1412435369220653896[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_1412435369220653896[2] = 0; - out_1412435369220653896[3] = 0; - out_1412435369220653896[4] = 0; - out_1412435369220653896[5] = 0; - out_1412435369220653896[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_1412435369220653896[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_1412435369220653896[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_1412435369220653896[9] = 0; - out_1412435369220653896[10] = 0; - out_1412435369220653896[11] = 0; - out_1412435369220653896[12] = 0; - out_1412435369220653896[13] = 0; - out_1412435369220653896[14] = 0; - out_1412435369220653896[15] = 0; - out_1412435369220653896[16] = 0; - out_1412435369220653896[17] = 0; - out_1412435369220653896[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_1412435369220653896[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_1412435369220653896[20] = 0; - out_1412435369220653896[21] = 0; - out_1412435369220653896[22] = 0; - out_1412435369220653896[23] = 0; - out_1412435369220653896[24] = 0; - out_1412435369220653896[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_1412435369220653896[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_1412435369220653896[27] = 0; - out_1412435369220653896[28] = 0; - out_1412435369220653896[29] = 0; - out_1412435369220653896[30] = 0; - out_1412435369220653896[31] = 0; - out_1412435369220653896[32] = 0; - out_1412435369220653896[33] = 0; - out_1412435369220653896[34] = 0; - out_1412435369220653896[35] = 0; - out_1412435369220653896[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_1412435369220653896[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_1412435369220653896[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_1412435369220653896[39] = 0; - out_1412435369220653896[40] = 0; - out_1412435369220653896[41] = 0; - out_1412435369220653896[42] = 0; - out_1412435369220653896[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_1412435369220653896[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_1412435369220653896[45] = 0; - out_1412435369220653896[46] = 0; - out_1412435369220653896[47] = 0; - out_1412435369220653896[48] = 0; - out_1412435369220653896[49] = 0; - out_1412435369220653896[50] = 0; - out_1412435369220653896[51] = 0; - out_1412435369220653896[52] = 0; - out_1412435369220653896[53] = 0; - out_1412435369220653896[54] = 0; - out_1412435369220653896[55] = 0; - out_1412435369220653896[56] = 0; - out_1412435369220653896[57] = 1; - out_1412435369220653896[58] = 0; - out_1412435369220653896[59] = 0; - out_1412435369220653896[60] = 0; - out_1412435369220653896[61] = 0; - out_1412435369220653896[62] = 0; - out_1412435369220653896[63] = 0; - out_1412435369220653896[64] = 0; - out_1412435369220653896[65] = 0; - out_1412435369220653896[66] = dt; - out_1412435369220653896[67] = 0; - out_1412435369220653896[68] = 0; - out_1412435369220653896[69] = 0; - out_1412435369220653896[70] = 0; - out_1412435369220653896[71] = 0; - out_1412435369220653896[72] = 0; - out_1412435369220653896[73] = 0; - out_1412435369220653896[74] = 0; - out_1412435369220653896[75] = 0; - out_1412435369220653896[76] = 1; - out_1412435369220653896[77] = 0; - out_1412435369220653896[78] = 0; - out_1412435369220653896[79] = 0; - out_1412435369220653896[80] = 0; - out_1412435369220653896[81] = 0; - out_1412435369220653896[82] = 0; - out_1412435369220653896[83] = 0; - out_1412435369220653896[84] = 0; - out_1412435369220653896[85] = dt; - out_1412435369220653896[86] = 0; - out_1412435369220653896[87] = 0; - out_1412435369220653896[88] = 0; - out_1412435369220653896[89] = 0; - out_1412435369220653896[90] = 0; - out_1412435369220653896[91] = 0; - out_1412435369220653896[92] = 0; - out_1412435369220653896[93] = 0; - out_1412435369220653896[94] = 0; - out_1412435369220653896[95] = 1; - out_1412435369220653896[96] = 0; - out_1412435369220653896[97] = 0; - out_1412435369220653896[98] = 0; - out_1412435369220653896[99] = 0; - out_1412435369220653896[100] = 0; - out_1412435369220653896[101] = 0; - out_1412435369220653896[102] = 0; - out_1412435369220653896[103] = 0; - out_1412435369220653896[104] = dt; - out_1412435369220653896[105] = 0; - out_1412435369220653896[106] = 0; - out_1412435369220653896[107] = 0; - out_1412435369220653896[108] = 0; - out_1412435369220653896[109] = 0; - out_1412435369220653896[110] = 0; - out_1412435369220653896[111] = 0; - out_1412435369220653896[112] = 0; - out_1412435369220653896[113] = 0; - out_1412435369220653896[114] = 1; - out_1412435369220653896[115] = 0; - out_1412435369220653896[116] = 0; - out_1412435369220653896[117] = 0; - out_1412435369220653896[118] = 0; - out_1412435369220653896[119] = 0; - out_1412435369220653896[120] = 0; - out_1412435369220653896[121] = 0; - out_1412435369220653896[122] = 0; - out_1412435369220653896[123] = 0; - out_1412435369220653896[124] = 0; - out_1412435369220653896[125] = 0; - out_1412435369220653896[126] = 0; - out_1412435369220653896[127] = 0; - out_1412435369220653896[128] = 0; - out_1412435369220653896[129] = 0; - out_1412435369220653896[130] = 0; - out_1412435369220653896[131] = 0; - out_1412435369220653896[132] = 0; - out_1412435369220653896[133] = 1; - out_1412435369220653896[134] = 0; - out_1412435369220653896[135] = 0; - out_1412435369220653896[136] = 0; - out_1412435369220653896[137] = 0; - out_1412435369220653896[138] = 0; - out_1412435369220653896[139] = 0; - out_1412435369220653896[140] = 0; - out_1412435369220653896[141] = 0; - out_1412435369220653896[142] = 0; - out_1412435369220653896[143] = 0; - out_1412435369220653896[144] = 0; - out_1412435369220653896[145] = 0; - out_1412435369220653896[146] = 0; - out_1412435369220653896[147] = 0; - out_1412435369220653896[148] = 0; - out_1412435369220653896[149] = 0; - out_1412435369220653896[150] = 0; - out_1412435369220653896[151] = 0; - out_1412435369220653896[152] = 1; - out_1412435369220653896[153] = 0; - out_1412435369220653896[154] = 0; - out_1412435369220653896[155] = 0; - out_1412435369220653896[156] = 0; - out_1412435369220653896[157] = 0; - out_1412435369220653896[158] = 0; - out_1412435369220653896[159] = 0; - out_1412435369220653896[160] = 0; - out_1412435369220653896[161] = 0; - out_1412435369220653896[162] = 0; - out_1412435369220653896[163] = 0; - out_1412435369220653896[164] = 0; - out_1412435369220653896[165] = 0; - out_1412435369220653896[166] = 0; - out_1412435369220653896[167] = 0; - out_1412435369220653896[168] = 0; - out_1412435369220653896[169] = 0; - out_1412435369220653896[170] = 0; - out_1412435369220653896[171] = 1; - out_1412435369220653896[172] = 0; - out_1412435369220653896[173] = 0; - out_1412435369220653896[174] = 0; - out_1412435369220653896[175] = 0; - out_1412435369220653896[176] = 0; - out_1412435369220653896[177] = 0; - out_1412435369220653896[178] = 0; - out_1412435369220653896[179] = 0; - out_1412435369220653896[180] = 0; - out_1412435369220653896[181] = 0; - out_1412435369220653896[182] = 0; - out_1412435369220653896[183] = 0; - out_1412435369220653896[184] = 0; - out_1412435369220653896[185] = 0; - out_1412435369220653896[186] = 0; - out_1412435369220653896[187] = 0; - out_1412435369220653896[188] = 0; - out_1412435369220653896[189] = 0; - out_1412435369220653896[190] = 1; - out_1412435369220653896[191] = 0; - out_1412435369220653896[192] = 0; - out_1412435369220653896[193] = 0; - out_1412435369220653896[194] = 0; - out_1412435369220653896[195] = 0; - out_1412435369220653896[196] = 0; - out_1412435369220653896[197] = 0; - out_1412435369220653896[198] = 0; - out_1412435369220653896[199] = 0; - out_1412435369220653896[200] = 0; - out_1412435369220653896[201] = 0; - out_1412435369220653896[202] = 0; - out_1412435369220653896[203] = 0; - out_1412435369220653896[204] = 0; - out_1412435369220653896[205] = 0; - out_1412435369220653896[206] = 0; - out_1412435369220653896[207] = 0; - out_1412435369220653896[208] = 0; - out_1412435369220653896[209] = 1; - out_1412435369220653896[210] = 0; - out_1412435369220653896[211] = 0; - out_1412435369220653896[212] = 0; - out_1412435369220653896[213] = 0; - out_1412435369220653896[214] = 0; - out_1412435369220653896[215] = 0; - out_1412435369220653896[216] = 0; - out_1412435369220653896[217] = 0; - out_1412435369220653896[218] = 0; - out_1412435369220653896[219] = 0; - out_1412435369220653896[220] = 0; - out_1412435369220653896[221] = 0; - out_1412435369220653896[222] = 0; - out_1412435369220653896[223] = 0; - out_1412435369220653896[224] = 0; - out_1412435369220653896[225] = 0; - out_1412435369220653896[226] = 0; - out_1412435369220653896[227] = 0; - out_1412435369220653896[228] = 1; - out_1412435369220653896[229] = 0; - out_1412435369220653896[230] = 0; - out_1412435369220653896[231] = 0; - out_1412435369220653896[232] = 0; - out_1412435369220653896[233] = 0; - out_1412435369220653896[234] = 0; - out_1412435369220653896[235] = 0; - out_1412435369220653896[236] = 0; - out_1412435369220653896[237] = 0; - out_1412435369220653896[238] = 0; - out_1412435369220653896[239] = 0; - out_1412435369220653896[240] = 0; - out_1412435369220653896[241] = 0; - out_1412435369220653896[242] = 0; - out_1412435369220653896[243] = 0; - out_1412435369220653896[244] = 0; - out_1412435369220653896[245] = 0; - out_1412435369220653896[246] = 0; - out_1412435369220653896[247] = 1; - out_1412435369220653896[248] = 0; - out_1412435369220653896[249] = 0; - out_1412435369220653896[250] = 0; - out_1412435369220653896[251] = 0; - out_1412435369220653896[252] = 0; - out_1412435369220653896[253] = 0; - out_1412435369220653896[254] = 0; - out_1412435369220653896[255] = 0; - out_1412435369220653896[256] = 0; - out_1412435369220653896[257] = 0; - out_1412435369220653896[258] = 0; - out_1412435369220653896[259] = 0; - out_1412435369220653896[260] = 0; - out_1412435369220653896[261] = 0; - out_1412435369220653896[262] = 0; - out_1412435369220653896[263] = 0; - out_1412435369220653896[264] = 0; - out_1412435369220653896[265] = 0; - out_1412435369220653896[266] = 1; - out_1412435369220653896[267] = 0; - out_1412435369220653896[268] = 0; - out_1412435369220653896[269] = 0; - out_1412435369220653896[270] = 0; - out_1412435369220653896[271] = 0; - out_1412435369220653896[272] = 0; - out_1412435369220653896[273] = 0; - out_1412435369220653896[274] = 0; - out_1412435369220653896[275] = 0; - out_1412435369220653896[276] = 0; - out_1412435369220653896[277] = 0; - out_1412435369220653896[278] = 0; - out_1412435369220653896[279] = 0; - out_1412435369220653896[280] = 0; - out_1412435369220653896[281] = 0; - out_1412435369220653896[282] = 0; - out_1412435369220653896[283] = 0; - out_1412435369220653896[284] = 0; - out_1412435369220653896[285] = 1; - out_1412435369220653896[286] = 0; - out_1412435369220653896[287] = 0; - out_1412435369220653896[288] = 0; - out_1412435369220653896[289] = 0; - out_1412435369220653896[290] = 0; - out_1412435369220653896[291] = 0; - out_1412435369220653896[292] = 0; - out_1412435369220653896[293] = 0; - out_1412435369220653896[294] = 0; - out_1412435369220653896[295] = 0; - out_1412435369220653896[296] = 0; - out_1412435369220653896[297] = 0; - out_1412435369220653896[298] = 0; - out_1412435369220653896[299] = 0; - out_1412435369220653896[300] = 0; - out_1412435369220653896[301] = 0; - out_1412435369220653896[302] = 0; - out_1412435369220653896[303] = 0; - out_1412435369220653896[304] = 1; - out_1412435369220653896[305] = 0; - out_1412435369220653896[306] = 0; - out_1412435369220653896[307] = 0; - out_1412435369220653896[308] = 0; - out_1412435369220653896[309] = 0; - out_1412435369220653896[310] = 0; - out_1412435369220653896[311] = 0; - out_1412435369220653896[312] = 0; - out_1412435369220653896[313] = 0; - out_1412435369220653896[314] = 0; - out_1412435369220653896[315] = 0; - out_1412435369220653896[316] = 0; - out_1412435369220653896[317] = 0; - out_1412435369220653896[318] = 0; - out_1412435369220653896[319] = 0; - out_1412435369220653896[320] = 0; - out_1412435369220653896[321] = 0; - out_1412435369220653896[322] = 0; - out_1412435369220653896[323] = 1; +void F_fun(double *state, double dt, double *out_3374367099061520939) { + out_3374367099061520939[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_3374367099061520939[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_3374367099061520939[2] = 0; + out_3374367099061520939[3] = 0; + out_3374367099061520939[4] = 0; + out_3374367099061520939[5] = 0; + out_3374367099061520939[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_3374367099061520939[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_3374367099061520939[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_3374367099061520939[9] = 0; + out_3374367099061520939[10] = 0; + out_3374367099061520939[11] = 0; + out_3374367099061520939[12] = 0; + out_3374367099061520939[13] = 0; + out_3374367099061520939[14] = 0; + out_3374367099061520939[15] = 0; + out_3374367099061520939[16] = 0; + out_3374367099061520939[17] = 0; + out_3374367099061520939[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_3374367099061520939[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_3374367099061520939[20] = 0; + out_3374367099061520939[21] = 0; + out_3374367099061520939[22] = 0; + out_3374367099061520939[23] = 0; + out_3374367099061520939[24] = 0; + out_3374367099061520939[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_3374367099061520939[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_3374367099061520939[27] = 0; + out_3374367099061520939[28] = 0; + out_3374367099061520939[29] = 0; + out_3374367099061520939[30] = 0; + out_3374367099061520939[31] = 0; + out_3374367099061520939[32] = 0; + out_3374367099061520939[33] = 0; + out_3374367099061520939[34] = 0; + out_3374367099061520939[35] = 0; + out_3374367099061520939[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_3374367099061520939[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_3374367099061520939[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_3374367099061520939[39] = 0; + out_3374367099061520939[40] = 0; + out_3374367099061520939[41] = 0; + out_3374367099061520939[42] = 0; + out_3374367099061520939[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_3374367099061520939[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_3374367099061520939[45] = 0; + out_3374367099061520939[46] = 0; + out_3374367099061520939[47] = 0; + out_3374367099061520939[48] = 0; + out_3374367099061520939[49] = 0; + out_3374367099061520939[50] = 0; + out_3374367099061520939[51] = 0; + out_3374367099061520939[52] = 0; + out_3374367099061520939[53] = 0; + out_3374367099061520939[54] = 0; + out_3374367099061520939[55] = 0; + out_3374367099061520939[56] = 0; + out_3374367099061520939[57] = 1; + out_3374367099061520939[58] = 0; + out_3374367099061520939[59] = 0; + out_3374367099061520939[60] = 0; + out_3374367099061520939[61] = 0; + out_3374367099061520939[62] = 0; + out_3374367099061520939[63] = 0; + out_3374367099061520939[64] = 0; + out_3374367099061520939[65] = 0; + out_3374367099061520939[66] = dt; + out_3374367099061520939[67] = 0; + out_3374367099061520939[68] = 0; + out_3374367099061520939[69] = 0; + out_3374367099061520939[70] = 0; + out_3374367099061520939[71] = 0; + out_3374367099061520939[72] = 0; + out_3374367099061520939[73] = 0; + out_3374367099061520939[74] = 0; + out_3374367099061520939[75] = 0; + out_3374367099061520939[76] = 1; + out_3374367099061520939[77] = 0; + out_3374367099061520939[78] = 0; + out_3374367099061520939[79] = 0; + out_3374367099061520939[80] = 0; + out_3374367099061520939[81] = 0; + out_3374367099061520939[82] = 0; + out_3374367099061520939[83] = 0; + out_3374367099061520939[84] = 0; + out_3374367099061520939[85] = dt; + out_3374367099061520939[86] = 0; + out_3374367099061520939[87] = 0; + out_3374367099061520939[88] = 0; + out_3374367099061520939[89] = 0; + out_3374367099061520939[90] = 0; + out_3374367099061520939[91] = 0; + out_3374367099061520939[92] = 0; + out_3374367099061520939[93] = 0; + out_3374367099061520939[94] = 0; + out_3374367099061520939[95] = 1; + out_3374367099061520939[96] = 0; + out_3374367099061520939[97] = 0; + out_3374367099061520939[98] = 0; + out_3374367099061520939[99] = 0; + out_3374367099061520939[100] = 0; + out_3374367099061520939[101] = 0; + out_3374367099061520939[102] = 0; + out_3374367099061520939[103] = 0; + out_3374367099061520939[104] = dt; + out_3374367099061520939[105] = 0; + out_3374367099061520939[106] = 0; + out_3374367099061520939[107] = 0; + out_3374367099061520939[108] = 0; + out_3374367099061520939[109] = 0; + out_3374367099061520939[110] = 0; + out_3374367099061520939[111] = 0; + out_3374367099061520939[112] = 0; + out_3374367099061520939[113] = 0; + out_3374367099061520939[114] = 1; + out_3374367099061520939[115] = 0; + out_3374367099061520939[116] = 0; + out_3374367099061520939[117] = 0; + out_3374367099061520939[118] = 0; + out_3374367099061520939[119] = 0; + out_3374367099061520939[120] = 0; + out_3374367099061520939[121] = 0; + out_3374367099061520939[122] = 0; + out_3374367099061520939[123] = 0; + out_3374367099061520939[124] = 0; + out_3374367099061520939[125] = 0; + out_3374367099061520939[126] = 0; + out_3374367099061520939[127] = 0; + out_3374367099061520939[128] = 0; + out_3374367099061520939[129] = 0; + out_3374367099061520939[130] = 0; + out_3374367099061520939[131] = 0; + out_3374367099061520939[132] = 0; + out_3374367099061520939[133] = 1; + out_3374367099061520939[134] = 0; + out_3374367099061520939[135] = 0; + out_3374367099061520939[136] = 0; + out_3374367099061520939[137] = 0; + out_3374367099061520939[138] = 0; + out_3374367099061520939[139] = 0; + out_3374367099061520939[140] = 0; + out_3374367099061520939[141] = 0; + out_3374367099061520939[142] = 0; + out_3374367099061520939[143] = 0; + out_3374367099061520939[144] = 0; + out_3374367099061520939[145] = 0; + out_3374367099061520939[146] = 0; + out_3374367099061520939[147] = 0; + out_3374367099061520939[148] = 0; + out_3374367099061520939[149] = 0; + out_3374367099061520939[150] = 0; + out_3374367099061520939[151] = 0; + out_3374367099061520939[152] = 1; + out_3374367099061520939[153] = 0; + out_3374367099061520939[154] = 0; + out_3374367099061520939[155] = 0; + out_3374367099061520939[156] = 0; + out_3374367099061520939[157] = 0; + out_3374367099061520939[158] = 0; + out_3374367099061520939[159] = 0; + out_3374367099061520939[160] = 0; + out_3374367099061520939[161] = 0; + out_3374367099061520939[162] = 0; + out_3374367099061520939[163] = 0; + out_3374367099061520939[164] = 0; + out_3374367099061520939[165] = 0; + out_3374367099061520939[166] = 0; + out_3374367099061520939[167] = 0; + out_3374367099061520939[168] = 0; + out_3374367099061520939[169] = 0; + out_3374367099061520939[170] = 0; + out_3374367099061520939[171] = 1; + out_3374367099061520939[172] = 0; + out_3374367099061520939[173] = 0; + out_3374367099061520939[174] = 0; + out_3374367099061520939[175] = 0; + out_3374367099061520939[176] = 0; + out_3374367099061520939[177] = 0; + out_3374367099061520939[178] = 0; + out_3374367099061520939[179] = 0; + out_3374367099061520939[180] = 0; + out_3374367099061520939[181] = 0; + out_3374367099061520939[182] = 0; + out_3374367099061520939[183] = 0; + out_3374367099061520939[184] = 0; + out_3374367099061520939[185] = 0; + out_3374367099061520939[186] = 0; + out_3374367099061520939[187] = 0; + out_3374367099061520939[188] = 0; + out_3374367099061520939[189] = 0; + out_3374367099061520939[190] = 1; + out_3374367099061520939[191] = 0; + out_3374367099061520939[192] = 0; + out_3374367099061520939[193] = 0; + out_3374367099061520939[194] = 0; + out_3374367099061520939[195] = 0; + out_3374367099061520939[196] = 0; + out_3374367099061520939[197] = 0; + out_3374367099061520939[198] = 0; + out_3374367099061520939[199] = 0; + out_3374367099061520939[200] = 0; + out_3374367099061520939[201] = 0; + out_3374367099061520939[202] = 0; + out_3374367099061520939[203] = 0; + out_3374367099061520939[204] = 0; + out_3374367099061520939[205] = 0; + out_3374367099061520939[206] = 0; + out_3374367099061520939[207] = 0; + out_3374367099061520939[208] = 0; + out_3374367099061520939[209] = 1; + out_3374367099061520939[210] = 0; + out_3374367099061520939[211] = 0; + out_3374367099061520939[212] = 0; + out_3374367099061520939[213] = 0; + out_3374367099061520939[214] = 0; + out_3374367099061520939[215] = 0; + out_3374367099061520939[216] = 0; + out_3374367099061520939[217] = 0; + out_3374367099061520939[218] = 0; + out_3374367099061520939[219] = 0; + out_3374367099061520939[220] = 0; + out_3374367099061520939[221] = 0; + out_3374367099061520939[222] = 0; + out_3374367099061520939[223] = 0; + out_3374367099061520939[224] = 0; + out_3374367099061520939[225] = 0; + out_3374367099061520939[226] = 0; + out_3374367099061520939[227] = 0; + out_3374367099061520939[228] = 1; + out_3374367099061520939[229] = 0; + out_3374367099061520939[230] = 0; + out_3374367099061520939[231] = 0; + out_3374367099061520939[232] = 0; + out_3374367099061520939[233] = 0; + out_3374367099061520939[234] = 0; + out_3374367099061520939[235] = 0; + out_3374367099061520939[236] = 0; + out_3374367099061520939[237] = 0; + out_3374367099061520939[238] = 0; + out_3374367099061520939[239] = 0; + out_3374367099061520939[240] = 0; + out_3374367099061520939[241] = 0; + out_3374367099061520939[242] = 0; + out_3374367099061520939[243] = 0; + out_3374367099061520939[244] = 0; + out_3374367099061520939[245] = 0; + out_3374367099061520939[246] = 0; + out_3374367099061520939[247] = 1; + out_3374367099061520939[248] = 0; + out_3374367099061520939[249] = 0; + out_3374367099061520939[250] = 0; + out_3374367099061520939[251] = 0; + out_3374367099061520939[252] = 0; + out_3374367099061520939[253] = 0; + out_3374367099061520939[254] = 0; + out_3374367099061520939[255] = 0; + out_3374367099061520939[256] = 0; + out_3374367099061520939[257] = 0; + out_3374367099061520939[258] = 0; + out_3374367099061520939[259] = 0; + out_3374367099061520939[260] = 0; + out_3374367099061520939[261] = 0; + out_3374367099061520939[262] = 0; + out_3374367099061520939[263] = 0; + out_3374367099061520939[264] = 0; + out_3374367099061520939[265] = 0; + out_3374367099061520939[266] = 1; + out_3374367099061520939[267] = 0; + out_3374367099061520939[268] = 0; + out_3374367099061520939[269] = 0; + out_3374367099061520939[270] = 0; + out_3374367099061520939[271] = 0; + out_3374367099061520939[272] = 0; + out_3374367099061520939[273] = 0; + out_3374367099061520939[274] = 0; + out_3374367099061520939[275] = 0; + out_3374367099061520939[276] = 0; + out_3374367099061520939[277] = 0; + out_3374367099061520939[278] = 0; + out_3374367099061520939[279] = 0; + out_3374367099061520939[280] = 0; + out_3374367099061520939[281] = 0; + out_3374367099061520939[282] = 0; + out_3374367099061520939[283] = 0; + out_3374367099061520939[284] = 0; + out_3374367099061520939[285] = 1; + out_3374367099061520939[286] = 0; + out_3374367099061520939[287] = 0; + out_3374367099061520939[288] = 0; + out_3374367099061520939[289] = 0; + out_3374367099061520939[290] = 0; + out_3374367099061520939[291] = 0; + out_3374367099061520939[292] = 0; + out_3374367099061520939[293] = 0; + out_3374367099061520939[294] = 0; + out_3374367099061520939[295] = 0; + out_3374367099061520939[296] = 0; + out_3374367099061520939[297] = 0; + out_3374367099061520939[298] = 0; + out_3374367099061520939[299] = 0; + out_3374367099061520939[300] = 0; + out_3374367099061520939[301] = 0; + out_3374367099061520939[302] = 0; + out_3374367099061520939[303] = 0; + out_3374367099061520939[304] = 1; + out_3374367099061520939[305] = 0; + out_3374367099061520939[306] = 0; + out_3374367099061520939[307] = 0; + out_3374367099061520939[308] = 0; + out_3374367099061520939[309] = 0; + out_3374367099061520939[310] = 0; + out_3374367099061520939[311] = 0; + out_3374367099061520939[312] = 0; + out_3374367099061520939[313] = 0; + out_3374367099061520939[314] = 0; + out_3374367099061520939[315] = 0; + out_3374367099061520939[316] = 0; + out_3374367099061520939[317] = 0; + out_3374367099061520939[318] = 0; + out_3374367099061520939[319] = 0; + out_3374367099061520939[320] = 0; + out_3374367099061520939[321] = 0; + out_3374367099061520939[322] = 0; + out_3374367099061520939[323] = 1; } -void h_4(double *state, double *unused, double *out_7211674724064901455) { - out_7211674724064901455[0] = state[6] + state[9]; - out_7211674724064901455[1] = state[7] + state[10]; - out_7211674724064901455[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_583201360619966174) { + out_583201360619966174[0] = state[6] + state[9]; + out_583201360619966174[1] = state[7] + state[10]; + out_583201360619966174[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_8220924299755286163) { - out_8220924299755286163[0] = 0; - out_8220924299755286163[1] = 0; - out_8220924299755286163[2] = 0; - out_8220924299755286163[3] = 0; - out_8220924299755286163[4] = 0; - out_8220924299755286163[5] = 0; - out_8220924299755286163[6] = 1; - out_8220924299755286163[7] = 0; - out_8220924299755286163[8] = 0; - out_8220924299755286163[9] = 1; - out_8220924299755286163[10] = 0; - out_8220924299755286163[11] = 0; - out_8220924299755286163[12] = 0; - out_8220924299755286163[13] = 0; - out_8220924299755286163[14] = 0; - out_8220924299755286163[15] = 0; - out_8220924299755286163[16] = 0; - out_8220924299755286163[17] = 0; - out_8220924299755286163[18] = 0; - out_8220924299755286163[19] = 0; - out_8220924299755286163[20] = 0; - out_8220924299755286163[21] = 0; - out_8220924299755286163[22] = 0; - out_8220924299755286163[23] = 0; - out_8220924299755286163[24] = 0; - out_8220924299755286163[25] = 1; - out_8220924299755286163[26] = 0; - out_8220924299755286163[27] = 0; - out_8220924299755286163[28] = 1; - out_8220924299755286163[29] = 0; - out_8220924299755286163[30] = 0; - out_8220924299755286163[31] = 0; - out_8220924299755286163[32] = 0; - out_8220924299755286163[33] = 0; - out_8220924299755286163[34] = 0; - out_8220924299755286163[35] = 0; - out_8220924299755286163[36] = 0; - out_8220924299755286163[37] = 0; - out_8220924299755286163[38] = 0; - out_8220924299755286163[39] = 0; - out_8220924299755286163[40] = 0; - out_8220924299755286163[41] = 0; - out_8220924299755286163[42] = 0; - out_8220924299755286163[43] = 0; - out_8220924299755286163[44] = 1; - out_8220924299755286163[45] = 0; - out_8220924299755286163[46] = 0; - out_8220924299755286163[47] = 1; - out_8220924299755286163[48] = 0; - out_8220924299755286163[49] = 0; - out_8220924299755286163[50] = 0; - out_8220924299755286163[51] = 0; - out_8220924299755286163[52] = 0; - out_8220924299755286163[53] = 0; +void H_4(double *state, double *unused, double *out_402417908502924402) { + out_402417908502924402[0] = 0; + out_402417908502924402[1] = 0; + out_402417908502924402[2] = 0; + out_402417908502924402[3] = 0; + out_402417908502924402[4] = 0; + out_402417908502924402[5] = 0; + out_402417908502924402[6] = 1; + out_402417908502924402[7] = 0; + out_402417908502924402[8] = 0; + out_402417908502924402[9] = 1; + out_402417908502924402[10] = 0; + out_402417908502924402[11] = 0; + out_402417908502924402[12] = 0; + out_402417908502924402[13] = 0; + out_402417908502924402[14] = 0; + out_402417908502924402[15] = 0; + out_402417908502924402[16] = 0; + out_402417908502924402[17] = 0; + out_402417908502924402[18] = 0; + out_402417908502924402[19] = 0; + out_402417908502924402[20] = 0; + out_402417908502924402[21] = 0; + out_402417908502924402[22] = 0; + out_402417908502924402[23] = 0; + out_402417908502924402[24] = 0; + out_402417908502924402[25] = 1; + out_402417908502924402[26] = 0; + out_402417908502924402[27] = 0; + out_402417908502924402[28] = 1; + out_402417908502924402[29] = 0; + out_402417908502924402[30] = 0; + out_402417908502924402[31] = 0; + out_402417908502924402[32] = 0; + out_402417908502924402[33] = 0; + out_402417908502924402[34] = 0; + out_402417908502924402[35] = 0; + out_402417908502924402[36] = 0; + out_402417908502924402[37] = 0; + out_402417908502924402[38] = 0; + out_402417908502924402[39] = 0; + out_402417908502924402[40] = 0; + out_402417908502924402[41] = 0; + out_402417908502924402[42] = 0; + out_402417908502924402[43] = 0; + out_402417908502924402[44] = 1; + out_402417908502924402[45] = 0; + out_402417908502924402[46] = 0; + out_402417908502924402[47] = 1; + out_402417908502924402[48] = 0; + out_402417908502924402[49] = 0; + out_402417908502924402[50] = 0; + out_402417908502924402[51] = 0; + out_402417908502924402[52] = 0; + out_402417908502924402[53] = 0; } -void h_10(double *state, double *unused, double *out_7091555343827122853) { - out_7091555343827122853[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_7091555343827122853[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_7091555343827122853[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_356753245810915962) { + out_356753245810915962[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_356753245810915962[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_356753245810915962[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_3357399461201359920) { - out_3357399461201359920[0] = 0; - out_3357399461201359920[1] = 9.8100000000000005*cos(state[1]); - out_3357399461201359920[2] = 0; - out_3357399461201359920[3] = 0; - out_3357399461201359920[4] = -state[8]; - out_3357399461201359920[5] = state[7]; - out_3357399461201359920[6] = 0; - out_3357399461201359920[7] = state[5]; - out_3357399461201359920[8] = -state[4]; - out_3357399461201359920[9] = 0; - out_3357399461201359920[10] = 0; - out_3357399461201359920[11] = 0; - out_3357399461201359920[12] = 1; - out_3357399461201359920[13] = 0; - out_3357399461201359920[14] = 0; - out_3357399461201359920[15] = 1; - out_3357399461201359920[16] = 0; - out_3357399461201359920[17] = 0; - out_3357399461201359920[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_3357399461201359920[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_3357399461201359920[20] = 0; - out_3357399461201359920[21] = state[8]; - out_3357399461201359920[22] = 0; - out_3357399461201359920[23] = -state[6]; - out_3357399461201359920[24] = -state[5]; - out_3357399461201359920[25] = 0; - out_3357399461201359920[26] = state[3]; - out_3357399461201359920[27] = 0; - out_3357399461201359920[28] = 0; - out_3357399461201359920[29] = 0; - out_3357399461201359920[30] = 0; - out_3357399461201359920[31] = 1; - out_3357399461201359920[32] = 0; - out_3357399461201359920[33] = 0; - out_3357399461201359920[34] = 1; - out_3357399461201359920[35] = 0; - out_3357399461201359920[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_3357399461201359920[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_3357399461201359920[38] = 0; - out_3357399461201359920[39] = -state[7]; - out_3357399461201359920[40] = state[6]; - out_3357399461201359920[41] = 0; - out_3357399461201359920[42] = state[4]; - out_3357399461201359920[43] = -state[3]; - out_3357399461201359920[44] = 0; - out_3357399461201359920[45] = 0; - out_3357399461201359920[46] = 0; - out_3357399461201359920[47] = 0; - out_3357399461201359920[48] = 0; - out_3357399461201359920[49] = 0; - out_3357399461201359920[50] = 1; - out_3357399461201359920[51] = 0; - out_3357399461201359920[52] = 0; - out_3357399461201359920[53] = 1; +void H_10(double *state, double *unused, double *out_7727584574864679993) { + out_7727584574864679993[0] = 0; + out_7727584574864679993[1] = 9.8100000000000005*cos(state[1]); + out_7727584574864679993[2] = 0; + out_7727584574864679993[3] = 0; + out_7727584574864679993[4] = -state[8]; + out_7727584574864679993[5] = state[7]; + out_7727584574864679993[6] = 0; + out_7727584574864679993[7] = state[5]; + out_7727584574864679993[8] = -state[4]; + out_7727584574864679993[9] = 0; + out_7727584574864679993[10] = 0; + out_7727584574864679993[11] = 0; + out_7727584574864679993[12] = 1; + out_7727584574864679993[13] = 0; + out_7727584574864679993[14] = 0; + out_7727584574864679993[15] = 1; + out_7727584574864679993[16] = 0; + out_7727584574864679993[17] = 0; + out_7727584574864679993[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_7727584574864679993[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_7727584574864679993[20] = 0; + out_7727584574864679993[21] = state[8]; + out_7727584574864679993[22] = 0; + out_7727584574864679993[23] = -state[6]; + out_7727584574864679993[24] = -state[5]; + out_7727584574864679993[25] = 0; + out_7727584574864679993[26] = state[3]; + out_7727584574864679993[27] = 0; + out_7727584574864679993[28] = 0; + out_7727584574864679993[29] = 0; + out_7727584574864679993[30] = 0; + out_7727584574864679993[31] = 1; + out_7727584574864679993[32] = 0; + out_7727584574864679993[33] = 0; + out_7727584574864679993[34] = 1; + out_7727584574864679993[35] = 0; + out_7727584574864679993[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_7727584574864679993[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_7727584574864679993[38] = 0; + out_7727584574864679993[39] = -state[7]; + out_7727584574864679993[40] = state[6]; + out_7727584574864679993[41] = 0; + out_7727584574864679993[42] = state[4]; + out_7727584574864679993[43] = -state[3]; + out_7727584574864679993[44] = 0; + out_7727584574864679993[45] = 0; + out_7727584574864679993[46] = 0; + out_7727584574864679993[47] = 0; + out_7727584574864679993[48] = 0; + out_7727584574864679993[49] = 0; + out_7727584574864679993[50] = 1; + out_7727584574864679993[51] = 0; + out_7727584574864679993[52] = 0; + out_7727584574864679993[53] = 1; } -void h_13(double *state, double *unused, double *out_4463366606757688719) { - out_4463366606757688719[0] = state[3]; - out_4463366606757688719[1] = state[4]; - out_4463366606757688719[2] = state[5]; +void h_13(double *state, double *unused, double *out_1340717926179505345) { + out_1340717926179505345[0] = state[3]; + out_1340717926179505345[1] = state[4]; + out_1340717926179505345[2] = state[5]; } -void H_13(double *state, double *unused, double *out_7013545948621932652) { - out_7013545948621932652[0] = 0; - out_7013545948621932652[1] = 0; - out_7013545948621932652[2] = 0; - out_7013545948621932652[3] = 1; - out_7013545948621932652[4] = 0; - out_7013545948621932652[5] = 0; - out_7013545948621932652[6] = 0; - out_7013545948621932652[7] = 0; - out_7013545948621932652[8] = 0; - out_7013545948621932652[9] = 0; - out_7013545948621932652[10] = 0; - out_7013545948621932652[11] = 0; - out_7013545948621932652[12] = 0; - out_7013545948621932652[13] = 0; - out_7013545948621932652[14] = 0; - out_7013545948621932652[15] = 0; - out_7013545948621932652[16] = 0; - out_7013545948621932652[17] = 0; - out_7013545948621932652[18] = 0; - out_7013545948621932652[19] = 0; - out_7013545948621932652[20] = 0; - out_7013545948621932652[21] = 0; - out_7013545948621932652[22] = 1; - out_7013545948621932652[23] = 0; - out_7013545948621932652[24] = 0; - out_7013545948621932652[25] = 0; - out_7013545948621932652[26] = 0; - out_7013545948621932652[27] = 0; - out_7013545948621932652[28] = 0; - out_7013545948621932652[29] = 0; - out_7013545948621932652[30] = 0; - out_7013545948621932652[31] = 0; - out_7013545948621932652[32] = 0; - out_7013545948621932652[33] = 0; - out_7013545948621932652[34] = 0; - out_7013545948621932652[35] = 0; - out_7013545948621932652[36] = 0; - out_7013545948621932652[37] = 0; - out_7013545948621932652[38] = 0; - out_7013545948621932652[39] = 0; - out_7013545948621932652[40] = 0; - out_7013545948621932652[41] = 1; - out_7013545948621932652[42] = 0; - out_7013545948621932652[43] = 0; - out_7013545948621932652[44] = 0; - out_7013545948621932652[45] = 0; - out_7013545948621932652[46] = 0; - out_7013545948621932652[47] = 0; - out_7013545948621932652[48] = 0; - out_7013545948621932652[49] = 0; - out_7013545948621932652[50] = 0; - out_7013545948621932652[51] = 0; - out_7013545948621932652[52] = 0; - out_7013545948621932652[53] = 0; +void H_13(double *state, double *unused, double *out_7208213299813776527) { + out_7208213299813776527[0] = 0; + out_7208213299813776527[1] = 0; + out_7208213299813776527[2] = 0; + out_7208213299813776527[3] = 1; + out_7208213299813776527[4] = 0; + out_7208213299813776527[5] = 0; + out_7208213299813776527[6] = 0; + out_7208213299813776527[7] = 0; + out_7208213299813776527[8] = 0; + out_7208213299813776527[9] = 0; + out_7208213299813776527[10] = 0; + out_7208213299813776527[11] = 0; + out_7208213299813776527[12] = 0; + out_7208213299813776527[13] = 0; + out_7208213299813776527[14] = 0; + out_7208213299813776527[15] = 0; + out_7208213299813776527[16] = 0; + out_7208213299813776527[17] = 0; + out_7208213299813776527[18] = 0; + out_7208213299813776527[19] = 0; + out_7208213299813776527[20] = 0; + out_7208213299813776527[21] = 0; + out_7208213299813776527[22] = 1; + out_7208213299813776527[23] = 0; + out_7208213299813776527[24] = 0; + out_7208213299813776527[25] = 0; + out_7208213299813776527[26] = 0; + out_7208213299813776527[27] = 0; + out_7208213299813776527[28] = 0; + out_7208213299813776527[29] = 0; + out_7208213299813776527[30] = 0; + out_7208213299813776527[31] = 0; + out_7208213299813776527[32] = 0; + out_7208213299813776527[33] = 0; + out_7208213299813776527[34] = 0; + out_7208213299813776527[35] = 0; + out_7208213299813776527[36] = 0; + out_7208213299813776527[37] = 0; + out_7208213299813776527[38] = 0; + out_7208213299813776527[39] = 0; + out_7208213299813776527[40] = 0; + out_7208213299813776527[41] = 1; + out_7208213299813776527[42] = 0; + out_7208213299813776527[43] = 0; + out_7208213299813776527[44] = 0; + out_7208213299813776527[45] = 0; + out_7208213299813776527[46] = 0; + out_7208213299813776527[47] = 0; + out_7208213299813776527[48] = 0; + out_7208213299813776527[49] = 0; + out_7208213299813776527[50] = 0; + out_7208213299813776527[51] = 0; + out_7208213299813776527[52] = 0; + out_7208213299813776527[53] = 0; } -void h_14(double *state, double *unused, double *out_2594435200759890716) { - out_2594435200759890716[0] = state[6]; - out_2594435200759890716[1] = state[7]; - out_2594435200759890716[2] = state[8]; +void h_14(double *state, double *unused, double *out_4852912044053324854) { + out_4852912044053324854[0] = state[6]; + out_4852912044053324854[1] = state[7]; + out_4852912044053324854[2] = state[8]; } -void H_14(double *state, double *unused, double *out_6262578917614780924) { - out_6262578917614780924[0] = 0; - out_6262578917614780924[1] = 0; - out_6262578917614780924[2] = 0; - out_6262578917614780924[3] = 0; - out_6262578917614780924[4] = 0; - out_6262578917614780924[5] = 0; - out_6262578917614780924[6] = 1; - out_6262578917614780924[7] = 0; - out_6262578917614780924[8] = 0; - out_6262578917614780924[9] = 0; - out_6262578917614780924[10] = 0; - out_6262578917614780924[11] = 0; - out_6262578917614780924[12] = 0; - out_6262578917614780924[13] = 0; - out_6262578917614780924[14] = 0; - out_6262578917614780924[15] = 0; - out_6262578917614780924[16] = 0; - out_6262578917614780924[17] = 0; - out_6262578917614780924[18] = 0; - out_6262578917614780924[19] = 0; - out_6262578917614780924[20] = 0; - out_6262578917614780924[21] = 0; - out_6262578917614780924[22] = 0; - out_6262578917614780924[23] = 0; - out_6262578917614780924[24] = 0; - out_6262578917614780924[25] = 1; - out_6262578917614780924[26] = 0; - out_6262578917614780924[27] = 0; - out_6262578917614780924[28] = 0; - out_6262578917614780924[29] = 0; - out_6262578917614780924[30] = 0; - out_6262578917614780924[31] = 0; - out_6262578917614780924[32] = 0; - out_6262578917614780924[33] = 0; - out_6262578917614780924[34] = 0; - out_6262578917614780924[35] = 0; - out_6262578917614780924[36] = 0; - out_6262578917614780924[37] = 0; - out_6262578917614780924[38] = 0; - out_6262578917614780924[39] = 0; - out_6262578917614780924[40] = 0; - out_6262578917614780924[41] = 0; - out_6262578917614780924[42] = 0; - out_6262578917614780924[43] = 0; - out_6262578917614780924[44] = 1; - out_6262578917614780924[45] = 0; - out_6262578917614780924[46] = 0; - out_6262578917614780924[47] = 0; - out_6262578917614780924[48] = 0; - out_6262578917614780924[49] = 0; - out_6262578917614780924[50] = 0; - out_6262578917614780924[51] = 0; - out_6262578917614780924[52] = 0; - out_6262578917614780924[53] = 0; +void H_14(double *state, double *unused, double *out_3560822947836560127) { + out_3560822947836560127[0] = 0; + out_3560822947836560127[1] = 0; + out_3560822947836560127[2] = 0; + out_3560822947836560127[3] = 0; + out_3560822947836560127[4] = 0; + out_3560822947836560127[5] = 0; + out_3560822947836560127[6] = 1; + out_3560822947836560127[7] = 0; + out_3560822947836560127[8] = 0; + out_3560822947836560127[9] = 0; + out_3560822947836560127[10] = 0; + out_3560822947836560127[11] = 0; + out_3560822947836560127[12] = 0; + out_3560822947836560127[13] = 0; + out_3560822947836560127[14] = 0; + out_3560822947836560127[15] = 0; + out_3560822947836560127[16] = 0; + out_3560822947836560127[17] = 0; + out_3560822947836560127[18] = 0; + out_3560822947836560127[19] = 0; + out_3560822947836560127[20] = 0; + out_3560822947836560127[21] = 0; + out_3560822947836560127[22] = 0; + out_3560822947836560127[23] = 0; + out_3560822947836560127[24] = 0; + out_3560822947836560127[25] = 1; + out_3560822947836560127[26] = 0; + out_3560822947836560127[27] = 0; + out_3560822947836560127[28] = 0; + out_3560822947836560127[29] = 0; + out_3560822947836560127[30] = 0; + out_3560822947836560127[31] = 0; + out_3560822947836560127[32] = 0; + out_3560822947836560127[33] = 0; + out_3560822947836560127[34] = 0; + out_3560822947836560127[35] = 0; + out_3560822947836560127[36] = 0; + out_3560822947836560127[37] = 0; + out_3560822947836560127[38] = 0; + out_3560822947836560127[39] = 0; + out_3560822947836560127[40] = 0; + out_3560822947836560127[41] = 0; + out_3560822947836560127[42] = 0; + out_3560822947836560127[43] = 0; + out_3560822947836560127[44] = 1; + out_3560822947836560127[45] = 0; + out_3560822947836560127[46] = 0; + out_3560822947836560127[47] = 0; + out_3560822947836560127[48] = 0; + out_3560822947836560127[49] = 0; + out_3560822947836560127[50] = 0; + out_3560822947836560127[51] = 0; + out_3560822947836560127[52] = 0; + out_3560822947836560127[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_4409321269442810496) { - err_fun(nom_x, delta_x, out_4409321269442810496); +void pose_err_fun(double *nom_x, double *delta_x, double *out_7992688260990922493) { + err_fun(nom_x, delta_x, out_7992688260990922493); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3789556254182165506) { - inv_err_fun(nom_x, true_x, out_3789556254182165506); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5809721741941332225) { + inv_err_fun(nom_x, true_x, out_5809721741941332225); } -void pose_H_mod_fun(double *state, double *out_3068405914716205428) { - H_mod_fun(state, out_3068405914716205428); +void pose_H_mod_fun(double *state, double *out_653094641834631323) { + H_mod_fun(state, out_653094641834631323); } -void pose_f_fun(double *state, double dt, double *out_5515990809264108996) { - f_fun(state, dt, out_5515990809264108996); +void pose_f_fun(double *state, double dt, double *out_5955110383788875610) { + f_fun(state, dt, out_5955110383788875610); } -void pose_F_fun(double *state, double dt, double *out_1412435369220653896) { - F_fun(state, dt, out_1412435369220653896); +void pose_F_fun(double *state, double dt, double *out_3374367099061520939) { + F_fun(state, dt, out_3374367099061520939); } -void pose_h_4(double *state, double *unused, double *out_7211674724064901455) { - h_4(state, unused, out_7211674724064901455); +void pose_h_4(double *state, double *unused, double *out_583201360619966174) { + h_4(state, unused, out_583201360619966174); } -void pose_H_4(double *state, double *unused, double *out_8220924299755286163) { - H_4(state, unused, out_8220924299755286163); +void pose_H_4(double *state, double *unused, double *out_402417908502924402) { + H_4(state, unused, out_402417908502924402); } -void pose_h_10(double *state, double *unused, double *out_7091555343827122853) { - h_10(state, unused, out_7091555343827122853); +void pose_h_10(double *state, double *unused, double *out_356753245810915962) { + h_10(state, unused, out_356753245810915962); } -void pose_H_10(double *state, double *unused, double *out_3357399461201359920) { - H_10(state, unused, out_3357399461201359920); +void pose_H_10(double *state, double *unused, double *out_7727584574864679993) { + H_10(state, unused, out_7727584574864679993); } -void pose_h_13(double *state, double *unused, double *out_4463366606757688719) { - h_13(state, unused, out_4463366606757688719); +void pose_h_13(double *state, double *unused, double *out_1340717926179505345) { + h_13(state, unused, out_1340717926179505345); } -void pose_H_13(double *state, double *unused, double *out_7013545948621932652) { - H_13(state, unused, out_7013545948621932652); +void pose_H_13(double *state, double *unused, double *out_7208213299813776527) { + H_13(state, unused, out_7208213299813776527); } -void pose_h_14(double *state, double *unused, double *out_2594435200759890716) { - h_14(state, unused, out_2594435200759890716); +void pose_h_14(double *state, double *unused, double *out_4852912044053324854) { + h_14(state, unused, out_4852912044053324854); } -void pose_H_14(double *state, double *unused, double *out_6262578917614780924) { - H_14(state, unused, out_6262578917614780924); +void pose_H_14(double *state, double *unused, double *out_3560822947836560127) { + H_14(state, unused, out_3560822947836560127); } 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 4e3bcad58..14abd9ddf 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_4409321269442810496); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3789556254182165506); -void pose_H_mod_fun(double *state, double *out_3068405914716205428); -void pose_f_fun(double *state, double dt, double *out_5515990809264108996); -void pose_F_fun(double *state, double dt, double *out_1412435369220653896); -void pose_h_4(double *state, double *unused, double *out_7211674724064901455); -void pose_H_4(double *state, double *unused, double *out_8220924299755286163); -void pose_h_10(double *state, double *unused, double *out_7091555343827122853); -void pose_H_10(double *state, double *unused, double *out_3357399461201359920); -void pose_h_13(double *state, double *unused, double *out_4463366606757688719); -void pose_H_13(double *state, double *unused, double *out_7013545948621932652); -void pose_h_14(double *state, double *unused, double *out_2594435200759890716); -void pose_H_14(double *state, double *unused, double *out_6262578917614780924); +void pose_err_fun(double *nom_x, double *delta_x, double *out_7992688260990922493); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5809721741941332225); +void pose_H_mod_fun(double *state, double *out_653094641834631323); +void pose_f_fun(double *state, double dt, double *out_5955110383788875610); +void pose_F_fun(double *state, double dt, double *out_3374367099061520939); +void pose_h_4(double *state, double *unused, double *out_583201360619966174); +void pose_H_4(double *state, double *unused, double *out_402417908502924402); +void pose_h_10(double *state, double *unused, double *out_356753245810915962); +void pose_H_10(double *state, double *unused, double *out_7727584574864679993); +void pose_h_13(double *state, double *unused, double *out_1340717926179505345); +void pose_H_13(double *state, double *unused, double *out_7208213299813776527); +void pose_h_14(double *state, double *unused, double *out_4852912044053324854); +void pose_H_14(double *state, double *unused, double *out_3560822947836560127); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/modeld/models/commonmodel_pyx.so b/selfdrive/modeld/models/commonmodel_pyx.so index edda0400c..3eae92751 100755 Binary files a/selfdrive/modeld/models/commonmodel_pyx.so and b/selfdrive/modeld/models/commonmodel_pyx.so differ diff --git a/selfdrive/pandad/pandad b/selfdrive/pandad/pandad index f4cd6cb6b..1e3c4e49b 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/pandad/pandad_api_impl.so b/selfdrive/pandad/pandad_api_impl.so index 5cff4089b..bc3a7dd77 100755 Binary files a/selfdrive/pandad/pandad_api_impl.so and b/selfdrive/pandad/pandad_api_impl.so differ diff --git a/system/camerad/camerad b/system/camerad/camerad index 51fd93134..deb17d8c0 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index e545d97f9..4c43993b3 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index 7045066ad..5bf8011c9 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index fbbda7bed..4e11f0ad3 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ