Robust lmpc

Matlab/Lower_d.csv

@@ -0,0 +1,6 @@
+-0.07927
+-0.023019
+-0.22604
+-0.030566
+0
+0

Matlab/Upper_d.csv

@@ -0,0 +1,6 @@
+0.11777
+0.014249
+0.17482
+0.038472
+0.24926
+0.041378

Matlab/main.m

@@ -28,7 +28,7 @@
 BasisVector = W.V'
 NumBasic = size(BasisVector,2)
 %%
-SampledPoint = []
+SampledPoint = [zeros(6,1)]
 
 for i = 1:5000
     Point = zeros(6, 1);
@@ -112,4 +112,7 @@
 
 %%
 save('Workspace')
-[(sum(Checking))/size(Checking,1) (sum(Checking1))/size(Checking1,1)]
+[(sum(Checking))/size(Checking,1) (sum(Checking1))/size(Checking1,1)]
+
+csvwrite('Lower_d.csv',Lower_d)
+csvwrite('Upper_d.csv',Upper_d)

src/A.csv

@@ -1,6 +1,6 @@
-0.968607,-0.129319,0.006073,-0.001993,-0.000103,-0.021791
--0.001194,0.051213,-0.000968,0.000619,0.000002,-0.000945
--0.034577,-0.023278,0.051117,0.024999,0.000252,0.075960
-0.019087,0.042285,0.018656,1.099578,0.000004,0.036122
-0.097217,-0.012832,0.001035,-0.010332,0.999988,-0.023891
-0.000600,0.032069,0.001013,0.066790,0.000000,1.001646
+0.969250,0.234262,-0.034163,0.001648,0.000030,-0.020499
+-0.002233,-0.173845,0.023431,-0.000976,0.000018,-0.000105
+-0.031850,-3.696466,0.437056,-0.013911,0.000277,-0.002173
+0.021240,-0.424709,0.067634,1.084435,0.000014,0.015983
+0.096828,0.055084,-0.006181,-0.008147,1.000002,-0.022821
+0.000835,-0.021105,0.006522,0.068194,0.000001,1.000161

src/B.csv

@@ -1,6 +1,6 @@
-0.030004,0.118322
-0.255475,0.005134
-2.461203,0.036751
-0.181724,0.001875
-0.000450,0.007761
-0.022923,0.000037
+0.022793,0.094321
+0.256710,0.004063
+2.466727,0.056683
+0.180044,0.004275
+0.000432,0.005775
+0.023212,0.000149

src/Error.csv

@@ -1,2 +1,2 @@
-0.268450,0.048219,0.531368,0.061210,0.016106,0.003147
--0.065760,-0.052656,-0.604120,-0.059533,-0.014188,-0.005731
+0.207061,0.024163,0.317387,0.054273,0.014816,0.005224
+-0.060978,-0.042772,-0.568602,-0.063722,-0.008494,-0.006069

src/Sel_xQfun.csv

@@ -0,0 +1,40 @@
+-12.000000
+-13.000000
+-14.000000
+-15.000000
+-16.000000
+-17.000000
+-18.000000
+-19.000000
+-20.000000
+-21.000000
+-22.000000
+-23.000000
+-24.000000
+-25.000000
+-26.000000
+-27.000000
+-28.000000
+-29.000000
+-30.000000
+-31.000000
+-9.000000
+-10.000000
+-11.000000
+-12.000000
+-13.000000
+-14.000000
+-15.000000
+-16.000000
+-17.000000
+-18.000000
+-19.000000
+-20.000000
+-21.000000
+-22.000000
+-23.000000
+-24.000000
+-25.000000
+-26.000000
+-27.000000
+-28.000000

src/fnc/FTOCP.py

@@ -1,5 +1,5 @@
 def FTOCP(M, q, G, L, E, F, b, x0, np, qp, matrix):
-    res_cons = qp(M, matrix(q), F, matrix(b), G, E * matrix(x0) + L)
+    res_cons = qp(M, matrix(q), F, matrix(b+np.dot(E, x0)), G,  matrix(L))
     if res_cons['status'] == 'optimal':
         feasible = 1
     else:
@@ -18,40 +18,43 @@ def BuildMatEqConst(A, B, C, N, n, d, np, spmatrix, TimeVarying):
     # We are going to build our optimization vector z \in \mathbb{R}^((N+1) \dot n \dot N \dot d), note that this vector
     # stucks the predicted trajectory x_{k|t} \forall k = t, \ldots, t+N+1 over the horizon and
     # the predicte input u_{k|t} \forall k = t, \ldots, t+N over the horizon
-    Gx = np.eye(n * (N + 1))
-    Gu = np.zeros((n * (N + 1), d * (N)))
+    Gx = np.eye(n * (N+1))
+    Gu = np.zeros((n * (N+1), d * (N)))
 
-    E = np.zeros((n * (N + 1), n))
-    E[np.arange(n)] = np.eye(n)
+    Ltot = np.zeros((n * (N+1), 1))
 
-    L = np.zeros((n * (N + 1), 1))
+    Etot = np.zeros((n * (N + 1), n))
+    Etot[np.arange(n)] = np.eye(n)
 
     for i in range(0, N):
-        ind1 = n + i * n + np.arange(n)
+        ind1 =  n + i * n + np.arange(n)
         ind2x = i * n + np.arange(n)
         ind2u = i * d + np.arange(d)
 
         if TimeVarying == 0:
             Gx[np.ix_(ind1, ind2x)] = -A
             Gu[np.ix_(ind1, ind2u)] = -B
-            L[ind1, :]              =  C
+            Ltot[ind1, :]           =  C
         else:
             Gx[np.ix_(ind1, ind2x)] = -A[i]
             Gu[np.ix_(ind1, ind2u)] = -B[i]
-            L[ind1, :]              =  C[i]
+            Ltot[ind1, :]           =  C[i]
 
-    G = np.hstack((Gx, Gu))
+    Gtot = np.hstack((Gx, Gu))
+
+    # Now select the rows related with model dynamics
+    G = Gtot[n:Gtot.shape[0], :]
+    L = Ltot[n:Ltot.shape[0]]
 
     # np.savetxt('G_MPC.csv', G, delimiter=',', fmt='%f')
     # np.savetxt('E_MPC.csv', E, delimiter=',', fmt='%f')
 
     G_sparse = spmatrix(G[np.nonzero(G)], np.nonzero(G)[0], np.nonzero(G)[1], G.shape)
-    E_sparse = spmatrix(E[np.nonzero(E)], np.nonzero(E)[0], np.nonzero(E)[1], E.shape)
     L_sparse = spmatrix(L[np.nonzero(L)], np.nonzero(L)[0], np.nonzero(L)[1], L.shape)
 
-    return G_sparse, E_sparse, L_sparse
+    return G_sparse, L_sparse
 
-def BuildMatIneqConst(N, n, np, linalg, spmatrix):
+def BuildMatIneqConst(N, n, np, linalg, spmatrix, Ke, Error_Bounds):
     # Buil the matrices for the state constraint in each region. In the region i we want Fx[i]x <= bx[b]
     Fx = np.array([[ 1., 0., 0., 0., 0., 0.],
                    [ 0., 0., 0., 0., 0., 1.],
@@ -62,6 +65,14 @@ def BuildMatIneqConst(N, n, np, linalg, spmatrix):
                    [ 2.]])# max ey
 
     # Buil the matrices for the input constraint in each region. In the region i we want Fx[i]x <= bx[b]
+    Fux = np.array([-Ke[0, :],  # Need to take into account  u_tot = -Kx + u
+                     Ke[0, :],  # Need to take into account -u_tot =  Kx - u
+                    -Ke[1, :],  # Need to take into account  u_tot = -Kx + u
+                     Ke[1, :]]) # Need to take into account -u_tot =  Kx - u
+
+    print "Fux ", np.squeeze(Fux)
+    print "Fx ", Fx
+
     Fu = np.array([[ 1., 0.],
                    [-1., 0.],
                    [ 0., 1.],
@@ -85,17 +96,44 @@ def BuildMatIneqConst(N, n, np, linalg, spmatrix):
     Futot = linalg.block_diag(*rep_b)
     butot = np.tile(np.squeeze(bu), N)
 
+    rep_c = [np.squeeze(Fux)] * (N)       # This parts considers that u_tot = -Kx + u
+    Mat1  = linalg.block_diag(*rep_c)
+    NoTerminalConstr_x_N = np.zeros((np.shape(Mat1)[0],n)) # No need to consider x_N as there is not u_N
+    Fxutot = np.hstack((Mat1, NoTerminalConstr_x_N))
+
+
     # Let's stack all together
     rFxtot, cFxtot = np.shape(Fxtot)
     rFutot, cFutot = np.shape(Futot)
     Dummy1 = np.hstack( (Fxtot                    , np.zeros((rFxtot,cFutot))))
-    Dummy2 = np.hstack( (np.zeros((rFutot,cFxtot)), Futot))
-    F = np.vstack( ( Dummy1, Dummy2) )
-    b = np.hstack((bxtot, butot))
+    Dummy2 = np.hstack( (Fxutot                   , Futot))
+    F_horizon = np.vstack( ( Dummy1, Dummy2) )
+    b_horizon = np.hstack((bxtot, butot))
+
+
+    # Now constraint x(t) - x_0 \in O_e, basically -x(t) + Lower_bound <= -x_0 <= -x(t) + Upper_bound
+    # --> -x_0 <= -x(t) + Upper_bound, x_0 <= x(t) - Lower_bound)
+    F_0 = np.zeros((2*n, F_horizon.shape[1]))
+    F_0[0:n,0:n]    = -np.eye(n)
+    F_0[n:2*n, 0:n] =  np.eye(n)
+
+    F = np.vstack((F_0, F_horizon))
+    Lower_bound = Error_Bounds[0]
+    Upper_bound = Error_Bounds[1]
+    b = np.hstack((Upper_bound, -Lower_bound, b_horizon))
+
+    E = np.zeros((F.shape[0], n))
+    E[0:n,0:n]   = -np.eye(n)
+    E[n:2*n,0:n] =  np.eye(n)
+
+    # np.savetxt('F_MPC.csv', F, delimiter=',', fmt='%f')
+    # np.savetxt('b_MPC.csv', b, delimiter=',', fmt='%f')
+    # np.savetxt('E_MPC.csv', E, delimiter=',', fmt='%f')
+
 
 
     F_sparse = spmatrix(F[np.nonzero(F)], np.nonzero(F)[0], np.nonzero(F)[1], F.shape)
-    return F_sparse, b
+    return F_sparse, b, E
 
 def BuildMatCost(Q, R, P, N, linalg, np, spmatrix, vt):