diff --git a/3rdparty/matplotlibcpp.h b/3rdparty/matplotlibcpp.h
index c974666..05e0098 100644
--- a/3rdparty/matplotlibcpp.h
+++ b/3rdparty/matplotlibcpp.h
@@ -15,6 +15,8 @@
 #include <functional>
 #include <string> // std::stod
 
+#include <Eigen/Core>
+
 #ifndef WITHOUT_NUMPY
 #  define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION
 #  include <numpy/arrayobject.h>
@@ -375,6 +377,16 @@ PyObject* get_array(const std::vector<Numeric>& v)
     return varray;
 }
 
+template<typename Numeric>
+PyObject* get_array(const Numeric& v)
+{
+    npy_intp vsize = v.rows();
+    NPY_TYPES type = NPY_DOUBLE;
+
+	_import_array();
+    PyObject* varray = PyArray_SimpleNewFromData(1, &vsize, type, (void*)(v.data()));
+    return varray;
+}
 
 template<typename Numeric>
 PyObject* get_2darray(const std::vector<::std::vector<Numeric>>& v)
@@ -2868,14 +2880,14 @@ struct plot_impl<std::true_type>
 } // end namespace detail
 
 // recursion stop for the above
-template<typename... Args>
-bool plot() { return true; }
+// template<typename... Args>
+// bool plot() { return true; }
 
-template<typename A, typename B, typename... Args>
-bool plot(const A& a, const B& b, const std::string& format, Args... args)
-{
-    return detail::plot_impl<typename detail::is_callable<B>::type>()(a,b,format) && plot(args...);
-}
+// template<typename A, typename B, typename... Args>
+// bool plot(const A& a, const B& b, const std::string& format, Args... args)
+// {
+//     return detail::plot_impl<typename detail::is_callable<B>::type>()(a,b,format) && plot(args...);
+// }
 
 /*
  * This group of plot() functions is needed to support initializer lists, i.e. calling
@@ -2893,6 +2905,29 @@ inline bool plot(const std::vector<double>& x, const std::vector<double>& y, con
     return plot<double>(x,y,keywords);
 }
 
+inline bool plot(const Eigen::VectorXd& x, const Eigen::VectorXd& y, const std::string& format = "") {
+    assert(x.rows() == y.rows());
+
+    detail::_interpreter::get();
+
+    PyObject* xarray = detail::get_array(x);
+    PyObject* yarray = detail::get_array(y);
+
+    PyObject* pystring = PyString_FromString(format.c_str());
+
+    PyObject* plot_args = PyTuple_New(3);
+    PyTuple_SetItem(plot_args, 0, xarray);
+    PyTuple_SetItem(plot_args, 1, yarray);
+    PyTuple_SetItem(plot_args, 2, pystring);
+
+    PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_plot, plot_args);
+
+    Py_DECREF(plot_args);
+    if(res) Py_DECREF(res);
+
+    return res;
+}
+
 /*
  * This class allows dynamic plots, ie changing the plotted data without clearing and re-plotting
  */
diff --git a/src/cilqr_solver.cpp b/src/cilqr_solver.cpp
index 6cb361d..b89d712 100644
--- a/src/cilqr_solver.cpp
+++ b/src/cilqr_solver.cpp
@@ -39,8 +39,8 @@ CILQRSolver::CILQRSolver(const YAML::Node& cfg) {
     velo_max = ego_veh_params["velo_max"].as<double>();
     velo_min = ego_veh_params["velo_min"].as<double>();
     yaw_lim = ego_veh_params["yaw_lim"].as<double>();
-    acc_max = ego_veh_params["a_max"].as<double>();
-    acc_min = ego_veh_params["a_min"].as<double>();
+    acc_max = ego_veh_params["acc_max"].as<double>();
+    acc_min = ego_veh_params["acc_min"].as<double>();
     stl_lim = ego_veh_params["stl_lim"].as<double>();
     double d_safe = ego_veh_params["d_safe"].as<double>();
 
@@ -102,7 +102,7 @@ Eigen::MatrixX4d CILQRSolver::const_velo_prediction(const Eigen::Vector4d& x0, s
     for (size_t i = 0; i < steps; ++i) {
         Eigen::Vector4d next_x = utils::kinematic_propagate(cur_x, cur_u, dt, wheelbase);
         cur_x = next_x;
-        predicted_states.row(i) = next_x;
+        predicted_states.row(i + 1) = next_x;
     }
 
     return predicted_states;
@@ -169,8 +169,8 @@ Eigen::MatrixX2d CILQRSolver::get_ref_exact_points(const Eigen::MatrixX4d& x,
     Eigen::MatrixX2d ref_exact_points = Eigen::MatrixX2d::Zero(x_shape, 2);
 
     for (uint16_t i = 0; i < x_shape; ++i) {
-        uint16_t min_idx = -1;
-        double min_distance = 0;
+        int32_t min_idx = -1;
+        double min_distance = std::numeric_limits<double>::max();
         for (size_t j = start_index; j < ref_waypoints.size(); ++j) {
             double cur_distance = hypot(x(i, 0) - ref_waypoints.x[j], x(i, 1) - ref_waypoints.y[j]);
             if (min_idx < 0 || cur_distance < min_distance) {
diff --git a/src/utils.cpp b/src/utils.cpp
index 00469ea..652a6f1 100644
--- a/src/utils.cpp
+++ b/src/utils.cpp
@@ -186,14 +186,14 @@ std::tuple<Eigen::MatrixX4d, Eigen::MatrixX2d> get_kinematic_model_derivatives(
     Eigen::MatrixX2d df_du(steps * 4, 2);
 
     for (uint32_t i = 0; i < steps; ++i) {
-        df_dx.block(i * 4, 0, 4, 4).setIdentity();
+        df_dx.block<4, 4>(i * 4, 0).setIdentity();
         df_dx(i * 4, 2) = cos(N_beta[i] + N_yaw[i]) * dt;
         df_dx(i * 4, 3) = N_velo[i] * (-sin(N_beta[i] + N_yaw[i])) * dt;
         df_dx(i * 4 + 1, 2) = sin(N_beta[i] + N_yaw[i]) * dt;
         df_dx(i * 4 + 1, 3) = N_velo[i] * cos(N_beta[i] + N_yaw[i]) * dt;
         df_dx(i * 4 + 3, 2) = 2 * sin(N_beta[i]) * dt / wheelbase;
 
-        df_du.block(i * 4, 0, 2, 2).setZero();
+        df_du.block<4, 2>(i * 4, 0).setZero();
         df_du(i * 4, 1) = N_velo[i] * (-sin(N_beta[i] + N_yaw[i])) * dt * N_beta_over_stl[i];
         df_du(i * 4 + 1, 1) = N_velo[i] * cos(N_beta[i] + N_yaw[i]) * dt * N_beta_over_stl[i];
         df_du(i * 4 + 2, 0) = dt;