diff --git a/CHANGELOGS.md b/CHANGELOGS.md
index 9571f01ee..8e0389608 100644
--- a/CHANGELOGS.md
+++ b/CHANGELOGS.md
@@ -8,6 +8,10 @@ dragonpilot [Latest] - EON/C2 Release
 * NEW: Dynamic End-to-End can be turned on when lead car is going below x km/h. (Useful for stop and go traffic.)
 * NEW: Toggle to adapt Dynamic End-to-End to Following Dist. Mode.
 * NEW: Toggle to adapt Dynamic End-to-End to Accel Mode.
+* NEW: Always On Lateral v2 (For Toyota/VAG/HKG/Honda).
+* NEW: VAG - vw-spam-resume (Thanks to @jyoung8607)
+* FIXED: Bugs in updated.
+* FIXED: Issue in C2 fan control.
 * TWEAK: Bug fixes and improvement.
 
 dragonpilot 2022.11.09 - EON/C2 Release
diff --git a/cereal/messaging/messaging_pyx.cpp b/cereal/messaging/messaging_pyx.cpp
index 534740c01..e6cf5df84 100644
--- a/cereal/messaging/messaging_pyx.cpp
+++ b/cereal/messaging/messaging_pyx.cpp
@@ -1,4 +1,4 @@
-/* Generated by Cython 0.29.26 */
+/* Generated by Cython 0.29.24 */
 
 /* BEGIN: Cython Metadata
 {
@@ -28,8 +28,8 @@ END: Cython Metadata */
 #elif PY_VERSION_HEX < 0x02060000 || (0x03000000 <= PY_VERSION_HEX && PY_VERSION_HEX < 0x03030000)
     #error Cython requires Python 2.6+ or Python 3.3+.
 #else
-#define CYTHON_ABI "0_29_26"
-#define CYTHON_HEX_VERSION 0x001D1AF0
+#define CYTHON_ABI "0_29_24"
+#define CYTHON_HEX_VERSION 0x001D18F0
 #define CYTHON_FUTURE_DIVISION 1
 #include <stddef.h>
 #ifndef offsetof
@@ -176,7 +176,7 @@ END: Cython Metadata */
   #ifndef CYTHON_USE_UNICODE_INTERNALS
     #define CYTHON_USE_UNICODE_INTERNALS 1
   #endif
-  #if PY_VERSION_HEX < 0x030300F0 || PY_VERSION_HEX >= 0x030B00A2
+  #if PY_VERSION_HEX < 0x030300F0
     #undef CYTHON_USE_UNICODE_WRITER
     #define CYTHON_USE_UNICODE_WRITER 0
   #elif !defined(CYTHON_USE_UNICODE_WRITER)
@@ -195,7 +195,7 @@ END: Cython Metadata */
     #define CYTHON_FAST_THREAD_STATE 1
   #endif
   #ifndef CYTHON_FAST_PYCALL
-    #define CYTHON_FAST_PYCALL (PY_VERSION_HEX < 0x030B00A1)
+    #define CYTHON_FAST_PYCALL 1
   #endif
   #ifndef CYTHON_PEP489_MULTI_PHASE_INIT
     #define CYTHON_PEP489_MULTI_PHASE_INIT (PY_VERSION_HEX >= 0x03050000)
@@ -214,9 +214,7 @@ END: Cython Metadata */
 #define CYTHON_FAST_PYCCALL  (CYTHON_FAST_PYCALL && PY_VERSION_HEX >= 0x030600B1)
 #endif
 #if CYTHON_USE_PYLONG_INTERNALS
-  #if PY_MAJOR_VERSION < 3
-    #include "longintrepr.h"
-  #endif
+  #include "longintrepr.h"
   #undef SHIFT
   #undef BASE
   #undef MASK
@@ -347,68 +345,9 @@ class __Pyx_FakeReference {
   #define __Pyx_DefaultClassType PyClass_Type
 #else
   #define __Pyx_BUILTIN_MODULE_NAME "builtins"
-  #define __Pyx_DefaultClassType PyType_Type
-#if PY_VERSION_HEX >= 0x030B00A1
-    static CYTHON_INLINE PyCodeObject* __Pyx_PyCode_New(int a, int k, int l, int s, int f,
-                                                    PyObject *code, PyObject *c, PyObject* n, PyObject *v,
-                                                    PyObject *fv, PyObject *cell, PyObject* fn,
-                                                    PyObject *name, int fline, PyObject *lnos) {
-        PyObject *kwds=NULL, *argcount=NULL, *posonlyargcount=NULL, *kwonlyargcount=NULL;
-        PyObject *nlocals=NULL, *stacksize=NULL, *flags=NULL, *replace=NULL, *call_result=NULL, *empty=NULL;
-        const char *fn_cstr=NULL;
-        const char *name_cstr=NULL;
-        PyCodeObject* co=NULL;
-        PyObject *type, *value, *traceback;
-        PyErr_Fetch(&type, &value, &traceback);
-        if (!(kwds=PyDict_New())) goto end;
-        if (!(argcount=PyLong_FromLong(a))) goto end;
-        if (PyDict_SetItemString(kwds, "co_argcount", argcount) != 0) goto end;
-        if (!(posonlyargcount=PyLong_FromLong(0))) goto end;
-        if (PyDict_SetItemString(kwds, "co_posonlyargcount", posonlyargcount) != 0) goto end;
-        if (!(kwonlyargcount=PyLong_FromLong(k))) goto end;
-        if (PyDict_SetItemString(kwds, "co_kwonlyargcount", kwonlyargcount) != 0) goto end;
-        if (!(nlocals=PyLong_FromLong(l))) goto end;
-        if (PyDict_SetItemString(kwds, "co_nlocals", nlocals) != 0) goto end;
-        if (!(stacksize=PyLong_FromLong(s))) goto end;
-        if (PyDict_SetItemString(kwds, "co_stacksize", stacksize) != 0) goto end;
-        if (!(flags=PyLong_FromLong(f))) goto end;
-        if (PyDict_SetItemString(kwds, "co_flags", flags) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_code", code) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_consts", c) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_names", n) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_varnames", v) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_freevars", fv) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_cellvars", cell) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_linetable", lnos) != 0) goto end;
-        if (!(fn_cstr=PyUnicode_AsUTF8AndSize(fn, NULL))) goto end;
-        if (!(name_cstr=PyUnicode_AsUTF8AndSize(name, NULL))) goto end;
-        if (!(co = PyCode_NewEmpty(fn_cstr, name_cstr, fline))) goto end;
-        if (!(replace = PyObject_GetAttrString((PyObject*)co, "replace"))) goto cleanup_code_too;
-        if (!(empty = PyTuple_New(0))) goto cleanup_code_too; // unfortunately __pyx_empty_tuple isn't available here
-        if (!(call_result = PyObject_Call(replace, empty, kwds))) goto cleanup_code_too;
-        Py_XDECREF((PyObject*)co);
-        co = (PyCodeObject*)call_result;
-        call_result = NULL;
-        if (0) {
-            cleanup_code_too:
-            Py_XDECREF((PyObject*)co);
-            co = NULL;
-        }
-        end:
-        Py_XDECREF(kwds);
-        Py_XDECREF(argcount);
-        Py_XDECREF(posonlyargcount);
-        Py_XDECREF(kwonlyargcount);
-        Py_XDECREF(nlocals);
-        Py_XDECREF(stacksize);
-        Py_XDECREF(replace);
-        Py_XDECREF(call_result);
-        Py_XDECREF(empty);
-        if (type) {
-            PyErr_Restore(type, value, traceback);
-        }
-        return co;
-    }
+#if PY_VERSION_HEX >= 0x030800A4 && PY_VERSION_HEX < 0x030800B2
+  #define __Pyx_PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)\
+          PyCode_New(a, 0, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)
 #else
   #define __Pyx_PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)\
           PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)
@@ -646,10 +585,10 @@ static CYTHON_INLINE void * PyThread_tss_get(Py_tss_t *key) {
 #if PY_VERSION_HEX < 0x030200A4
   typedef long Py_hash_t;
   #define __Pyx_PyInt_FromHash_t PyInt_FromLong
-  #define __Pyx_PyInt_AsHash_t   __Pyx_PyIndex_AsHash_t
+  #define __Pyx_PyInt_AsHash_t   PyInt_AsLong
 #else
   #define __Pyx_PyInt_FromHash_t PyInt_FromSsize_t
-  #define __Pyx_PyInt_AsHash_t   __Pyx_PyIndex_AsSsize_t
+  #define __Pyx_PyInt_AsHash_t   PyInt_AsSsize_t
 #endif
 #if PY_MAJOR_VERSION >= 3
   #define __Pyx_PyMethod_New(func, self, klass) ((self) ? ((void)(klass), PyMethod_New(func, self)) : __Pyx_NewRef(func))
@@ -814,7 +753,6 @@ static CYTHON_INLINE PyObject* __Pyx_PyNumber_IntOrLong(PyObject* x);
     (likely(PyTuple_CheckExact(obj)) ? __Pyx_NewRef(obj) : PySequence_Tuple(obj))
 static CYTHON_INLINE Py_ssize_t __Pyx_PyIndex_AsSsize_t(PyObject*);
 static CYTHON_INLINE PyObject * __Pyx_PyInt_FromSize_t(size_t);
-static CYTHON_INLINE Py_hash_t __Pyx_PyIndex_AsHash_t(PyObject*);
 #if CYTHON_ASSUME_SAFE_MACROS
 #define __pyx_PyFloat_AsDouble(x) (PyFloat_CheckExact(x) ? PyFloat_AS_DOUBLE(x) : PyFloat_AsDouble(x))
 #else
@@ -1182,7 +1120,6 @@ static PyObject *__Pyx_PyFunction_FastCallDict(PyObject *func, PyObject **args,
 #ifndef Py_MEMBER_SIZE
 #define Py_MEMBER_SIZE(type, member) sizeof(((type *)0)->member)
 #endif
-#if CYTHON_FAST_PYCALL
   static size_t __pyx_pyframe_localsplus_offset = 0;
   #include "frameobject.h"
   #define __Pxy_PyFrame_Initialize_Offsets()\
@@ -1190,7 +1127,6 @@ static PyObject *__Pyx_PyFunction_FastCallDict(PyObject *func, PyObject **args,
      (void)(__pyx_pyframe_localsplus_offset = ((size_t)PyFrame_Type.tp_basicsize) - Py_MEMBER_SIZE(PyFrameObject, f_localsplus)))
   #define __Pyx_PyFrame_GetLocalsplus(frame)\
     (assert(__pyx_pyframe_localsplus_offset), (PyObject **)(((char *)(frame)) + __pyx_pyframe_localsplus_offset))
-#endif // CYTHON_FAST_PYCALL
 #endif
 
 /* PyObjectCallMethO.proto */
@@ -3986,9 +3922,6 @@ static PyTypeObject __pyx_type_6cereal_9messaging_13messaging_pyx_Context = {
   #if PY_VERSION_HEX >= 0x030800b4 && PY_VERSION_HEX < 0x03090000
   0, /*tp_print*/
   #endif
-  #if CYTHON_COMPILING_IN_PYPY && PYPY_VERSION_NUM+0 >= 0x06000000
-  0, /*tp_pypy_flags*/
-  #endif
 };
 
 static PyObject *__pyx_tp_new_6cereal_9messaging_13messaging_pyx_Poller(PyTypeObject *t, CYTHON_UNUSED PyObject *a, CYTHON_UNUSED PyObject *k) {
@@ -4122,9 +4055,6 @@ static PyTypeObject __pyx_type_6cereal_9messaging_13messaging_pyx_Poller = {
   #if PY_VERSION_HEX >= 0x030800b4 && PY_VERSION_HEX < 0x03090000
   0, /*tp_print*/
   #endif
-  #if CYTHON_COMPILING_IN_PYPY && PYPY_VERSION_NUM+0 >= 0x06000000
-  0, /*tp_pypy_flags*/
-  #endif
 };
 static struct __pyx_vtabstruct_6cereal_9messaging_13messaging_pyx_SubSocket __pyx_vtable_6cereal_9messaging_13messaging_pyx_SubSocket;
 
@@ -4239,9 +4169,6 @@ static PyTypeObject __pyx_type_6cereal_9messaging_13messaging_pyx_SubSocket = {
   #if PY_VERSION_HEX >= 0x030800b4 && PY_VERSION_HEX < 0x03090000
   0, /*tp_print*/
   #endif
-  #if CYTHON_COMPILING_IN_PYPY && PYPY_VERSION_NUM+0 >= 0x06000000
-  0, /*tp_pypy_flags*/
-  #endif
 };
 
 static PyObject *__pyx_tp_new_6cereal_9messaging_13messaging_pyx_PubSocket(PyTypeObject *t, CYTHON_UNUSED PyObject *a, CYTHON_UNUSED PyObject *k) {
@@ -4352,9 +4279,6 @@ static PyTypeObject __pyx_type_6cereal_9messaging_13messaging_pyx_PubSocket = {
   #if PY_VERSION_HEX >= 0x030800b4 && PY_VERSION_HEX < 0x03090000
   0, /*tp_print*/
   #endif
-  #if CYTHON_COMPILING_IN_PYPY && PYPY_VERSION_NUM+0 >= 0x06000000
-  0, /*tp_pypy_flags*/
-  #endif
 };
 
 static PyMethodDef __pyx_methods[] = {
@@ -6156,7 +6080,7 @@ static int __Pyx_CLineForTraceback(CYTHON_NCP_UNUSED PyThreadState *tstate, int
     }
     if (!use_cline) {
         c_line = 0;
-        (void) PyObject_SetAttr(__pyx_cython_runtime, __pyx_n_s_cline_in_traceback, Py_False);
+        PyObject_SetAttr(__pyx_cython_runtime, __pyx_n_s_cline_in_traceback, Py_False);
     }
     else if (use_cline == Py_False || (use_cline != Py_True && PyObject_Not(use_cline) != 0)) {
         c_line = 0;
@@ -6253,31 +6177,30 @@ static void __pyx_insert_code_object(int code_line, PyCodeObject* code_object) {
 static PyCodeObject* __Pyx_CreateCodeObjectForTraceback(
             const char *funcname, int c_line,
             int py_line, const char *filename) {
-    PyCodeObject *py_code = NULL;
-    PyObject *py_funcname = NULL;
+    PyCodeObject *py_code = 0;
+    PyObject *py_srcfile = 0;
+    PyObject *py_funcname = 0;
     #if PY_MAJOR_VERSION < 3
-    PyObject *py_srcfile = NULL;
     py_srcfile = PyString_FromString(filename);
-    if (!py_srcfile) goto bad;
+    #else
+    py_srcfile = PyUnicode_FromString(filename);
     #endif
+    if (!py_srcfile) goto bad;
     if (c_line) {
         #if PY_MAJOR_VERSION < 3
         py_funcname = PyString_FromFormat( "%s (%s:%d)", funcname, __pyx_cfilenm, c_line);
-        if (!py_funcname) goto bad;
         #else
         py_funcname = PyUnicode_FromFormat( "%s (%s:%d)", funcname, __pyx_cfilenm, c_line);
-        if (!py_funcname) goto bad;
-        funcname = PyUnicode_AsUTF8(py_funcname);
-        if (!funcname) goto bad;
         #endif
     }
     else {
         #if PY_MAJOR_VERSION < 3
         py_funcname = PyString_FromString(funcname);
-        if (!py_funcname) goto bad;
+        #else
+        py_funcname = PyUnicode_FromString(funcname);
         #endif
     }
-    #if PY_MAJOR_VERSION < 3
+    if (!py_funcname) goto bad;
     py_code = __Pyx_PyCode_New(
         0,
         0,
@@ -6296,16 +6219,11 @@ static PyCodeObject* __Pyx_CreateCodeObjectForTraceback(
         __pyx_empty_bytes  /*PyObject *lnotab*/
     );
     Py_DECREF(py_srcfile);
-    #else
-    py_code = PyCode_NewEmpty(filename, funcname, py_line);
-    #endif
-    Py_XDECREF(py_funcname);  // XDECREF since it's only set on Py3 if cline
+    Py_DECREF(py_funcname);
     return py_code;
 bad:
-    Py_XDECREF(py_funcname);
-    #if PY_MAJOR_VERSION < 3
     Py_XDECREF(py_srcfile);
-    #endif
+    Py_XDECREF(py_funcname);
     return NULL;
 }
 static void __Pyx_AddTraceback(const char *funcname, int c_line,
@@ -7153,23 +7071,6 @@ static CYTHON_INLINE Py_ssize_t __Pyx_PyIndex_AsSsize_t(PyObject* b) {
   Py_DECREF(x);
   return ival;
 }
-static CYTHON_INLINE Py_hash_t __Pyx_PyIndex_AsHash_t(PyObject* o) {
-  if (sizeof(Py_hash_t) == sizeof(Py_ssize_t)) {
-    return (Py_hash_t) __Pyx_PyIndex_AsSsize_t(o);
-#if PY_MAJOR_VERSION < 3
-  } else if (likely(PyInt_CheckExact(o))) {
-    return PyInt_AS_LONG(o);
-#endif
-  } else {
-    Py_ssize_t ival;
-    PyObject *x;
-    x = PyNumber_Index(o);
-    if (!x) return -1;
-    ival = PyInt_AsLong(x);
-    Py_DECREF(x);
-    return ival;
-  }
-}
 static CYTHON_INLINE PyObject * __Pyx_PyBool_FromLong(long b) {
   return b ? __Pyx_NewRef(Py_True) : __Pyx_NewRef(Py_False);
 }
diff --git a/cereal/messaging/messaging_pyx.so b/cereal/messaging/messaging_pyx.so
index a4fb858cc..df3dddf07 100755
Binary files a/cereal/messaging/messaging_pyx.so and b/cereal/messaging/messaging_pyx.so differ
diff --git a/cereal/visionipc/visionipc_pyx.so b/cereal/visionipc/visionipc_pyx.so
index 366a4f25d..4bbbe3d68 100755
Binary files a/cereal/visionipc/visionipc_pyx.so and b/cereal/visionipc/visionipc_pyx.so differ
diff --git a/common/clock.cpp b/common/clock.cpp
index c2a5b2412..887714eb4 100644
--- a/common/clock.cpp
+++ b/common/clock.cpp
@@ -1,4 +1,4 @@
-/* Generated by Cython 0.29.26 */
+/* Generated by Cython 0.29.24 */
 
 /* BEGIN: Cython Metadata
 {
@@ -23,8 +23,8 @@ END: Cython Metadata */
 #elif PY_VERSION_HEX < 0x02060000 || (0x03000000 <= PY_VERSION_HEX && PY_VERSION_HEX < 0x03030000)
     #error Cython requires Python 2.6+ or Python 3.3+.
 #else
-#define CYTHON_ABI "0_29_26"
-#define CYTHON_HEX_VERSION 0x001D1AF0
+#define CYTHON_ABI "0_29_24"
+#define CYTHON_HEX_VERSION 0x001D18F0
 #define CYTHON_FUTURE_DIVISION 1
 #include <stddef.h>
 #ifndef offsetof
@@ -171,7 +171,7 @@ END: Cython Metadata */
   #ifndef CYTHON_USE_UNICODE_INTERNALS
     #define CYTHON_USE_UNICODE_INTERNALS 1
   #endif
-  #if PY_VERSION_HEX < 0x030300F0 || PY_VERSION_HEX >= 0x030B00A2
+  #if PY_VERSION_HEX < 0x030300F0
     #undef CYTHON_USE_UNICODE_WRITER
     #define CYTHON_USE_UNICODE_WRITER 0
   #elif !defined(CYTHON_USE_UNICODE_WRITER)
@@ -190,7 +190,7 @@ END: Cython Metadata */
     #define CYTHON_FAST_THREAD_STATE 1
   #endif
   #ifndef CYTHON_FAST_PYCALL
-    #define CYTHON_FAST_PYCALL (PY_VERSION_HEX < 0x030B00A1)
+    #define CYTHON_FAST_PYCALL 1
   #endif
   #ifndef CYTHON_PEP489_MULTI_PHASE_INIT
     #define CYTHON_PEP489_MULTI_PHASE_INIT (PY_VERSION_HEX >= 0x03050000)
@@ -209,9 +209,7 @@ END: Cython Metadata */
 #define CYTHON_FAST_PYCCALL  (CYTHON_FAST_PYCALL && PY_VERSION_HEX >= 0x030600B1)
 #endif
 #if CYTHON_USE_PYLONG_INTERNALS
-  #if PY_MAJOR_VERSION < 3
-    #include "longintrepr.h"
-  #endif
+  #include "longintrepr.h"
   #undef SHIFT
   #undef BASE
   #undef MASK
@@ -342,68 +340,9 @@ class __Pyx_FakeReference {
   #define __Pyx_DefaultClassType PyClass_Type
 #else
   #define __Pyx_BUILTIN_MODULE_NAME "builtins"
-  #define __Pyx_DefaultClassType PyType_Type
-#if PY_VERSION_HEX >= 0x030B00A1
-    static CYTHON_INLINE PyCodeObject* __Pyx_PyCode_New(int a, int k, int l, int s, int f,
-                                                    PyObject *code, PyObject *c, PyObject* n, PyObject *v,
-                                                    PyObject *fv, PyObject *cell, PyObject* fn,
-                                                    PyObject *name, int fline, PyObject *lnos) {
-        PyObject *kwds=NULL, *argcount=NULL, *posonlyargcount=NULL, *kwonlyargcount=NULL;
-        PyObject *nlocals=NULL, *stacksize=NULL, *flags=NULL, *replace=NULL, *call_result=NULL, *empty=NULL;
-        const char *fn_cstr=NULL;
-        const char *name_cstr=NULL;
-        PyCodeObject* co=NULL;
-        PyObject *type, *value, *traceback;
-        PyErr_Fetch(&type, &value, &traceback);
-        if (!(kwds=PyDict_New())) goto end;
-        if (!(argcount=PyLong_FromLong(a))) goto end;
-        if (PyDict_SetItemString(kwds, "co_argcount", argcount) != 0) goto end;
-        if (!(posonlyargcount=PyLong_FromLong(0))) goto end;
-        if (PyDict_SetItemString(kwds, "co_posonlyargcount", posonlyargcount) != 0) goto end;
-        if (!(kwonlyargcount=PyLong_FromLong(k))) goto end;
-        if (PyDict_SetItemString(kwds, "co_kwonlyargcount", kwonlyargcount) != 0) goto end;
-        if (!(nlocals=PyLong_FromLong(l))) goto end;
-        if (PyDict_SetItemString(kwds, "co_nlocals", nlocals) != 0) goto end;
-        if (!(stacksize=PyLong_FromLong(s))) goto end;
-        if (PyDict_SetItemString(kwds, "co_stacksize", stacksize) != 0) goto end;
-        if (!(flags=PyLong_FromLong(f))) goto end;
-        if (PyDict_SetItemString(kwds, "co_flags", flags) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_code", code) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_consts", c) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_names", n) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_varnames", v) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_freevars", fv) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_cellvars", cell) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_linetable", lnos) != 0) goto end;
-        if (!(fn_cstr=PyUnicode_AsUTF8AndSize(fn, NULL))) goto end;
-        if (!(name_cstr=PyUnicode_AsUTF8AndSize(name, NULL))) goto end;
-        if (!(co = PyCode_NewEmpty(fn_cstr, name_cstr, fline))) goto end;
-        if (!(replace = PyObject_GetAttrString((PyObject*)co, "replace"))) goto cleanup_code_too;
-        if (!(empty = PyTuple_New(0))) goto cleanup_code_too; // unfortunately __pyx_empty_tuple isn't available here
-        if (!(call_result = PyObject_Call(replace, empty, kwds))) goto cleanup_code_too;
-        Py_XDECREF((PyObject*)co);
-        co = (PyCodeObject*)call_result;
-        call_result = NULL;
-        if (0) {
-            cleanup_code_too:
-            Py_XDECREF((PyObject*)co);
-            co = NULL;
-        }
-        end:
-        Py_XDECREF(kwds);
-        Py_XDECREF(argcount);
-        Py_XDECREF(posonlyargcount);
-        Py_XDECREF(kwonlyargcount);
-        Py_XDECREF(nlocals);
-        Py_XDECREF(stacksize);
-        Py_XDECREF(replace);
-        Py_XDECREF(call_result);
-        Py_XDECREF(empty);
-        if (type) {
-            PyErr_Restore(type, value, traceback);
-        }
-        return co;
-    }
+#if PY_VERSION_HEX >= 0x030800A4 && PY_VERSION_HEX < 0x030800B2
+  #define __Pyx_PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)\
+          PyCode_New(a, 0, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)
 #else
   #define __Pyx_PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)\
           PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)
@@ -641,10 +580,10 @@ static CYTHON_INLINE void * PyThread_tss_get(Py_tss_t *key) {
 #if PY_VERSION_HEX < 0x030200A4
   typedef long Py_hash_t;
   #define __Pyx_PyInt_FromHash_t PyInt_FromLong
-  #define __Pyx_PyInt_AsHash_t   __Pyx_PyIndex_AsHash_t
+  #define __Pyx_PyInt_AsHash_t   PyInt_AsLong
 #else
   #define __Pyx_PyInt_FromHash_t PyInt_FromSsize_t
-  #define __Pyx_PyInt_AsHash_t   __Pyx_PyIndex_AsSsize_t
+  #define __Pyx_PyInt_AsHash_t   PyInt_AsSsize_t
 #endif
 #if PY_MAJOR_VERSION >= 3
   #define __Pyx_PyMethod_New(func, self, klass) ((self) ? ((void)(klass), PyMethod_New(func, self)) : __Pyx_NewRef(func))
@@ -803,7 +742,6 @@ static CYTHON_INLINE PyObject* __Pyx_PyNumber_IntOrLong(PyObject* x);
     (likely(PyTuple_CheckExact(obj)) ? __Pyx_NewRef(obj) : PySequence_Tuple(obj))
 static CYTHON_INLINE Py_ssize_t __Pyx_PyIndex_AsSsize_t(PyObject*);
 static CYTHON_INLINE PyObject * __Pyx_PyInt_FromSize_t(size_t);
-static CYTHON_INLINE Py_hash_t __Pyx_PyIndex_AsHash_t(PyObject*);
 #if CYTHON_ASSUME_SAFE_MACROS
 #define __pyx_PyFloat_AsDouble(x) (PyFloat_CheckExact(x) ? PyFloat_AS_DOUBLE(x) : PyFloat_AsDouble(x))
 #else
@@ -1864,7 +1802,7 @@ static int __Pyx_CLineForTraceback(CYTHON_NCP_UNUSED PyThreadState *tstate, int
     }
     if (!use_cline) {
         c_line = 0;
-        (void) PyObject_SetAttr(__pyx_cython_runtime, __pyx_n_s_cline_in_traceback, Py_False);
+        PyObject_SetAttr(__pyx_cython_runtime, __pyx_n_s_cline_in_traceback, Py_False);
     }
     else if (use_cline == Py_False || (use_cline != Py_True && PyObject_Not(use_cline) != 0)) {
         c_line = 0;
@@ -1961,31 +1899,30 @@ static void __pyx_insert_code_object(int code_line, PyCodeObject* code_object) {
 static PyCodeObject* __Pyx_CreateCodeObjectForTraceback(
             const char *funcname, int c_line,
             int py_line, const char *filename) {
-    PyCodeObject *py_code = NULL;
-    PyObject *py_funcname = NULL;
+    PyCodeObject *py_code = 0;
+    PyObject *py_srcfile = 0;
+    PyObject *py_funcname = 0;
     #if PY_MAJOR_VERSION < 3
-    PyObject *py_srcfile = NULL;
     py_srcfile = PyString_FromString(filename);
-    if (!py_srcfile) goto bad;
+    #else
+    py_srcfile = PyUnicode_FromString(filename);
     #endif
+    if (!py_srcfile) goto bad;
     if (c_line) {
         #if PY_MAJOR_VERSION < 3
         py_funcname = PyString_FromFormat( "%s (%s:%d)", funcname, __pyx_cfilenm, c_line);
-        if (!py_funcname) goto bad;
         #else
         py_funcname = PyUnicode_FromFormat( "%s (%s:%d)", funcname, __pyx_cfilenm, c_line);
-        if (!py_funcname) goto bad;
-        funcname = PyUnicode_AsUTF8(py_funcname);
-        if (!funcname) goto bad;
         #endif
     }
     else {
         #if PY_MAJOR_VERSION < 3
         py_funcname = PyString_FromString(funcname);
-        if (!py_funcname) goto bad;
+        #else
+        py_funcname = PyUnicode_FromString(funcname);
         #endif
     }
-    #if PY_MAJOR_VERSION < 3
+    if (!py_funcname) goto bad;
     py_code = __Pyx_PyCode_New(
         0,
         0,
@@ -2004,16 +1941,11 @@ static PyCodeObject* __Pyx_CreateCodeObjectForTraceback(
         __pyx_empty_bytes  /*PyObject *lnotab*/
     );
     Py_DECREF(py_srcfile);
-    #else
-    py_code = PyCode_NewEmpty(filename, funcname, py_line);
-    #endif
-    Py_XDECREF(py_funcname);  // XDECREF since it's only set on Py3 if cline
+    Py_DECREF(py_funcname);
     return py_code;
 bad:
-    Py_XDECREF(py_funcname);
-    #if PY_MAJOR_VERSION < 3
     Py_XDECREF(py_srcfile);
-    #endif
+    Py_XDECREF(py_funcname);
     return NULL;
 }
 static void __Pyx_AddTraceback(const char *funcname, int c_line,
@@ -2861,23 +2793,6 @@ static CYTHON_INLINE Py_ssize_t __Pyx_PyIndex_AsSsize_t(PyObject* b) {
   Py_DECREF(x);
   return ival;
 }
-static CYTHON_INLINE Py_hash_t __Pyx_PyIndex_AsHash_t(PyObject* o) {
-  if (sizeof(Py_hash_t) == sizeof(Py_ssize_t)) {
-    return (Py_hash_t) __Pyx_PyIndex_AsSsize_t(o);
-#if PY_MAJOR_VERSION < 3
-  } else if (likely(PyInt_CheckExact(o))) {
-    return PyInt_AS_LONG(o);
-#endif
-  } else {
-    Py_ssize_t ival;
-    PyObject *x;
-    x = PyNumber_Index(o);
-    if (!x) return -1;
-    ival = PyInt_AsLong(x);
-    Py_DECREF(x);
-    return ival;
-  }
-}
 static CYTHON_INLINE PyObject * __Pyx_PyBool_FromLong(long b) {
   return b ? __Pyx_NewRef(Py_True) : __Pyx_NewRef(Py_False);
 }
diff --git a/common/clock.so b/common/clock.so
index 93fbdf0cb..849db7ad2 100755
Binary files a/common/clock.so and b/common/clock.so differ
diff --git a/common/kalman/simple_kalman_impl.cpp b/common/kalman/simple_kalman_impl.cpp
index faa41d50f..5d5b9ee54 100644
--- a/common/kalman/simple_kalman_impl.cpp
+++ b/common/kalman/simple_kalman_impl.cpp
@@ -1,4 +1,4 @@
-/* Generated by Cython 0.29.26 */
+/* Generated by Cython 0.29.24 */
 
 /* BEGIN: Cython Metadata
 {
@@ -22,8 +22,8 @@ END: Cython Metadata */
 #elif PY_VERSION_HEX < 0x02060000 || (0x03000000 <= PY_VERSION_HEX && PY_VERSION_HEX < 0x03030000)
     #error Cython requires Python 2.6+ or Python 3.3+.
 #else
-#define CYTHON_ABI "0_29_26"
-#define CYTHON_HEX_VERSION 0x001D1AF0
+#define CYTHON_ABI "0_29_24"
+#define CYTHON_HEX_VERSION 0x001D18F0
 #define CYTHON_FUTURE_DIVISION 1
 #include <stddef.h>
 #ifndef offsetof
@@ -170,7 +170,7 @@ END: Cython Metadata */
   #ifndef CYTHON_USE_UNICODE_INTERNALS
     #define CYTHON_USE_UNICODE_INTERNALS 1
   #endif
-  #if PY_VERSION_HEX < 0x030300F0 || PY_VERSION_HEX >= 0x030B00A2
+  #if PY_VERSION_HEX < 0x030300F0
     #undef CYTHON_USE_UNICODE_WRITER
     #define CYTHON_USE_UNICODE_WRITER 0
   #elif !defined(CYTHON_USE_UNICODE_WRITER)
@@ -189,7 +189,7 @@ END: Cython Metadata */
     #define CYTHON_FAST_THREAD_STATE 1
   #endif
   #ifndef CYTHON_FAST_PYCALL
-    #define CYTHON_FAST_PYCALL (PY_VERSION_HEX < 0x030B00A1)
+    #define CYTHON_FAST_PYCALL 1
   #endif
   #ifndef CYTHON_PEP489_MULTI_PHASE_INIT
     #define CYTHON_PEP489_MULTI_PHASE_INIT (PY_VERSION_HEX >= 0x03050000)
@@ -208,9 +208,7 @@ END: Cython Metadata */
 #define CYTHON_FAST_PYCCALL  (CYTHON_FAST_PYCALL && PY_VERSION_HEX >= 0x030600B1)
 #endif
 #if CYTHON_USE_PYLONG_INTERNALS
-  #if PY_MAJOR_VERSION < 3
-    #include "longintrepr.h"
-  #endif
+  #include "longintrepr.h"
   #undef SHIFT
   #undef BASE
   #undef MASK
@@ -341,68 +339,9 @@ class __Pyx_FakeReference {
   #define __Pyx_DefaultClassType PyClass_Type
 #else
   #define __Pyx_BUILTIN_MODULE_NAME "builtins"
-  #define __Pyx_DefaultClassType PyType_Type
-#if PY_VERSION_HEX >= 0x030B00A1
-    static CYTHON_INLINE PyCodeObject* __Pyx_PyCode_New(int a, int k, int l, int s, int f,
-                                                    PyObject *code, PyObject *c, PyObject* n, PyObject *v,
-                                                    PyObject *fv, PyObject *cell, PyObject* fn,
-                                                    PyObject *name, int fline, PyObject *lnos) {
-        PyObject *kwds=NULL, *argcount=NULL, *posonlyargcount=NULL, *kwonlyargcount=NULL;
-        PyObject *nlocals=NULL, *stacksize=NULL, *flags=NULL, *replace=NULL, *call_result=NULL, *empty=NULL;
-        const char *fn_cstr=NULL;
-        const char *name_cstr=NULL;
-        PyCodeObject* co=NULL;
-        PyObject *type, *value, *traceback;
-        PyErr_Fetch(&type, &value, &traceback);
-        if (!(kwds=PyDict_New())) goto end;
-        if (!(argcount=PyLong_FromLong(a))) goto end;
-        if (PyDict_SetItemString(kwds, "co_argcount", argcount) != 0) goto end;
-        if (!(posonlyargcount=PyLong_FromLong(0))) goto end;
-        if (PyDict_SetItemString(kwds, "co_posonlyargcount", posonlyargcount) != 0) goto end;
-        if (!(kwonlyargcount=PyLong_FromLong(k))) goto end;
-        if (PyDict_SetItemString(kwds, "co_kwonlyargcount", kwonlyargcount) != 0) goto end;
-        if (!(nlocals=PyLong_FromLong(l))) goto end;
-        if (PyDict_SetItemString(kwds, "co_nlocals", nlocals) != 0) goto end;
-        if (!(stacksize=PyLong_FromLong(s))) goto end;
-        if (PyDict_SetItemString(kwds, "co_stacksize", stacksize) != 0) goto end;
-        if (!(flags=PyLong_FromLong(f))) goto end;
-        if (PyDict_SetItemString(kwds, "co_flags", flags) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_code", code) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_consts", c) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_names", n) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_varnames", v) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_freevars", fv) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_cellvars", cell) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_linetable", lnos) != 0) goto end;
-        if (!(fn_cstr=PyUnicode_AsUTF8AndSize(fn, NULL))) goto end;
-        if (!(name_cstr=PyUnicode_AsUTF8AndSize(name, NULL))) goto end;
-        if (!(co = PyCode_NewEmpty(fn_cstr, name_cstr, fline))) goto end;
-        if (!(replace = PyObject_GetAttrString((PyObject*)co, "replace"))) goto cleanup_code_too;
-        if (!(empty = PyTuple_New(0))) goto cleanup_code_too; // unfortunately __pyx_empty_tuple isn't available here
-        if (!(call_result = PyObject_Call(replace, empty, kwds))) goto cleanup_code_too;
-        Py_XDECREF((PyObject*)co);
-        co = (PyCodeObject*)call_result;
-        call_result = NULL;
-        if (0) {
-            cleanup_code_too:
-            Py_XDECREF((PyObject*)co);
-            co = NULL;
-        }
-        end:
-        Py_XDECREF(kwds);
-        Py_XDECREF(argcount);
-        Py_XDECREF(posonlyargcount);
-        Py_XDECREF(kwonlyargcount);
-        Py_XDECREF(nlocals);
-        Py_XDECREF(stacksize);
-        Py_XDECREF(replace);
-        Py_XDECREF(call_result);
-        Py_XDECREF(empty);
-        if (type) {
-            PyErr_Restore(type, value, traceback);
-        }
-        return co;
-    }
+#if PY_VERSION_HEX >= 0x030800A4 && PY_VERSION_HEX < 0x030800B2
+  #define __Pyx_PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)\
+          PyCode_New(a, 0, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)
 #else
   #define __Pyx_PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)\
           PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)
@@ -640,10 +579,10 @@ static CYTHON_INLINE void * PyThread_tss_get(Py_tss_t *key) {
 #if PY_VERSION_HEX < 0x030200A4
   typedef long Py_hash_t;
   #define __Pyx_PyInt_FromHash_t PyInt_FromLong
-  #define __Pyx_PyInt_AsHash_t   __Pyx_PyIndex_AsHash_t
+  #define __Pyx_PyInt_AsHash_t   PyInt_AsLong
 #else
   #define __Pyx_PyInt_FromHash_t PyInt_FromSsize_t
-  #define __Pyx_PyInt_AsHash_t   __Pyx_PyIndex_AsSsize_t
+  #define __Pyx_PyInt_AsHash_t   PyInt_AsSsize_t
 #endif
 #if PY_MAJOR_VERSION >= 3
   #define __Pyx_PyMethod_New(func, self, klass) ((self) ? ((void)(klass), PyMethod_New(func, self)) : __Pyx_NewRef(func))
@@ -799,7 +738,6 @@ static CYTHON_INLINE PyObject* __Pyx_PyNumber_IntOrLong(PyObject* x);
     (likely(PyTuple_CheckExact(obj)) ? __Pyx_NewRef(obj) : PySequence_Tuple(obj))
 static CYTHON_INLINE Py_ssize_t __Pyx_PyIndex_AsSsize_t(PyObject*);
 static CYTHON_INLINE PyObject * __Pyx_PyInt_FromSize_t(size_t);
-static CYTHON_INLINE Py_hash_t __Pyx_PyIndex_AsHash_t(PyObject*);
 #if CYTHON_ASSUME_SAFE_MACROS
 #define __pyx_PyFloat_AsDouble(x) (PyFloat_CheckExact(x) ? PyFloat_AS_DOUBLE(x) : PyFloat_AsDouble(x))
 #else
@@ -1178,7 +1116,6 @@ static PyObject *__Pyx_PyFunction_FastCallDict(PyObject *func, PyObject **args,
 #ifndef Py_MEMBER_SIZE
 #define Py_MEMBER_SIZE(type, member) sizeof(((type *)0)->member)
 #endif
-#if CYTHON_FAST_PYCALL
   static size_t __pyx_pyframe_localsplus_offset = 0;
   #include "frameobject.h"
   #define __Pxy_PyFrame_Initialize_Offsets()\
@@ -1186,7 +1123,6 @@ static PyObject *__Pyx_PyFunction_FastCallDict(PyObject *func, PyObject **args,
      (void)(__pyx_pyframe_localsplus_offset = ((size_t)PyFrame_Type.tp_basicsize) - Py_MEMBER_SIZE(PyFrameObject, f_localsplus)))
   #define __Pyx_PyFrame_GetLocalsplus(frame)\
     (assert(__pyx_pyframe_localsplus_offset), (PyObject **)(((char *)(frame)) + __pyx_pyframe_localsplus_offset))
-#endif // CYTHON_FAST_PYCALL
 #endif
 
 /* PyObjectCall.proto */
@@ -4393,9 +4329,6 @@ static PyTypeObject __pyx_type_6common_6kalman_18simple_kalman_impl_KF1D = {
   #if PY_VERSION_HEX >= 0x030800b4 && PY_VERSION_HEX < 0x03090000
   0, /*tp_print*/
   #endif
-  #if CYTHON_COMPILING_IN_PYPY && PYPY_VERSION_NUM+0 >= 0x06000000
-  0, /*tp_pypy_flags*/
-  #endif
 };
 
 static PyMethodDef __pyx_methods[] = {
@@ -5940,7 +5873,7 @@ static int __Pyx_CLineForTraceback(CYTHON_NCP_UNUSED PyThreadState *tstate, int
     }
     if (!use_cline) {
         c_line = 0;
-        (void) PyObject_SetAttr(__pyx_cython_runtime, __pyx_n_s_cline_in_traceback, Py_False);
+        PyObject_SetAttr(__pyx_cython_runtime, __pyx_n_s_cline_in_traceback, Py_False);
     }
     else if (use_cline == Py_False || (use_cline != Py_True && PyObject_Not(use_cline) != 0)) {
         c_line = 0;
@@ -6037,31 +5970,30 @@ static void __pyx_insert_code_object(int code_line, PyCodeObject* code_object) {
 static PyCodeObject* __Pyx_CreateCodeObjectForTraceback(
             const char *funcname, int c_line,
             int py_line, const char *filename) {
-    PyCodeObject *py_code = NULL;
-    PyObject *py_funcname = NULL;
+    PyCodeObject *py_code = 0;
+    PyObject *py_srcfile = 0;
+    PyObject *py_funcname = 0;
     #if PY_MAJOR_VERSION < 3
-    PyObject *py_srcfile = NULL;
     py_srcfile = PyString_FromString(filename);
-    if (!py_srcfile) goto bad;
+    #else
+    py_srcfile = PyUnicode_FromString(filename);
     #endif
+    if (!py_srcfile) goto bad;
     if (c_line) {
         #if PY_MAJOR_VERSION < 3
         py_funcname = PyString_FromFormat( "%s (%s:%d)", funcname, __pyx_cfilenm, c_line);
-        if (!py_funcname) goto bad;
         #else
         py_funcname = PyUnicode_FromFormat( "%s (%s:%d)", funcname, __pyx_cfilenm, c_line);
-        if (!py_funcname) goto bad;
-        funcname = PyUnicode_AsUTF8(py_funcname);
-        if (!funcname) goto bad;
         #endif
     }
     else {
         #if PY_MAJOR_VERSION < 3
         py_funcname = PyString_FromString(funcname);
-        if (!py_funcname) goto bad;
+        #else
+        py_funcname = PyUnicode_FromString(funcname);
         #endif
     }
-    #if PY_MAJOR_VERSION < 3
+    if (!py_funcname) goto bad;
     py_code = __Pyx_PyCode_New(
         0,
         0,
@@ -6080,16 +6012,11 @@ static PyCodeObject* __Pyx_CreateCodeObjectForTraceback(
         __pyx_empty_bytes  /*PyObject *lnotab*/
     );
     Py_DECREF(py_srcfile);
-    #else
-    py_code = PyCode_NewEmpty(filename, funcname, py_line);
-    #endif
-    Py_XDECREF(py_funcname);  // XDECREF since it's only set on Py3 if cline
+    Py_DECREF(py_funcname);
     return py_code;
 bad:
-    Py_XDECREF(py_funcname);
-    #if PY_MAJOR_VERSION < 3
     Py_XDECREF(py_srcfile);
-    #endif
+    Py_XDECREF(py_funcname);
     return NULL;
 }
 static void __Pyx_AddTraceback(const char *funcname, int c_line,
@@ -6937,23 +6864,6 @@ static CYTHON_INLINE Py_ssize_t __Pyx_PyIndex_AsSsize_t(PyObject* b) {
   Py_DECREF(x);
   return ival;
 }
-static CYTHON_INLINE Py_hash_t __Pyx_PyIndex_AsHash_t(PyObject* o) {
-  if (sizeof(Py_hash_t) == sizeof(Py_ssize_t)) {
-    return (Py_hash_t) __Pyx_PyIndex_AsSsize_t(o);
-#if PY_MAJOR_VERSION < 3
-  } else if (likely(PyInt_CheckExact(o))) {
-    return PyInt_AS_LONG(o);
-#endif
-  } else {
-    Py_ssize_t ival;
-    PyObject *x;
-    x = PyNumber_Index(o);
-    if (!x) return -1;
-    ival = PyInt_AsLong(x);
-    Py_DECREF(x);
-    return ival;
-  }
-}
 static CYTHON_INLINE PyObject * __Pyx_PyBool_FromLong(long b) {
   return b ? __Pyx_NewRef(Py_True) : __Pyx_NewRef(Py_False);
 }
diff --git a/common/kalman/simple_kalman_impl.so b/common/kalman/simple_kalman_impl.so
index 4fb04cb93..a6e48593a 100755
Binary files a/common/kalman/simple_kalman_impl.so and b/common/kalman/simple_kalman_impl.so differ
diff --git a/common/params_pyx.so b/common/params_pyx.so
index e346cff4d..7f17f9802 100755
Binary files a/common/params_pyx.so and b/common/params_pyx.so differ
diff --git a/common/transformations/transformations.cpp b/common/transformations/transformations.cpp
index 4080d4a8b..f08fb62f8 100644
--- a/common/transformations/transformations.cpp
+++ b/common/transformations/transformations.cpp
@@ -1,4 +1,4 @@
-/* Generated by Cython 0.29.26 */
+/* Generated by Cython 0.29.24 */
 
 /* BEGIN: Cython Metadata
 {
@@ -32,8 +32,8 @@ END: Cython Metadata */
 #elif PY_VERSION_HEX < 0x02060000 || (0x03000000 <= PY_VERSION_HEX && PY_VERSION_HEX < 0x03030000)
     #error Cython requires Python 2.6+ or Python 3.3+.
 #else
-#define CYTHON_ABI "0_29_26"
-#define CYTHON_HEX_VERSION 0x001D1AF0
+#define CYTHON_ABI "0_29_24"
+#define CYTHON_HEX_VERSION 0x001D18F0
 #define CYTHON_FUTURE_DIVISION 1
 #include <stddef.h>
 #ifndef offsetof
@@ -180,7 +180,7 @@ END: Cython Metadata */
   #ifndef CYTHON_USE_UNICODE_INTERNALS
     #define CYTHON_USE_UNICODE_INTERNALS 1
   #endif
-  #if PY_VERSION_HEX < 0x030300F0 || PY_VERSION_HEX >= 0x030B00A2
+  #if PY_VERSION_HEX < 0x030300F0
     #undef CYTHON_USE_UNICODE_WRITER
     #define CYTHON_USE_UNICODE_WRITER 0
   #elif !defined(CYTHON_USE_UNICODE_WRITER)
@@ -199,7 +199,7 @@ END: Cython Metadata */
     #define CYTHON_FAST_THREAD_STATE 1
   #endif
   #ifndef CYTHON_FAST_PYCALL
-    #define CYTHON_FAST_PYCALL (PY_VERSION_HEX < 0x030B00A1)
+    #define CYTHON_FAST_PYCALL 1
   #endif
   #ifndef CYTHON_PEP489_MULTI_PHASE_INIT
     #define CYTHON_PEP489_MULTI_PHASE_INIT (PY_VERSION_HEX >= 0x03050000)
@@ -218,9 +218,7 @@ END: Cython Metadata */
 #define CYTHON_FAST_PYCCALL  (CYTHON_FAST_PYCALL && PY_VERSION_HEX >= 0x030600B1)
 #endif
 #if CYTHON_USE_PYLONG_INTERNALS
-  #if PY_MAJOR_VERSION < 3
-    #include "longintrepr.h"
-  #endif
+  #include "longintrepr.h"
   #undef SHIFT
   #undef BASE
   #undef MASK
@@ -351,68 +349,9 @@ class __Pyx_FakeReference {
   #define __Pyx_DefaultClassType PyClass_Type
 #else
   #define __Pyx_BUILTIN_MODULE_NAME "builtins"
-  #define __Pyx_DefaultClassType PyType_Type
-#if PY_VERSION_HEX >= 0x030B00A1
-    static CYTHON_INLINE PyCodeObject* __Pyx_PyCode_New(int a, int k, int l, int s, int f,
-                                                    PyObject *code, PyObject *c, PyObject* n, PyObject *v,
-                                                    PyObject *fv, PyObject *cell, PyObject* fn,
-                                                    PyObject *name, int fline, PyObject *lnos) {
-        PyObject *kwds=NULL, *argcount=NULL, *posonlyargcount=NULL, *kwonlyargcount=NULL;
-        PyObject *nlocals=NULL, *stacksize=NULL, *flags=NULL, *replace=NULL, *call_result=NULL, *empty=NULL;
-        const char *fn_cstr=NULL;
-        const char *name_cstr=NULL;
-        PyCodeObject* co=NULL;
-        PyObject *type, *value, *traceback;
-        PyErr_Fetch(&type, &value, &traceback);
-        if (!(kwds=PyDict_New())) goto end;
-        if (!(argcount=PyLong_FromLong(a))) goto end;
-        if (PyDict_SetItemString(kwds, "co_argcount", argcount) != 0) goto end;
-        if (!(posonlyargcount=PyLong_FromLong(0))) goto end;
-        if (PyDict_SetItemString(kwds, "co_posonlyargcount", posonlyargcount) != 0) goto end;
-        if (!(kwonlyargcount=PyLong_FromLong(k))) goto end;
-        if (PyDict_SetItemString(kwds, "co_kwonlyargcount", kwonlyargcount) != 0) goto end;
-        if (!(nlocals=PyLong_FromLong(l))) goto end;
-        if (PyDict_SetItemString(kwds, "co_nlocals", nlocals) != 0) goto end;
-        if (!(stacksize=PyLong_FromLong(s))) goto end;
-        if (PyDict_SetItemString(kwds, "co_stacksize", stacksize) != 0) goto end;
-        if (!(flags=PyLong_FromLong(f))) goto end;
-        if (PyDict_SetItemString(kwds, "co_flags", flags) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_code", code) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_consts", c) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_names", n) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_varnames", v) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_freevars", fv) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_cellvars", cell) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_linetable", lnos) != 0) goto end;
-        if (!(fn_cstr=PyUnicode_AsUTF8AndSize(fn, NULL))) goto end;
-        if (!(name_cstr=PyUnicode_AsUTF8AndSize(name, NULL))) goto end;
-        if (!(co = PyCode_NewEmpty(fn_cstr, name_cstr, fline))) goto end;
-        if (!(replace = PyObject_GetAttrString((PyObject*)co, "replace"))) goto cleanup_code_too;
-        if (!(empty = PyTuple_New(0))) goto cleanup_code_too; // unfortunately __pyx_empty_tuple isn't available here
-        if (!(call_result = PyObject_Call(replace, empty, kwds))) goto cleanup_code_too;
-        Py_XDECREF((PyObject*)co);
-        co = (PyCodeObject*)call_result;
-        call_result = NULL;
-        if (0) {
-            cleanup_code_too:
-            Py_XDECREF((PyObject*)co);
-            co = NULL;
-        }
-        end:
-        Py_XDECREF(kwds);
-        Py_XDECREF(argcount);
-        Py_XDECREF(posonlyargcount);
-        Py_XDECREF(kwonlyargcount);
-        Py_XDECREF(nlocals);
-        Py_XDECREF(stacksize);
-        Py_XDECREF(replace);
-        Py_XDECREF(call_result);
-        Py_XDECREF(empty);
-        if (type) {
-            PyErr_Restore(type, value, traceback);
-        }
-        return co;
-    }
+#if PY_VERSION_HEX >= 0x030800A4 && PY_VERSION_HEX < 0x030800B2
+  #define __Pyx_PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)\
+          PyCode_New(a, 0, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)
 #else
   #define __Pyx_PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)\
           PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)
@@ -650,10 +589,10 @@ static CYTHON_INLINE void * PyThread_tss_get(Py_tss_t *key) {
 #if PY_VERSION_HEX < 0x030200A4
   typedef long Py_hash_t;
   #define __Pyx_PyInt_FromHash_t PyInt_FromLong
-  #define __Pyx_PyInt_AsHash_t   __Pyx_PyIndex_AsHash_t
+  #define __Pyx_PyInt_AsHash_t   PyInt_AsLong
 #else
   #define __Pyx_PyInt_FromHash_t PyInt_FromSsize_t
-  #define __Pyx_PyInt_AsHash_t   __Pyx_PyIndex_AsSsize_t
+  #define __Pyx_PyInt_AsHash_t   PyInt_AsSsize_t
 #endif
 #if PY_MAJOR_VERSION >= 3
   #define __Pyx_PyMethod_New(func, self, klass) ((self) ? ((void)(klass), PyMethod_New(func, self)) : __Pyx_NewRef(func))
@@ -823,7 +762,6 @@ static CYTHON_INLINE PyObject* __Pyx_PyNumber_IntOrLong(PyObject* x);
     (likely(PyTuple_CheckExact(obj)) ? __Pyx_NewRef(obj) : PySequence_Tuple(obj))
 static CYTHON_INLINE Py_ssize_t __Pyx_PyIndex_AsSsize_t(PyObject*);
 static CYTHON_INLINE PyObject * __Pyx_PyInt_FromSize_t(size_t);
-static CYTHON_INLINE Py_hash_t __Pyx_PyIndex_AsHash_t(PyObject*);
 #if CYTHON_ASSUME_SAFE_MACROS
 #define __pyx_PyFloat_AsDouble(x) (PyFloat_CheckExact(x) ? PyFloat_AS_DOUBLE(x) : PyFloat_AsDouble(x))
 #else
@@ -1407,7 +1345,6 @@ static PyObject *__Pyx_PyFunction_FastCallDict(PyObject *func, PyObject **args,
 #ifndef Py_MEMBER_SIZE
 #define Py_MEMBER_SIZE(type, member) sizeof(((type *)0)->member)
 #endif
-#if CYTHON_FAST_PYCALL
   static size_t __pyx_pyframe_localsplus_offset = 0;
   #include "frameobject.h"
   #define __Pxy_PyFrame_Initialize_Offsets()\
@@ -1415,7 +1352,6 @@ static PyObject *__Pyx_PyFunction_FastCallDict(PyObject *func, PyObject **args,
      (void)(__pyx_pyframe_localsplus_offset = ((size_t)PyFrame_Type.tp_basicsize) - Py_MEMBER_SIZE(PyFrameObject, f_localsplus)))
   #define __Pyx_PyFrame_GetLocalsplus(frame)\
     (assert(__pyx_pyframe_localsplus_offset), (PyObject **)(((char *)(frame)) + __pyx_pyframe_localsplus_offset))
-#endif // CYTHON_FAST_PYCALL
 #endif
 
 /* PyObjectCall.proto */
@@ -6342,9 +6278,6 @@ static PyTypeObject __pyx_type_6common_15transformations_15transformations_Local
   #if PY_VERSION_HEX >= 0x030800b4 && PY_VERSION_HEX < 0x03090000
   0, /*tp_print*/
   #endif
-  #if CYTHON_COMPILING_IN_PYPY && PYPY_VERSION_NUM+0 >= 0x06000000
-  0, /*tp_pypy_flags*/
-  #endif
 };
 
 static PyMethodDef __pyx_methods[] = {
@@ -9109,7 +9042,7 @@ static int __Pyx_CLineForTraceback(CYTHON_NCP_UNUSED PyThreadState *tstate, int
     }
     if (!use_cline) {
         c_line = 0;
-        (void) PyObject_SetAttr(__pyx_cython_runtime, __pyx_n_s_cline_in_traceback, Py_False);
+        PyObject_SetAttr(__pyx_cython_runtime, __pyx_n_s_cline_in_traceback, Py_False);
     }
     else if (use_cline == Py_False || (use_cline != Py_True && PyObject_Not(use_cline) != 0)) {
         c_line = 0;
@@ -9206,31 +9139,30 @@ static void __pyx_insert_code_object(int code_line, PyCodeObject* code_object) {
 static PyCodeObject* __Pyx_CreateCodeObjectForTraceback(
             const char *funcname, int c_line,
             int py_line, const char *filename) {
-    PyCodeObject *py_code = NULL;
-    PyObject *py_funcname = NULL;
+    PyCodeObject *py_code = 0;
+    PyObject *py_srcfile = 0;
+    PyObject *py_funcname = 0;
     #if PY_MAJOR_VERSION < 3
-    PyObject *py_srcfile = NULL;
     py_srcfile = PyString_FromString(filename);
-    if (!py_srcfile) goto bad;
+    #else
+    py_srcfile = PyUnicode_FromString(filename);
     #endif
+    if (!py_srcfile) goto bad;
     if (c_line) {
         #if PY_MAJOR_VERSION < 3
         py_funcname = PyString_FromFormat( "%s (%s:%d)", funcname, __pyx_cfilenm, c_line);
-        if (!py_funcname) goto bad;
         #else
         py_funcname = PyUnicode_FromFormat( "%s (%s:%d)", funcname, __pyx_cfilenm, c_line);
-        if (!py_funcname) goto bad;
-        funcname = PyUnicode_AsUTF8(py_funcname);
-        if (!funcname) goto bad;
         #endif
     }
     else {
         #if PY_MAJOR_VERSION < 3
         py_funcname = PyString_FromString(funcname);
-        if (!py_funcname) goto bad;
+        #else
+        py_funcname = PyUnicode_FromString(funcname);
         #endif
     }
-    #if PY_MAJOR_VERSION < 3
+    if (!py_funcname) goto bad;
     py_code = __Pyx_PyCode_New(
         0,
         0,
@@ -9249,16 +9181,11 @@ static PyCodeObject* __Pyx_CreateCodeObjectForTraceback(
         __pyx_empty_bytes  /*PyObject *lnotab*/
     );
     Py_DECREF(py_srcfile);
-    #else
-    py_code = PyCode_NewEmpty(filename, funcname, py_line);
-    #endif
-    Py_XDECREF(py_funcname);  // XDECREF since it's only set on Py3 if cline
+    Py_DECREF(py_funcname);
     return py_code;
 bad:
-    Py_XDECREF(py_funcname);
-    #if PY_MAJOR_VERSION < 3
     Py_XDECREF(py_srcfile);
-    #endif
+    Py_XDECREF(py_funcname);
     return NULL;
 }
 static void __Pyx_AddTraceback(const char *funcname, int c_line,
@@ -10434,23 +10361,6 @@ static CYTHON_INLINE Py_ssize_t __Pyx_PyIndex_AsSsize_t(PyObject* b) {
   Py_DECREF(x);
   return ival;
 }
-static CYTHON_INLINE Py_hash_t __Pyx_PyIndex_AsHash_t(PyObject* o) {
-  if (sizeof(Py_hash_t) == sizeof(Py_ssize_t)) {
-    return (Py_hash_t) __Pyx_PyIndex_AsSsize_t(o);
-#if PY_MAJOR_VERSION < 3
-  } else if (likely(PyInt_CheckExact(o))) {
-    return PyInt_AS_LONG(o);
-#endif
-  } else {
-    Py_ssize_t ival;
-    PyObject *x;
-    x = PyNumber_Index(o);
-    if (!x) return -1;
-    ival = PyInt_AsLong(x);
-    Py_DECREF(x);
-    return ival;
-  }
-}
 static CYTHON_INLINE PyObject * __Pyx_PyBool_FromLong(long b) {
   return b ? __Pyx_NewRef(Py_True) : __Pyx_NewRef(Py_False);
 }
diff --git a/common/transformations/transformations.so b/common/transformations/transformations.so
index dd810f7f0..f168bb89b 100755
Binary files a/common/transformations/transformations.so and b/common/transformations/transformations.so differ
diff --git a/common/version.h b/common/version.h
index a2c1974ec..0d8cff9d8 100644
--- a/common/version.h
+++ b/common/version.h
@@ -1 +1 @@
-#define COMMA_VERSION "2022.12.30"
+#define COMMA_VERSION "2023.02.07"
diff --git a/opendbc/can/libdbc.so b/opendbc/can/libdbc.so
index ad0357253..42aa5318e 100755
Binary files a/opendbc/can/libdbc.so and b/opendbc/can/libdbc.so differ
diff --git a/panda/board/obj/bootstub.panda.bin b/panda/board/obj/bootstub.panda.bin
index 193b41e2f..ab66be26a 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/panda.bin.signed b/panda/board/obj/panda.bin.signed
index 60a6a66aa..61042cfcb 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.bin.testing.signed b/panda/board/obj/panda.bin.testing.signed
deleted file mode 100644
index 5491e1a59..000000000
Binary files a/panda/board/obj/panda.bin.testing.signed and /dev/null differ
diff --git a/panda/python/__init__.py b/panda/python/__init__.py
index 76c9030c4..e5a80163e 100644
--- a/panda/python/__init__.py
+++ b/panda/python/__init__.py
@@ -206,11 +206,13 @@ class Panda:
   # first byte is for EPS scaling factor
   FLAG_TOYOTA_ALT_BRAKE = (1 << 8)
   FLAG_TOYOTA_STOCK_LONGITUDINAL = (2 << 8)
+  FLAG_TOYOTA_ALKA = (4 << 8)
 
   FLAG_HONDA_ALT_BRAKE = 1
   FLAG_HONDA_BOSCH_LONG = 2
   FLAG_HONDA_NIDEC_ALT = 4
   FLAG_HONDA_RADARLESS = 8
+  FLAG_HONDA_ALKA = 16
 
   FLAG_HYUNDAI_EV_GAS = 1
   FLAG_HYUNDAI_HYBRID_GAS = 2
@@ -219,11 +221,13 @@ class Panda:
   FLAG_HYUNDAI_CANFD_HDA2 = 16
   FLAG_HYUNDAI_CANFD_ALT_BUTTONS = 32
   FLAG_HYUNDAI_ALT_LIMITS = 64
+  FLAG_HYUNDAI_ALKA = 128
 
   FLAG_TESLA_POWERTRAIN = 1
   FLAG_TESLA_LONG_CONTROL = 2
 
   FLAG_VOLKSWAGEN_LONG_CONTROL = 1
+  FLAG_VOLKSWAGEN_ALKA = 2
 
   FLAG_CHRYSLER_RAM_DT = 1
   FLAG_CHRYSLER_RAM_HD = 2
diff --git a/panda/python/config.py b/panda/python/config.py
index ba9ad3d42..56940ed72 100644
--- a/panda/python/config.py
+++ b/panda/python/config.py
@@ -9,29 +9,6 @@ APP_ADDRESS_FX = 0x8004000
 SECTOR_SIZES_FX = [0x4000 for _ in range(4)] + [0x10000] + [0x20000 for _ in range(11)]
 DEVICE_SERIAL_NUMBER_ADDR_FX = 0x1FFF79C0
 DEFAULT_FW_FN = os.path.join(BASEDIR, "board", "obj", "panda.bin.signed")
-DEFAULT_SSPOOF_FW_FN = os.path.join(BASEDIR, "board", "obj", "panda.bin.sspoof.signed")
-TESTING_FW_FN = os.path.join(BASEDIR, "board", "obj", "panda.bin.testing.signed")
-TESTING_SSPOOF_FW_FN = os.path.join(BASEDIR, "board", "obj", "panda.bin.testing.sspoof.signed")
-
-atl_enabled = False
-if os.path.exists('/data/params/d/dp_atl'):
-  with open('/data/params/d/dp_atl') as f:
-    if (int(f.read().strip())) != 0:
-      atl_enabled = True
-
-sspoof_enabled = False
-# if os.path.exists('/data/params/d/dp_sspoof'):
-#   with open('/data/params/d/dp_sspoof') as f:
-#     if (int(f.read().strip())) != 0:
-#       sspoof_enabled = True
-
-if atl_enabled and sspoof_enabled and os.path.exists(TESTING_SSPOOF_FW_FN):
-  DEFAULT_FW_FN = TESTING_SSPOOF_FW_FN
-elif atl_enabled and not sspoof_enabled and os.path.exists(TESTING_FW_FN):
-  DEFAULT_FW_FN = TESTING_FW_FN
-elif not atl_enabled and sspoof_enabled and os.path.exists(DEFAULT_SSPOOF_FW_FN):
-  DEFAULT_FW_FN = DEFAULT_SSPOOF_FW_FN
-
 DEFAULT_BOOTSTUB_FN = os.path.join(BASEDIR, "board", "obj", "bootstub.panda.bin")
 
 BLOCK_SIZE_H7 = 0x400
diff --git a/rednose/helpers/ekf_sym_pyx.so b/rednose/helpers/ekf_sym_pyx.so
index 3ecfa1503..5dbc1c3ab 100755
Binary files a/rednose/helpers/ekf_sym_pyx.so and b/rednose/helpers/ekf_sym_pyx.so differ
diff --git a/scripts/panda_recovery.sh b/scripts/panda_recovery.sh
index da5223a0e..bef77b9a1 100755
--- a/scripts/panda_recovery.sh
+++ b/scripts/panda_recovery.sh
@@ -3,42 +3,13 @@
 DFU_UTIL="dfu-util"
 
 DEFAULT_FW_FN="/data/openpilot/panda/board/obj/panda.bin.signed"
-DEFAULT_SSPOOF_FW_FN="/data/openpilot/panda/board/obj/panda.bin.sspoof.signed"
-TESTING_FW_FN="/data/openpilot/panda/board/obj/panda.bin.testing.signed"
-TESTING_SSPOOF_FW_FN="/data/openpilot/panda/board/obj/panda.bin.testing.sspoof.signed"
-
-ATL_ENABLED=0
-ATL=`cat /data/params/d/dp_atl`
-if [ -f /data/params/d/dp_atl ] && [ $ATL != "0" ]; then
-  ATL_ENABLED=1
-fi
-
-SSPOOF_ENABLED=0
-#SSPOOF=`cat /data/params/d/dp_sspoof`
-#if [ -f /data/params/d/dp_sspoof ] && [ $SSPOOF != "0" ]; then
-#  SSPOOF_ENABLED=1
-#fi
-
-FW=$DEFAULT_FW_FN
-if [ "$ATL_ENABLED" = "1" ] && [ "$SSPOOF_ENABLED" = "1" ] && [ -f $TESTING_SSPOOF_FW_FN ]; then
-  echo "Use testing + sspoof firmware..."
-  FW=$TESTING_SSPOOF_FW_FN
-elif [ "$ATL_ENABLED" = "1" ] && [ "$SSPOOF_ENABLED" = "0" ] && [ -f $TESTING_FW_FN ]; then
-  echo "Use testing firmware..."
-  FW=$TESTING_FW_FN
-elif [ "$ATL_ENABLED" = "0" ] && [ "$SSPOOF_ENABLED" = "1" ] && [ -f $DEFAULT_SSPOOF_FW_FN ]; then
-  echo "Use sspoof firmware..."
-  FW=$DEFAULT_SSPOOF_FW_FN
-else
-  echo "Use stock firmware..."
-fi
 
 /data/openpilot/selfdrive/ui/qt/spinner &
 pkill -f boardd
 python -c "from panda import Panda; Panda().reset(enter_bootstub=True); Panda().reset(enter_bootloader=True)" || true
 sleep 1
 echo "\n\n\nUpdating panda.bin..."
-$DFU_UTIL -d 0483:df11 -a 0 -s 0x08004000 -D $FW
+$DFU_UTIL -d 0483:df11 -a 0 -s 0x08004000 -D $DEFAULT_FW_FN
 echo "\n\n\nUpdating bootstub.panda.bin..."
 $DFU_UTIL -d 0483:df11 -a 0 -s 0x08000000:leave -D /data/openpilot/panda/board/obj/bootstub.panda.bin
 sleep 1
diff --git a/selfdrive/boardd/boardd b/selfdrive/boardd/boardd
index 9675acf3f..8dce10c9e 100755
Binary files a/selfdrive/boardd/boardd and b/selfdrive/boardd/boardd differ
diff --git a/selfdrive/boardd/boardd_api_impl.cpp b/selfdrive/boardd/boardd_api_impl.cpp
index 814e8a01d..a17d8b490 100644
--- a/selfdrive/boardd/boardd_api_impl.cpp
+++ b/selfdrive/boardd/boardd_api_impl.cpp
@@ -1,4 +1,4 @@
-/* Generated by Cython 0.29.26 */
+/* Generated by Cython 0.29.24 */
 
 /* BEGIN: Cython Metadata
 {
@@ -23,8 +23,8 @@ END: Cython Metadata */
 #elif PY_VERSION_HEX < 0x02060000 || (0x03000000 <= PY_VERSION_HEX && PY_VERSION_HEX < 0x03030000)
     #error Cython requires Python 2.6+ or Python 3.3+.
 #else
-#define CYTHON_ABI "0_29_26"
-#define CYTHON_HEX_VERSION 0x001D1AF0
+#define CYTHON_ABI "0_29_24"
+#define CYTHON_HEX_VERSION 0x001D18F0
 #define CYTHON_FUTURE_DIVISION 1
 #include <stddef.h>
 #ifndef offsetof
@@ -171,7 +171,7 @@ END: Cython Metadata */
   #ifndef CYTHON_USE_UNICODE_INTERNALS
     #define CYTHON_USE_UNICODE_INTERNALS 1
   #endif
-  #if PY_VERSION_HEX < 0x030300F0 || PY_VERSION_HEX >= 0x030B00A2
+  #if PY_VERSION_HEX < 0x030300F0
     #undef CYTHON_USE_UNICODE_WRITER
     #define CYTHON_USE_UNICODE_WRITER 0
   #elif !defined(CYTHON_USE_UNICODE_WRITER)
@@ -190,7 +190,7 @@ END: Cython Metadata */
     #define CYTHON_FAST_THREAD_STATE 1
   #endif
   #ifndef CYTHON_FAST_PYCALL
-    #define CYTHON_FAST_PYCALL (PY_VERSION_HEX < 0x030B00A1)
+    #define CYTHON_FAST_PYCALL 1
   #endif
   #ifndef CYTHON_PEP489_MULTI_PHASE_INIT
     #define CYTHON_PEP489_MULTI_PHASE_INIT (PY_VERSION_HEX >= 0x03050000)
@@ -209,9 +209,7 @@ END: Cython Metadata */
 #define CYTHON_FAST_PYCCALL  (CYTHON_FAST_PYCALL && PY_VERSION_HEX >= 0x030600B1)
 #endif
 #if CYTHON_USE_PYLONG_INTERNALS
-  #if PY_MAJOR_VERSION < 3
-    #include "longintrepr.h"
-  #endif
+  #include "longintrepr.h"
   #undef SHIFT
   #undef BASE
   #undef MASK
@@ -342,68 +340,9 @@ class __Pyx_FakeReference {
   #define __Pyx_DefaultClassType PyClass_Type
 #else
   #define __Pyx_BUILTIN_MODULE_NAME "builtins"
-  #define __Pyx_DefaultClassType PyType_Type
-#if PY_VERSION_HEX >= 0x030B00A1
-    static CYTHON_INLINE PyCodeObject* __Pyx_PyCode_New(int a, int k, int l, int s, int f,
-                                                    PyObject *code, PyObject *c, PyObject* n, PyObject *v,
-                                                    PyObject *fv, PyObject *cell, PyObject* fn,
-                                                    PyObject *name, int fline, PyObject *lnos) {
-        PyObject *kwds=NULL, *argcount=NULL, *posonlyargcount=NULL, *kwonlyargcount=NULL;
-        PyObject *nlocals=NULL, *stacksize=NULL, *flags=NULL, *replace=NULL, *call_result=NULL, *empty=NULL;
-        const char *fn_cstr=NULL;
-        const char *name_cstr=NULL;
-        PyCodeObject* co=NULL;
-        PyObject *type, *value, *traceback;
-        PyErr_Fetch(&type, &value, &traceback);
-        if (!(kwds=PyDict_New())) goto end;
-        if (!(argcount=PyLong_FromLong(a))) goto end;
-        if (PyDict_SetItemString(kwds, "co_argcount", argcount) != 0) goto end;
-        if (!(posonlyargcount=PyLong_FromLong(0))) goto end;
-        if (PyDict_SetItemString(kwds, "co_posonlyargcount", posonlyargcount) != 0) goto end;
-        if (!(kwonlyargcount=PyLong_FromLong(k))) goto end;
-        if (PyDict_SetItemString(kwds, "co_kwonlyargcount", kwonlyargcount) != 0) goto end;
-        if (!(nlocals=PyLong_FromLong(l))) goto end;
-        if (PyDict_SetItemString(kwds, "co_nlocals", nlocals) != 0) goto end;
-        if (!(stacksize=PyLong_FromLong(s))) goto end;
-        if (PyDict_SetItemString(kwds, "co_stacksize", stacksize) != 0) goto end;
-        if (!(flags=PyLong_FromLong(f))) goto end;
-        if (PyDict_SetItemString(kwds, "co_flags", flags) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_code", code) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_consts", c) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_names", n) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_varnames", v) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_freevars", fv) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_cellvars", cell) != 0) goto end;
-        if (PyDict_SetItemString(kwds, "co_linetable", lnos) != 0) goto end;
-        if (!(fn_cstr=PyUnicode_AsUTF8AndSize(fn, NULL))) goto end;
-        if (!(name_cstr=PyUnicode_AsUTF8AndSize(name, NULL))) goto end;
-        if (!(co = PyCode_NewEmpty(fn_cstr, name_cstr, fline))) goto end;
-        if (!(replace = PyObject_GetAttrString((PyObject*)co, "replace"))) goto cleanup_code_too;
-        if (!(empty = PyTuple_New(0))) goto cleanup_code_too; // unfortunately __pyx_empty_tuple isn't available here
-        if (!(call_result = PyObject_Call(replace, empty, kwds))) goto cleanup_code_too;
-        Py_XDECREF((PyObject*)co);
-        co = (PyCodeObject*)call_result;
-        call_result = NULL;
-        if (0) {
-            cleanup_code_too:
-            Py_XDECREF((PyObject*)co);
-            co = NULL;
-        }
-        end:
-        Py_XDECREF(kwds);
-        Py_XDECREF(argcount);
-        Py_XDECREF(posonlyargcount);
-        Py_XDECREF(kwonlyargcount);
-        Py_XDECREF(nlocals);
-        Py_XDECREF(stacksize);
-        Py_XDECREF(replace);
-        Py_XDECREF(call_result);
-        Py_XDECREF(empty);
-        if (type) {
-            PyErr_Restore(type, value, traceback);
-        }
-        return co;
-    }
+#if PY_VERSION_HEX >= 0x030800A4 && PY_VERSION_HEX < 0x030800B2
+  #define __Pyx_PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)\
+          PyCode_New(a, 0, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)
 #else
   #define __Pyx_PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)\
           PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)
@@ -641,10 +580,10 @@ static CYTHON_INLINE void * PyThread_tss_get(Py_tss_t *key) {
 #if PY_VERSION_HEX < 0x030200A4
   typedef long Py_hash_t;
   #define __Pyx_PyInt_FromHash_t PyInt_FromLong
-  #define __Pyx_PyInt_AsHash_t   __Pyx_PyIndex_AsHash_t
+  #define __Pyx_PyInt_AsHash_t   PyInt_AsLong
 #else
   #define __Pyx_PyInt_FromHash_t PyInt_FromSsize_t
-  #define __Pyx_PyInt_AsHash_t   __Pyx_PyIndex_AsSsize_t
+  #define __Pyx_PyInt_AsHash_t   PyInt_AsSsize_t
 #endif
 #if PY_MAJOR_VERSION >= 3
   #define __Pyx_PyMethod_New(func, self, klass) ((self) ? ((void)(klass), PyMethod_New(func, self)) : __Pyx_NewRef(func))
@@ -807,7 +746,6 @@ static CYTHON_INLINE PyObject* __Pyx_PyNumber_IntOrLong(PyObject* x);
     (likely(PyTuple_CheckExact(obj)) ? __Pyx_NewRef(obj) : PySequence_Tuple(obj))
 static CYTHON_INLINE Py_ssize_t __Pyx_PyIndex_AsSsize_t(PyObject*);
 static CYTHON_INLINE PyObject * __Pyx_PyInt_FromSize_t(size_t);
-static CYTHON_INLINE Py_hash_t __Pyx_PyIndex_AsHash_t(PyObject*);
 #if CYTHON_ASSUME_SAFE_MACROS
 #define __pyx_PyFloat_AsDouble(x) (PyFloat_CheckExact(x) ? PyFloat_AS_DOUBLE(x) : PyFloat_AsDouble(x))
 #else
@@ -2630,7 +2568,7 @@ static int __Pyx_CLineForTraceback(CYTHON_NCP_UNUSED PyThreadState *tstate, int
     }
     if (!use_cline) {
         c_line = 0;
-        (void) PyObject_SetAttr(__pyx_cython_runtime, __pyx_n_s_cline_in_traceback, Py_False);
+        PyObject_SetAttr(__pyx_cython_runtime, __pyx_n_s_cline_in_traceback, Py_False);
     }
     else if (use_cline == Py_False || (use_cline != Py_True && PyObject_Not(use_cline) != 0)) {
         c_line = 0;
@@ -2727,31 +2665,30 @@ static void __pyx_insert_code_object(int code_line, PyCodeObject* code_object) {
 static PyCodeObject* __Pyx_CreateCodeObjectForTraceback(
             const char *funcname, int c_line,
             int py_line, const char *filename) {
-    PyCodeObject *py_code = NULL;
-    PyObject *py_funcname = NULL;
+    PyCodeObject *py_code = 0;
+    PyObject *py_srcfile = 0;
+    PyObject *py_funcname = 0;
     #if PY_MAJOR_VERSION < 3
-    PyObject *py_srcfile = NULL;
     py_srcfile = PyString_FromString(filename);
-    if (!py_srcfile) goto bad;
+    #else
+    py_srcfile = PyUnicode_FromString(filename);
     #endif
+    if (!py_srcfile) goto bad;
     if (c_line) {
         #if PY_MAJOR_VERSION < 3
         py_funcname = PyString_FromFormat( "%s (%s:%d)", funcname, __pyx_cfilenm, c_line);
-        if (!py_funcname) goto bad;
         #else
         py_funcname = PyUnicode_FromFormat( "%s (%s:%d)", funcname, __pyx_cfilenm, c_line);
-        if (!py_funcname) goto bad;
-        funcname = PyUnicode_AsUTF8(py_funcname);
-        if (!funcname) goto bad;
         #endif
     }
     else {
         #if PY_MAJOR_VERSION < 3
         py_funcname = PyString_FromString(funcname);
-        if (!py_funcname) goto bad;
+        #else
+        py_funcname = PyUnicode_FromString(funcname);
         #endif
     }
-    #if PY_MAJOR_VERSION < 3
+    if (!py_funcname) goto bad;
     py_code = __Pyx_PyCode_New(
         0,
         0,
@@ -2770,16 +2707,11 @@ static PyCodeObject* __Pyx_CreateCodeObjectForTraceback(
         __pyx_empty_bytes  /*PyObject *lnotab*/
     );
     Py_DECREF(py_srcfile);
-    #else
-    py_code = PyCode_NewEmpty(filename, funcname, py_line);
-    #endif
-    Py_XDECREF(py_funcname);  // XDECREF since it's only set on Py3 if cline
+    Py_DECREF(py_funcname);
     return py_code;
 bad:
-    Py_XDECREF(py_funcname);
-    #if PY_MAJOR_VERSION < 3
     Py_XDECREF(py_srcfile);
-    #endif
+    Py_XDECREF(py_funcname);
     return NULL;
 }
 static void __Pyx_AddTraceback(const char *funcname, int c_line,
@@ -3627,23 +3559,6 @@ static CYTHON_INLINE Py_ssize_t __Pyx_PyIndex_AsSsize_t(PyObject* b) {
   Py_DECREF(x);
   return ival;
 }
-static CYTHON_INLINE Py_hash_t __Pyx_PyIndex_AsHash_t(PyObject* o) {
-  if (sizeof(Py_hash_t) == sizeof(Py_ssize_t)) {
-    return (Py_hash_t) __Pyx_PyIndex_AsSsize_t(o);
-#if PY_MAJOR_VERSION < 3
-  } else if (likely(PyInt_CheckExact(o))) {
-    return PyInt_AS_LONG(o);
-#endif
-  } else {
-    Py_ssize_t ival;
-    PyObject *x;
-    x = PyNumber_Index(o);
-    if (!x) return -1;
-    ival = PyInt_AsLong(x);
-    Py_DECREF(x);
-    return ival;
-  }
-}
 static CYTHON_INLINE PyObject * __Pyx_PyBool_FromLong(long b) {
   return b ? __Pyx_NewRef(Py_True) : __Pyx_NewRef(Py_False);
 }
diff --git a/selfdrive/boardd/boardd_api_impl.so b/selfdrive/boardd/boardd_api_impl.so
index 3dc27c8cd..f0be8f150 100755
Binary files a/selfdrive/boardd/boardd_api_impl.so and b/selfdrive/boardd/boardd_api_impl.so differ
diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py
index 6d45666d2..6e32fb7c9 100755
--- a/selfdrive/car/chrysler/interface.py
+++ b/selfdrive/car/chrysler/interface.py
@@ -74,7 +74,7 @@ class CarInterface(CarInterfaceBase):
       raise ValueError(f"Unsupported car: {candidate}")
 
     CarInterfaceBase.configure_dp_tune(candidate, ret.lateralTuning)
-    
+
     if ret.flags & ChryslerFlags.HIGHER_MIN_STEERING_SPEED:
       # TODO: allow these cars to steer down to 13 m/s if already engaged.
       ret.minSteerSpeed = 17.5  # m/s 17 on the way up, 13 on the way down once engaged.
@@ -86,11 +86,9 @@ class CarInterface(CarInterfaceBase):
 
   def _update(self, c):
     ret = self.CS.update(self.cp, self.cp_cam)
-    ret.cruiseState.enabled, ret.cruiseState.available = self.dp_atl_mode(ret)
 
     # events
     events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.low])
-    events = self.dp_atl_warning(ret, events)
 
     # Low speed steer alert hysteresis logic
     if self.CP.minSteerSpeed > 0. and ret.vEgo < (self.CP.minSteerSpeed + 0.5):
diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py
index 0885151cc..d26fa09e0 100644
--- a/selfdrive/car/ford/interface.py
+++ b/selfdrive/car/ford/interface.py
@@ -61,10 +61,8 @@ class CarInterface(CarInterfaceBase):
 
   def _update(self, c):
     ret = self.CS.update(self.cp, self.cp_cam)
-    ret.cruiseState.enabled, ret.cruiseState.available = self.dp_atl_mode(ret)
 
     events = self.create_common_events(ret, extra_gears=[GearShifter.manumatic])
-    events = self.dp_atl_warning(ret, events)
     ret.events = events.to_msg()
 
     return ret
diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py
index cac638069..36ffafc92 100755
--- a/selfdrive/car/gm/interface.py
+++ b/selfdrive/car/gm/interface.py
@@ -206,7 +206,6 @@ class CarInterface(CarInterfaceBase):
   # returns a car.CarState
   def _update(self, c):
     ret = self.CS.update(self.cp, self.cp_cam, self.cp_loopback)
-    ret.cruiseState.enabled, ret.cruiseState.available = self.dp_atl_mode(ret)
 
     if self.CS.cruise_buttons != self.CS.prev_cruise_buttons and self.CS.prev_cruise_buttons != CruiseButtons.INIT:
       buttonEvents = [create_button_event(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT, CruiseButtons.UNPRESS)]
@@ -220,7 +219,6 @@ class CarInterface(CarInterfaceBase):
     events = self.create_common_events(ret, extra_gears=[GearShifter.sport, GearShifter.low,
                                                          GearShifter.eco, GearShifter.manumatic],
                                        pcm_enable=self.CP.pcmCruise, enable_buttons=(ButtonType.decelCruise,))
-    events = self.dp_atl_warning(ret, events)
     if not self.CP.pcmCruise:
       if any(b.type == ButtonType.accelCruise and b.pressed for b in ret.buttonEvents):
         events.add(EventName.buttonEnable)
diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py
index 3241bc7ee..2d17bd9b0 100755
--- a/selfdrive/car/honda/interface.py
+++ b/selfdrive/car/honda/interface.py
@@ -53,10 +53,14 @@ class CarInterface(CarInterfaceBase):
 
     # dp - attempt to disable op long
     params = Params()
-    if int(params.get("dp_atl").decode('utf-8')) == 1:
-      ret.openpilotLongitudinalControl = False
-      if candidate in HONDA_BOSCH:
-        ret.pcmCruise = not ret.openpilotLongitudinalControl
+    dp_atl = int(params.get("dp_atl").decode('utf-8'))
+    if dp_atl > 0:
+      ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_ALKA
+      if dp_atl == 1:
+        ret.openpilotLongitudinalControl = False
+        # update pcmCruise again
+        if candidate in HONDA_BOSCH:
+          ret.pcmCruise = not ret.openpilotLongitudinalControl
 
     if candidate == CAR.CRV_5G:
       ret.enableBsm = 0x12f8bfa7 in fingerprint[0]
@@ -335,7 +339,6 @@ class CarInterface(CarInterfaceBase):
   # returns a car.CarState
   def _update(self, c):
     ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)
-    ret.cruiseState.enabled, ret.cruiseState.available = self.dp_atl_mode(ret)
 
     #dp
     ret.engineRPM = self.CS.engineRPM
@@ -352,7 +355,6 @@ class CarInterface(CarInterfaceBase):
 
     # events
     events = self.create_common_events(ret, pcm_enable=False)
-    events = self.dp_atl_warning(ret, events)
     if self.CS.brake_error:
       events.add(EventName.brakeUnavailable)
 
diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index 5582499f2..822b1ef8e 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -113,7 +113,7 @@ class CarController:
       hda2_long = hda2 and self.CP.openpilotLongitudinalControl
 
       # steering control
-      can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, CC.enabled, lat_active, apply_steer))
+      can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, CC.latActive, lat_active, apply_steer))
 
       # disable LFA on HDA2
       if self.frame % 5 == 0 and hda2:
@@ -121,7 +121,7 @@ class CarController:
 
       # LFA and HDA icons
       if self.frame % 5 == 0 and (not hda2 or hda2_long):
-        can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CP, CC.enabled))
+        can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CP, CC.latActive))
 
       # blinkers
       if hda2 and self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
@@ -158,7 +158,7 @@ class CarController:
               self.last_button_frame = self.frame
     else:
       can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, lat_active,
-                                                torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled,
+                                                torque_fault, CS.lkas11, sys_warning, sys_state, CC.latActive,
                                                 hud_control.leftLaneVisible, hud_control.rightLaneVisible,
                                                 left_lane_warning, right_lane_warning))
 
diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index 0545c75eb..88466e331 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -227,7 +227,15 @@ class CarInterface(CarInterfaceBase):
       ret.longitudinalTuning.kpV = [0.5]
       ret.longitudinalTuning.kiV = [0.0]
       ret.experimentalLongitudinalAvailable = candidate not in (LEGACY_SAFETY_MODE_CAR | CAMERA_SCC_CAR)
-    ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable
+
+    params = Params()
+    dp_atl = int(params.get("dp_atl").decode('utf-8'))
+    if dp_atl > 0:
+      ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_ALKA
+      if dp_atl == 1:
+        ret.openpilotLongitudinalControl = False
+
+    ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable and dp_atl != 1
     ret.pcmCruise = not ret.openpilotLongitudinalControl
 
     ret.stoppingControl = True
@@ -285,9 +293,6 @@ class CarInterface(CarInterfaceBase):
     # mass and CG position, so all cars will have approximately similar dyn behaviors
     ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
                                                                          tire_stiffness_factor=tire_stiffness_factor)
-    params = Params()
-    if int(params.get("dp_atl").decode('utf-8')) == 1:
-      ret.openpilotLongitudinalControl = False
     params.put("dp_lateral_steer_rate_cost", "0.5")
     return ret
 
@@ -305,7 +310,6 @@ class CarInterface(CarInterfaceBase):
 
   def _update(self, c):
     ret = self.CS.update(self.cp, self.cp_cam)
-    ret.cruiseState.enabled, ret.cruiseState.available = self.dp_atl_mode(ret)
 
     if self.CS.CP.openpilotLongitudinalControl and self.CS.cruise_buttons[-1] != self.CS.prev_cruise_buttons:
       buttonEvents = [create_button_event(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT)]
@@ -320,7 +324,6 @@ class CarInterface(CarInterfaceBase):
     # Main button also can trigger an engagement on these cars
     allow_enable = any(btn in ENABLE_BUTTONS for btn in self.CS.cruise_buttons) or any(self.CS.main_buttons)
     events = self.create_common_events(ret, pcm_enable=self.CS.CP.pcmCruise, allow_enable=allow_enable)
-    events = self.dp_atl_warning(ret, events)
 
     # low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s)
     if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.:
diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py
index 7a8abc59d..7f0497a1a 100644
--- a/selfdrive/car/interfaces.py
+++ b/selfdrive/car/interfaces.py
@@ -267,12 +267,12 @@ class CarInterfaceBase(ABC):
       events.add(EventName.doorOpen)
     if cs_out.seatbeltUnlatched:
       events.add(EventName.seatbeltNotLatched)
-    if self.dragonconf.dpAtl != 1 and cs_out.gearShifter != GearShifter.drive and (extra_gears is None or
+    if cs_out.gearShifter != GearShifter.drive and (extra_gears is None or
        cs_out.gearShifter not in extra_gears):
       events.add(EventName.wrongGear)
     if cs_out.gearShifter == GearShifter.reverse:
       events.add(EventName.reverseGear)
-    if self.dragonconf.dpAtl == 0 and not cs_out.cruiseState.available:
+    if not cs_out.cruiseState.available:
       events.add(EventName.wrongCarMode)
     if cs_out.espDisabled:
       events.add(EventName.espDisabled)
@@ -282,13 +282,13 @@ class CarInterfaceBase(ABC):
       events.add(EventName.stockAeb)
     if self.dragonconf.dpSpeedCheck and cs_out.vEgo > MAX_CTRL_SPEED:
       events.add(EventName.speedTooHigh)
-    if self.dragonconf.dpAtl != 1 and cs_out.cruiseState.nonAdaptive:
+    if cs_out.cruiseState.nonAdaptive:
       events.add(EventName.wrongCruiseMode)
-    if self.dragonconf.dpAtl != 1 and cs_out.brakeHoldActive and self.CP.openpilotLongitudinalControl:
+    if cs_out.brakeHoldActive and self.CP.openpilotLongitudinalControl:
       events.add(EventName.brakeHold)
-    if self.dragonconf.dpAtl != 1 and cs_out.parkingBrake:
+    if cs_out.parkingBrake:
       events.add(EventName.parkBrake)
-    if self.dragonconf.dpAtl != 1 and cs_out.accFaulted:
+    if cs_out.accFaulted:
       events.add(EventName.accFaulted)
     if cs_out.steeringPressed:
       events.add(EventName.steerOverride)
@@ -326,29 +326,6 @@ class CarInterfaceBase(ABC):
 
     return events
 
-  def dp_atl_warning(self, ret, events):
-    if self.dragonconf.dpAtl > 0:
-      if self.dp_last_cruise_actual_enabled and not ret.cruiseActualEnabled:
-        events.add(EventName.communityFeatureDisallowedDEPRECATED)
-      elif ret.cruiseState.enabled != ret.cruiseActualEnabled:
-        events.add(EventName.gasPressedOverride)
-      self.dp_last_cruise_actual_enabled = ret.cruiseActualEnabled
-    return events
-
-  def dp_atl_mode(self, ret):
-    enable = ret.cruiseState.enabled
-    available = ret.cruiseState.available
-    if self.dragonconf.dpAtl > 0 and available:
-      enable = True
-      if ret.gearShifter in [car.CarState.GearShifter.reverse, car.CarState.GearShifter.park]:
-        enable = False
-        available = False
-      if ret.seatbeltUnlatched or ret.doorOpen:
-        enable = False
-        available = False
-    return enable, available
-
-
 class RadarInterfaceBase(ABC):
   def __init__(self, CP):
     self.rcp = None
diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py
index ff3cafed3..4a91f74ba 100755
--- a/selfdrive/car/mazda/interface.py
+++ b/selfdrive/car/mazda/interface.py
@@ -59,11 +59,9 @@ class CarInterface(CarInterfaceBase):
   # returns a car.CarState
   def _update(self, c):
     ret = self.CS.update(self.cp, self.cp_cam)
-    ret.cruiseState.enabled, ret.cruiseState.available = self.dp_atl_mode(ret)
 
     # events
     events = self.create_common_events(ret)
-    events = self.dp_atl_warning(ret, events)
 
     if self.CS.lkas_disabled:
       events.add(EventName.lkasDisabled)
diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py
index b20210ce3..57d8a3892 100644
--- a/selfdrive/car/nissan/interface.py
+++ b/selfdrive/car/nissan/interface.py
@@ -43,7 +43,6 @@ class CarInterface(CarInterfaceBase):
   # returns a car.CarState
   def _update(self, c):
     ret = self.CS.update(self.cp, self.cp_adas, self.cp_cam)
-    ret.cruiseState.enabled, ret.cruiseState.available = self.dp_atl_mode(ret)
 
     buttonEvents = []
     be = car.CarState.ButtonEvent.new_message()
@@ -51,7 +50,6 @@ class CarInterface(CarInterfaceBase):
     buttonEvents.append(be)
 
     events = self.create_common_events(ret)
-    events = self.dp_atl_warning(ret, events)
 
     if self.CS.lkas_enabled:
       events.add(car.CarEvent.EventName.invalidLkasSetting)
diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py
index 1615f7bae..28aacebb4 100644
--- a/selfdrive/car/subaru/interface.py
+++ b/selfdrive/car/subaru/interface.py
@@ -110,12 +110,10 @@ class CarInterface(CarInterfaceBase):
   def _update(self, c):
 
     ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)
-    ret.cruiseState.enabled, ret.cruiseState.available = self.dp_atl_mode(ret)
 
     ret.events = self.create_common_events(ret).to_msg()
 
     events = self.create_common_events(ret)
-    events = self.dp_atl_warning(ret, events)
 
     ret.events = events.to_msg()
 
diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index f8a81c511..2d82886a8 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -43,7 +43,6 @@ class CarController:
 
     # dp
     self.dp_toyota_sng = False
-    self.dp_atl = 0
 
     self.dp_toyota_auto_lock = False
     self.dp_toyota_auto_unlock = False
@@ -53,7 +52,6 @@ class CarController:
   def update(self, CC, CS, dragonconf):
     if dragonconf is not None:
       self.dp_toyota_sng = dragonconf.dpToyotaSng
-      self.dp_atl = dragonconf.dpAtl
       self.dp_toyota_auto_lock = dragonconf.dpToyotaAutoLock
       self.dp_toyota_auto_unlock = dragonconf.dpToyotaAutoUnlock
     actuators = CC.actuators
@@ -184,7 +182,7 @@ class CarController:
       if self.frame % 100 == 0 or send_ui:
         can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, hud_control.leftLaneVisible,
                                            hud_control.rightLaneVisible, hud_control.leftLaneDepart,
-                                           hud_control.rightLaneDepart, CC.enabled, CS.lkas_hud))
+                                           hud_control.rightLaneDepart, CC.latActive, CS.lkas_hud))
 
       if (self.frame % 100 == 0 or send_ui) and self.CP.enableDsu:
         can_sends.append(create_fcw_command(self.packer, fcw_alert))
diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 5d91a5188..dc17d4451 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -222,11 +222,15 @@ class CarInterface(CarInterfaceBase):
     ret.openpilotLongitudinalControl = smartDsu or ret.enableDsu or candidate in (TSS2_CAR - RADAR_ACC_CAR)
     ret.autoResumeSng = ret.openpilotLongitudinalControl and candidate in NO_STOP_TIMER_CAR
 
-    if int(params.get("dp_atl").decode('utf-8')) == 1:
-      ret.openpilotLongitudinalControl = False
+    dp_atl = int(Params().get("dp_atl").decode('utf-8'))
+    if dp_atl > 0:
+      ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_ALKA
 
-    if smartDsu and int(params.get("dp_atl").decode('utf-8')) == 2:
-      ret.openpilotLongitudinalControl = True
+      if dp_atl == 1:
+        ret.openpilotLongitudinalControl = False
+
+      if smartDsu and dp_atl == 2:
+        ret.openpilotLongitudinalControl = True
 
     if candidate == CAR.CHR_TSS2:
       ret.enableBsm = True
@@ -281,9 +285,6 @@ class CarInterface(CarInterfaceBase):
   def _update(self, c):
     ret = self.CS.update(self.cp, self.cp_cam)
 
-    # dp
-    ret.cruiseState.enabled, ret.cruiseState.available = self.dp_atl_mode(ret)
-
     # low speed re-write
     if self.dragonconf.dpToyotaCruiseOverride:
       if self.dragonconf.dpToyotaCruiseOverrideSpeed != self.dp_override_speed_last:
@@ -299,7 +300,6 @@ class CarInterface(CarInterfaceBase):
 
     # events
     events = self.create_common_events(ret)
-    events = self.dp_atl_warning(ret, events)
 
     if self.CP.openpilotLongitudinalControl:
       if ret.cruiseState.standstill and not ret.brakePressed and not self.CP.enableGasInterceptor:
diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py
index 628962de7..d3fc56793 100644
--- a/selfdrive/car/volkswagen/carcontroller.py
+++ b/selfdrive/car/volkswagen/carcontroller.py
@@ -5,7 +5,7 @@ from common.conversions import Conversions as CV
 from common.realtime import DT_CTRL
 from selfdrive.car import apply_std_steer_torque_limits
 from selfdrive.car.volkswagen import mqbcan, pqcan
-from selfdrive.car.volkswagen.values import CANBUS, PQ_CARS, CarControllerParams
+from selfdrive.car.volkswagen.values import CANBUS, PQ_CARS, CarControllerParams, STANDING_RESUME_SPAM_CARS
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 LongCtrlState = car.CarControl.Actuators.LongControlState
@@ -85,7 +85,7 @@ class CarController:
       hud_alert = 0
       if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw):
         hud_alert = self.CCP.LDW_MESSAGES["laneAssistTakeOver"]
-      can_sends.append(self.CCS.create_lka_hud_control(self.packer_pt, CANBUS.pt, CS.ldw_stock_values, CC.enabled,
+      can_sends.append(self.CCS.create_lka_hud_control(self.packer_pt, CANBUS.pt, CS.ldw_stock_values, CC.latActive,
                                                        CS.out.steeringPressed, hud_alert, hud_control))
 
     if self.frame % self.CCP.ACC_HUD_STEP == 0 and self.CP.openpilotLongitudinalControl:
@@ -99,11 +99,17 @@ class CarController:
 
     # **** Stock ACC Button Controls **************************************** #
 
-    gra_send_ready = self.CP.pcmCruise and CS.gra_stock_values["COUNTER"] != self.gra_acc_counter_last
-    if gra_send_ready and (CC.cruiseControl.cancel or CC.cruiseControl.resume):
-      counter = (CS.gra_stock_values["COUNTER"] + 1) % 16
-      can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, ext_bus, CS.gra_stock_values, counter,
-                                                           cancel=CC.cruiseControl.cancel, resume=CC.cruiseControl.resume))
+    if self.CP.pcmCruise and CS.gra_stock_values["COUNTER"] != self.gra_acc_counter_last:  # send just after stock
+      standing_resume_spam = CS.out.cruiseState.standstill and self.CP.carFingerprint in STANDING_RESUME_SPAM_CARS
+      spam_window = self.frame % 50 < 25  # 0.25 second gap between virtual button presses
+
+      press_cancel = CC.cruiseControl.cancel
+      press_resume = CC.cruiseControl.resume or (standing_resume_spam and spam_window)
+
+      if press_cancel or press_resume:
+        counter = (CS.gra_stock_values["COUNTER"] + 1) % 16
+        can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, ext_bus, CS.gra_stock_values, counter,
+                                                             cancel=press_cancel, resume=press_resume))
 
     new_actuators = actuators.copy()
     new_actuators.steer = self.apply_steer_last / self.CCP.STEER_MAX
diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py
index 2d2660ce5..c423b26c1 100644
--- a/selfdrive/car/volkswagen/interface.py
+++ b/selfdrive/car/volkswagen/interface.py
@@ -4,6 +4,7 @@ from common.conversions import Conversions as CV
 from selfdrive.car import STD_CARGO_KG, get_safety_config
 from selfdrive.car.interfaces import CarInterfaceBase
 from selfdrive.car.volkswagen.values import CAR, PQ_CARS, CANBUS, NetworkLocation, TransmissionType, GearShifter
+from common.params import Params
 
 ButtonType = car.CarState.ButtonEvent.Type
 EventName = car.CarEvent.EventName
@@ -80,8 +81,14 @@ class CarInterface(CarInterfaceBase):
 
     # Global longitudinal tuning defaults, can be overridden per-vehicle
 
+    dp_atl = int(Params().get("dp_atl").decode('utf-8'))
+    if dp_atl > 0:
+      ret.safetyConfigs[0].safetyParam |= Panda.FLAG_VOLKSWAGEN_ALKA
+      if dp_atl == 1:
+        ret.openpilotLongitudinalControl = False
+
     ret.experimentalLongitudinalAvailable = ret.networkLocation == NetworkLocation.gateway or use_off_car_defaults
-    if experimental_long:
+    if experimental_long and dp_atl != 1:
       # Proof-of-concept, prep for E2E only. No radar points available. Panda ALLOW_DEBUG firmware required.
       ret.openpilotLongitudinalControl = True
       ret.safetyConfigs[0].safetyParam |= Panda.FLAG_VOLKSWAGEN_LONG_CONTROL
@@ -218,12 +225,10 @@ class CarInterface(CarInterfaceBase):
   # returns a car.CarState
   def _update(self, c):
     ret = self.CS.update(self.cp, self.cp_cam, self.cp_ext, self.CP.transmissionType)
-    ret.cruiseState.enabled, ret.cruiseState.available = self.dp_atl_mode(ret)
 
     events = self.create_common_events(ret, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic],
                                        pcm_enable=not self.CS.CP.openpilotLongitudinalControl,
                                        enable_buttons=(ButtonType.setCruise, ButtonType.resumeCruise))
-    events = self.dp_atl_warning(ret, events)
 
     # Low speed steer alert hysteresis logic
     if self.CP.minSteerSpeed > 0. and ret.vEgo < (self.CP.minSteerSpeed + 1.):
diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py
index 208d146e2..1e3eeef54 100755
--- a/selfdrive/car/volkswagen/values.py
+++ b/selfdrive/car/volkswagen/values.py
@@ -136,8 +136,15 @@ class CAR:
   SKODA_OCTAVIA_MK3 = "SKODA OCTAVIA 3RD GEN"       # Chassis NE, Mk3 Skoda Octavia and variants
 
 
+# CARs based on the PQ35/PQ46/NMS platforms and using PQ-style CAN messaging (default is MQB)
 PQ_CARS = {CAR.PASSAT_NMS, CAR.SHARAN_MK2}
 
+# CARs that benefit from continuous Resume spam at a stop, effectively upgrading stock FtS to SnG
+# Appears to be MQB-A0s and the Transporter family
+# TODO: try to determine this from ABS firmware instead
+STANDING_RESUME_SPAM_CARS = {CAR.POLO_MK6, CAR.TCROSS_MK1, CAR.TROC_MK1, CAR.SKODA_KAMIQ_MK1,
+                             CAR.SKODA_SCALA_MK1, CAR.TRANSPORTER_T61}
+
 
 DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict("vw_mqb_2010", None))
 for car_type in PQ_CARS:
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 7887b93a2..e29f7b16f 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -121,8 +121,6 @@ class Controls:
 
     # set alternative experiences from parameters
     self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator")
-    if self.sm['dragonConf'].dpAtl > 0:
-      self.disengage_on_accelerator = False
     self.CP.alternativeExperience = 0
     if not self.disengage_on_accelerator:
       self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS
@@ -258,13 +256,13 @@ class Controls:
 
     # Block resume if cruise never previously enabled
     resume_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents)
-    if self.sm['dragonConf'].dpAtl == 0 and not self.CP.pcmCruise and not self.v_cruise_helper.v_cruise_initialized and resume_pressed:
+    if not self.CP.pcmCruise and not self.v_cruise_helper.v_cruise_initialized and resume_pressed:
       self.events.add(EventName.resumeBlocked)
 
     # Disable on rising edge of accelerator or brake. Also disable on brake when speed > 0
-    if self.sm['dragonConf'].dpAtl == 0 and ((CS.gasPressed and not self.CS_prev.gasPressed and self.disengage_on_accelerator) or \
+    if (CS.gasPressed and not self.CS_prev.gasPressed and self.disengage_on_accelerator) or \
       (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) or \
-      (CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill))):
+      (CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill)):
       self.events.add(EventName.pedalPressed)
 
     if CS.brakePressed and CS.standstill:
@@ -402,7 +400,7 @@ class Controls:
     if not self.sm['liveLocationKalman'].deviceStable:
       self.events.add(EventName.deviceFalling)
 
-    if self.sm['dragonConf'].dpAtl == 0 and not REPLAY:
+    if not REPLAY:
       # Check for mismatch between openpilot and car's PCM
       cruise_mismatch = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise)
       self.cruise_mismatch_counter = self.cruise_mismatch_counter + 1 if cruise_mismatch else 0
@@ -549,8 +547,6 @@ class Controls:
 
     # DISABLED
     elif self.state == State.disabled:
-      if CS.cruiseState.available and not CS.cruiseActualEnabled and self.sm['dragonConf'].dpAtl > 0 and not self.events.any(ET.NO_ENTRY):
-        self.state = State.overriding
       if self.events.any(ET.ENABLE):
         if self.events.any(ET.NO_ENTRY):
           self.current_alert_types.append(ET.NO_ENTRY)
@@ -596,8 +592,23 @@ class Controls:
                    CS.vEgo > self.CP.minSteerSpeed and not CS.standstill
     CC.longActive = self.enabled and not self.events.any(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl
 
-    if self.sm['dragonConf'].dpAtl == 2 and not CS.cruiseActualEnabled:
-      CC.longActive = False
+    if self.sm['dragonConf'].dpAtl > 0:
+      if not CS.cruiseState.available:
+        pass
+        # CC.latActive = False
+      elif CS.steerFaultTemporary:
+        pass
+        # CC.latActive = False
+      elif CS.steerFaultPermanent:
+        pass
+        # CC.latActive = False
+      elif CS.standstill:
+        pass
+        # CC.latActive = False
+      elif CS.gearShifter == car.CarState.GearShifter.reverse:
+        pass
+      else:
+        CC.latActive = True
 
     actuators = CC.actuators
     actuators.longControlState = self.LoC.long_control_state
@@ -701,12 +712,9 @@ class Controls:
     if self.joystick_mode and self.sm.rcv_frame['testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]:
       CC.cruiseControl.cancel = True
 
-    if self.dp_vag_resume_fix or self.sm['dragonConf'].dpAtl == 1:
-      CC.cruiseControl.resume = CS.cruiseActualEnabled and CS.cruiseState.standstill
-    else:
-      speeds = self.sm['longitudinalPlan'].speeds
-      if len(speeds):
-        CC.cruiseControl.resume = self.enabled and CS.cruiseState.standstill and speeds[-1] > 0.1
+    speeds = self.sm['longitudinalPlan'].speeds
+    if len(speeds):
+      CC.cruiseControl.resume = self.enabled and CS.cruiseState.standstill and speeds[-1] > 0.1
 
     hudControl = CC.hudControl
     hudControl.setSpeed = float(self.v_cruise_helper.v_cruise_cluster_kph * CV.KPH_TO_MS)
diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py
index 2f7642377..de86d61a3 100644
--- a/selfdrive/controls/lib/events.py
+++ b/selfdrive/controls/lib/events.py
@@ -195,7 +195,7 @@ class NormalPermanentAlert(Alert):
 
 
 class StartupAlert(Alert):
-  def __init__(self, alert_text_1: str, alert_text_2: str = _("Happy Holiday / New Years! Safe Travel, cya in 2023"), alert_status=AlertStatus.normal):
+  def __init__(self, alert_text_1: str, alert_text_2: str = _("Always keep hands on wheel and eyes on road"), alert_status=AlertStatus.normal):
     super().__init__(alert_text_1, alert_text_2,
                      alert_status, AlertSize.mid,
                      Priority.LOWER, VisualAlert.none, AudibleAlert.none, 10.),
@@ -449,7 +449,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
   },
 
   EventName.steerTempUnavailableSilent: {
-    ET.WARNING: Alert(
+    ET.PERMANENT: Alert(
       _("Steering Temporarily Unavailable"),
       "",
       AlertStatus.userPrompt, AlertSize.small,
@@ -525,7 +525,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
   },
 
   EventName.preLaneChangeLeft: {
-    ET.WARNING: Alert(
+    ET.PERMANENT: Alert(
       _("Steer Left to Start Lane Change Once Safe"),
       "",
       AlertStatus.normal, AlertSize.small,
@@ -533,7 +533,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
   },
 
   EventName.preLaneChangeRight: {
-    ET.WARNING: Alert(
+    ET.PERMANENT: Alert(
       _("Steer Right to Start Lane Change Once Safe"),
       "",
       AlertStatus.normal, AlertSize.small,
@@ -541,7 +541,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
   },
 
   EventName.laneChangeBlocked: {
-    ET.WARNING: Alert(
+    ET.PERMANENT: Alert(
       _("Car Detected in Blindspot"),
       "",
       AlertStatus.userPrompt, AlertSize.small,
@@ -549,7 +549,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
   },
 
   EventName.laneChange: {
-    ET.WARNING: Alert(
+    ET.PERMANENT: Alert(
       _("Changing Lanes"),
       "",
       AlertStatus.normal, AlertSize.small,
@@ -557,7 +557,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
   },
 
   EventName.steerSaturated: {
-    ET.WARNING: Alert(
+    ET.PERMANENT: Alert(
       _("Take Control"),
       _("Turn Exceeds Steering Limit"),
       AlertStatus.userPrompt, AlertSize.mid,
@@ -680,6 +680,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
   },
 
   EventName.steerTempUnavailable: {
+    ET.PERMANENT: soft_disable_alert(_("Steering Temporarily Unavailable")),
     ET.SOFT_DISABLE: soft_disable_alert(_("Steering Temporarily Unavailable")),
     ET.NO_ENTRY: NoEntryAlert(_("Steering Temporarily Unavailable")),
   },
diff --git a/selfdrive/controls/lib/lateral_mpc_lib/acados_ocp_lat.json b/selfdrive/controls/lib/lateral_mpc_lib/acados_ocp_lat.json
new file mode 100644
index 000000000..af9c99f25
--- /dev/null
+++ b/selfdrive/controls/lib/lateral_mpc_lib/acados_ocp_lat.json
@@ -0,0 +1,544 @@
+{
+    "acados_include_path": "/data/openpilot/third_party/acados/include/acados/include",
+    "acados_lib_path": "/data/openpilot/third_party/acados/include/acados/lib",
+    "code_export_directory": "/data/openpilot/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code",
+    "constraints": {
+        "C": [],
+        "C_e": [],
+        "D": [],
+        "constr_type": "BGH",
+        "constr_type_e": "BGH",
+        "idxbu": [],
+        "idxbx": [
+            2,
+            3
+        ],
+        "idxbx_0": [
+            0,
+            1,
+            2,
+            3
+        ],
+        "idxbx_e": [],
+        "idxbxe_0": [
+            0,
+            1,
+            2,
+            3
+        ],
+        "idxsbu": [],
+        "idxsbx": [],
+        "idxsbx_e": [],
+        "idxsg": [],
+        "idxsg_e": [],
+        "idxsh": [],
+        "idxsh_e": [],
+        "idxsphi": [],
+        "idxsphi_e": [],
+        "lbu": [],
+        "lbx": [
+            -1.5707963267948966,
+            -0.8726646259971648
+        ],
+        "lbx_0": [
+            0.0,
+            0.0,
+            0.0,
+            0.0
+        ],
+        "lbx_e": [],
+        "lg": [],
+        "lg_e": [],
+        "lh": [],
+        "lh_e": [],
+        "lphi": [],
+        "lphi_e": [],
+        "lsbu": [],
+        "lsbx": [],
+        "lsbx_e": [],
+        "lsg": [],
+        "lsg_e": [],
+        "lsh": [],
+        "lsh_e": [],
+        "lsphi": [],
+        "lsphi_e": [],
+        "ubu": [],
+        "ubx": [
+            1.5707963267948966,
+            0.8726646259971648
+        ],
+        "ubx_0": [
+            0.0,
+            0.0,
+            0.0,
+            0.0
+        ],
+        "ubx_e": [],
+        "ug": [],
+        "ug_e": [],
+        "uh": [],
+        "uh_e": [],
+        "uphi": [],
+        "uphi_e": [],
+        "usbu": [],
+        "usbx": [],
+        "usbx_e": [],
+        "usg": [],
+        "usg_e": [],
+        "ush": [],
+        "ush_e": [],
+        "usphi": [],
+        "usphi_e": []
+    },
+    "cost": {
+        "Vu": [
+            [
+                0.0
+            ],
+            [
+                0.0
+            ],
+            [
+                0.0
+            ],
+            [
+                0.0
+            ],
+            [
+                0.0
+            ]
+        ],
+        "Vu_0": [
+            [
+                0.0
+            ],
+            [
+                0.0
+            ],
+            [
+                0.0
+            ],
+            [
+                0.0
+            ],
+            [
+                0.0
+            ]
+        ],
+        "Vx": [
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ]
+        ],
+        "Vx_0": [
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ]
+        ],
+        "Vx_e": [
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ]
+        ],
+        "Vz": [],
+        "Vz_0": [],
+        "W": [
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ]
+        ],
+        "W_0": [
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ]
+        ],
+        "W_e": [
+            [
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0
+            ]
+        ],
+        "Zl": [],
+        "Zl_e": [],
+        "Zu": [],
+        "Zu_e": [],
+        "cost_ext_fun_type": "casadi",
+        "cost_ext_fun_type_0": "casadi",
+        "cost_ext_fun_type_e": "casadi",
+        "cost_type": "NONLINEAR_LS",
+        "cost_type_0": "NONLINEAR_LS",
+        "cost_type_e": "NONLINEAR_LS",
+        "yref": [
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0
+        ],
+        "yref_0": [
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0
+        ],
+        "yref_e": [
+            0.0,
+            0.0,
+            0.0
+        ],
+        "zl": [],
+        "zl_e": [],
+        "zu": [],
+        "zu_e": []
+    },
+    "cython_include_dirs": "/data/data/com.termux/files/usr/lib/python3.8/site-packages/numpy/core/include",
+    "dims": {
+        "N": 16,
+        "nbu": 0,
+        "nbx": 2,
+        "nbx_0": 4,
+        "nbx_e": 0,
+        "nbxe_0": 4,
+        "ng": 0,
+        "ng_e": 0,
+        "nh": 0,
+        "nh_e": 0,
+        "np": 2,
+        "nphi": 0,
+        "nphi_e": 0,
+        "nr": 0,
+        "nr_e": 0,
+        "ns": 0,
+        "ns_e": 0,
+        "nsbu": 0,
+        "nsbx": 0,
+        "nsbx_e": 0,
+        "nsg": 0,
+        "nsg_e": 0,
+        "nsh": 0,
+        "nsh_e": 0,
+        "nsphi": 0,
+        "nsphi_e": 0,
+        "nu": 1,
+        "nx": 4,
+        "ny": 5,
+        "ny_0": 5,
+        "ny_e": 3,
+        "nz": 0
+    },
+    "model": {
+        "dyn_disc_fun": null,
+        "dyn_disc_fun_jac": null,
+        "dyn_disc_fun_jac_hess": null,
+        "dyn_ext_fun_type": "casadi",
+        "dyn_source_discrete": null,
+        "gnsf": {
+            "nontrivial_f_LO": 1,
+            "purely_linear": 0
+        },
+        "name": "lat"
+    },
+    "parameter_values": [
+        0.0,
+        0.0
+    ],
+    "problem_class": "OCP",
+    "simulink_opts": {
+        "inputs": {
+            "cost_W": 0,
+            "cost_W_0": 0,
+            "cost_W_e": 0,
+            "lbu": 1,
+            "lbx": 1,
+            "lbx_0": 1,
+            "lbx_e": 1,
+            "lg": 1,
+            "lh": 1,
+            "parameter_traj": 1,
+            "reset_solver": 0,
+            "u_init": 0,
+            "ubu": 1,
+            "ubx": 1,
+            "ubx_0": 1,
+            "ubx_e": 1,
+            "ug": 1,
+            "uh": 1,
+            "x_init": 0,
+            "y_ref": 1,
+            "y_ref_0": 1,
+            "y_ref_e": 1
+        },
+        "outputs": {
+            "CPU_time": 1,
+            "CPU_time_lin": 0,
+            "CPU_time_qp": 0,
+            "CPU_time_sim": 0,
+            "KKT_residual": 1,
+            "solver_status": 1,
+            "sqp_iter": 1,
+            "u0": 1,
+            "utraj": 0,
+            "x1": 1,
+            "xtraj": 0
+        },
+        "samplingtime": "t0"
+    },
+    "solver_options": {
+        "Tsim": 0.009765625,
+        "alpha_min": 0.05,
+        "alpha_reduction": 0.7,
+        "collocation_type": "GAUSS_LEGENDRE",
+        "eps_sufficient_descent": 0.0001,
+        "exact_hess_constr": 1,
+        "exact_hess_cost": 1,
+        "exact_hess_dyn": 1,
+        "ext_cost_num_hess": 0,
+        "full_step_dual": 0,
+        "globalization": "FIXED_STEP",
+        "globalization_use_SOC": 0,
+        "hessian_approx": "GAUSS_NEWTON",
+        "hpipm_mode": "BALANCE",
+        "initialize_t_slacks": 0,
+        "integrator_type": "ERK",
+        "levenberg_marquardt": 0.0,
+        "line_search_use_sufficient_descent": 0,
+        "model_external_shared_lib_dir": null,
+        "model_external_shared_lib_name": null,
+        "nlp_solver_max_iter": 100,
+        "nlp_solver_step_length": 1.0,
+        "nlp_solver_tol_comp": 1e-06,
+        "nlp_solver_tol_eq": 1e-06,
+        "nlp_solver_tol_ineq": 1e-06,
+        "nlp_solver_tol_stat": 1e-06,
+        "nlp_solver_type": "SQP_RTI",
+        "print_level": 0,
+        "qp_solver": "PARTIAL_CONDENSING_HPIPM",
+        "qp_solver_cond_N": 1,
+        "qp_solver_iter_max": 1,
+        "qp_solver_tol_comp": null,
+        "qp_solver_tol_eq": null,
+        "qp_solver_tol_ineq": null,
+        "qp_solver_tol_stat": null,
+        "qp_solver_warm_start": 0,
+        "regularize_method": null,
+        "sim_method_jac_reuse": [
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0
+        ],
+        "sim_method_newton_iter": 3,
+        "sim_method_num_stages": [
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4
+        ],
+        "sim_method_num_steps": [
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1
+        ],
+        "tf": 2.5,
+        "time_steps": [
+            0.009765625,
+            0.029296875,
+            0.048828125,
+            0.068359375,
+            0.087890625,
+            0.107421875,
+            0.126953125,
+            0.146484375,
+            0.166015625,
+            0.185546875,
+            0.205078125,
+            0.224609375,
+            0.244140625,
+            0.263671875,
+            0.283203125,
+            0.302734375
+        ]
+    }
+}
\ No newline at end of file
diff --git a/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c b/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c
new file mode 100644
index 000000000..d0c541d3b
--- /dev/null
+++ b/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c
@@ -0,0 +1,278 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+// standard
+#include <stdio.h>
+#include <stdlib.h>
+
+// acados
+#include "acados_c/external_function_interface.h"
+#include "acados_c/sim_interface.h"
+#include "acados_c/external_function_interface.h"
+
+#include "acados/sim/sim_common.h"
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/print.h"
+
+
+// example specific
+#include "lat_model/lat_model.h"
+#include "acados_sim_solver_lat.h"
+
+
+// ** solver data **
+
+sim_solver_capsule * lat_acados_sim_solver_create_capsule()
+{
+    void* capsule_mem = malloc(sizeof(sim_solver_capsule));
+    sim_solver_capsule *capsule = (sim_solver_capsule *) capsule_mem;
+
+    return capsule;
+}
+
+
+int lat_acados_sim_solver_free_capsule(sim_solver_capsule * capsule)
+{
+    free(capsule);
+    return 0;
+}
+
+
+int lat_acados_sim_create(sim_solver_capsule * capsule)
+{
+    // initialize
+    const int nx = LAT_NX;
+    const int nu = LAT_NU;
+    const int nz = LAT_NZ;
+    const int np = LAT_NP;
+    bool tmp_bool;
+
+    
+    double Tsim = 0.009765625;
+
+    
+    // explicit ode
+    capsule->sim_forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+    capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+
+    capsule->sim_forw_vde_casadi->casadi_fun = &lat_expl_vde_forw;
+    capsule->sim_forw_vde_casadi->casadi_n_in = &lat_expl_vde_forw_n_in;
+    capsule->sim_forw_vde_casadi->casadi_n_out = &lat_expl_vde_forw_n_out;
+    capsule->sim_forw_vde_casadi->casadi_sparsity_in = &lat_expl_vde_forw_sparsity_in;
+    capsule->sim_forw_vde_casadi->casadi_sparsity_out = &lat_expl_vde_forw_sparsity_out;
+    capsule->sim_forw_vde_casadi->casadi_work = &lat_expl_vde_forw_work;
+    external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np);
+
+    capsule->sim_expl_ode_fun_casadi->casadi_fun = &lat_expl_ode_fun;
+    capsule->sim_expl_ode_fun_casadi->casadi_n_in = &lat_expl_ode_fun_n_in;
+    capsule->sim_expl_ode_fun_casadi->casadi_n_out = &lat_expl_ode_fun_n_out;
+    capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &lat_expl_ode_fun_sparsity_in;
+    capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &lat_expl_ode_fun_sparsity_out;
+    capsule->sim_expl_ode_fun_casadi->casadi_work = &lat_expl_ode_fun_work;
+    external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, np);
+
+    
+
+    // sim plan & config
+    sim_solver_plan_t plan;
+    plan.sim_solver = ERK;
+
+    // create correct config based on plan
+    sim_config * lat_sim_config = sim_config_create(plan);
+    capsule->acados_sim_config = lat_sim_config;
+
+    // sim dims
+    void *lat_sim_dims = sim_dims_create(lat_sim_config);
+    capsule->acados_sim_dims = lat_sim_dims;
+    sim_dims_set(lat_sim_config, lat_sim_dims, "nx", &nx);
+    sim_dims_set(lat_sim_config, lat_sim_dims, "nu", &nu);
+    sim_dims_set(lat_sim_config, lat_sim_dims, "nz", &nz);
+
+
+    // sim opts
+    sim_opts *lat_sim_opts = sim_opts_create(lat_sim_config, lat_sim_dims);
+    capsule->acados_sim_opts = lat_sim_opts;
+    int tmp_int = 3;
+    sim_opts_set(lat_sim_config, lat_sim_opts, "newton_iter", &tmp_int);
+    sim_collocation_type collocation_type = GAUSS_LEGENDRE;
+    sim_opts_set(lat_sim_config, lat_sim_opts, "collocation_type", &collocation_type);
+
+ 
+    tmp_int = 4;
+    sim_opts_set(lat_sim_config, lat_sim_opts, "num_stages", &tmp_int);
+    tmp_int = 1;
+    sim_opts_set(lat_sim_config, lat_sim_opts, "num_steps", &tmp_int);
+    tmp_bool = 0;
+    sim_opts_set(lat_sim_config, lat_sim_opts, "jac_reuse", &tmp_bool);
+
+
+    // sim in / out
+    sim_in *lat_sim_in = sim_in_create(lat_sim_config, lat_sim_dims);
+    capsule->acados_sim_in = lat_sim_in;
+    sim_out *lat_sim_out = sim_out_create(lat_sim_config, lat_sim_dims);
+    capsule->acados_sim_out = lat_sim_out;
+
+    sim_in_set(lat_sim_config, lat_sim_dims,
+               lat_sim_in, "T", &Tsim);
+
+    // model functions
+    lat_sim_config->model_set(lat_sim_in->model,
+                 "expl_vde_for", capsule->sim_forw_vde_casadi);
+    lat_sim_config->model_set(lat_sim_in->model,
+                 "expl_ode_fun", capsule->sim_expl_ode_fun_casadi);
+
+    // sim solver
+    sim_solver *lat_sim_solver = sim_solver_create(lat_sim_config,
+                                               lat_sim_dims, lat_sim_opts);
+    capsule->acados_sim_solver = lat_sim_solver;
+
+
+    /* initialize parameter values */
+    double* p = calloc(np, sizeof(double));
+    
+
+    lat_acados_sim_update_params(capsule, p, np);
+    free(p);
+
+
+    /* initialize input */
+    // x
+    double x0[4];
+    for (int ii = 0; ii < 4; ii++)
+        x0[ii] = 0.0;
+
+    sim_in_set(lat_sim_config, lat_sim_dims,
+               lat_sim_in, "x", x0);
+
+
+    // u
+    double u0[1];
+    for (int ii = 0; ii < 1; ii++)
+        u0[ii] = 0.0;
+
+    sim_in_set(lat_sim_config, lat_sim_dims,
+               lat_sim_in, "u", u0);
+
+    // S_forw
+    double S_forw[20];
+    for (int ii = 0; ii < 20; ii++)
+        S_forw[ii] = 0.0;
+    for (int ii = 0; ii < 4; ii++)
+        S_forw[ii + ii * 4 ] = 1.0;
+
+
+    sim_in_set(lat_sim_config, lat_sim_dims,
+               lat_sim_in, "S_forw", S_forw);
+
+    int status = sim_precompute(lat_sim_solver, lat_sim_in, lat_sim_out);
+
+    return status;
+}
+
+
+int lat_acados_sim_solve(sim_solver_capsule *capsule)
+{
+    // integrate dynamics using acados sim_solver
+    int status = sim_solve(capsule->acados_sim_solver,
+                           capsule->acados_sim_in, capsule->acados_sim_out);
+    if (status != 0)
+        printf("error in lat_acados_sim_solve()! Exiting.\n");
+
+    return status;
+}
+
+
+int lat_acados_sim_free(sim_solver_capsule *capsule)
+{
+    // free memory
+    sim_solver_destroy(capsule->acados_sim_solver);
+    sim_in_destroy(capsule->acados_sim_in);
+    sim_out_destroy(capsule->acados_sim_out);
+    sim_opts_destroy(capsule->acados_sim_opts);
+    sim_dims_destroy(capsule->acados_sim_dims);
+    sim_config_destroy(capsule->acados_sim_config);
+
+    // free external function
+    external_function_param_casadi_free(capsule->sim_forw_vde_casadi);
+    external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi);
+
+    return 0;
+}
+
+
+int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np)
+{
+    int status = 0;
+    int casadi_np = LAT_NP;
+
+    if (casadi_np != np) {
+        printf("lat_acados_sim_update_params: trying to set %i parameters for external functions."
+            " External function has %i parameters. Exiting.\n", np, casadi_np);
+        exit(1);
+    }
+    capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p);
+    capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p);
+
+    return status;
+}
+
+/* getters pointers to C objects*/
+sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_config;
+};
+
+sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_in;
+};
+
+sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_out;
+};
+
+void * lat_acados_get_sim_dims(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_dims;
+};
+
+sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_opts;
+};
+
+sim_solver  * lat_acados_get_sim_solver(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_solver;
+};
+
diff --git a/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h b/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h
new file mode 100644
index 000000000..86b9c84c9
--- /dev/null
+++ b/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h
@@ -0,0 +1,103 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+#ifndef ACADOS_SIM_lat_H_
+#define ACADOS_SIM_lat_H_
+
+#include "acados_c/sim_interface.h"
+#include "acados_c/external_function_interface.h"
+
+#define LAT_NX     4
+#define LAT_NZ     0
+#define LAT_NU     1
+#define LAT_NP     2
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+// ** capsule for solver data **
+typedef struct sim_solver_capsule
+{
+    // acados objects
+    sim_in *acados_sim_in;
+    sim_out *acados_sim_out;
+    sim_solver *acados_sim_solver;
+    sim_opts *acados_sim_opts;
+    sim_config *acados_sim_config;
+    void *acados_sim_dims;
+
+    /* external functions */
+    // ERK
+    external_function_param_casadi * sim_forw_vde_casadi;
+    external_function_param_casadi * sim_expl_ode_fun_casadi;
+    external_function_param_casadi * sim_expl_ode_hess;
+
+    // IRK
+    external_function_param_casadi * sim_impl_dae_fun;
+    external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z;
+    external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z;
+    external_function_param_casadi * sim_impl_dae_hess;
+
+    // GNSF
+    external_function_param_casadi * sim_gnsf_phi_fun;
+    external_function_param_casadi * sim_gnsf_phi_fun_jac_y;
+    external_function_param_casadi * sim_gnsf_phi_jac_y_uhat;
+    external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z;
+    external_function_param_casadi * sim_gnsf_get_matrices_fun;
+
+} sim_solver_capsule;
+
+
+ACADOS_SYMBOL_EXPORT int lat_acados_sim_create(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT int lat_acados_sim_solve(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT int lat_acados_sim_free(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *value, int np);
+
+ACADOS_SYMBOL_EXPORT sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT void * lat_acados_get_sim_dims(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT sim_solver * lat_acados_get_sim_solver(sim_solver_capsule *capsule);
+
+
+ACADOS_SYMBOL_EXPORT sim_solver_capsule * lat_acados_sim_solver_create_capsule(void);
+ACADOS_SYMBOL_EXPORT int lat_acados_sim_solver_free_capsule(sim_solver_capsule *capsule);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // ACADOS_SIM_lat_H_
diff --git a/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c b/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c
new file mode 100644
index 000000000..9294d2b70
--- /dev/null
+++ b/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c
@@ -0,0 +1,264 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+#define S_FUNCTION_NAME acados_solver_sfunction_lat
+#define S_FUNCTION_LEVEL 2
+
+#define MDL_START
+
+// acados
+// #include "acados/utils/print.h"
+#include "acados_c/sim_interface.h"
+#include "acados_c/external_function_interface.h"
+
+// example specific
+#include "lat_model/lat_model.h"
+#include "acados_solver_lat.h"
+
+#include "simstruc.h"
+
+#define SAMPLINGTIME 0.009765625
+
+static void mdlInitializeSizes (SimStruct *S)
+{
+    // specify the number of continuous and discrete states
+    ssSetNumContStates(S, 0);
+    ssSetNumDiscStates(S, 0);// specify the number of input ports
+    if ( !ssSetNumInputPorts(S, 8) )
+        return;
+
+    // specify the number of output ports
+    if ( !ssSetNumOutputPorts(S, 6) )
+        return;
+
+    // specify dimension information for the input ports
+    // lbx_0
+    ssSetInputPortVectorDimension(S, 0, 4);
+    // ubx_0
+    ssSetInputPortVectorDimension(S, 1, 4);
+    // parameters
+    ssSetInputPortVectorDimension(S, 2, (16+1) * 2);
+    // y_ref_0
+    ssSetInputPortVectorDimension(S, 3, 5);
+    // y_ref
+    ssSetInputPortVectorDimension(S, 4, 75);
+    // y_ref_e
+    ssSetInputPortVectorDimension(S, 5, 3);
+    // lbx
+    ssSetInputPortVectorDimension(S, 6, 30);
+    // ubx
+    ssSetInputPortVectorDimension(S, 7, 30);/* specify dimension information for the OUTPUT ports */
+    ssSetOutputPortVectorDimension(S, 0, 1 );
+    ssSetOutputPortVectorDimension(S, 1, 1 );
+    ssSetOutputPortVectorDimension(S, 2, 1 );
+    ssSetOutputPortVectorDimension(S, 3, 4 ); // state at shooting node 1
+    ssSetOutputPortVectorDimension(S, 4, 1);
+    ssSetOutputPortVectorDimension(S, 5, 1 );
+
+    // specify the direct feedthrough status
+    // should be set to 1 for all inputs used in mdlOutputs
+    ssSetInputPortDirectFeedThrough(S, 0, 1);
+    ssSetInputPortDirectFeedThrough(S, 1, 1);
+    ssSetInputPortDirectFeedThrough(S, 2, 1);
+    ssSetInputPortDirectFeedThrough(S, 3, 1);
+    ssSetInputPortDirectFeedThrough(S, 4, 1);
+    ssSetInputPortDirectFeedThrough(S, 5, 1);
+    ssSetInputPortDirectFeedThrough(S, 6, 1);
+    ssSetInputPortDirectFeedThrough(S, 7, 1);
+
+    // one sample time
+    ssSetNumSampleTimes(S, 1);
+}
+
+
+#if defined(MATLAB_MEX_FILE)
+
+#define MDL_SET_INPUT_PORT_DIMENSION_INFO
+#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
+
+static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
+{
+    if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
+         return;
+}
+
+static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
+{
+    if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
+         return;
+}
+
+#endif /* MATLAB_MEX_FILE */
+
+
+static void mdlInitializeSampleTimes(SimStruct *S)
+{
+    ssSetSampleTime(S, 0, SAMPLINGTIME);
+    ssSetOffsetTime(S, 0, 0.0);
+}
+
+
+static void mdlStart(SimStruct *S)
+{
+    lat_solver_capsule *capsule = lat_acados_create_capsule();
+    lat_acados_create(capsule);
+
+    ssSetUserData(S, (void*)capsule);
+}
+
+
+static void mdlOutputs(SimStruct *S, int_T tid)
+{
+    lat_solver_capsule *capsule = ssGetUserData(S);
+    ocp_nlp_config *nlp_config = lat_acados_get_nlp_config(capsule);
+    ocp_nlp_dims *nlp_dims = lat_acados_get_nlp_dims(capsule);
+    ocp_nlp_in *nlp_in = lat_acados_get_nlp_in(capsule);
+    ocp_nlp_out *nlp_out = lat_acados_get_nlp_out(capsule);
+
+    InputRealPtrsType in_sign;      
+
+    // local buffer
+    real_t buffer[5];
+
+    /* go through inputs */
+    // lbx_0
+    in_sign = ssGetInputPortRealSignalPtrs(S, 0);
+    for (int i = 0; i < 4; i++)
+        buffer[i] = (double)(*in_sign[i]);
+
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", buffer);
+    // ubx_0
+    in_sign = ssGetInputPortRealSignalPtrs(S, 1);
+    for (int i = 0; i < 4; i++)
+        buffer[i] = (double)(*in_sign[i]);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", buffer);
+    // parameters - stage-variant !!!
+    in_sign = ssGetInputPortRealSignalPtrs(S, 2);
+
+    // update value of parameters
+    for (int ii = 0; ii <= 16; ii++)
+    {
+        for (int jj = 0; jj < 2; jj++)
+            buffer[jj] = (double)(*in_sign[ii*2+jj]);
+        lat_acados_update_params(capsule, ii, buffer, 2);
+    }
+
+  
+    // y_ref_0
+    in_sign = ssGetInputPortRealSignalPtrs(S, 3);
+
+    for (int i = 0; i < 5; i++)
+        buffer[i] = (double)(*in_sign[i]);
+
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", (void *) buffer);
+
+  
+    // y_ref - for stages 1 to N-1
+    in_sign = ssGetInputPortRealSignalPtrs(S, 4);
+
+    for (int ii = 1; ii < 16; ii++)
+    {
+        for (int jj = 0; jj < 5; jj++)
+            buffer[jj] = (double)(*in_sign[(ii-1)*5+jj]);
+        ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "yref", (void *) buffer);
+    }
+
+  
+    // y_ref_e
+    in_sign = ssGetInputPortRealSignalPtrs(S, 5);
+
+    for (int i = 0; i < 3; i++)
+        buffer[i] = (double)(*in_sign[i]);
+
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 16, "yref", (void *) buffer);
+    // lbx
+    in_sign = ssGetInputPortRealSignalPtrs(S, 6);
+    for (int ii = 1; ii < 16; ii++)
+    {
+        for (int jj = 0; jj < 2; jj++)
+            buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbx", (void *) buffer);
+    }
+    // ubx
+    in_sign = ssGetInputPortRealSignalPtrs(S, 7);
+    for (int ii = 1; ii < 16; ii++)
+    {
+        for (int jj = 0; jj < 2; jj++)
+            buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubx", (void *) buffer);
+    }
+
+    /* call solver */
+    int rti_phase = 0;
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "rti_phase", &rti_phase);
+    int acados_status = lat_acados_solve(capsule);
+
+
+    /* set outputs */
+    // assign pointers to output signals
+    real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin;
+    int tmp_int;
+    out_u0 = ssGetOutputPortRealSignal(S, 0);
+    ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", (void *) out_u0);
+
+  
+    out_status = ssGetOutputPortRealSignal(S, 1);
+    *out_status = (real_t) acados_status;
+    out_KKT_res = ssGetOutputPortRealSignal(S, 2);
+    *out_KKT_res = (real_t) nlp_out->inf_norm_res;
+    out_x1 = ssGetOutputPortRealSignal(S, 3);
+    ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 1, "x", (void *) out_x1);
+    out_cpu_time = ssGetOutputPortRealSignal(S, 4);
+    // get solution time
+    ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_tot", (void *) out_cpu_time);
+    out_sqp_iter = ssGetOutputPortRealSignal(S, 5);
+    // get sqp iter
+    ocp_nlp_get(nlp_config, capsule->nlp_solver, "sqp_iter", (void *) &tmp_int);
+    *out_sqp_iter = (real_t) tmp_int;
+
+}
+
+static void mdlTerminate(SimStruct *S)
+{
+    lat_solver_capsule *capsule = ssGetUserData(S);
+
+    lat_acados_free(capsule);
+    lat_acados_free_capsule(capsule);
+}
+
+
+#ifdef  MATLAB_MEX_FILE
+#include "simulink.c"
+#else
+#include "cg_sfun.h"
+#endif
diff --git a/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c b/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c
new file mode 100644
index 000000000..c536654fd
--- /dev/null
+++ b/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c
@@ -0,0 +1,128 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+// standard
+#include <stdio.h>
+#include <stdlib.h>
+// acados
+#include "acados/utils/print.h"
+#include "acados/utils/math.h"
+#include "acados_c/sim_interface.h"
+#include "acados_sim_solver_lat.h"
+
+#define NX     LAT_NX
+#define NZ     LAT_NZ
+#define NU     LAT_NU
+#define NP     LAT_NP
+
+
+int main()
+{
+    int status = 0;
+    sim_solver_capsule *capsule = lat_acados_sim_solver_create_capsule();
+    status = lat_acados_sim_create(capsule);
+
+    if (status)
+    {
+        printf("acados_create() returned status %d. Exiting.\n", status);
+        exit(1);
+    }
+
+    sim_config *acados_sim_config = lat_acados_get_sim_config(capsule);
+    sim_in *acados_sim_in = lat_acados_get_sim_in(capsule);
+    sim_out *acados_sim_out = lat_acados_get_sim_out(capsule);
+    void *acados_sim_dims = lat_acados_get_sim_dims(capsule);
+
+    // initial condition
+    double x_current[NX];
+    x_current[0] = 0.0;
+    x_current[1] = 0.0;
+    x_current[2] = 0.0;
+    x_current[3] = 0.0;
+
+  
+    x_current[0] = 0;
+    x_current[1] = 0;
+    x_current[2] = 0;
+    x_current[3] = 0;
+    
+  
+
+
+    // initial value for control input
+    double u0[NU];
+    u0[0] = 0.0;
+    // set parameters
+    double p[NP];
+    p[0] = 0;
+    p[1] = 0;
+
+    lat_acados_sim_update_params(capsule, p, NP);
+  
+
+    int n_sim_steps = 3;
+    // solve ocp in loop
+    for (int ii = 0; ii < n_sim_steps; ii++)
+    {
+        sim_in_set(acados_sim_config, acados_sim_dims,
+            acados_sim_in, "x", x_current);
+        status = lat_acados_sim_solve(capsule);
+
+        if (status != ACADOS_SUCCESS)
+        {
+            printf("acados_solve() failed with status %d.\n", status);
+        }
+
+        sim_out_get(acados_sim_config, acados_sim_dims,
+               acados_sim_out, "x", x_current);
+        
+        printf("\nx_current, %d\n", ii);
+        for (int jj = 0; jj < NX; jj++)
+        {
+            printf("%e\n", x_current[jj]);
+        }
+    }
+
+    printf("\nPerformed %d simulation steps with acados integrator successfully.\n\n", n_sim_steps);
+
+    // free solver
+    status = lat_acados_sim_free(capsule);
+    if (status) {
+        printf("lat_acados_sim_free() returned status %d. \n", status);
+    }
+
+    lat_acados_sim_solver_free_capsule(capsule);
+
+    return status;
+}
diff --git a/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m b/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m
new file mode 100644
index 000000000..f8371f3e8
--- /dev/null
+++ b/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m
@@ -0,0 +1,125 @@
+%
+% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+%
+% This file is part of acados.
+%
+% The 2-Clause BSD License
+%
+% Redistribution and use in source and binary forms, with or without
+% modification, are permitted provided that the following conditions are met:
+%
+% 1. Redistributions of source code must retain the above copyright notice,
+% this list of conditions and the following disclaimer.
+%
+% 2. Redistributions in binary form must reproduce the above copyright notice,
+% this list of conditions and the following disclaimer in the documentation
+% and/or other materials provided with the distribution.
+%
+% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+% POSSIBILITY OF SUCH DAMAGE.;
+%
+
+SOURCES = { ...
+            'lat_model/lat_expl_ode_fun.c', ...
+            'lat_model/lat_expl_vde_forw.c',...
+            'lat_cost/lat_cost_y_0_fun.c',...
+            'lat_cost/lat_cost_y_0_fun_jac_ut_xt.c',...
+            'lat_cost/lat_cost_y_0_hess.c',...
+            'lat_cost/lat_cost_y_fun.c',...
+            'lat_cost/lat_cost_y_fun_jac_ut_xt.c',...
+            'lat_cost/lat_cost_y_hess.c',...
+            'lat_cost/lat_cost_y_e_fun.c',...
+            'lat_cost/lat_cost_y_e_fun_jac_ut_xt.c',...
+            'lat_cost/lat_cost_y_e_hess.c',...
+            'acados_solver_sfunction_lat.c', ...
+            'acados_solver_lat.c'
+          };
+
+INC_PATH = '/data/openpilot/third_party/acados/include/acados/include';
+
+INCS = {['-I', fullfile(INC_PATH, 'blasfeo', 'include')], ...
+        ['-I', fullfile(INC_PATH, 'hpipm', 'include')], ...
+        ['-I', fullfile(INC_PATH, 'acados')], ...
+        ['-I', fullfile(INC_PATH)]};
+
+
+
+CFLAGS = 'CFLAGS=$CFLAGS';
+LDFLAGS = 'LDFLAGS=$LDFLAGS';
+COMPFLAGS = 'COMPFLAGS=$COMPFLAGS';
+COMPDEFINES = 'COMPDEFINES=$COMPDEFINES';
+
+
+
+LIB_PATH = ['-L', fullfile('/data/openpilot/third_party/acados/include/acados/lib')];
+
+LIBS = {'-lacados', '-lhpipm', '-lblasfeo'};
+
+% acados linking libraries and flags
+    
+
+mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ...
+    LIB_PATH, LIBS{:}, SOURCES{:}, ...
+    '-output', 'acados_solver_sfunction_lat' );
+
+fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_lat', '.', ...
+    eval('mexext')] );
+
+
+%% print note on usage of s-function
+fprintf('\n\nNote: Usage of Sfunction is as follows:\n')
+input_note = 'Inputs are:\n';
+i_in = 1;
+input_note = strcat(input_note, num2str(i_in), ') lbx_0 - lower bound on x for stage 0,',...
+                    ' size [4]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',...
+                    ' size [4]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all shooting nodes 0 to N+1,',...
+                    ' size [34]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [5]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') y_ref - concatenated for shooting nodes 1 to N-1,',...
+                    ' size [75]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [3]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') lbx for shooting nodes 1 to N-1, size [30]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') ubx for shooting nodes 1 to N-1, size [30]\n ');
+i_in = i_in + 1;
+
+fprintf(input_note)
+
+disp(' ')
+
+output_note = 'Outputs are:\n';
+i_out = 0;
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') u0, control input at node 0, size [1]\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') acados solver status (0 = SUCCESS)\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') KKT residual\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') x1, state at node 1\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') CPU time\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n ');
+
+fprintf(output_note)
diff --git a/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/acados_ocp_lat.json b/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/acados_ocp_lat.json
new file mode 100644
index 000000000..1cbe32253
--- /dev/null
+++ b/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/acados_ocp_lat.json
@@ -0,0 +1,450 @@
+{
+    "acados_include_path": "/data/openpilot/third_party/acados/include/acados/include",
+    "acados_lib_path": "/data/openpilot/third_party/acados/include/acados/lib",
+    "code_export_directory": "/data/openpilot/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code",
+    "constraints": {
+        "C": [],
+        "C_e": [],
+        "D": [],
+        "constr_type": "BGH",
+        "constr_type_e": "BGH",
+        "idxbu": [],
+        "idxbx": [
+            2,
+            3
+        ],
+        "idxbx_0": [
+            0,
+            1,
+            2,
+            3
+        ],
+        "idxbx_e": [],
+        "idxbxe_0": [
+            0,
+            1,
+            2,
+            3
+        ],
+        "idxsbu": [],
+        "idxsbx": [],
+        "idxsbx_e": [],
+        "idxsg": [],
+        "idxsg_e": [],
+        "idxsh": [],
+        "idxsh_e": [],
+        "idxsphi": [],
+        "idxsphi_e": [],
+        "lbu": [],
+        "lbx": [
+            -1.5707963267948966,
+            -0.8726646259971648
+        ],
+        "lbx_0": [
+            0.0,
+            0.0,
+            0.0,
+            0.0
+        ],
+        "lbx_e": [],
+        "lg": [],
+        "lg_e": [],
+        "lh": [],
+        "lh_e": [],
+        "lphi": [],
+        "lphi_e": [],
+        "lsbu": [],
+        "lsbx": [],
+        "lsbx_e": [],
+        "lsg": [],
+        "lsg_e": [],
+        "lsh": [],
+        "lsh_e": [],
+        "lsphi": [],
+        "lsphi_e": [],
+        "ubu": [],
+        "ubx": [
+            1.5707963267948966,
+            0.8726646259971648
+        ],
+        "ubx_0": [
+            0.0,
+            0.0,
+            0.0,
+            0.0
+        ],
+        "ubx_e": [],
+        "ug": [],
+        "ug_e": [],
+        "uh": [],
+        "uh_e": [],
+        "uphi": [],
+        "uphi_e": [],
+        "usbu": [],
+        "usbx": [],
+        "usbx_e": [],
+        "usg": [],
+        "usg_e": [],
+        "ush": [],
+        "ush_e": [],
+        "usphi": [],
+        "usphi_e": []
+    },
+    "cost": {
+        "Vu": [
+            [
+                0.0
+            ],
+            [
+                0.0
+            ],
+            [
+                0.0
+            ]
+        ],
+        "Vu_0": [
+            [
+                0.0
+            ],
+            [
+                0.0
+            ],
+            [
+                0.0
+            ]
+        ],
+        "Vx": [
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ]
+        ],
+        "Vx_0": [
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ]
+        ],
+        "Vx_e": [
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ]
+        ],
+        "Vz": [],
+        "Vz_0": [],
+        "W": [
+            [
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0
+            ]
+        ],
+        "W_0": [
+            [
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0
+            ]
+        ],
+        "W_e": [
+            [
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0
+            ]
+        ],
+        "Zl": [],
+        "Zl_e": [],
+        "Zu": [],
+        "Zu_e": [],
+        "cost_ext_fun_type": "casadi",
+        "cost_ext_fun_type_0": "casadi",
+        "cost_ext_fun_type_e": "casadi",
+        "cost_type": "NONLINEAR_LS",
+        "cost_type_0": "NONLINEAR_LS",
+        "cost_type_e": "NONLINEAR_LS",
+        "yref": [
+            0.0,
+            0.0,
+            0.0
+        ],
+        "yref_0": [
+            0.0,
+            0.0,
+            0.0
+        ],
+        "yref_e": [
+            0.0,
+            0.0
+        ],
+        "zl": [],
+        "zl_e": [],
+        "zu": [],
+        "zu_e": []
+    },
+    "cython_include_dirs": "/data/data/com.termux/files/usr/lib/python3.8/site-packages/numpy/core/include",
+    "dims": {
+        "N": 16,
+        "nbu": 0,
+        "nbx": 2,
+        "nbx_0": 4,
+        "nbx_e": 0,
+        "nbxe_0": 4,
+        "ng": 0,
+        "ng_e": 0,
+        "nh": 0,
+        "nh_e": 0,
+        "np": 2,
+        "nphi": 0,
+        "nphi_e": 0,
+        "nr": 0,
+        "nr_e": 0,
+        "ns": 0,
+        "ns_e": 0,
+        "nsbu": 0,
+        "nsbx": 0,
+        "nsbx_e": 0,
+        "nsg": 0,
+        "nsg_e": 0,
+        "nsh": 0,
+        "nsh_e": 0,
+        "nsphi": 0,
+        "nsphi_e": 0,
+        "nu": 1,
+        "nx": 4,
+        "ny": 3,
+        "ny_0": 3,
+        "ny_e": 2,
+        "nz": 0
+    },
+    "model": {
+        "dyn_disc_fun": null,
+        "dyn_disc_fun_jac": null,
+        "dyn_disc_fun_jac_hess": null,
+        "dyn_ext_fun_type": "casadi",
+        "dyn_source_discrete": null,
+        "gnsf": {
+            "nontrivial_f_LO": 1,
+            "purely_linear": 0
+        },
+        "name": "lat"
+    },
+    "parameter_values": [
+        0.0,
+        0.0
+    ],
+    "problem_class": "OCP",
+    "simulink_opts": {
+        "inputs": {
+            "cost_W": 0,
+            "cost_W_0": 0,
+            "cost_W_e": 0,
+            "lbu": 1,
+            "lbx": 1,
+            "lbx_0": 1,
+            "lbx_e": 1,
+            "lg": 1,
+            "lh": 1,
+            "parameter_traj": 1,
+            "reset_solver": 0,
+            "u_init": 0,
+            "ubu": 1,
+            "ubx": 1,
+            "ubx_0": 1,
+            "ubx_e": 1,
+            "ug": 1,
+            "uh": 1,
+            "x_init": 0,
+            "y_ref": 1,
+            "y_ref_0": 1,
+            "y_ref_e": 1
+        },
+        "outputs": {
+            "CPU_time": 1,
+            "CPU_time_lin": 0,
+            "CPU_time_qp": 0,
+            "CPU_time_sim": 0,
+            "KKT_residual": 1,
+            "solver_status": 1,
+            "sqp_iter": 1,
+            "u0": 1,
+            "utraj": 0,
+            "x1": 1,
+            "xtraj": 0
+        },
+        "samplingtime": "t0"
+    },
+    "solver_options": {
+        "Tsim": 0.009765625,
+        "alpha_min": 0.05,
+        "alpha_reduction": 0.7,
+        "collocation_type": "GAUSS_LEGENDRE",
+        "eps_sufficient_descent": 0.0001,
+        "exact_hess_constr": 1,
+        "exact_hess_cost": 1,
+        "exact_hess_dyn": 1,
+        "ext_cost_num_hess": 0,
+        "full_step_dual": 0,
+        "globalization": "FIXED_STEP",
+        "globalization_use_SOC": 0,
+        "hessian_approx": "GAUSS_NEWTON",
+        "hpipm_mode": "BALANCE",
+        "initialize_t_slacks": 0,
+        "integrator_type": "ERK",
+        "levenberg_marquardt": 0.0,
+        "line_search_use_sufficient_descent": 0,
+        "model_external_shared_lib_dir": null,
+        "model_external_shared_lib_name": null,
+        "nlp_solver_max_iter": 100,
+        "nlp_solver_step_length": 1.0,
+        "nlp_solver_tol_comp": 1e-06,
+        "nlp_solver_tol_eq": 1e-06,
+        "nlp_solver_tol_ineq": 1e-06,
+        "nlp_solver_tol_stat": 1e-06,
+        "nlp_solver_type": "SQP_RTI",
+        "print_level": 0,
+        "qp_solver": "PARTIAL_CONDENSING_HPIPM",
+        "qp_solver_cond_N": 1,
+        "qp_solver_iter_max": 1,
+        "qp_solver_tol_comp": null,
+        "qp_solver_tol_eq": null,
+        "qp_solver_tol_ineq": null,
+        "qp_solver_tol_stat": null,
+        "qp_solver_warm_start": 0,
+        "regularize_method": null,
+        "sim_method_jac_reuse": [
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0
+        ],
+        "sim_method_newton_iter": 3,
+        "sim_method_num_stages": [
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4
+        ],
+        "sim_method_num_steps": [
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1
+        ],
+        "tf": 2.5,
+        "time_steps": [
+            0.009765625,
+            0.029296875,
+            0.048828125,
+            0.068359375,
+            0.087890625,
+            0.107421875,
+            0.126953125,
+            0.146484375,
+            0.166015625,
+            0.185546875,
+            0.205078125,
+            0.224609375,
+            0.244140625,
+            0.263671875,
+            0.283203125,
+            0.302734375
+        ]
+    }
+}
\ No newline at end of file
diff --git a/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c b/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c
new file mode 100644
index 000000000..d0c541d3b
--- /dev/null
+++ b/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c
@@ -0,0 +1,278 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+// standard
+#include <stdio.h>
+#include <stdlib.h>
+
+// acados
+#include "acados_c/external_function_interface.h"
+#include "acados_c/sim_interface.h"
+#include "acados_c/external_function_interface.h"
+
+#include "acados/sim/sim_common.h"
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/print.h"
+
+
+// example specific
+#include "lat_model/lat_model.h"
+#include "acados_sim_solver_lat.h"
+
+
+// ** solver data **
+
+sim_solver_capsule * lat_acados_sim_solver_create_capsule()
+{
+    void* capsule_mem = malloc(sizeof(sim_solver_capsule));
+    sim_solver_capsule *capsule = (sim_solver_capsule *) capsule_mem;
+
+    return capsule;
+}
+
+
+int lat_acados_sim_solver_free_capsule(sim_solver_capsule * capsule)
+{
+    free(capsule);
+    return 0;
+}
+
+
+int lat_acados_sim_create(sim_solver_capsule * capsule)
+{
+    // initialize
+    const int nx = LAT_NX;
+    const int nu = LAT_NU;
+    const int nz = LAT_NZ;
+    const int np = LAT_NP;
+    bool tmp_bool;
+
+    
+    double Tsim = 0.009765625;
+
+    
+    // explicit ode
+    capsule->sim_forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+    capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+
+    capsule->sim_forw_vde_casadi->casadi_fun = &lat_expl_vde_forw;
+    capsule->sim_forw_vde_casadi->casadi_n_in = &lat_expl_vde_forw_n_in;
+    capsule->sim_forw_vde_casadi->casadi_n_out = &lat_expl_vde_forw_n_out;
+    capsule->sim_forw_vde_casadi->casadi_sparsity_in = &lat_expl_vde_forw_sparsity_in;
+    capsule->sim_forw_vde_casadi->casadi_sparsity_out = &lat_expl_vde_forw_sparsity_out;
+    capsule->sim_forw_vde_casadi->casadi_work = &lat_expl_vde_forw_work;
+    external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np);
+
+    capsule->sim_expl_ode_fun_casadi->casadi_fun = &lat_expl_ode_fun;
+    capsule->sim_expl_ode_fun_casadi->casadi_n_in = &lat_expl_ode_fun_n_in;
+    capsule->sim_expl_ode_fun_casadi->casadi_n_out = &lat_expl_ode_fun_n_out;
+    capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &lat_expl_ode_fun_sparsity_in;
+    capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &lat_expl_ode_fun_sparsity_out;
+    capsule->sim_expl_ode_fun_casadi->casadi_work = &lat_expl_ode_fun_work;
+    external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, np);
+
+    
+
+    // sim plan & config
+    sim_solver_plan_t plan;
+    plan.sim_solver = ERK;
+
+    // create correct config based on plan
+    sim_config * lat_sim_config = sim_config_create(plan);
+    capsule->acados_sim_config = lat_sim_config;
+
+    // sim dims
+    void *lat_sim_dims = sim_dims_create(lat_sim_config);
+    capsule->acados_sim_dims = lat_sim_dims;
+    sim_dims_set(lat_sim_config, lat_sim_dims, "nx", &nx);
+    sim_dims_set(lat_sim_config, lat_sim_dims, "nu", &nu);
+    sim_dims_set(lat_sim_config, lat_sim_dims, "nz", &nz);
+
+
+    // sim opts
+    sim_opts *lat_sim_opts = sim_opts_create(lat_sim_config, lat_sim_dims);
+    capsule->acados_sim_opts = lat_sim_opts;
+    int tmp_int = 3;
+    sim_opts_set(lat_sim_config, lat_sim_opts, "newton_iter", &tmp_int);
+    sim_collocation_type collocation_type = GAUSS_LEGENDRE;
+    sim_opts_set(lat_sim_config, lat_sim_opts, "collocation_type", &collocation_type);
+
+ 
+    tmp_int = 4;
+    sim_opts_set(lat_sim_config, lat_sim_opts, "num_stages", &tmp_int);
+    tmp_int = 1;
+    sim_opts_set(lat_sim_config, lat_sim_opts, "num_steps", &tmp_int);
+    tmp_bool = 0;
+    sim_opts_set(lat_sim_config, lat_sim_opts, "jac_reuse", &tmp_bool);
+
+
+    // sim in / out
+    sim_in *lat_sim_in = sim_in_create(lat_sim_config, lat_sim_dims);
+    capsule->acados_sim_in = lat_sim_in;
+    sim_out *lat_sim_out = sim_out_create(lat_sim_config, lat_sim_dims);
+    capsule->acados_sim_out = lat_sim_out;
+
+    sim_in_set(lat_sim_config, lat_sim_dims,
+               lat_sim_in, "T", &Tsim);
+
+    // model functions
+    lat_sim_config->model_set(lat_sim_in->model,
+                 "expl_vde_for", capsule->sim_forw_vde_casadi);
+    lat_sim_config->model_set(lat_sim_in->model,
+                 "expl_ode_fun", capsule->sim_expl_ode_fun_casadi);
+
+    // sim solver
+    sim_solver *lat_sim_solver = sim_solver_create(lat_sim_config,
+                                               lat_sim_dims, lat_sim_opts);
+    capsule->acados_sim_solver = lat_sim_solver;
+
+
+    /* initialize parameter values */
+    double* p = calloc(np, sizeof(double));
+    
+
+    lat_acados_sim_update_params(capsule, p, np);
+    free(p);
+
+
+    /* initialize input */
+    // x
+    double x0[4];
+    for (int ii = 0; ii < 4; ii++)
+        x0[ii] = 0.0;
+
+    sim_in_set(lat_sim_config, lat_sim_dims,
+               lat_sim_in, "x", x0);
+
+
+    // u
+    double u0[1];
+    for (int ii = 0; ii < 1; ii++)
+        u0[ii] = 0.0;
+
+    sim_in_set(lat_sim_config, lat_sim_dims,
+               lat_sim_in, "u", u0);
+
+    // S_forw
+    double S_forw[20];
+    for (int ii = 0; ii < 20; ii++)
+        S_forw[ii] = 0.0;
+    for (int ii = 0; ii < 4; ii++)
+        S_forw[ii + ii * 4 ] = 1.0;
+
+
+    sim_in_set(lat_sim_config, lat_sim_dims,
+               lat_sim_in, "S_forw", S_forw);
+
+    int status = sim_precompute(lat_sim_solver, lat_sim_in, lat_sim_out);
+
+    return status;
+}
+
+
+int lat_acados_sim_solve(sim_solver_capsule *capsule)
+{
+    // integrate dynamics using acados sim_solver
+    int status = sim_solve(capsule->acados_sim_solver,
+                           capsule->acados_sim_in, capsule->acados_sim_out);
+    if (status != 0)
+        printf("error in lat_acados_sim_solve()! Exiting.\n");
+
+    return status;
+}
+
+
+int lat_acados_sim_free(sim_solver_capsule *capsule)
+{
+    // free memory
+    sim_solver_destroy(capsule->acados_sim_solver);
+    sim_in_destroy(capsule->acados_sim_in);
+    sim_out_destroy(capsule->acados_sim_out);
+    sim_opts_destroy(capsule->acados_sim_opts);
+    sim_dims_destroy(capsule->acados_sim_dims);
+    sim_config_destroy(capsule->acados_sim_config);
+
+    // free external function
+    external_function_param_casadi_free(capsule->sim_forw_vde_casadi);
+    external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi);
+
+    return 0;
+}
+
+
+int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np)
+{
+    int status = 0;
+    int casadi_np = LAT_NP;
+
+    if (casadi_np != np) {
+        printf("lat_acados_sim_update_params: trying to set %i parameters for external functions."
+            " External function has %i parameters. Exiting.\n", np, casadi_np);
+        exit(1);
+    }
+    capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p);
+    capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p);
+
+    return status;
+}
+
+/* getters pointers to C objects*/
+sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_config;
+};
+
+sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_in;
+};
+
+sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_out;
+};
+
+void * lat_acados_get_sim_dims(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_dims;
+};
+
+sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_opts;
+};
+
+sim_solver  * lat_acados_get_sim_solver(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_solver;
+};
+
diff --git a/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h b/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h
new file mode 100644
index 000000000..86b9c84c9
--- /dev/null
+++ b/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h
@@ -0,0 +1,103 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+#ifndef ACADOS_SIM_lat_H_
+#define ACADOS_SIM_lat_H_
+
+#include "acados_c/sim_interface.h"
+#include "acados_c/external_function_interface.h"
+
+#define LAT_NX     4
+#define LAT_NZ     0
+#define LAT_NU     1
+#define LAT_NP     2
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+// ** capsule for solver data **
+typedef struct sim_solver_capsule
+{
+    // acados objects
+    sim_in *acados_sim_in;
+    sim_out *acados_sim_out;
+    sim_solver *acados_sim_solver;
+    sim_opts *acados_sim_opts;
+    sim_config *acados_sim_config;
+    void *acados_sim_dims;
+
+    /* external functions */
+    // ERK
+    external_function_param_casadi * sim_forw_vde_casadi;
+    external_function_param_casadi * sim_expl_ode_fun_casadi;
+    external_function_param_casadi * sim_expl_ode_hess;
+
+    // IRK
+    external_function_param_casadi * sim_impl_dae_fun;
+    external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z;
+    external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z;
+    external_function_param_casadi * sim_impl_dae_hess;
+
+    // GNSF
+    external_function_param_casadi * sim_gnsf_phi_fun;
+    external_function_param_casadi * sim_gnsf_phi_fun_jac_y;
+    external_function_param_casadi * sim_gnsf_phi_jac_y_uhat;
+    external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z;
+    external_function_param_casadi * sim_gnsf_get_matrices_fun;
+
+} sim_solver_capsule;
+
+
+ACADOS_SYMBOL_EXPORT int lat_acados_sim_create(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT int lat_acados_sim_solve(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT int lat_acados_sim_free(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *value, int np);
+
+ACADOS_SYMBOL_EXPORT sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT void * lat_acados_get_sim_dims(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT sim_solver * lat_acados_get_sim_solver(sim_solver_capsule *capsule);
+
+
+ACADOS_SYMBOL_EXPORT sim_solver_capsule * lat_acados_sim_solver_create_capsule(void);
+ACADOS_SYMBOL_EXPORT int lat_acados_sim_solver_free_capsule(sim_solver_capsule *capsule);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // ACADOS_SIM_lat_H_
diff --git a/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c b/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c
new file mode 100644
index 000000000..827b6c7f0
--- /dev/null
+++ b/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c
@@ -0,0 +1,264 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+#define S_FUNCTION_NAME acados_solver_sfunction_lat
+#define S_FUNCTION_LEVEL 2
+
+#define MDL_START
+
+// acados
+// #include "acados/utils/print.h"
+#include "acados_c/sim_interface.h"
+#include "acados_c/external_function_interface.h"
+
+// example specific
+#include "lat_model/lat_model.h"
+#include "acados_solver_lat.h"
+
+#include "simstruc.h"
+
+#define SAMPLINGTIME 0.009765625
+
+static void mdlInitializeSizes (SimStruct *S)
+{
+    // specify the number of continuous and discrete states
+    ssSetNumContStates(S, 0);
+    ssSetNumDiscStates(S, 0);// specify the number of input ports
+    if ( !ssSetNumInputPorts(S, 8) )
+        return;
+
+    // specify the number of output ports
+    if ( !ssSetNumOutputPorts(S, 6) )
+        return;
+
+    // specify dimension information for the input ports
+    // lbx_0
+    ssSetInputPortVectorDimension(S, 0, 4);
+    // ubx_0
+    ssSetInputPortVectorDimension(S, 1, 4);
+    // parameters
+    ssSetInputPortVectorDimension(S, 2, (16+1) * 2);
+    // y_ref_0
+    ssSetInputPortVectorDimension(S, 3, 3);
+    // y_ref
+    ssSetInputPortVectorDimension(S, 4, 45);
+    // y_ref_e
+    ssSetInputPortVectorDimension(S, 5, 2);
+    // lbx
+    ssSetInputPortVectorDimension(S, 6, 30);
+    // ubx
+    ssSetInputPortVectorDimension(S, 7, 30);/* specify dimension information for the OUTPUT ports */
+    ssSetOutputPortVectorDimension(S, 0, 1 );
+    ssSetOutputPortVectorDimension(S, 1, 1 );
+    ssSetOutputPortVectorDimension(S, 2, 1 );
+    ssSetOutputPortVectorDimension(S, 3, 4 ); // state at shooting node 1
+    ssSetOutputPortVectorDimension(S, 4, 1);
+    ssSetOutputPortVectorDimension(S, 5, 1 );
+
+    // specify the direct feedthrough status
+    // should be set to 1 for all inputs used in mdlOutputs
+    ssSetInputPortDirectFeedThrough(S, 0, 1);
+    ssSetInputPortDirectFeedThrough(S, 1, 1);
+    ssSetInputPortDirectFeedThrough(S, 2, 1);
+    ssSetInputPortDirectFeedThrough(S, 3, 1);
+    ssSetInputPortDirectFeedThrough(S, 4, 1);
+    ssSetInputPortDirectFeedThrough(S, 5, 1);
+    ssSetInputPortDirectFeedThrough(S, 6, 1);
+    ssSetInputPortDirectFeedThrough(S, 7, 1);
+
+    // one sample time
+    ssSetNumSampleTimes(S, 1);
+}
+
+
+#if defined(MATLAB_MEX_FILE)
+
+#define MDL_SET_INPUT_PORT_DIMENSION_INFO
+#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
+
+static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
+{
+    if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
+         return;
+}
+
+static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
+{
+    if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
+         return;
+}
+
+#endif /* MATLAB_MEX_FILE */
+
+
+static void mdlInitializeSampleTimes(SimStruct *S)
+{
+    ssSetSampleTime(S, 0, SAMPLINGTIME);
+    ssSetOffsetTime(S, 0, 0.0);
+}
+
+
+static void mdlStart(SimStruct *S)
+{
+    lat_solver_capsule *capsule = lat_acados_create_capsule();
+    lat_acados_create(capsule);
+
+    ssSetUserData(S, (void*)capsule);
+}
+
+
+static void mdlOutputs(SimStruct *S, int_T tid)
+{
+    lat_solver_capsule *capsule = ssGetUserData(S);
+    ocp_nlp_config *nlp_config = lat_acados_get_nlp_config(capsule);
+    ocp_nlp_dims *nlp_dims = lat_acados_get_nlp_dims(capsule);
+    ocp_nlp_in *nlp_in = lat_acados_get_nlp_in(capsule);
+    ocp_nlp_out *nlp_out = lat_acados_get_nlp_out(capsule);
+
+    InputRealPtrsType in_sign;      
+
+    // local buffer
+    real_t buffer[4];
+
+    /* go through inputs */
+    // lbx_0
+    in_sign = ssGetInputPortRealSignalPtrs(S, 0);
+    for (int i = 0; i < 4; i++)
+        buffer[i] = (double)(*in_sign[i]);
+
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", buffer);
+    // ubx_0
+    in_sign = ssGetInputPortRealSignalPtrs(S, 1);
+    for (int i = 0; i < 4; i++)
+        buffer[i] = (double)(*in_sign[i]);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", buffer);
+    // parameters - stage-variant !!!
+    in_sign = ssGetInputPortRealSignalPtrs(S, 2);
+
+    // update value of parameters
+    for (int ii = 0; ii <= 16; ii++)
+    {
+        for (int jj = 0; jj < 2; jj++)
+            buffer[jj] = (double)(*in_sign[ii*2+jj]);
+        lat_acados_update_params(capsule, ii, buffer, 2);
+    }
+
+  
+    // y_ref_0
+    in_sign = ssGetInputPortRealSignalPtrs(S, 3);
+
+    for (int i = 0; i < 3; i++)
+        buffer[i] = (double)(*in_sign[i]);
+
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", (void *) buffer);
+
+  
+    // y_ref - for stages 1 to N-1
+    in_sign = ssGetInputPortRealSignalPtrs(S, 4);
+
+    for (int ii = 1; ii < 16; ii++)
+    {
+        for (int jj = 0; jj < 3; jj++)
+            buffer[jj] = (double)(*in_sign[(ii-1)*3+jj]);
+        ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "yref", (void *) buffer);
+    }
+
+  
+    // y_ref_e
+    in_sign = ssGetInputPortRealSignalPtrs(S, 5);
+
+    for (int i = 0; i < 2; i++)
+        buffer[i] = (double)(*in_sign[i]);
+
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 16, "yref", (void *) buffer);
+    // lbx
+    in_sign = ssGetInputPortRealSignalPtrs(S, 6);
+    for (int ii = 1; ii < 16; ii++)
+    {
+        for (int jj = 0; jj < 2; jj++)
+            buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbx", (void *) buffer);
+    }
+    // ubx
+    in_sign = ssGetInputPortRealSignalPtrs(S, 7);
+    for (int ii = 1; ii < 16; ii++)
+    {
+        for (int jj = 0; jj < 2; jj++)
+            buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubx", (void *) buffer);
+    }
+
+    /* call solver */
+    int rti_phase = 0;
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "rti_phase", &rti_phase);
+    int acados_status = lat_acados_solve(capsule);
+
+
+    /* set outputs */
+    // assign pointers to output signals
+    real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin;
+    int tmp_int;
+    out_u0 = ssGetOutputPortRealSignal(S, 0);
+    ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", (void *) out_u0);
+
+  
+    out_status = ssGetOutputPortRealSignal(S, 1);
+    *out_status = (real_t) acados_status;
+    out_KKT_res = ssGetOutputPortRealSignal(S, 2);
+    *out_KKT_res = (real_t) nlp_out->inf_norm_res;
+    out_x1 = ssGetOutputPortRealSignal(S, 3);
+    ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 1, "x", (void *) out_x1);
+    out_cpu_time = ssGetOutputPortRealSignal(S, 4);
+    // get solution time
+    ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_tot", (void *) out_cpu_time);
+    out_sqp_iter = ssGetOutputPortRealSignal(S, 5);
+    // get sqp iter
+    ocp_nlp_get(nlp_config, capsule->nlp_solver, "sqp_iter", (void *) &tmp_int);
+    *out_sqp_iter = (real_t) tmp_int;
+
+}
+
+static void mdlTerminate(SimStruct *S)
+{
+    lat_solver_capsule *capsule = ssGetUserData(S);
+
+    lat_acados_free(capsule);
+    lat_acados_free_capsule(capsule);
+}
+
+
+#ifdef  MATLAB_MEX_FILE
+#include "simulink.c"
+#else
+#include "cg_sfun.h"
+#endif
diff --git a/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c b/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c
new file mode 100644
index 000000000..c536654fd
--- /dev/null
+++ b/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c
@@ -0,0 +1,128 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+// standard
+#include <stdio.h>
+#include <stdlib.h>
+// acados
+#include "acados/utils/print.h"
+#include "acados/utils/math.h"
+#include "acados_c/sim_interface.h"
+#include "acados_sim_solver_lat.h"
+
+#define NX     LAT_NX
+#define NZ     LAT_NZ
+#define NU     LAT_NU
+#define NP     LAT_NP
+
+
+int main()
+{
+    int status = 0;
+    sim_solver_capsule *capsule = lat_acados_sim_solver_create_capsule();
+    status = lat_acados_sim_create(capsule);
+
+    if (status)
+    {
+        printf("acados_create() returned status %d. Exiting.\n", status);
+        exit(1);
+    }
+
+    sim_config *acados_sim_config = lat_acados_get_sim_config(capsule);
+    sim_in *acados_sim_in = lat_acados_get_sim_in(capsule);
+    sim_out *acados_sim_out = lat_acados_get_sim_out(capsule);
+    void *acados_sim_dims = lat_acados_get_sim_dims(capsule);
+
+    // initial condition
+    double x_current[NX];
+    x_current[0] = 0.0;
+    x_current[1] = 0.0;
+    x_current[2] = 0.0;
+    x_current[3] = 0.0;
+
+  
+    x_current[0] = 0;
+    x_current[1] = 0;
+    x_current[2] = 0;
+    x_current[3] = 0;
+    
+  
+
+
+    // initial value for control input
+    double u0[NU];
+    u0[0] = 0.0;
+    // set parameters
+    double p[NP];
+    p[0] = 0;
+    p[1] = 0;
+
+    lat_acados_sim_update_params(capsule, p, NP);
+  
+
+    int n_sim_steps = 3;
+    // solve ocp in loop
+    for (int ii = 0; ii < n_sim_steps; ii++)
+    {
+        sim_in_set(acados_sim_config, acados_sim_dims,
+            acados_sim_in, "x", x_current);
+        status = lat_acados_sim_solve(capsule);
+
+        if (status != ACADOS_SUCCESS)
+        {
+            printf("acados_solve() failed with status %d.\n", status);
+        }
+
+        sim_out_get(acados_sim_config, acados_sim_dims,
+               acados_sim_out, "x", x_current);
+        
+        printf("\nx_current, %d\n", ii);
+        for (int jj = 0; jj < NX; jj++)
+        {
+            printf("%e\n", x_current[jj]);
+        }
+    }
+
+    printf("\nPerformed %d simulation steps with acados integrator successfully.\n\n", n_sim_steps);
+
+    // free solver
+    status = lat_acados_sim_free(capsule);
+    if (status) {
+        printf("lat_acados_sim_free() returned status %d. \n", status);
+    }
+
+    lat_acados_sim_solver_free_capsule(capsule);
+
+    return status;
+}
diff --git a/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m b/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m
new file mode 100644
index 000000000..2c6679ecb
--- /dev/null
+++ b/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m
@@ -0,0 +1,125 @@
+%
+% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+%
+% This file is part of acados.
+%
+% The 2-Clause BSD License
+%
+% Redistribution and use in source and binary forms, with or without
+% modification, are permitted provided that the following conditions are met:
+%
+% 1. Redistributions of source code must retain the above copyright notice,
+% this list of conditions and the following disclaimer.
+%
+% 2. Redistributions in binary form must reproduce the above copyright notice,
+% this list of conditions and the following disclaimer in the documentation
+% and/or other materials provided with the distribution.
+%
+% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+% POSSIBILITY OF SUCH DAMAGE.;
+%
+
+SOURCES = { ...
+            'lat_model/lat_expl_ode_fun.c', ...
+            'lat_model/lat_expl_vde_forw.c',...
+            'lat_cost/lat_cost_y_0_fun.c',...
+            'lat_cost/lat_cost_y_0_fun_jac_ut_xt.c',...
+            'lat_cost/lat_cost_y_0_hess.c',...
+            'lat_cost/lat_cost_y_fun.c',...
+            'lat_cost/lat_cost_y_fun_jac_ut_xt.c',...
+            'lat_cost/lat_cost_y_hess.c',...
+            'lat_cost/lat_cost_y_e_fun.c',...
+            'lat_cost/lat_cost_y_e_fun_jac_ut_xt.c',...
+            'lat_cost/lat_cost_y_e_hess.c',...
+            'acados_solver_sfunction_lat.c', ...
+            'acados_solver_lat.c'
+          };
+
+INC_PATH = '/data/openpilot/third_party/acados/include/acados/include';
+
+INCS = {['-I', fullfile(INC_PATH, 'blasfeo', 'include')], ...
+        ['-I', fullfile(INC_PATH, 'hpipm', 'include')], ...
+        ['-I', fullfile(INC_PATH, 'acados')], ...
+        ['-I', fullfile(INC_PATH)]};
+
+
+
+CFLAGS = 'CFLAGS=$CFLAGS';
+LDFLAGS = 'LDFLAGS=$LDFLAGS';
+COMPFLAGS = 'COMPFLAGS=$COMPFLAGS';
+COMPDEFINES = 'COMPDEFINES=$COMPDEFINES';
+
+
+
+LIB_PATH = ['-L', fullfile('/data/openpilot/third_party/acados/include/acados/lib')];
+
+LIBS = {'-lacados', '-lhpipm', '-lblasfeo'};
+
+% acados linking libraries and flags
+    
+
+mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ...
+    LIB_PATH, LIBS{:}, SOURCES{:}, ...
+    '-output', 'acados_solver_sfunction_lat' );
+
+fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_lat', '.', ...
+    eval('mexext')] );
+
+
+%% print note on usage of s-function
+fprintf('\n\nNote: Usage of Sfunction is as follows:\n')
+input_note = 'Inputs are:\n';
+i_in = 1;
+input_note = strcat(input_note, num2str(i_in), ') lbx_0 - lower bound on x for stage 0,',...
+                    ' size [4]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',...
+                    ' size [4]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all shooting nodes 0 to N+1,',...
+                    ' size [34]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [3]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') y_ref - concatenated for shooting nodes 1 to N-1,',...
+                    ' size [45]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [2]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') lbx for shooting nodes 1 to N-1, size [30]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') ubx for shooting nodes 1 to N-1, size [30]\n ');
+i_in = i_in + 1;
+
+fprintf(input_note)
+
+disp(' ')
+
+output_note = 'Outputs are:\n';
+i_out = 0;
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') u0, control input at node 0, size [1]\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') acados solver status (0 = SUCCESS)\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') KKT residual\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') x1, state at node 1\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') CPU time\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n ');
+
+fprintf(output_note)
diff --git a/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/acados_ocp_lat.json b/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/acados_ocp_lat.json
new file mode 100644
index 000000000..f7b5bb7f8
--- /dev/null
+++ b/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/acados_ocp_lat.json
@@ -0,0 +1,450 @@
+{
+    "acados_include_path": "/data/openpilot/third_party/acados/include/acados/include",
+    "acados_lib_path": "/data/openpilot/third_party/acados/include/acados/lib",
+    "code_export_directory": "/data/openpilot/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code",
+    "constraints": {
+        "C": [],
+        "C_e": [],
+        "D": [],
+        "constr_type": "BGH",
+        "constr_type_e": "BGH",
+        "idxbu": [],
+        "idxbx": [
+            2,
+            3
+        ],
+        "idxbx_0": [
+            0,
+            1,
+            2,
+            3
+        ],
+        "idxbx_e": [],
+        "idxbxe_0": [
+            0,
+            1,
+            2,
+            3
+        ],
+        "idxsbu": [],
+        "idxsbx": [],
+        "idxsbx_e": [],
+        "idxsg": [],
+        "idxsg_e": [],
+        "idxsh": [],
+        "idxsh_e": [],
+        "idxsphi": [],
+        "idxsphi_e": [],
+        "lbu": [],
+        "lbx": [
+            -1.5707963267948966,
+            -0.8726646259971648
+        ],
+        "lbx_0": [
+            0.0,
+            0.0,
+            0.0,
+            0.0
+        ],
+        "lbx_e": [],
+        "lg": [],
+        "lg_e": [],
+        "lh": [],
+        "lh_e": [],
+        "lphi": [],
+        "lphi_e": [],
+        "lsbu": [],
+        "lsbx": [],
+        "lsbx_e": [],
+        "lsg": [],
+        "lsg_e": [],
+        "lsh": [],
+        "lsh_e": [],
+        "lsphi": [],
+        "lsphi_e": [],
+        "ubu": [],
+        "ubx": [
+            1.5707963267948966,
+            0.8726646259971648
+        ],
+        "ubx_0": [
+            0.0,
+            0.0,
+            0.0,
+            0.0
+        ],
+        "ubx_e": [],
+        "ug": [],
+        "ug_e": [],
+        "uh": [],
+        "uh_e": [],
+        "uphi": [],
+        "uphi_e": [],
+        "usbu": [],
+        "usbx": [],
+        "usbx_e": [],
+        "usg": [],
+        "usg_e": [],
+        "ush": [],
+        "ush_e": [],
+        "usphi": [],
+        "usphi_e": []
+    },
+    "cost": {
+        "Vu": [
+            [
+                0.0
+            ],
+            [
+                0.0
+            ],
+            [
+                0.0
+            ]
+        ],
+        "Vu_0": [
+            [
+                0.0
+            ],
+            [
+                0.0
+            ],
+            [
+                0.0
+            ]
+        ],
+        "Vx": [
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ]
+        ],
+        "Vx_0": [
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ]
+        ],
+        "Vx_e": [
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ]
+        ],
+        "Vz": [],
+        "Vz_0": [],
+        "W": [
+            [
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0
+            ]
+        ],
+        "W_0": [
+            [
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0
+            ]
+        ],
+        "W_e": [
+            [
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0
+            ]
+        ],
+        "Zl": [],
+        "Zl_e": [],
+        "Zu": [],
+        "Zu_e": [],
+        "cost_ext_fun_type": "casadi",
+        "cost_ext_fun_type_0": "casadi",
+        "cost_ext_fun_type_e": "casadi",
+        "cost_type": "NONLINEAR_LS",
+        "cost_type_0": "NONLINEAR_LS",
+        "cost_type_e": "NONLINEAR_LS",
+        "yref": [
+            0.0,
+            0.0,
+            0.0
+        ],
+        "yref_0": [
+            0.0,
+            0.0,
+            0.0
+        ],
+        "yref_e": [
+            0.0,
+            0.0
+        ],
+        "zl": [],
+        "zl_e": [],
+        "zu": [],
+        "zu_e": []
+    },
+    "cython_include_dirs": "/data/data/com.termux/files/usr/lib/python3.8/site-packages/numpy/core/include",
+    "dims": {
+        "N": 16,
+        "nbu": 0,
+        "nbx": 2,
+        "nbx_0": 4,
+        "nbx_e": 0,
+        "nbxe_0": 4,
+        "ng": 0,
+        "ng_e": 0,
+        "nh": 0,
+        "nh_e": 0,
+        "np": 2,
+        "nphi": 0,
+        "nphi_e": 0,
+        "nr": 0,
+        "nr_e": 0,
+        "ns": 0,
+        "ns_e": 0,
+        "nsbu": 0,
+        "nsbx": 0,
+        "nsbx_e": 0,
+        "nsg": 0,
+        "nsg_e": 0,
+        "nsh": 0,
+        "nsh_e": 0,
+        "nsphi": 0,
+        "nsphi_e": 0,
+        "nu": 1,
+        "nx": 4,
+        "ny": 3,
+        "ny_0": 3,
+        "ny_e": 2,
+        "nz": 0
+    },
+    "model": {
+        "dyn_disc_fun": null,
+        "dyn_disc_fun_jac": null,
+        "dyn_disc_fun_jac_hess": null,
+        "dyn_ext_fun_type": "casadi",
+        "dyn_source_discrete": null,
+        "gnsf": {
+            "nontrivial_f_LO": 1,
+            "purely_linear": 0
+        },
+        "name": "lat"
+    },
+    "parameter_values": [
+        0.0,
+        0.0
+    ],
+    "problem_class": "OCP",
+    "simulink_opts": {
+        "inputs": {
+            "cost_W": 0,
+            "cost_W_0": 0,
+            "cost_W_e": 0,
+            "lbu": 1,
+            "lbx": 1,
+            "lbx_0": 1,
+            "lbx_e": 1,
+            "lg": 1,
+            "lh": 1,
+            "parameter_traj": 1,
+            "reset_solver": 0,
+            "u_init": 0,
+            "ubu": 1,
+            "ubx": 1,
+            "ubx_0": 1,
+            "ubx_e": 1,
+            "ug": 1,
+            "uh": 1,
+            "x_init": 0,
+            "y_ref": 1,
+            "y_ref_0": 1,
+            "y_ref_e": 1
+        },
+        "outputs": {
+            "CPU_time": 1,
+            "CPU_time_lin": 0,
+            "CPU_time_qp": 0,
+            "CPU_time_sim": 0,
+            "KKT_residual": 1,
+            "solver_status": 1,
+            "sqp_iter": 1,
+            "u0": 1,
+            "utraj": 0,
+            "x1": 1,
+            "xtraj": 0
+        },
+        "samplingtime": "t0"
+    },
+    "solver_options": {
+        "Tsim": 0.009765625,
+        "alpha_min": 0.05,
+        "alpha_reduction": 0.7,
+        "collocation_type": "GAUSS_LEGENDRE",
+        "eps_sufficient_descent": 0.0001,
+        "exact_hess_constr": 1,
+        "exact_hess_cost": 1,
+        "exact_hess_dyn": 1,
+        "ext_cost_num_hess": 0,
+        "full_step_dual": 0,
+        "globalization": "FIXED_STEP",
+        "globalization_use_SOC": 0,
+        "hessian_approx": "GAUSS_NEWTON",
+        "hpipm_mode": "BALANCE",
+        "initialize_t_slacks": 0,
+        "integrator_type": "ERK",
+        "levenberg_marquardt": 0.0,
+        "line_search_use_sufficient_descent": 0,
+        "model_external_shared_lib_dir": null,
+        "model_external_shared_lib_name": null,
+        "nlp_solver_max_iter": 100,
+        "nlp_solver_step_length": 1.0,
+        "nlp_solver_tol_comp": 1e-06,
+        "nlp_solver_tol_eq": 1e-06,
+        "nlp_solver_tol_ineq": 1e-06,
+        "nlp_solver_tol_stat": 1e-06,
+        "nlp_solver_type": "SQP_RTI",
+        "print_level": 0,
+        "qp_solver": "PARTIAL_CONDENSING_HPIPM",
+        "qp_solver_cond_N": 1,
+        "qp_solver_iter_max": 1,
+        "qp_solver_tol_comp": null,
+        "qp_solver_tol_eq": null,
+        "qp_solver_tol_ineq": null,
+        "qp_solver_tol_stat": null,
+        "qp_solver_warm_start": 0,
+        "regularize_method": null,
+        "sim_method_jac_reuse": [
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0
+        ],
+        "sim_method_newton_iter": 3,
+        "sim_method_num_stages": [
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4
+        ],
+        "sim_method_num_steps": [
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1
+        ],
+        "tf": 2.5,
+        "time_steps": [
+            0.009765625,
+            0.029296875,
+            0.048828125,
+            0.068359375,
+            0.087890625,
+            0.107421875,
+            0.126953125,
+            0.146484375,
+            0.166015625,
+            0.185546875,
+            0.205078125,
+            0.224609375,
+            0.244140625,
+            0.263671875,
+            0.283203125,
+            0.302734375
+        ]
+    }
+}
\ No newline at end of file
diff --git a/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c b/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c
new file mode 100644
index 000000000..d0c541d3b
--- /dev/null
+++ b/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c
@@ -0,0 +1,278 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+// standard
+#include <stdio.h>
+#include <stdlib.h>
+
+// acados
+#include "acados_c/external_function_interface.h"
+#include "acados_c/sim_interface.h"
+#include "acados_c/external_function_interface.h"
+
+#include "acados/sim/sim_common.h"
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/print.h"
+
+
+// example specific
+#include "lat_model/lat_model.h"
+#include "acados_sim_solver_lat.h"
+
+
+// ** solver data **
+
+sim_solver_capsule * lat_acados_sim_solver_create_capsule()
+{
+    void* capsule_mem = malloc(sizeof(sim_solver_capsule));
+    sim_solver_capsule *capsule = (sim_solver_capsule *) capsule_mem;
+
+    return capsule;
+}
+
+
+int lat_acados_sim_solver_free_capsule(sim_solver_capsule * capsule)
+{
+    free(capsule);
+    return 0;
+}
+
+
+int lat_acados_sim_create(sim_solver_capsule * capsule)
+{
+    // initialize
+    const int nx = LAT_NX;
+    const int nu = LAT_NU;
+    const int nz = LAT_NZ;
+    const int np = LAT_NP;
+    bool tmp_bool;
+
+    
+    double Tsim = 0.009765625;
+
+    
+    // explicit ode
+    capsule->sim_forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+    capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+
+    capsule->sim_forw_vde_casadi->casadi_fun = &lat_expl_vde_forw;
+    capsule->sim_forw_vde_casadi->casadi_n_in = &lat_expl_vde_forw_n_in;
+    capsule->sim_forw_vde_casadi->casadi_n_out = &lat_expl_vde_forw_n_out;
+    capsule->sim_forw_vde_casadi->casadi_sparsity_in = &lat_expl_vde_forw_sparsity_in;
+    capsule->sim_forw_vde_casadi->casadi_sparsity_out = &lat_expl_vde_forw_sparsity_out;
+    capsule->sim_forw_vde_casadi->casadi_work = &lat_expl_vde_forw_work;
+    external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np);
+
+    capsule->sim_expl_ode_fun_casadi->casadi_fun = &lat_expl_ode_fun;
+    capsule->sim_expl_ode_fun_casadi->casadi_n_in = &lat_expl_ode_fun_n_in;
+    capsule->sim_expl_ode_fun_casadi->casadi_n_out = &lat_expl_ode_fun_n_out;
+    capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &lat_expl_ode_fun_sparsity_in;
+    capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &lat_expl_ode_fun_sparsity_out;
+    capsule->sim_expl_ode_fun_casadi->casadi_work = &lat_expl_ode_fun_work;
+    external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, np);
+
+    
+
+    // sim plan & config
+    sim_solver_plan_t plan;
+    plan.sim_solver = ERK;
+
+    // create correct config based on plan
+    sim_config * lat_sim_config = sim_config_create(plan);
+    capsule->acados_sim_config = lat_sim_config;
+
+    // sim dims
+    void *lat_sim_dims = sim_dims_create(lat_sim_config);
+    capsule->acados_sim_dims = lat_sim_dims;
+    sim_dims_set(lat_sim_config, lat_sim_dims, "nx", &nx);
+    sim_dims_set(lat_sim_config, lat_sim_dims, "nu", &nu);
+    sim_dims_set(lat_sim_config, lat_sim_dims, "nz", &nz);
+
+
+    // sim opts
+    sim_opts *lat_sim_opts = sim_opts_create(lat_sim_config, lat_sim_dims);
+    capsule->acados_sim_opts = lat_sim_opts;
+    int tmp_int = 3;
+    sim_opts_set(lat_sim_config, lat_sim_opts, "newton_iter", &tmp_int);
+    sim_collocation_type collocation_type = GAUSS_LEGENDRE;
+    sim_opts_set(lat_sim_config, lat_sim_opts, "collocation_type", &collocation_type);
+
+ 
+    tmp_int = 4;
+    sim_opts_set(lat_sim_config, lat_sim_opts, "num_stages", &tmp_int);
+    tmp_int = 1;
+    sim_opts_set(lat_sim_config, lat_sim_opts, "num_steps", &tmp_int);
+    tmp_bool = 0;
+    sim_opts_set(lat_sim_config, lat_sim_opts, "jac_reuse", &tmp_bool);
+
+
+    // sim in / out
+    sim_in *lat_sim_in = sim_in_create(lat_sim_config, lat_sim_dims);
+    capsule->acados_sim_in = lat_sim_in;
+    sim_out *lat_sim_out = sim_out_create(lat_sim_config, lat_sim_dims);
+    capsule->acados_sim_out = lat_sim_out;
+
+    sim_in_set(lat_sim_config, lat_sim_dims,
+               lat_sim_in, "T", &Tsim);
+
+    // model functions
+    lat_sim_config->model_set(lat_sim_in->model,
+                 "expl_vde_for", capsule->sim_forw_vde_casadi);
+    lat_sim_config->model_set(lat_sim_in->model,
+                 "expl_ode_fun", capsule->sim_expl_ode_fun_casadi);
+
+    // sim solver
+    sim_solver *lat_sim_solver = sim_solver_create(lat_sim_config,
+                                               lat_sim_dims, lat_sim_opts);
+    capsule->acados_sim_solver = lat_sim_solver;
+
+
+    /* initialize parameter values */
+    double* p = calloc(np, sizeof(double));
+    
+
+    lat_acados_sim_update_params(capsule, p, np);
+    free(p);
+
+
+    /* initialize input */
+    // x
+    double x0[4];
+    for (int ii = 0; ii < 4; ii++)
+        x0[ii] = 0.0;
+
+    sim_in_set(lat_sim_config, lat_sim_dims,
+               lat_sim_in, "x", x0);
+
+
+    // u
+    double u0[1];
+    for (int ii = 0; ii < 1; ii++)
+        u0[ii] = 0.0;
+
+    sim_in_set(lat_sim_config, lat_sim_dims,
+               lat_sim_in, "u", u0);
+
+    // S_forw
+    double S_forw[20];
+    for (int ii = 0; ii < 20; ii++)
+        S_forw[ii] = 0.0;
+    for (int ii = 0; ii < 4; ii++)
+        S_forw[ii + ii * 4 ] = 1.0;
+
+
+    sim_in_set(lat_sim_config, lat_sim_dims,
+               lat_sim_in, "S_forw", S_forw);
+
+    int status = sim_precompute(lat_sim_solver, lat_sim_in, lat_sim_out);
+
+    return status;
+}
+
+
+int lat_acados_sim_solve(sim_solver_capsule *capsule)
+{
+    // integrate dynamics using acados sim_solver
+    int status = sim_solve(capsule->acados_sim_solver,
+                           capsule->acados_sim_in, capsule->acados_sim_out);
+    if (status != 0)
+        printf("error in lat_acados_sim_solve()! Exiting.\n");
+
+    return status;
+}
+
+
+int lat_acados_sim_free(sim_solver_capsule *capsule)
+{
+    // free memory
+    sim_solver_destroy(capsule->acados_sim_solver);
+    sim_in_destroy(capsule->acados_sim_in);
+    sim_out_destroy(capsule->acados_sim_out);
+    sim_opts_destroy(capsule->acados_sim_opts);
+    sim_dims_destroy(capsule->acados_sim_dims);
+    sim_config_destroy(capsule->acados_sim_config);
+
+    // free external function
+    external_function_param_casadi_free(capsule->sim_forw_vde_casadi);
+    external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi);
+
+    return 0;
+}
+
+
+int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np)
+{
+    int status = 0;
+    int casadi_np = LAT_NP;
+
+    if (casadi_np != np) {
+        printf("lat_acados_sim_update_params: trying to set %i parameters for external functions."
+            " External function has %i parameters. Exiting.\n", np, casadi_np);
+        exit(1);
+    }
+    capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p);
+    capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p);
+
+    return status;
+}
+
+/* getters pointers to C objects*/
+sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_config;
+};
+
+sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_in;
+};
+
+sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_out;
+};
+
+void * lat_acados_get_sim_dims(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_dims;
+};
+
+sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_opts;
+};
+
+sim_solver  * lat_acados_get_sim_solver(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_solver;
+};
+
diff --git a/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h b/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h
new file mode 100644
index 000000000..86b9c84c9
--- /dev/null
+++ b/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h
@@ -0,0 +1,103 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+#ifndef ACADOS_SIM_lat_H_
+#define ACADOS_SIM_lat_H_
+
+#include "acados_c/sim_interface.h"
+#include "acados_c/external_function_interface.h"
+
+#define LAT_NX     4
+#define LAT_NZ     0
+#define LAT_NU     1
+#define LAT_NP     2
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+// ** capsule for solver data **
+typedef struct sim_solver_capsule
+{
+    // acados objects
+    sim_in *acados_sim_in;
+    sim_out *acados_sim_out;
+    sim_solver *acados_sim_solver;
+    sim_opts *acados_sim_opts;
+    sim_config *acados_sim_config;
+    void *acados_sim_dims;
+
+    /* external functions */
+    // ERK
+    external_function_param_casadi * sim_forw_vde_casadi;
+    external_function_param_casadi * sim_expl_ode_fun_casadi;
+    external_function_param_casadi * sim_expl_ode_hess;
+
+    // IRK
+    external_function_param_casadi * sim_impl_dae_fun;
+    external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z;
+    external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z;
+    external_function_param_casadi * sim_impl_dae_hess;
+
+    // GNSF
+    external_function_param_casadi * sim_gnsf_phi_fun;
+    external_function_param_casadi * sim_gnsf_phi_fun_jac_y;
+    external_function_param_casadi * sim_gnsf_phi_jac_y_uhat;
+    external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z;
+    external_function_param_casadi * sim_gnsf_get_matrices_fun;
+
+} sim_solver_capsule;
+
+
+ACADOS_SYMBOL_EXPORT int lat_acados_sim_create(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT int lat_acados_sim_solve(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT int lat_acados_sim_free(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *value, int np);
+
+ACADOS_SYMBOL_EXPORT sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT void * lat_acados_get_sim_dims(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT sim_solver * lat_acados_get_sim_solver(sim_solver_capsule *capsule);
+
+
+ACADOS_SYMBOL_EXPORT sim_solver_capsule * lat_acados_sim_solver_create_capsule(void);
+ACADOS_SYMBOL_EXPORT int lat_acados_sim_solver_free_capsule(sim_solver_capsule *capsule);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // ACADOS_SIM_lat_H_
diff --git a/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c b/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c
new file mode 100644
index 000000000..827b6c7f0
--- /dev/null
+++ b/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c
@@ -0,0 +1,264 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+#define S_FUNCTION_NAME acados_solver_sfunction_lat
+#define S_FUNCTION_LEVEL 2
+
+#define MDL_START
+
+// acados
+// #include "acados/utils/print.h"
+#include "acados_c/sim_interface.h"
+#include "acados_c/external_function_interface.h"
+
+// example specific
+#include "lat_model/lat_model.h"
+#include "acados_solver_lat.h"
+
+#include "simstruc.h"
+
+#define SAMPLINGTIME 0.009765625
+
+static void mdlInitializeSizes (SimStruct *S)
+{
+    // specify the number of continuous and discrete states
+    ssSetNumContStates(S, 0);
+    ssSetNumDiscStates(S, 0);// specify the number of input ports
+    if ( !ssSetNumInputPorts(S, 8) )
+        return;
+
+    // specify the number of output ports
+    if ( !ssSetNumOutputPorts(S, 6) )
+        return;
+
+    // specify dimension information for the input ports
+    // lbx_0
+    ssSetInputPortVectorDimension(S, 0, 4);
+    // ubx_0
+    ssSetInputPortVectorDimension(S, 1, 4);
+    // parameters
+    ssSetInputPortVectorDimension(S, 2, (16+1) * 2);
+    // y_ref_0
+    ssSetInputPortVectorDimension(S, 3, 3);
+    // y_ref
+    ssSetInputPortVectorDimension(S, 4, 45);
+    // y_ref_e
+    ssSetInputPortVectorDimension(S, 5, 2);
+    // lbx
+    ssSetInputPortVectorDimension(S, 6, 30);
+    // ubx
+    ssSetInputPortVectorDimension(S, 7, 30);/* specify dimension information for the OUTPUT ports */
+    ssSetOutputPortVectorDimension(S, 0, 1 );
+    ssSetOutputPortVectorDimension(S, 1, 1 );
+    ssSetOutputPortVectorDimension(S, 2, 1 );
+    ssSetOutputPortVectorDimension(S, 3, 4 ); // state at shooting node 1
+    ssSetOutputPortVectorDimension(S, 4, 1);
+    ssSetOutputPortVectorDimension(S, 5, 1 );
+
+    // specify the direct feedthrough status
+    // should be set to 1 for all inputs used in mdlOutputs
+    ssSetInputPortDirectFeedThrough(S, 0, 1);
+    ssSetInputPortDirectFeedThrough(S, 1, 1);
+    ssSetInputPortDirectFeedThrough(S, 2, 1);
+    ssSetInputPortDirectFeedThrough(S, 3, 1);
+    ssSetInputPortDirectFeedThrough(S, 4, 1);
+    ssSetInputPortDirectFeedThrough(S, 5, 1);
+    ssSetInputPortDirectFeedThrough(S, 6, 1);
+    ssSetInputPortDirectFeedThrough(S, 7, 1);
+
+    // one sample time
+    ssSetNumSampleTimes(S, 1);
+}
+
+
+#if defined(MATLAB_MEX_FILE)
+
+#define MDL_SET_INPUT_PORT_DIMENSION_INFO
+#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
+
+static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
+{
+    if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
+         return;
+}
+
+static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
+{
+    if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
+         return;
+}
+
+#endif /* MATLAB_MEX_FILE */
+
+
+static void mdlInitializeSampleTimes(SimStruct *S)
+{
+    ssSetSampleTime(S, 0, SAMPLINGTIME);
+    ssSetOffsetTime(S, 0, 0.0);
+}
+
+
+static void mdlStart(SimStruct *S)
+{
+    lat_solver_capsule *capsule = lat_acados_create_capsule();
+    lat_acados_create(capsule);
+
+    ssSetUserData(S, (void*)capsule);
+}
+
+
+static void mdlOutputs(SimStruct *S, int_T tid)
+{
+    lat_solver_capsule *capsule = ssGetUserData(S);
+    ocp_nlp_config *nlp_config = lat_acados_get_nlp_config(capsule);
+    ocp_nlp_dims *nlp_dims = lat_acados_get_nlp_dims(capsule);
+    ocp_nlp_in *nlp_in = lat_acados_get_nlp_in(capsule);
+    ocp_nlp_out *nlp_out = lat_acados_get_nlp_out(capsule);
+
+    InputRealPtrsType in_sign;      
+
+    // local buffer
+    real_t buffer[4];
+
+    /* go through inputs */
+    // lbx_0
+    in_sign = ssGetInputPortRealSignalPtrs(S, 0);
+    for (int i = 0; i < 4; i++)
+        buffer[i] = (double)(*in_sign[i]);
+
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", buffer);
+    // ubx_0
+    in_sign = ssGetInputPortRealSignalPtrs(S, 1);
+    for (int i = 0; i < 4; i++)
+        buffer[i] = (double)(*in_sign[i]);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", buffer);
+    // parameters - stage-variant !!!
+    in_sign = ssGetInputPortRealSignalPtrs(S, 2);
+
+    // update value of parameters
+    for (int ii = 0; ii <= 16; ii++)
+    {
+        for (int jj = 0; jj < 2; jj++)
+            buffer[jj] = (double)(*in_sign[ii*2+jj]);
+        lat_acados_update_params(capsule, ii, buffer, 2);
+    }
+
+  
+    // y_ref_0
+    in_sign = ssGetInputPortRealSignalPtrs(S, 3);
+
+    for (int i = 0; i < 3; i++)
+        buffer[i] = (double)(*in_sign[i]);
+
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", (void *) buffer);
+
+  
+    // y_ref - for stages 1 to N-1
+    in_sign = ssGetInputPortRealSignalPtrs(S, 4);
+
+    for (int ii = 1; ii < 16; ii++)
+    {
+        for (int jj = 0; jj < 3; jj++)
+            buffer[jj] = (double)(*in_sign[(ii-1)*3+jj]);
+        ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "yref", (void *) buffer);
+    }
+
+  
+    // y_ref_e
+    in_sign = ssGetInputPortRealSignalPtrs(S, 5);
+
+    for (int i = 0; i < 2; i++)
+        buffer[i] = (double)(*in_sign[i]);
+
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 16, "yref", (void *) buffer);
+    // lbx
+    in_sign = ssGetInputPortRealSignalPtrs(S, 6);
+    for (int ii = 1; ii < 16; ii++)
+    {
+        for (int jj = 0; jj < 2; jj++)
+            buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbx", (void *) buffer);
+    }
+    // ubx
+    in_sign = ssGetInputPortRealSignalPtrs(S, 7);
+    for (int ii = 1; ii < 16; ii++)
+    {
+        for (int jj = 0; jj < 2; jj++)
+            buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubx", (void *) buffer);
+    }
+
+    /* call solver */
+    int rti_phase = 0;
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "rti_phase", &rti_phase);
+    int acados_status = lat_acados_solve(capsule);
+
+
+    /* set outputs */
+    // assign pointers to output signals
+    real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin;
+    int tmp_int;
+    out_u0 = ssGetOutputPortRealSignal(S, 0);
+    ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", (void *) out_u0);
+
+  
+    out_status = ssGetOutputPortRealSignal(S, 1);
+    *out_status = (real_t) acados_status;
+    out_KKT_res = ssGetOutputPortRealSignal(S, 2);
+    *out_KKT_res = (real_t) nlp_out->inf_norm_res;
+    out_x1 = ssGetOutputPortRealSignal(S, 3);
+    ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 1, "x", (void *) out_x1);
+    out_cpu_time = ssGetOutputPortRealSignal(S, 4);
+    // get solution time
+    ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_tot", (void *) out_cpu_time);
+    out_sqp_iter = ssGetOutputPortRealSignal(S, 5);
+    // get sqp iter
+    ocp_nlp_get(nlp_config, capsule->nlp_solver, "sqp_iter", (void *) &tmp_int);
+    *out_sqp_iter = (real_t) tmp_int;
+
+}
+
+static void mdlTerminate(SimStruct *S)
+{
+    lat_solver_capsule *capsule = ssGetUserData(S);
+
+    lat_acados_free(capsule);
+    lat_acados_free_capsule(capsule);
+}
+
+
+#ifdef  MATLAB_MEX_FILE
+#include "simulink.c"
+#else
+#include "cg_sfun.h"
+#endif
diff --git a/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c b/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c
new file mode 100644
index 000000000..c536654fd
--- /dev/null
+++ b/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c
@@ -0,0 +1,128 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+// standard
+#include <stdio.h>
+#include <stdlib.h>
+// acados
+#include "acados/utils/print.h"
+#include "acados/utils/math.h"
+#include "acados_c/sim_interface.h"
+#include "acados_sim_solver_lat.h"
+
+#define NX     LAT_NX
+#define NZ     LAT_NZ
+#define NU     LAT_NU
+#define NP     LAT_NP
+
+
+int main()
+{
+    int status = 0;
+    sim_solver_capsule *capsule = lat_acados_sim_solver_create_capsule();
+    status = lat_acados_sim_create(capsule);
+
+    if (status)
+    {
+        printf("acados_create() returned status %d. Exiting.\n", status);
+        exit(1);
+    }
+
+    sim_config *acados_sim_config = lat_acados_get_sim_config(capsule);
+    sim_in *acados_sim_in = lat_acados_get_sim_in(capsule);
+    sim_out *acados_sim_out = lat_acados_get_sim_out(capsule);
+    void *acados_sim_dims = lat_acados_get_sim_dims(capsule);
+
+    // initial condition
+    double x_current[NX];
+    x_current[0] = 0.0;
+    x_current[1] = 0.0;
+    x_current[2] = 0.0;
+    x_current[3] = 0.0;
+
+  
+    x_current[0] = 0;
+    x_current[1] = 0;
+    x_current[2] = 0;
+    x_current[3] = 0;
+    
+  
+
+
+    // initial value for control input
+    double u0[NU];
+    u0[0] = 0.0;
+    // set parameters
+    double p[NP];
+    p[0] = 0;
+    p[1] = 0;
+
+    lat_acados_sim_update_params(capsule, p, NP);
+  
+
+    int n_sim_steps = 3;
+    // solve ocp in loop
+    for (int ii = 0; ii < n_sim_steps; ii++)
+    {
+        sim_in_set(acados_sim_config, acados_sim_dims,
+            acados_sim_in, "x", x_current);
+        status = lat_acados_sim_solve(capsule);
+
+        if (status != ACADOS_SUCCESS)
+        {
+            printf("acados_solve() failed with status %d.\n", status);
+        }
+
+        sim_out_get(acados_sim_config, acados_sim_dims,
+               acados_sim_out, "x", x_current);
+        
+        printf("\nx_current, %d\n", ii);
+        for (int jj = 0; jj < NX; jj++)
+        {
+            printf("%e\n", x_current[jj]);
+        }
+    }
+
+    printf("\nPerformed %d simulation steps with acados integrator successfully.\n\n", n_sim_steps);
+
+    // free solver
+    status = lat_acados_sim_free(capsule);
+    if (status) {
+        printf("lat_acados_sim_free() returned status %d. \n", status);
+    }
+
+    lat_acados_sim_solver_free_capsule(capsule);
+
+    return status;
+}
diff --git a/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m b/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m
new file mode 100644
index 000000000..2c6679ecb
--- /dev/null
+++ b/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m
@@ -0,0 +1,125 @@
+%
+% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+%
+% This file is part of acados.
+%
+% The 2-Clause BSD License
+%
+% Redistribution and use in source and binary forms, with or without
+% modification, are permitted provided that the following conditions are met:
+%
+% 1. Redistributions of source code must retain the above copyright notice,
+% this list of conditions and the following disclaimer.
+%
+% 2. Redistributions in binary form must reproduce the above copyright notice,
+% this list of conditions and the following disclaimer in the documentation
+% and/or other materials provided with the distribution.
+%
+% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+% POSSIBILITY OF SUCH DAMAGE.;
+%
+
+SOURCES = { ...
+            'lat_model/lat_expl_ode_fun.c', ...
+            'lat_model/lat_expl_vde_forw.c',...
+            'lat_cost/lat_cost_y_0_fun.c',...
+            'lat_cost/lat_cost_y_0_fun_jac_ut_xt.c',...
+            'lat_cost/lat_cost_y_0_hess.c',...
+            'lat_cost/lat_cost_y_fun.c',...
+            'lat_cost/lat_cost_y_fun_jac_ut_xt.c',...
+            'lat_cost/lat_cost_y_hess.c',...
+            'lat_cost/lat_cost_y_e_fun.c',...
+            'lat_cost/lat_cost_y_e_fun_jac_ut_xt.c',...
+            'lat_cost/lat_cost_y_e_hess.c',...
+            'acados_solver_sfunction_lat.c', ...
+            'acados_solver_lat.c'
+          };
+
+INC_PATH = '/data/openpilot/third_party/acados/include/acados/include';
+
+INCS = {['-I', fullfile(INC_PATH, 'blasfeo', 'include')], ...
+        ['-I', fullfile(INC_PATH, 'hpipm', 'include')], ...
+        ['-I', fullfile(INC_PATH, 'acados')], ...
+        ['-I', fullfile(INC_PATH)]};
+
+
+
+CFLAGS = 'CFLAGS=$CFLAGS';
+LDFLAGS = 'LDFLAGS=$LDFLAGS';
+COMPFLAGS = 'COMPFLAGS=$COMPFLAGS';
+COMPDEFINES = 'COMPDEFINES=$COMPDEFINES';
+
+
+
+LIB_PATH = ['-L', fullfile('/data/openpilot/third_party/acados/include/acados/lib')];
+
+LIBS = {'-lacados', '-lhpipm', '-lblasfeo'};
+
+% acados linking libraries and flags
+    
+
+mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ...
+    LIB_PATH, LIBS{:}, SOURCES{:}, ...
+    '-output', 'acados_solver_sfunction_lat' );
+
+fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_lat', '.', ...
+    eval('mexext')] );
+
+
+%% print note on usage of s-function
+fprintf('\n\nNote: Usage of Sfunction is as follows:\n')
+input_note = 'Inputs are:\n';
+i_in = 1;
+input_note = strcat(input_note, num2str(i_in), ') lbx_0 - lower bound on x for stage 0,',...
+                    ' size [4]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',...
+                    ' size [4]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all shooting nodes 0 to N+1,',...
+                    ' size [34]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [3]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') y_ref - concatenated for shooting nodes 1 to N-1,',...
+                    ' size [45]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [2]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') lbx for shooting nodes 1 to N-1, size [30]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') ubx for shooting nodes 1 to N-1, size [30]\n ');
+i_in = i_in + 1;
+
+fprintf(input_note)
+
+disp(' ')
+
+output_note = 'Outputs are:\n';
+i_out = 0;
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') u0, control input at node 0, size [1]\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') acados solver status (0 = SUCCESS)\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') KKT residual\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') x1, state at node 1\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') CPU time\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n ');
+
+fprintf(output_note)
diff --git a/selfdrive/locationd/locationd b/selfdrive/locationd/locationd
index 7093f054b..dbf34ab29 100755
Binary files a/selfdrive/locationd/locationd and b/selfdrive/locationd/locationd differ
diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp
index 1b9cab977..df8e9853c 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_1840749225442711100) {
-   out_1840749225442711100[0] = delta_x[0] + nom_x[0];
-   out_1840749225442711100[1] = delta_x[1] + nom_x[1];
-   out_1840749225442711100[2] = delta_x[2] + nom_x[2];
-   out_1840749225442711100[3] = delta_x[3] + nom_x[3];
-   out_1840749225442711100[4] = delta_x[4] + nom_x[4];
-   out_1840749225442711100[5] = delta_x[5] + nom_x[5];
-   out_1840749225442711100[6] = delta_x[6] + nom_x[6];
-   out_1840749225442711100[7] = delta_x[7] + nom_x[7];
-   out_1840749225442711100[8] = delta_x[8] + nom_x[8];
+void err_fun(double *nom_x, double *delta_x, double *out_5232708683747839446) {
+   out_5232708683747839446[0] = delta_x[0] + nom_x[0];
+   out_5232708683747839446[1] = delta_x[1] + nom_x[1];
+   out_5232708683747839446[2] = delta_x[2] + nom_x[2];
+   out_5232708683747839446[3] = delta_x[3] + nom_x[3];
+   out_5232708683747839446[4] = delta_x[4] + nom_x[4];
+   out_5232708683747839446[5] = delta_x[5] + nom_x[5];
+   out_5232708683747839446[6] = delta_x[6] + nom_x[6];
+   out_5232708683747839446[7] = delta_x[7] + nom_x[7];
+   out_5232708683747839446[8] = delta_x[8] + nom_x[8];
 }
-void inv_err_fun(double *nom_x, double *true_x, double *out_7887258947850824197) {
-   out_7887258947850824197[0] = -nom_x[0] + true_x[0];
-   out_7887258947850824197[1] = -nom_x[1] + true_x[1];
-   out_7887258947850824197[2] = -nom_x[2] + true_x[2];
-   out_7887258947850824197[3] = -nom_x[3] + true_x[3];
-   out_7887258947850824197[4] = -nom_x[4] + true_x[4];
-   out_7887258947850824197[5] = -nom_x[5] + true_x[5];
-   out_7887258947850824197[6] = -nom_x[6] + true_x[6];
-   out_7887258947850824197[7] = -nom_x[7] + true_x[7];
-   out_7887258947850824197[8] = -nom_x[8] + true_x[8];
+void inv_err_fun(double *nom_x, double *true_x, double *out_6273110318621425138) {
+   out_6273110318621425138[0] = -nom_x[0] + true_x[0];
+   out_6273110318621425138[1] = -nom_x[1] + true_x[1];
+   out_6273110318621425138[2] = -nom_x[2] + true_x[2];
+   out_6273110318621425138[3] = -nom_x[3] + true_x[3];
+   out_6273110318621425138[4] = -nom_x[4] + true_x[4];
+   out_6273110318621425138[5] = -nom_x[5] + true_x[5];
+   out_6273110318621425138[6] = -nom_x[6] + true_x[6];
+   out_6273110318621425138[7] = -nom_x[7] + true_x[7];
+   out_6273110318621425138[8] = -nom_x[8] + true_x[8];
 }
-void H_mod_fun(double *state, double *out_5849575757077882696) {
-   out_5849575757077882696[0] = 1.0;
-   out_5849575757077882696[1] = 0;
-   out_5849575757077882696[2] = 0;
-   out_5849575757077882696[3] = 0;
-   out_5849575757077882696[4] = 0;
-   out_5849575757077882696[5] = 0;
-   out_5849575757077882696[6] = 0;
-   out_5849575757077882696[7] = 0;
-   out_5849575757077882696[8] = 0;
-   out_5849575757077882696[9] = 0;
-   out_5849575757077882696[10] = 1.0;
-   out_5849575757077882696[11] = 0;
-   out_5849575757077882696[12] = 0;
-   out_5849575757077882696[13] = 0;
-   out_5849575757077882696[14] = 0;
-   out_5849575757077882696[15] = 0;
-   out_5849575757077882696[16] = 0;
-   out_5849575757077882696[17] = 0;
-   out_5849575757077882696[18] = 0;
-   out_5849575757077882696[19] = 0;
-   out_5849575757077882696[20] = 1.0;
-   out_5849575757077882696[21] = 0;
-   out_5849575757077882696[22] = 0;
-   out_5849575757077882696[23] = 0;
-   out_5849575757077882696[24] = 0;
-   out_5849575757077882696[25] = 0;
-   out_5849575757077882696[26] = 0;
-   out_5849575757077882696[27] = 0;
-   out_5849575757077882696[28] = 0;
-   out_5849575757077882696[29] = 0;
-   out_5849575757077882696[30] = 1.0;
-   out_5849575757077882696[31] = 0;
-   out_5849575757077882696[32] = 0;
-   out_5849575757077882696[33] = 0;
-   out_5849575757077882696[34] = 0;
-   out_5849575757077882696[35] = 0;
-   out_5849575757077882696[36] = 0;
-   out_5849575757077882696[37] = 0;
-   out_5849575757077882696[38] = 0;
-   out_5849575757077882696[39] = 0;
-   out_5849575757077882696[40] = 1.0;
-   out_5849575757077882696[41] = 0;
-   out_5849575757077882696[42] = 0;
-   out_5849575757077882696[43] = 0;
-   out_5849575757077882696[44] = 0;
-   out_5849575757077882696[45] = 0;
-   out_5849575757077882696[46] = 0;
-   out_5849575757077882696[47] = 0;
-   out_5849575757077882696[48] = 0;
-   out_5849575757077882696[49] = 0;
-   out_5849575757077882696[50] = 1.0;
-   out_5849575757077882696[51] = 0;
-   out_5849575757077882696[52] = 0;
-   out_5849575757077882696[53] = 0;
-   out_5849575757077882696[54] = 0;
-   out_5849575757077882696[55] = 0;
-   out_5849575757077882696[56] = 0;
-   out_5849575757077882696[57] = 0;
-   out_5849575757077882696[58] = 0;
-   out_5849575757077882696[59] = 0;
-   out_5849575757077882696[60] = 1.0;
-   out_5849575757077882696[61] = 0;
-   out_5849575757077882696[62] = 0;
-   out_5849575757077882696[63] = 0;
-   out_5849575757077882696[64] = 0;
-   out_5849575757077882696[65] = 0;
-   out_5849575757077882696[66] = 0;
-   out_5849575757077882696[67] = 0;
-   out_5849575757077882696[68] = 0;
-   out_5849575757077882696[69] = 0;
-   out_5849575757077882696[70] = 1.0;
-   out_5849575757077882696[71] = 0;
-   out_5849575757077882696[72] = 0;
-   out_5849575757077882696[73] = 0;
-   out_5849575757077882696[74] = 0;
-   out_5849575757077882696[75] = 0;
-   out_5849575757077882696[76] = 0;
-   out_5849575757077882696[77] = 0;
-   out_5849575757077882696[78] = 0;
-   out_5849575757077882696[79] = 0;
-   out_5849575757077882696[80] = 1.0;
+void H_mod_fun(double *state, double *out_376479380516898605) {
+   out_376479380516898605[0] = 1.0;
+   out_376479380516898605[1] = 0;
+   out_376479380516898605[2] = 0;
+   out_376479380516898605[3] = 0;
+   out_376479380516898605[4] = 0;
+   out_376479380516898605[5] = 0;
+   out_376479380516898605[6] = 0;
+   out_376479380516898605[7] = 0;
+   out_376479380516898605[8] = 0;
+   out_376479380516898605[9] = 0;
+   out_376479380516898605[10] = 1.0;
+   out_376479380516898605[11] = 0;
+   out_376479380516898605[12] = 0;
+   out_376479380516898605[13] = 0;
+   out_376479380516898605[14] = 0;
+   out_376479380516898605[15] = 0;
+   out_376479380516898605[16] = 0;
+   out_376479380516898605[17] = 0;
+   out_376479380516898605[18] = 0;
+   out_376479380516898605[19] = 0;
+   out_376479380516898605[20] = 1.0;
+   out_376479380516898605[21] = 0;
+   out_376479380516898605[22] = 0;
+   out_376479380516898605[23] = 0;
+   out_376479380516898605[24] = 0;
+   out_376479380516898605[25] = 0;
+   out_376479380516898605[26] = 0;
+   out_376479380516898605[27] = 0;
+   out_376479380516898605[28] = 0;
+   out_376479380516898605[29] = 0;
+   out_376479380516898605[30] = 1.0;
+   out_376479380516898605[31] = 0;
+   out_376479380516898605[32] = 0;
+   out_376479380516898605[33] = 0;
+   out_376479380516898605[34] = 0;
+   out_376479380516898605[35] = 0;
+   out_376479380516898605[36] = 0;
+   out_376479380516898605[37] = 0;
+   out_376479380516898605[38] = 0;
+   out_376479380516898605[39] = 0;
+   out_376479380516898605[40] = 1.0;
+   out_376479380516898605[41] = 0;
+   out_376479380516898605[42] = 0;
+   out_376479380516898605[43] = 0;
+   out_376479380516898605[44] = 0;
+   out_376479380516898605[45] = 0;
+   out_376479380516898605[46] = 0;
+   out_376479380516898605[47] = 0;
+   out_376479380516898605[48] = 0;
+   out_376479380516898605[49] = 0;
+   out_376479380516898605[50] = 1.0;
+   out_376479380516898605[51] = 0;
+   out_376479380516898605[52] = 0;
+   out_376479380516898605[53] = 0;
+   out_376479380516898605[54] = 0;
+   out_376479380516898605[55] = 0;
+   out_376479380516898605[56] = 0;
+   out_376479380516898605[57] = 0;
+   out_376479380516898605[58] = 0;
+   out_376479380516898605[59] = 0;
+   out_376479380516898605[60] = 1.0;
+   out_376479380516898605[61] = 0;
+   out_376479380516898605[62] = 0;
+   out_376479380516898605[63] = 0;
+   out_376479380516898605[64] = 0;
+   out_376479380516898605[65] = 0;
+   out_376479380516898605[66] = 0;
+   out_376479380516898605[67] = 0;
+   out_376479380516898605[68] = 0;
+   out_376479380516898605[69] = 0;
+   out_376479380516898605[70] = 1.0;
+   out_376479380516898605[71] = 0;
+   out_376479380516898605[72] = 0;
+   out_376479380516898605[73] = 0;
+   out_376479380516898605[74] = 0;
+   out_376479380516898605[75] = 0;
+   out_376479380516898605[76] = 0;
+   out_376479380516898605[77] = 0;
+   out_376479380516898605[78] = 0;
+   out_376479380516898605[79] = 0;
+   out_376479380516898605[80] = 1.0;
 }
-void f_fun(double *state, double dt, double *out_1306455665935687231) {
-   out_1306455665935687231[0] = state[0];
-   out_1306455665935687231[1] = state[1];
-   out_1306455665935687231[2] = state[2];
-   out_1306455665935687231[3] = state[3];
-   out_1306455665935687231[4] = state[4];
-   out_1306455665935687231[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8000000000000007*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_1306455665935687231[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_1306455665935687231[7] = state[7];
-   out_1306455665935687231[8] = state[8];
+void f_fun(double *state, double dt, double *out_7586787697879010325) {
+   out_7586787697879010325[0] = state[0];
+   out_7586787697879010325[1] = state[1];
+   out_7586787697879010325[2] = state[2];
+   out_7586787697879010325[3] = state[3];
+   out_7586787697879010325[4] = state[4];
+   out_7586787697879010325[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8000000000000007*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_7586787697879010325[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_7586787697879010325[7] = state[7];
+   out_7586787697879010325[8] = state[8];
 }
-void F_fun(double *state, double dt, double *out_1015750461961158607) {
-   out_1015750461961158607[0] = 1;
-   out_1015750461961158607[1] = 0;
-   out_1015750461961158607[2] = 0;
-   out_1015750461961158607[3] = 0;
-   out_1015750461961158607[4] = 0;
-   out_1015750461961158607[5] = 0;
-   out_1015750461961158607[6] = 0;
-   out_1015750461961158607[7] = 0;
-   out_1015750461961158607[8] = 0;
-   out_1015750461961158607[9] = 0;
-   out_1015750461961158607[10] = 1;
-   out_1015750461961158607[11] = 0;
-   out_1015750461961158607[12] = 0;
-   out_1015750461961158607[13] = 0;
-   out_1015750461961158607[14] = 0;
-   out_1015750461961158607[15] = 0;
-   out_1015750461961158607[16] = 0;
-   out_1015750461961158607[17] = 0;
-   out_1015750461961158607[18] = 0;
-   out_1015750461961158607[19] = 0;
-   out_1015750461961158607[20] = 1;
-   out_1015750461961158607[21] = 0;
-   out_1015750461961158607[22] = 0;
-   out_1015750461961158607[23] = 0;
-   out_1015750461961158607[24] = 0;
-   out_1015750461961158607[25] = 0;
-   out_1015750461961158607[26] = 0;
-   out_1015750461961158607[27] = 0;
-   out_1015750461961158607[28] = 0;
-   out_1015750461961158607[29] = 0;
-   out_1015750461961158607[30] = 1;
-   out_1015750461961158607[31] = 0;
-   out_1015750461961158607[32] = 0;
-   out_1015750461961158607[33] = 0;
-   out_1015750461961158607[34] = 0;
-   out_1015750461961158607[35] = 0;
-   out_1015750461961158607[36] = 0;
-   out_1015750461961158607[37] = 0;
-   out_1015750461961158607[38] = 0;
-   out_1015750461961158607[39] = 0;
-   out_1015750461961158607[40] = 1;
-   out_1015750461961158607[41] = 0;
-   out_1015750461961158607[42] = 0;
-   out_1015750461961158607[43] = 0;
-   out_1015750461961158607[44] = 0;
-   out_1015750461961158607[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_1015750461961158607[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
-   out_1015750461961158607[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
-   out_1015750461961158607[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
-   out_1015750461961158607[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_1015750461961158607[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
-   out_1015750461961158607[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
-   out_1015750461961158607[52] = dt*stiffness_front*state[0]/(mass*state[1]);
-   out_1015750461961158607[53] = -9.8000000000000007*dt;
-   out_1015750461961158607[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_1015750461961158607[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
-   out_1015750461961158607[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_1015750461961158607[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_1015750461961158607[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_1015750461961158607[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
-   out_1015750461961158607[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_1015750461961158607[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_1015750461961158607[62] = 0;
-   out_1015750461961158607[63] = 0;
-   out_1015750461961158607[64] = 0;
-   out_1015750461961158607[65] = 0;
-   out_1015750461961158607[66] = 0;
-   out_1015750461961158607[67] = 0;
-   out_1015750461961158607[68] = 0;
-   out_1015750461961158607[69] = 0;
-   out_1015750461961158607[70] = 1;
-   out_1015750461961158607[71] = 0;
-   out_1015750461961158607[72] = 0;
-   out_1015750461961158607[73] = 0;
-   out_1015750461961158607[74] = 0;
-   out_1015750461961158607[75] = 0;
-   out_1015750461961158607[76] = 0;
-   out_1015750461961158607[77] = 0;
-   out_1015750461961158607[78] = 0;
-   out_1015750461961158607[79] = 0;
-   out_1015750461961158607[80] = 1;
+void F_fun(double *state, double dt, double *out_472790716300753243) {
+   out_472790716300753243[0] = 1;
+   out_472790716300753243[1] = 0;
+   out_472790716300753243[2] = 0;
+   out_472790716300753243[3] = 0;
+   out_472790716300753243[4] = 0;
+   out_472790716300753243[5] = 0;
+   out_472790716300753243[6] = 0;
+   out_472790716300753243[7] = 0;
+   out_472790716300753243[8] = 0;
+   out_472790716300753243[9] = 0;
+   out_472790716300753243[10] = 1;
+   out_472790716300753243[11] = 0;
+   out_472790716300753243[12] = 0;
+   out_472790716300753243[13] = 0;
+   out_472790716300753243[14] = 0;
+   out_472790716300753243[15] = 0;
+   out_472790716300753243[16] = 0;
+   out_472790716300753243[17] = 0;
+   out_472790716300753243[18] = 0;
+   out_472790716300753243[19] = 0;
+   out_472790716300753243[20] = 1;
+   out_472790716300753243[21] = 0;
+   out_472790716300753243[22] = 0;
+   out_472790716300753243[23] = 0;
+   out_472790716300753243[24] = 0;
+   out_472790716300753243[25] = 0;
+   out_472790716300753243[26] = 0;
+   out_472790716300753243[27] = 0;
+   out_472790716300753243[28] = 0;
+   out_472790716300753243[29] = 0;
+   out_472790716300753243[30] = 1;
+   out_472790716300753243[31] = 0;
+   out_472790716300753243[32] = 0;
+   out_472790716300753243[33] = 0;
+   out_472790716300753243[34] = 0;
+   out_472790716300753243[35] = 0;
+   out_472790716300753243[36] = 0;
+   out_472790716300753243[37] = 0;
+   out_472790716300753243[38] = 0;
+   out_472790716300753243[39] = 0;
+   out_472790716300753243[40] = 1;
+   out_472790716300753243[41] = 0;
+   out_472790716300753243[42] = 0;
+   out_472790716300753243[43] = 0;
+   out_472790716300753243[44] = 0;
+   out_472790716300753243[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_472790716300753243[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
+   out_472790716300753243[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
+   out_472790716300753243[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
+   out_472790716300753243[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_472790716300753243[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
+   out_472790716300753243[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
+   out_472790716300753243[52] = dt*stiffness_front*state[0]/(mass*state[1]);
+   out_472790716300753243[53] = -9.8000000000000007*dt;
+   out_472790716300753243[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_472790716300753243[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
+   out_472790716300753243[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_472790716300753243[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_472790716300753243[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_472790716300753243[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
+   out_472790716300753243[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_472790716300753243[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_472790716300753243[62] = 0;
+   out_472790716300753243[63] = 0;
+   out_472790716300753243[64] = 0;
+   out_472790716300753243[65] = 0;
+   out_472790716300753243[66] = 0;
+   out_472790716300753243[67] = 0;
+   out_472790716300753243[68] = 0;
+   out_472790716300753243[69] = 0;
+   out_472790716300753243[70] = 1;
+   out_472790716300753243[71] = 0;
+   out_472790716300753243[72] = 0;
+   out_472790716300753243[73] = 0;
+   out_472790716300753243[74] = 0;
+   out_472790716300753243[75] = 0;
+   out_472790716300753243[76] = 0;
+   out_472790716300753243[77] = 0;
+   out_472790716300753243[78] = 0;
+   out_472790716300753243[79] = 0;
+   out_472790716300753243[80] = 1;
 }
-void h_25(double *state, double *unused, double *out_579481060641732431) {
-   out_579481060641732431[0] = state[6];
+void h_25(double *state, double *unused, double *out_6846635756302794664) {
+   out_6846635756302794664[0] = state[6];
 }
-void H_25(double *state, double *unused, double *out_8838814511200069080) {
-   out_8838814511200069080[0] = 0;
-   out_8838814511200069080[1] = 0;
-   out_8838814511200069080[2] = 0;
-   out_8838814511200069080[3] = 0;
-   out_8838814511200069080[4] = 0;
-   out_8838814511200069080[5] = 0;
-   out_8838814511200069080[6] = 1;
-   out_8838814511200069080[7] = 0;
-   out_8838814511200069080[8] = 0;
+void H_25(double *state, double *unused, double *out_1681825998279428652) {
+   out_1681825998279428652[0] = 0;
+   out_1681825998279428652[1] = 0;
+   out_1681825998279428652[2] = 0;
+   out_1681825998279428652[3] = 0;
+   out_1681825998279428652[4] = 0;
+   out_1681825998279428652[5] = 0;
+   out_1681825998279428652[6] = 1;
+   out_1681825998279428652[7] = 0;
+   out_1681825998279428652[8] = 0;
 }
-void h_24(double *state, double *unused, double *out_8628826922044058076) {
-   out_8628826922044058076[0] = state[4];
-   out_8628826922044058076[1] = state[5];
+void h_24(double *state, double *unused, double *out_7639061379755997248) {
+   out_7639061379755997248[0] = state[4];
+   out_7639061379755997248[1] = state[5];
 }
-void H_24(double *state, double *unused, double *out_7435279963503982970) {
-   out_7435279963503982970[0] = 0;
-   out_7435279963503982970[1] = 0;
-   out_7435279963503982970[2] = 0;
-   out_7435279963503982970[3] = 0;
-   out_7435279963503982970[4] = 1;
-   out_7435279963503982970[5] = 0;
-   out_7435279963503982970[6] = 0;
-   out_7435279963503982970[7] = 0;
-   out_7435279963503982970[8] = 0;
-   out_7435279963503982970[9] = 0;
-   out_7435279963503982970[10] = 0;
-   out_7435279963503982970[11] = 0;
-   out_7435279963503982970[12] = 0;
-   out_7435279963503982970[13] = 0;
-   out_7435279963503982970[14] = 1;
-   out_7435279963503982970[15] = 0;
-   out_7435279963503982970[16] = 0;
-   out_7435279963503982970[17] = 0;
+void H_24(double *state, double *unused, double *out_4893745808312089449) {
+   out_4893745808312089449[0] = 0;
+   out_4893745808312089449[1] = 0;
+   out_4893745808312089449[2] = 0;
+   out_4893745808312089449[3] = 0;
+   out_4893745808312089449[4] = 1;
+   out_4893745808312089449[5] = 0;
+   out_4893745808312089449[6] = 0;
+   out_4893745808312089449[7] = 0;
+   out_4893745808312089449[8] = 0;
+   out_4893745808312089449[9] = 0;
+   out_4893745808312089449[10] = 0;
+   out_4893745808312089449[11] = 0;
+   out_4893745808312089449[12] = 0;
+   out_4893745808312089449[13] = 0;
+   out_4893745808312089449[14] = 1;
+   out_4893745808312089449[15] = 0;
+   out_4893745808312089449[16] = 0;
+   out_4893745808312089449[17] = 0;
 }
-void h_30(double *state, double *unused, double *out_8060396416076809453) {
-   out_8060396416076809453[0] = state[4];
+void h_30(double *state, double *unused, double *out_2046273354615198377) {
+   out_2046273354615198377[0] = state[4];
 }
-void H_30(double *state, double *unused, double *out_6320481552692820453) {
-   out_6320481552692820453[0] = 0;
-   out_6320481552692820453[1] = 0;
-   out_6320481552692820453[2] = 0;
-   out_6320481552692820453[3] = 0;
-   out_6320481552692820453[4] = 1;
-   out_6320481552692820453[5] = 0;
-   out_6320481552692820453[6] = 0;
-   out_6320481552692820453[7] = 0;
-   out_6320481552692820453[8] = 0;
+void H_30(double *state, double *unused, double *out_5234864343212188103) {
+   out_5234864343212188103[0] = 0;
+   out_5234864343212188103[1] = 0;
+   out_5234864343212188103[2] = 0;
+   out_5234864343212188103[3] = 0;
+   out_5234864343212188103[4] = 1;
+   out_5234864343212188103[5] = 0;
+   out_5234864343212188103[6] = 0;
+   out_5234864343212188103[7] = 0;
+   out_5234864343212188103[8] = 0;
 }
-void h_26(double *state, double *unused, double *out_8663712300753286970) {
-   out_8663712300753286970[0] = state[7];
+void h_26(double *state, double *unused, double *out_4200177702844975091) {
+   out_4200177702844975091[0] = state[7];
 }
-void H_26(double *state, double *unused, double *out_5866426243635426312) {
-   out_5866426243635426312[0] = 0;
-   out_5866426243635426312[1] = 0;
-   out_5866426243635426312[2] = 0;
-   out_5866426243635426312[3] = 0;
-   out_5866426243635426312[4] = 0;
-   out_5866426243635426312[5] = 0;
-   out_5866426243635426312[6] = 0;
-   out_5866426243635426312[7] = 1;
-   out_5866426243635426312[8] = 0;
+void H_26(double *state, double *unused, double *out_5423329317153484876) {
+   out_5423329317153484876[0] = 0;
+   out_5423329317153484876[1] = 0;
+   out_5423329317153484876[2] = 0;
+   out_5423329317153484876[3] = 0;
+   out_5423329317153484876[4] = 0;
+   out_5423329317153484876[5] = 0;
+   out_5423329317153484876[6] = 0;
+   out_5423329317153484876[7] = 1;
+   out_5423329317153484876[8] = 0;
 }
-void h_27(double *state, double *unused, double *out_2233071042628120952) {
-   out_2233071042628120952[0] = state[3];
+void h_27(double *state, double *unused, double *out_6628820938264559055) {
+   out_6628820938264559055[0] = state[3];
 }
-void H_27(double *state, double *unused, double *out_4096887481508877236) {
-   out_4096887481508877236[0] = 0;
-   out_4096887481508877236[1] = 0;
-   out_4096887481508877236[2] = 0;
-   out_4096887481508877236[3] = 1;
-   out_4096887481508877236[4] = 0;
-   out_4096887481508877236[5] = 0;
-   out_4096887481508877236[6] = 0;
-   out_4096887481508877236[7] = 0;
-   out_4096887481508877236[8] = 0;
+void H_27(double *state, double *unused, double *out_3985928257223093633) {
+   out_3985928257223093633[0] = 0;
+   out_3985928257223093633[1] = 0;
+   out_3985928257223093633[2] = 0;
+   out_3985928257223093633[3] = 1;
+   out_3985928257223093633[4] = 0;
+   out_3985928257223093633[5] = 0;
+   out_3985928257223093633[6] = 0;
+   out_3985928257223093633[7] = 0;
+   out_3985928257223093633[8] = 0;
 }
-void h_29(double *state, double *unused, double *out_8528880318658063037) {
-   out_8528880318658063037[0] = state[1];
+void h_29(double *state, double *unused, double *out_1102763501713535942) {
+   out_1102763501713535942[0] = state[1];
 }
-void H_29(double *state, double *unused, double *out_5810250208378428269) {
-   out_5810250208378428269[0] = 0;
-   out_5810250208378428269[1] = 1;
-   out_5810250208378428269[2] = 0;
-   out_5810250208378428269[3] = 0;
-   out_5810250208378428269[4] = 0;
-   out_5810250208378428269[5] = 0;
-   out_5810250208378428269[6] = 0;
-   out_5810250208378428269[7] = 0;
-   out_5810250208378428269[8] = 0;
+void H_29(double *state, double *unused, double *out_5699290984092644666) {
+   out_5699290984092644666[0] = 0;
+   out_5699290984092644666[1] = 1;
+   out_5699290984092644666[2] = 0;
+   out_5699290984092644666[3] = 0;
+   out_5699290984092644666[4] = 0;
+   out_5699290984092644666[5] = 0;
+   out_5699290984092644666[6] = 0;
+   out_5699290984092644666[7] = 0;
+   out_5699290984092644666[8] = 0;
 }
-void h_28(double *state, double *unused, double *out_4837476363436818603) {
-   out_4837476363436818603[0] = state[0];
+void h_28(double *state, double *unused, double *out_6175193264364035726) {
+   out_6175193264364035726[0] = state[0];
 }
-void H_28(double *state, double *unused, double *out_7554094848261592773) {
-   out_7554094848261592773[0] = 1;
-   out_7554094848261592773[1] = 0;
-   out_7554094848261592773[2] = 0;
-   out_7554094848261592773[3] = 0;
-   out_7554094848261592773[4] = 0;
-   out_7554094848261592773[5] = 0;
-   out_7554094848261592773[6] = 0;
-   out_7554094848261592773[7] = 0;
-   out_7554094848261592773[8] = 0;
+void H_28(double *state, double *unused, double *out_3735660712527318415) {
+   out_3735660712527318415[0] = 1;
+   out_3735660712527318415[1] = 0;
+   out_3735660712527318415[2] = 0;
+   out_3735660712527318415[3] = 0;
+   out_3735660712527318415[4] = 0;
+   out_3735660712527318415[5] = 0;
+   out_3735660712527318415[6] = 0;
+   out_3735660712527318415[7] = 0;
+   out_3735660712527318415[8] = 0;
 }
-void h_31(double *state, double *unused, double *out_7350316286992083367) {
-   out_7350316286992083367[0] = state[8];
+void h_31(double *state, double *unused, double *out_3077779063232596577) {
+   out_3077779063232596577[0] = state[8];
 }
-void H_31(double *state, double *unused, double *out_5240218141402074836) {
-   out_5240218141402074836[0] = 0;
-   out_5240218141402074836[1] = 0;
-   out_5240218141402074836[2] = 0;
-   out_5240218141402074836[3] = 0;
-   out_5240218141402074836[4] = 0;
-   out_5240218141402074836[5] = 0;
-   out_5240218141402074836[6] = 0;
-   out_5240218141402074836[7] = 0;
-   out_5240218141402074836[8] = 1;
+void H_31(double *state, double *unused, double *out_1651180036402468224) {
+   out_1651180036402468224[0] = 0;
+   out_1651180036402468224[1] = 0;
+   out_1651180036402468224[2] = 0;
+   out_1651180036402468224[3] = 0;
+   out_1651180036402468224[4] = 0;
+   out_1651180036402468224[5] = 0;
+   out_1651180036402468224[6] = 0;
+   out_1651180036402468224[7] = 0;
+   out_1651180036402468224[8] = 1;
 }
 #include <eigen3/Eigen/Dense>
 #include <iostream>
@@ -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_1840749225442711100) {
-  err_fun(nom_x, delta_x, out_1840749225442711100);
+void car_err_fun(double *nom_x, double *delta_x, double *out_5232708683747839446) {
+  err_fun(nom_x, delta_x, out_5232708683747839446);
 }
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_7887258947850824197) {
-  inv_err_fun(nom_x, true_x, out_7887258947850824197);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_6273110318621425138) {
+  inv_err_fun(nom_x, true_x, out_6273110318621425138);
 }
-void car_H_mod_fun(double *state, double *out_5849575757077882696) {
-  H_mod_fun(state, out_5849575757077882696);
+void car_H_mod_fun(double *state, double *out_376479380516898605) {
+  H_mod_fun(state, out_376479380516898605);
 }
-void car_f_fun(double *state, double dt, double *out_1306455665935687231) {
-  f_fun(state,  dt, out_1306455665935687231);
+void car_f_fun(double *state, double dt, double *out_7586787697879010325) {
+  f_fun(state,  dt, out_7586787697879010325);
 }
-void car_F_fun(double *state, double dt, double *out_1015750461961158607) {
-  F_fun(state,  dt, out_1015750461961158607);
+void car_F_fun(double *state, double dt, double *out_472790716300753243) {
+  F_fun(state,  dt, out_472790716300753243);
 }
-void car_h_25(double *state, double *unused, double *out_579481060641732431) {
-  h_25(state, unused, out_579481060641732431);
+void car_h_25(double *state, double *unused, double *out_6846635756302794664) {
+  h_25(state, unused, out_6846635756302794664);
 }
-void car_H_25(double *state, double *unused, double *out_8838814511200069080) {
-  H_25(state, unused, out_8838814511200069080);
+void car_H_25(double *state, double *unused, double *out_1681825998279428652) {
+  H_25(state, unused, out_1681825998279428652);
 }
-void car_h_24(double *state, double *unused, double *out_8628826922044058076) {
-  h_24(state, unused, out_8628826922044058076);
+void car_h_24(double *state, double *unused, double *out_7639061379755997248) {
+  h_24(state, unused, out_7639061379755997248);
 }
-void car_H_24(double *state, double *unused, double *out_7435279963503982970) {
-  H_24(state, unused, out_7435279963503982970);
+void car_H_24(double *state, double *unused, double *out_4893745808312089449) {
+  H_24(state, unused, out_4893745808312089449);
 }
-void car_h_30(double *state, double *unused, double *out_8060396416076809453) {
-  h_30(state, unused, out_8060396416076809453);
+void car_h_30(double *state, double *unused, double *out_2046273354615198377) {
+  h_30(state, unused, out_2046273354615198377);
 }
-void car_H_30(double *state, double *unused, double *out_6320481552692820453) {
-  H_30(state, unused, out_6320481552692820453);
+void car_H_30(double *state, double *unused, double *out_5234864343212188103) {
+  H_30(state, unused, out_5234864343212188103);
 }
-void car_h_26(double *state, double *unused, double *out_8663712300753286970) {
-  h_26(state, unused, out_8663712300753286970);
+void car_h_26(double *state, double *unused, double *out_4200177702844975091) {
+  h_26(state, unused, out_4200177702844975091);
 }
-void car_H_26(double *state, double *unused, double *out_5866426243635426312) {
-  H_26(state, unused, out_5866426243635426312);
+void car_H_26(double *state, double *unused, double *out_5423329317153484876) {
+  H_26(state, unused, out_5423329317153484876);
 }
-void car_h_27(double *state, double *unused, double *out_2233071042628120952) {
-  h_27(state, unused, out_2233071042628120952);
+void car_h_27(double *state, double *unused, double *out_6628820938264559055) {
+  h_27(state, unused, out_6628820938264559055);
 }
-void car_H_27(double *state, double *unused, double *out_4096887481508877236) {
-  H_27(state, unused, out_4096887481508877236);
+void car_H_27(double *state, double *unused, double *out_3985928257223093633) {
+  H_27(state, unused, out_3985928257223093633);
 }
-void car_h_29(double *state, double *unused, double *out_8528880318658063037) {
-  h_29(state, unused, out_8528880318658063037);
+void car_h_29(double *state, double *unused, double *out_1102763501713535942) {
+  h_29(state, unused, out_1102763501713535942);
 }
-void car_H_29(double *state, double *unused, double *out_5810250208378428269) {
-  H_29(state, unused, out_5810250208378428269);
+void car_H_29(double *state, double *unused, double *out_5699290984092644666) {
+  H_29(state, unused, out_5699290984092644666);
 }
-void car_h_28(double *state, double *unused, double *out_4837476363436818603) {
-  h_28(state, unused, out_4837476363436818603);
+void car_h_28(double *state, double *unused, double *out_6175193264364035726) {
+  h_28(state, unused, out_6175193264364035726);
 }
-void car_H_28(double *state, double *unused, double *out_7554094848261592773) {
-  H_28(state, unused, out_7554094848261592773);
+void car_H_28(double *state, double *unused, double *out_3735660712527318415) {
+  H_28(state, unused, out_3735660712527318415);
 }
-void car_h_31(double *state, double *unused, double *out_7350316286992083367) {
-  h_31(state, unused, out_7350316286992083367);
+void car_h_31(double *state, double *unused, double *out_3077779063232596577) {
+  h_31(state, unused, out_3077779063232596577);
 }
-void car_H_31(double *state, double *unused, double *out_5240218141402074836) {
-  H_31(state, unused, out_5240218141402074836);
+void car_H_31(double *state, double *unused, double *out_1651180036402468224) {
+  H_31(state, unused, out_1651180036402468224);
 }
 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 908149833..c648170bb 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_1840749225442711100);
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_7887258947850824197);
-void car_H_mod_fun(double *state, double *out_5849575757077882696);
-void car_f_fun(double *state, double dt, double *out_1306455665935687231);
-void car_F_fun(double *state, double dt, double *out_1015750461961158607);
-void car_h_25(double *state, double *unused, double *out_579481060641732431);
-void car_H_25(double *state, double *unused, double *out_8838814511200069080);
-void car_h_24(double *state, double *unused, double *out_8628826922044058076);
-void car_H_24(double *state, double *unused, double *out_7435279963503982970);
-void car_h_30(double *state, double *unused, double *out_8060396416076809453);
-void car_H_30(double *state, double *unused, double *out_6320481552692820453);
-void car_h_26(double *state, double *unused, double *out_8663712300753286970);
-void car_H_26(double *state, double *unused, double *out_5866426243635426312);
-void car_h_27(double *state, double *unused, double *out_2233071042628120952);
-void car_H_27(double *state, double *unused, double *out_4096887481508877236);
-void car_h_29(double *state, double *unused, double *out_8528880318658063037);
-void car_H_29(double *state, double *unused, double *out_5810250208378428269);
-void car_h_28(double *state, double *unused, double *out_4837476363436818603);
-void car_H_28(double *state, double *unused, double *out_7554094848261592773);
-void car_h_31(double *state, double *unused, double *out_7350316286992083367);
-void car_H_31(double *state, double *unused, double *out_5240218141402074836);
+void car_err_fun(double *nom_x, double *delta_x, double *out_5232708683747839446);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_6273110318621425138);
+void car_H_mod_fun(double *state, double *out_376479380516898605);
+void car_f_fun(double *state, double dt, double *out_7586787697879010325);
+void car_F_fun(double *state, double dt, double *out_472790716300753243);
+void car_h_25(double *state, double *unused, double *out_6846635756302794664);
+void car_H_25(double *state, double *unused, double *out_1681825998279428652);
+void car_h_24(double *state, double *unused, double *out_7639061379755997248);
+void car_H_24(double *state, double *unused, double *out_4893745808312089449);
+void car_h_30(double *state, double *unused, double *out_2046273354615198377);
+void car_H_30(double *state, double *unused, double *out_5234864343212188103);
+void car_h_26(double *state, double *unused, double *out_4200177702844975091);
+void car_H_26(double *state, double *unused, double *out_5423329317153484876);
+void car_h_27(double *state, double *unused, double *out_6628820938264559055);
+void car_H_27(double *state, double *unused, double *out_3985928257223093633);
+void car_h_29(double *state, double *unused, double *out_1102763501713535942);
+void car_H_29(double *state, double *unused, double *out_5699290984092644666);
+void car_h_28(double *state, double *unused, double *out_6175193264364035726);
+void car_H_28(double *state, double *unused, double *out_3735660712527318415);
+void car_h_31(double *state, double *unused, double *out_3077779063232596577);
+void car_H_31(double *state, double *unused, double *out_1651180036402468224);
 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/gnss.cpp b/selfdrive/locationd/models/generated/gnss.cpp
index bc1f786a8..7edb96132 100644
--- a/selfdrive/locationd/models/generated/gnss.cpp
+++ b/selfdrive/locationd/models/generated/gnss.cpp
@@ -17,354 +17,354 @@ const static double MAHA_THRESH_21 = 3.8414588206941227;
  *                                                                            *
  *                         This file is part of 'ekf'                         *
  ******************************************************************************/
-void err_fun(double *nom_x, double *delta_x, double *out_2351947283659598660) {
-   out_2351947283659598660[0] = delta_x[0] + nom_x[0];
-   out_2351947283659598660[1] = delta_x[1] + nom_x[1];
-   out_2351947283659598660[2] = delta_x[2] + nom_x[2];
-   out_2351947283659598660[3] = delta_x[3] + nom_x[3];
-   out_2351947283659598660[4] = delta_x[4] + nom_x[4];
-   out_2351947283659598660[5] = delta_x[5] + nom_x[5];
-   out_2351947283659598660[6] = delta_x[6] + nom_x[6];
-   out_2351947283659598660[7] = delta_x[7] + nom_x[7];
-   out_2351947283659598660[8] = delta_x[8] + nom_x[8];
-   out_2351947283659598660[9] = delta_x[9] + nom_x[9];
-   out_2351947283659598660[10] = delta_x[10] + nom_x[10];
+void err_fun(double *nom_x, double *delta_x, double *out_4281415265925619745) {
+   out_4281415265925619745[0] = delta_x[0] + nom_x[0];
+   out_4281415265925619745[1] = delta_x[1] + nom_x[1];
+   out_4281415265925619745[2] = delta_x[2] + nom_x[2];
+   out_4281415265925619745[3] = delta_x[3] + nom_x[3];
+   out_4281415265925619745[4] = delta_x[4] + nom_x[4];
+   out_4281415265925619745[5] = delta_x[5] + nom_x[5];
+   out_4281415265925619745[6] = delta_x[6] + nom_x[6];
+   out_4281415265925619745[7] = delta_x[7] + nom_x[7];
+   out_4281415265925619745[8] = delta_x[8] + nom_x[8];
+   out_4281415265925619745[9] = delta_x[9] + nom_x[9];
+   out_4281415265925619745[10] = delta_x[10] + nom_x[10];
 }
-void inv_err_fun(double *nom_x, double *true_x, double *out_8539310053726918653) {
-   out_8539310053726918653[0] = -nom_x[0] + true_x[0];
-   out_8539310053726918653[1] = -nom_x[1] + true_x[1];
-   out_8539310053726918653[2] = -nom_x[2] + true_x[2];
-   out_8539310053726918653[3] = -nom_x[3] + true_x[3];
-   out_8539310053726918653[4] = -nom_x[4] + true_x[4];
-   out_8539310053726918653[5] = -nom_x[5] + true_x[5];
-   out_8539310053726918653[6] = -nom_x[6] + true_x[6];
-   out_8539310053726918653[7] = -nom_x[7] + true_x[7];
-   out_8539310053726918653[8] = -nom_x[8] + true_x[8];
-   out_8539310053726918653[9] = -nom_x[9] + true_x[9];
-   out_8539310053726918653[10] = -nom_x[10] + true_x[10];
+void inv_err_fun(double *nom_x, double *true_x, double *out_8058048663833159220) {
+   out_8058048663833159220[0] = -nom_x[0] + true_x[0];
+   out_8058048663833159220[1] = -nom_x[1] + true_x[1];
+   out_8058048663833159220[2] = -nom_x[2] + true_x[2];
+   out_8058048663833159220[3] = -nom_x[3] + true_x[3];
+   out_8058048663833159220[4] = -nom_x[4] + true_x[4];
+   out_8058048663833159220[5] = -nom_x[5] + true_x[5];
+   out_8058048663833159220[6] = -nom_x[6] + true_x[6];
+   out_8058048663833159220[7] = -nom_x[7] + true_x[7];
+   out_8058048663833159220[8] = -nom_x[8] + true_x[8];
+   out_8058048663833159220[9] = -nom_x[9] + true_x[9];
+   out_8058048663833159220[10] = -nom_x[10] + true_x[10];
 }
-void H_mod_fun(double *state, double *out_2843304307356815987) {
-   out_2843304307356815987[0] = 1.0;
-   out_2843304307356815987[1] = 0;
-   out_2843304307356815987[2] = 0;
-   out_2843304307356815987[3] = 0;
-   out_2843304307356815987[4] = 0;
-   out_2843304307356815987[5] = 0;
-   out_2843304307356815987[6] = 0;
-   out_2843304307356815987[7] = 0;
-   out_2843304307356815987[8] = 0;
-   out_2843304307356815987[9] = 0;
-   out_2843304307356815987[10] = 0;
-   out_2843304307356815987[11] = 0;
-   out_2843304307356815987[12] = 1.0;
-   out_2843304307356815987[13] = 0;
-   out_2843304307356815987[14] = 0;
-   out_2843304307356815987[15] = 0;
-   out_2843304307356815987[16] = 0;
-   out_2843304307356815987[17] = 0;
-   out_2843304307356815987[18] = 0;
-   out_2843304307356815987[19] = 0;
-   out_2843304307356815987[20] = 0;
-   out_2843304307356815987[21] = 0;
-   out_2843304307356815987[22] = 0;
-   out_2843304307356815987[23] = 0;
-   out_2843304307356815987[24] = 1.0;
-   out_2843304307356815987[25] = 0;
-   out_2843304307356815987[26] = 0;
-   out_2843304307356815987[27] = 0;
-   out_2843304307356815987[28] = 0;
-   out_2843304307356815987[29] = 0;
-   out_2843304307356815987[30] = 0;
-   out_2843304307356815987[31] = 0;
-   out_2843304307356815987[32] = 0;
-   out_2843304307356815987[33] = 0;
-   out_2843304307356815987[34] = 0;
-   out_2843304307356815987[35] = 0;
-   out_2843304307356815987[36] = 1.0;
-   out_2843304307356815987[37] = 0;
-   out_2843304307356815987[38] = 0;
-   out_2843304307356815987[39] = 0;
-   out_2843304307356815987[40] = 0;
-   out_2843304307356815987[41] = 0;
-   out_2843304307356815987[42] = 0;
-   out_2843304307356815987[43] = 0;
-   out_2843304307356815987[44] = 0;
-   out_2843304307356815987[45] = 0;
-   out_2843304307356815987[46] = 0;
-   out_2843304307356815987[47] = 0;
-   out_2843304307356815987[48] = 1.0;
-   out_2843304307356815987[49] = 0;
-   out_2843304307356815987[50] = 0;
-   out_2843304307356815987[51] = 0;
-   out_2843304307356815987[52] = 0;
-   out_2843304307356815987[53] = 0;
-   out_2843304307356815987[54] = 0;
-   out_2843304307356815987[55] = 0;
-   out_2843304307356815987[56] = 0;
-   out_2843304307356815987[57] = 0;
-   out_2843304307356815987[58] = 0;
-   out_2843304307356815987[59] = 0;
-   out_2843304307356815987[60] = 1.0;
-   out_2843304307356815987[61] = 0;
-   out_2843304307356815987[62] = 0;
-   out_2843304307356815987[63] = 0;
-   out_2843304307356815987[64] = 0;
-   out_2843304307356815987[65] = 0;
-   out_2843304307356815987[66] = 0;
-   out_2843304307356815987[67] = 0;
-   out_2843304307356815987[68] = 0;
-   out_2843304307356815987[69] = 0;
-   out_2843304307356815987[70] = 0;
-   out_2843304307356815987[71] = 0;
-   out_2843304307356815987[72] = 1.0;
-   out_2843304307356815987[73] = 0;
-   out_2843304307356815987[74] = 0;
-   out_2843304307356815987[75] = 0;
-   out_2843304307356815987[76] = 0;
-   out_2843304307356815987[77] = 0;
-   out_2843304307356815987[78] = 0;
-   out_2843304307356815987[79] = 0;
-   out_2843304307356815987[80] = 0;
-   out_2843304307356815987[81] = 0;
-   out_2843304307356815987[82] = 0;
-   out_2843304307356815987[83] = 0;
-   out_2843304307356815987[84] = 1.0;
-   out_2843304307356815987[85] = 0;
-   out_2843304307356815987[86] = 0;
-   out_2843304307356815987[87] = 0;
-   out_2843304307356815987[88] = 0;
-   out_2843304307356815987[89] = 0;
-   out_2843304307356815987[90] = 0;
-   out_2843304307356815987[91] = 0;
-   out_2843304307356815987[92] = 0;
-   out_2843304307356815987[93] = 0;
-   out_2843304307356815987[94] = 0;
-   out_2843304307356815987[95] = 0;
-   out_2843304307356815987[96] = 1.0;
-   out_2843304307356815987[97] = 0;
-   out_2843304307356815987[98] = 0;
-   out_2843304307356815987[99] = 0;
-   out_2843304307356815987[100] = 0;
-   out_2843304307356815987[101] = 0;
-   out_2843304307356815987[102] = 0;
-   out_2843304307356815987[103] = 0;
-   out_2843304307356815987[104] = 0;
-   out_2843304307356815987[105] = 0;
-   out_2843304307356815987[106] = 0;
-   out_2843304307356815987[107] = 0;
-   out_2843304307356815987[108] = 1.0;
-   out_2843304307356815987[109] = 0;
-   out_2843304307356815987[110] = 0;
-   out_2843304307356815987[111] = 0;
-   out_2843304307356815987[112] = 0;
-   out_2843304307356815987[113] = 0;
-   out_2843304307356815987[114] = 0;
-   out_2843304307356815987[115] = 0;
-   out_2843304307356815987[116] = 0;
-   out_2843304307356815987[117] = 0;
-   out_2843304307356815987[118] = 0;
-   out_2843304307356815987[119] = 0;
-   out_2843304307356815987[120] = 1.0;
+void H_mod_fun(double *state, double *out_3690945488544052836) {
+   out_3690945488544052836[0] = 1.0;
+   out_3690945488544052836[1] = 0;
+   out_3690945488544052836[2] = 0;
+   out_3690945488544052836[3] = 0;
+   out_3690945488544052836[4] = 0;
+   out_3690945488544052836[5] = 0;
+   out_3690945488544052836[6] = 0;
+   out_3690945488544052836[7] = 0;
+   out_3690945488544052836[8] = 0;
+   out_3690945488544052836[9] = 0;
+   out_3690945488544052836[10] = 0;
+   out_3690945488544052836[11] = 0;
+   out_3690945488544052836[12] = 1.0;
+   out_3690945488544052836[13] = 0;
+   out_3690945488544052836[14] = 0;
+   out_3690945488544052836[15] = 0;
+   out_3690945488544052836[16] = 0;
+   out_3690945488544052836[17] = 0;
+   out_3690945488544052836[18] = 0;
+   out_3690945488544052836[19] = 0;
+   out_3690945488544052836[20] = 0;
+   out_3690945488544052836[21] = 0;
+   out_3690945488544052836[22] = 0;
+   out_3690945488544052836[23] = 0;
+   out_3690945488544052836[24] = 1.0;
+   out_3690945488544052836[25] = 0;
+   out_3690945488544052836[26] = 0;
+   out_3690945488544052836[27] = 0;
+   out_3690945488544052836[28] = 0;
+   out_3690945488544052836[29] = 0;
+   out_3690945488544052836[30] = 0;
+   out_3690945488544052836[31] = 0;
+   out_3690945488544052836[32] = 0;
+   out_3690945488544052836[33] = 0;
+   out_3690945488544052836[34] = 0;
+   out_3690945488544052836[35] = 0;
+   out_3690945488544052836[36] = 1.0;
+   out_3690945488544052836[37] = 0;
+   out_3690945488544052836[38] = 0;
+   out_3690945488544052836[39] = 0;
+   out_3690945488544052836[40] = 0;
+   out_3690945488544052836[41] = 0;
+   out_3690945488544052836[42] = 0;
+   out_3690945488544052836[43] = 0;
+   out_3690945488544052836[44] = 0;
+   out_3690945488544052836[45] = 0;
+   out_3690945488544052836[46] = 0;
+   out_3690945488544052836[47] = 0;
+   out_3690945488544052836[48] = 1.0;
+   out_3690945488544052836[49] = 0;
+   out_3690945488544052836[50] = 0;
+   out_3690945488544052836[51] = 0;
+   out_3690945488544052836[52] = 0;
+   out_3690945488544052836[53] = 0;
+   out_3690945488544052836[54] = 0;
+   out_3690945488544052836[55] = 0;
+   out_3690945488544052836[56] = 0;
+   out_3690945488544052836[57] = 0;
+   out_3690945488544052836[58] = 0;
+   out_3690945488544052836[59] = 0;
+   out_3690945488544052836[60] = 1.0;
+   out_3690945488544052836[61] = 0;
+   out_3690945488544052836[62] = 0;
+   out_3690945488544052836[63] = 0;
+   out_3690945488544052836[64] = 0;
+   out_3690945488544052836[65] = 0;
+   out_3690945488544052836[66] = 0;
+   out_3690945488544052836[67] = 0;
+   out_3690945488544052836[68] = 0;
+   out_3690945488544052836[69] = 0;
+   out_3690945488544052836[70] = 0;
+   out_3690945488544052836[71] = 0;
+   out_3690945488544052836[72] = 1.0;
+   out_3690945488544052836[73] = 0;
+   out_3690945488544052836[74] = 0;
+   out_3690945488544052836[75] = 0;
+   out_3690945488544052836[76] = 0;
+   out_3690945488544052836[77] = 0;
+   out_3690945488544052836[78] = 0;
+   out_3690945488544052836[79] = 0;
+   out_3690945488544052836[80] = 0;
+   out_3690945488544052836[81] = 0;
+   out_3690945488544052836[82] = 0;
+   out_3690945488544052836[83] = 0;
+   out_3690945488544052836[84] = 1.0;
+   out_3690945488544052836[85] = 0;
+   out_3690945488544052836[86] = 0;
+   out_3690945488544052836[87] = 0;
+   out_3690945488544052836[88] = 0;
+   out_3690945488544052836[89] = 0;
+   out_3690945488544052836[90] = 0;
+   out_3690945488544052836[91] = 0;
+   out_3690945488544052836[92] = 0;
+   out_3690945488544052836[93] = 0;
+   out_3690945488544052836[94] = 0;
+   out_3690945488544052836[95] = 0;
+   out_3690945488544052836[96] = 1.0;
+   out_3690945488544052836[97] = 0;
+   out_3690945488544052836[98] = 0;
+   out_3690945488544052836[99] = 0;
+   out_3690945488544052836[100] = 0;
+   out_3690945488544052836[101] = 0;
+   out_3690945488544052836[102] = 0;
+   out_3690945488544052836[103] = 0;
+   out_3690945488544052836[104] = 0;
+   out_3690945488544052836[105] = 0;
+   out_3690945488544052836[106] = 0;
+   out_3690945488544052836[107] = 0;
+   out_3690945488544052836[108] = 1.0;
+   out_3690945488544052836[109] = 0;
+   out_3690945488544052836[110] = 0;
+   out_3690945488544052836[111] = 0;
+   out_3690945488544052836[112] = 0;
+   out_3690945488544052836[113] = 0;
+   out_3690945488544052836[114] = 0;
+   out_3690945488544052836[115] = 0;
+   out_3690945488544052836[116] = 0;
+   out_3690945488544052836[117] = 0;
+   out_3690945488544052836[118] = 0;
+   out_3690945488544052836[119] = 0;
+   out_3690945488544052836[120] = 1.0;
 }
-void f_fun(double *state, double dt, double *out_774769938980785647) {
-   out_774769938980785647[0] = dt*state[3] + state[0];
-   out_774769938980785647[1] = dt*state[4] + state[1];
-   out_774769938980785647[2] = dt*state[5] + state[2];
-   out_774769938980785647[3] = state[3];
-   out_774769938980785647[4] = state[4];
-   out_774769938980785647[5] = state[5];
-   out_774769938980785647[6] = dt*state[7] + state[6];
-   out_774769938980785647[7] = dt*state[8] + state[7];
-   out_774769938980785647[8] = state[8];
-   out_774769938980785647[9] = state[9];
-   out_774769938980785647[10] = state[10];
+void f_fun(double *state, double dt, double *out_2954456239899519205) {
+   out_2954456239899519205[0] = dt*state[3] + state[0];
+   out_2954456239899519205[1] = dt*state[4] + state[1];
+   out_2954456239899519205[2] = dt*state[5] + state[2];
+   out_2954456239899519205[3] = state[3];
+   out_2954456239899519205[4] = state[4];
+   out_2954456239899519205[5] = state[5];
+   out_2954456239899519205[6] = dt*state[7] + state[6];
+   out_2954456239899519205[7] = dt*state[8] + state[7];
+   out_2954456239899519205[8] = state[8];
+   out_2954456239899519205[9] = state[9];
+   out_2954456239899519205[10] = state[10];
 }
-void F_fun(double *state, double dt, double *out_4467768114936024642) {
-   out_4467768114936024642[0] = 1;
-   out_4467768114936024642[1] = 0;
-   out_4467768114936024642[2] = 0;
-   out_4467768114936024642[3] = dt;
-   out_4467768114936024642[4] = 0;
-   out_4467768114936024642[5] = 0;
-   out_4467768114936024642[6] = 0;
-   out_4467768114936024642[7] = 0;
-   out_4467768114936024642[8] = 0;
-   out_4467768114936024642[9] = 0;
-   out_4467768114936024642[10] = 0;
-   out_4467768114936024642[11] = 0;
-   out_4467768114936024642[12] = 1;
-   out_4467768114936024642[13] = 0;
-   out_4467768114936024642[14] = 0;
-   out_4467768114936024642[15] = dt;
-   out_4467768114936024642[16] = 0;
-   out_4467768114936024642[17] = 0;
-   out_4467768114936024642[18] = 0;
-   out_4467768114936024642[19] = 0;
-   out_4467768114936024642[20] = 0;
-   out_4467768114936024642[21] = 0;
-   out_4467768114936024642[22] = 0;
-   out_4467768114936024642[23] = 0;
-   out_4467768114936024642[24] = 1;
-   out_4467768114936024642[25] = 0;
-   out_4467768114936024642[26] = 0;
-   out_4467768114936024642[27] = dt;
-   out_4467768114936024642[28] = 0;
-   out_4467768114936024642[29] = 0;
-   out_4467768114936024642[30] = 0;
-   out_4467768114936024642[31] = 0;
-   out_4467768114936024642[32] = 0;
-   out_4467768114936024642[33] = 0;
-   out_4467768114936024642[34] = 0;
-   out_4467768114936024642[35] = 0;
-   out_4467768114936024642[36] = 1;
-   out_4467768114936024642[37] = 0;
-   out_4467768114936024642[38] = 0;
-   out_4467768114936024642[39] = 0;
-   out_4467768114936024642[40] = 0;
-   out_4467768114936024642[41] = 0;
-   out_4467768114936024642[42] = 0;
-   out_4467768114936024642[43] = 0;
-   out_4467768114936024642[44] = 0;
-   out_4467768114936024642[45] = 0;
-   out_4467768114936024642[46] = 0;
-   out_4467768114936024642[47] = 0;
-   out_4467768114936024642[48] = 1;
-   out_4467768114936024642[49] = 0;
-   out_4467768114936024642[50] = 0;
-   out_4467768114936024642[51] = 0;
-   out_4467768114936024642[52] = 0;
-   out_4467768114936024642[53] = 0;
-   out_4467768114936024642[54] = 0;
-   out_4467768114936024642[55] = 0;
-   out_4467768114936024642[56] = 0;
-   out_4467768114936024642[57] = 0;
-   out_4467768114936024642[58] = 0;
-   out_4467768114936024642[59] = 0;
-   out_4467768114936024642[60] = 1;
-   out_4467768114936024642[61] = 0;
-   out_4467768114936024642[62] = 0;
-   out_4467768114936024642[63] = 0;
-   out_4467768114936024642[64] = 0;
-   out_4467768114936024642[65] = 0;
-   out_4467768114936024642[66] = 0;
-   out_4467768114936024642[67] = 0;
-   out_4467768114936024642[68] = 0;
-   out_4467768114936024642[69] = 0;
-   out_4467768114936024642[70] = 0;
-   out_4467768114936024642[71] = 0;
-   out_4467768114936024642[72] = 1;
-   out_4467768114936024642[73] = dt;
-   out_4467768114936024642[74] = 0;
-   out_4467768114936024642[75] = 0;
-   out_4467768114936024642[76] = 0;
-   out_4467768114936024642[77] = 0;
-   out_4467768114936024642[78] = 0;
-   out_4467768114936024642[79] = 0;
-   out_4467768114936024642[80] = 0;
-   out_4467768114936024642[81] = 0;
-   out_4467768114936024642[82] = 0;
-   out_4467768114936024642[83] = 0;
-   out_4467768114936024642[84] = 1;
-   out_4467768114936024642[85] = dt;
-   out_4467768114936024642[86] = 0;
-   out_4467768114936024642[87] = 0;
-   out_4467768114936024642[88] = 0;
-   out_4467768114936024642[89] = 0;
-   out_4467768114936024642[90] = 0;
-   out_4467768114936024642[91] = 0;
-   out_4467768114936024642[92] = 0;
-   out_4467768114936024642[93] = 0;
-   out_4467768114936024642[94] = 0;
-   out_4467768114936024642[95] = 0;
-   out_4467768114936024642[96] = 1;
-   out_4467768114936024642[97] = 0;
-   out_4467768114936024642[98] = 0;
-   out_4467768114936024642[99] = 0;
-   out_4467768114936024642[100] = 0;
-   out_4467768114936024642[101] = 0;
-   out_4467768114936024642[102] = 0;
-   out_4467768114936024642[103] = 0;
-   out_4467768114936024642[104] = 0;
-   out_4467768114936024642[105] = 0;
-   out_4467768114936024642[106] = 0;
-   out_4467768114936024642[107] = 0;
-   out_4467768114936024642[108] = 1;
-   out_4467768114936024642[109] = 0;
-   out_4467768114936024642[110] = 0;
-   out_4467768114936024642[111] = 0;
-   out_4467768114936024642[112] = 0;
-   out_4467768114936024642[113] = 0;
-   out_4467768114936024642[114] = 0;
-   out_4467768114936024642[115] = 0;
-   out_4467768114936024642[116] = 0;
-   out_4467768114936024642[117] = 0;
-   out_4467768114936024642[118] = 0;
-   out_4467768114936024642[119] = 0;
-   out_4467768114936024642[120] = 1;
+void F_fun(double *state, double dt, double *out_709514566261682476) {
+   out_709514566261682476[0] = 1;
+   out_709514566261682476[1] = 0;
+   out_709514566261682476[2] = 0;
+   out_709514566261682476[3] = dt;
+   out_709514566261682476[4] = 0;
+   out_709514566261682476[5] = 0;
+   out_709514566261682476[6] = 0;
+   out_709514566261682476[7] = 0;
+   out_709514566261682476[8] = 0;
+   out_709514566261682476[9] = 0;
+   out_709514566261682476[10] = 0;
+   out_709514566261682476[11] = 0;
+   out_709514566261682476[12] = 1;
+   out_709514566261682476[13] = 0;
+   out_709514566261682476[14] = 0;
+   out_709514566261682476[15] = dt;
+   out_709514566261682476[16] = 0;
+   out_709514566261682476[17] = 0;
+   out_709514566261682476[18] = 0;
+   out_709514566261682476[19] = 0;
+   out_709514566261682476[20] = 0;
+   out_709514566261682476[21] = 0;
+   out_709514566261682476[22] = 0;
+   out_709514566261682476[23] = 0;
+   out_709514566261682476[24] = 1;
+   out_709514566261682476[25] = 0;
+   out_709514566261682476[26] = 0;
+   out_709514566261682476[27] = dt;
+   out_709514566261682476[28] = 0;
+   out_709514566261682476[29] = 0;
+   out_709514566261682476[30] = 0;
+   out_709514566261682476[31] = 0;
+   out_709514566261682476[32] = 0;
+   out_709514566261682476[33] = 0;
+   out_709514566261682476[34] = 0;
+   out_709514566261682476[35] = 0;
+   out_709514566261682476[36] = 1;
+   out_709514566261682476[37] = 0;
+   out_709514566261682476[38] = 0;
+   out_709514566261682476[39] = 0;
+   out_709514566261682476[40] = 0;
+   out_709514566261682476[41] = 0;
+   out_709514566261682476[42] = 0;
+   out_709514566261682476[43] = 0;
+   out_709514566261682476[44] = 0;
+   out_709514566261682476[45] = 0;
+   out_709514566261682476[46] = 0;
+   out_709514566261682476[47] = 0;
+   out_709514566261682476[48] = 1;
+   out_709514566261682476[49] = 0;
+   out_709514566261682476[50] = 0;
+   out_709514566261682476[51] = 0;
+   out_709514566261682476[52] = 0;
+   out_709514566261682476[53] = 0;
+   out_709514566261682476[54] = 0;
+   out_709514566261682476[55] = 0;
+   out_709514566261682476[56] = 0;
+   out_709514566261682476[57] = 0;
+   out_709514566261682476[58] = 0;
+   out_709514566261682476[59] = 0;
+   out_709514566261682476[60] = 1;
+   out_709514566261682476[61] = 0;
+   out_709514566261682476[62] = 0;
+   out_709514566261682476[63] = 0;
+   out_709514566261682476[64] = 0;
+   out_709514566261682476[65] = 0;
+   out_709514566261682476[66] = 0;
+   out_709514566261682476[67] = 0;
+   out_709514566261682476[68] = 0;
+   out_709514566261682476[69] = 0;
+   out_709514566261682476[70] = 0;
+   out_709514566261682476[71] = 0;
+   out_709514566261682476[72] = 1;
+   out_709514566261682476[73] = dt;
+   out_709514566261682476[74] = 0;
+   out_709514566261682476[75] = 0;
+   out_709514566261682476[76] = 0;
+   out_709514566261682476[77] = 0;
+   out_709514566261682476[78] = 0;
+   out_709514566261682476[79] = 0;
+   out_709514566261682476[80] = 0;
+   out_709514566261682476[81] = 0;
+   out_709514566261682476[82] = 0;
+   out_709514566261682476[83] = 0;
+   out_709514566261682476[84] = 1;
+   out_709514566261682476[85] = dt;
+   out_709514566261682476[86] = 0;
+   out_709514566261682476[87] = 0;
+   out_709514566261682476[88] = 0;
+   out_709514566261682476[89] = 0;
+   out_709514566261682476[90] = 0;
+   out_709514566261682476[91] = 0;
+   out_709514566261682476[92] = 0;
+   out_709514566261682476[93] = 0;
+   out_709514566261682476[94] = 0;
+   out_709514566261682476[95] = 0;
+   out_709514566261682476[96] = 1;
+   out_709514566261682476[97] = 0;
+   out_709514566261682476[98] = 0;
+   out_709514566261682476[99] = 0;
+   out_709514566261682476[100] = 0;
+   out_709514566261682476[101] = 0;
+   out_709514566261682476[102] = 0;
+   out_709514566261682476[103] = 0;
+   out_709514566261682476[104] = 0;
+   out_709514566261682476[105] = 0;
+   out_709514566261682476[106] = 0;
+   out_709514566261682476[107] = 0;
+   out_709514566261682476[108] = 1;
+   out_709514566261682476[109] = 0;
+   out_709514566261682476[110] = 0;
+   out_709514566261682476[111] = 0;
+   out_709514566261682476[112] = 0;
+   out_709514566261682476[113] = 0;
+   out_709514566261682476[114] = 0;
+   out_709514566261682476[115] = 0;
+   out_709514566261682476[116] = 0;
+   out_709514566261682476[117] = 0;
+   out_709514566261682476[118] = 0;
+   out_709514566261682476[119] = 0;
+   out_709514566261682476[120] = 1;
 }
-void h_6(double *state, double *sat_pos, double *out_8206867657584711155) {
-   out_8206867657584711155[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + state[6];
+void h_6(double *state, double *sat_pos, double *out_2648966289655583926) {
+   out_2648966289655583926[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + state[6];
 }
-void H_6(double *state, double *sat_pos, double *out_2748205046550560630) {
-   out_2748205046550560630[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
-   out_2748205046550560630[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
-   out_2748205046550560630[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
-   out_2748205046550560630[3] = 0;
-   out_2748205046550560630[4] = 0;
-   out_2748205046550560630[5] = 0;
-   out_2748205046550560630[6] = 1;
-   out_2748205046550560630[7] = 0;
-   out_2748205046550560630[8] = 0;
-   out_2748205046550560630[9] = 0;
-   out_2748205046550560630[10] = 0;
+void H_6(double *state, double *sat_pos, double *out_1516634108393932450) {
+   out_1516634108393932450[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
+   out_1516634108393932450[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
+   out_1516634108393932450[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
+   out_1516634108393932450[3] = 0;
+   out_1516634108393932450[4] = 0;
+   out_1516634108393932450[5] = 0;
+   out_1516634108393932450[6] = 1;
+   out_1516634108393932450[7] = 0;
+   out_1516634108393932450[8] = 0;
+   out_1516634108393932450[9] = 0;
+   out_1516634108393932450[10] = 0;
 }
-void h_20(double *state, double *sat_pos, double *out_3144072620339266513) {
-   out_3144072620339266513[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + sat_pos[3]*state[10] + state[6] + state[9];
+void h_20(double *state, double *sat_pos, double *out_1322673160422067744) {
+   out_1322673160422067744[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + sat_pos[3]*state[10] + state[6] + state[9];
 }
-void H_20(double *state, double *sat_pos, double *out_8303647801952025828) {
-   out_8303647801952025828[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
-   out_8303647801952025828[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
-   out_8303647801952025828[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
-   out_8303647801952025828[3] = 0;
-   out_8303647801952025828[4] = 0;
-   out_8303647801952025828[5] = 0;
-   out_8303647801952025828[6] = 1;
-   out_8303647801952025828[7] = 0;
-   out_8303647801952025828[8] = 0;
-   out_8303647801952025828[9] = 1;
-   out_8303647801952025828[10] = sat_pos[3];
+void H_20(double *state, double *sat_pos, double *out_6746823634573947585) {
+   out_6746823634573947585[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
+   out_6746823634573947585[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
+   out_6746823634573947585[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
+   out_6746823634573947585[3] = 0;
+   out_6746823634573947585[4] = 0;
+   out_6746823634573947585[5] = 0;
+   out_6746823634573947585[6] = 1;
+   out_6746823634573947585[7] = 0;
+   out_6746823634573947585[8] = 0;
+   out_6746823634573947585[9] = 1;
+   out_6746823634573947585[10] = sat_pos[3];
 }
-void h_7(double *state, double *sat_pos_vel, double *out_7730590468597126339) {
-   out_7730590468597126339[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
+void h_7(double *state, double *sat_pos_vel, double *out_3768133833178917308) {
+   out_3768133833178917308[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
 }
-void H_7(double *state, double *sat_pos_vel, double *out_1168235388167943403) {
-   out_1168235388167943403[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
-   out_1168235388167943403[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
-   out_1168235388167943403[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
-   out_1168235388167943403[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
-   out_1168235388167943403[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
-   out_1168235388167943403[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
-   out_1168235388167943403[6] = 0;
-   out_1168235388167943403[7] = 1;
-   out_1168235388167943403[8] = 0;
-   out_1168235388167943403[9] = 0;
-   out_1168235388167943403[10] = 0;
+void H_7(double *state, double *sat_pos_vel, double *out_6978532755514735815) {
+   out_6978532755514735815[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+   out_6978532755514735815[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+   out_6978532755514735815[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+   out_6978532755514735815[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+   out_6978532755514735815[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+   out_6978532755514735815[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+   out_6978532755514735815[6] = 0;
+   out_6978532755514735815[7] = 1;
+   out_6978532755514735815[8] = 0;
+   out_6978532755514735815[9] = 0;
+   out_6978532755514735815[10] = 0;
 }
-void h_21(double *state, double *sat_pos_vel, double *out_7730590468597126339) {
-   out_7730590468597126339[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
+void h_21(double *state, double *sat_pos_vel, double *out_3768133833178917308) {
+   out_3768133833178917308[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
 }
-void H_21(double *state, double *sat_pos_vel, double *out_1168235388167943403) {
-   out_1168235388167943403[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
-   out_1168235388167943403[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
-   out_1168235388167943403[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
-   out_1168235388167943403[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
-   out_1168235388167943403[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
-   out_1168235388167943403[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
-   out_1168235388167943403[6] = 0;
-   out_1168235388167943403[7] = 1;
-   out_1168235388167943403[8] = 0;
-   out_1168235388167943403[9] = 0;
-   out_1168235388167943403[10] = 0;
+void H_21(double *state, double *sat_pos_vel, double *out_6978532755514735815) {
+   out_6978532755514735815[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+   out_6978532755514735815[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+   out_6978532755514735815[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+   out_6978532755514735815[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+   out_6978532755514735815[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+   out_6978532755514735815[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+   out_6978532755514735815[6] = 0;
+   out_6978532755514735815[7] = 1;
+   out_6978532755514735815[8] = 0;
+   out_6978532755514735815[9] = 0;
+   out_6978532755514735815[10] = 0;
 }
 #include <eigen3/Eigen/Dense>
 #include <iostream>
@@ -506,44 +506,44 @@ void gnss_update_7(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void gnss_update_21(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
   update<1, 3, 0>(in_x, in_P, h_21, H_21, NULL, in_z, in_R, in_ea, MAHA_THRESH_21);
 }
-void gnss_err_fun(double *nom_x, double *delta_x, double *out_2351947283659598660) {
-  err_fun(nom_x, delta_x, out_2351947283659598660);
+void gnss_err_fun(double *nom_x, double *delta_x, double *out_4281415265925619745) {
+  err_fun(nom_x, delta_x, out_4281415265925619745);
 }
-void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_8539310053726918653) {
-  inv_err_fun(nom_x, true_x, out_8539310053726918653);
+void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_8058048663833159220) {
+  inv_err_fun(nom_x, true_x, out_8058048663833159220);
 }
-void gnss_H_mod_fun(double *state, double *out_2843304307356815987) {
-  H_mod_fun(state, out_2843304307356815987);
+void gnss_H_mod_fun(double *state, double *out_3690945488544052836) {
+  H_mod_fun(state, out_3690945488544052836);
 }
-void gnss_f_fun(double *state, double dt, double *out_774769938980785647) {
-  f_fun(state,  dt, out_774769938980785647);
+void gnss_f_fun(double *state, double dt, double *out_2954456239899519205) {
+  f_fun(state,  dt, out_2954456239899519205);
 }
-void gnss_F_fun(double *state, double dt, double *out_4467768114936024642) {
-  F_fun(state,  dt, out_4467768114936024642);
+void gnss_F_fun(double *state, double dt, double *out_709514566261682476) {
+  F_fun(state,  dt, out_709514566261682476);
 }
-void gnss_h_6(double *state, double *sat_pos, double *out_8206867657584711155) {
-  h_6(state, sat_pos, out_8206867657584711155);
+void gnss_h_6(double *state, double *sat_pos, double *out_2648966289655583926) {
+  h_6(state, sat_pos, out_2648966289655583926);
 }
-void gnss_H_6(double *state, double *sat_pos, double *out_2748205046550560630) {
-  H_6(state, sat_pos, out_2748205046550560630);
+void gnss_H_6(double *state, double *sat_pos, double *out_1516634108393932450) {
+  H_6(state, sat_pos, out_1516634108393932450);
 }
-void gnss_h_20(double *state, double *sat_pos, double *out_3144072620339266513) {
-  h_20(state, sat_pos, out_3144072620339266513);
+void gnss_h_20(double *state, double *sat_pos, double *out_1322673160422067744) {
+  h_20(state, sat_pos, out_1322673160422067744);
 }
-void gnss_H_20(double *state, double *sat_pos, double *out_8303647801952025828) {
-  H_20(state, sat_pos, out_8303647801952025828);
+void gnss_H_20(double *state, double *sat_pos, double *out_6746823634573947585) {
+  H_20(state, sat_pos, out_6746823634573947585);
 }
-void gnss_h_7(double *state, double *sat_pos_vel, double *out_7730590468597126339) {
-  h_7(state, sat_pos_vel, out_7730590468597126339);
+void gnss_h_7(double *state, double *sat_pos_vel, double *out_3768133833178917308) {
+  h_7(state, sat_pos_vel, out_3768133833178917308);
 }
-void gnss_H_7(double *state, double *sat_pos_vel, double *out_1168235388167943403) {
-  H_7(state, sat_pos_vel, out_1168235388167943403);
+void gnss_H_7(double *state, double *sat_pos_vel, double *out_6978532755514735815) {
+  H_7(state, sat_pos_vel, out_6978532755514735815);
 }
-void gnss_h_21(double *state, double *sat_pos_vel, double *out_7730590468597126339) {
-  h_21(state, sat_pos_vel, out_7730590468597126339);
+void gnss_h_21(double *state, double *sat_pos_vel, double *out_3768133833178917308) {
+  h_21(state, sat_pos_vel, out_3768133833178917308);
 }
-void gnss_H_21(double *state, double *sat_pos_vel, double *out_1168235388167943403) {
-  H_21(state, sat_pos_vel, out_1168235388167943403);
+void gnss_H_21(double *state, double *sat_pos_vel, double *out_6978532755514735815) {
+  H_21(state, sat_pos_vel, out_6978532755514735815);
 }
 void gnss_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/gnss.h b/selfdrive/locationd/models/generated/gnss.h
index 54d03e2a3..ee031ed22 100644
--- a/selfdrive/locationd/models/generated/gnss.h
+++ b/selfdrive/locationd/models/generated/gnss.h
@@ -5,18 +5,18 @@ void gnss_update_6(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void gnss_update_20(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void gnss_update_7(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void gnss_update_21(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void gnss_err_fun(double *nom_x, double *delta_x, double *out_2351947283659598660);
-void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_8539310053726918653);
-void gnss_H_mod_fun(double *state, double *out_2843304307356815987);
-void gnss_f_fun(double *state, double dt, double *out_774769938980785647);
-void gnss_F_fun(double *state, double dt, double *out_4467768114936024642);
-void gnss_h_6(double *state, double *sat_pos, double *out_8206867657584711155);
-void gnss_H_6(double *state, double *sat_pos, double *out_2748205046550560630);
-void gnss_h_20(double *state, double *sat_pos, double *out_3144072620339266513);
-void gnss_H_20(double *state, double *sat_pos, double *out_8303647801952025828);
-void gnss_h_7(double *state, double *sat_pos_vel, double *out_7730590468597126339);
-void gnss_H_7(double *state, double *sat_pos_vel, double *out_1168235388167943403);
-void gnss_h_21(double *state, double *sat_pos_vel, double *out_7730590468597126339);
-void gnss_H_21(double *state, double *sat_pos_vel, double *out_1168235388167943403);
+void gnss_err_fun(double *nom_x, double *delta_x, double *out_4281415265925619745);
+void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_8058048663833159220);
+void gnss_H_mod_fun(double *state, double *out_3690945488544052836);
+void gnss_f_fun(double *state, double dt, double *out_2954456239899519205);
+void gnss_F_fun(double *state, double dt, double *out_709514566261682476);
+void gnss_h_6(double *state, double *sat_pos, double *out_2648966289655583926);
+void gnss_H_6(double *state, double *sat_pos, double *out_1516634108393932450);
+void gnss_h_20(double *state, double *sat_pos, double *out_1322673160422067744);
+void gnss_H_20(double *state, double *sat_pos, double *out_6746823634573947585);
+void gnss_h_7(double *state, double *sat_pos_vel, double *out_3768133833178917308);
+void gnss_H_7(double *state, double *sat_pos_vel, double *out_6978532755514735815);
+void gnss_h_21(double *state, double *sat_pos_vel, double *out_3768133833178917308);
+void gnss_H_21(double *state, double *sat_pos_vel, double *out_6978532755514735815);
 void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt);
 }
\ No newline at end of file
diff --git a/selfdrive/locationd/models/generated/libkf.so b/selfdrive/locationd/models/generated/libkf.so
index a872de715..7ada2c02e 100755
Binary files a/selfdrive/locationd/models/generated/libkf.so and b/selfdrive/locationd/models/generated/libkf.so differ
diff --git a/selfdrive/locationd/models/generated/live.cpp b/selfdrive/locationd/models/generated/live.cpp
index 0c0281f27..5a75cde58 100644
--- a/selfdrive/locationd/models/generated/live.cpp
+++ b/selfdrive/locationd/models/generated/live.cpp
@@ -22,1683 +22,1683 @@ const static double MAHA_THRESH_33 = 7.814727903251177;
  *                                                                            *
  *                         This file is part of 'ekf'                         *
  ******************************************************************************/
-void H(double *in_vec, double *out_8687680363762493539) {
-   out_8687680363762493539[0] = 0;
-   out_8687680363762493539[1] = -sin(in_vec[1])*sin(in_vec[2])*in_vec[4] - sin(in_vec[1])*cos(in_vec[2])*in_vec[3] - cos(in_vec[1])*in_vec[5];
-   out_8687680363762493539[2] = -sin(in_vec[2])*cos(in_vec[1])*in_vec[3] + cos(in_vec[1])*cos(in_vec[2])*in_vec[4];
-   out_8687680363762493539[3] = cos(in_vec[1])*cos(in_vec[2]);
-   out_8687680363762493539[4] = sin(in_vec[2])*cos(in_vec[1]);
-   out_8687680363762493539[5] = -sin(in_vec[1]);
-   out_8687680363762493539[6] = (sin(in_vec[0])*sin(in_vec[2]) + sin(in_vec[1])*cos(in_vec[0])*cos(in_vec[2]))*in_vec[3] + (-sin(in_vec[0])*cos(in_vec[2]) + sin(in_vec[1])*sin(in_vec[2])*cos(in_vec[0]))*in_vec[4] + cos(in_vec[0])*cos(in_vec[1])*in_vec[5];
-   out_8687680363762493539[7] = -sin(in_vec[0])*sin(in_vec[1])*in_vec[5] + sin(in_vec[0])*sin(in_vec[2])*cos(in_vec[1])*in_vec[4] + sin(in_vec[0])*cos(in_vec[1])*cos(in_vec[2])*in_vec[3];
-   out_8687680363762493539[8] = (-sin(in_vec[0])*sin(in_vec[1])*sin(in_vec[2]) - cos(in_vec[0])*cos(in_vec[2]))*in_vec[3] + (sin(in_vec[0])*sin(in_vec[1])*cos(in_vec[2]) - sin(in_vec[2])*cos(in_vec[0]))*in_vec[4];
-   out_8687680363762493539[9] = sin(in_vec[0])*sin(in_vec[1])*cos(in_vec[2]) - sin(in_vec[2])*cos(in_vec[0]);
-   out_8687680363762493539[10] = sin(in_vec[0])*sin(in_vec[1])*sin(in_vec[2]) + cos(in_vec[0])*cos(in_vec[2]);
-   out_8687680363762493539[11] = sin(in_vec[0])*cos(in_vec[1]);
-   out_8687680363762493539[12] = (-sin(in_vec[0])*sin(in_vec[1])*sin(in_vec[2]) - cos(in_vec[0])*cos(in_vec[2]))*in_vec[4] + (-sin(in_vec[0])*sin(in_vec[1])*cos(in_vec[2]) + sin(in_vec[2])*cos(in_vec[0]))*in_vec[3] - sin(in_vec[0])*cos(in_vec[1])*in_vec[5];
-   out_8687680363762493539[13] = -sin(in_vec[1])*cos(in_vec[0])*in_vec[5] + sin(in_vec[2])*cos(in_vec[0])*cos(in_vec[1])*in_vec[4] + cos(in_vec[0])*cos(in_vec[1])*cos(in_vec[2])*in_vec[3];
-   out_8687680363762493539[14] = (sin(in_vec[0])*sin(in_vec[2]) + sin(in_vec[1])*cos(in_vec[0])*cos(in_vec[2]))*in_vec[4] + (sin(in_vec[0])*cos(in_vec[2]) - sin(in_vec[1])*sin(in_vec[2])*cos(in_vec[0]))*in_vec[3];
-   out_8687680363762493539[15] = sin(in_vec[0])*sin(in_vec[2]) + sin(in_vec[1])*cos(in_vec[0])*cos(in_vec[2]);
-   out_8687680363762493539[16] = -sin(in_vec[0])*cos(in_vec[2]) + sin(in_vec[1])*sin(in_vec[2])*cos(in_vec[0]);
-   out_8687680363762493539[17] = cos(in_vec[0])*cos(in_vec[1]);
+void H(double *in_vec, double *out_907497564161450989) {
+   out_907497564161450989[0] = 0;
+   out_907497564161450989[1] = -sin(in_vec[1])*sin(in_vec[2])*in_vec[4] - sin(in_vec[1])*cos(in_vec[2])*in_vec[3] - cos(in_vec[1])*in_vec[5];
+   out_907497564161450989[2] = -sin(in_vec[2])*cos(in_vec[1])*in_vec[3] + cos(in_vec[1])*cos(in_vec[2])*in_vec[4];
+   out_907497564161450989[3] = cos(in_vec[1])*cos(in_vec[2]);
+   out_907497564161450989[4] = sin(in_vec[2])*cos(in_vec[1]);
+   out_907497564161450989[5] = -sin(in_vec[1]);
+   out_907497564161450989[6] = (sin(in_vec[0])*sin(in_vec[2]) + sin(in_vec[1])*cos(in_vec[0])*cos(in_vec[2]))*in_vec[3] + (-sin(in_vec[0])*cos(in_vec[2]) + sin(in_vec[1])*sin(in_vec[2])*cos(in_vec[0]))*in_vec[4] + cos(in_vec[0])*cos(in_vec[1])*in_vec[5];
+   out_907497564161450989[7] = -sin(in_vec[0])*sin(in_vec[1])*in_vec[5] + sin(in_vec[0])*sin(in_vec[2])*cos(in_vec[1])*in_vec[4] + sin(in_vec[0])*cos(in_vec[1])*cos(in_vec[2])*in_vec[3];
+   out_907497564161450989[8] = (-sin(in_vec[0])*sin(in_vec[1])*sin(in_vec[2]) - cos(in_vec[0])*cos(in_vec[2]))*in_vec[3] + (sin(in_vec[0])*sin(in_vec[1])*cos(in_vec[2]) - sin(in_vec[2])*cos(in_vec[0]))*in_vec[4];
+   out_907497564161450989[9] = sin(in_vec[0])*sin(in_vec[1])*cos(in_vec[2]) - sin(in_vec[2])*cos(in_vec[0]);
+   out_907497564161450989[10] = sin(in_vec[0])*sin(in_vec[1])*sin(in_vec[2]) + cos(in_vec[0])*cos(in_vec[2]);
+   out_907497564161450989[11] = sin(in_vec[0])*cos(in_vec[1]);
+   out_907497564161450989[12] = (-sin(in_vec[0])*sin(in_vec[1])*sin(in_vec[2]) - cos(in_vec[0])*cos(in_vec[2]))*in_vec[4] + (-sin(in_vec[0])*sin(in_vec[1])*cos(in_vec[2]) + sin(in_vec[2])*cos(in_vec[0]))*in_vec[3] - sin(in_vec[0])*cos(in_vec[1])*in_vec[5];
+   out_907497564161450989[13] = -sin(in_vec[1])*cos(in_vec[0])*in_vec[5] + sin(in_vec[2])*cos(in_vec[0])*cos(in_vec[1])*in_vec[4] + cos(in_vec[0])*cos(in_vec[1])*cos(in_vec[2])*in_vec[3];
+   out_907497564161450989[14] = (sin(in_vec[0])*sin(in_vec[2]) + sin(in_vec[1])*cos(in_vec[0])*cos(in_vec[2]))*in_vec[4] + (sin(in_vec[0])*cos(in_vec[2]) - sin(in_vec[1])*sin(in_vec[2])*cos(in_vec[0]))*in_vec[3];
+   out_907497564161450989[15] = sin(in_vec[0])*sin(in_vec[2]) + sin(in_vec[1])*cos(in_vec[0])*cos(in_vec[2]);
+   out_907497564161450989[16] = -sin(in_vec[0])*cos(in_vec[2]) + sin(in_vec[1])*sin(in_vec[2])*cos(in_vec[0]);
+   out_907497564161450989[17] = cos(in_vec[0])*cos(in_vec[1]);
 }
-void err_fun(double *nom_x, double *delta_x, double *out_8136490426412747881) {
-   out_8136490426412747881[0] = delta_x[0] + nom_x[0];
-   out_8136490426412747881[1] = delta_x[1] + nom_x[1];
-   out_8136490426412747881[2] = delta_x[2] + nom_x[2];
-   out_8136490426412747881[3] = -0.5*delta_x[3]*nom_x[4] - 0.5*delta_x[4]*nom_x[5] - 0.5*delta_x[5]*nom_x[6] + 1.0*nom_x[3];
-   out_8136490426412747881[4] = 0.5*delta_x[3]*nom_x[3] + 0.5*delta_x[4]*nom_x[6] - 0.5*delta_x[5]*nom_x[5] + 1.0*nom_x[4];
-   out_8136490426412747881[5] = -0.5*delta_x[3]*nom_x[6] + 0.5*delta_x[4]*nom_x[3] + 0.5*delta_x[5]*nom_x[4] + 1.0*nom_x[5];
-   out_8136490426412747881[6] = 0.5*delta_x[3]*nom_x[5] - 0.5*delta_x[4]*nom_x[4] + 0.5*delta_x[5]*nom_x[3] + 1.0*nom_x[6];
-   out_8136490426412747881[7] = delta_x[6] + nom_x[7];
-   out_8136490426412747881[8] = delta_x[7] + nom_x[8];
-   out_8136490426412747881[9] = delta_x[8] + nom_x[9];
-   out_8136490426412747881[10] = delta_x[9] + nom_x[10];
-   out_8136490426412747881[11] = delta_x[10] + nom_x[11];
-   out_8136490426412747881[12] = delta_x[11] + nom_x[12];
-   out_8136490426412747881[13] = delta_x[12] + nom_x[13];
-   out_8136490426412747881[14] = delta_x[13] + nom_x[14];
-   out_8136490426412747881[15] = delta_x[14] + nom_x[15];
-   out_8136490426412747881[16] = delta_x[15] + nom_x[16];
-   out_8136490426412747881[17] = delta_x[16] + nom_x[17];
-   out_8136490426412747881[18] = delta_x[17] + nom_x[18];
-   out_8136490426412747881[19] = delta_x[18] + nom_x[19];
-   out_8136490426412747881[20] = delta_x[19] + nom_x[20];
-   out_8136490426412747881[21] = delta_x[20] + nom_x[21];
+void err_fun(double *nom_x, double *delta_x, double *out_403017012035771763) {
+   out_403017012035771763[0] = delta_x[0] + nom_x[0];
+   out_403017012035771763[1] = delta_x[1] + nom_x[1];
+   out_403017012035771763[2] = delta_x[2] + nom_x[2];
+   out_403017012035771763[3] = -0.5*delta_x[3]*nom_x[4] - 0.5*delta_x[4]*nom_x[5] - 0.5*delta_x[5]*nom_x[6] + 1.0*nom_x[3];
+   out_403017012035771763[4] = 0.5*delta_x[3]*nom_x[3] + 0.5*delta_x[4]*nom_x[6] - 0.5*delta_x[5]*nom_x[5] + 1.0*nom_x[4];
+   out_403017012035771763[5] = -0.5*delta_x[3]*nom_x[6] + 0.5*delta_x[4]*nom_x[3] + 0.5*delta_x[5]*nom_x[4] + 1.0*nom_x[5];
+   out_403017012035771763[6] = 0.5*delta_x[3]*nom_x[5] - 0.5*delta_x[4]*nom_x[4] + 0.5*delta_x[5]*nom_x[3] + 1.0*nom_x[6];
+   out_403017012035771763[7] = delta_x[6] + nom_x[7];
+   out_403017012035771763[8] = delta_x[7] + nom_x[8];
+   out_403017012035771763[9] = delta_x[8] + nom_x[9];
+   out_403017012035771763[10] = delta_x[9] + nom_x[10];
+   out_403017012035771763[11] = delta_x[10] + nom_x[11];
+   out_403017012035771763[12] = delta_x[11] + nom_x[12];
+   out_403017012035771763[13] = delta_x[12] + nom_x[13];
+   out_403017012035771763[14] = delta_x[13] + nom_x[14];
+   out_403017012035771763[15] = delta_x[14] + nom_x[15];
+   out_403017012035771763[16] = delta_x[15] + nom_x[16];
+   out_403017012035771763[17] = delta_x[16] + nom_x[17];
+   out_403017012035771763[18] = delta_x[17] + nom_x[18];
+   out_403017012035771763[19] = delta_x[18] + nom_x[19];
+   out_403017012035771763[20] = delta_x[19] + nom_x[20];
+   out_403017012035771763[21] = delta_x[20] + nom_x[21];
 }
-void inv_err_fun(double *nom_x, double *true_x, double *out_1524655933590429885) {
-   out_1524655933590429885[0] = -nom_x[0] + true_x[0];
-   out_1524655933590429885[1] = -nom_x[1] + true_x[1];
-   out_1524655933590429885[2] = -nom_x[2] + true_x[2];
-   out_1524655933590429885[3] = 2*nom_x[3]*true_x[4] - 2*nom_x[4]*true_x[3] + 2*nom_x[5]*true_x[6] - 2*nom_x[6]*true_x[5];
-   out_1524655933590429885[4] = 2*nom_x[3]*true_x[5] - 2*nom_x[4]*true_x[6] - 2*nom_x[5]*true_x[3] + 2*nom_x[6]*true_x[4];
-   out_1524655933590429885[5] = 2*nom_x[3]*true_x[6] + 2*nom_x[4]*true_x[5] - 2*nom_x[5]*true_x[4] - 2*nom_x[6]*true_x[3];
-   out_1524655933590429885[6] = -nom_x[7] + true_x[7];
-   out_1524655933590429885[7] = -nom_x[8] + true_x[8];
-   out_1524655933590429885[8] = -nom_x[9] + true_x[9];
-   out_1524655933590429885[9] = -nom_x[10] + true_x[10];
-   out_1524655933590429885[10] = -nom_x[11] + true_x[11];
-   out_1524655933590429885[11] = -nom_x[12] + true_x[12];
-   out_1524655933590429885[12] = -nom_x[13] + true_x[13];
-   out_1524655933590429885[13] = -nom_x[14] + true_x[14];
-   out_1524655933590429885[14] = -nom_x[15] + true_x[15];
-   out_1524655933590429885[15] = -nom_x[16] + true_x[16];
-   out_1524655933590429885[16] = -nom_x[17] + true_x[17];
-   out_1524655933590429885[17] = -nom_x[18] + true_x[18];
-   out_1524655933590429885[18] = -nom_x[19] + true_x[19];
-   out_1524655933590429885[19] = -nom_x[20] + true_x[20];
-   out_1524655933590429885[20] = -nom_x[21] + true_x[21];
+void inv_err_fun(double *nom_x, double *true_x, double *out_2932713747370631150) {
+   out_2932713747370631150[0] = -nom_x[0] + true_x[0];
+   out_2932713747370631150[1] = -nom_x[1] + true_x[1];
+   out_2932713747370631150[2] = -nom_x[2] + true_x[2];
+   out_2932713747370631150[3] = 2*nom_x[3]*true_x[4] - 2*nom_x[4]*true_x[3] + 2*nom_x[5]*true_x[6] - 2*nom_x[6]*true_x[5];
+   out_2932713747370631150[4] = 2*nom_x[3]*true_x[5] - 2*nom_x[4]*true_x[6] - 2*nom_x[5]*true_x[3] + 2*nom_x[6]*true_x[4];
+   out_2932713747370631150[5] = 2*nom_x[3]*true_x[6] + 2*nom_x[4]*true_x[5] - 2*nom_x[5]*true_x[4] - 2*nom_x[6]*true_x[3];
+   out_2932713747370631150[6] = -nom_x[7] + true_x[7];
+   out_2932713747370631150[7] = -nom_x[8] + true_x[8];
+   out_2932713747370631150[8] = -nom_x[9] + true_x[9];
+   out_2932713747370631150[9] = -nom_x[10] + true_x[10];
+   out_2932713747370631150[10] = -nom_x[11] + true_x[11];
+   out_2932713747370631150[11] = -nom_x[12] + true_x[12];
+   out_2932713747370631150[12] = -nom_x[13] + true_x[13];
+   out_2932713747370631150[13] = -nom_x[14] + true_x[14];
+   out_2932713747370631150[14] = -nom_x[15] + true_x[15];
+   out_2932713747370631150[15] = -nom_x[16] + true_x[16];
+   out_2932713747370631150[16] = -nom_x[17] + true_x[17];
+   out_2932713747370631150[17] = -nom_x[18] + true_x[18];
+   out_2932713747370631150[18] = -nom_x[19] + true_x[19];
+   out_2932713747370631150[19] = -nom_x[20] + true_x[20];
+   out_2932713747370631150[20] = -nom_x[21] + true_x[21];
 }
-void H_mod_fun(double *state, double *out_7453067886472422609) {
-   out_7453067886472422609[0] = 1.0;
-   out_7453067886472422609[1] = 0;
-   out_7453067886472422609[2] = 0;
-   out_7453067886472422609[3] = 0;
-   out_7453067886472422609[4] = 0;
-   out_7453067886472422609[5] = 0;
-   out_7453067886472422609[6] = 0;
-   out_7453067886472422609[7] = 0;
-   out_7453067886472422609[8] = 0;
-   out_7453067886472422609[9] = 0;
-   out_7453067886472422609[10] = 0;
-   out_7453067886472422609[11] = 0;
-   out_7453067886472422609[12] = 0;
-   out_7453067886472422609[13] = 0;
-   out_7453067886472422609[14] = 0;
-   out_7453067886472422609[15] = 0;
-   out_7453067886472422609[16] = 0;
-   out_7453067886472422609[17] = 0;
-   out_7453067886472422609[18] = 0;
-   out_7453067886472422609[19] = 0;
-   out_7453067886472422609[20] = 0;
-   out_7453067886472422609[21] = 0;
-   out_7453067886472422609[22] = 1.0;
-   out_7453067886472422609[23] = 0;
-   out_7453067886472422609[24] = 0;
-   out_7453067886472422609[25] = 0;
-   out_7453067886472422609[26] = 0;
-   out_7453067886472422609[27] = 0;
-   out_7453067886472422609[28] = 0;
-   out_7453067886472422609[29] = 0;
-   out_7453067886472422609[30] = 0;
-   out_7453067886472422609[31] = 0;
-   out_7453067886472422609[32] = 0;
-   out_7453067886472422609[33] = 0;
-   out_7453067886472422609[34] = 0;
-   out_7453067886472422609[35] = 0;
-   out_7453067886472422609[36] = 0;
-   out_7453067886472422609[37] = 0;
-   out_7453067886472422609[38] = 0;
-   out_7453067886472422609[39] = 0;
-   out_7453067886472422609[40] = 0;
-   out_7453067886472422609[41] = 0;
-   out_7453067886472422609[42] = 0;
-   out_7453067886472422609[43] = 0;
-   out_7453067886472422609[44] = 1.0;
-   out_7453067886472422609[45] = 0;
-   out_7453067886472422609[46] = 0;
-   out_7453067886472422609[47] = 0;
-   out_7453067886472422609[48] = 0;
-   out_7453067886472422609[49] = 0;
-   out_7453067886472422609[50] = 0;
-   out_7453067886472422609[51] = 0;
-   out_7453067886472422609[52] = 0;
-   out_7453067886472422609[53] = 0;
-   out_7453067886472422609[54] = 0;
-   out_7453067886472422609[55] = 0;
-   out_7453067886472422609[56] = 0;
-   out_7453067886472422609[57] = 0;
-   out_7453067886472422609[58] = 0;
-   out_7453067886472422609[59] = 0;
-   out_7453067886472422609[60] = 0;
-   out_7453067886472422609[61] = 0;
-   out_7453067886472422609[62] = 0;
-   out_7453067886472422609[63] = 0;
-   out_7453067886472422609[64] = 0;
-   out_7453067886472422609[65] = 0;
-   out_7453067886472422609[66] = -0.5*state[4];
-   out_7453067886472422609[67] = -0.5*state[5];
-   out_7453067886472422609[68] = -0.5*state[6];
-   out_7453067886472422609[69] = 0;
-   out_7453067886472422609[70] = 0;
-   out_7453067886472422609[71] = 0;
-   out_7453067886472422609[72] = 0;
-   out_7453067886472422609[73] = 0;
-   out_7453067886472422609[74] = 0;
-   out_7453067886472422609[75] = 0;
-   out_7453067886472422609[76] = 0;
-   out_7453067886472422609[77] = 0;
-   out_7453067886472422609[78] = 0;
-   out_7453067886472422609[79] = 0;
-   out_7453067886472422609[80] = 0;
-   out_7453067886472422609[81] = 0;
-   out_7453067886472422609[82] = 0;
-   out_7453067886472422609[83] = 0;
-   out_7453067886472422609[84] = 0;
-   out_7453067886472422609[85] = 0;
-   out_7453067886472422609[86] = 0;
-   out_7453067886472422609[87] = 0.5*state[3];
-   out_7453067886472422609[88] = 0.5*state[6];
-   out_7453067886472422609[89] = -0.5*state[5];
-   out_7453067886472422609[90] = 0;
-   out_7453067886472422609[91] = 0;
-   out_7453067886472422609[92] = 0;
-   out_7453067886472422609[93] = 0;
-   out_7453067886472422609[94] = 0;
-   out_7453067886472422609[95] = 0;
-   out_7453067886472422609[96] = 0;
-   out_7453067886472422609[97] = 0;
-   out_7453067886472422609[98] = 0;
-   out_7453067886472422609[99] = 0;
-   out_7453067886472422609[100] = 0;
-   out_7453067886472422609[101] = 0;
-   out_7453067886472422609[102] = 0;
-   out_7453067886472422609[103] = 0;
-   out_7453067886472422609[104] = 0;
-   out_7453067886472422609[105] = 0;
-   out_7453067886472422609[106] = 0;
-   out_7453067886472422609[107] = 0;
-   out_7453067886472422609[108] = -0.5*state[6];
-   out_7453067886472422609[109] = 0.5*state[3];
-   out_7453067886472422609[110] = 0.5*state[4];
-   out_7453067886472422609[111] = 0;
-   out_7453067886472422609[112] = 0;
-   out_7453067886472422609[113] = 0;
-   out_7453067886472422609[114] = 0;
-   out_7453067886472422609[115] = 0;
-   out_7453067886472422609[116] = 0;
-   out_7453067886472422609[117] = 0;
-   out_7453067886472422609[118] = 0;
-   out_7453067886472422609[119] = 0;
-   out_7453067886472422609[120] = 0;
-   out_7453067886472422609[121] = 0;
-   out_7453067886472422609[122] = 0;
-   out_7453067886472422609[123] = 0;
-   out_7453067886472422609[124] = 0;
-   out_7453067886472422609[125] = 0;
-   out_7453067886472422609[126] = 0;
-   out_7453067886472422609[127] = 0;
-   out_7453067886472422609[128] = 0;
-   out_7453067886472422609[129] = 0.5*state[5];
-   out_7453067886472422609[130] = -0.5*state[4];
-   out_7453067886472422609[131] = 0.5*state[3];
-   out_7453067886472422609[132] = 0;
-   out_7453067886472422609[133] = 0;
-   out_7453067886472422609[134] = 0;
-   out_7453067886472422609[135] = 0;
-   out_7453067886472422609[136] = 0;
-   out_7453067886472422609[137] = 0;
-   out_7453067886472422609[138] = 0;
-   out_7453067886472422609[139] = 0;
-   out_7453067886472422609[140] = 0;
-   out_7453067886472422609[141] = 0;
-   out_7453067886472422609[142] = 0;
-   out_7453067886472422609[143] = 0;
-   out_7453067886472422609[144] = 0;
-   out_7453067886472422609[145] = 0;
-   out_7453067886472422609[146] = 0;
-   out_7453067886472422609[147] = 0;
-   out_7453067886472422609[148] = 0;
-   out_7453067886472422609[149] = 0;
-   out_7453067886472422609[150] = 0;
-   out_7453067886472422609[151] = 0;
-   out_7453067886472422609[152] = 0;
-   out_7453067886472422609[153] = 1.0;
-   out_7453067886472422609[154] = 0;
-   out_7453067886472422609[155] = 0;
-   out_7453067886472422609[156] = 0;
-   out_7453067886472422609[157] = 0;
-   out_7453067886472422609[158] = 0;
-   out_7453067886472422609[159] = 0;
-   out_7453067886472422609[160] = 0;
-   out_7453067886472422609[161] = 0;
-   out_7453067886472422609[162] = 0;
-   out_7453067886472422609[163] = 0;
-   out_7453067886472422609[164] = 0;
-   out_7453067886472422609[165] = 0;
-   out_7453067886472422609[166] = 0;
-   out_7453067886472422609[167] = 0;
-   out_7453067886472422609[168] = 0;
-   out_7453067886472422609[169] = 0;
-   out_7453067886472422609[170] = 0;
-   out_7453067886472422609[171] = 0;
-   out_7453067886472422609[172] = 0;
-   out_7453067886472422609[173] = 0;
-   out_7453067886472422609[174] = 0;
-   out_7453067886472422609[175] = 1.0;
-   out_7453067886472422609[176] = 0;
-   out_7453067886472422609[177] = 0;
-   out_7453067886472422609[178] = 0;
-   out_7453067886472422609[179] = 0;
-   out_7453067886472422609[180] = 0;
-   out_7453067886472422609[181] = 0;
-   out_7453067886472422609[182] = 0;
-   out_7453067886472422609[183] = 0;
-   out_7453067886472422609[184] = 0;
-   out_7453067886472422609[185] = 0;
-   out_7453067886472422609[186] = 0;
-   out_7453067886472422609[187] = 0;
-   out_7453067886472422609[188] = 0;
-   out_7453067886472422609[189] = 0;
-   out_7453067886472422609[190] = 0;
-   out_7453067886472422609[191] = 0;
-   out_7453067886472422609[192] = 0;
-   out_7453067886472422609[193] = 0;
-   out_7453067886472422609[194] = 0;
-   out_7453067886472422609[195] = 0;
-   out_7453067886472422609[196] = 0;
-   out_7453067886472422609[197] = 1.0;
-   out_7453067886472422609[198] = 0;
-   out_7453067886472422609[199] = 0;
-   out_7453067886472422609[200] = 0;
-   out_7453067886472422609[201] = 0;
-   out_7453067886472422609[202] = 0;
-   out_7453067886472422609[203] = 0;
-   out_7453067886472422609[204] = 0;
-   out_7453067886472422609[205] = 0;
-   out_7453067886472422609[206] = 0;
-   out_7453067886472422609[207] = 0;
-   out_7453067886472422609[208] = 0;
-   out_7453067886472422609[209] = 0;
-   out_7453067886472422609[210] = 0;
-   out_7453067886472422609[211] = 0;
-   out_7453067886472422609[212] = 0;
-   out_7453067886472422609[213] = 0;
-   out_7453067886472422609[214] = 0;
-   out_7453067886472422609[215] = 0;
-   out_7453067886472422609[216] = 0;
-   out_7453067886472422609[217] = 0;
-   out_7453067886472422609[218] = 0;
-   out_7453067886472422609[219] = 1.0;
-   out_7453067886472422609[220] = 0;
-   out_7453067886472422609[221] = 0;
-   out_7453067886472422609[222] = 0;
-   out_7453067886472422609[223] = 0;
-   out_7453067886472422609[224] = 0;
-   out_7453067886472422609[225] = 0;
-   out_7453067886472422609[226] = 0;
-   out_7453067886472422609[227] = 0;
-   out_7453067886472422609[228] = 0;
-   out_7453067886472422609[229] = 0;
-   out_7453067886472422609[230] = 0;
-   out_7453067886472422609[231] = 0;
-   out_7453067886472422609[232] = 0;
-   out_7453067886472422609[233] = 0;
-   out_7453067886472422609[234] = 0;
-   out_7453067886472422609[235] = 0;
-   out_7453067886472422609[236] = 0;
-   out_7453067886472422609[237] = 0;
-   out_7453067886472422609[238] = 0;
-   out_7453067886472422609[239] = 0;
-   out_7453067886472422609[240] = 0;
-   out_7453067886472422609[241] = 1.0;
-   out_7453067886472422609[242] = 0;
-   out_7453067886472422609[243] = 0;
-   out_7453067886472422609[244] = 0;
-   out_7453067886472422609[245] = 0;
-   out_7453067886472422609[246] = 0;
-   out_7453067886472422609[247] = 0;
-   out_7453067886472422609[248] = 0;
-   out_7453067886472422609[249] = 0;
-   out_7453067886472422609[250] = 0;
-   out_7453067886472422609[251] = 0;
-   out_7453067886472422609[252] = 0;
-   out_7453067886472422609[253] = 0;
-   out_7453067886472422609[254] = 0;
-   out_7453067886472422609[255] = 0;
-   out_7453067886472422609[256] = 0;
-   out_7453067886472422609[257] = 0;
-   out_7453067886472422609[258] = 0;
-   out_7453067886472422609[259] = 0;
-   out_7453067886472422609[260] = 0;
-   out_7453067886472422609[261] = 0;
-   out_7453067886472422609[262] = 0;
-   out_7453067886472422609[263] = 1.0;
-   out_7453067886472422609[264] = 0;
-   out_7453067886472422609[265] = 0;
-   out_7453067886472422609[266] = 0;
-   out_7453067886472422609[267] = 0;
-   out_7453067886472422609[268] = 0;
-   out_7453067886472422609[269] = 0;
-   out_7453067886472422609[270] = 0;
-   out_7453067886472422609[271] = 0;
-   out_7453067886472422609[272] = 0;
-   out_7453067886472422609[273] = 0;
-   out_7453067886472422609[274] = 0;
-   out_7453067886472422609[275] = 0;
-   out_7453067886472422609[276] = 0;
-   out_7453067886472422609[277] = 0;
-   out_7453067886472422609[278] = 0;
-   out_7453067886472422609[279] = 0;
-   out_7453067886472422609[280] = 0;
-   out_7453067886472422609[281] = 0;
-   out_7453067886472422609[282] = 0;
-   out_7453067886472422609[283] = 0;
-   out_7453067886472422609[284] = 0;
-   out_7453067886472422609[285] = 1.0;
-   out_7453067886472422609[286] = 0;
-   out_7453067886472422609[287] = 0;
-   out_7453067886472422609[288] = 0;
-   out_7453067886472422609[289] = 0;
-   out_7453067886472422609[290] = 0;
-   out_7453067886472422609[291] = 0;
-   out_7453067886472422609[292] = 0;
-   out_7453067886472422609[293] = 0;
-   out_7453067886472422609[294] = 0;
-   out_7453067886472422609[295] = 0;
-   out_7453067886472422609[296] = 0;
-   out_7453067886472422609[297] = 0;
-   out_7453067886472422609[298] = 0;
-   out_7453067886472422609[299] = 0;
-   out_7453067886472422609[300] = 0;
-   out_7453067886472422609[301] = 0;
-   out_7453067886472422609[302] = 0;
-   out_7453067886472422609[303] = 0;
-   out_7453067886472422609[304] = 0;
-   out_7453067886472422609[305] = 0;
-   out_7453067886472422609[306] = 0;
-   out_7453067886472422609[307] = 1.0;
-   out_7453067886472422609[308] = 0;
-   out_7453067886472422609[309] = 0;
-   out_7453067886472422609[310] = 0;
-   out_7453067886472422609[311] = 0;
-   out_7453067886472422609[312] = 0;
-   out_7453067886472422609[313] = 0;
-   out_7453067886472422609[314] = 0;
-   out_7453067886472422609[315] = 0;
-   out_7453067886472422609[316] = 0;
-   out_7453067886472422609[317] = 0;
-   out_7453067886472422609[318] = 0;
-   out_7453067886472422609[319] = 0;
-   out_7453067886472422609[320] = 0;
-   out_7453067886472422609[321] = 0;
-   out_7453067886472422609[322] = 0;
-   out_7453067886472422609[323] = 0;
-   out_7453067886472422609[324] = 0;
-   out_7453067886472422609[325] = 0;
-   out_7453067886472422609[326] = 0;
-   out_7453067886472422609[327] = 0;
-   out_7453067886472422609[328] = 0;
-   out_7453067886472422609[329] = 1.0;
-   out_7453067886472422609[330] = 0;
-   out_7453067886472422609[331] = 0;
-   out_7453067886472422609[332] = 0;
-   out_7453067886472422609[333] = 0;
-   out_7453067886472422609[334] = 0;
-   out_7453067886472422609[335] = 0;
-   out_7453067886472422609[336] = 0;
-   out_7453067886472422609[337] = 0;
-   out_7453067886472422609[338] = 0;
-   out_7453067886472422609[339] = 0;
-   out_7453067886472422609[340] = 0;
-   out_7453067886472422609[341] = 0;
-   out_7453067886472422609[342] = 0;
-   out_7453067886472422609[343] = 0;
-   out_7453067886472422609[344] = 0;
-   out_7453067886472422609[345] = 0;
-   out_7453067886472422609[346] = 0;
-   out_7453067886472422609[347] = 0;
-   out_7453067886472422609[348] = 0;
-   out_7453067886472422609[349] = 0;
-   out_7453067886472422609[350] = 0;
-   out_7453067886472422609[351] = 1.0;
-   out_7453067886472422609[352] = 0;
-   out_7453067886472422609[353] = 0;
-   out_7453067886472422609[354] = 0;
-   out_7453067886472422609[355] = 0;
-   out_7453067886472422609[356] = 0;
-   out_7453067886472422609[357] = 0;
-   out_7453067886472422609[358] = 0;
-   out_7453067886472422609[359] = 0;
-   out_7453067886472422609[360] = 0;
-   out_7453067886472422609[361] = 0;
-   out_7453067886472422609[362] = 0;
-   out_7453067886472422609[363] = 0;
-   out_7453067886472422609[364] = 0;
-   out_7453067886472422609[365] = 0;
-   out_7453067886472422609[366] = 0;
-   out_7453067886472422609[367] = 0;
-   out_7453067886472422609[368] = 0;
-   out_7453067886472422609[369] = 0;
-   out_7453067886472422609[370] = 0;
-   out_7453067886472422609[371] = 0;
-   out_7453067886472422609[372] = 0;
-   out_7453067886472422609[373] = 1.0;
-   out_7453067886472422609[374] = 0;
-   out_7453067886472422609[375] = 0;
-   out_7453067886472422609[376] = 0;
-   out_7453067886472422609[377] = 0;
-   out_7453067886472422609[378] = 0;
-   out_7453067886472422609[379] = 0;
-   out_7453067886472422609[380] = 0;
-   out_7453067886472422609[381] = 0;
-   out_7453067886472422609[382] = 0;
-   out_7453067886472422609[383] = 0;
-   out_7453067886472422609[384] = 0;
-   out_7453067886472422609[385] = 0;
-   out_7453067886472422609[386] = 0;
-   out_7453067886472422609[387] = 0;
-   out_7453067886472422609[388] = 0;
-   out_7453067886472422609[389] = 0;
-   out_7453067886472422609[390] = 0;
-   out_7453067886472422609[391] = 0;
-   out_7453067886472422609[392] = 0;
-   out_7453067886472422609[393] = 0;
-   out_7453067886472422609[394] = 0;
-   out_7453067886472422609[395] = 1.0;
-   out_7453067886472422609[396] = 0;
-   out_7453067886472422609[397] = 0;
-   out_7453067886472422609[398] = 0;
-   out_7453067886472422609[399] = 0;
-   out_7453067886472422609[400] = 0;
-   out_7453067886472422609[401] = 0;
-   out_7453067886472422609[402] = 0;
-   out_7453067886472422609[403] = 0;
-   out_7453067886472422609[404] = 0;
-   out_7453067886472422609[405] = 0;
-   out_7453067886472422609[406] = 0;
-   out_7453067886472422609[407] = 0;
-   out_7453067886472422609[408] = 0;
-   out_7453067886472422609[409] = 0;
-   out_7453067886472422609[410] = 0;
-   out_7453067886472422609[411] = 0;
-   out_7453067886472422609[412] = 0;
-   out_7453067886472422609[413] = 0;
-   out_7453067886472422609[414] = 0;
-   out_7453067886472422609[415] = 0;
-   out_7453067886472422609[416] = 0;
-   out_7453067886472422609[417] = 1.0;
-   out_7453067886472422609[418] = 0;
-   out_7453067886472422609[419] = 0;
-   out_7453067886472422609[420] = 0;
-   out_7453067886472422609[421] = 0;
-   out_7453067886472422609[422] = 0;
-   out_7453067886472422609[423] = 0;
-   out_7453067886472422609[424] = 0;
-   out_7453067886472422609[425] = 0;
-   out_7453067886472422609[426] = 0;
-   out_7453067886472422609[427] = 0;
-   out_7453067886472422609[428] = 0;
-   out_7453067886472422609[429] = 0;
-   out_7453067886472422609[430] = 0;
-   out_7453067886472422609[431] = 0;
-   out_7453067886472422609[432] = 0;
-   out_7453067886472422609[433] = 0;
-   out_7453067886472422609[434] = 0;
-   out_7453067886472422609[435] = 0;
-   out_7453067886472422609[436] = 0;
-   out_7453067886472422609[437] = 0;
-   out_7453067886472422609[438] = 0;
-   out_7453067886472422609[439] = 1.0;
-   out_7453067886472422609[440] = 0;
-   out_7453067886472422609[441] = 0;
-   out_7453067886472422609[442] = 0;
-   out_7453067886472422609[443] = 0;
-   out_7453067886472422609[444] = 0;
-   out_7453067886472422609[445] = 0;
-   out_7453067886472422609[446] = 0;
-   out_7453067886472422609[447] = 0;
-   out_7453067886472422609[448] = 0;
-   out_7453067886472422609[449] = 0;
-   out_7453067886472422609[450] = 0;
-   out_7453067886472422609[451] = 0;
-   out_7453067886472422609[452] = 0;
-   out_7453067886472422609[453] = 0;
-   out_7453067886472422609[454] = 0;
-   out_7453067886472422609[455] = 0;
-   out_7453067886472422609[456] = 0;
-   out_7453067886472422609[457] = 0;
-   out_7453067886472422609[458] = 0;
-   out_7453067886472422609[459] = 0;
-   out_7453067886472422609[460] = 0;
-   out_7453067886472422609[461] = 1.0;
+void H_mod_fun(double *state, double *out_6036032580712144890) {
+   out_6036032580712144890[0] = 1.0;
+   out_6036032580712144890[1] = 0;
+   out_6036032580712144890[2] = 0;
+   out_6036032580712144890[3] = 0;
+   out_6036032580712144890[4] = 0;
+   out_6036032580712144890[5] = 0;
+   out_6036032580712144890[6] = 0;
+   out_6036032580712144890[7] = 0;
+   out_6036032580712144890[8] = 0;
+   out_6036032580712144890[9] = 0;
+   out_6036032580712144890[10] = 0;
+   out_6036032580712144890[11] = 0;
+   out_6036032580712144890[12] = 0;
+   out_6036032580712144890[13] = 0;
+   out_6036032580712144890[14] = 0;
+   out_6036032580712144890[15] = 0;
+   out_6036032580712144890[16] = 0;
+   out_6036032580712144890[17] = 0;
+   out_6036032580712144890[18] = 0;
+   out_6036032580712144890[19] = 0;
+   out_6036032580712144890[20] = 0;
+   out_6036032580712144890[21] = 0;
+   out_6036032580712144890[22] = 1.0;
+   out_6036032580712144890[23] = 0;
+   out_6036032580712144890[24] = 0;
+   out_6036032580712144890[25] = 0;
+   out_6036032580712144890[26] = 0;
+   out_6036032580712144890[27] = 0;
+   out_6036032580712144890[28] = 0;
+   out_6036032580712144890[29] = 0;
+   out_6036032580712144890[30] = 0;
+   out_6036032580712144890[31] = 0;
+   out_6036032580712144890[32] = 0;
+   out_6036032580712144890[33] = 0;
+   out_6036032580712144890[34] = 0;
+   out_6036032580712144890[35] = 0;
+   out_6036032580712144890[36] = 0;
+   out_6036032580712144890[37] = 0;
+   out_6036032580712144890[38] = 0;
+   out_6036032580712144890[39] = 0;
+   out_6036032580712144890[40] = 0;
+   out_6036032580712144890[41] = 0;
+   out_6036032580712144890[42] = 0;
+   out_6036032580712144890[43] = 0;
+   out_6036032580712144890[44] = 1.0;
+   out_6036032580712144890[45] = 0;
+   out_6036032580712144890[46] = 0;
+   out_6036032580712144890[47] = 0;
+   out_6036032580712144890[48] = 0;
+   out_6036032580712144890[49] = 0;
+   out_6036032580712144890[50] = 0;
+   out_6036032580712144890[51] = 0;
+   out_6036032580712144890[52] = 0;
+   out_6036032580712144890[53] = 0;
+   out_6036032580712144890[54] = 0;
+   out_6036032580712144890[55] = 0;
+   out_6036032580712144890[56] = 0;
+   out_6036032580712144890[57] = 0;
+   out_6036032580712144890[58] = 0;
+   out_6036032580712144890[59] = 0;
+   out_6036032580712144890[60] = 0;
+   out_6036032580712144890[61] = 0;
+   out_6036032580712144890[62] = 0;
+   out_6036032580712144890[63] = 0;
+   out_6036032580712144890[64] = 0;
+   out_6036032580712144890[65] = 0;
+   out_6036032580712144890[66] = -0.5*state[4];
+   out_6036032580712144890[67] = -0.5*state[5];
+   out_6036032580712144890[68] = -0.5*state[6];
+   out_6036032580712144890[69] = 0;
+   out_6036032580712144890[70] = 0;
+   out_6036032580712144890[71] = 0;
+   out_6036032580712144890[72] = 0;
+   out_6036032580712144890[73] = 0;
+   out_6036032580712144890[74] = 0;
+   out_6036032580712144890[75] = 0;
+   out_6036032580712144890[76] = 0;
+   out_6036032580712144890[77] = 0;
+   out_6036032580712144890[78] = 0;
+   out_6036032580712144890[79] = 0;
+   out_6036032580712144890[80] = 0;
+   out_6036032580712144890[81] = 0;
+   out_6036032580712144890[82] = 0;
+   out_6036032580712144890[83] = 0;
+   out_6036032580712144890[84] = 0;
+   out_6036032580712144890[85] = 0;
+   out_6036032580712144890[86] = 0;
+   out_6036032580712144890[87] = 0.5*state[3];
+   out_6036032580712144890[88] = 0.5*state[6];
+   out_6036032580712144890[89] = -0.5*state[5];
+   out_6036032580712144890[90] = 0;
+   out_6036032580712144890[91] = 0;
+   out_6036032580712144890[92] = 0;
+   out_6036032580712144890[93] = 0;
+   out_6036032580712144890[94] = 0;
+   out_6036032580712144890[95] = 0;
+   out_6036032580712144890[96] = 0;
+   out_6036032580712144890[97] = 0;
+   out_6036032580712144890[98] = 0;
+   out_6036032580712144890[99] = 0;
+   out_6036032580712144890[100] = 0;
+   out_6036032580712144890[101] = 0;
+   out_6036032580712144890[102] = 0;
+   out_6036032580712144890[103] = 0;
+   out_6036032580712144890[104] = 0;
+   out_6036032580712144890[105] = 0;
+   out_6036032580712144890[106] = 0;
+   out_6036032580712144890[107] = 0;
+   out_6036032580712144890[108] = -0.5*state[6];
+   out_6036032580712144890[109] = 0.5*state[3];
+   out_6036032580712144890[110] = 0.5*state[4];
+   out_6036032580712144890[111] = 0;
+   out_6036032580712144890[112] = 0;
+   out_6036032580712144890[113] = 0;
+   out_6036032580712144890[114] = 0;
+   out_6036032580712144890[115] = 0;
+   out_6036032580712144890[116] = 0;
+   out_6036032580712144890[117] = 0;
+   out_6036032580712144890[118] = 0;
+   out_6036032580712144890[119] = 0;
+   out_6036032580712144890[120] = 0;
+   out_6036032580712144890[121] = 0;
+   out_6036032580712144890[122] = 0;
+   out_6036032580712144890[123] = 0;
+   out_6036032580712144890[124] = 0;
+   out_6036032580712144890[125] = 0;
+   out_6036032580712144890[126] = 0;
+   out_6036032580712144890[127] = 0;
+   out_6036032580712144890[128] = 0;
+   out_6036032580712144890[129] = 0.5*state[5];
+   out_6036032580712144890[130] = -0.5*state[4];
+   out_6036032580712144890[131] = 0.5*state[3];
+   out_6036032580712144890[132] = 0;
+   out_6036032580712144890[133] = 0;
+   out_6036032580712144890[134] = 0;
+   out_6036032580712144890[135] = 0;
+   out_6036032580712144890[136] = 0;
+   out_6036032580712144890[137] = 0;
+   out_6036032580712144890[138] = 0;
+   out_6036032580712144890[139] = 0;
+   out_6036032580712144890[140] = 0;
+   out_6036032580712144890[141] = 0;
+   out_6036032580712144890[142] = 0;
+   out_6036032580712144890[143] = 0;
+   out_6036032580712144890[144] = 0;
+   out_6036032580712144890[145] = 0;
+   out_6036032580712144890[146] = 0;
+   out_6036032580712144890[147] = 0;
+   out_6036032580712144890[148] = 0;
+   out_6036032580712144890[149] = 0;
+   out_6036032580712144890[150] = 0;
+   out_6036032580712144890[151] = 0;
+   out_6036032580712144890[152] = 0;
+   out_6036032580712144890[153] = 1.0;
+   out_6036032580712144890[154] = 0;
+   out_6036032580712144890[155] = 0;
+   out_6036032580712144890[156] = 0;
+   out_6036032580712144890[157] = 0;
+   out_6036032580712144890[158] = 0;
+   out_6036032580712144890[159] = 0;
+   out_6036032580712144890[160] = 0;
+   out_6036032580712144890[161] = 0;
+   out_6036032580712144890[162] = 0;
+   out_6036032580712144890[163] = 0;
+   out_6036032580712144890[164] = 0;
+   out_6036032580712144890[165] = 0;
+   out_6036032580712144890[166] = 0;
+   out_6036032580712144890[167] = 0;
+   out_6036032580712144890[168] = 0;
+   out_6036032580712144890[169] = 0;
+   out_6036032580712144890[170] = 0;
+   out_6036032580712144890[171] = 0;
+   out_6036032580712144890[172] = 0;
+   out_6036032580712144890[173] = 0;
+   out_6036032580712144890[174] = 0;
+   out_6036032580712144890[175] = 1.0;
+   out_6036032580712144890[176] = 0;
+   out_6036032580712144890[177] = 0;
+   out_6036032580712144890[178] = 0;
+   out_6036032580712144890[179] = 0;
+   out_6036032580712144890[180] = 0;
+   out_6036032580712144890[181] = 0;
+   out_6036032580712144890[182] = 0;
+   out_6036032580712144890[183] = 0;
+   out_6036032580712144890[184] = 0;
+   out_6036032580712144890[185] = 0;
+   out_6036032580712144890[186] = 0;
+   out_6036032580712144890[187] = 0;
+   out_6036032580712144890[188] = 0;
+   out_6036032580712144890[189] = 0;
+   out_6036032580712144890[190] = 0;
+   out_6036032580712144890[191] = 0;
+   out_6036032580712144890[192] = 0;
+   out_6036032580712144890[193] = 0;
+   out_6036032580712144890[194] = 0;
+   out_6036032580712144890[195] = 0;
+   out_6036032580712144890[196] = 0;
+   out_6036032580712144890[197] = 1.0;
+   out_6036032580712144890[198] = 0;
+   out_6036032580712144890[199] = 0;
+   out_6036032580712144890[200] = 0;
+   out_6036032580712144890[201] = 0;
+   out_6036032580712144890[202] = 0;
+   out_6036032580712144890[203] = 0;
+   out_6036032580712144890[204] = 0;
+   out_6036032580712144890[205] = 0;
+   out_6036032580712144890[206] = 0;
+   out_6036032580712144890[207] = 0;
+   out_6036032580712144890[208] = 0;
+   out_6036032580712144890[209] = 0;
+   out_6036032580712144890[210] = 0;
+   out_6036032580712144890[211] = 0;
+   out_6036032580712144890[212] = 0;
+   out_6036032580712144890[213] = 0;
+   out_6036032580712144890[214] = 0;
+   out_6036032580712144890[215] = 0;
+   out_6036032580712144890[216] = 0;
+   out_6036032580712144890[217] = 0;
+   out_6036032580712144890[218] = 0;
+   out_6036032580712144890[219] = 1.0;
+   out_6036032580712144890[220] = 0;
+   out_6036032580712144890[221] = 0;
+   out_6036032580712144890[222] = 0;
+   out_6036032580712144890[223] = 0;
+   out_6036032580712144890[224] = 0;
+   out_6036032580712144890[225] = 0;
+   out_6036032580712144890[226] = 0;
+   out_6036032580712144890[227] = 0;
+   out_6036032580712144890[228] = 0;
+   out_6036032580712144890[229] = 0;
+   out_6036032580712144890[230] = 0;
+   out_6036032580712144890[231] = 0;
+   out_6036032580712144890[232] = 0;
+   out_6036032580712144890[233] = 0;
+   out_6036032580712144890[234] = 0;
+   out_6036032580712144890[235] = 0;
+   out_6036032580712144890[236] = 0;
+   out_6036032580712144890[237] = 0;
+   out_6036032580712144890[238] = 0;
+   out_6036032580712144890[239] = 0;
+   out_6036032580712144890[240] = 0;
+   out_6036032580712144890[241] = 1.0;
+   out_6036032580712144890[242] = 0;
+   out_6036032580712144890[243] = 0;
+   out_6036032580712144890[244] = 0;
+   out_6036032580712144890[245] = 0;
+   out_6036032580712144890[246] = 0;
+   out_6036032580712144890[247] = 0;
+   out_6036032580712144890[248] = 0;
+   out_6036032580712144890[249] = 0;
+   out_6036032580712144890[250] = 0;
+   out_6036032580712144890[251] = 0;
+   out_6036032580712144890[252] = 0;
+   out_6036032580712144890[253] = 0;
+   out_6036032580712144890[254] = 0;
+   out_6036032580712144890[255] = 0;
+   out_6036032580712144890[256] = 0;
+   out_6036032580712144890[257] = 0;
+   out_6036032580712144890[258] = 0;
+   out_6036032580712144890[259] = 0;
+   out_6036032580712144890[260] = 0;
+   out_6036032580712144890[261] = 0;
+   out_6036032580712144890[262] = 0;
+   out_6036032580712144890[263] = 1.0;
+   out_6036032580712144890[264] = 0;
+   out_6036032580712144890[265] = 0;
+   out_6036032580712144890[266] = 0;
+   out_6036032580712144890[267] = 0;
+   out_6036032580712144890[268] = 0;
+   out_6036032580712144890[269] = 0;
+   out_6036032580712144890[270] = 0;
+   out_6036032580712144890[271] = 0;
+   out_6036032580712144890[272] = 0;
+   out_6036032580712144890[273] = 0;
+   out_6036032580712144890[274] = 0;
+   out_6036032580712144890[275] = 0;
+   out_6036032580712144890[276] = 0;
+   out_6036032580712144890[277] = 0;
+   out_6036032580712144890[278] = 0;
+   out_6036032580712144890[279] = 0;
+   out_6036032580712144890[280] = 0;
+   out_6036032580712144890[281] = 0;
+   out_6036032580712144890[282] = 0;
+   out_6036032580712144890[283] = 0;
+   out_6036032580712144890[284] = 0;
+   out_6036032580712144890[285] = 1.0;
+   out_6036032580712144890[286] = 0;
+   out_6036032580712144890[287] = 0;
+   out_6036032580712144890[288] = 0;
+   out_6036032580712144890[289] = 0;
+   out_6036032580712144890[290] = 0;
+   out_6036032580712144890[291] = 0;
+   out_6036032580712144890[292] = 0;
+   out_6036032580712144890[293] = 0;
+   out_6036032580712144890[294] = 0;
+   out_6036032580712144890[295] = 0;
+   out_6036032580712144890[296] = 0;
+   out_6036032580712144890[297] = 0;
+   out_6036032580712144890[298] = 0;
+   out_6036032580712144890[299] = 0;
+   out_6036032580712144890[300] = 0;
+   out_6036032580712144890[301] = 0;
+   out_6036032580712144890[302] = 0;
+   out_6036032580712144890[303] = 0;
+   out_6036032580712144890[304] = 0;
+   out_6036032580712144890[305] = 0;
+   out_6036032580712144890[306] = 0;
+   out_6036032580712144890[307] = 1.0;
+   out_6036032580712144890[308] = 0;
+   out_6036032580712144890[309] = 0;
+   out_6036032580712144890[310] = 0;
+   out_6036032580712144890[311] = 0;
+   out_6036032580712144890[312] = 0;
+   out_6036032580712144890[313] = 0;
+   out_6036032580712144890[314] = 0;
+   out_6036032580712144890[315] = 0;
+   out_6036032580712144890[316] = 0;
+   out_6036032580712144890[317] = 0;
+   out_6036032580712144890[318] = 0;
+   out_6036032580712144890[319] = 0;
+   out_6036032580712144890[320] = 0;
+   out_6036032580712144890[321] = 0;
+   out_6036032580712144890[322] = 0;
+   out_6036032580712144890[323] = 0;
+   out_6036032580712144890[324] = 0;
+   out_6036032580712144890[325] = 0;
+   out_6036032580712144890[326] = 0;
+   out_6036032580712144890[327] = 0;
+   out_6036032580712144890[328] = 0;
+   out_6036032580712144890[329] = 1.0;
+   out_6036032580712144890[330] = 0;
+   out_6036032580712144890[331] = 0;
+   out_6036032580712144890[332] = 0;
+   out_6036032580712144890[333] = 0;
+   out_6036032580712144890[334] = 0;
+   out_6036032580712144890[335] = 0;
+   out_6036032580712144890[336] = 0;
+   out_6036032580712144890[337] = 0;
+   out_6036032580712144890[338] = 0;
+   out_6036032580712144890[339] = 0;
+   out_6036032580712144890[340] = 0;
+   out_6036032580712144890[341] = 0;
+   out_6036032580712144890[342] = 0;
+   out_6036032580712144890[343] = 0;
+   out_6036032580712144890[344] = 0;
+   out_6036032580712144890[345] = 0;
+   out_6036032580712144890[346] = 0;
+   out_6036032580712144890[347] = 0;
+   out_6036032580712144890[348] = 0;
+   out_6036032580712144890[349] = 0;
+   out_6036032580712144890[350] = 0;
+   out_6036032580712144890[351] = 1.0;
+   out_6036032580712144890[352] = 0;
+   out_6036032580712144890[353] = 0;
+   out_6036032580712144890[354] = 0;
+   out_6036032580712144890[355] = 0;
+   out_6036032580712144890[356] = 0;
+   out_6036032580712144890[357] = 0;
+   out_6036032580712144890[358] = 0;
+   out_6036032580712144890[359] = 0;
+   out_6036032580712144890[360] = 0;
+   out_6036032580712144890[361] = 0;
+   out_6036032580712144890[362] = 0;
+   out_6036032580712144890[363] = 0;
+   out_6036032580712144890[364] = 0;
+   out_6036032580712144890[365] = 0;
+   out_6036032580712144890[366] = 0;
+   out_6036032580712144890[367] = 0;
+   out_6036032580712144890[368] = 0;
+   out_6036032580712144890[369] = 0;
+   out_6036032580712144890[370] = 0;
+   out_6036032580712144890[371] = 0;
+   out_6036032580712144890[372] = 0;
+   out_6036032580712144890[373] = 1.0;
+   out_6036032580712144890[374] = 0;
+   out_6036032580712144890[375] = 0;
+   out_6036032580712144890[376] = 0;
+   out_6036032580712144890[377] = 0;
+   out_6036032580712144890[378] = 0;
+   out_6036032580712144890[379] = 0;
+   out_6036032580712144890[380] = 0;
+   out_6036032580712144890[381] = 0;
+   out_6036032580712144890[382] = 0;
+   out_6036032580712144890[383] = 0;
+   out_6036032580712144890[384] = 0;
+   out_6036032580712144890[385] = 0;
+   out_6036032580712144890[386] = 0;
+   out_6036032580712144890[387] = 0;
+   out_6036032580712144890[388] = 0;
+   out_6036032580712144890[389] = 0;
+   out_6036032580712144890[390] = 0;
+   out_6036032580712144890[391] = 0;
+   out_6036032580712144890[392] = 0;
+   out_6036032580712144890[393] = 0;
+   out_6036032580712144890[394] = 0;
+   out_6036032580712144890[395] = 1.0;
+   out_6036032580712144890[396] = 0;
+   out_6036032580712144890[397] = 0;
+   out_6036032580712144890[398] = 0;
+   out_6036032580712144890[399] = 0;
+   out_6036032580712144890[400] = 0;
+   out_6036032580712144890[401] = 0;
+   out_6036032580712144890[402] = 0;
+   out_6036032580712144890[403] = 0;
+   out_6036032580712144890[404] = 0;
+   out_6036032580712144890[405] = 0;
+   out_6036032580712144890[406] = 0;
+   out_6036032580712144890[407] = 0;
+   out_6036032580712144890[408] = 0;
+   out_6036032580712144890[409] = 0;
+   out_6036032580712144890[410] = 0;
+   out_6036032580712144890[411] = 0;
+   out_6036032580712144890[412] = 0;
+   out_6036032580712144890[413] = 0;
+   out_6036032580712144890[414] = 0;
+   out_6036032580712144890[415] = 0;
+   out_6036032580712144890[416] = 0;
+   out_6036032580712144890[417] = 1.0;
+   out_6036032580712144890[418] = 0;
+   out_6036032580712144890[419] = 0;
+   out_6036032580712144890[420] = 0;
+   out_6036032580712144890[421] = 0;
+   out_6036032580712144890[422] = 0;
+   out_6036032580712144890[423] = 0;
+   out_6036032580712144890[424] = 0;
+   out_6036032580712144890[425] = 0;
+   out_6036032580712144890[426] = 0;
+   out_6036032580712144890[427] = 0;
+   out_6036032580712144890[428] = 0;
+   out_6036032580712144890[429] = 0;
+   out_6036032580712144890[430] = 0;
+   out_6036032580712144890[431] = 0;
+   out_6036032580712144890[432] = 0;
+   out_6036032580712144890[433] = 0;
+   out_6036032580712144890[434] = 0;
+   out_6036032580712144890[435] = 0;
+   out_6036032580712144890[436] = 0;
+   out_6036032580712144890[437] = 0;
+   out_6036032580712144890[438] = 0;
+   out_6036032580712144890[439] = 1.0;
+   out_6036032580712144890[440] = 0;
+   out_6036032580712144890[441] = 0;
+   out_6036032580712144890[442] = 0;
+   out_6036032580712144890[443] = 0;
+   out_6036032580712144890[444] = 0;
+   out_6036032580712144890[445] = 0;
+   out_6036032580712144890[446] = 0;
+   out_6036032580712144890[447] = 0;
+   out_6036032580712144890[448] = 0;
+   out_6036032580712144890[449] = 0;
+   out_6036032580712144890[450] = 0;
+   out_6036032580712144890[451] = 0;
+   out_6036032580712144890[452] = 0;
+   out_6036032580712144890[453] = 0;
+   out_6036032580712144890[454] = 0;
+   out_6036032580712144890[455] = 0;
+   out_6036032580712144890[456] = 0;
+   out_6036032580712144890[457] = 0;
+   out_6036032580712144890[458] = 0;
+   out_6036032580712144890[459] = 0;
+   out_6036032580712144890[460] = 0;
+   out_6036032580712144890[461] = 1.0;
 }
-void f_fun(double *state, double dt, double *out_3547963935686363060) {
-   out_3547963935686363060[0] = dt*state[7] + state[0];
-   out_3547963935686363060[1] = dt*state[8] + state[1];
-   out_3547963935686363060[2] = dt*state[9] + state[2];
-   out_3547963935686363060[3] = dt*(-0.5*state[4]*state[10] - 0.5*state[5]*state[11] - 0.5*state[6]*state[12]) + state[3];
-   out_3547963935686363060[4] = dt*(0.5*state[3]*state[10] + 0.5*state[5]*state[12] - 0.5*state[6]*state[11]) + state[4];
-   out_3547963935686363060[5] = dt*(0.5*state[3]*state[11] - 0.5*state[4]*state[12] + 0.5*state[6]*state[10]) + state[5];
-   out_3547963935686363060[6] = dt*(0.5*state[3]*state[12] + 0.5*state[4]*state[11] - 0.5*state[5]*state[10]) + state[6];
-   out_3547963935686363060[7] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[18] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[17] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[16]) + state[7];
-   out_3547963935686363060[8] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[18] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[16] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[17]) + state[8];
-   out_3547963935686363060[9] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[17] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[16] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[18]) + state[9];
-   out_3547963935686363060[10] = state[10];
-   out_3547963935686363060[11] = state[11];
-   out_3547963935686363060[12] = state[12];
-   out_3547963935686363060[13] = state[13];
-   out_3547963935686363060[14] = state[14];
-   out_3547963935686363060[15] = state[15];
-   out_3547963935686363060[16] = state[16];
-   out_3547963935686363060[17] = state[17];
-   out_3547963935686363060[18] = state[18];
-   out_3547963935686363060[19] = state[19];
-   out_3547963935686363060[20] = state[20];
-   out_3547963935686363060[21] = state[21];
+void f_fun(double *state, double dt, double *out_653667760006229873) {
+   out_653667760006229873[0] = dt*state[7] + state[0];
+   out_653667760006229873[1] = dt*state[8] + state[1];
+   out_653667760006229873[2] = dt*state[9] + state[2];
+   out_653667760006229873[3] = dt*(-0.5*state[4]*state[10] - 0.5*state[5]*state[11] - 0.5*state[6]*state[12]) + state[3];
+   out_653667760006229873[4] = dt*(0.5*state[3]*state[10] + 0.5*state[5]*state[12] - 0.5*state[6]*state[11]) + state[4];
+   out_653667760006229873[5] = dt*(0.5*state[3]*state[11] - 0.5*state[4]*state[12] + 0.5*state[6]*state[10]) + state[5];
+   out_653667760006229873[6] = dt*(0.5*state[3]*state[12] + 0.5*state[4]*state[11] - 0.5*state[5]*state[10]) + state[6];
+   out_653667760006229873[7] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[18] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[17] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[16]) + state[7];
+   out_653667760006229873[8] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[18] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[16] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[17]) + state[8];
+   out_653667760006229873[9] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[17] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[16] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[18]) + state[9];
+   out_653667760006229873[10] = state[10];
+   out_653667760006229873[11] = state[11];
+   out_653667760006229873[12] = state[12];
+   out_653667760006229873[13] = state[13];
+   out_653667760006229873[14] = state[14];
+   out_653667760006229873[15] = state[15];
+   out_653667760006229873[16] = state[16];
+   out_653667760006229873[17] = state[17];
+   out_653667760006229873[18] = state[18];
+   out_653667760006229873[19] = state[19];
+   out_653667760006229873[20] = state[20];
+   out_653667760006229873[21] = state[21];
 }
-void F_fun(double *state, double dt, double *out_3254137990445528781) {
-   out_3254137990445528781[0] = 1;
-   out_3254137990445528781[1] = 0;
-   out_3254137990445528781[2] = 0;
-   out_3254137990445528781[3] = 0;
-   out_3254137990445528781[4] = 0;
-   out_3254137990445528781[5] = 0;
-   out_3254137990445528781[6] = dt;
-   out_3254137990445528781[7] = 0;
-   out_3254137990445528781[8] = 0;
-   out_3254137990445528781[9] = 0;
-   out_3254137990445528781[10] = 0;
-   out_3254137990445528781[11] = 0;
-   out_3254137990445528781[12] = 0;
-   out_3254137990445528781[13] = 0;
-   out_3254137990445528781[14] = 0;
-   out_3254137990445528781[15] = 0;
-   out_3254137990445528781[16] = 0;
-   out_3254137990445528781[17] = 0;
-   out_3254137990445528781[18] = 0;
-   out_3254137990445528781[19] = 0;
-   out_3254137990445528781[20] = 0;
-   out_3254137990445528781[21] = 0;
-   out_3254137990445528781[22] = 1;
-   out_3254137990445528781[23] = 0;
-   out_3254137990445528781[24] = 0;
-   out_3254137990445528781[25] = 0;
-   out_3254137990445528781[26] = 0;
-   out_3254137990445528781[27] = 0;
-   out_3254137990445528781[28] = dt;
-   out_3254137990445528781[29] = 0;
-   out_3254137990445528781[30] = 0;
-   out_3254137990445528781[31] = 0;
-   out_3254137990445528781[32] = 0;
-   out_3254137990445528781[33] = 0;
-   out_3254137990445528781[34] = 0;
-   out_3254137990445528781[35] = 0;
-   out_3254137990445528781[36] = 0;
-   out_3254137990445528781[37] = 0;
-   out_3254137990445528781[38] = 0;
-   out_3254137990445528781[39] = 0;
-   out_3254137990445528781[40] = 0;
-   out_3254137990445528781[41] = 0;
-   out_3254137990445528781[42] = 0;
-   out_3254137990445528781[43] = 0;
-   out_3254137990445528781[44] = 1;
-   out_3254137990445528781[45] = 0;
-   out_3254137990445528781[46] = 0;
-   out_3254137990445528781[47] = 0;
-   out_3254137990445528781[48] = 0;
-   out_3254137990445528781[49] = 0;
-   out_3254137990445528781[50] = dt;
-   out_3254137990445528781[51] = 0;
-   out_3254137990445528781[52] = 0;
-   out_3254137990445528781[53] = 0;
-   out_3254137990445528781[54] = 0;
-   out_3254137990445528781[55] = 0;
-   out_3254137990445528781[56] = 0;
-   out_3254137990445528781[57] = 0;
-   out_3254137990445528781[58] = 0;
-   out_3254137990445528781[59] = 0;
-   out_3254137990445528781[60] = 0;
-   out_3254137990445528781[61] = 0;
-   out_3254137990445528781[62] = 0;
-   out_3254137990445528781[63] = 0;
-   out_3254137990445528781[64] = 0;
-   out_3254137990445528781[65] = 0;
-   out_3254137990445528781[66] = 1;
-   out_3254137990445528781[67] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[11] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[10] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[12]);
-   out_3254137990445528781[68] = dt*((2*state[3]*state[4] - 2*state[5]*state[6])*state[12] + (-2*state[3]*state[6] - 2*state[4]*state[5])*state[10] + (-pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[11]);
-   out_3254137990445528781[69] = 0;
-   out_3254137990445528781[70] = 0;
-   out_3254137990445528781[71] = 0;
-   out_3254137990445528781[72] = dt*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2));
-   out_3254137990445528781[73] = dt*(-2*state[3]*state[6] + 2*state[4]*state[5]);
-   out_3254137990445528781[74] = dt*(2*state[3]*state[5] + 2*state[4]*state[6]);
-   out_3254137990445528781[75] = 0;
-   out_3254137990445528781[76] = 0;
-   out_3254137990445528781[77] = 0;
-   out_3254137990445528781[78] = 0;
-   out_3254137990445528781[79] = 0;
-   out_3254137990445528781[80] = 0;
-   out_3254137990445528781[81] = 0;
-   out_3254137990445528781[82] = 0;
-   out_3254137990445528781[83] = 0;
-   out_3254137990445528781[84] = 0;
-   out_3254137990445528781[85] = 0;
-   out_3254137990445528781[86] = 0;
-   out_3254137990445528781[87] = dt*(-(2*state[3]*state[4] + 2*state[5]*state[6])*state[11] - (-2*state[3]*state[5] + 2*state[4]*state[6])*state[10] - (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[12]);
-   out_3254137990445528781[88] = 1;
-   out_3254137990445528781[89] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[12] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[11] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[10]);
-   out_3254137990445528781[90] = 0;
-   out_3254137990445528781[91] = 0;
-   out_3254137990445528781[92] = 0;
-   out_3254137990445528781[93] = dt*(2*state[3]*state[6] + 2*state[4]*state[5]);
-   out_3254137990445528781[94] = dt*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2));
-   out_3254137990445528781[95] = dt*(-2*state[3]*state[4] + 2*state[5]*state[6]);
-   out_3254137990445528781[96] = 0;
-   out_3254137990445528781[97] = 0;
-   out_3254137990445528781[98] = 0;
-   out_3254137990445528781[99] = 0;
-   out_3254137990445528781[100] = 0;
-   out_3254137990445528781[101] = 0;
-   out_3254137990445528781[102] = 0;
-   out_3254137990445528781[103] = 0;
-   out_3254137990445528781[104] = 0;
-   out_3254137990445528781[105] = 0;
-   out_3254137990445528781[106] = 0;
-   out_3254137990445528781[107] = 0;
-   out_3254137990445528781[108] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[12] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[10] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[11]);
-   out_3254137990445528781[109] = dt*((-2*state[3]*state[5] - 2*state[4]*state[6])*state[12] + (2*state[3]*state[6] - 2*state[4]*state[5])*state[11] + (-pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) + pow(state[6], 2))*state[10]);
-   out_3254137990445528781[110] = 1;
-   out_3254137990445528781[111] = 0;
-   out_3254137990445528781[112] = 0;
-   out_3254137990445528781[113] = 0;
-   out_3254137990445528781[114] = dt*(-2*state[3]*state[5] + 2*state[4]*state[6]);
-   out_3254137990445528781[115] = dt*(2*state[3]*state[4] + 2*state[5]*state[6]);
-   out_3254137990445528781[116] = dt*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2));
-   out_3254137990445528781[117] = 0;
-   out_3254137990445528781[118] = 0;
-   out_3254137990445528781[119] = 0;
-   out_3254137990445528781[120] = 0;
-   out_3254137990445528781[121] = 0;
-   out_3254137990445528781[122] = 0;
-   out_3254137990445528781[123] = 0;
-   out_3254137990445528781[124] = 0;
-   out_3254137990445528781[125] = 0;
-   out_3254137990445528781[126] = 0;
-   out_3254137990445528781[127] = 0;
-   out_3254137990445528781[128] = 0;
-   out_3254137990445528781[129] = 0;
-   out_3254137990445528781[130] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[17] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[16] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[18]);
-   out_3254137990445528781[131] = dt*((2*state[3]*state[4] - 2*state[5]*state[6])*state[18] + (-2*state[3]*state[6] - 2*state[4]*state[5])*state[16] + (-pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[17]);
-   out_3254137990445528781[132] = 1;
-   out_3254137990445528781[133] = 0;
-   out_3254137990445528781[134] = 0;
-   out_3254137990445528781[135] = 0;
-   out_3254137990445528781[136] = 0;
-   out_3254137990445528781[137] = 0;
-   out_3254137990445528781[138] = 0;
-   out_3254137990445528781[139] = 0;
-   out_3254137990445528781[140] = 0;
-   out_3254137990445528781[141] = dt*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2));
-   out_3254137990445528781[142] = dt*(-2*state[3]*state[6] + 2*state[4]*state[5]);
-   out_3254137990445528781[143] = dt*(2*state[3]*state[5] + 2*state[4]*state[6]);
-   out_3254137990445528781[144] = 0;
-   out_3254137990445528781[145] = 0;
-   out_3254137990445528781[146] = 0;
-   out_3254137990445528781[147] = 0;
-   out_3254137990445528781[148] = 0;
-   out_3254137990445528781[149] = 0;
-   out_3254137990445528781[150] = dt*(-(2*state[3]*state[4] + 2*state[5]*state[6])*state[17] - (-2*state[3]*state[5] + 2*state[4]*state[6])*state[16] - (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[18]);
-   out_3254137990445528781[151] = 0;
-   out_3254137990445528781[152] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[18] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[17] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[16]);
-   out_3254137990445528781[153] = 0;
-   out_3254137990445528781[154] = 1;
-   out_3254137990445528781[155] = 0;
-   out_3254137990445528781[156] = 0;
-   out_3254137990445528781[157] = 0;
-   out_3254137990445528781[158] = 0;
-   out_3254137990445528781[159] = 0;
-   out_3254137990445528781[160] = 0;
-   out_3254137990445528781[161] = 0;
-   out_3254137990445528781[162] = dt*(2*state[3]*state[6] + 2*state[4]*state[5]);
-   out_3254137990445528781[163] = dt*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2));
-   out_3254137990445528781[164] = dt*(-2*state[3]*state[4] + 2*state[5]*state[6]);
-   out_3254137990445528781[165] = 0;
-   out_3254137990445528781[166] = 0;
-   out_3254137990445528781[167] = 0;
-   out_3254137990445528781[168] = 0;
-   out_3254137990445528781[169] = 0;
-   out_3254137990445528781[170] = 0;
-   out_3254137990445528781[171] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[18] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[16] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[17]);
-   out_3254137990445528781[172] = dt*((-2*state[3]*state[5] - 2*state[4]*state[6])*state[18] + (2*state[3]*state[6] - 2*state[4]*state[5])*state[17] + (-pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) + pow(state[6], 2))*state[16]);
-   out_3254137990445528781[173] = 0;
-   out_3254137990445528781[174] = 0;
-   out_3254137990445528781[175] = 0;
-   out_3254137990445528781[176] = 1;
-   out_3254137990445528781[177] = 0;
-   out_3254137990445528781[178] = 0;
-   out_3254137990445528781[179] = 0;
-   out_3254137990445528781[180] = 0;
-   out_3254137990445528781[181] = 0;
-   out_3254137990445528781[182] = 0;
-   out_3254137990445528781[183] = dt*(-2*state[3]*state[5] + 2*state[4]*state[6]);
-   out_3254137990445528781[184] = dt*(2*state[3]*state[4] + 2*state[5]*state[6]);
-   out_3254137990445528781[185] = dt*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2));
-   out_3254137990445528781[186] = 0;
-   out_3254137990445528781[187] = 0;
-   out_3254137990445528781[188] = 0;
-   out_3254137990445528781[189] = 0;
-   out_3254137990445528781[190] = 0;
-   out_3254137990445528781[191] = 0;
-   out_3254137990445528781[192] = 0;
-   out_3254137990445528781[193] = 0;
-   out_3254137990445528781[194] = 0;
-   out_3254137990445528781[195] = 0;
-   out_3254137990445528781[196] = 0;
-   out_3254137990445528781[197] = 0;
-   out_3254137990445528781[198] = 1;
-   out_3254137990445528781[199] = 0;
-   out_3254137990445528781[200] = 0;
-   out_3254137990445528781[201] = 0;
-   out_3254137990445528781[202] = 0;
-   out_3254137990445528781[203] = 0;
-   out_3254137990445528781[204] = 0;
-   out_3254137990445528781[205] = 0;
-   out_3254137990445528781[206] = 0;
-   out_3254137990445528781[207] = 0;
-   out_3254137990445528781[208] = 0;
-   out_3254137990445528781[209] = 0;
-   out_3254137990445528781[210] = 0;
-   out_3254137990445528781[211] = 0;
-   out_3254137990445528781[212] = 0;
-   out_3254137990445528781[213] = 0;
-   out_3254137990445528781[214] = 0;
-   out_3254137990445528781[215] = 0;
-   out_3254137990445528781[216] = 0;
-   out_3254137990445528781[217] = 0;
-   out_3254137990445528781[218] = 0;
-   out_3254137990445528781[219] = 0;
-   out_3254137990445528781[220] = 1;
-   out_3254137990445528781[221] = 0;
-   out_3254137990445528781[222] = 0;
-   out_3254137990445528781[223] = 0;
-   out_3254137990445528781[224] = 0;
-   out_3254137990445528781[225] = 0;
-   out_3254137990445528781[226] = 0;
-   out_3254137990445528781[227] = 0;
-   out_3254137990445528781[228] = 0;
-   out_3254137990445528781[229] = 0;
-   out_3254137990445528781[230] = 0;
-   out_3254137990445528781[231] = 0;
-   out_3254137990445528781[232] = 0;
-   out_3254137990445528781[233] = 0;
-   out_3254137990445528781[234] = 0;
-   out_3254137990445528781[235] = 0;
-   out_3254137990445528781[236] = 0;
-   out_3254137990445528781[237] = 0;
-   out_3254137990445528781[238] = 0;
-   out_3254137990445528781[239] = 0;
-   out_3254137990445528781[240] = 0;
-   out_3254137990445528781[241] = 0;
-   out_3254137990445528781[242] = 1;
-   out_3254137990445528781[243] = 0;
-   out_3254137990445528781[244] = 0;
-   out_3254137990445528781[245] = 0;
-   out_3254137990445528781[246] = 0;
-   out_3254137990445528781[247] = 0;
-   out_3254137990445528781[248] = 0;
-   out_3254137990445528781[249] = 0;
-   out_3254137990445528781[250] = 0;
-   out_3254137990445528781[251] = 0;
-   out_3254137990445528781[252] = 0;
-   out_3254137990445528781[253] = 0;
-   out_3254137990445528781[254] = 0;
-   out_3254137990445528781[255] = 0;
-   out_3254137990445528781[256] = 0;
-   out_3254137990445528781[257] = 0;
-   out_3254137990445528781[258] = 0;
-   out_3254137990445528781[259] = 0;
-   out_3254137990445528781[260] = 0;
-   out_3254137990445528781[261] = 0;
-   out_3254137990445528781[262] = 0;
-   out_3254137990445528781[263] = 0;
-   out_3254137990445528781[264] = 1;
-   out_3254137990445528781[265] = 0;
-   out_3254137990445528781[266] = 0;
-   out_3254137990445528781[267] = 0;
-   out_3254137990445528781[268] = 0;
-   out_3254137990445528781[269] = 0;
-   out_3254137990445528781[270] = 0;
-   out_3254137990445528781[271] = 0;
-   out_3254137990445528781[272] = 0;
-   out_3254137990445528781[273] = 0;
-   out_3254137990445528781[274] = 0;
-   out_3254137990445528781[275] = 0;
-   out_3254137990445528781[276] = 0;
-   out_3254137990445528781[277] = 0;
-   out_3254137990445528781[278] = 0;
-   out_3254137990445528781[279] = 0;
-   out_3254137990445528781[280] = 0;
-   out_3254137990445528781[281] = 0;
-   out_3254137990445528781[282] = 0;
-   out_3254137990445528781[283] = 0;
-   out_3254137990445528781[284] = 0;
-   out_3254137990445528781[285] = 0;
-   out_3254137990445528781[286] = 1;
-   out_3254137990445528781[287] = 0;
-   out_3254137990445528781[288] = 0;
-   out_3254137990445528781[289] = 0;
-   out_3254137990445528781[290] = 0;
-   out_3254137990445528781[291] = 0;
-   out_3254137990445528781[292] = 0;
-   out_3254137990445528781[293] = 0;
-   out_3254137990445528781[294] = 0;
-   out_3254137990445528781[295] = 0;
-   out_3254137990445528781[296] = 0;
-   out_3254137990445528781[297] = 0;
-   out_3254137990445528781[298] = 0;
-   out_3254137990445528781[299] = 0;
-   out_3254137990445528781[300] = 0;
-   out_3254137990445528781[301] = 0;
-   out_3254137990445528781[302] = 0;
-   out_3254137990445528781[303] = 0;
-   out_3254137990445528781[304] = 0;
-   out_3254137990445528781[305] = 0;
-   out_3254137990445528781[306] = 0;
-   out_3254137990445528781[307] = 0;
-   out_3254137990445528781[308] = 1;
-   out_3254137990445528781[309] = 0;
-   out_3254137990445528781[310] = 0;
-   out_3254137990445528781[311] = 0;
-   out_3254137990445528781[312] = 0;
-   out_3254137990445528781[313] = 0;
-   out_3254137990445528781[314] = 0;
-   out_3254137990445528781[315] = 0;
-   out_3254137990445528781[316] = 0;
-   out_3254137990445528781[317] = 0;
-   out_3254137990445528781[318] = 0;
-   out_3254137990445528781[319] = 0;
-   out_3254137990445528781[320] = 0;
-   out_3254137990445528781[321] = 0;
-   out_3254137990445528781[322] = 0;
-   out_3254137990445528781[323] = 0;
-   out_3254137990445528781[324] = 0;
-   out_3254137990445528781[325] = 0;
-   out_3254137990445528781[326] = 0;
-   out_3254137990445528781[327] = 0;
-   out_3254137990445528781[328] = 0;
-   out_3254137990445528781[329] = 0;
-   out_3254137990445528781[330] = 1;
-   out_3254137990445528781[331] = 0;
-   out_3254137990445528781[332] = 0;
-   out_3254137990445528781[333] = 0;
-   out_3254137990445528781[334] = 0;
-   out_3254137990445528781[335] = 0;
-   out_3254137990445528781[336] = 0;
-   out_3254137990445528781[337] = 0;
-   out_3254137990445528781[338] = 0;
-   out_3254137990445528781[339] = 0;
-   out_3254137990445528781[340] = 0;
-   out_3254137990445528781[341] = 0;
-   out_3254137990445528781[342] = 0;
-   out_3254137990445528781[343] = 0;
-   out_3254137990445528781[344] = 0;
-   out_3254137990445528781[345] = 0;
-   out_3254137990445528781[346] = 0;
-   out_3254137990445528781[347] = 0;
-   out_3254137990445528781[348] = 0;
-   out_3254137990445528781[349] = 0;
-   out_3254137990445528781[350] = 0;
-   out_3254137990445528781[351] = 0;
-   out_3254137990445528781[352] = 1;
-   out_3254137990445528781[353] = 0;
-   out_3254137990445528781[354] = 0;
-   out_3254137990445528781[355] = 0;
-   out_3254137990445528781[356] = 0;
-   out_3254137990445528781[357] = 0;
-   out_3254137990445528781[358] = 0;
-   out_3254137990445528781[359] = 0;
-   out_3254137990445528781[360] = 0;
-   out_3254137990445528781[361] = 0;
-   out_3254137990445528781[362] = 0;
-   out_3254137990445528781[363] = 0;
-   out_3254137990445528781[364] = 0;
-   out_3254137990445528781[365] = 0;
-   out_3254137990445528781[366] = 0;
-   out_3254137990445528781[367] = 0;
-   out_3254137990445528781[368] = 0;
-   out_3254137990445528781[369] = 0;
-   out_3254137990445528781[370] = 0;
-   out_3254137990445528781[371] = 0;
-   out_3254137990445528781[372] = 0;
-   out_3254137990445528781[373] = 0;
-   out_3254137990445528781[374] = 1;
-   out_3254137990445528781[375] = 0;
-   out_3254137990445528781[376] = 0;
-   out_3254137990445528781[377] = 0;
-   out_3254137990445528781[378] = 0;
-   out_3254137990445528781[379] = 0;
-   out_3254137990445528781[380] = 0;
-   out_3254137990445528781[381] = 0;
-   out_3254137990445528781[382] = 0;
-   out_3254137990445528781[383] = 0;
-   out_3254137990445528781[384] = 0;
-   out_3254137990445528781[385] = 0;
-   out_3254137990445528781[386] = 0;
-   out_3254137990445528781[387] = 0;
-   out_3254137990445528781[388] = 0;
-   out_3254137990445528781[389] = 0;
-   out_3254137990445528781[390] = 0;
-   out_3254137990445528781[391] = 0;
-   out_3254137990445528781[392] = 0;
-   out_3254137990445528781[393] = 0;
-   out_3254137990445528781[394] = 0;
-   out_3254137990445528781[395] = 0;
-   out_3254137990445528781[396] = 1;
-   out_3254137990445528781[397] = 0;
-   out_3254137990445528781[398] = 0;
-   out_3254137990445528781[399] = 0;
-   out_3254137990445528781[400] = 0;
-   out_3254137990445528781[401] = 0;
-   out_3254137990445528781[402] = 0;
-   out_3254137990445528781[403] = 0;
-   out_3254137990445528781[404] = 0;
-   out_3254137990445528781[405] = 0;
-   out_3254137990445528781[406] = 0;
-   out_3254137990445528781[407] = 0;
-   out_3254137990445528781[408] = 0;
-   out_3254137990445528781[409] = 0;
-   out_3254137990445528781[410] = 0;
-   out_3254137990445528781[411] = 0;
-   out_3254137990445528781[412] = 0;
-   out_3254137990445528781[413] = 0;
-   out_3254137990445528781[414] = 0;
-   out_3254137990445528781[415] = 0;
-   out_3254137990445528781[416] = 0;
-   out_3254137990445528781[417] = 0;
-   out_3254137990445528781[418] = 1;
-   out_3254137990445528781[419] = 0;
-   out_3254137990445528781[420] = 0;
-   out_3254137990445528781[421] = 0;
-   out_3254137990445528781[422] = 0;
-   out_3254137990445528781[423] = 0;
-   out_3254137990445528781[424] = 0;
-   out_3254137990445528781[425] = 0;
-   out_3254137990445528781[426] = 0;
-   out_3254137990445528781[427] = 0;
-   out_3254137990445528781[428] = 0;
-   out_3254137990445528781[429] = 0;
-   out_3254137990445528781[430] = 0;
-   out_3254137990445528781[431] = 0;
-   out_3254137990445528781[432] = 0;
-   out_3254137990445528781[433] = 0;
-   out_3254137990445528781[434] = 0;
-   out_3254137990445528781[435] = 0;
-   out_3254137990445528781[436] = 0;
-   out_3254137990445528781[437] = 0;
-   out_3254137990445528781[438] = 0;
-   out_3254137990445528781[439] = 0;
-   out_3254137990445528781[440] = 1;
+void F_fun(double *state, double dt, double *out_8944506884882647575) {
+   out_8944506884882647575[0] = 1;
+   out_8944506884882647575[1] = 0;
+   out_8944506884882647575[2] = 0;
+   out_8944506884882647575[3] = 0;
+   out_8944506884882647575[4] = 0;
+   out_8944506884882647575[5] = 0;
+   out_8944506884882647575[6] = dt;
+   out_8944506884882647575[7] = 0;
+   out_8944506884882647575[8] = 0;
+   out_8944506884882647575[9] = 0;
+   out_8944506884882647575[10] = 0;
+   out_8944506884882647575[11] = 0;
+   out_8944506884882647575[12] = 0;
+   out_8944506884882647575[13] = 0;
+   out_8944506884882647575[14] = 0;
+   out_8944506884882647575[15] = 0;
+   out_8944506884882647575[16] = 0;
+   out_8944506884882647575[17] = 0;
+   out_8944506884882647575[18] = 0;
+   out_8944506884882647575[19] = 0;
+   out_8944506884882647575[20] = 0;
+   out_8944506884882647575[21] = 0;
+   out_8944506884882647575[22] = 1;
+   out_8944506884882647575[23] = 0;
+   out_8944506884882647575[24] = 0;
+   out_8944506884882647575[25] = 0;
+   out_8944506884882647575[26] = 0;
+   out_8944506884882647575[27] = 0;
+   out_8944506884882647575[28] = dt;
+   out_8944506884882647575[29] = 0;
+   out_8944506884882647575[30] = 0;
+   out_8944506884882647575[31] = 0;
+   out_8944506884882647575[32] = 0;
+   out_8944506884882647575[33] = 0;
+   out_8944506884882647575[34] = 0;
+   out_8944506884882647575[35] = 0;
+   out_8944506884882647575[36] = 0;
+   out_8944506884882647575[37] = 0;
+   out_8944506884882647575[38] = 0;
+   out_8944506884882647575[39] = 0;
+   out_8944506884882647575[40] = 0;
+   out_8944506884882647575[41] = 0;
+   out_8944506884882647575[42] = 0;
+   out_8944506884882647575[43] = 0;
+   out_8944506884882647575[44] = 1;
+   out_8944506884882647575[45] = 0;
+   out_8944506884882647575[46] = 0;
+   out_8944506884882647575[47] = 0;
+   out_8944506884882647575[48] = 0;
+   out_8944506884882647575[49] = 0;
+   out_8944506884882647575[50] = dt;
+   out_8944506884882647575[51] = 0;
+   out_8944506884882647575[52] = 0;
+   out_8944506884882647575[53] = 0;
+   out_8944506884882647575[54] = 0;
+   out_8944506884882647575[55] = 0;
+   out_8944506884882647575[56] = 0;
+   out_8944506884882647575[57] = 0;
+   out_8944506884882647575[58] = 0;
+   out_8944506884882647575[59] = 0;
+   out_8944506884882647575[60] = 0;
+   out_8944506884882647575[61] = 0;
+   out_8944506884882647575[62] = 0;
+   out_8944506884882647575[63] = 0;
+   out_8944506884882647575[64] = 0;
+   out_8944506884882647575[65] = 0;
+   out_8944506884882647575[66] = 1;
+   out_8944506884882647575[67] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[11] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[10] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[12]);
+   out_8944506884882647575[68] = dt*((2*state[3]*state[4] - 2*state[5]*state[6])*state[12] + (-2*state[3]*state[6] - 2*state[4]*state[5])*state[10] + (-pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[11]);
+   out_8944506884882647575[69] = 0;
+   out_8944506884882647575[70] = 0;
+   out_8944506884882647575[71] = 0;
+   out_8944506884882647575[72] = dt*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2));
+   out_8944506884882647575[73] = dt*(-2*state[3]*state[6] + 2*state[4]*state[5]);
+   out_8944506884882647575[74] = dt*(2*state[3]*state[5] + 2*state[4]*state[6]);
+   out_8944506884882647575[75] = 0;
+   out_8944506884882647575[76] = 0;
+   out_8944506884882647575[77] = 0;
+   out_8944506884882647575[78] = 0;
+   out_8944506884882647575[79] = 0;
+   out_8944506884882647575[80] = 0;
+   out_8944506884882647575[81] = 0;
+   out_8944506884882647575[82] = 0;
+   out_8944506884882647575[83] = 0;
+   out_8944506884882647575[84] = 0;
+   out_8944506884882647575[85] = 0;
+   out_8944506884882647575[86] = 0;
+   out_8944506884882647575[87] = dt*(-(2*state[3]*state[4] + 2*state[5]*state[6])*state[11] - (-2*state[3]*state[5] + 2*state[4]*state[6])*state[10] - (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[12]);
+   out_8944506884882647575[88] = 1;
+   out_8944506884882647575[89] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[12] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[11] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[10]);
+   out_8944506884882647575[90] = 0;
+   out_8944506884882647575[91] = 0;
+   out_8944506884882647575[92] = 0;
+   out_8944506884882647575[93] = dt*(2*state[3]*state[6] + 2*state[4]*state[5]);
+   out_8944506884882647575[94] = dt*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2));
+   out_8944506884882647575[95] = dt*(-2*state[3]*state[4] + 2*state[5]*state[6]);
+   out_8944506884882647575[96] = 0;
+   out_8944506884882647575[97] = 0;
+   out_8944506884882647575[98] = 0;
+   out_8944506884882647575[99] = 0;
+   out_8944506884882647575[100] = 0;
+   out_8944506884882647575[101] = 0;
+   out_8944506884882647575[102] = 0;
+   out_8944506884882647575[103] = 0;
+   out_8944506884882647575[104] = 0;
+   out_8944506884882647575[105] = 0;
+   out_8944506884882647575[106] = 0;
+   out_8944506884882647575[107] = 0;
+   out_8944506884882647575[108] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[12] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[10] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[11]);
+   out_8944506884882647575[109] = dt*((-2*state[3]*state[5] - 2*state[4]*state[6])*state[12] + (2*state[3]*state[6] - 2*state[4]*state[5])*state[11] + (-pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) + pow(state[6], 2))*state[10]);
+   out_8944506884882647575[110] = 1;
+   out_8944506884882647575[111] = 0;
+   out_8944506884882647575[112] = 0;
+   out_8944506884882647575[113] = 0;
+   out_8944506884882647575[114] = dt*(-2*state[3]*state[5] + 2*state[4]*state[6]);
+   out_8944506884882647575[115] = dt*(2*state[3]*state[4] + 2*state[5]*state[6]);
+   out_8944506884882647575[116] = dt*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2));
+   out_8944506884882647575[117] = 0;
+   out_8944506884882647575[118] = 0;
+   out_8944506884882647575[119] = 0;
+   out_8944506884882647575[120] = 0;
+   out_8944506884882647575[121] = 0;
+   out_8944506884882647575[122] = 0;
+   out_8944506884882647575[123] = 0;
+   out_8944506884882647575[124] = 0;
+   out_8944506884882647575[125] = 0;
+   out_8944506884882647575[126] = 0;
+   out_8944506884882647575[127] = 0;
+   out_8944506884882647575[128] = 0;
+   out_8944506884882647575[129] = 0;
+   out_8944506884882647575[130] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[17] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[16] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[18]);
+   out_8944506884882647575[131] = dt*((2*state[3]*state[4] - 2*state[5]*state[6])*state[18] + (-2*state[3]*state[6] - 2*state[4]*state[5])*state[16] + (-pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[17]);
+   out_8944506884882647575[132] = 1;
+   out_8944506884882647575[133] = 0;
+   out_8944506884882647575[134] = 0;
+   out_8944506884882647575[135] = 0;
+   out_8944506884882647575[136] = 0;
+   out_8944506884882647575[137] = 0;
+   out_8944506884882647575[138] = 0;
+   out_8944506884882647575[139] = 0;
+   out_8944506884882647575[140] = 0;
+   out_8944506884882647575[141] = dt*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2));
+   out_8944506884882647575[142] = dt*(-2*state[3]*state[6] + 2*state[4]*state[5]);
+   out_8944506884882647575[143] = dt*(2*state[3]*state[5] + 2*state[4]*state[6]);
+   out_8944506884882647575[144] = 0;
+   out_8944506884882647575[145] = 0;
+   out_8944506884882647575[146] = 0;
+   out_8944506884882647575[147] = 0;
+   out_8944506884882647575[148] = 0;
+   out_8944506884882647575[149] = 0;
+   out_8944506884882647575[150] = dt*(-(2*state[3]*state[4] + 2*state[5]*state[6])*state[17] - (-2*state[3]*state[5] + 2*state[4]*state[6])*state[16] - (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[18]);
+   out_8944506884882647575[151] = 0;
+   out_8944506884882647575[152] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[18] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[17] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[16]);
+   out_8944506884882647575[153] = 0;
+   out_8944506884882647575[154] = 1;
+   out_8944506884882647575[155] = 0;
+   out_8944506884882647575[156] = 0;
+   out_8944506884882647575[157] = 0;
+   out_8944506884882647575[158] = 0;
+   out_8944506884882647575[159] = 0;
+   out_8944506884882647575[160] = 0;
+   out_8944506884882647575[161] = 0;
+   out_8944506884882647575[162] = dt*(2*state[3]*state[6] + 2*state[4]*state[5]);
+   out_8944506884882647575[163] = dt*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2));
+   out_8944506884882647575[164] = dt*(-2*state[3]*state[4] + 2*state[5]*state[6]);
+   out_8944506884882647575[165] = 0;
+   out_8944506884882647575[166] = 0;
+   out_8944506884882647575[167] = 0;
+   out_8944506884882647575[168] = 0;
+   out_8944506884882647575[169] = 0;
+   out_8944506884882647575[170] = 0;
+   out_8944506884882647575[171] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[18] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[16] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[17]);
+   out_8944506884882647575[172] = dt*((-2*state[3]*state[5] - 2*state[4]*state[6])*state[18] + (2*state[3]*state[6] - 2*state[4]*state[5])*state[17] + (-pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) + pow(state[6], 2))*state[16]);
+   out_8944506884882647575[173] = 0;
+   out_8944506884882647575[174] = 0;
+   out_8944506884882647575[175] = 0;
+   out_8944506884882647575[176] = 1;
+   out_8944506884882647575[177] = 0;
+   out_8944506884882647575[178] = 0;
+   out_8944506884882647575[179] = 0;
+   out_8944506884882647575[180] = 0;
+   out_8944506884882647575[181] = 0;
+   out_8944506884882647575[182] = 0;
+   out_8944506884882647575[183] = dt*(-2*state[3]*state[5] + 2*state[4]*state[6]);
+   out_8944506884882647575[184] = dt*(2*state[3]*state[4] + 2*state[5]*state[6]);
+   out_8944506884882647575[185] = dt*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2));
+   out_8944506884882647575[186] = 0;
+   out_8944506884882647575[187] = 0;
+   out_8944506884882647575[188] = 0;
+   out_8944506884882647575[189] = 0;
+   out_8944506884882647575[190] = 0;
+   out_8944506884882647575[191] = 0;
+   out_8944506884882647575[192] = 0;
+   out_8944506884882647575[193] = 0;
+   out_8944506884882647575[194] = 0;
+   out_8944506884882647575[195] = 0;
+   out_8944506884882647575[196] = 0;
+   out_8944506884882647575[197] = 0;
+   out_8944506884882647575[198] = 1;
+   out_8944506884882647575[199] = 0;
+   out_8944506884882647575[200] = 0;
+   out_8944506884882647575[201] = 0;
+   out_8944506884882647575[202] = 0;
+   out_8944506884882647575[203] = 0;
+   out_8944506884882647575[204] = 0;
+   out_8944506884882647575[205] = 0;
+   out_8944506884882647575[206] = 0;
+   out_8944506884882647575[207] = 0;
+   out_8944506884882647575[208] = 0;
+   out_8944506884882647575[209] = 0;
+   out_8944506884882647575[210] = 0;
+   out_8944506884882647575[211] = 0;
+   out_8944506884882647575[212] = 0;
+   out_8944506884882647575[213] = 0;
+   out_8944506884882647575[214] = 0;
+   out_8944506884882647575[215] = 0;
+   out_8944506884882647575[216] = 0;
+   out_8944506884882647575[217] = 0;
+   out_8944506884882647575[218] = 0;
+   out_8944506884882647575[219] = 0;
+   out_8944506884882647575[220] = 1;
+   out_8944506884882647575[221] = 0;
+   out_8944506884882647575[222] = 0;
+   out_8944506884882647575[223] = 0;
+   out_8944506884882647575[224] = 0;
+   out_8944506884882647575[225] = 0;
+   out_8944506884882647575[226] = 0;
+   out_8944506884882647575[227] = 0;
+   out_8944506884882647575[228] = 0;
+   out_8944506884882647575[229] = 0;
+   out_8944506884882647575[230] = 0;
+   out_8944506884882647575[231] = 0;
+   out_8944506884882647575[232] = 0;
+   out_8944506884882647575[233] = 0;
+   out_8944506884882647575[234] = 0;
+   out_8944506884882647575[235] = 0;
+   out_8944506884882647575[236] = 0;
+   out_8944506884882647575[237] = 0;
+   out_8944506884882647575[238] = 0;
+   out_8944506884882647575[239] = 0;
+   out_8944506884882647575[240] = 0;
+   out_8944506884882647575[241] = 0;
+   out_8944506884882647575[242] = 1;
+   out_8944506884882647575[243] = 0;
+   out_8944506884882647575[244] = 0;
+   out_8944506884882647575[245] = 0;
+   out_8944506884882647575[246] = 0;
+   out_8944506884882647575[247] = 0;
+   out_8944506884882647575[248] = 0;
+   out_8944506884882647575[249] = 0;
+   out_8944506884882647575[250] = 0;
+   out_8944506884882647575[251] = 0;
+   out_8944506884882647575[252] = 0;
+   out_8944506884882647575[253] = 0;
+   out_8944506884882647575[254] = 0;
+   out_8944506884882647575[255] = 0;
+   out_8944506884882647575[256] = 0;
+   out_8944506884882647575[257] = 0;
+   out_8944506884882647575[258] = 0;
+   out_8944506884882647575[259] = 0;
+   out_8944506884882647575[260] = 0;
+   out_8944506884882647575[261] = 0;
+   out_8944506884882647575[262] = 0;
+   out_8944506884882647575[263] = 0;
+   out_8944506884882647575[264] = 1;
+   out_8944506884882647575[265] = 0;
+   out_8944506884882647575[266] = 0;
+   out_8944506884882647575[267] = 0;
+   out_8944506884882647575[268] = 0;
+   out_8944506884882647575[269] = 0;
+   out_8944506884882647575[270] = 0;
+   out_8944506884882647575[271] = 0;
+   out_8944506884882647575[272] = 0;
+   out_8944506884882647575[273] = 0;
+   out_8944506884882647575[274] = 0;
+   out_8944506884882647575[275] = 0;
+   out_8944506884882647575[276] = 0;
+   out_8944506884882647575[277] = 0;
+   out_8944506884882647575[278] = 0;
+   out_8944506884882647575[279] = 0;
+   out_8944506884882647575[280] = 0;
+   out_8944506884882647575[281] = 0;
+   out_8944506884882647575[282] = 0;
+   out_8944506884882647575[283] = 0;
+   out_8944506884882647575[284] = 0;
+   out_8944506884882647575[285] = 0;
+   out_8944506884882647575[286] = 1;
+   out_8944506884882647575[287] = 0;
+   out_8944506884882647575[288] = 0;
+   out_8944506884882647575[289] = 0;
+   out_8944506884882647575[290] = 0;
+   out_8944506884882647575[291] = 0;
+   out_8944506884882647575[292] = 0;
+   out_8944506884882647575[293] = 0;
+   out_8944506884882647575[294] = 0;
+   out_8944506884882647575[295] = 0;
+   out_8944506884882647575[296] = 0;
+   out_8944506884882647575[297] = 0;
+   out_8944506884882647575[298] = 0;
+   out_8944506884882647575[299] = 0;
+   out_8944506884882647575[300] = 0;
+   out_8944506884882647575[301] = 0;
+   out_8944506884882647575[302] = 0;
+   out_8944506884882647575[303] = 0;
+   out_8944506884882647575[304] = 0;
+   out_8944506884882647575[305] = 0;
+   out_8944506884882647575[306] = 0;
+   out_8944506884882647575[307] = 0;
+   out_8944506884882647575[308] = 1;
+   out_8944506884882647575[309] = 0;
+   out_8944506884882647575[310] = 0;
+   out_8944506884882647575[311] = 0;
+   out_8944506884882647575[312] = 0;
+   out_8944506884882647575[313] = 0;
+   out_8944506884882647575[314] = 0;
+   out_8944506884882647575[315] = 0;
+   out_8944506884882647575[316] = 0;
+   out_8944506884882647575[317] = 0;
+   out_8944506884882647575[318] = 0;
+   out_8944506884882647575[319] = 0;
+   out_8944506884882647575[320] = 0;
+   out_8944506884882647575[321] = 0;
+   out_8944506884882647575[322] = 0;
+   out_8944506884882647575[323] = 0;
+   out_8944506884882647575[324] = 0;
+   out_8944506884882647575[325] = 0;
+   out_8944506884882647575[326] = 0;
+   out_8944506884882647575[327] = 0;
+   out_8944506884882647575[328] = 0;
+   out_8944506884882647575[329] = 0;
+   out_8944506884882647575[330] = 1;
+   out_8944506884882647575[331] = 0;
+   out_8944506884882647575[332] = 0;
+   out_8944506884882647575[333] = 0;
+   out_8944506884882647575[334] = 0;
+   out_8944506884882647575[335] = 0;
+   out_8944506884882647575[336] = 0;
+   out_8944506884882647575[337] = 0;
+   out_8944506884882647575[338] = 0;
+   out_8944506884882647575[339] = 0;
+   out_8944506884882647575[340] = 0;
+   out_8944506884882647575[341] = 0;
+   out_8944506884882647575[342] = 0;
+   out_8944506884882647575[343] = 0;
+   out_8944506884882647575[344] = 0;
+   out_8944506884882647575[345] = 0;
+   out_8944506884882647575[346] = 0;
+   out_8944506884882647575[347] = 0;
+   out_8944506884882647575[348] = 0;
+   out_8944506884882647575[349] = 0;
+   out_8944506884882647575[350] = 0;
+   out_8944506884882647575[351] = 0;
+   out_8944506884882647575[352] = 1;
+   out_8944506884882647575[353] = 0;
+   out_8944506884882647575[354] = 0;
+   out_8944506884882647575[355] = 0;
+   out_8944506884882647575[356] = 0;
+   out_8944506884882647575[357] = 0;
+   out_8944506884882647575[358] = 0;
+   out_8944506884882647575[359] = 0;
+   out_8944506884882647575[360] = 0;
+   out_8944506884882647575[361] = 0;
+   out_8944506884882647575[362] = 0;
+   out_8944506884882647575[363] = 0;
+   out_8944506884882647575[364] = 0;
+   out_8944506884882647575[365] = 0;
+   out_8944506884882647575[366] = 0;
+   out_8944506884882647575[367] = 0;
+   out_8944506884882647575[368] = 0;
+   out_8944506884882647575[369] = 0;
+   out_8944506884882647575[370] = 0;
+   out_8944506884882647575[371] = 0;
+   out_8944506884882647575[372] = 0;
+   out_8944506884882647575[373] = 0;
+   out_8944506884882647575[374] = 1;
+   out_8944506884882647575[375] = 0;
+   out_8944506884882647575[376] = 0;
+   out_8944506884882647575[377] = 0;
+   out_8944506884882647575[378] = 0;
+   out_8944506884882647575[379] = 0;
+   out_8944506884882647575[380] = 0;
+   out_8944506884882647575[381] = 0;
+   out_8944506884882647575[382] = 0;
+   out_8944506884882647575[383] = 0;
+   out_8944506884882647575[384] = 0;
+   out_8944506884882647575[385] = 0;
+   out_8944506884882647575[386] = 0;
+   out_8944506884882647575[387] = 0;
+   out_8944506884882647575[388] = 0;
+   out_8944506884882647575[389] = 0;
+   out_8944506884882647575[390] = 0;
+   out_8944506884882647575[391] = 0;
+   out_8944506884882647575[392] = 0;
+   out_8944506884882647575[393] = 0;
+   out_8944506884882647575[394] = 0;
+   out_8944506884882647575[395] = 0;
+   out_8944506884882647575[396] = 1;
+   out_8944506884882647575[397] = 0;
+   out_8944506884882647575[398] = 0;
+   out_8944506884882647575[399] = 0;
+   out_8944506884882647575[400] = 0;
+   out_8944506884882647575[401] = 0;
+   out_8944506884882647575[402] = 0;
+   out_8944506884882647575[403] = 0;
+   out_8944506884882647575[404] = 0;
+   out_8944506884882647575[405] = 0;
+   out_8944506884882647575[406] = 0;
+   out_8944506884882647575[407] = 0;
+   out_8944506884882647575[408] = 0;
+   out_8944506884882647575[409] = 0;
+   out_8944506884882647575[410] = 0;
+   out_8944506884882647575[411] = 0;
+   out_8944506884882647575[412] = 0;
+   out_8944506884882647575[413] = 0;
+   out_8944506884882647575[414] = 0;
+   out_8944506884882647575[415] = 0;
+   out_8944506884882647575[416] = 0;
+   out_8944506884882647575[417] = 0;
+   out_8944506884882647575[418] = 1;
+   out_8944506884882647575[419] = 0;
+   out_8944506884882647575[420] = 0;
+   out_8944506884882647575[421] = 0;
+   out_8944506884882647575[422] = 0;
+   out_8944506884882647575[423] = 0;
+   out_8944506884882647575[424] = 0;
+   out_8944506884882647575[425] = 0;
+   out_8944506884882647575[426] = 0;
+   out_8944506884882647575[427] = 0;
+   out_8944506884882647575[428] = 0;
+   out_8944506884882647575[429] = 0;
+   out_8944506884882647575[430] = 0;
+   out_8944506884882647575[431] = 0;
+   out_8944506884882647575[432] = 0;
+   out_8944506884882647575[433] = 0;
+   out_8944506884882647575[434] = 0;
+   out_8944506884882647575[435] = 0;
+   out_8944506884882647575[436] = 0;
+   out_8944506884882647575[437] = 0;
+   out_8944506884882647575[438] = 0;
+   out_8944506884882647575[439] = 0;
+   out_8944506884882647575[440] = 1;
 }
-void h_4(double *state, double *unused, double *out_4410787759120580449) {
-   out_4410787759120580449[0] = state[10] + state[13];
-   out_4410787759120580449[1] = state[11] + state[14];
-   out_4410787759120580449[2] = state[12] + state[15];
+void h_4(double *state, double *unused, double *out_6967130088459663654) {
+   out_6967130088459663654[0] = state[10] + state[13];
+   out_6967130088459663654[1] = state[11] + state[14];
+   out_6967130088459663654[2] = state[12] + state[15];
 }
-void H_4(double *state, double *unused, double *out_5512660098689845475) {
-   out_5512660098689845475[0] = 0;
-   out_5512660098689845475[1] = 0;
-   out_5512660098689845475[2] = 0;
-   out_5512660098689845475[3] = 0;
-   out_5512660098689845475[4] = 0;
-   out_5512660098689845475[5] = 0;
-   out_5512660098689845475[6] = 0;
-   out_5512660098689845475[7] = 0;
-   out_5512660098689845475[8] = 0;
-   out_5512660098689845475[9] = 0;
-   out_5512660098689845475[10] = 1;
-   out_5512660098689845475[11] = 0;
-   out_5512660098689845475[12] = 0;
-   out_5512660098689845475[13] = 1;
-   out_5512660098689845475[14] = 0;
-   out_5512660098689845475[15] = 0;
-   out_5512660098689845475[16] = 0;
-   out_5512660098689845475[17] = 0;
-   out_5512660098689845475[18] = 0;
-   out_5512660098689845475[19] = 0;
-   out_5512660098689845475[20] = 0;
-   out_5512660098689845475[21] = 0;
-   out_5512660098689845475[22] = 0;
-   out_5512660098689845475[23] = 0;
-   out_5512660098689845475[24] = 0;
-   out_5512660098689845475[25] = 0;
-   out_5512660098689845475[26] = 0;
-   out_5512660098689845475[27] = 0;
-   out_5512660098689845475[28] = 0;
-   out_5512660098689845475[29] = 0;
-   out_5512660098689845475[30] = 0;
-   out_5512660098689845475[31] = 0;
-   out_5512660098689845475[32] = 0;
-   out_5512660098689845475[33] = 1;
-   out_5512660098689845475[34] = 0;
-   out_5512660098689845475[35] = 0;
-   out_5512660098689845475[36] = 1;
-   out_5512660098689845475[37] = 0;
-   out_5512660098689845475[38] = 0;
-   out_5512660098689845475[39] = 0;
-   out_5512660098689845475[40] = 0;
-   out_5512660098689845475[41] = 0;
-   out_5512660098689845475[42] = 0;
-   out_5512660098689845475[43] = 0;
-   out_5512660098689845475[44] = 0;
-   out_5512660098689845475[45] = 0;
-   out_5512660098689845475[46] = 0;
-   out_5512660098689845475[47] = 0;
-   out_5512660098689845475[48] = 0;
-   out_5512660098689845475[49] = 0;
-   out_5512660098689845475[50] = 0;
-   out_5512660098689845475[51] = 0;
-   out_5512660098689845475[52] = 0;
-   out_5512660098689845475[53] = 0;
-   out_5512660098689845475[54] = 0;
-   out_5512660098689845475[55] = 0;
-   out_5512660098689845475[56] = 1;
-   out_5512660098689845475[57] = 0;
-   out_5512660098689845475[58] = 0;
-   out_5512660098689845475[59] = 1;
-   out_5512660098689845475[60] = 0;
-   out_5512660098689845475[61] = 0;
-   out_5512660098689845475[62] = 0;
-   out_5512660098689845475[63] = 0;
-   out_5512660098689845475[64] = 0;
-   out_5512660098689845475[65] = 0;
+void H_4(double *state, double *unused, double *out_2805776108884249108) {
+   out_2805776108884249108[0] = 0;
+   out_2805776108884249108[1] = 0;
+   out_2805776108884249108[2] = 0;
+   out_2805776108884249108[3] = 0;
+   out_2805776108884249108[4] = 0;
+   out_2805776108884249108[5] = 0;
+   out_2805776108884249108[6] = 0;
+   out_2805776108884249108[7] = 0;
+   out_2805776108884249108[8] = 0;
+   out_2805776108884249108[9] = 0;
+   out_2805776108884249108[10] = 1;
+   out_2805776108884249108[11] = 0;
+   out_2805776108884249108[12] = 0;
+   out_2805776108884249108[13] = 1;
+   out_2805776108884249108[14] = 0;
+   out_2805776108884249108[15] = 0;
+   out_2805776108884249108[16] = 0;
+   out_2805776108884249108[17] = 0;
+   out_2805776108884249108[18] = 0;
+   out_2805776108884249108[19] = 0;
+   out_2805776108884249108[20] = 0;
+   out_2805776108884249108[21] = 0;
+   out_2805776108884249108[22] = 0;
+   out_2805776108884249108[23] = 0;
+   out_2805776108884249108[24] = 0;
+   out_2805776108884249108[25] = 0;
+   out_2805776108884249108[26] = 0;
+   out_2805776108884249108[27] = 0;
+   out_2805776108884249108[28] = 0;
+   out_2805776108884249108[29] = 0;
+   out_2805776108884249108[30] = 0;
+   out_2805776108884249108[31] = 0;
+   out_2805776108884249108[32] = 0;
+   out_2805776108884249108[33] = 1;
+   out_2805776108884249108[34] = 0;
+   out_2805776108884249108[35] = 0;
+   out_2805776108884249108[36] = 1;
+   out_2805776108884249108[37] = 0;
+   out_2805776108884249108[38] = 0;
+   out_2805776108884249108[39] = 0;
+   out_2805776108884249108[40] = 0;
+   out_2805776108884249108[41] = 0;
+   out_2805776108884249108[42] = 0;
+   out_2805776108884249108[43] = 0;
+   out_2805776108884249108[44] = 0;
+   out_2805776108884249108[45] = 0;
+   out_2805776108884249108[46] = 0;
+   out_2805776108884249108[47] = 0;
+   out_2805776108884249108[48] = 0;
+   out_2805776108884249108[49] = 0;
+   out_2805776108884249108[50] = 0;
+   out_2805776108884249108[51] = 0;
+   out_2805776108884249108[52] = 0;
+   out_2805776108884249108[53] = 0;
+   out_2805776108884249108[54] = 0;
+   out_2805776108884249108[55] = 0;
+   out_2805776108884249108[56] = 1;
+   out_2805776108884249108[57] = 0;
+   out_2805776108884249108[58] = 0;
+   out_2805776108884249108[59] = 1;
+   out_2805776108884249108[60] = 0;
+   out_2805776108884249108[61] = 0;
+   out_2805776108884249108[62] = 0;
+   out_2805776108884249108[63] = 0;
+   out_2805776108884249108[64] = 0;
+   out_2805776108884249108[65] = 0;
 }
-void h_9(double *state, double *unused, double *out_4264504783842197092) {
-   out_4264504783842197092[0] = state[10];
-   out_4264504783842197092[1] = state[11];
-   out_4264504783842197092[2] = state[12];
+void h_9(double *state, double *unused, double *out_5944074116138401933) {
+   out_5944074116138401933[0] = state[10];
+   out_5944074116138401933[1] = state[11];
+   out_5944074116138401933[2] = state[12];
 }
-void H_9(double *state, double *unused, double *out_5271470452060254830) {
-   out_5271470452060254830[0] = 0;
-   out_5271470452060254830[1] = 0;
-   out_5271470452060254830[2] = 0;
-   out_5271470452060254830[3] = 0;
-   out_5271470452060254830[4] = 0;
-   out_5271470452060254830[5] = 0;
-   out_5271470452060254830[6] = 0;
-   out_5271470452060254830[7] = 0;
-   out_5271470452060254830[8] = 0;
-   out_5271470452060254830[9] = 0;
-   out_5271470452060254830[10] = 1;
-   out_5271470452060254830[11] = 0;
-   out_5271470452060254830[12] = 0;
-   out_5271470452060254830[13] = 0;
-   out_5271470452060254830[14] = 0;
-   out_5271470452060254830[15] = 0;
-   out_5271470452060254830[16] = 0;
-   out_5271470452060254830[17] = 0;
-   out_5271470452060254830[18] = 0;
-   out_5271470452060254830[19] = 0;
-   out_5271470452060254830[20] = 0;
-   out_5271470452060254830[21] = 0;
-   out_5271470452060254830[22] = 0;
-   out_5271470452060254830[23] = 0;
-   out_5271470452060254830[24] = 0;
-   out_5271470452060254830[25] = 0;
-   out_5271470452060254830[26] = 0;
-   out_5271470452060254830[27] = 0;
-   out_5271470452060254830[28] = 0;
-   out_5271470452060254830[29] = 0;
-   out_5271470452060254830[30] = 0;
-   out_5271470452060254830[31] = 0;
-   out_5271470452060254830[32] = 0;
-   out_5271470452060254830[33] = 1;
-   out_5271470452060254830[34] = 0;
-   out_5271470452060254830[35] = 0;
-   out_5271470452060254830[36] = 0;
-   out_5271470452060254830[37] = 0;
-   out_5271470452060254830[38] = 0;
-   out_5271470452060254830[39] = 0;
-   out_5271470452060254830[40] = 0;
-   out_5271470452060254830[41] = 0;
-   out_5271470452060254830[42] = 0;
-   out_5271470452060254830[43] = 0;
-   out_5271470452060254830[44] = 0;
-   out_5271470452060254830[45] = 0;
-   out_5271470452060254830[46] = 0;
-   out_5271470452060254830[47] = 0;
-   out_5271470452060254830[48] = 0;
-   out_5271470452060254830[49] = 0;
-   out_5271470452060254830[50] = 0;
-   out_5271470452060254830[51] = 0;
-   out_5271470452060254830[52] = 0;
-   out_5271470452060254830[53] = 0;
-   out_5271470452060254830[54] = 0;
-   out_5271470452060254830[55] = 0;
-   out_5271470452060254830[56] = 1;
-   out_5271470452060254830[57] = 0;
-   out_5271470452060254830[58] = 0;
-   out_5271470452060254830[59] = 0;
-   out_5271470452060254830[60] = 0;
-   out_5271470452060254830[61] = 0;
-   out_5271470452060254830[62] = 0;
-   out_5271470452060254830[63] = 0;
-   out_5271470452060254830[64] = 0;
-   out_5271470452060254830[65] = 0;
+void H_9(double *state, double *unused, double *out_2564586462254658463) {
+   out_2564586462254658463[0] = 0;
+   out_2564586462254658463[1] = 0;
+   out_2564586462254658463[2] = 0;
+   out_2564586462254658463[3] = 0;
+   out_2564586462254658463[4] = 0;
+   out_2564586462254658463[5] = 0;
+   out_2564586462254658463[6] = 0;
+   out_2564586462254658463[7] = 0;
+   out_2564586462254658463[8] = 0;
+   out_2564586462254658463[9] = 0;
+   out_2564586462254658463[10] = 1;
+   out_2564586462254658463[11] = 0;
+   out_2564586462254658463[12] = 0;
+   out_2564586462254658463[13] = 0;
+   out_2564586462254658463[14] = 0;
+   out_2564586462254658463[15] = 0;
+   out_2564586462254658463[16] = 0;
+   out_2564586462254658463[17] = 0;
+   out_2564586462254658463[18] = 0;
+   out_2564586462254658463[19] = 0;
+   out_2564586462254658463[20] = 0;
+   out_2564586462254658463[21] = 0;
+   out_2564586462254658463[22] = 0;
+   out_2564586462254658463[23] = 0;
+   out_2564586462254658463[24] = 0;
+   out_2564586462254658463[25] = 0;
+   out_2564586462254658463[26] = 0;
+   out_2564586462254658463[27] = 0;
+   out_2564586462254658463[28] = 0;
+   out_2564586462254658463[29] = 0;
+   out_2564586462254658463[30] = 0;
+   out_2564586462254658463[31] = 0;
+   out_2564586462254658463[32] = 0;
+   out_2564586462254658463[33] = 1;
+   out_2564586462254658463[34] = 0;
+   out_2564586462254658463[35] = 0;
+   out_2564586462254658463[36] = 0;
+   out_2564586462254658463[37] = 0;
+   out_2564586462254658463[38] = 0;
+   out_2564586462254658463[39] = 0;
+   out_2564586462254658463[40] = 0;
+   out_2564586462254658463[41] = 0;
+   out_2564586462254658463[42] = 0;
+   out_2564586462254658463[43] = 0;
+   out_2564586462254658463[44] = 0;
+   out_2564586462254658463[45] = 0;
+   out_2564586462254658463[46] = 0;
+   out_2564586462254658463[47] = 0;
+   out_2564586462254658463[48] = 0;
+   out_2564586462254658463[49] = 0;
+   out_2564586462254658463[50] = 0;
+   out_2564586462254658463[51] = 0;
+   out_2564586462254658463[52] = 0;
+   out_2564586462254658463[53] = 0;
+   out_2564586462254658463[54] = 0;
+   out_2564586462254658463[55] = 0;
+   out_2564586462254658463[56] = 1;
+   out_2564586462254658463[57] = 0;
+   out_2564586462254658463[58] = 0;
+   out_2564586462254658463[59] = 0;
+   out_2564586462254658463[60] = 0;
+   out_2564586462254658463[61] = 0;
+   out_2564586462254658463[62] = 0;
+   out_2564586462254658463[63] = 0;
+   out_2564586462254658463[64] = 0;
+   out_2564586462254658463[65] = 0;
 }
-void h_10(double *state, double *unused, double *out_1276705416788479855) {
-   out_1276705416788479855[0] = 398600500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2] + 398600500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0] + state[16] + state[19];
-   out_1276705416788479855[1] = 398600500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2] + 398600500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[1] + state[17] + state[20];
-   out_1276705416788479855[2] = 398600500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1] + 398600500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[2] + state[18] + state[21];
+void h_10(double *state, double *unused, double *out_3024176660760462318) {
+   out_3024176660760462318[0] = 398600500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2] + 398600500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0] + state[16] + state[19];
+   out_3024176660760462318[1] = 398600500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2] + 398600500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[1] + state[17] + state[20];
+   out_3024176660760462318[2] = 398600500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1] + 398600500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[2] + state[18] + state[21];
 }
-void H_10(double *state, double *unused, double *out_6589591562994162559) {
-   out_6589591562994162559[0] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*pow(state[0], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2));
-   out_6589591562994162559[1] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[1], 2) + 398600500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0]*state[1];
-   out_6589591562994162559[2] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[2], 2) + 398600500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0]*state[2];
-   out_6589591562994162559[3] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5];
-   out_6589591562994162559[4] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6];
-   out_6589591562994162559[5] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3];
-   out_6589591562994162559[6] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4];
-   out_6589591562994162559[7] = 0;
-   out_6589591562994162559[8] = 0;
-   out_6589591562994162559[9] = 0;
-   out_6589591562994162559[10] = 0;
-   out_6589591562994162559[11] = 0;
-   out_6589591562994162559[12] = 0;
-   out_6589591562994162559[13] = 0;
-   out_6589591562994162559[14] = 0;
-   out_6589591562994162559[15] = 0;
-   out_6589591562994162559[16] = 1;
-   out_6589591562994162559[17] = 0;
-   out_6589591562994162559[18] = 0;
-   out_6589591562994162559[19] = 1;
-   out_6589591562994162559[20] = 0;
-   out_6589591562994162559[21] = 0;
-   out_6589591562994162559[22] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[0], 2) + 398600500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[0]*state[1];
-   out_6589591562994162559[23] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*pow(state[1], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2));
-   out_6589591562994162559[24] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[2], 2) + 398600500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[1]*state[2];
-   out_6589591562994162559[25] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4];
-   out_6589591562994162559[26] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3];
-   out_6589591562994162559[27] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6];
-   out_6589591562994162559[28] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5];
-   out_6589591562994162559[29] = 0;
-   out_6589591562994162559[30] = 0;
-   out_6589591562994162559[31] = 0;
-   out_6589591562994162559[32] = 0;
-   out_6589591562994162559[33] = 0;
-   out_6589591562994162559[34] = 0;
-   out_6589591562994162559[35] = 0;
-   out_6589591562994162559[36] = 0;
-   out_6589591562994162559[37] = 0;
-   out_6589591562994162559[38] = 0;
-   out_6589591562994162559[39] = 1;
-   out_6589591562994162559[40] = 0;
-   out_6589591562994162559[41] = 0;
-   out_6589591562994162559[42] = 1;
-   out_6589591562994162559[43] = 0;
-   out_6589591562994162559[44] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[0], 2) + 398600500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[0]*state[2];
-   out_6589591562994162559[45] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[1], 2) + 398600500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[1]*state[2];
-   out_6589591562994162559[46] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*pow(state[2], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2));
-   out_6589591562994162559[47] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3];
-   out_6589591562994162559[48] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4];
-   out_6589591562994162559[49] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5];
-   out_6589591562994162559[50] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6];
-   out_6589591562994162559[51] = 0;
-   out_6589591562994162559[52] = 0;
-   out_6589591562994162559[53] = 0;
-   out_6589591562994162559[54] = 0;
-   out_6589591562994162559[55] = 0;
-   out_6589591562994162559[56] = 0;
-   out_6589591562994162559[57] = 0;
-   out_6589591562994162559[58] = 0;
-   out_6589591562994162559[59] = 0;
-   out_6589591562994162559[60] = 0;
-   out_6589591562994162559[61] = 0;
-   out_6589591562994162559[62] = 1;
-   out_6589591562994162559[63] = 0;
-   out_6589591562994162559[64] = 0;
-   out_6589591562994162559[65] = 1;
+void H_10(double *state, double *unused, double *out_3594957374532062379) {
+   out_3594957374532062379[0] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*pow(state[0], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2));
+   out_3594957374532062379[1] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[1], 2) + 398600500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0]*state[1];
+   out_3594957374532062379[2] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[2], 2) + 398600500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0]*state[2];
+   out_3594957374532062379[3] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5];
+   out_3594957374532062379[4] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6];
+   out_3594957374532062379[5] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3];
+   out_3594957374532062379[6] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4];
+   out_3594957374532062379[7] = 0;
+   out_3594957374532062379[8] = 0;
+   out_3594957374532062379[9] = 0;
+   out_3594957374532062379[10] = 0;
+   out_3594957374532062379[11] = 0;
+   out_3594957374532062379[12] = 0;
+   out_3594957374532062379[13] = 0;
+   out_3594957374532062379[14] = 0;
+   out_3594957374532062379[15] = 0;
+   out_3594957374532062379[16] = 1;
+   out_3594957374532062379[17] = 0;
+   out_3594957374532062379[18] = 0;
+   out_3594957374532062379[19] = 1;
+   out_3594957374532062379[20] = 0;
+   out_3594957374532062379[21] = 0;
+   out_3594957374532062379[22] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[0], 2) + 398600500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[0]*state[1];
+   out_3594957374532062379[23] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*pow(state[1], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2));
+   out_3594957374532062379[24] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[2], 2) + 398600500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[1]*state[2];
+   out_3594957374532062379[25] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4];
+   out_3594957374532062379[26] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3];
+   out_3594957374532062379[27] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6];
+   out_3594957374532062379[28] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5];
+   out_3594957374532062379[29] = 0;
+   out_3594957374532062379[30] = 0;
+   out_3594957374532062379[31] = 0;
+   out_3594957374532062379[32] = 0;
+   out_3594957374532062379[33] = 0;
+   out_3594957374532062379[34] = 0;
+   out_3594957374532062379[35] = 0;
+   out_3594957374532062379[36] = 0;
+   out_3594957374532062379[37] = 0;
+   out_3594957374532062379[38] = 0;
+   out_3594957374532062379[39] = 1;
+   out_3594957374532062379[40] = 0;
+   out_3594957374532062379[41] = 0;
+   out_3594957374532062379[42] = 1;
+   out_3594957374532062379[43] = 0;
+   out_3594957374532062379[44] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[0], 2) + 398600500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[0]*state[2];
+   out_3594957374532062379[45] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[1], 2) + 398600500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[1]*state[2];
+   out_3594957374532062379[46] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*pow(state[2], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2));
+   out_3594957374532062379[47] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3];
+   out_3594957374532062379[48] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4];
+   out_3594957374532062379[49] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5];
+   out_3594957374532062379[50] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6];
+   out_3594957374532062379[51] = 0;
+   out_3594957374532062379[52] = 0;
+   out_3594957374532062379[53] = 0;
+   out_3594957374532062379[54] = 0;
+   out_3594957374532062379[55] = 0;
+   out_3594957374532062379[56] = 0;
+   out_3594957374532062379[57] = 0;
+   out_3594957374532062379[58] = 0;
+   out_3594957374532062379[59] = 0;
+   out_3594957374532062379[60] = 0;
+   out_3594957374532062379[61] = 0;
+   out_3594957374532062379[62] = 1;
+   out_3594957374532062379[63] = 0;
+   out_3594957374532062379[64] = 0;
+   out_3594957374532062379[65] = 1;
 }
-void h_12(double *state, double *unused, double *out_4489758644873872208) {
-   out_4489758644873872208[0] = state[0];
-   out_4489758644873872208[1] = state[1];
-   out_4489758644873872208[2] = state[2];
+void h_12(double *state, double *unused, double *out_1781567485764299655) {
+   out_1781567485764299655[0] = state[0];
+   out_1781567485764299655[1] = state[1];
+   out_1781567485764299655[2] = state[2];
 }
-void H_12(double *state, double *unused, double *out_493203690657883680) {
-   out_493203690657883680[0] = 1;
-   out_493203690657883680[1] = 0;
-   out_493203690657883680[2] = 0;
-   out_493203690657883680[3] = 0;
-   out_493203690657883680[4] = 0;
-   out_493203690657883680[5] = 0;
-   out_493203690657883680[6] = 0;
-   out_493203690657883680[7] = 0;
-   out_493203690657883680[8] = 0;
-   out_493203690657883680[9] = 0;
-   out_493203690657883680[10] = 0;
-   out_493203690657883680[11] = 0;
-   out_493203690657883680[12] = 0;
-   out_493203690657883680[13] = 0;
-   out_493203690657883680[14] = 0;
-   out_493203690657883680[15] = 0;
-   out_493203690657883680[16] = 0;
-   out_493203690657883680[17] = 0;
-   out_493203690657883680[18] = 0;
-   out_493203690657883680[19] = 0;
-   out_493203690657883680[20] = 0;
-   out_493203690657883680[21] = 0;
-   out_493203690657883680[22] = 0;
-   out_493203690657883680[23] = 1;
-   out_493203690657883680[24] = 0;
-   out_493203690657883680[25] = 0;
-   out_493203690657883680[26] = 0;
-   out_493203690657883680[27] = 0;
-   out_493203690657883680[28] = 0;
-   out_493203690657883680[29] = 0;
-   out_493203690657883680[30] = 0;
-   out_493203690657883680[31] = 0;
-   out_493203690657883680[32] = 0;
-   out_493203690657883680[33] = 0;
-   out_493203690657883680[34] = 0;
-   out_493203690657883680[35] = 0;
-   out_493203690657883680[36] = 0;
-   out_493203690657883680[37] = 0;
-   out_493203690657883680[38] = 0;
-   out_493203690657883680[39] = 0;
-   out_493203690657883680[40] = 0;
-   out_493203690657883680[41] = 0;
-   out_493203690657883680[42] = 0;
-   out_493203690657883680[43] = 0;
-   out_493203690657883680[44] = 0;
-   out_493203690657883680[45] = 0;
-   out_493203690657883680[46] = 1;
-   out_493203690657883680[47] = 0;
-   out_493203690657883680[48] = 0;
-   out_493203690657883680[49] = 0;
-   out_493203690657883680[50] = 0;
-   out_493203690657883680[51] = 0;
-   out_493203690657883680[52] = 0;
-   out_493203690657883680[53] = 0;
-   out_493203690657883680[54] = 0;
-   out_493203690657883680[55] = 0;
-   out_493203690657883680[56] = 0;
-   out_493203690657883680[57] = 0;
-   out_493203690657883680[58] = 0;
-   out_493203690657883680[59] = 0;
-   out_493203690657883680[60] = 0;
-   out_493203690657883680[61] = 0;
-   out_493203690657883680[62] = 0;
-   out_493203690657883680[63] = 0;
-   out_493203690657883680[64] = 0;
-   out_493203690657883680[65] = 0;
+void H_12(double *state, double *unused, double *out_2213680299147712687) {
+   out_2213680299147712687[0] = 1;
+   out_2213680299147712687[1] = 0;
+   out_2213680299147712687[2] = 0;
+   out_2213680299147712687[3] = 0;
+   out_2213680299147712687[4] = 0;
+   out_2213680299147712687[5] = 0;
+   out_2213680299147712687[6] = 0;
+   out_2213680299147712687[7] = 0;
+   out_2213680299147712687[8] = 0;
+   out_2213680299147712687[9] = 0;
+   out_2213680299147712687[10] = 0;
+   out_2213680299147712687[11] = 0;
+   out_2213680299147712687[12] = 0;
+   out_2213680299147712687[13] = 0;
+   out_2213680299147712687[14] = 0;
+   out_2213680299147712687[15] = 0;
+   out_2213680299147712687[16] = 0;
+   out_2213680299147712687[17] = 0;
+   out_2213680299147712687[18] = 0;
+   out_2213680299147712687[19] = 0;
+   out_2213680299147712687[20] = 0;
+   out_2213680299147712687[21] = 0;
+   out_2213680299147712687[22] = 0;
+   out_2213680299147712687[23] = 1;
+   out_2213680299147712687[24] = 0;
+   out_2213680299147712687[25] = 0;
+   out_2213680299147712687[26] = 0;
+   out_2213680299147712687[27] = 0;
+   out_2213680299147712687[28] = 0;
+   out_2213680299147712687[29] = 0;
+   out_2213680299147712687[30] = 0;
+   out_2213680299147712687[31] = 0;
+   out_2213680299147712687[32] = 0;
+   out_2213680299147712687[33] = 0;
+   out_2213680299147712687[34] = 0;
+   out_2213680299147712687[35] = 0;
+   out_2213680299147712687[36] = 0;
+   out_2213680299147712687[37] = 0;
+   out_2213680299147712687[38] = 0;
+   out_2213680299147712687[39] = 0;
+   out_2213680299147712687[40] = 0;
+   out_2213680299147712687[41] = 0;
+   out_2213680299147712687[42] = 0;
+   out_2213680299147712687[43] = 0;
+   out_2213680299147712687[44] = 0;
+   out_2213680299147712687[45] = 0;
+   out_2213680299147712687[46] = 1;
+   out_2213680299147712687[47] = 0;
+   out_2213680299147712687[48] = 0;
+   out_2213680299147712687[49] = 0;
+   out_2213680299147712687[50] = 0;
+   out_2213680299147712687[51] = 0;
+   out_2213680299147712687[52] = 0;
+   out_2213680299147712687[53] = 0;
+   out_2213680299147712687[54] = 0;
+   out_2213680299147712687[55] = 0;
+   out_2213680299147712687[56] = 0;
+   out_2213680299147712687[57] = 0;
+   out_2213680299147712687[58] = 0;
+   out_2213680299147712687[59] = 0;
+   out_2213680299147712687[60] = 0;
+   out_2213680299147712687[61] = 0;
+   out_2213680299147712687[62] = 0;
+   out_2213680299147712687[63] = 0;
+   out_2213680299147712687[64] = 0;
+   out_2213680299147712687[65] = 0;
 }
-void h_35(double *state, double *unused, double *out_8696122944070004010) {
-   out_8696122944070004010[0] = state[7];
-   out_8696122944070004010[1] = state[8];
-   out_8696122944070004010[2] = state[9];
+void h_35(double *state, double *unused, double *out_3392135172365373817) {
+   out_3392135172365373817[0] = state[7];
+   out_3392135172365373817[1] = state[8];
+   out_3392135172365373817[2] = state[9];
 }
-void H_35(double *state, double *unused, double *out_2252359341667130029) {
-   out_2252359341667130029[0] = 0;
-   out_2252359341667130029[1] = 0;
-   out_2252359341667130029[2] = 0;
-   out_2252359341667130029[3] = 0;
-   out_2252359341667130029[4] = 0;
-   out_2252359341667130029[5] = 0;
-   out_2252359341667130029[6] = 0;
-   out_2252359341667130029[7] = 1;
-   out_2252359341667130029[8] = 0;
-   out_2252359341667130029[9] = 0;
-   out_2252359341667130029[10] = 0;
-   out_2252359341667130029[11] = 0;
-   out_2252359341667130029[12] = 0;
-   out_2252359341667130029[13] = 0;
-   out_2252359341667130029[14] = 0;
-   out_2252359341667130029[15] = 0;
-   out_2252359341667130029[16] = 0;
-   out_2252359341667130029[17] = 0;
-   out_2252359341667130029[18] = 0;
-   out_2252359341667130029[19] = 0;
-   out_2252359341667130029[20] = 0;
-   out_2252359341667130029[21] = 0;
-   out_2252359341667130029[22] = 0;
-   out_2252359341667130029[23] = 0;
-   out_2252359341667130029[24] = 0;
-   out_2252359341667130029[25] = 0;
-   out_2252359341667130029[26] = 0;
-   out_2252359341667130029[27] = 0;
-   out_2252359341667130029[28] = 0;
-   out_2252359341667130029[29] = 0;
-   out_2252359341667130029[30] = 1;
-   out_2252359341667130029[31] = 0;
-   out_2252359341667130029[32] = 0;
-   out_2252359341667130029[33] = 0;
-   out_2252359341667130029[34] = 0;
-   out_2252359341667130029[35] = 0;
-   out_2252359341667130029[36] = 0;
-   out_2252359341667130029[37] = 0;
-   out_2252359341667130029[38] = 0;
-   out_2252359341667130029[39] = 0;
-   out_2252359341667130029[40] = 0;
-   out_2252359341667130029[41] = 0;
-   out_2252359341667130029[42] = 0;
-   out_2252359341667130029[43] = 0;
-   out_2252359341667130029[44] = 0;
-   out_2252359341667130029[45] = 0;
-   out_2252359341667130029[46] = 0;
-   out_2252359341667130029[47] = 0;
-   out_2252359341667130029[48] = 0;
-   out_2252359341667130029[49] = 0;
-   out_2252359341667130029[50] = 0;
-   out_2252359341667130029[51] = 0;
-   out_2252359341667130029[52] = 0;
-   out_2252359341667130029[53] = 1;
-   out_2252359341667130029[54] = 0;
-   out_2252359341667130029[55] = 0;
-   out_2252359341667130029[56] = 0;
-   out_2252359341667130029[57] = 0;
-   out_2252359341667130029[58] = 0;
-   out_2252359341667130029[59] = 0;
-   out_2252359341667130029[60] = 0;
-   out_2252359341667130029[61] = 0;
-   out_2252359341667130029[62] = 0;
-   out_2252359341667130029[63] = 0;
-   out_2252359341667130029[64] = 0;
-   out_2252359341667130029[65] = 0;
+void H_35(double *state, double *unused, double *out_4959243331472726396) {
+   out_4959243331472726396[0] = 0;
+   out_4959243331472726396[1] = 0;
+   out_4959243331472726396[2] = 0;
+   out_4959243331472726396[3] = 0;
+   out_4959243331472726396[4] = 0;
+   out_4959243331472726396[5] = 0;
+   out_4959243331472726396[6] = 0;
+   out_4959243331472726396[7] = 1;
+   out_4959243331472726396[8] = 0;
+   out_4959243331472726396[9] = 0;
+   out_4959243331472726396[10] = 0;
+   out_4959243331472726396[11] = 0;
+   out_4959243331472726396[12] = 0;
+   out_4959243331472726396[13] = 0;
+   out_4959243331472726396[14] = 0;
+   out_4959243331472726396[15] = 0;
+   out_4959243331472726396[16] = 0;
+   out_4959243331472726396[17] = 0;
+   out_4959243331472726396[18] = 0;
+   out_4959243331472726396[19] = 0;
+   out_4959243331472726396[20] = 0;
+   out_4959243331472726396[21] = 0;
+   out_4959243331472726396[22] = 0;
+   out_4959243331472726396[23] = 0;
+   out_4959243331472726396[24] = 0;
+   out_4959243331472726396[25] = 0;
+   out_4959243331472726396[26] = 0;
+   out_4959243331472726396[27] = 0;
+   out_4959243331472726396[28] = 0;
+   out_4959243331472726396[29] = 0;
+   out_4959243331472726396[30] = 1;
+   out_4959243331472726396[31] = 0;
+   out_4959243331472726396[32] = 0;
+   out_4959243331472726396[33] = 0;
+   out_4959243331472726396[34] = 0;
+   out_4959243331472726396[35] = 0;
+   out_4959243331472726396[36] = 0;
+   out_4959243331472726396[37] = 0;
+   out_4959243331472726396[38] = 0;
+   out_4959243331472726396[39] = 0;
+   out_4959243331472726396[40] = 0;
+   out_4959243331472726396[41] = 0;
+   out_4959243331472726396[42] = 0;
+   out_4959243331472726396[43] = 0;
+   out_4959243331472726396[44] = 0;
+   out_4959243331472726396[45] = 0;
+   out_4959243331472726396[46] = 0;
+   out_4959243331472726396[47] = 0;
+   out_4959243331472726396[48] = 0;
+   out_4959243331472726396[49] = 0;
+   out_4959243331472726396[50] = 0;
+   out_4959243331472726396[51] = 0;
+   out_4959243331472726396[52] = 0;
+   out_4959243331472726396[53] = 1;
+   out_4959243331472726396[54] = 0;
+   out_4959243331472726396[55] = 0;
+   out_4959243331472726396[56] = 0;
+   out_4959243331472726396[57] = 0;
+   out_4959243331472726396[58] = 0;
+   out_4959243331472726396[59] = 0;
+   out_4959243331472726396[60] = 0;
+   out_4959243331472726396[61] = 0;
+   out_4959243331472726396[62] = 0;
+   out_4959243331472726396[63] = 0;
+   out_4959243331472726396[64] = 0;
+   out_4959243331472726396[65] = 0;
 }
-void h_32(double *state, double *unused, double *out_2592146495080937647) {
-   out_2592146495080937647[0] = state[3];
-   out_2592146495080937647[1] = state[4];
-   out_2592146495080937647[2] = state[5];
-   out_2592146495080937647[3] = state[6];
+void h_32(double *state, double *unused, double *out_5713026107165599538) {
+   out_5713026107165599538[0] = state[3];
+   out_5713026107165599538[1] = state[4];
+   out_5713026107165599538[2] = state[5];
+   out_5713026107165599538[3] = state[6];
 }
-void H_32(double *state, double *unused, double *out_8522971196901590084) {
-   out_8522971196901590084[0] = 0;
-   out_8522971196901590084[1] = 0;
-   out_8522971196901590084[2] = 0;
-   out_8522971196901590084[3] = 1;
-   out_8522971196901590084[4] = 0;
-   out_8522971196901590084[5] = 0;
-   out_8522971196901590084[6] = 0;
-   out_8522971196901590084[7] = 0;
-   out_8522971196901590084[8] = 0;
-   out_8522971196901590084[9] = 0;
-   out_8522971196901590084[10] = 0;
-   out_8522971196901590084[11] = 0;
-   out_8522971196901590084[12] = 0;
-   out_8522971196901590084[13] = 0;
-   out_8522971196901590084[14] = 0;
-   out_8522971196901590084[15] = 0;
-   out_8522971196901590084[16] = 0;
-   out_8522971196901590084[17] = 0;
-   out_8522971196901590084[18] = 0;
-   out_8522971196901590084[19] = 0;
-   out_8522971196901590084[20] = 0;
-   out_8522971196901590084[21] = 0;
-   out_8522971196901590084[22] = 0;
-   out_8522971196901590084[23] = 0;
-   out_8522971196901590084[24] = 0;
-   out_8522971196901590084[25] = 0;
-   out_8522971196901590084[26] = 1;
-   out_8522971196901590084[27] = 0;
-   out_8522971196901590084[28] = 0;
-   out_8522971196901590084[29] = 0;
-   out_8522971196901590084[30] = 0;
-   out_8522971196901590084[31] = 0;
-   out_8522971196901590084[32] = 0;
-   out_8522971196901590084[33] = 0;
-   out_8522971196901590084[34] = 0;
-   out_8522971196901590084[35] = 0;
-   out_8522971196901590084[36] = 0;
-   out_8522971196901590084[37] = 0;
-   out_8522971196901590084[38] = 0;
-   out_8522971196901590084[39] = 0;
-   out_8522971196901590084[40] = 0;
-   out_8522971196901590084[41] = 0;
-   out_8522971196901590084[42] = 0;
-   out_8522971196901590084[43] = 0;
-   out_8522971196901590084[44] = 0;
-   out_8522971196901590084[45] = 0;
-   out_8522971196901590084[46] = 0;
-   out_8522971196901590084[47] = 0;
-   out_8522971196901590084[48] = 0;
-   out_8522971196901590084[49] = 1;
-   out_8522971196901590084[50] = 0;
-   out_8522971196901590084[51] = 0;
-   out_8522971196901590084[52] = 0;
-   out_8522971196901590084[53] = 0;
-   out_8522971196901590084[54] = 0;
-   out_8522971196901590084[55] = 0;
-   out_8522971196901590084[56] = 0;
-   out_8522971196901590084[57] = 0;
-   out_8522971196901590084[58] = 0;
-   out_8522971196901590084[59] = 0;
-   out_8522971196901590084[60] = 0;
-   out_8522971196901590084[61] = 0;
-   out_8522971196901590084[62] = 0;
-   out_8522971196901590084[63] = 0;
-   out_8522971196901590084[64] = 0;
-   out_8522971196901590084[65] = 0;
-   out_8522971196901590084[66] = 0;
-   out_8522971196901590084[67] = 0;
-   out_8522971196901590084[68] = 0;
-   out_8522971196901590084[69] = 0;
-   out_8522971196901590084[70] = 0;
-   out_8522971196901590084[71] = 0;
-   out_8522971196901590084[72] = 1;
-   out_8522971196901590084[73] = 0;
-   out_8522971196901590084[74] = 0;
-   out_8522971196901590084[75] = 0;
-   out_8522971196901590084[76] = 0;
-   out_8522971196901590084[77] = 0;
-   out_8522971196901590084[78] = 0;
-   out_8522971196901590084[79] = 0;
-   out_8522971196901590084[80] = 0;
-   out_8522971196901590084[81] = 0;
-   out_8522971196901590084[82] = 0;
-   out_8522971196901590084[83] = 0;
-   out_8522971196901590084[84] = 0;
-   out_8522971196901590084[85] = 0;
-   out_8522971196901590084[86] = 0;
-   out_8522971196901590084[87] = 0;
+void H_32(double *state, double *unused, double *out_8507087671459885125) {
+   out_8507087671459885125[0] = 0;
+   out_8507087671459885125[1] = 0;
+   out_8507087671459885125[2] = 0;
+   out_8507087671459885125[3] = 1;
+   out_8507087671459885125[4] = 0;
+   out_8507087671459885125[5] = 0;
+   out_8507087671459885125[6] = 0;
+   out_8507087671459885125[7] = 0;
+   out_8507087671459885125[8] = 0;
+   out_8507087671459885125[9] = 0;
+   out_8507087671459885125[10] = 0;
+   out_8507087671459885125[11] = 0;
+   out_8507087671459885125[12] = 0;
+   out_8507087671459885125[13] = 0;
+   out_8507087671459885125[14] = 0;
+   out_8507087671459885125[15] = 0;
+   out_8507087671459885125[16] = 0;
+   out_8507087671459885125[17] = 0;
+   out_8507087671459885125[18] = 0;
+   out_8507087671459885125[19] = 0;
+   out_8507087671459885125[20] = 0;
+   out_8507087671459885125[21] = 0;
+   out_8507087671459885125[22] = 0;
+   out_8507087671459885125[23] = 0;
+   out_8507087671459885125[24] = 0;
+   out_8507087671459885125[25] = 0;
+   out_8507087671459885125[26] = 1;
+   out_8507087671459885125[27] = 0;
+   out_8507087671459885125[28] = 0;
+   out_8507087671459885125[29] = 0;
+   out_8507087671459885125[30] = 0;
+   out_8507087671459885125[31] = 0;
+   out_8507087671459885125[32] = 0;
+   out_8507087671459885125[33] = 0;
+   out_8507087671459885125[34] = 0;
+   out_8507087671459885125[35] = 0;
+   out_8507087671459885125[36] = 0;
+   out_8507087671459885125[37] = 0;
+   out_8507087671459885125[38] = 0;
+   out_8507087671459885125[39] = 0;
+   out_8507087671459885125[40] = 0;
+   out_8507087671459885125[41] = 0;
+   out_8507087671459885125[42] = 0;
+   out_8507087671459885125[43] = 0;
+   out_8507087671459885125[44] = 0;
+   out_8507087671459885125[45] = 0;
+   out_8507087671459885125[46] = 0;
+   out_8507087671459885125[47] = 0;
+   out_8507087671459885125[48] = 0;
+   out_8507087671459885125[49] = 1;
+   out_8507087671459885125[50] = 0;
+   out_8507087671459885125[51] = 0;
+   out_8507087671459885125[52] = 0;
+   out_8507087671459885125[53] = 0;
+   out_8507087671459885125[54] = 0;
+   out_8507087671459885125[55] = 0;
+   out_8507087671459885125[56] = 0;
+   out_8507087671459885125[57] = 0;
+   out_8507087671459885125[58] = 0;
+   out_8507087671459885125[59] = 0;
+   out_8507087671459885125[60] = 0;
+   out_8507087671459885125[61] = 0;
+   out_8507087671459885125[62] = 0;
+   out_8507087671459885125[63] = 0;
+   out_8507087671459885125[64] = 0;
+   out_8507087671459885125[65] = 0;
+   out_8507087671459885125[66] = 0;
+   out_8507087671459885125[67] = 0;
+   out_8507087671459885125[68] = 0;
+   out_8507087671459885125[69] = 0;
+   out_8507087671459885125[70] = 0;
+   out_8507087671459885125[71] = 0;
+   out_8507087671459885125[72] = 1;
+   out_8507087671459885125[73] = 0;
+   out_8507087671459885125[74] = 0;
+   out_8507087671459885125[75] = 0;
+   out_8507087671459885125[76] = 0;
+   out_8507087671459885125[77] = 0;
+   out_8507087671459885125[78] = 0;
+   out_8507087671459885125[79] = 0;
+   out_8507087671459885125[80] = 0;
+   out_8507087671459885125[81] = 0;
+   out_8507087671459885125[82] = 0;
+   out_8507087671459885125[83] = 0;
+   out_8507087671459885125[84] = 0;
+   out_8507087671459885125[85] = 0;
+   out_8507087671459885125[86] = 0;
+   out_8507087671459885125[87] = 0;
 }
-void h_13(double *state, double *unused, double *out_5320318911182793356) {
-   out_5320318911182793356[0] = (-2*state[3]*state[5] + 2*state[4]*state[6])*state[9] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[8] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[7];
-   out_5320318911182793356[1] = (2*state[3]*state[4] + 2*state[5]*state[6])*state[9] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[7] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[8];
-   out_5320318911182793356[2] = (-2*state[3]*state[4] + 2*state[5]*state[6])*state[8] + (2*state[3]*state[5] + 2*state[4]*state[6])*state[7] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[9];
+void h_13(double *state, double *unused, double *out_8591731605088062953) {
+   out_8591731605088062953[0] = (-2*state[3]*state[5] + 2*state[4]*state[6])*state[9] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[8] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[7];
+   out_8591731605088062953[1] = (2*state[3]*state[4] + 2*state[5]*state[6])*state[9] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[7] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[8];
+   out_8591731605088062953[2] = (-2*state[3]*state[4] + 2*state[5]*state[6])*state[8] + (2*state[3]*state[5] + 2*state[4]*state[6])*state[7] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[9];
 }
-void H_13(double *state, double *unused, double *out_6260235854627074248) {
-   out_6260235854627074248[0] = 0;
-   out_6260235854627074248[1] = 0;
-   out_6260235854627074248[2] = 0;
-   out_6260235854627074248[3] = 2*state[3]*state[7] - 2*state[5]*state[9] + 2*state[6]*state[8];
-   out_6260235854627074248[4] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9];
-   out_6260235854627074248[5] = -2*state[3]*state[9] + 2*state[4]*state[8] - 2*state[5]*state[7];
-   out_6260235854627074248[6] = 2*state[3]*state[8] + 2*state[4]*state[9] - 2*state[6]*state[7];
-   out_6260235854627074248[7] = pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2);
-   out_6260235854627074248[8] = 2*state[3]*state[6] + 2*state[4]*state[5];
-   out_6260235854627074248[9] = -2*state[3]*state[5] + 2*state[4]*state[6];
-   out_6260235854627074248[10] = 0;
-   out_6260235854627074248[11] = 0;
-   out_6260235854627074248[12] = 0;
-   out_6260235854627074248[13] = 0;
-   out_6260235854627074248[14] = 0;
-   out_6260235854627074248[15] = 0;
-   out_6260235854627074248[16] = 0;
-   out_6260235854627074248[17] = 0;
-   out_6260235854627074248[18] = 0;
-   out_6260235854627074248[19] = 0;
-   out_6260235854627074248[20] = 0;
-   out_6260235854627074248[21] = 0;
-   out_6260235854627074248[22] = 0;
-   out_6260235854627074248[23] = 0;
-   out_6260235854627074248[24] = 0;
-   out_6260235854627074248[25] = 2*state[3]*state[8] + 2*state[4]*state[9] - 2*state[6]*state[7];
-   out_6260235854627074248[26] = 2*state[3]*state[9] - 2*state[4]*state[8] + 2*state[5]*state[7];
-   out_6260235854627074248[27] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9];
-   out_6260235854627074248[28] = -2*state[3]*state[7] + 2*state[5]*state[9] - 2*state[6]*state[8];
-   out_6260235854627074248[29] = -2*state[3]*state[6] + 2*state[4]*state[5];
-   out_6260235854627074248[30] = pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2);
-   out_6260235854627074248[31] = 2*state[3]*state[4] + 2*state[5]*state[6];
-   out_6260235854627074248[32] = 0;
-   out_6260235854627074248[33] = 0;
-   out_6260235854627074248[34] = 0;
-   out_6260235854627074248[35] = 0;
-   out_6260235854627074248[36] = 0;
-   out_6260235854627074248[37] = 0;
-   out_6260235854627074248[38] = 0;
-   out_6260235854627074248[39] = 0;
-   out_6260235854627074248[40] = 0;
-   out_6260235854627074248[41] = 0;
-   out_6260235854627074248[42] = 0;
-   out_6260235854627074248[43] = 0;
-   out_6260235854627074248[44] = 0;
-   out_6260235854627074248[45] = 0;
-   out_6260235854627074248[46] = 0;
-   out_6260235854627074248[47] = 2*state[3]*state[9] - 2*state[4]*state[8] + 2*state[5]*state[7];
-   out_6260235854627074248[48] = -2*state[3]*state[8] - 2*state[4]*state[9] + 2*state[6]*state[7];
-   out_6260235854627074248[49] = 2*state[3]*state[7] - 2*state[5]*state[9] + 2*state[6]*state[8];
-   out_6260235854627074248[50] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9];
-   out_6260235854627074248[51] = 2*state[3]*state[5] + 2*state[4]*state[6];
-   out_6260235854627074248[52] = -2*state[3]*state[4] + 2*state[5]*state[6];
-   out_6260235854627074248[53] = pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2);
-   out_6260235854627074248[54] = 0;
-   out_6260235854627074248[55] = 0;
-   out_6260235854627074248[56] = 0;
-   out_6260235854627074248[57] = 0;
-   out_6260235854627074248[58] = 0;
-   out_6260235854627074248[59] = 0;
-   out_6260235854627074248[60] = 0;
-   out_6260235854627074248[61] = 0;
-   out_6260235854627074248[62] = 0;
-   out_6260235854627074248[63] = 0;
-   out_6260235854627074248[64] = 0;
-   out_6260235854627074248[65] = 0;
+void H_13(double *state, double *unused, double *out_1226506526531533769) {
+   out_1226506526531533769[0] = 0;
+   out_1226506526531533769[1] = 0;
+   out_1226506526531533769[2] = 0;
+   out_1226506526531533769[3] = 2*state[3]*state[7] - 2*state[5]*state[9] + 2*state[6]*state[8];
+   out_1226506526531533769[4] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9];
+   out_1226506526531533769[5] = -2*state[3]*state[9] + 2*state[4]*state[8] - 2*state[5]*state[7];
+   out_1226506526531533769[6] = 2*state[3]*state[8] + 2*state[4]*state[9] - 2*state[6]*state[7];
+   out_1226506526531533769[7] = pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2);
+   out_1226506526531533769[8] = 2*state[3]*state[6] + 2*state[4]*state[5];
+   out_1226506526531533769[9] = -2*state[3]*state[5] + 2*state[4]*state[6];
+   out_1226506526531533769[10] = 0;
+   out_1226506526531533769[11] = 0;
+   out_1226506526531533769[12] = 0;
+   out_1226506526531533769[13] = 0;
+   out_1226506526531533769[14] = 0;
+   out_1226506526531533769[15] = 0;
+   out_1226506526531533769[16] = 0;
+   out_1226506526531533769[17] = 0;
+   out_1226506526531533769[18] = 0;
+   out_1226506526531533769[19] = 0;
+   out_1226506526531533769[20] = 0;
+   out_1226506526531533769[21] = 0;
+   out_1226506526531533769[22] = 0;
+   out_1226506526531533769[23] = 0;
+   out_1226506526531533769[24] = 0;
+   out_1226506526531533769[25] = 2*state[3]*state[8] + 2*state[4]*state[9] - 2*state[6]*state[7];
+   out_1226506526531533769[26] = 2*state[3]*state[9] - 2*state[4]*state[8] + 2*state[5]*state[7];
+   out_1226506526531533769[27] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9];
+   out_1226506526531533769[28] = -2*state[3]*state[7] + 2*state[5]*state[9] - 2*state[6]*state[8];
+   out_1226506526531533769[29] = -2*state[3]*state[6] + 2*state[4]*state[5];
+   out_1226506526531533769[30] = pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2);
+   out_1226506526531533769[31] = 2*state[3]*state[4] + 2*state[5]*state[6];
+   out_1226506526531533769[32] = 0;
+   out_1226506526531533769[33] = 0;
+   out_1226506526531533769[34] = 0;
+   out_1226506526531533769[35] = 0;
+   out_1226506526531533769[36] = 0;
+   out_1226506526531533769[37] = 0;
+   out_1226506526531533769[38] = 0;
+   out_1226506526531533769[39] = 0;
+   out_1226506526531533769[40] = 0;
+   out_1226506526531533769[41] = 0;
+   out_1226506526531533769[42] = 0;
+   out_1226506526531533769[43] = 0;
+   out_1226506526531533769[44] = 0;
+   out_1226506526531533769[45] = 0;
+   out_1226506526531533769[46] = 0;
+   out_1226506526531533769[47] = 2*state[3]*state[9] - 2*state[4]*state[8] + 2*state[5]*state[7];
+   out_1226506526531533769[48] = -2*state[3]*state[8] - 2*state[4]*state[9] + 2*state[6]*state[7];
+   out_1226506526531533769[49] = 2*state[3]*state[7] - 2*state[5]*state[9] + 2*state[6]*state[8];
+   out_1226506526531533769[50] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9];
+   out_1226506526531533769[51] = 2*state[3]*state[5] + 2*state[4]*state[6];
+   out_1226506526531533769[52] = -2*state[3]*state[4] + 2*state[5]*state[6];
+   out_1226506526531533769[53] = pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2);
+   out_1226506526531533769[54] = 0;
+   out_1226506526531533769[55] = 0;
+   out_1226506526531533769[56] = 0;
+   out_1226506526531533769[57] = 0;
+   out_1226506526531533769[58] = 0;
+   out_1226506526531533769[59] = 0;
+   out_1226506526531533769[60] = 0;
+   out_1226506526531533769[61] = 0;
+   out_1226506526531533769[62] = 0;
+   out_1226506526531533769[63] = 0;
+   out_1226506526531533769[64] = 0;
+   out_1226506526531533769[65] = 0;
 }
-void h_14(double *state, double *unused, double *out_4264504783842197092) {
-   out_4264504783842197092[0] = state[10];
-   out_4264504783842197092[1] = state[11];
-   out_4264504783842197092[2] = state[12];
+void h_14(double *state, double *unused, double *out_5944074116138401933) {
+   out_5944074116138401933[0] = state[10];
+   out_5944074116138401933[1] = state[11];
+   out_5944074116138401933[2] = state[12];
 }
-void H_14(double *state, double *unused, double *out_5271470452060254830) {
-   out_5271470452060254830[0] = 0;
-   out_5271470452060254830[1] = 0;
-   out_5271470452060254830[2] = 0;
-   out_5271470452060254830[3] = 0;
-   out_5271470452060254830[4] = 0;
-   out_5271470452060254830[5] = 0;
-   out_5271470452060254830[6] = 0;
-   out_5271470452060254830[7] = 0;
-   out_5271470452060254830[8] = 0;
-   out_5271470452060254830[9] = 0;
-   out_5271470452060254830[10] = 1;
-   out_5271470452060254830[11] = 0;
-   out_5271470452060254830[12] = 0;
-   out_5271470452060254830[13] = 0;
-   out_5271470452060254830[14] = 0;
-   out_5271470452060254830[15] = 0;
-   out_5271470452060254830[16] = 0;
-   out_5271470452060254830[17] = 0;
-   out_5271470452060254830[18] = 0;
-   out_5271470452060254830[19] = 0;
-   out_5271470452060254830[20] = 0;
-   out_5271470452060254830[21] = 0;
-   out_5271470452060254830[22] = 0;
-   out_5271470452060254830[23] = 0;
-   out_5271470452060254830[24] = 0;
-   out_5271470452060254830[25] = 0;
-   out_5271470452060254830[26] = 0;
-   out_5271470452060254830[27] = 0;
-   out_5271470452060254830[28] = 0;
-   out_5271470452060254830[29] = 0;
-   out_5271470452060254830[30] = 0;
-   out_5271470452060254830[31] = 0;
-   out_5271470452060254830[32] = 0;
-   out_5271470452060254830[33] = 1;
-   out_5271470452060254830[34] = 0;
-   out_5271470452060254830[35] = 0;
-   out_5271470452060254830[36] = 0;
-   out_5271470452060254830[37] = 0;
-   out_5271470452060254830[38] = 0;
-   out_5271470452060254830[39] = 0;
-   out_5271470452060254830[40] = 0;
-   out_5271470452060254830[41] = 0;
-   out_5271470452060254830[42] = 0;
-   out_5271470452060254830[43] = 0;
-   out_5271470452060254830[44] = 0;
-   out_5271470452060254830[45] = 0;
-   out_5271470452060254830[46] = 0;
-   out_5271470452060254830[47] = 0;
-   out_5271470452060254830[48] = 0;
-   out_5271470452060254830[49] = 0;
-   out_5271470452060254830[50] = 0;
-   out_5271470452060254830[51] = 0;
-   out_5271470452060254830[52] = 0;
-   out_5271470452060254830[53] = 0;
-   out_5271470452060254830[54] = 0;
-   out_5271470452060254830[55] = 0;
-   out_5271470452060254830[56] = 1;
-   out_5271470452060254830[57] = 0;
-   out_5271470452060254830[58] = 0;
-   out_5271470452060254830[59] = 0;
-   out_5271470452060254830[60] = 0;
-   out_5271470452060254830[61] = 0;
-   out_5271470452060254830[62] = 0;
-   out_5271470452060254830[63] = 0;
-   out_5271470452060254830[64] = 0;
-   out_5271470452060254830[65] = 0;
+void H_14(double *state, double *unused, double *out_2564586462254658463) {
+   out_2564586462254658463[0] = 0;
+   out_2564586462254658463[1] = 0;
+   out_2564586462254658463[2] = 0;
+   out_2564586462254658463[3] = 0;
+   out_2564586462254658463[4] = 0;
+   out_2564586462254658463[5] = 0;
+   out_2564586462254658463[6] = 0;
+   out_2564586462254658463[7] = 0;
+   out_2564586462254658463[8] = 0;
+   out_2564586462254658463[9] = 0;
+   out_2564586462254658463[10] = 1;
+   out_2564586462254658463[11] = 0;
+   out_2564586462254658463[12] = 0;
+   out_2564586462254658463[13] = 0;
+   out_2564586462254658463[14] = 0;
+   out_2564586462254658463[15] = 0;
+   out_2564586462254658463[16] = 0;
+   out_2564586462254658463[17] = 0;
+   out_2564586462254658463[18] = 0;
+   out_2564586462254658463[19] = 0;
+   out_2564586462254658463[20] = 0;
+   out_2564586462254658463[21] = 0;
+   out_2564586462254658463[22] = 0;
+   out_2564586462254658463[23] = 0;
+   out_2564586462254658463[24] = 0;
+   out_2564586462254658463[25] = 0;
+   out_2564586462254658463[26] = 0;
+   out_2564586462254658463[27] = 0;
+   out_2564586462254658463[28] = 0;
+   out_2564586462254658463[29] = 0;
+   out_2564586462254658463[30] = 0;
+   out_2564586462254658463[31] = 0;
+   out_2564586462254658463[32] = 0;
+   out_2564586462254658463[33] = 1;
+   out_2564586462254658463[34] = 0;
+   out_2564586462254658463[35] = 0;
+   out_2564586462254658463[36] = 0;
+   out_2564586462254658463[37] = 0;
+   out_2564586462254658463[38] = 0;
+   out_2564586462254658463[39] = 0;
+   out_2564586462254658463[40] = 0;
+   out_2564586462254658463[41] = 0;
+   out_2564586462254658463[42] = 0;
+   out_2564586462254658463[43] = 0;
+   out_2564586462254658463[44] = 0;
+   out_2564586462254658463[45] = 0;
+   out_2564586462254658463[46] = 0;
+   out_2564586462254658463[47] = 0;
+   out_2564586462254658463[48] = 0;
+   out_2564586462254658463[49] = 0;
+   out_2564586462254658463[50] = 0;
+   out_2564586462254658463[51] = 0;
+   out_2564586462254658463[52] = 0;
+   out_2564586462254658463[53] = 0;
+   out_2564586462254658463[54] = 0;
+   out_2564586462254658463[55] = 0;
+   out_2564586462254658463[56] = 1;
+   out_2564586462254658463[57] = 0;
+   out_2564586462254658463[58] = 0;
+   out_2564586462254658463[59] = 0;
+   out_2564586462254658463[60] = 0;
+   out_2564586462254658463[61] = 0;
+   out_2564586462254658463[62] = 0;
+   out_2564586462254658463[63] = 0;
+   out_2564586462254658463[64] = 0;
+   out_2564586462254658463[65] = 0;
 }
-void h_33(double *state, double *unused, double *out_57093419937807770) {
-   out_57093419937807770[0] = state[16];
-   out_57093419937807770[1] = state[17];
-   out_57093419937807770[2] = state[18];
+void h_33(double *state, double *unused, double *out_3376577180674409820) {
+   out_3376577180674409820[0] = state[16];
+   out_3376577180674409820[1] = state[17];
+   out_3376577180674409820[2] = state[18];
 }
-void H_33(double *state, double *unused, double *out_5402916346305987633) {
-   out_5402916346305987633[0] = 0;
-   out_5402916346305987633[1] = 0;
-   out_5402916346305987633[2] = 0;
-   out_5402916346305987633[3] = 0;
-   out_5402916346305987633[4] = 0;
-   out_5402916346305987633[5] = 0;
-   out_5402916346305987633[6] = 0;
-   out_5402916346305987633[7] = 0;
-   out_5402916346305987633[8] = 0;
-   out_5402916346305987633[9] = 0;
-   out_5402916346305987633[10] = 0;
-   out_5402916346305987633[11] = 0;
-   out_5402916346305987633[12] = 0;
-   out_5402916346305987633[13] = 0;
-   out_5402916346305987633[14] = 0;
-   out_5402916346305987633[15] = 0;
-   out_5402916346305987633[16] = 1;
-   out_5402916346305987633[17] = 0;
-   out_5402916346305987633[18] = 0;
-   out_5402916346305987633[19] = 0;
-   out_5402916346305987633[20] = 0;
-   out_5402916346305987633[21] = 0;
-   out_5402916346305987633[22] = 0;
-   out_5402916346305987633[23] = 0;
-   out_5402916346305987633[24] = 0;
-   out_5402916346305987633[25] = 0;
-   out_5402916346305987633[26] = 0;
-   out_5402916346305987633[27] = 0;
-   out_5402916346305987633[28] = 0;
-   out_5402916346305987633[29] = 0;
-   out_5402916346305987633[30] = 0;
-   out_5402916346305987633[31] = 0;
-   out_5402916346305987633[32] = 0;
-   out_5402916346305987633[33] = 0;
-   out_5402916346305987633[34] = 0;
-   out_5402916346305987633[35] = 0;
-   out_5402916346305987633[36] = 0;
-   out_5402916346305987633[37] = 0;
-   out_5402916346305987633[38] = 0;
-   out_5402916346305987633[39] = 1;
-   out_5402916346305987633[40] = 0;
-   out_5402916346305987633[41] = 0;
-   out_5402916346305987633[42] = 0;
-   out_5402916346305987633[43] = 0;
-   out_5402916346305987633[44] = 0;
-   out_5402916346305987633[45] = 0;
-   out_5402916346305987633[46] = 0;
-   out_5402916346305987633[47] = 0;
-   out_5402916346305987633[48] = 0;
-   out_5402916346305987633[49] = 0;
-   out_5402916346305987633[50] = 0;
-   out_5402916346305987633[51] = 0;
-   out_5402916346305987633[52] = 0;
-   out_5402916346305987633[53] = 0;
-   out_5402916346305987633[54] = 0;
-   out_5402916346305987633[55] = 0;
-   out_5402916346305987633[56] = 0;
-   out_5402916346305987633[57] = 0;
-   out_5402916346305987633[58] = 0;
-   out_5402916346305987633[59] = 0;
-   out_5402916346305987633[60] = 0;
-   out_5402916346305987633[61] = 0;
-   out_5402916346305987633[62] = 1;
-   out_5402916346305987633[63] = 0;
-   out_5402916346305987633[64] = 0;
-   out_5402916346305987633[65] = 0;
+void H_33(double *state, double *unused, double *out_3711442953127215872) {
+   out_3711442953127215872[0] = 0;
+   out_3711442953127215872[1] = 0;
+   out_3711442953127215872[2] = 0;
+   out_3711442953127215872[3] = 0;
+   out_3711442953127215872[4] = 0;
+   out_3711442953127215872[5] = 0;
+   out_3711442953127215872[6] = 0;
+   out_3711442953127215872[7] = 0;
+   out_3711442953127215872[8] = 0;
+   out_3711442953127215872[9] = 0;
+   out_3711442953127215872[10] = 0;
+   out_3711442953127215872[11] = 0;
+   out_3711442953127215872[12] = 0;
+   out_3711442953127215872[13] = 0;
+   out_3711442953127215872[14] = 0;
+   out_3711442953127215872[15] = 0;
+   out_3711442953127215872[16] = 1;
+   out_3711442953127215872[17] = 0;
+   out_3711442953127215872[18] = 0;
+   out_3711442953127215872[19] = 0;
+   out_3711442953127215872[20] = 0;
+   out_3711442953127215872[21] = 0;
+   out_3711442953127215872[22] = 0;
+   out_3711442953127215872[23] = 0;
+   out_3711442953127215872[24] = 0;
+   out_3711442953127215872[25] = 0;
+   out_3711442953127215872[26] = 0;
+   out_3711442953127215872[27] = 0;
+   out_3711442953127215872[28] = 0;
+   out_3711442953127215872[29] = 0;
+   out_3711442953127215872[30] = 0;
+   out_3711442953127215872[31] = 0;
+   out_3711442953127215872[32] = 0;
+   out_3711442953127215872[33] = 0;
+   out_3711442953127215872[34] = 0;
+   out_3711442953127215872[35] = 0;
+   out_3711442953127215872[36] = 0;
+   out_3711442953127215872[37] = 0;
+   out_3711442953127215872[38] = 0;
+   out_3711442953127215872[39] = 1;
+   out_3711442953127215872[40] = 0;
+   out_3711442953127215872[41] = 0;
+   out_3711442953127215872[42] = 0;
+   out_3711442953127215872[43] = 0;
+   out_3711442953127215872[44] = 0;
+   out_3711442953127215872[45] = 0;
+   out_3711442953127215872[46] = 0;
+   out_3711442953127215872[47] = 0;
+   out_3711442953127215872[48] = 0;
+   out_3711442953127215872[49] = 0;
+   out_3711442953127215872[50] = 0;
+   out_3711442953127215872[51] = 0;
+   out_3711442953127215872[52] = 0;
+   out_3711442953127215872[53] = 0;
+   out_3711442953127215872[54] = 0;
+   out_3711442953127215872[55] = 0;
+   out_3711442953127215872[56] = 0;
+   out_3711442953127215872[57] = 0;
+   out_3711442953127215872[58] = 0;
+   out_3711442953127215872[59] = 0;
+   out_3711442953127215872[60] = 0;
+   out_3711442953127215872[61] = 0;
+   out_3711442953127215872[62] = 1;
+   out_3711442953127215872[63] = 0;
+   out_3711442953127215872[64] = 0;
+   out_3711442953127215872[65] = 0;
 }
 #include <eigen3/Eigen/Dense>
 #include <iostream>
@@ -1855,77 +1855,77 @@ void live_update_14(double *in_x, double *in_P, double *in_z, double *in_R, doub
 void live_update_33(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
   update<3, 3, 0>(in_x, in_P, h_33, H_33, NULL, in_z, in_R, in_ea, MAHA_THRESH_33);
 }
-void live_H(double *in_vec, double *out_8687680363762493539) {
-  H(in_vec, out_8687680363762493539);
+void live_H(double *in_vec, double *out_907497564161450989) {
+  H(in_vec, out_907497564161450989);
 }
-void live_err_fun(double *nom_x, double *delta_x, double *out_8136490426412747881) {
-  err_fun(nom_x, delta_x, out_8136490426412747881);
+void live_err_fun(double *nom_x, double *delta_x, double *out_403017012035771763) {
+  err_fun(nom_x, delta_x, out_403017012035771763);
 }
-void live_inv_err_fun(double *nom_x, double *true_x, double *out_1524655933590429885) {
-  inv_err_fun(nom_x, true_x, out_1524655933590429885);
+void live_inv_err_fun(double *nom_x, double *true_x, double *out_2932713747370631150) {
+  inv_err_fun(nom_x, true_x, out_2932713747370631150);
 }
-void live_H_mod_fun(double *state, double *out_7453067886472422609) {
-  H_mod_fun(state, out_7453067886472422609);
+void live_H_mod_fun(double *state, double *out_6036032580712144890) {
+  H_mod_fun(state, out_6036032580712144890);
 }
-void live_f_fun(double *state, double dt, double *out_3547963935686363060) {
-  f_fun(state,  dt, out_3547963935686363060);
+void live_f_fun(double *state, double dt, double *out_653667760006229873) {
+  f_fun(state,  dt, out_653667760006229873);
 }
-void live_F_fun(double *state, double dt, double *out_3254137990445528781) {
-  F_fun(state,  dt, out_3254137990445528781);
+void live_F_fun(double *state, double dt, double *out_8944506884882647575) {
+  F_fun(state,  dt, out_8944506884882647575);
 }
-void live_h_4(double *state, double *unused, double *out_4410787759120580449) {
-  h_4(state, unused, out_4410787759120580449);
+void live_h_4(double *state, double *unused, double *out_6967130088459663654) {
+  h_4(state, unused, out_6967130088459663654);
 }
-void live_H_4(double *state, double *unused, double *out_5512660098689845475) {
-  H_4(state, unused, out_5512660098689845475);
+void live_H_4(double *state, double *unused, double *out_2805776108884249108) {
+  H_4(state, unused, out_2805776108884249108);
 }
-void live_h_9(double *state, double *unused, double *out_4264504783842197092) {
-  h_9(state, unused, out_4264504783842197092);
+void live_h_9(double *state, double *unused, double *out_5944074116138401933) {
+  h_9(state, unused, out_5944074116138401933);
 }
-void live_H_9(double *state, double *unused, double *out_5271470452060254830) {
-  H_9(state, unused, out_5271470452060254830);
+void live_H_9(double *state, double *unused, double *out_2564586462254658463) {
+  H_9(state, unused, out_2564586462254658463);
 }
-void live_h_10(double *state, double *unused, double *out_1276705416788479855) {
-  h_10(state, unused, out_1276705416788479855);
+void live_h_10(double *state, double *unused, double *out_3024176660760462318) {
+  h_10(state, unused, out_3024176660760462318);
 }
-void live_H_10(double *state, double *unused, double *out_6589591562994162559) {
-  H_10(state, unused, out_6589591562994162559);
+void live_H_10(double *state, double *unused, double *out_3594957374532062379) {
+  H_10(state, unused, out_3594957374532062379);
 }
-void live_h_12(double *state, double *unused, double *out_4489758644873872208) {
-  h_12(state, unused, out_4489758644873872208);
+void live_h_12(double *state, double *unused, double *out_1781567485764299655) {
+  h_12(state, unused, out_1781567485764299655);
 }
-void live_H_12(double *state, double *unused, double *out_493203690657883680) {
-  H_12(state, unused, out_493203690657883680);
+void live_H_12(double *state, double *unused, double *out_2213680299147712687) {
+  H_12(state, unused, out_2213680299147712687);
 }
-void live_h_35(double *state, double *unused, double *out_8696122944070004010) {
-  h_35(state, unused, out_8696122944070004010);
+void live_h_35(double *state, double *unused, double *out_3392135172365373817) {
+  h_35(state, unused, out_3392135172365373817);
 }
-void live_H_35(double *state, double *unused, double *out_2252359341667130029) {
-  H_35(state, unused, out_2252359341667130029);
+void live_H_35(double *state, double *unused, double *out_4959243331472726396) {
+  H_35(state, unused, out_4959243331472726396);
 }
-void live_h_32(double *state, double *unused, double *out_2592146495080937647) {
-  h_32(state, unused, out_2592146495080937647);
+void live_h_32(double *state, double *unused, double *out_5713026107165599538) {
+  h_32(state, unused, out_5713026107165599538);
 }
-void live_H_32(double *state, double *unused, double *out_8522971196901590084) {
-  H_32(state, unused, out_8522971196901590084);
+void live_H_32(double *state, double *unused, double *out_8507087671459885125) {
+  H_32(state, unused, out_8507087671459885125);
 }
-void live_h_13(double *state, double *unused, double *out_5320318911182793356) {
-  h_13(state, unused, out_5320318911182793356);
+void live_h_13(double *state, double *unused, double *out_8591731605088062953) {
+  h_13(state, unused, out_8591731605088062953);
 }
-void live_H_13(double *state, double *unused, double *out_6260235854627074248) {
-  H_13(state, unused, out_6260235854627074248);
+void live_H_13(double *state, double *unused, double *out_1226506526531533769) {
+  H_13(state, unused, out_1226506526531533769);
 }
-void live_h_14(double *state, double *unused, double *out_4264504783842197092) {
-  h_14(state, unused, out_4264504783842197092);
+void live_h_14(double *state, double *unused, double *out_5944074116138401933) {
+  h_14(state, unused, out_5944074116138401933);
 }
-void live_H_14(double *state, double *unused, double *out_5271470452060254830) {
-  H_14(state, unused, out_5271470452060254830);
+void live_H_14(double *state, double *unused, double *out_2564586462254658463) {
+  H_14(state, unused, out_2564586462254658463);
 }
-void live_h_33(double *state, double *unused, double *out_57093419937807770) {
-  h_33(state, unused, out_57093419937807770);
+void live_h_33(double *state, double *unused, double *out_3376577180674409820) {
+  h_33(state, unused, out_3376577180674409820);
 }
-void live_H_33(double *state, double *unused, double *out_5402916346305987633) {
-  H_33(state, unused, out_5402916346305987633);
+void live_H_33(double *state, double *unused, double *out_3711442953127215872) {
+  H_33(state, unused, out_3711442953127215872);
 }
 void live_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/live.h b/selfdrive/locationd/models/generated/live.h
index 505753497..95350f4c6 100644
--- a/selfdrive/locationd/models/generated/live.h
+++ b/selfdrive/locationd/models/generated/live.h
@@ -10,29 +10,29 @@ void live_update_32(double *in_x, double *in_P, double *in_z, double *in_R, doub
 void live_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void live_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void live_update_33(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void live_H(double *in_vec, double *out_8687680363762493539);
-void live_err_fun(double *nom_x, double *delta_x, double *out_8136490426412747881);
-void live_inv_err_fun(double *nom_x, double *true_x, double *out_1524655933590429885);
-void live_H_mod_fun(double *state, double *out_7453067886472422609);
-void live_f_fun(double *state, double dt, double *out_3547963935686363060);
-void live_F_fun(double *state, double dt, double *out_3254137990445528781);
-void live_h_4(double *state, double *unused, double *out_4410787759120580449);
-void live_H_4(double *state, double *unused, double *out_5512660098689845475);
-void live_h_9(double *state, double *unused, double *out_4264504783842197092);
-void live_H_9(double *state, double *unused, double *out_5271470452060254830);
-void live_h_10(double *state, double *unused, double *out_1276705416788479855);
-void live_H_10(double *state, double *unused, double *out_6589591562994162559);
-void live_h_12(double *state, double *unused, double *out_4489758644873872208);
-void live_H_12(double *state, double *unused, double *out_493203690657883680);
-void live_h_35(double *state, double *unused, double *out_8696122944070004010);
-void live_H_35(double *state, double *unused, double *out_2252359341667130029);
-void live_h_32(double *state, double *unused, double *out_2592146495080937647);
-void live_H_32(double *state, double *unused, double *out_8522971196901590084);
-void live_h_13(double *state, double *unused, double *out_5320318911182793356);
-void live_H_13(double *state, double *unused, double *out_6260235854627074248);
-void live_h_14(double *state, double *unused, double *out_4264504783842197092);
-void live_H_14(double *state, double *unused, double *out_5271470452060254830);
-void live_h_33(double *state, double *unused, double *out_57093419937807770);
-void live_H_33(double *state, double *unused, double *out_5402916346305987633);
+void live_H(double *in_vec, double *out_907497564161450989);
+void live_err_fun(double *nom_x, double *delta_x, double *out_403017012035771763);
+void live_inv_err_fun(double *nom_x, double *true_x, double *out_2932713747370631150);
+void live_H_mod_fun(double *state, double *out_6036032580712144890);
+void live_f_fun(double *state, double dt, double *out_653667760006229873);
+void live_F_fun(double *state, double dt, double *out_8944506884882647575);
+void live_h_4(double *state, double *unused, double *out_6967130088459663654);
+void live_H_4(double *state, double *unused, double *out_2805776108884249108);
+void live_h_9(double *state, double *unused, double *out_5944074116138401933);
+void live_H_9(double *state, double *unused, double *out_2564586462254658463);
+void live_h_10(double *state, double *unused, double *out_3024176660760462318);
+void live_H_10(double *state, double *unused, double *out_3594957374532062379);
+void live_h_12(double *state, double *unused, double *out_1781567485764299655);
+void live_H_12(double *state, double *unused, double *out_2213680299147712687);
+void live_h_35(double *state, double *unused, double *out_3392135172365373817);
+void live_H_35(double *state, double *unused, double *out_4959243331472726396);
+void live_h_32(double *state, double *unused, double *out_5713026107165599538);
+void live_H_32(double *state, double *unused, double *out_8507087671459885125);
+void live_h_13(double *state, double *unused, double *out_8591731605088062953);
+void live_H_13(double *state, double *unused, double *out_1226506526531533769);
+void live_h_14(double *state, double *unused, double *out_5944074116138401933);
+void live_H_14(double *state, double *unused, double *out_2564586462254658463);
+void live_h_33(double *state, double *unused, double *out_3376577180674409820);
+void live_H_33(double *state, double *unused, double *out_3711442953127215872);
 void live_predict(double *in_x, double *in_P, double *in_Q, double dt);
 }
\ No newline at end of file
diff --git a/selfdrive/locationd/ubloxd b/selfdrive/locationd/ubloxd
index 0855f7ca6..aa35152d4 100755
Binary files a/selfdrive/locationd/ubloxd and b/selfdrive/locationd/ubloxd differ
diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py
index 36196a49b..a1d53bbf9 100755
--- a/selfdrive/manager/manager.py
+++ b/selfdrive/manager/manager.py
@@ -141,7 +141,7 @@ def manager_thread() -> None:
   dp_mapd = params.get_bool('dp_mapd')
   ignore += ['mapd'] if not dp_mapd else []
   ignore += ['gpxd'] if not dp_otisserv and not dp_mapd and not params.get_bool('dp_gpxd') else []
-  ignore += ['uploader'] if not params.get_bool('dp_api_custom') and (int(params.get('dp_atl', encoding='utf8')) > 0 or params.get_bool('dp_jetson')) else []
+  ignore += ['uploader'] if not params.get_bool('dp_api_custom') and params.get_bool('dp_jetson') else []
 
   if params.get("DongleId", encoding='utf8') in (None, UNREGISTERED_DONGLE_ID):
     ignore += ["manage_athenad", "uploader"]
diff --git a/selfdrive/modeld/_dmonitoringmodeld b/selfdrive/modeld/_dmonitoringmodeld
index 6a3e62098..a247e16ce 100755
Binary files a/selfdrive/modeld/_dmonitoringmodeld and b/selfdrive/modeld/_dmonitoringmodeld differ
diff --git a/selfdrive/modeld/_modeld b/selfdrive/modeld/_modeld
index 7bcbe9c80..f77b30bc4 100755
Binary files a/selfdrive/modeld/_modeld and b/selfdrive/modeld/_modeld differ
diff --git a/selfdrive/modeld/models/supercombo.thneed b/selfdrive/modeld/models/supercombo.thneed
index e5b4db3b4..d142a04f0 100644
Binary files a/selfdrive/modeld/models/supercombo.thneed and b/selfdrive/modeld/models/supercombo.thneed differ
diff --git a/selfdrive/sensord/_sensord b/selfdrive/sensord/_sensord
index 49a259486..242f646df 100755
Binary files a/selfdrive/sensord/_sensord and b/selfdrive/sensord/_sensord differ
diff --git a/selfdrive/ui/_ui b/selfdrive/ui/_ui
index 2adbb432d..9acecf683 100755
Binary files a/selfdrive/ui/_ui and b/selfdrive/ui/_ui differ
diff --git a/selfdrive/ui/_ui_nonav b/selfdrive/ui/_ui_nonav
index 064565c92..64d7291b4 100755
Binary files a/selfdrive/ui/_ui_nonav and b/selfdrive/ui/_ui_nonav differ
diff --git a/selfdrive/ui/qt/spinner b/selfdrive/ui/qt/spinner
index 9ca1f5658..87b4bb7ca 100755
Binary files a/selfdrive/ui/qt/spinner and b/selfdrive/ui/qt/spinner differ
diff --git a/selfdrive/ui/qt/text b/selfdrive/ui/qt/text
index 37c3a5335..ccc58cd0c 100755
Binary files a/selfdrive/ui/qt/text and b/selfdrive/ui/qt/text differ
diff --git a/selfdrive/ui/soundd/_soundd b/selfdrive/ui/soundd/_soundd
index 56af8835f..a49518549 100755
Binary files a/selfdrive/ui/soundd/_soundd and b/selfdrive/ui/soundd/_soundd differ
diff --git a/selfdrive/updated.py b/selfdrive/updated.py
index 239841ee0..1aa8a015d 100755
--- a/selfdrive/updated.py
+++ b/selfdrive/updated.py
@@ -222,7 +222,7 @@ def handle_agnos_update() -> None:
   set_offroad_alert("Offroad_NeosUpdate", False)
 
 
-def handle_neos_update(wait_helper: WaitTimeHelper) -> None:
+def handle_neos_update() -> None:
   from system.hardware.eon.neos import download_neos_update
 
   cur_neos = HARDWARE.get_os_version()
diff --git a/system/camerad/camerad b/system/camerad/camerad
index 6e0251f60..a0397bef9 100755
Binary files a/system/camerad/camerad and b/system/camerad/camerad differ