% pvtol_lqr.m - LQR control for a vector thrust aircraft
% RMM, 14 Jan 03
%
% This file implements the LQR example described in 'Optimal Control'.
%
% Set the OBCPRINT variable to a nonzero value to generate printouts
% suitable for inclusion in the book.

% See if we want to print
if (exist('OBCPRINT') && OBCPRINT) printf=1; else printf=0; end;

%%
%% Ducted fan dynamics
%%
%% These are the dynamics for the ducted fan, written in state space
%% form.  This is basically the same as the PVTOL example, except for
%% the inclusing on the l \neq 0 term.
%%

% System parameters
J = 0.0475;			% inertia around pitch axis
m = 1.5;			% mass of fan
r = 0.25;			% distance to flaps
g = 10;				% gravitational constant
gamma = 0.51;			% counterweighted mass
d = 0.2;	 		% damping factor (estimated)
l = 0.05;			% offset of center of mass

% System matrices (entire plant: 2 input, 2 output)
A = [  0     0     0     1     0     0;
       0     0     0     0     1     0;
       0     0     0     0     0     1;
       0     0  -gamma -d/m    0     0;
       0     0     0     0   -d/m    0;
       0     0 -m*g*l/J  0     0     0 ];
 
B = [  0     0;
       0     0;
       0     0;
      1/m    0;
       0    1/m;
      r/J    0    ];

C = [  1     0    0      0     0     0;
       0     1    0      0     0     0 ];

D = [  0     0;   0      0];

%%
%% Construct inputs and outputs corresponding to steps in xy position
%%
%% The vectors xd and yd correspond to the states that are the desired
%% equilibrium states for the system.  The matrices Cx and Cy are the 
%% corresponding outputs.
%%
%% The way these vectors are used is to compute the closed loop system
%% dynamics as
%%
%%	xdot = Ax + B u		=>	xdot = (A-BK)x + K xd
%%         u = -K(x - xd)		   y = Cx
%%
%% The closed loop dynamics can be simulated using the "step" command, 
%% with K*xd as the input vector (assumes that the "input" is unit size,
%% so that xd corresponds to the desired steady state.
%%

xd = [1; 0; 0; 0; 0; 0];  Cx = [1 0 0 0 0 0];
yd = [0; 1; 0; 0; 0; 0];  Cy = [0 1 0 0 0 0];

%%
%% LQR design
%%

% Start with a diagonal weighting
Qx1 = diag([1, 1, 1, 1, 1, 1]);
Qu1a = 0.1 * diag([1, 1]);
K1a = lqr(A, B, Qx1, Qu1a);

% Close the loop: xdot = Ax + B K (x-xd)
H1ax = ss(A-B*K1a,B(:,1)*K1a(1,:)*xd,Cx,0);
H1ay = ss(A-B*K1a,B(:,2)*K1a(2,:)*yd,Cy,0);
figure; clf; if (printf) subplot(221); end;
step(H1ax, H1ay, 10); title('');
lgh = legend('x', 'y'); legend(lgh, 'boxoff');
if (printf) print -depsc 'pvtol-lqrstep1.eps'; end;

% Look at different input weightings
Qu1b = diag([1, 1]); K1b = lqr(A, B, Qx1, Qu1b);
H1bx = ss(A-B*K1b,B(:,1)*K1b(1,:)*xd,Cx,0);

Qu1c = diag([10, 10]); K1c = lqr(A, B, Qx1, Qu1c);
H1cx = ss(A-B*K1c,B(:,1)*K1c(1,:)*xd,Cx,0);

figure; clf; if (printf) subplot(221); end;
step(H1ax, H1bx, H1cx, 10); title('');
lgh = legend('rho = 0.1', 'rho = 1', 'rho = 10', 'Location', 'southeast');
legend(lgh, 'boxoff');
if (printf) print -depsc 'pvtol-lqrstep2.eps'; end;

% Output weighting
Qx2 = [Cx; Cy]' * [Cx; Cy];
Qu2 = 0.1 * diag([1, 1]);
K2 = lqr(A, B, Qx2, Qu2);

H2x = ss(A-B*K2,B(:,1)*K2(1,:)*xd,Cx,0);
H2y = ss(A-B*K2,B(:,2)*K2(2,:)*yd,Cy,0);
figure; step(H2x, H2y, 10);
lgh = legend('x', 'y'); 

%%
%% Physically motivated weighting
%%
%% Shoot for 1 cm error in x, 10 cm error in y.  Try to keep the angle
%% less than 5 degrees in making the adjustments.  Penalize side forces
%% due to loss in efficiency.
%%

Qx3 = diag([100, 1, 2*pi/9, 0, 0, 0]);
Qu3 = diag([10, 1]);
K3 = lqr(A, B, Qx3, Qu3);

H3x = ss(A-B*K3,B(:,1)*K3(1,:)*xd,Cx,0);
H3y = ss(A-B*K3,B(:,2)*K3(2,:)*yd,Cy,0);
[x, Tx, states] = step(H3x, 10);
u1 = K3(1,:) * states';

[y, Ty, states] = step(H3y, 10);
u2 = K3(2,:) * states';

% First plot the x and y position
figure; clf; if (printf) subplot(221); end;
plot(Tx, x, '-', Ty, y, '--');

lgh = legend('x', 'y'); legend(lgh, 'boxoff');
if (printf) print -depsc 'pvtol-lqr-phystates.eps'; end;

% Now print the inputs
figure; clf; if (printf) subplot(221); end;
plot(Tx, u1, '-', Ty, u2, '--');

lgh = legend('u1', 'u2'); legend(lgh, 'boxoff');
if (printf) print -depsc 'pvtol-lqr-phyinputs.eps'; end;

%%
%% Velocity control
%%
%% In this example, we modify the system so that we control the
%% velocity of the system in the x direction.  We ignore the 
%% dynamics in the vertical (y) direction.  These dynamics demonstrate
%% the role of the feedforward system since the equilibrium point 
%% corresponding to vd neq 0 requires a nonzero input.
%%
%% For this example, we use a control law u = -K(x-xd) + ud and convert 
%% this to the form u = -K x + N r, where r is the reference input and
%% N is computed as described in class.
%%

% Extract system dynamics: theta, xdot, thdot
Av = A([3 4 6], [3 4 6]);
Bv = B([3 4 6], 1);
Cv = [0 1 0];				% choose vx as output
Dv = 0;	

% Design the feedback term using LQR
Qxv = diag([2*pi/5, 10, 0]);
Quv = 0.1;
Kv = lqr(Av, Bv, Qxv, Quv);

% Design the feedforward term by solve for eq pt in terms of reference r
T = [Av Bv; Cv Dv];			% system matrix
Nxu = T \ [0; 0; 0; 1];			% compute [Nx; Nu]
Nx = Nxu(1:3); Nu = Nxu(4);		% extract Nx and Nu
N = Nu + Kv*Nx;				% compute feedforward term

%%
%% Design #1: no feedforward input, ud
%%

Nv1 = [0; 1; 0];
Hv1 = ss(Av-Bv*Kv, Bv*Kv*Nx, Cv, 0);
figure; step(Hv1, 10);

%%
%% Design #2: compute feedforward gain corresponding to equilibrium point
%%

Hv2 = ss(Av-Bv*Kv, Bv*N, Cv, 0);
figure; step(Hv2, 10);

%%
%% Design #3: integral action
%%
%% Add a new state to the system that is given by xidot = v - vd.  We
%% construct the control law by computing an LQR gain for the augmented
%% system.
%%

Ai = [Av, [0; 0; 0]; [Cv, 0]];
Bi = [Bv; 0];
Ci = [Cv, 0];
Di = Dv;

% Design the feedback term, including weight on integrator error
Qxi = diag([2*pi/5, 10, 0, 10]);	% Try setting Qxi4 to 1, 0.1, etc
Qui = 0.1;
Ki = lqr(Ai, Bi, Qxi, Qui);

% Desired state (augmented)
xid = [0; 1; 0; 0];

% Construct the closed loop system (including integrator)
Hi = ss(Ai-Bi*Ki,Bi*Ki*xid - [0; 0; 0; Ci*xid],Ci,0);
figure; step(Hi, 10);
