Built augmented state matrices, added cost function weights and control...

Built augmented state matrices, added cost function weights and control constraints. Also, added formQPMatrices.m and tested it for a horizon of 10.

Our Model/formQPMatrices.m

+%% formQPMatrices
+function [H, L, G, W, T, IMPC] = formQPMatrices(A, B, Q, R, ...
+                                                P, xlim, ulim, N)
+    
+    %Setup Matrices                                        
+    nx = size(A,1);
+    nu = size(B,2);
+
+    S = zeros(nx * N, nu * N);
+    M = zeros(nx * N, nx);
+    IMPC = [];
+    Qbar = [];
+    Rbar = [];
+    
+    xMin = zeros(nx*N, 1);
+    xMax = zeros(nx*N, 1);
+    uMin = zeros(nu*N, 1);
+    uMax = zeros(nu*N, 1);
+    
+    for i = 1:N
+        %Variables to construct diagonal elements        
+        startX = (i-1)*nx + 1;
+        endX = i * nx;
+        
+        startU = (i-1)*nu + 1;
+        endU = i * nu;
+        
+        %Construct S Matrix
+        S(startX:endX, startU:endU) = B;
+    
+        for j = i:-1:2
+            startXInner = (i-1)*nx + 1;
+            endXInner = (i) * nx;
+
+            startUInner = (j-2)*nu + 1;
+            endUInner = (j-1) * nu;
+
+            S(startXInner:endXInner, startUInner:endUInner) = A^(i-j + 1)*B;
+        end
+        
+        %M Matrix
+        M(startX:endX, :) = A^i;
+        
+        %Qbar 
+        if(i ~= N)
+            Qbar = blkdiag(Qbar, Q);
+        else
+            Qbar = blkdiag(Qbar, P);
+        end
+        
+        if( i == 1)
+            IMPC = eye(nu);
+        else
+            IMPC = [IMPC zeros(nu, nu)];
+        end
+        
+        %Rbar
+        Rbar = blkdiag(Rbar, R);
+        
+        %Constraints Vectors
+        xMax(startX:endX) = xlim.max';
+        xMin(startX:endX) = xlim.min';
+        uMax(startU:endU) = ulim.max';
+        uMin(startU:endU) = ulim.min';
+    end
+    
+    %Compute H
+    H = S'*Qbar*S + Rbar;
+    
+    %Compute gain for Q
+    L = S'*Qbar*M;
+    
+    T = [-M; M; zeros(N*nu, nx); zeros(N*nu, nx)];
+    
+%     W = [xlim.max'; -xlim.min'; ulim.max'; -ulim.min'];
+    W = [xMax; -xMin; uMax; -uMin];
+    
+    G = [S; -S; eye(N); -eye(N)];
+    
+end
\ No newline at end of file

Our Model/main.asv

@@ -3,25 +3,53 @@
 %% 1) First Obtain Linearized A,B Matrices
 x0 = [0,0,0,0];
 u0 = 0;
-[A,B] = symLin(x0,u0);
+[Ac,Bc] = symLin(x0,u0);
 
 %% 1.01) Obtain C and D matrices
-R = 80/
-C =  
+R = 80/1000; %% TODO Update mip_model if you change this and vice versa
+% C = [R 0 0 0;
+%      0 R 0 0];
+C = [R 0 0 0];
+D = zeros(size(C,1),size(Bc,2));
 
 %% 1.1) Open Loop Stable?
-[V,D] = eig(A);
+[Vec,Val] = eig(Ac);
 % not stable! Duh!
 
 %% 1.2) Controllable?s
-Co = ctrb(A,B);
+Co = ctrb(Ac,Bc);
 rank(Co)
 % it's controllable!!!! :) :) :)
 
 %% 2) Discretize
 Ts = 0.01;  % 100 Hz in ROB 550
-sysC = ss(A,B,C,D)
+sysC = ss(Ac,Bc,C,D);
 sysD = c2d(sysC,Ts);
-[Ad,Bd,Cd,Dd,Ts] = ssdata(sysD);
+[Ad,Bd,Cd,Dd,Ts] = ssdata(sysD)
+
+%% 2.1) Open Loop Stable?
+[Vec,Val] = eig(Ad)
+% No, outside of the unit ball
+
+%% 2.2) Controllable?
+Cont = ctrb(Ad,Bd);
+rank(Cont)
+% Yes, rank = 4
+
+%% 3.1) Construct Augmented Dyanmic Matrices
+%Useful Values
+nx = length(Ad);    %Size of state
+nu = size(Bd,2);    %Size of input
+ne = 1;             %Size of Error
+nr = ne;             %Size of Reference
+
+A = [Ad, zeros(nx, nu + ne + nr)];
+B = [Bd, zeros(nx, nu + ne + nr)]
+%% 3.2) LQ-MPC Tracking Problem
+
+
+%Weights
+% Q = eye(
+
 
 

Our Model/main.m

@@ -3,26 +3,27 @@
 %% 1) First Obtain Linearized A,B Matrices
 x0 = [0,0,0,0];
 u0 = 0;
-[A,B] = symLin(x0,u0);
+[Ac,Bc] = symLin(x0,u0);
 
 %% 1.01) Obtain C and D matrices
 R = 80/1000; %% TODO Update mip_model if you change this and vice versa
-C = [R 0 0 0;
-     0 R 0 0];
-D = zeros(size(C,1),size(B,2));
+% C = [R 0 0 0;
+%      0 R 0 0];
+C = [R 0 0 0];
+D = zeros(size(C,1),size(Bc,2));
 
 %% 1.1) Open Loop Stable?
-[Vec,Val] = eig(A);
+[Vec,Val] = eig(Ac);
 % not stable! Duh!
 
 %% 1.2) Controllable?s
-Co = ctrb(A,B);
+Co = ctrb(Ac,Bc);
 rank(Co)
 % it's controllable!!!! :) :) :)
 
 %% 2) Discretize
 Ts = 0.01;  % 100 Hz in ROB 550
-sysC = ss(A,B,C,D);
+sysC = ss(Ac,Bc,C,D);
 sysD = c2d(sysC,Ts);
 [Ad,Bd,Cd,Dd,Ts] = ssdata(sysD)
 
@@ -35,6 +36,48 @@ Cont = ctrb(Ad,Bd);
 rank(Cont)
 % Yes, rank = 4
 
-%% 3) LQ-MPC Tracking Problem
+%% 3.1) Construct Augmented Dyanmic Matrices
+%Useful Values
+nx = length(Ad);    %Number of states
+nu = size(Bd,2);    %Number of inputs
+ne = 1;             %Number of error terms
+nr = ne;            %Number of References (same as ne)
+ny = size(Cd, 1);   %Number of outputs
+nxe = nx + ne;      %Useful for submatrices when calling DARE
+
+A = [Ad, zeros(nx, nu + ne + nr);
+     C, zeros(ny, ne + nu + nr);
+     zeros(nu, nx + ne), ones(nu, nu), zeros(nu, nr);
+     1, 0, 0, 0, 0, 0, 1];
+B = [Bd;
+     0;
+     1;
+     0];
+%% 3.2) LQ-MPC Tracking Problem
+%Weights
+Q = diag([zeros(1, nx), 1, zeros(1, nu + nr)]);
+R = 1;
+Pxe = dare(A(1:nxe, 1:nxe), B(1:nxe), ...
+          Q(1:nxe, 1:nxe), R);
+P = blkdiag(Pxe, zeros(nu + nr));
+      
+%Constraints
+lN = inf;       %Large number (sometimes infinity doesn't work)
+tMax = 0.2;     %Maximum torque [units]
+
+%State Constraints
+xlim.max = [lN, lN, lN, tMax, lN, lN, lN];
+xlim.min = -xlim.max;
+
+%Input constraints (TODO: review why this is just infinity and not tMax)
+ulim.max = lN;
+ulim.min = -ulim.max;
+
+%Horizon
+N = 10;
+
+[H, L, G, W, T, IMPC] = formQPMatrices(A, B, Q, R, P, xlim, ulim, N)
+
+
 
 

Our Model/mip_model.asv

-function [xdot] = mip_model(x,u)
-%mip_model
-    % Inputs
-        % x: the state (phi,phidot,theta,thetadot) (4x1)
-        % u: the input (motor torque) (1x1)
-    % Output
-        % xdot: the derivative of the state (4x1)
-        
-    % Parameters
-%     mw = (2*30)/1000;
-%     mr = 750/1000;
-%     R = 80/1000;
-%     L = 10/100;
-%     Iw = 2*0.001;
-%     Ir = 0.012;
-%     g = 9.8; 
-    %%
-    syms mw mr R L Iw Ir g
-    syms tao phi_dd th_dd th_d th
-    
-    % w1*phi_dd + w2*theta_dd            = w3*sin(theta)           - Tao
-    % w4*phi_dd + w5*cos(theta)*theta_dd = theta_dd^2 * sin(theta) + Tao
-    
-    w1 = mr * R * L;
-    w2 = Ir + mr * L*L;
-    w3 = mr * g * L;
-    w4 = Iw + (mr + mw)*R*R;
-    w5 = mr * R * L;
-    w6 = mr * R * L;
-    
-    %% Prince's Algebra
-    syms phi_dd_ th_dd_
-    phi_dd_ = (w3*w1*cos(th)*sin(th)-w2*w1*th_d*th_d*sin(th)-(w1*cos(th)+w2)*tao)...
-        / (w1*w1*cos(th)*cos(th)-w2*w4);
-    th_dd_ = (w4*th_d*th_d*sin(th)+tao-w4*phi_dd_) / (w1*cos(th));
-    
-    %% Substitute
-    subs(phi_dd,[],[]
-    subs(cos(a) + sin(b), [a, b], [sym('alpha'), 2])
-    
-end
-