% pvtol_est.m - observability and estimation for the vectored thrust aircraft
% RMM, 7 Jan 03

aminit;
amuse('pvtol');

pvtol_params;			% 	System parameters

% System matrices (entire plant: 2 input, 2 output)
[A, B, C, D] = pvtol_linearize([0 0 0 0 0 0], [0 m*g]);

%% Extract out the lateral dynamics
Ax = A([1 3 4 6], [1 3 4 6]);
Bx = B([1 3 4 6], 1);

% single output (position)
Cx = C(1, [1 3 4 6]); Dx = D(1, 1);
pvsys1 = ss(Ax, Bx, Cx, Dx);

% two output (position, angle)
Cx2 = [1 0 0 0; 0 1 0 0]; Dx2 = Dx;
pvsys2 = ss(Ax, Bx, Cx2, Dx2);

%%
%% State space control design
%%
%% We use an LQR design to choose the state feedback gains
%%

K = lqr(Ax, Bx, eye(size(Ax)), eye(size(Bx'*Bx)));

%%
%% Estimator
%% 
%% Pole placement to set estimator gains. 
%%

% Determine desired pole locations using butterworth configuration
[Af,Bf,Cf,Df]=butter(4,10,'s');
[numf, denf] = ss2tf(Af,Bf,Cf,Df);
eigs = pole(tf(numf,denf));

figure(1); clf; subplot(221);
  plot(real(eigs), imag(eigs), 'x', 'linewidth', AM_data_linewidth); hold on;
  plot([-15 5], [0 0], 'k-', 'linewidth', AM_ref_linewidth); hold on;
  plot([0 0], [-10 10], 'k-', 'linewidth', AM_ref_linewidth); hold on;
  amaxis([-15 5 -10 10]); 
  xlabel('real'); ylabel('imag');
  
% fid = fopen('pvtol-butterworth.dat', 'w');
% for eig = eigs
%   fprintf(fid, '%g %g\n', real(eig), imag(eig));
% end;

  amprint('pvtol-butterworth.eps');

L1 = transpose( place(Ax', Cx', eigs) )

% Look at the estimator response to initial condition error
% Do this by constructing a system without output = estimated states
est = ss(Ax - L1*Cx, Bx, eye(size(Ax)), 0);
[Y, T] = initial(est, [0.1; 1/180*pi; 0.01; 0], 2);

figure(2); clf; subplot(221);
  plot(T, Y(:,1), 'b-', 'linewidth', AM_data_linewidth); hold on;
  plot(T, Y(:,2), 'b--', 'linewidth', AM_data_linewidth); hold on;
  plot(T, Y(:,3), 'b-.', 'linewidth', AM_data_linewidth); hold on;
  plot(T, Y(:,4), 'b:', 'linewidth', AM_data_linewidth); hold on;

  amaxis([0 2 -4 3]); 
  xlabel('states'); ylabel('time');

  lgh = legend('x', 'th', 'xd', 'td', 'Location', 'southeast'); 
  legend(lgh, 'boxoff');

  amprint('pvtol-eigplace.eps');

%%
%% Kalman filter
%% 
%%

% Construct a Kalman filter to do the estimation
Fk = eye(4);		% previous: [0 0; 0 0; 1 0; 0 1];

% Kalman filter with only position data
Ck1 = [1 0 0 0];
Lk1 = lqe(Ax, Fk, Ck1, 0.1*eye(size(Fk)), ...
	0.0001*eye(size(Ck1*Ck1')), 0*eye(size(Fk'*Ck1')))

% Estimator  response from the Kalman filter using position output
kf = ss(Ax - Lk1*Ck1, Bx, eye(size(Ax)), 0);
[Y, T] = initial(kf, [0.1; 1/180*pi; 0.01; 0], 4);

figure(3); clf; subplot(221); hold on;
  plot(T, Y(:,1), 'b-', 'linewidth', AM_data_linewidth); hold on;
  plot(T, Y(:,2), 'b--', 'linewidth', AM_data_linewidth); hold on;
  plot(T, Y(:,3), '-.', 'linewidth', AM_data_linewidth, 'Color', [0 0.5 0]);
  plot(T, Y(:,4), ':', 'linewidth', AM_data_linewidth, 'Color', [0 0.5 0]);
  
  amaxis([0 2 -0.4 0.1]); box on;
  xlabel('time'); ylabel('states'); 

  lgh = legend('x', 'th', 'xd', 'td', 'location', 'southeast') ; 
  legend(lgh, 'boxoff');
  
  amprint('pvtol-kalman1.eps');

% Kalman filter with position and orientation
Ck2 = [1 0 0 0; 0 1 0 0];
Lk2 = lqe(Ax, Fk, Ck2, 0.1*eye(size(Fk)), ...
	0.0001*eye(size(Ck2*Ck2')), 0*eye(size(Fk'*Ck2')))

% Estimator  response from the Kalman filter using full outputs
kf = ss(Ax - Lk2*Ck2, Bx, eye(size(Ax)), 0);
[Y, T] = initial(kf, [0.1; 1/180*pi; 0.01; 0], 4);

figure(4); clf; subplot(221); hold on;
  plot(T, Y(:,1), 'b-', 'linewidth', AM_data_linewidth); hold on;
  plot(T, Y(:,2), 'b--', 'linewidth', AM_data_linewidth); hold on;
  plot(T, Y(:,3), '-.', 'linewidth', AM_data_linewidth, 'Color', [0 0.5 0]);
  plot(T, Y(:,4), ':', 'linewidth', AM_data_linewidth, 'Color', [0 0.5 0]);
  
  amaxis([0 2 -0.4 0.1]); box on;
  xlabel('time'); ylabel('states'); 

  lgh = legend('x', 'th', 'xd', 'td', 'location', 'southeast') ; 
  legend(lgh, 'boxoff');

  amprint('pvtol-kalman2.eps');

%%
%% Closed loop plant
%%
%% Here we use the reg command to construct the regulator from the
%% state space estimator and controller.  We invert out the minus sign
%% that reg assumes so that we can use the negative feedback in the 
%% frequency domain (usual sign convention)
%%

pvctr1 = -tf(reg(pvsys1, K, L1));
pvctrk = -tf(reg(pvsys2, K, Lk2));

% Plot out the step response to a step change in desired output
% Note: we can do much better than this (and will)
figure(7); clf
  [Y, T] = step(feedback(pvsys2*pvctrk, eye(2)), 8);
  
  subplot(221); plot(T, Y(:,1,1), 'LineWidth', AM_data_linewidth);
  amaxis([0 8 0 1.5]); 
  xlabel('time'); ylabel('position');
  
  subplot(222); plot(T, Y(:,2,1), 'LineWidth', AM_data_linewidth);
  amaxis([0 8 -0.5 1]); 
  xlabel('time'); ylabel('angle');

amprint('pvtol-step.eps');
