Setup initial problem. Linearized and discretized system. Ready for LQ-MPC tracking.

Our Model/main.asv

+% main
+
+%% 1) First Obtain Linearized A,B Matrices
+x0 = [0,0,0,0];
+u0 = 0;
+[A,B] = symLin(x0,u0);
+
+%% 1.01) Obtain C and D matrices
+R = 80/
+C =  
+
+%% 1.1) Open Loop Stable?
+[V,D] = eig(A);
+% not stable! Duh!
+
+%% 1.2) Controllable?s
+Co = ctrb(A,B);
+rank(Co)
+% it's controllable!!!! :) :) :)
+
+%% 2) Discretize
+Ts = 0.01;  % 100 Hz in ROB 550
+sysC = ss(A,B,C,D)
+sysD = c2d(sysC,Ts);
+[Ad,Bd,Cd,Dd,Ts] = ssdata(sysD);
+
+

Our Model/main.m

+% main
+
+%% 1) First Obtain Linearized A,B Matrices
+x0 = [0,0,0,0];
+u0 = 0;
+[A,B] = 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));
+
+%% 1.1) Open Loop Stable?
+[Vec,Val] = eig(A);
+% not stable! Duh!
+
+%% 1.2) Controllable?s
+Co = ctrb(A,B);
+rank(Co)
+% it's controllable!!!! :) :) :)
+
+%% 2) Discretize
+Ts = 0.01;  % 100 Hz in ROB 550
+sysC = ss(A,B,C,D);
+sysD = c2d(sysC,Ts);
+[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) LQ-MPC Tracking Problem
+
+

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
+

Our Model/mip_model.m

+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
+    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
+    phi_dd = subs(phi_dd,[mw, mr, R, L, Iw, Ir, g],[mw_, mr_, R_, L_, Iw_, Ir_, g_]);
+    phi_dd = subs(phi_dd,[th, th_d, tao],[x(3), x(4), u]);
+    
+    th_dd = subs(th_dd,[mw, mr, R, L, Iw, Ir, g],[mw_, mr_, R_, L_, Iw_, Ir_, g_]);
+    th_dd = subs(th_dd,[th, th_d, tao],[x(3), x(4), u]);
+    
+    %% Return the derivative of the state
+    xdot = [x(2); phi_dd; x(4); th_dd];
+    %xdot = double(xdot);
+end
+

Our Model/symLin.m

+function [A,B] = symLin(x0,u0)
+%symLin Computes the Linearization at x0,u0
+    % Using Symbolic Toolbox
+    % Inputs: 
+    %           x0: state to lin about (4x1) 
+    %           u0: input to lin about (scalar)
+    % Outputs:
+    %           A: A Matrix (4x4)
+    %           B: B Matrix (4x1)
+
+syms x1 x2 x3 x4 u A_ B_
+xdot = mip_model([x1;x2;x3;x4],u);
+
+A_ = jacobian(xdot,[x1;x2;x3;x4]);
+B_ = jacobian(xdot,u);
+
+% Evaluate at equilibrium points
+x1 = x0(1);
+x2 = x0(2);
+u = u0;
+
+% Obtain Numerical Constants 
+A = subs(A_,[x1,x2,x3,x4,u],[x0,u]);
+B = subs(B_,[x1,x2,x3,x4,u],[x0,u]);
+
+% A = vpa(A);
+% B = vpa(B);
+
+A = double(A);
+B = double(B);
+%A = vpa(subs(A_,[],[]),5);
+%B = vpa(subs(B_),5);
+
+end
+

Our Model/test_model.m

+% Test Model
+
+%% 0 case
+x1 = [0;0;0;0];
+u1 = [0];
+xdot1 = mip_model(x1,u1)
+
+%% 1 case
+x1 = [0;0;0;0];
+u1 = [1];
+xdot1 = mip_model(x1,u1)
+
+%% syms case
+syms x1 x2 x3 x4 u
+xdot2 = mip_model([x1;x2;x3;x4],u);
+
+%% Test symLin (case 1)
+x0 = [0,0,0,0];
+u0 = 0;
+[A,B] = symLin(x0,u0)