Spring-linked two mass system

robust_control/two_mass_system.m

@@ -0,0 +1,182 @@
+
+%% Plant definition
+% refer to to Sinha, FIGURE 5.1.1
+clear;
+
+k1 = ureal('k1', 100, 'Percentage', 20);
+k2 = ureal('k2', 500, 'Percentage', 20);
+m = ureal('m', 1, 'Percentage', 1);
+alpha = ureal('alpha', 1, 'Percentage', 5);
+
+A = [
+    0 0 1 0;
+    0 0 0 1;
+    -(k1+k2)/m k2/m -alpha/m 0;
+    k2/m -(k1+k2)/m 0 -alpha/m;
+    ];
+
+B = [0 0; 0 0; 1 0; 0 1];
+C = [eye(2) zeros(2)];
+D = zeros(2);
+
+G = uss(A, B, C, D);
+G_fullstate = uss(A, B, eye(4), zeros(4, 2));
+
+% multiplicative uncertainty
+w = logspace(0, 3, 40);
+systems = usample(G, 40);
+data = frd(systems, w)  ;
+[G2, Info] = ucover(data, G.NominalValue, 4, []);
+Im = tf(Info.W1);
+
+%% Controller Definition
+Gn = G.NominalValue;
+Gn_fullstate = G_fullstate.NominalValue;
+
+s = tf('s');
+
+%%%% loop invertion controller
+% K = inv(Gn);
+% K = K*1/s * 1/(1+10*s) * (1+0.7*s) * 100;
+% 
+% Gcl = feedback(G*K, eye(2));
+% L = Gn*K;
+
+%%%% PI diagonal controller
+% D = evalfr(inv(G), 0);
+% K = D*(s^2 + s + 100) / (1+s) / (1+s/10);
+% 
+% Gcl = feedback(G*K, eye(2));
+% L = Gn*K;
+
+%%%% LQI controller
+% state weight + integrators weight
+Q = blkdiag(1e3*eye(4), 1e9*eye(2));
+% input weight
+R = 1e-6*eye(2);
+K = -lqi(Gn, Q, R, zeros(6, 2));
+K = ss(K);
+K.InputName = {'x(1)', 'x(2)', 'x(3)', 'x(4)', 'e(1)', 'e(2)'};
+K.OutputName = 'u';
+G_fullstate.InputName = 'u';
+G_fullstate.OutputName = 'x';
+G_feedback = connect(G_fullstate, K, "e", "x");
+% only select first and second output
+G_feedback = G_feedback([1 2], [1 2]);
+G_feedback.OutputName = 'y';
+
+Gcl = feedback(G_feedback*1/s, eye(2));
+L = G_feedback.NominalValue*1/s;
+
+%%%% Sensitivity functions
+S = inv(eye(2)+L);
+T = eye(2) - S;
+
+if ~isstable(T)
+    disp('Nominal closed loop is not stable');
+else
+    disp('Nominal closed loop is stable');
+end
+
+%% Simulation
+t = 0:0.001:1;
+u1 = 0*t + 1;
+u2 = 0*t;
+
+y = lsim(T, [u1; u2], t);
+realizations = usample(Gcl, 20);
+ninputs = 2;
+y_unc = zeros(ninputs*length(realizations), length(t));
+for i=1:length(realizations)
+    p = lsim(realizations(:,:, i, 1), [u1; u2], t);
+    y_unc(ninputs*i, :) = p(:, 1);
+    y_unc(ninputs*i+1, :) = p(:, 2);
+end
+
+%% close figures
+close all;
+
+%% Plant sv and multiplicative uncertainty
+figure('Name', 'Open Loop System');
+subplot(2, 1, 1);
+sigma(G);
+hold on;
+title('Singular values of the system');
+
+subplot(2, 1, 2);
+sigma(Im);
+title('Multiplicative Uncertainty Magnitude');
+
+%% Singular value robustness and performance analysis
+figure('Name', 'Loop Analysis');
+subplot(2, 2, 1);
+w = logspace(-1, 10, 100);
+SV = sigma(eye(2) + inv(L), w);
+semilogx(w, 20*log10(min(SV, [], 1))); hold on;
+sigma(Im, w, 'r'); hold off;
+title('Robust stability analysis')
+
+% open loop performance specifications
+subplot(2, 2, 2);
+sigma(L);
+title('Open loop specifications: L');
+
+subplot(2, 2, 3);
+[SV, W] = sigma(S);
+semilogx(W, 20*log10(max(SV, [], 1)));
+title('Open loop specifications: S');
+
+subplot(2, 2, 4);
+[SV, W] = sigma(T);
+semilogx(W, 20*log10(max(SV, [], 1)));
+title('Open loop specifications: T');
+
+%% Structured singular value analysis
+opts = robOptions('VaryFrequency','on');
+[stabmarg,wcg,info] = robstab(Gcl,opts);
+semilogx(info.Frequency,info.Bounds)
+title('Stability Margin vs. Frequency');
+ylabel('Margin');
+xlabel('Frequency');
+legend('Lower bound','Upper bound');
+
+if stabmarg.LowerBound > 1
+    fprintf('Closed loop system is robustly stable: mu=%f-%f at %f rad/s\n', ...
+        stabmarg.LowerBound, stabmarg.UpperBound, stabmarg.CriticalFrequency);
+else
+    fprintf('Closed loop system is not robustly stable: mu=%f-%f at %f rad/s\n', ...
+        stabmarg.LowerBound, stabmarg.UpperBound, stabmarg.CriticalFrequency);
+end
+
+Twc = usubs(Gcl, wcg);
+y_wc = lsim(Twc, [u1; u2], t);
+
+%% Transient plot
+figure('Name', 'Transient simulation');
+subplot(2, 3, 1);
+plot(t, u1, '--b', t, y(:, 1), 'r');
+title('Nominal system response');
+subplot(2, 3, 4);
+plot(t, u2, '--b', t, y(:, 2), 'r');
+
+subplot(2, 3, 2);
+plot(t, u1, '--b'); hold on;
+for i=1:length(realizations)
+    plot(t, y_unc(ninputs*i, :), 'r');
+end
+hold off;
+title('Uncertain system response');
+
+subplot(2, 3, 5);
+plot(t, u2, '--b'); hold on;
+for i=1:length(realizations)
+    plot(t, y_unc(ninputs*i+1, :), 'r');
+end
+hold off;
+
+subplot(2, 3, 3);
+plot(t, u1, '--b', t, y_wc(:, 1), 'r');
+title('Worst case system response');
+
+subplot(2, 3, 6);
+plot(t, u2, '--b', t, y_wc(:, 2), 'r');