longitudinal e2e mpc

5 years ago · 34f2c0da75
parent d02f55ae0b
commit 34f2c0da75
17 changed files with 6469 additions and 0 deletions
--- a/1
+++ b/1
@ -215,6 +215,7 @@ if arch != "Darwin":
 SConscript(['selfdrive/controls/lib/cluster/SConscript'])
 SConscript(['selfdrive/controls/lib/lateral_mpc/SConscript'])
 SConscript(['selfdrive/controls/lib/longitudinal_mpc/SConscript'])
+SConscript(['selfdrive/controls/lib/longitudinal_mpc_model/SConscript'])

 SConscript(['selfdrive/boardd/SConscript'])
 SConscript(['selfdrive/proclogd/SConscript'])
--- a/release/files_common
+++ b/release/files_common
@ -214,6 +214,7 @@ selfdrive/controls/lib/vehicle_model.py
 selfdrive/controls/lib/speed_smoother.py
 selfdrive/controls/lib/fcw.py
 selfdrive/controls/lib/long_mpc.py
+selfdrive/controls/lib/long_mpc_model.py
 selfdrive/controls/lib/gps_helpers.py

 selfdrive/controls/lib/cluster/*
@ -234,6 +235,14 @@ selfdrive/controls/lib/longitudinal_mpc/generator.cpp
 selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py
 selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c

+selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/*
+selfdrive/controls/lib/longitudinal_mpc_model/.gitignore
+selfdrive/controls/lib/longitudinal_mpc_model/SConscript
+selfdrive/controls/lib/longitudinal_mpc_model/__init__.py
+selfdrive/controls/lib/longitudinal_mpc_model/generator.cpp
+selfdrive/controls/lib/longitudinal_mpc_model/libmpc_py.py
+selfdrive/controls/lib/longitudinal_mpc_model/longitudinal_mpc.c
+
 selfdrive/locationd/__init__.py
 selfdrive/locationd/.gitignore
 selfdrive/locationd/SConscript
--- a/selfdrive/controls/lib/long_mpc_model.py
+++ b/selfdrive/controls/lib/long_mpc_model.py
@ -0,0 +1,77 @@
+import numpy as np
+import math
+
+from selfdrive.swaglog import cloudlog
+from common.realtime import sec_since_boot
+from selfdrive.controls.lib.longitudinal_mpc_model import libmpc_py
+
+
+class LongitudinalMpcModel():
+  def __init__(self):
+
+    self.setup_mpc()
+    self.v_mpc = 0.0
+    self.v_mpc_future = 0.0
+    self.a_mpc = 0.0
+    self.last_cloudlog_t = 0.0
+    self.ts = list(range(10))
+
+    self.valid = False
+
+  def setup_mpc(self, v_ego=0.0):
+    self.libmpc = libmpc_py.libmpc
+    self.libmpc.init(1.0, 1.0, 1.0, 1.0, 1.0)
+    self.libmpc.init_with_simulation(v_ego)
+
+    self.mpc_solution = libmpc_py.ffi.new("log_t *")
+    self.cur_state = libmpc_py.ffi.new("state_t *")
+
+    self.cur_state[0].x_ego = 0
+    self.cur_state[0].v_ego = 0
+    self.cur_state[0].a_ego = 0
+
+  def set_cur_state(self, v, a):
+    self.cur_state[0].x_ego = 0.0
+    self.cur_state[0].v_ego = v
+    self.cur_state[0].a_ego = a
+
+  def update(self, CS, model):
+    v_ego = CS.vEgo
+
+    longitudinal = model.longitudinal
+
+    if len(longitudinal.distances) == 0:
+      self.valid = False
+      return
+
+    x_poly = list(map(float, np.polyfit(self.ts, longitudinal.distances, 3)))
+    v_poly = list(map(float, np.polyfit(self.ts, longitudinal.speeds, 3)))
+    a_poly = list(map(float, np.polyfit(self.ts, longitudinal.accelerations, 3)))
+
+    # Calculate mpc
+    self.libmpc.run_mpc(self.cur_state, self.mpc_solution, x_poly, v_poly, a_poly)
+
+    # Get solution. MPC timestep is 0.2 s, so interpolation to 0.05 s is needed
+    self.v_mpc = self.mpc_solution[0].v_ego[1]
+    self.a_mpc = self.mpc_solution[0].a_ego[1]
+    self.v_mpc_future = self.mpc_solution[0].v_ego[10]
+    self.valid = True
+
+    # Reset if NaN or goes through lead car
+    nans = any(math.isnan(x) for x in self.mpc_solution[0].v_ego)
+
+    t = sec_since_boot()
+    if nans:
+      if t > self.last_cloudlog_t + 5.0:
+        self.last_cloudlog_t = t
+        cloudlog.warning("Longitudinal model mpc reset - backwards")
+
+      self.libmpc.init(1.0, 1.0, 1.0, 1.0, 1.0)
+      self.libmpc.init_with_simulation(v_ego)
+
+      self.cur_state[0].v_ego = v_ego
+      self.cur_state[0].a_ego = 0.0
+
+      self.v_mpc = v_ego
+      self.a_mpc = CS.aEgo
+      self.valid = False
--- a/selfdrive/controls/lib/longitudinal_mpc_model/.gitignore
+++ b/selfdrive/controls/lib/longitudinal_mpc_model/.gitignore
@ -0,0 +1,2 @@
+generator
+lib_qp/
--- a/selfdrive/controls/lib/longitudinal_mpc_model/SConscript
+++ b/selfdrive/controls/lib/longitudinal_mpc_model/SConscript
@ -0,0 +1,31 @@
+Import('env', 'arch')
+
+
+cpp_path = [
+    "#phonelibs/acado/include",
+    "#phonelibs/acado/include/acado",
+    "#phonelibs/qpoases/INCLUDE",
+    "#phonelibs/qpoases/INCLUDE/EXTRAS",
+    "#phonelibs/qpoases/SRC/",
+    "#phonelibs/qpoases",
+    "lib_mpc_export"
+
+]
+
+mpc_files = [
+    "longitudinal_mpc.c",
+    Glob("lib_mpc_export/*.c"),
+    Glob("lib_mpc_export/*.cpp"),
+]
+
+interface_dir = Dir('lib_mpc_export')
+
+SConscript(['#phonelibs/qpoases/SConscript'], variant_dir='lib_qp', exports=['interface_dir'])
+
+env.SharedLibrary('mpc', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path)
+
+# if arch != "aarch64":
+#     acado_libs = [File("#phonelibs/acado/x64/lib/libacado_toolkit.a"),
+#                   File("#phonelibs/acado/x64/lib/libacado_casadi.a"),
+#                   File("#phonelibs/acado/x64/lib/libacado_csparse.a")]
+#     env.Program('generator', 'generator.cpp', LIBS=acado_libs, CPPPATH=cpp_path)
--- a/selfdrive/controls/lib/longitudinal_mpc_model/init.py
+++ b/selfdrive/controls/lib/longitudinal_mpc_model/init.py
--- a/selfdrive/controls/lib/longitudinal_mpc_model/generator.cpp
+++ b/selfdrive/controls/lib/longitudinal_mpc_model/generator.cpp
@ -0,0 +1,99 @@
+#include <acado_code_generation.hpp>
+
+const int controlHorizon = 50;
+
+using namespace std;
+
+
+int main( )
+{
+  USING_NAMESPACE_ACADO
+
+
+  DifferentialEquation f;
+
+  DifferentialState x_ego, v_ego, a_ego, t;
+
+  OnlineData x_poly_r0, x_poly_r1, x_poly_r2, x_poly_r3;
+  OnlineData v_poly_r0, v_poly_r1, v_poly_r2, v_poly_r3;
+  OnlineData a_poly_r0, a_poly_r1, a_poly_r2, a_poly_r3;
+
+  Control j_ego;
+
+  // Equations of motion
+  f << dot(x_ego) == v_ego;
+  f << dot(v_ego) == a_ego;
+  f << dot(a_ego) == j_ego;
+  f << dot(t) == 1;
+
+  auto poly_x = x_poly_r0*(t*t*t) + x_poly_r1*(t*t) + x_poly_r2*t + x_poly_r3;
+  auto poly_v = v_poly_r0*(t*t*t) + v_poly_r1*(t*t) + v_poly_r2*t + v_poly_r3;
+  auto poly_a = a_poly_r0*(t*t*t) + a_poly_r1*(t*t) + a_poly_r2*t + a_poly_r3;
+
+  // Running cost
+  Function h;
+  h << x_ego - poly_x;
+  h << v_ego - poly_v;
+  h << a_ego - poly_a;
+  h << a_ego * (0.1 * v_ego + 1.0);
+  h << j_ego * (0.1 * v_ego + 1.0);
+
+  // Weights are defined in mpc.
+  BMatrix Q(5,5); Q.setAll(true);
+
+  // Terminal cost
+  Function hN;
+  hN << x_ego - poly_x;
+  hN << v_ego - poly_v;
+  hN << a_ego - poly_a;
+  hN << a_ego * (0.1 * v_ego + 1.0);
+
+  // Weights are defined in mpc.
+  BMatrix QN(4,4); QN.setAll(true);
+
+  // Non uniform time grid
+  // First 5 timesteps are 0.2, after that it's 0.6
+  DMatrix numSteps(20, 1);
+  for (int i = 0; i < 5; i++){
+    numSteps(i) = 1;
+  }
+  for (int i = 5; i < 20; i++){
+    numSteps(i) = 3;
+  }
+
+  // Setup Optimal Control Problem
+  const double tStart = 0.0;
+  const double tEnd   = 10.0;
+
+  OCP ocp( tStart, tEnd, numSteps);
+  ocp.subjectTo(f);
+
+  ocp.minimizeLSQ(Q, h);
+  ocp.minimizeLSQEndTerm(QN, hN);
+
+  //ocp.subjectTo( 0.0 <= v_ego);
+  ocp.setNOD(12);
+
+  OCPexport mpc(ocp);
+  mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
+  mpc.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING );
+  mpc.set( INTEGRATOR_TYPE, INT_RK4 );
+  mpc.set( NUM_INTEGRATOR_STEPS, controlHorizon);
+  mpc.set( MAX_NUM_QP_ITERATIONS, 500);
+  mpc.set( CG_USE_VARIABLE_WEIGHTING_MATRIX, YES);
+
+  mpc.set( SPARSE_QP_SOLUTION, CONDENSING );
+  mpc.set( QP_SOLVER, QP_QPOASES );
+  mpc.set( HOTSTART_QP, YES );
+  mpc.set( GENERATE_TEST_FILE, NO);
+  mpc.set( GENERATE_MAKE_FILE, NO );
+  mpc.set( GENERATE_MATLAB_INTERFACE, NO );
+  mpc.set( GENERATE_SIMULINK_INTERFACE, NO );
+
+  if (mpc.exportCode( "lib_mpc_export" ) != SUCCESSFUL_RETURN)
+    exit( EXIT_FAILURE );
+
+  mpc.printDimensionsQP( );
+
+  return EXIT_SUCCESS;
+}
--- a/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_auxiliary_functions.c
+++ b/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_auxiliary_functions.c
@ -0,0 +1,212 @@
+/*
+ *    This file was auto-generated using the ACADO Toolkit.
+ *    
+ *    While ACADO Toolkit is free software released under the terms of
+ *    the GNU Lesser General Public License (LGPL), the generated code
+ *    as such remains the property of the user who used ACADO Toolkit
+ *    to generate this code. In particular, user dependent data of the code
+ *    do not inherit the GNU LGPL license. On the other hand, parts of the
+ *    generated code that are a direct copy of source code from the
+ *    ACADO Toolkit or the software tools it is based on, remain, as derived
+ *    work, automatically covered by the LGPL license.
+ *    
+ *    ACADO Toolkit is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *    
+ */
+
+
+#include "acado_auxiliary_functions.h"
+
+#include <stdio.h>
+
+real_t* acado_getVariablesX( )
+{
+	return acadoVariables.x;
+}
+
+real_t* acado_getVariablesU( )
+{
+	return acadoVariables.u;
+}
+
+#if ACADO_NY > 0
+real_t* acado_getVariablesY( )
+{
+	return acadoVariables.y;
+}
+#endif
+
+#if ACADO_NYN > 0
+real_t* acado_getVariablesYN( )
+{
+	return acadoVariables.yN;
+}
+#endif
+
+real_t* acado_getVariablesX0( )
+{
+#if ACADO_INITIAL_VALUE_FIXED
+	return acadoVariables.x0;
+#else
+	return 0;
+#endif
+}
+
+/** Print differential variables. */
+void acado_printDifferentialVariables( )
+{
+	int i, j;
+	printf("\nDifferential variables:\n[\n");
+	for (i = 0; i < ACADO_N + 1; ++i)
+	{
+		for (j = 0; j < ACADO_NX; ++j)
+			printf("\t%e", acadoVariables.x[i * ACADO_NX + j]);
+		printf("\n");
+	}
+	printf("]\n\n");
+}
+
+/** Print control variables. */
+void acado_printControlVariables( )
+{
+	int i, j;
+	printf("\nControl variables:\n[\n");
+	for (i = 0; i < ACADO_N; ++i)
+	{
+		for (j = 0; j < ACADO_NU; ++j)
+			printf("\t%e", acadoVariables.u[i * ACADO_NU + j]);
+		printf("\n");
+	}
+	printf("]\n\n");
+}
+
+/** Print ACADO code generation notice. */
+void acado_printHeader( )
+{
+	printf(
+		"\nACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.\n"
+		"Copyright (C) 2008-2015 by Boris Houska, Hans Joachim Ferreau,\n" 
+		"Milan Vukov and Rien Quirynen, KU Leuven.\n"
+	);
+	
+	printf(
+		"Developed within the Optimization in Engineering Center (OPTEC) under\n"
+		"supervision of Moritz Diehl. All rights reserved.\n\n"
+		"ACADO Toolkit is distributed under the terms of the GNU Lesser\n"
+		"General Public License 3 in the hope that it will be useful,\n"
+		"but WITHOUT ANY WARRANTY; without even the implied warranty of\n"
+		"MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n"
+		"GNU Lesser General Public License for more details.\n\n"
+	);
+}
+
+#if !(defined _DSPACE)
+#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__)
+
+void acado_tic( acado_timer* t )
+{
+	QueryPerformanceFrequency(&t->freq);
+	QueryPerformanceCounter(&t->tic);
+}
+
+real_t acado_toc( acado_timer* t )
+{
+	QueryPerformanceCounter(&t->toc);
+	return ((t->toc.QuadPart - t->tic.QuadPart) / (real_t)t->freq.QuadPart);
+}
+
+
+#elif (defined __APPLE__)
+
+void acado_tic( acado_timer* t )
+{
+    /* read current clock cycles */
+    t->tic = mach_absolute_time();
+}
+
+real_t acado_toc( acado_timer* t )
+{
+
+    uint64_t duration; /* elapsed time in clock cycles*/
+    
+    t->toc = mach_absolute_time();
+    duration = t->toc - t->tic;
+    
+    /*conversion from clock cycles to nanoseconds*/
+    mach_timebase_info(&(t->tinfo));
+    duration *= t->tinfo.numer;
+    duration /= t->tinfo.denom;
+
+    return (real_t)duration / 1e9;
+}
+
+#else
+
+#if __STDC_VERSION__ >= 199901L
+/* C99 mode */
+
+/* read current time */
+void acado_tic( acado_timer* t )
+{
+	gettimeofday(&t->tic, 0);
+}
+
+/* return time passed since last call to tic on this timer */
+real_t acado_toc( acado_timer* t )
+{
+	struct timeval temp;
+	
+	gettimeofday(&t->toc, 0);
+    
+	if ((t->toc.tv_usec - t->tic.tv_usec) < 0)
+	{
+		temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec - 1;
+		temp.tv_usec = 1000000 + t->toc.tv_usec - t->tic.tv_usec;
+	}
+	else
+	{
+		temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec;
+		temp.tv_usec = t->toc.tv_usec - t->tic.tv_usec;
+	}
+	
+	return (real_t)temp.tv_sec + (real_t)temp.tv_usec / 1e6;
+}
+
+#else
+/* ANSI */
+
+/* read current time */
+void acado_tic( acado_timer* t )
+{
+	clock_gettime(CLOCK_MONOTONIC, &t->tic);
+}
+
+
+/* return time passed since last call to tic on this timer */
+real_t acado_toc( acado_timer* t )
+{
+	struct timespec temp;
+    
+	clock_gettime(CLOCK_MONOTONIC, &t->toc);	
+    
+	if ((t->toc.tv_nsec - t->tic.tv_nsec) < 0)
+	{
+		temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec - 1;
+		temp.tv_nsec = 1000000000+t->toc.tv_nsec - t->tic.tv_nsec;
+	}
+	else
+	{
+		temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec;
+		temp.tv_nsec = t->toc.tv_nsec - t->tic.tv_nsec;
+	}
+	
+	return (real_t)temp.tv_sec + (real_t)temp.tv_nsec / 1e9;
+}
+
+#endif /* __STDC_VERSION__ >= 199901L */
+
+#endif /* (defined _WIN32 || _WIN64) */
+
+#endif
--- a/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_auxiliary_functions.h
+++ b/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_auxiliary_functions.h
@ -0,0 +1,138 @@
+/*
+ *    This file was auto-generated using the ACADO Toolkit.
+ *    
+ *    While ACADO Toolkit is free software released under the terms of
+ *    the GNU Lesser General Public License (LGPL), the generated code
+ *    as such remains the property of the user who used ACADO Toolkit
+ *    to generate this code. In particular, user dependent data of the code
+ *    do not inherit the GNU LGPL license. On the other hand, parts of the
+ *    generated code that are a direct copy of source code from the
+ *    ACADO Toolkit or the software tools it is based on, remain, as derived
+ *    work, automatically covered by the LGPL license.
+ *    
+ *    ACADO Toolkit is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *    
+ */
+
+
+#ifndef ACADO_AUXILIARY_FUNCTIONS_H
+#define ACADO_AUXILIARY_FUNCTIONS_H
+
+#include "acado_common.h"
+
+#ifndef __MATLAB__
+#ifdef __cplusplus
+extern "C"
+{
+#endif /* __cplusplus */
+#endif /* __MATLAB__ */
+
+/** Get pointer to the matrix with differential variables. */
+real_t* acado_getVariablesX( );
+
+/** Get pointer to the matrix with control variables. */
+real_t* acado_getVariablesU( );
+
+#if ACADO_NY > 0
+/** Get pointer to the matrix with references/measurements. */
+real_t* acado_getVariablesY( );
+#endif
+
+#if ACADO_NYN > 0
+/** Get pointer to the vector with references/measurement on the last node. */
+real_t* acado_getVariablesYN( );
+#endif
+
+/** Get pointer to the current state feedback vector. Only applicable for NMPC. */
+real_t* acado_getVariablesX0( );
+
+/** Print differential variables. */
+void acado_printDifferentialVariables( );
+
+/** Print control variables. */
+void acado_printControlVariables( );
+
+/** Print ACADO code generation notice. */
+void acado_printHeader( );
+
+/*
+ * A huge thanks goes to Alexander Domahidi from ETHZ, Switzerland, for 
+ * providing us with the following timing routines.
+ */
+
+#if !(defined _DSPACE)
+#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__)
+
+/* Use Windows QueryPerformanceCounter for timing. */
+#include <Windows.h>
+
+/** A structure for keeping internal timer data. */
+typedef struct acado_timer_
+{
+	LARGE_INTEGER tic;
+	LARGE_INTEGER toc;
+	LARGE_INTEGER freq;
+} acado_timer;
+
+
+#elif (defined __APPLE__)
+
+#include "unistd.h"
+#include <mach/mach_time.h>
+
+/** A structure for keeping internal timer data. */
+typedef struct acado_timer_
+{
+	uint64_t tic;
+	uint64_t toc;
+	mach_timebase_info_data_t tinfo;
+} acado_timer;
+
+#else
+
+/* Use POSIX clock_gettime() for timing on non-Windows machines. */
+#include <time.h>
+
+#if __STDC_VERSION__ >= 199901L
+/* C99 mode of operation. */
+
+#include <sys/stat.h>
+#include <sys/time.h>
+
+typedef struct acado_timer_
+{
+	struct timeval tic;
+	struct timeval toc;
+} acado_timer;
+
+#else
+/* ANSI C */
+
+/** A structure for keeping internal timer data. */
+typedef struct acado_timer_
+{
+	struct timespec tic;
+	struct timespec toc;
+} acado_timer;
+
+#endif /* __STDC_VERSION__ >= 199901L */
+
+#endif /* (defined _WIN32 || defined _WIN64) */
+
+/** A function for measurement of the current time. */
+void acado_tic( acado_timer* t );
+
+/** A function which returns the elapsed time. */
+real_t acado_toc( acado_timer* t );
+
+#endif
+
+#ifndef __MATLAB__
+#ifdef __cplusplus
+} /* extern "C" */
+#endif /* __cplusplus */
+#endif /* __MATLAB__ */
+
+#endif /* ACADO_AUXILIARY_FUNCTIONS_H */
--- a/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_common.h
+++ b/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_common.h
@ -0,0 +1,342 @@
+/*
+ *    This file was auto-generated using the ACADO Toolkit.
+ *    
+ *    While ACADO Toolkit is free software released under the terms of
+ *    the GNU Lesser General Public License (LGPL), the generated code
+ *    as such remains the property of the user who used ACADO Toolkit
+ *    to generate this code. In particular, user dependent data of the code
+ *    do not inherit the GNU LGPL license. On the other hand, parts of the
+ *    generated code that are a direct copy of source code from the
+ *    ACADO Toolkit or the software tools it is based on, remain, as derived
+ *    work, automatically covered by the LGPL license.
+ *    
+ *    ACADO Toolkit is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *    
+ */
+
+
+#ifndef ACADO_COMMON_H
+#define ACADO_COMMON_H
+
+#include <math.h>
+#include <string.h>
+
+#ifndef __MATLAB__
+#ifdef __cplusplus
+extern "C"
+{
+#endif /* __cplusplus */
+#endif /* __MATLAB__ */
+
+/** \defgroup ACADO ACADO CGT generated module. */
+/** @{ */
+
+/** qpOASES QP solver indicator. */
+#define ACADO_QPOASES  0
+#define ACADO_QPOASES3 1
+/** FORCES QP solver indicator.*/
+#define ACADO_FORCES   2
+/** qpDUNES QP solver indicator.*/
+#define ACADO_QPDUNES  3
+/** HPMPC QP solver indicator. */
+#define ACADO_HPMPC    4
+#define ACADO_GENERIC    5
+
+/** Indicator for determining the QP solver used by the ACADO solver code. */
+#define ACADO_QP_SOLVER ACADO_QPOASES
+
+#include "acado_qpoases_interface.hpp"
+
+
+/*
+ * Common definitions
+ */
+/** User defined block based condensing. */
+#define ACADO_BLOCK_CONDENSING 0
+/** Compute covariance matrix of the last state estimate. */
+#define ACADO_COMPUTE_COVARIANCE_MATRIX 0
+/** Flag indicating whether constraint values are hard-coded or not. */
+#define ACADO_HARDCODED_CONSTRAINT_VALUES 1
+/** Indicator for fixed initial state. */
+#define ACADO_INITIAL_STATE_FIXED 1
+/** Number of control/estimation intervals. */
+#define ACADO_N 20
+/** Number of online data values. */
+#define ACADO_NOD 12
+/** Number of path constraints. */
+#define ACADO_NPAC 0
+/** Number of control variables. */
+#define ACADO_NU 1
+/** Number of differential variables. */
+#define ACADO_NX 4
+/** Number of algebraic variables. */
+#define ACADO_NXA 0
+/** Number of differential derivative variables. */
+#define ACADO_NXD 0
+/** Number of references/measurements per node on the first N nodes. */
+#define ACADO_NY 5
+/** Number of references/measurements on the last (N + 1)st node. */
+#define ACADO_NYN 4
+/** Total number of QP optimization variables. */
+#define ACADO_QP_NV 24
+/** Number of Runge-Kutta stages per integration step. */
+#define ACADO_RK_NSTAGES 4
+/** Providing interface for arrival cost. */
+#define ACADO_USE_ARRIVAL_COST 0
+/** Indicator for usage of non-hard-coded linear terms in the objective. */
+#define ACADO_USE_LINEAR_TERMS 0
+/** Indicator for type of fixed weighting matrices. */
+#define ACADO_WEIGHTING_MATRICES_TYPE 2
+
+
+/*
+ * Globally used structure definitions
+ */
+
+/** The structure containing the user data.
+ * 
+ *  Via this structure the user "communicates" with the solver code.
+ */
+typedef struct ACADOvariables_
+{
+int dummy;
+/** Matrix of size: 21 x 4 (row major format)
+ * 
+ *  Matrix containing 21 differential variable vectors.
+ */
+real_t x[ 84 ];
+
+/** Column vector of size: 20
+ * 
+ *  Matrix containing 20 control variable vectors.
+ */
+real_t u[ 20 ];
+
+/** Matrix of size: 21 x 12 (row major format)
+ * 
+ *  Matrix containing 21 online data vectors.
+ */
+real_t od[ 252 ];
+
+/** Column vector of size: 100
+ * 
+ *  Matrix containing 20 reference/measurement vectors of size 5 for first 20 nodes.
+ */
+real_t y[ 100 ];
+
+/** Column vector of size: 4
+ * 
+ *  Reference/measurement vector for the 21. node.
+ */
+real_t yN[ 4 ];
+
+/** Matrix of size: 100 x 5 (row major format) */
+real_t W[ 500 ];
+
+/** Matrix of size: 4 x 4 (row major format) */
+real_t WN[ 16 ];
+
+/** Column vector of size: 4
+ * 
+ *  Current state feedback vector.
+ */
+real_t x0[ 4 ];
+
+
+} ACADOvariables;
+
+/** Private workspace used by the auto-generated code.
+ * 
+ *  Data members of this structure are private to the solver.
+ *  In other words, the user code should not modify values of this 
+ *  structure. 
+ */
+typedef struct ACADOworkspace_
+{
+real_t rk_ttt;
+
+/** Row vector of size: 37 */
+real_t rk_xxx[ 37 ];
+
+/** Matrix of size: 4 x 24 (row major format) */
+real_t rk_kkk[ 96 ];
+
+/** Row vector of size: 37 */
+real_t state[ 37 ];
+
+/** Column vector of size: 80 */
+real_t d[ 80 ];
+
+/** Column vector of size: 100 */
+real_t Dy[ 100 ];
+
+/** Column vector of size: 4 */
+real_t DyN[ 4 ];
+
+/** Matrix of size: 80 x 4 (row major format) */
+real_t evGx[ 320 ];
+
+/** Column vector of size: 80 */
+real_t evGu[ 80 ];
+
+/** Row vector of size: 17 */
+real_t objValueIn[ 17 ];
+
+/** Row vector of size: 30 */
+real_t objValueOut[ 30 ];
+
+/** Matrix of size: 80 x 4 (row major format) */
+real_t Q1[ 320 ];
+
+/** Matrix of size: 80 x 5 (row major format) */
+real_t Q2[ 400 ];
+
+/** Column vector of size: 20 */
+real_t R1[ 20 ];
+
+/** Matrix of size: 20 x 5 (row major format) */
+real_t R2[ 100 ];
+
+/** Column vector of size: 80 */
+real_t S1[ 80 ];
+
+/** Matrix of size: 4 x 4 (row major format) */
+real_t QN1[ 16 ];
+
+/** Matrix of size: 4 x 4 (row major format) */
+real_t QN2[ 16 ];
+
+/** Column vector of size: 4 */
+real_t Dx0[ 4 ];
+
+/** Matrix of size: 4 x 4 (row major format) */
+real_t T[ 16 ];
+
+/** Column vector of size: 840 */
+real_t E[ 840 ];
+
+/** Column vector of size: 840 */
+real_t QE[ 840 ];
+
+/** Matrix of size: 80 x 4 (row major format) */
+real_t QGx[ 320 ];
+
+/** Column vector of size: 80 */
+real_t Qd[ 80 ];
+
+/** Column vector of size: 84 */
+real_t QDy[ 84 ];
+
+/** Matrix of size: 20 x 4 (row major format) */
+real_t H10[ 80 ];
+
+/** Matrix of size: 24 x 24 (row major format) */
+real_t H[ 576 ];
+
+/** Column vector of size: 24 */
+real_t g[ 24 ];
+
+/** Column vector of size: 24 */
+real_t lb[ 24 ];
+
+/** Column vector of size: 24 */
+real_t ub[ 24 ];
+
+/** Column vector of size: 24 */
+real_t x[ 24 ];
+
+/** Column vector of size: 24 */
+real_t y[ 24 ];
+
+
+} ACADOworkspace;
+
+/* 
+ * Forward function declarations. 
+ */
+
+
+/** Performs the integration and sensitivity propagation for one shooting interval.
+ *
+ *  \param rk_eta Working array to pass the input values and return the results.
+ *  \param resetIntegrator The internal memory of the integrator can be reset.
+ *  \param rk_index Number of the shooting interval.
+ *
+ *  \return Status code of the integrator.
+ */
+int acado_integrate( real_t* const rk_eta, int resetIntegrator, int rk_index );
+
+/** Export of an ACADO symbolic function.
+ *
+ *  \param in Input to the exported function.
+ *  \param out Output of the exported function.
+ */
+void acado_rhs_forw(const real_t* in, real_t* out);
+
+/** Preparation step of the RTI scheme.
+ *
+ *  \return Status of the integration module. =0: OK, otherwise the error code.
+ */
+int acado_preparationStep(  );
+
+/** Feedback/estimation step of the RTI scheme.
+ *
+ *  \return Status code of the qpOASES QP solver.
+ */
+int acado_feedbackStep(  );
+
+/** Solver initialization. Must be called once before any other function call.
+ *
+ *  \return =0: OK, otherwise an error code of a QP solver.
+ */
+int acado_initializeSolver(  );
+
+/** Initialize shooting nodes by a forward simulation starting from the first node.
+ */
+void acado_initializeNodesByForwardSimulation(  );
+
+/** Shift differential variables vector by one interval.
+ *
+ *  \param strategy Shifting strategy: 1. Initialize node 21 with xEnd. 2. Initialize node 21 by forward simulation.
+ *  \param xEnd Value for the x vector on the last node. If =0 the old value is used.
+ *  \param uEnd Value for the u vector on the second to last node. If =0 the old value is used.
+ */
+void acado_shiftStates( int strategy, real_t* const xEnd, real_t* const uEnd );
+
+/** Shift controls vector by one interval.
+ *
+ *  \param uEnd Value for the u vector on the second to last node. If =0 the old value is used.
+ */
+void acado_shiftControls( real_t* const uEnd );
+
+/** Get the KKT tolerance of the current iterate.
+ *
+ *  \return The KKT tolerance value.
+ */
+real_t acado_getKKT(  );
+
+/** Calculate the objective value.
+ *
+ *  \return Value of the objective function.
+ */
+real_t acado_getObjective(  );
+
+
+/* 
+ * Extern declarations. 
+ */
+
+extern ACADOworkspace acadoWorkspace;
+extern ACADOvariables acadoVariables;
+
+/** @} */
+
+#ifndef __MATLAB__
+#ifdef __cplusplus
+} /* extern "C" */
+#endif /* __cplusplus */
+#endif /* __MATLAB__ */
+
+#endif /* ACADO_COMMON_H */
--- a/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_integrator.c
+++ b/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_integrator.c
@ -0,0 +1,231 @@
+/*
+ *    This file was auto-generated using the ACADO Toolkit.
+ *    
+ *    While ACADO Toolkit is free software released under the terms of
+ *    the GNU Lesser General Public License (LGPL), the generated code
+ *    as such remains the property of the user who used ACADO Toolkit
+ *    to generate this code. In particular, user dependent data of the code
+ *    do not inherit the GNU LGPL license. On the other hand, parts of the
+ *    generated code that are a direct copy of source code from the
+ *    ACADO Toolkit or the software tools it is based on, remain, as derived
+ *    work, automatically covered by the LGPL license.
+ *    
+ *    ACADO Toolkit is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *    
+ */
+
+
+#include "acado_common.h"
+
+
+void acado_rhs_forw(const real_t* in, real_t* out)
+{
+const real_t* xd = in;
+const real_t* u = in + 24;
+
+/* Compute outputs: */
+out[0] = xd[1];
+out[1] = xd[2];
+out[2] = u[0];
+out[3] = (real_t)(1.0000000000000000e+00);
+out[4] = xd[8];
+out[5] = xd[9];
+out[6] = xd[10];
+out[7] = xd[11];
+out[8] = xd[12];
+out[9] = xd[13];
+out[10] = xd[14];
+out[11] = xd[15];
+out[12] = (real_t)(0.0000000000000000e+00);
+out[13] = (real_t)(0.0000000000000000e+00);
+out[14] = (real_t)(0.0000000000000000e+00);
+out[15] = (real_t)(0.0000000000000000e+00);
+out[16] = (real_t)(0.0000000000000000e+00);
+out[17] = (real_t)(0.0000000000000000e+00);
+out[18] = (real_t)(0.0000000000000000e+00);
+out[19] = (real_t)(0.0000000000000000e+00);
+out[20] = xd[21];
+out[21] = xd[22];
+out[22] = (real_t)(1.0000000000000000e+00);
+out[23] = (real_t)(0.0000000000000000e+00);
+}
+
+/* Fixed step size:0.2 */
+int acado_integrate( real_t* const rk_eta, int resetIntegrator, int rk_index )
+{
+int error;
+
+int run1;
+int numSteps[20] = {1, 1, 1, 1, 1, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3};
+int numInts = numSteps[rk_index];
+acadoWorkspace.rk_ttt = 0.0000000000000000e+00;
+rk_eta[4] = 1.0000000000000000e+00;
+rk_eta[5] = 0.0000000000000000e+00;
+rk_eta[6] = 0.0000000000000000e+00;
+rk_eta[7] = 0.0000000000000000e+00;
+rk_eta[8] = 0.0000000000000000e+00;
+rk_eta[9] = 1.0000000000000000e+00;
+rk_eta[10] = 0.0000000000000000e+00;
+rk_eta[11] = 0.0000000000000000e+00;
+rk_eta[12] = 0.0000000000000000e+00;
+rk_eta[13] = 0.0000000000000000e+00;
+rk_eta[14] = 1.0000000000000000e+00;
+rk_eta[15] = 0.0000000000000000e+00;
+rk_eta[16] = 0.0000000000000000e+00;
+rk_eta[17] = 0.0000000000000000e+00;
+rk_eta[18] = 0.0000000000000000e+00;
+rk_eta[19] = 1.0000000000000000e+00;
+rk_eta[20] = 0.0000000000000000e+00;
+rk_eta[21] = 0.0000000000000000e+00;
+rk_eta[22] = 0.0000000000000000e+00;
+rk_eta[23] = 0.0000000000000000e+00;
+acadoWorkspace.rk_xxx[24] = rk_eta[24];
+acadoWorkspace.rk_xxx[25] = rk_eta[25];
+acadoWorkspace.rk_xxx[26] = rk_eta[26];
+acadoWorkspace.rk_xxx[27] = rk_eta[27];
+acadoWorkspace.rk_xxx[28] = rk_eta[28];
+acadoWorkspace.rk_xxx[29] = rk_eta[29];
+acadoWorkspace.rk_xxx[30] = rk_eta[30];
+acadoWorkspace.rk_xxx[31] = rk_eta[31];
+acadoWorkspace.rk_xxx[32] = rk_eta[32];
+acadoWorkspace.rk_xxx[33] = rk_eta[33];
+acadoWorkspace.rk_xxx[34] = rk_eta[34];
+acadoWorkspace.rk_xxx[35] = rk_eta[35];
+acadoWorkspace.rk_xxx[36] = rk_eta[36];
+
+for (run1 = 0; run1 < 1; ++run1)
+{
+for(run1 = 0; run1 < numInts; run1++ ) {
+acadoWorkspace.rk_xxx[0] = + rk_eta[0];
+acadoWorkspace.rk_xxx[1] = + rk_eta[1];
+acadoWorkspace.rk_xxx[2] = + rk_eta[2];
+acadoWorkspace.rk_xxx[3] = + rk_eta[3];
+acadoWorkspace.rk_xxx[4] = + rk_eta[4];
+acadoWorkspace.rk_xxx[5] = + rk_eta[5];
+acadoWorkspace.rk_xxx[6] = + rk_eta[6];
+acadoWorkspace.rk_xxx[7] = + rk_eta[7];
+acadoWorkspace.rk_xxx[8] = + rk_eta[8];
+acadoWorkspace.rk_xxx[9] = + rk_eta[9];
+acadoWorkspace.rk_xxx[10] = + rk_eta[10];
+acadoWorkspace.rk_xxx[11] = + rk_eta[11];
+acadoWorkspace.rk_xxx[12] = + rk_eta[12];
+acadoWorkspace.rk_xxx[13] = + rk_eta[13];
+acadoWorkspace.rk_xxx[14] = + rk_eta[14];
+acadoWorkspace.rk_xxx[15] = + rk_eta[15];
+acadoWorkspace.rk_xxx[16] = + rk_eta[16];
+acadoWorkspace.rk_xxx[17] = + rk_eta[17];
+acadoWorkspace.rk_xxx[18] = + rk_eta[18];
+acadoWorkspace.rk_xxx[19] = + rk_eta[19];
+acadoWorkspace.rk_xxx[20] = + rk_eta[20];
+acadoWorkspace.rk_xxx[21] = + rk_eta[21];
+acadoWorkspace.rk_xxx[22] = + rk_eta[22];
+acadoWorkspace.rk_xxx[23] = + rk_eta[23];
+acado_rhs_forw( acadoWorkspace.rk_xxx, acadoWorkspace.rk_kkk );
+acadoWorkspace.rk_xxx[0] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[0] + rk_eta[0];
+acadoWorkspace.rk_xxx[1] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[1] + rk_eta[1];
+acadoWorkspace.rk_xxx[2] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[2] + rk_eta[2];
+acadoWorkspace.rk_xxx[3] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[3] + rk_eta[3];
+acadoWorkspace.rk_xxx[4] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[4] + rk_eta[4];
+acadoWorkspace.rk_xxx[5] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[5] + rk_eta[5];
+acadoWorkspace.rk_xxx[6] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[6] + rk_eta[6];
+acadoWorkspace.rk_xxx[7] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[7] + rk_eta[7];
+acadoWorkspace.rk_xxx[8] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[8] + rk_eta[8];
+acadoWorkspace.rk_xxx[9] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[9] + rk_eta[9];
+acadoWorkspace.rk_xxx[10] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[10] + rk_eta[10];
+acadoWorkspace.rk_xxx[11] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[11] + rk_eta[11];
+acadoWorkspace.rk_xxx[12] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[12] + rk_eta[12];
+acadoWorkspace.rk_xxx[13] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[13] + rk_eta[13];
+acadoWorkspace.rk_xxx[14] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[14] + rk_eta[14];
+acadoWorkspace.rk_xxx[15] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[15] + rk_eta[15];
+acadoWorkspace.rk_xxx[16] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[16] + rk_eta[16];
+acadoWorkspace.rk_xxx[17] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[17] + rk_eta[17];
+acadoWorkspace.rk_xxx[18] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[18] + rk_eta[18];
+acadoWorkspace.rk_xxx[19] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[19] + rk_eta[19];
+acadoWorkspace.rk_xxx[20] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[20] + rk_eta[20];
+acadoWorkspace.rk_xxx[21] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[21] + rk_eta[21];
+acadoWorkspace.rk_xxx[22] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[22] + rk_eta[22];
+acadoWorkspace.rk_xxx[23] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[23] + rk_eta[23];
+acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 24 ]) );
+acadoWorkspace.rk_xxx[0] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[24] + rk_eta[0];
+acadoWorkspace.rk_xxx[1] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[25] + rk_eta[1];
+acadoWorkspace.rk_xxx[2] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[26] + rk_eta[2];
+acadoWorkspace.rk_xxx[3] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[27] + rk_eta[3];
+acadoWorkspace.rk_xxx[4] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[28] + rk_eta[4];
+acadoWorkspace.rk_xxx[5] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[29] + rk_eta[5];
+acadoWorkspace.rk_xxx[6] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[30] + rk_eta[6];
+acadoWorkspace.rk_xxx[7] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[31] + rk_eta[7];
+acadoWorkspace.rk_xxx[8] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[32] + rk_eta[8];
+acadoWorkspace.rk_xxx[9] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[33] + rk_eta[9];
+acadoWorkspace.rk_xxx[10] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[34] + rk_eta[10];
+acadoWorkspace.rk_xxx[11] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[35] + rk_eta[11];
+acadoWorkspace.rk_xxx[12] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[36] + rk_eta[12];
+acadoWorkspace.rk_xxx[13] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[37] + rk_eta[13];
+acadoWorkspace.rk_xxx[14] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[38] + rk_eta[14];
+acadoWorkspace.rk_xxx[15] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[39] + rk_eta[15];
+acadoWorkspace.rk_xxx[16] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[40] + rk_eta[16];
+acadoWorkspace.rk_xxx[17] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[41] + rk_eta[17];
+acadoWorkspace.rk_xxx[18] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[42] + rk_eta[18];
+acadoWorkspace.rk_xxx[19] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[43] + rk_eta[19];
+acadoWorkspace.rk_xxx[20] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[44] + rk_eta[20];
+acadoWorkspace.rk_xxx[21] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[45] + rk_eta[21];
+acadoWorkspace.rk_xxx[22] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[46] + rk_eta[22];
+acadoWorkspace.rk_xxx[23] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[47] + rk_eta[23];
+acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 48 ]) );
+acadoWorkspace.rk_xxx[0] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[48] + rk_eta[0];
+acadoWorkspace.rk_xxx[1] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[49] + rk_eta[1];
+acadoWorkspace.rk_xxx[2] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[50] + rk_eta[2];
+acadoWorkspace.rk_xxx[3] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[51] + rk_eta[3];
+acadoWorkspace.rk_xxx[4] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[52] + rk_eta[4];
+acadoWorkspace.rk_xxx[5] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[53] + rk_eta[5];
+acadoWorkspace.rk_xxx[6] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[54] + rk_eta[6];
+acadoWorkspace.rk_xxx[7] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[55] + rk_eta[7];
+acadoWorkspace.rk_xxx[8] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[56] + rk_eta[8];
+acadoWorkspace.rk_xxx[9] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[57] + rk_eta[9];
+acadoWorkspace.rk_xxx[10] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[58] + rk_eta[10];
+acadoWorkspace.rk_xxx[11] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[59] + rk_eta[11];
+acadoWorkspace.rk_xxx[12] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[60] + rk_eta[12];
+acadoWorkspace.rk_xxx[13] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[61] + rk_eta[13];
+acadoWorkspace.rk_xxx[14] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[62] + rk_eta[14];
+acadoWorkspace.rk_xxx[15] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[63] + rk_eta[15];
+acadoWorkspace.rk_xxx[16] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[64] + rk_eta[16];
+acadoWorkspace.rk_xxx[17] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[65] + rk_eta[17];
+acadoWorkspace.rk_xxx[18] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[66] + rk_eta[18];
+acadoWorkspace.rk_xxx[19] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[67] + rk_eta[19];
+acadoWorkspace.rk_xxx[20] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[68] + rk_eta[20];
+acadoWorkspace.rk_xxx[21] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[69] + rk_eta[21];
+acadoWorkspace.rk_xxx[22] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[70] + rk_eta[22];
+acadoWorkspace.rk_xxx[23] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[71] + rk_eta[23];
+acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 72 ]) );
+rk_eta[0] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[0] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[24] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[48] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[72];
+rk_eta[1] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[1] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[25] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[49] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[73];
+rk_eta[2] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[2] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[26] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[50] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[74];
+rk_eta[3] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[3] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[27] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[51] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[75];
+rk_eta[4] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[4] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[28] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[52] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[76];
+rk_eta[5] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[5] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[29] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[53] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[77];
+rk_eta[6] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[6] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[30] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[54] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[78];
+rk_eta[7] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[7] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[31] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[55] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[79];
+rk_eta[8] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[8] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[32] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[56] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[80];
+rk_eta[9] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[9] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[33] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[57] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[81];
+rk_eta[10] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[10] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[34] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[58] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[82];
+rk_eta[11] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[11] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[35] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[59] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[83];
+rk_eta[12] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[12] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[36] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[60] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[84];
+rk_eta[13] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[13] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[37] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[61] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[85];
+rk_eta[14] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[14] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[38] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[62] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[86];
+rk_eta[15] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[15] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[39] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[63] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[87];
+rk_eta[16] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[16] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[40] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[64] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[88];
+rk_eta[17] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[17] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[41] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[65] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[89];
+rk_eta[18] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[18] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[42] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[66] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[90];
+rk_eta[19] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[19] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[43] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[67] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[91];
+rk_eta[20] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[20] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[44] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[68] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[92];
+rk_eta[21] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[21] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[45] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[69] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[93];
+rk_eta[22] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[22] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[46] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[70] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[94];
+rk_eta[23] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[23] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[47] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[71] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[95];
+acadoWorkspace.rk_ttt += 1.0000000000000000e+00;
+}
+}
+error = 0;
+return error;
+}
+
--- a/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_qpoases_interface.cpp
+++ b/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_qpoases_interface.cpp
@ -0,0 +1,70 @@
+/*
+ *    This file was auto-generated using the ACADO Toolkit.
+ *    
+ *    While ACADO Toolkit is free software released under the terms of
+ *    the GNU Lesser General Public License (LGPL), the generated code
+ *    as such remains the property of the user who used ACADO Toolkit
+ *    to generate this code. In particular, user dependent data of the code
+ *    do not inherit the GNU LGPL license. On the other hand, parts of the
+ *    generated code that are a direct copy of source code from the
+ *    ACADO Toolkit or the software tools it is based on, remain, as derived
+ *    work, automatically covered by the LGPL license.
+ *    
+ *    ACADO Toolkit is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *    
+ */
+
+
+extern "C"
+{
+#include "acado_common.h"
+}
+
+#include "INCLUDE/QProblemB.hpp"
+
+#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
+#include "INCLUDE/EXTRAS/SolutionAnalysis.hpp"
+#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */
+
+static int acado_nWSR;
+
+
+
+#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
+static SolutionAnalysis acado_sa;
+#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */
+
+int acado_solve( void )
+{
+	acado_nWSR = QPOASES_NWSRMAX;
+
+	QProblemB qp( 24 );
+	
+	returnValue retVal = qp.init(acadoWorkspace.H, acadoWorkspace.g, acadoWorkspace.lb, acadoWorkspace.ub, acado_nWSR, acadoWorkspace.y);
+
+    qp.getPrimalSolution( acadoWorkspace.x );
+    qp.getDualSolution( acadoWorkspace.y );
+	
+#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
+
+	if (retVal != SUCCESSFUL_RETURN)
+		return (int)retVal;
+		
+	retVal = acado_sa.getHessianInverse( &qp,var );
+
+#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */
+
+	return (int)retVal;
+}
+
+int acado_getNWSR( void )
+{
+	return acado_nWSR;
+}
+
+const char* acado_getErrorString( int error )
+{
+	return MessageHandling::getErrorString( error );
+}
--- a/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_qpoases_interface.hpp
+++ b/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_qpoases_interface.hpp
@ -0,0 +1,65 @@
+/*
+ *    This file was auto-generated using the ACADO Toolkit.
+ *    
+ *    While ACADO Toolkit is free software released under the terms of
+ *    the GNU Lesser General Public License (LGPL), the generated code
+ *    as such remains the property of the user who used ACADO Toolkit
+ *    to generate this code. In particular, user dependent data of the code
+ *    do not inherit the GNU LGPL license. On the other hand, parts of the
+ *    generated code that are a direct copy of source code from the
+ *    ACADO Toolkit or the software tools it is based on, remain, as derived
+ *    work, automatically covered by the LGPL license.
+ *    
+ *    ACADO Toolkit is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *    
+ */
+
+
+#ifndef QPOASES_HEADER
+#define QPOASES_HEADER
+
+#ifdef PC_DEBUG
+#include <stdio.h>
+#endif /* PC_DEBUG */
+
+#include <math.h>
+
+#ifdef __cplusplus
+#define EXTERNC extern "C"
+#else
+#define EXTERNC
+#endif
+
+/*
+ * A set of options for qpOASES
+ */
+
+/** Maximum number of optimization variables. */
+#define QPOASES_NVMAX      24
+/** Maximum number of constraints. */
+#define QPOASES_NCMAX      0
+/** Maximum number of working set recalculations. */
+#define QPOASES_NWSRMAX    500
+/** Print level for qpOASES. */
+#define QPOASES_PRINTLEVEL PL_NONE
+/** The value of EPS */
+#define QPOASES_EPS        2.221e-16
+/** Internally used floating point type */
+typedef double real_t;
+
+/*
+ * Forward function declarations
+ */
+
+/** A function that calls the QP solver */
+EXTERNC int acado_solve( void );
+
+/** Get the number of active set changes */
+EXTERNC int acado_getNWSR( void );
+
+/** Get the error string. */
+const char* acado_getErrorString( int error );
+
+#endif /* QPOASES_HEADER */
--- a/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_solver.c
+++ b/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_solver.c
--- a/selfdrive/controls/lib/longitudinal_mpc_model/libmpc_py.py
+++ b/selfdrive/controls/lib/longitudinal_mpc_model/libmpc_py.py
@ -0,0 +1,32 @@
+import os
+
+from cffi import FFI
+from common.ffi_wrapper import suffix
+
+mpc_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)))
+libmpc_fn = os.path.join(mpc_dir, "libmpc"+suffix())
+
+ffi = FFI()
+ffi.cdef("""
+
+typedef struct {
+double x_ego, v_ego, a_ego;
+} state_t;
+
+
+typedef struct {
+double x_ego[21];
+double v_ego[21];
+double a_ego[21];
+double t[21];
+double j_ego[20];
+double cost;
+} log_t;
+
+
+void init(double xCost, double vCost, double aCost, double accelCost, double jerkCost);
+void init_with_simulation(double v_ego);
+int run_mpc(state_t * x0, log_t * solution, double x_poly[4], double v_poly[4], double a_poly[4]);
+""")
+
+libmpc = ffi.dlopen(libmpc_fn)
--- a/selfdrive/controls/lib/longitudinal_mpc_model/longitudinal_mpc.c
+++ b/selfdrive/controls/lib/longitudinal_mpc_model/longitudinal_mpc.c
@ -0,0 +1,141 @@
+#include "acado_common.h"
+#include "acado_auxiliary_functions.h"
+
+#include <stdio.h>
+#include <math.h>
+
+#define NX          ACADO_NX  /* Number of differential state variables.  */
+#define NXA         ACADO_NXA /* Number of algebraic variables. */
+#define NU          ACADO_NU  /* Number of control inputs. */
+#define NOD         ACADO_NOD  /* Number of online data values. */
+
+#define NY          ACADO_NY  /* Number of measurements/references on nodes 0..N - 1. */
+#define NYN         ACADO_NYN /* Number of measurements/references on node N. */
+
+#define N           ACADO_N   /* Number of intervals in the horizon. */
+
+ACADOvariables acadoVariables;
+ACADOworkspace acadoWorkspace;
+
+typedef struct {
+  double x_ego, v_ego, a_ego;
+} state_t;
+
+
+typedef struct {
+  double x_ego[N+1];
+  double v_ego[N+1];
+  double a_ego[N+1];
+  double t[N+1];
+  double j_ego[N];
+  double cost;
+} log_t;
+
+void init(double xCost, double vCost, double aCost, double accelCost, double jerkCost){
+  acado_initializeSolver();
+  int    i;
+  const int STEP_MULTIPLIER = 3;
+
+  /* Initialize the states and controls. */
+  for (i = 0; i < NX * (N + 1); ++i)  acadoVariables.x[ i ] = 0.0;
+  for (i = 0; i < NU * N; ++i)  acadoVariables.u[ i ] = 0.0;
+
+  /* Initialize the measurements/reference. */
+  for (i = 0; i < NY * N; ++i)  acadoVariables.y[ i ] = 0.0;
+  for (i = 0; i < NYN; ++i)  acadoVariables.yN[ i ] = 0.0;
+
+  /* MPC: initialize the current state feedback. */
+  for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0;
+  // Set weights
+
+  for (i = 0; i < N; i++) {
+    int f = 1;
+    if (i > 4){
+      f = STEP_MULTIPLIER;
+    }
+    // Setup diagonal entries
+    acadoVariables.W[NY*NY*i + (NY+1)*0] = xCost * f;
+    acadoVariables.W[NY*NY*i + (NY+1)*1] = vCost * f;
+    acadoVariables.W[NY*NY*i + (NY+1)*2] = aCost * f;
+    acadoVariables.W[NY*NY*i + (NY+1)*3] = accelCost * f;
+    acadoVariables.W[NY*NY*i + (NY+1)*4] = jerkCost * f;
+  }
+  acadoVariables.WN[(NYN+1)*0] = xCost * STEP_MULTIPLIER;
+  acadoVariables.WN[(NYN+1)*1] = vCost * STEP_MULTIPLIER;
+  acadoVariables.WN[(NYN+1)*2] = aCost * STEP_MULTIPLIER;
+  acadoVariables.WN[(NYN+1)*3] = accelCost * STEP_MULTIPLIER;
+
+}
+
+void init_with_simulation(double v_ego){
+  int i;
+
+  double x_ego = 0.0;
+
+  double dt = 0.2;
+  double t = 0.0;
+
+  for (i = 0; i < N + 1; ++i){
+    if (i > 4){
+      dt = 0.6;
+    }
+
+    acadoVariables.x[i*NX] = x_ego;
+    acadoVariables.x[i*NX+1] = v_ego;
+    acadoVariables.x[i*NX+2] = 0;
+    acadoVariables.x[i*NX+3] = t;
+
+    x_ego += v_ego * dt;
+    t += dt;
+  }
+
+  for (i = 0; i < NU * N; ++i)  acadoVariables.u[ i ] = 0.0;
+  for (i = 0; i < NY * N; ++i)  acadoVariables.y[ i ] = 0.0;
+  for (i = 0; i < NYN; ++i)  acadoVariables.yN[ i ] = 0.0;
+}
+
+int run_mpc(state_t * x0, log_t * solution,
+            double x_poly[4], double v_poly[4], double a_poly[4]){
+  int i;
+
+  for (i = 0; i < N + 1; ++i){
+    acadoVariables.od[i*NOD+0] = x_poly[0];
+    acadoVariables.od[i*NOD+1] = x_poly[1];
+    acadoVariables.od[i*NOD+2] = x_poly[2];
+    acadoVariables.od[i*NOD+3] = x_poly[3];
+
+    acadoVariables.od[i*NOD+4] = v_poly[0];
+    acadoVariables.od[i*NOD+5] = v_poly[1];
+    acadoVariables.od[i*NOD+6] = v_poly[2];
+    acadoVariables.od[i*NOD+7] = v_poly[3];
+
+    acadoVariables.od[i*NOD+8] = a_poly[0];
+    acadoVariables.od[i*NOD+9] = a_poly[1];
+    acadoVariables.od[i*NOD+10] = a_poly[2];
+    acadoVariables.od[i*NOD+11] = a_poly[3];
+  }
+
+  acadoVariables.x[0] = acadoVariables.x0[0] = x0->x_ego;
+  acadoVariables.x[1] = acadoVariables.x0[1] = x0->v_ego;
+  acadoVariables.x[2] = acadoVariables.x0[2] = x0->a_ego;
+  acadoVariables.x[3] = acadoVariables.x0[3] = 0;
+
+  acado_preparationStep();
+  acado_feedbackStep();
+
+  for (i = 0; i <= N; i++){
+    solution->x_ego[i] = acadoVariables.x[i*NX];
+    solution->v_ego[i] = acadoVariables.x[i*NX+1];
+    solution->a_ego[i] = acadoVariables.x[i*NX+2];
+    solution->t[i] = acadoVariables.x[i*NX+3];
+
+    if (i < N){
+      solution->j_ego[i] = acadoVariables.u[i];
+    }
+  }
+  solution->cost = acado_getObjective();
+
+  // Dont shift states here. Current solution is closer to next timestep than if
+  // we shift by 0.1 seconds.
+  return acado_getNWSR();
+}
--- a/selfdrive/debug/mpc/longitudinal_mpc_model.py
+++ b/selfdrive/debug/mpc/longitudinal_mpc_model.py
@ -0,0 +1,75 @@
+#!/usr/bin/env python3
+import numpy as np
+
+import matplotlib.pyplot as plt
+
+from selfdrive.controls.lib.longitudinal_mpc_model import libmpc_py
+
+libmpc = libmpc_py.libmpc
+
+dt = 1
+speeds = [6.109375, 5.9765625, 6.6367188, 7.6875, 8.7578125, 9.4375, 10.21875, 11.070312, 11.679688, 12.21875]
+accelerations = [0.15405273, 0.39575195, 0.36669922, 0.29248047, 0.27856445, 0.27832031, 0.29736328, 0.22705078, 0.16003418, 0.15185547]
+ts = [t * dt for t in range(len(speeds))]
+
+# TODO: Get from actual model packet
+x = 0.0
+positions = []
+for v in speeds:
+  positions.append(x)
+  x += v * dt
+
+
+# Polyfit trajectories
+x_poly = list(map(float, np.polyfit(ts, positions, 3)))
+v_poly = list(map(float, np.polyfit(ts, speeds, 3)))
+a_poly = list(map(float, np.polyfit(ts, accelerations, 3)))
+
+x_poly = libmpc_py.ffi.new("double[4]", x_poly)
+v_poly = libmpc_py.ffi.new("double[4]", v_poly)
+a_poly = libmpc_py.ffi.new("double[4]", a_poly)
+
+cur_state = libmpc_py.ffi.new("state_t *")
+cur_state[0].x_ego = 0
+cur_state[0].v_ego = 10
+cur_state[0].a_ego = 0
+
+libmpc.init(1.0, 1.0, 1.0, 1.0, 1.0)
+
+mpc_solution = libmpc_py.ffi.new("log_t *")
+libmpc.init_with_simulation(cur_state[0].v_ego)
+
+libmpc.run_mpc(cur_state, mpc_solution, x_poly, v_poly, a_poly)
+
+# Converge to solution
+for _ in range(10):
+  libmpc.run_mpc(cur_state, mpc_solution, x_poly, v_poly, a_poly)
+
+
+ts_sol = list(mpc_solution[0].t)
+x_sol = list(mpc_solution[0].x_ego)
+v_sol = list(mpc_solution[0].v_ego)
+a_sol = list(mpc_solution[0].a_ego)
+
+
+plt.figure()
+plt.subplot(3, 1, 1)
+plt.plot(ts, positions, 'k--')
+plt.plot(ts_sol, x_sol)
+plt.ylabel('Position [m]')
+plt.xlabel('Time [s]')
+
+plt.subplot(3, 1, 2)
+plt.plot(ts, speeds, 'k--')
+plt.plot(ts_sol, v_sol)
+plt.xlabel('Time [s]')
+plt.ylabel('Speed [m/s]')
+
+plt.subplot(3, 1, 3)
+plt.plot(ts, accelerations, 'k--')
+plt.plot(ts_sol, a_sol)
+
+plt.xlabel('Time [s]')
+plt.ylabel('Acceleration [m/s^2]')
+
+plt.show()