From 5b92f781bce037bca71e40cf0611e88ffecaa8a3 Mon Sep 17 00:00:00 2001 From: Enrico Lumetti Date: Sun, 29 Aug 2021 18:41:58 +0200 Subject: [PATCH] Add LQG controller for two-mass system --- robust_control/two_mass_system_lqg.m | 311 +++++++++++++++++++++++++++ 1 file changed, 311 insertions(+) create mode 100644 robust_control/two_mass_system_lqg.m diff --git a/robust_control/two_mass_system_lqg.m b/robust_control/two_mass_system_lqg.m new file mode 100644 index 0000000..8aa1314 --- /dev/null +++ b/robust_control/two_mass_system_lqg.m @@ -0,0 +1,311 @@ + +%% 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) ; +[~, Info] = ucover(data, G.NominalValue, 4, []); +Im = tf(Info.W1); + +%% Controller Definition +Gn = G.NominalValue; +Gn_fullstate = G_fullstate.NominalValue; + +s = tf('s'); + +% output sensitivity weight +Ms = 1.7; % peak sensitivity (~ stability margin) +omega_b = 5; % open loop bandwidth +eps_s = 0.01; % low frequency sensitivity upper bound (~ robust tracking) +Ws = (s/Ms + omega_b)/(s+omega_b*eps_s); + +%%%% loop invertion controller +% K = inv(Gn); +% K = K*1/s * 1/(1+10*s) * (1+0.7*s) * 100; +% K.OutputName = 'u'; +% +% AP_u = AnalysisPoint('u', 2); +% Gcl = feedback(G*AP_u*K, eye(2)); +% Gcl.InputName = 'r'; +% Gcl.OutputName = 'y'; +% 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), 1e7*eye(2)); +% input weight +R = 1e-3*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', {'u', '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)); +Gcl.InputName = 'r'; +L = ss(G_feedback)*1/s; + +%%%% LQG controller +% Kalman filter +G.InputName = 'uin'; +G.OutputName = 'y'; +Sum = sumblk('uin = u + w', 2); +% G_noise models the plant G with additive input noise w +G_noise = connect(G, Sum, {'u', 'w'}, 'y'); +% input noise covariance matrix +Q = 0.1 * eye(2); +% measurement noise covariance matrix +R = 0.1 * eye(2); %1 centimeter stdev +N = zeros(2); +Kf = kalman(G_noise.NominalValue, Q, R, N, [1 2], [1 2]); +Klqg = lqgtrack(Kf, K.D, '1dof'); +G_lqg = connect(G_noise, Klqg, {'e1', 'e2', 'w'}, {'y'}); +G_lqg.InputName = {'e(1)', 'e(2)', 'w(1)', 'w(2)'}; +Sum2 = sumblk('e = r - y', 2); +G_lqg_cl = connect(G_lqg, Sum2, {'r', 'w'}, 'y'); + +L_lqg = ss(G_lqg([1 2], [1 2])); +%L = L_lqg; + +%%%% 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 + +if hinfnorm(S*Ws) <= 1 + disp('Nominal performance specifications on sensitivity are met') +else + disp('Nominal performance specifications on sensitivity are not met') +end +%% close figures +close all; + +%% Singular value robustness and performance analysis +figure('Name', 'Robust stability analysis'); +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') + +figure('Name', 'Sensitivity and loop performance'); +% open loop performance specifications +subplot(2, 2, 1); +sigma(L); +title('Open loop specifications: L'); + +subplot(2, 2, 2); +[SV, W] = sigma(S); +semilogx(W, 20*log10(max(SV, [], 1))); hold on; +% sensitivity weight +r = freqresp(1/Ws, W); +semilogx(W, 20*log10(abs(r(:, :))), 'r'); +hold off; +title('Open loop specifications: S'); + +subplot(2, 2, 3); +[SV, W] = sigma(T); +semilogx(W, 20*log10(max(SV, [], 1))); +title('Open loop specifications: T'); + +%% LQG/LTR analysis +figure('Name', 'LTR robust stability analysis'); +w = logspace(-1, 10, 100); +SV = sigma(eye(2) + inv(L_lqg), w); +semilogx(w, 20*log10(min(SV, [], 1))); hold on; +sigma(Im, w, 'r'); hold off; +title('Robust stability analysis') + +S_lqg = inv(eye(2)+L_lqg); +T_lqg = eye(2) - S_lqg; + +figure('Name', 'LTR sensitivity and loop performance'); +% open loop performance specifications +subplot(2, 2, 1); +sigma(L_lqg); hold on; +sigma(L); hold off; +title('Open loop specifications: L'); +legend('L LTR', 'L'); + +subplot(2, 2, 2); +[SV, W] = sigma(S_lqg); +semilogx(W, 20*log10(max(SV, [], 1))); hold on; +% sensitivity weight +r = freqresp(1/Ws, W); +semilogx(W, 20*log10(abs(r(:, :))), 'r'); +hold off; +title('Open loop specifications: S'); + +subplot(2, 2, 3); +[SV, W] = sigma(T_lqg); +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); +figure('Name', 'mu analysis'); +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 + +%% 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'); + +%% Simulation +t = 0:0.001:3; +r1 = 0*t + 0.1; % 10 centimeters displacement +r2 = 0*t; + +% noise +w = 0.05 .* randn(2, length(t)); + +% transfer function from reference to controller output +G_ur = getIOTransfer(Gcl, 'r', 'u'); + +y = lsim(T, [r1; r2], t); +u = lsim(G_ur, [r1; r2], t); +realizations = usample(Gcl, 20); +ninputs = 2; +y_unc = zeros(ninputs*length(realizations), length(t)); +u_unc = zeros(ninputs*length(realizations), length(t)); + +for i=1:length(realizations) + G_ur_i = getIOTransfer(realizations(:, :, i, 1), 'r', 'u'); + p = lsim(realizations(:,:, i, 1), [r1; r2], t); + q = lsim(G_ur_i, [r1; r2], t); + y_unc(ninputs*i, :) = p(:, 1); + y_unc(ninputs*i+1, :) = p(:, 2); + u_unc(ninputs*i, :) = q(:, 1); + u_unc(ninputs*i+1, :) = q(:, 2); +end + +Twc = usubs(Gcl, wcg); +Tur = usubs(G_ur, wcg); +y_wc = lsim(Twc, [r1; r2], t); +u_wc = lsim(Tur, [r1; r2], t); + +y_noisy = lsim(G_lqg_cl, [r1; r2; w], t); + +%% Output and control action plot +figure('Name', 'Transient simulation: output'); +subplot(2, 3, 1); +plot(t, r1, '--b', t, y(:, 1), 'r'); +title('Nominal system output'); +subplot(2, 3, 4); +plot(t, r2, '--b', t, y(:, 2), 'r'); + +subplot(2, 3, 2); +plot(t, r1, '--b'); hold on; +for i=1:length(realizations) + plot(t, y_unc(ninputs*i, :), 'r'); +end +hold off; +title('Uncertain system output'); + +subplot(2, 3, 5); +plot(t, r2, '--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, r1, '--b', t, y_wc(:, 1), 'r'); +title('Worst case system output'); + +subplot(2, 3, 6); +plot(t, r2, '--b', t, y_wc(:, 2), 'r'); + +figure('Name', 'Transient simulation: control'); +subplot(2, 3, 1); +plot(t, r1, '--b', t, u(:, 1), 'r'); +title('Nominal system control action'); +subplot(2, 3, 4); +plot(t, r2, '--b', t, u(:, 2), 'r'); + +subplot(2, 3, 2); +plot(t, r1, '--b'); hold on; +for i=1:length(realizations) + plot(t, u_unc(ninputs*i, :), 'r'); +end +hold off; +title('Uncertain system control action'); + +subplot(2, 3, 5); +plot(t, r2, '--b'); hold on; +for i=1:length(realizations) + plot(t, u_unc(ninputs*i+1, :), 'r'); +end +hold off; + +subplot(2, 3, 3); +plot(t, r1, '--b', t, u_wc(:, 1), 'r'); +title('Worst case system control action'); + +subplot(2, 3, 6); +plot(t, r2, '--b', t, u_wc(:, 2), 'r'); + +figure('Name', 'Noisy system transient'); +subplot(2, 1, 1); +plot(t, r1, '--b', t, y_noisy(:, 1), 'r'); +title('Noisy system output'); +subplot(2, 1, 2); +plot(t, r2, '--b', t, y_noisy(:, 2), 'r'); \ No newline at end of file