From d8d596bb14778746c1a36d11479c09694815ce00 Mon Sep 17 00:00:00 2001
From: matssteinweg <mats.steinweg@me.com>
Date: Fri, 6 Dec 2019 10:45:20 +0100
Subject: [PATCH] Accumulate time in prediction horizon

---
 MPC.py                    | 6 +++---
 simulation.py             | 2 +-
 spatial_bicycle_models.py | 6 ++++--
 3 files changed, 8 insertions(+), 6 deletions(-)

diff --git a/MPC.py b/MPC.py
index ac12f6c..0a54163 100644
--- a/MPC.py
+++ b/MPC.py
@@ -90,14 +90,14 @@ class MPC:
             kappa_r = current_waypoint.kappa
 
             # Compute LTV matrices
-            A_lin, B_lin = self.model.linearize(v, kappa_r, delta_s)
+            f, A_lin, B_lin = self.model.linearize(v, kappa_r, delta_s)
             A[nx + n * nx:n * nx + 2 * nx, n * nx:n * nx + nx] = A_lin
             B[nx + n * nx:n * nx + 2 * nx, n * nu:n * nu + nu] = B_lin
 
             # Set kappa_r to reference for input signal
             ur[n] = kappa_r
             # Compute equality constraint offset (B*ur)
-            uq[n * nx:n * nx + nx] = B_lin[:, 0] * kappa_r
+            uq[n * nx:n * nx + nx] = B_lin[:, 0] * kappa_r - f
             lb, ub = self.model.reference_path.update_bounds(
                 self.model.wp_id + n, self.model.safety_margin[1])
             xmin_dyn[nx * n] = lb
@@ -211,7 +211,7 @@ class MPC:
             associated_waypoint = self.model.reference_path.waypoints[self.model.wp_id+n]
             predicted_temporal_state = self.model.s2t(associated_waypoint,
                                             spatial_state_prediction[n, :])
-
+            print(spatial_state_prediction[n, 2])
             #print('delta: ', u)
             #print('e_y: ', spatial_state_prediction[n, 0])
             #print('e_psi: ', spatial_state_prediction[n, 1])
diff --git a/simulation.py b/simulation.py
index cdd8422..510b318 100644
--- a/simulation.py
+++ b/simulation.py
@@ -77,7 +77,7 @@ if __name__ == '__main__':
     N = 30
     Q = sparse.diags([1.0, 0.0, 0.0])
     R = sparse.diags([0.01])
-    QN = Q
+    QN = sparse.diags([0.0, 0.0, 1.0])
     InputConstraints = {'umin': np.array([-np.tan(0.66)/car.l]),
                         'umax': np.array([np.tan(0.66)/car.l])}
     StateConstraints = {'xmin': np.array([-np.inf, -np.inf, -np.inf]),
diff --git a/spatial_bicycle_models.py b/spatial_bicycle_models.py
index 48eb160..f133716 100644
--- a/spatial_bicycle_models.py
+++ b/spatial_bicycle_models.py
@@ -434,13 +434,15 @@ class BicycleModel(SpatialBicycleModel):
         # Construct Jacobian Matrix
         a_1 = np.array([1,                    delta_s,      0])
         a_2 = np.array([-kappa_r**2*delta_s,  1,            0])
-        a_3 = np.array([-kappa_r/v*delta_s,   0,            0])
+        a_3 = np.array([-kappa_r/v*delta_s,   0,            1])
 
         b_1 = np.array([0, ])
         b_2 = np.array([delta_s, ])
         b_3 = np.array([0, ])
 
+        f = np.array([0.0, 0.0, 1/v*delta_s])
+
         A = np.stack((a_1, a_2, a_3), axis=0)
         B = np.stack((b_1, b_2, b_3), axis=0)
 
-        return A, B
\ No newline at end of file
+        return f, A, B
\ No newline at end of file