State Space Control
Housekeeping
close all; clear; clc; s = tf('s');
STATE SPACE MODEL:
vehicle mass 
damping coefficient 
n = 100; % number of timesteps / q & r test values
t_start = 0; % simulation start time
t_stop = 10; % simulation stop time
t_step = t_stop/n; % simulation step time
t = t_start:t_step:t_stop; % simulation time array
m = 1000; % vehicle mass
b = 50; % damping (wind resistance)
uOL = 500*ones(size(t)); % open-loop input force
uCL = ones(size(t)); % closed-loop input force
A = [-b/m]; % state matrix
B = [1/m]; % input matrix
C = [1]; % output matrix
D = [0]; % feedforward matrix
sys = ss(A,B,C,D); % state space system
x0 = [0]; % initial condition[s]
ref = 10; % reference input (target)
InputForceLimit = 1.5e5; % maximum available input (set to inf if unknown)
OPEN LOOP SIMULATION:
[y, tout] = lsim(sys,uOL,t,x0);
f1 = figure(); f1.Position = [0 0 800 500];
plot(tout, y);
title('Open Loop Simulation');
xlabel('time (s)');
ylabel('speed (m/s)');
LQR SURFACE EVALUATION:
Assuming 
and 
, find q and r such that response error is minimized.
q = linspace(1/100,100000,n); % test values for Q matrix
r = linspace(1/100,1,n); % test values for R matrix
bandwidth_low = 0.9; % lower bound for "near reference" (1.0 = 100%)
bandwidth_high = 1.0; % upper bound for "near reference" (1.0 = 100%)
for q_ind = 1:n % looping through potential q-values
for r_ind = 1:n % looping through potential r-values
Q = (q(q_ind))*(C'*C); % candidate Q matrix
R = r(r_ind)*(1); % candidate R matrix
[K,S,P] = lqr(sys,Q,R); % candidate feedback matrix K
Nbar = rscale(sys,K)*ref; % candidate reference scaling
sys_cl = ss(A-B*K,B*Nbar,C,D); % candidate system
[y, tout] = lsim(sys_cl,uCL,t,x0); % candidate system's response
err(r_ind,q_ind) = sum(y/ref>bandwidth_low & y/ref<bandwidth_high); % percent of time spent within target bandwidth
maxinput(r_ind,q_ind) = max(ref*Nbar-K*y); % maximum input used by control system
end
end
LQR SURFACE PLOT:
[X,Y] = meshgrid(q,r);
f2 = figure(); f2.Position = [0 0 1500 500];
subplot(1,2,1);
Z1 = err;
s1 = surf(X,Y,Z1);
colorbar; shading interp;
title('percent of time near target');
xlabel('q'); ylabel('r'); zlabel('err');
axis([0 100000 0 1 0 100]);
view([225 30]);
subplot(1,2,2);
Z2 = maxinput;
s2 = surf(X,Y,Z2);
colorbar; shading interp;
title('maximum input force used');
xlabel('q'); ylabel('r'); zlabel('maximum input force');
%axis([0 100000 0 1 0 100]);
view([225 30]);
CONSTRAINED LQR SURFACE PLOT:
inputmap = maxinput < InputForceLimit;
f3 = figure(); f3.Position = [0 0 1500 500];
subplot(1,2,1);
Z3 = err.*inputmap;
s1 = surf(X,Y,Z3);
colorbar;
shading interp;
title('percent of time near target (constrained)');
xlabel('q'); ylabel('r'); zlabel('err');
axis([0 100000 0 1 0 100]);
view([225 30]);
subplot(1,2,2);
contourf(X,Y,Z3,20,':');
set(gca,'YAxisLocation','right');
colorbar;
title('percent of time near target (constrained)');
xlabel('q'); ylabel('r');
axis([0 100000 0 1 0 100]);
FINDING THE OPTIMUM SOLUTION:
[best_err location] = max(Z3, [], 'all');
[r_loc, q_loc] = ind2sub(size(err), location);
qq = q(q_loc);
rr = r(r_loc);
PLOT OF SELECTED LQR SYSTEM:
%qq = 10e4; % selected from plots above
%rr = 0.05; % selected from plots above
Q = qq*(C'*C); % computing optimum Q matrix
R = rr*(1); % computing optimum R matrix
[K,S,P] = lqr(sys,Q,R); % finding the optimum feedback gain matrix K
Nbar = rscale(sys,K)*ref; % computing input scaling for steady-state error
sys_cl = ss(A-B*K,B*Nbar,C,D); % constructing LQR controller
[y, tout] = lsim(sys_cl,uCL,t,x0); % calculation of the system response
inputforce = (ref*Nbar-K*y); % calculataing input force applied to plant by controller
f4 = figure(); f4.Position = [0 0 1500 500];
subplot(1,2,1);
plot(tout, y);
title('Response of Selected System'); xlabel('time (s)'); ylabel('Speed (m/s)');
hold on;
yline(ref, '--'); hold on;
yline([0.9*ref, 1.1*ref], ':'); hold off;
axis([0 10 0 ref*1.2]);
legend('response','reference')
subplot(1,2,2);
plot(inputforce)
title('Input Force of Selected System'); xlabel('time (s)'); ylabel('force (N)');
function[Nbar] = rscale(a,b,c,d,k)
% Given the single-input linear system:
% x = Ax + Bu
% y = Cx + Du
% and the feedback matrix K,
%
% the function rscale(sys,K) or rscale(A,B,C,D,K) finds the scale factor N which will
% eliminate the steady-state error to a step reference for a continuous-time,
% single-input system with full-state feedback using the schematic below:
%
% /---------\
% R + u | . |
% ---> N --->() ----> | X=Ax+Bu |--> y = Cx ---> y
% -| \---------/
% | |
% |<---- K <----|
%
% 8/21/96 Yanjie Sun of the University of Michigan under the supervision of Prof. D. Tilbury
% 6/12/98 John Yook, Dawn Tilbury revised error(nargchk(2,5,nargin));
% --- Determine which syntax is being used --- nargin1 = nargin;
if (nargin==2), % System form
[A,B,C,D] = ssdata(a); K=b;
elseif (nargin==5), % A,B,C,D matrices A=a;
A = a;
B = b;
C = c;
D = d;
K = k;
else
error('Input must be of the form (sys,K) or (A,B,C,D,K)')
end
% compute Nbar s = size(A,1);
s = size(A,1);
Z = [zeros([1,s]) 1];
N = inv([A,B;C,D])*Z';
Nx = N(1:s);
Nu = N(1+s);
Nbar = Nu + K*Nx;
end