84 lines
No EOL
3.1 KiB
Matlab
Executable file
84 lines
No EOL
3.1 KiB
Matlab
Executable file
% DEMO LQ CONTROL
|
|
% Author: C. Poussot-Vassal [MOR Digital Systems / Onera]
|
|
% Date : December 2014 (creation)
|
|
% March 2022 (update)
|
|
%
|
|
% Description
|
|
% Demonstration script for LQ control design and robustness properties.
|
|
% This small script illustrates how to construct and analyse the LQ control
|
|
% design in a SISO setting. In particular, we compute the LQ controller by
|
|
% solving the CARE equation, then we illustrate some classic margin
|
|
% properties of this special and well known control method.
|
|
%
|
|
% Note
|
|
% The script uses the "+insapack" matlab package.
|
|
%
|
|
clear all, close all, clc;
|
|
set(groot,'DefaultFigurePosition', [300 100 1000 600]);
|
|
set(groot,'defaultlinelinewidth',2)
|
|
set(groot,'defaultlinemarkersize',14)
|
|
set(groot,'defaultaxesfontsize',18)
|
|
list_factory = fieldnames(get(groot,'factory')); index_interpreter = find(contains(list_factory,'Interpreter')); for iloe1 = 1:length(index_interpreter); set(groot, strrep(list_factory{index_interpreter(iloe1)},'factory','default'),'latex'); end
|
|
|
|
%%% INSAPACK
|
|
addpath('/Users/charles/Documents/GIT/insapack')
|
|
|
|
%%% >> Chose a SISO model (2 cases are proposed)
|
|
nom = 'tf4'
|
|
%nom = 'random'
|
|
switch lower(nom)
|
|
case 'tf4'
|
|
Gs = tf([1 9 6],conv([1/100 2*.3/10 1],[10 5 1]));
|
|
Gs_ss = ss(Gs);
|
|
case 'random'
|
|
Gs_ss = stabsep(rss(30));
|
|
Gs = tf(Gs_ss);
|
|
end
|
|
|
|
%%% >> LQ control with infinite horizon, u = -K*(x-x_ref)
|
|
% Design parameters
|
|
[A,B,C,D] = ssdata(Gs_ss);
|
|
n = length(A);
|
|
nu = size(B,2);
|
|
R = eye(nu);
|
|
%
|
|
rho = 1e3; % You may play with rho>0
|
|
Q = rho*eye(n);
|
|
% Continuous Algebraic Riccati Equation resolution ...
|
|
[P,L,G] = care(A,B,Q,R); % CARE
|
|
% ... and controller construction
|
|
K = R\eye(nu)*B'*P; % Optimal control u = -Kx
|
|
% Transfer of interest
|
|
Ls = tf(ss(A,B,K,0)); % Loop transfer => for robustness
|
|
Ss = (1+Ls)\1; % Sensitivity function => for disturbance rejection
|
|
Ts = 1-Ss; % Complementary sensitivity function
|
|
h = 1/(C*((-A+B*K)\B)); % Steady-state closed-loop gain
|
|
CLs = h*tf(ss(A-B*K,B,C,0)); % Closed-loop => input-output performance
|
|
% Margin and Hinf-norm
|
|
gamma = norm(CLs,inf) % Hinf norm
|
|
S = allmargin(Ls) % Margins
|
|
% Frequency responses
|
|
figure
|
|
subplot(121), hold on
|
|
bodemag(Ss,'b',Ts,'r-',tf(1,1),'k-'), grid on
|
|
title('Sensitivity function')
|
|
legend('$S(\imath \omega)$','$T(\imath \omega)$','1','Location','Best')
|
|
subplot(122), hold on
|
|
sigma(CLs,'b',gamma*tf(1,1),'r'), grid on
|
|
title('Closed-loop')
|
|
legend('$T(\imath \omega)$','$\gamma$','Location','Best')
|
|
% Robustness margin illustration
|
|
figure
|
|
subplot(121), hold on
|
|
margin(Ls)
|
|
legend('$L(\imath \omega)$','Location','Best')
|
|
subplot(122), hold on
|
|
nyquist(Ls,'b-'), grid on
|
|
theta = linspace(0,2*pi,1e3);
|
|
plot(cos(theta),sin(theta),'g-');
|
|
plot(cos(theta)-.5,sin(theta),'r--');
|
|
legend('$L(\imath \omega)$','PM = $L(\imath \omega)$ crosses circle','$L(\imath \omega)$ outside circle','Location','Best'), axis equal
|
|
% %% Step response
|
|
% figure, hold on
|
|
% step(Gs,'b-')
|
|
% step(CLs,'r-') |