%	SURVEY --  Linear System Survey 
%	January 8, 2009  
%	===============================================================
%	Copyright 2009 by ROBERT F. STENGEL.  All rights reserved.

	clear
	'SURVEY'
    date

%	This is the SCRIPT FILE.  It contains the Main Program for analyzing
%	linear, time-invariant dynamic models, including the calculation of:
%		Longitudinal and lateral-directional models
%		Transient response to initial conditions and control inputs
%		Static response to controls and commands
%		Controllability and observability
%		Stability
%		Modes of motion
%		Transfer functions
%		Frequency response (Bode, Nyquist, and Nichols charts)
		
%	Functions used by SURVEY
%		LonLatDir.m	Reduced-order model derived from Fmodel and Gmodel
%		Trans.m	Transient response
%		Static.m	Static Response
%		ConObs.m	Controllability and Observability
%		NatFreq.m	Matural Frequencies
%		StabMode.m	Stability and Modes of Motion

%	Linear Modelof State Dynamics and Control
	load Fmodel
	load Gmodel

%	DEFINITION OF THE STATE VECTOR FOR Fmodel and Gmodel
%		x(1) = 		Body-axis x inertial velocity, ub, m/s
%		x(2) =		Body-axis y inertial velocity, vb, m/s
%		x(3) =		Body-axis z inertial velocity, wb, m/s
%		x(4) =		North position of center of mass WRT Earth, xe, m
%		x(5) =		East position of center of mass WRT Earth, ye, m
%		x(6) =		Negative of c.m. altitude WRT Earth, ze = -h, m
%		x(7) =		Body-axis roll rate, pr, rad/s
%		x(8) =		Body-axis pitch rate, qr, rad/s
%		x(9) =		Body-axis yaw rate, rr,rad/s
%		x(10) =		Roll angle of body WRT Earth, phir, rad
%		x(11) =		Pitch angle of body WRT Earth, thetar, rad
%		x(12) =		Yaw angle of body WRT Earth, psir, rad
	
%	DEFINITION OF THE CONTROL VECTOR FOR Gmodel
%		u(1) = 		Elevator, dEr, rad
%		u(2) = 		Aileron, dAr, rad
%		u(3) = 		Rudder, dRr, rad
%		u(4) = 		Throttle, dT, % / 100
%		u(5) =		Asymmetric Spoiler, dASr, rad
%		u(6) =		Flap, dFr, rad
%		u(7) =		Stabilator, dSr, rad

%	SURVEY Flags and Conditions to be chosen by the User

	LLD =		1;		% Original model (0) or reduced-order model (1)
	SBA =		1;		% Body axes (0) or stability/hybrid axes (1)
	LON =		1;		% Lateral-directional (0) or longitudinal (1) model
	DIM =		4;		% Reduced-order model dimension (4 or 6)
	IC =		1;		% Initial-condition response: OFF (0) or ON (1)
	SR=			1;		% Static response: OFF (0) or ON (1)
	CO =		0;		% Controllability and observability: OFF (0) or ON (1)
	EVAL=		1;		% Eigenvalues: OFF (0) or ON (1)
	EVEC =		0;		% Eigenvectors: OFF (0) or ON (1)
	TRF =		1;		% Transfer functions: OFF (0) or ON (1)
	BODE =		1;		% Bode plots: OFF (0) or ON (1)
	NYQUIST =	0;		% Nyquist charts: OFF (0) or ON (1)
	NICHOLS =	1;		% Nichols charts: OFF (0) or ON (1)
	
%	Trim Condition to be chosen by the user 
%   [Be sure that this matches your trim condition if using stability axes]
%   =======================================================================
	Vt =		150;	% True Air Speed, TAS, m/s	(relative to air mass)
	ht =		3048;	% Altitude above Sea Level, m
    
	alphat =	1;      % Angle of attack, deg
	alphar =	alphat * 0.01745329;
    
	betat =		0;		% Sideslip angle, deg	
	betar =		betat * 0.01745329;
	
%	Perturbation Initial Conditions for Time Response Calculation
%   to be chosen by the User
%   [Initial condition response, step response, or combination of the two]
%   ======================================================================

%   Body-axis Initial Conditions
	duo =		0;		% Axial velocity, m/s
	dvo =		0;		% Side velocity, m/s
	dwo =		0;		% Normal velocity, m/s
    
%   Stability-axis Initial Conditions
	dalphao =	1;		% Angle of attack, deg
	dbetao =	0;		% Sideslip angle, deg
	dVo = 		0;		% Forward velocity, m/s
    
%   Either Body-Axis or Stability-Axis Initial Conditions 
	dxo =		0;		% North position, m
	dyo =		0;		% East position, m
	dzo =		0;		% Vertical position, m
	dpo =		0;		% Roll rate, deg/s
	dqo =		0;		% Pitch rate, deg/s
	dro =		0;		% Yaw rate, deg/s
	dphio =		0;		% Roll angle, deg
	dthetao =	0;		% Pitch angle, deg
	dpsio =		0;		% Yaw angle, deg
	
	dEo =		0;		% Elevator angle, deg
	dAo =		0;		% Aileron angle, deg
	dRo =		0;		% Rudder angle, deg
	dTo =		0;      % Throttle, dT, % / 100
	dASo =		0;		% Asymmetric spoiler angle, deg
	dFo =		0;		% Flap angle, deg
	dSo =		0;		% Stabilator angle, deg
	
%   Degrees to radians conversion
	dalphar =	dalphao * 0.01745329;
	dbetar =	dbetao * 0.01745329;
	dpr =		dpo * 0.01745329;
	dqr =		dqo * 0.01745329;
	drr =		dro * 0.01745329;
	dphir =		dphio * 0.01745329;
	dthetar =	dthetao * 0.01745329;
	dpsir =		dpsio * 0.01745329;
	
	dEr =		dEo * 0.01745329;
	dAr =		dAo * 0.01745329;
	dRr =		dRo * 0.01745329;
	dASr =		dASo * 0.01745329;
	dFr =		dFo * 0.01745329;
	dSr =		dSo * 0.01745329;
	
%	Definitions to be chosen by the User

	delt =		0.1;		% Time increment, sec
	tf = 		30;			% Final time, sec
    
	Hstat =		[1 0 0 0	% Output matrix for Static Response
				0 1 0 0];
            
	Hobs =		[0 0 0 1];	% Output matrix for Observability
	Gcon =		[0;1;0;0];	% Selector matrix for Controllability
		
%	Reduced-Order Longitudinal and Lateral-Directional Models
%	=========================================================
	if LLD >= 1
		'Reduced-Order Model'
		[F,G]	=	LonLatDir(Fmodel,Gmodel,Vt,betar,alphar,SBA,LON,DIM)
	else
		'Original Twelfth-Order Model'
		F	=	Fmodel
		G	=	Gmodel
	end
	
%	State-Space System Definition to be chosen by the User
%	======================================================

%   As shown, the output matrix provides a scalar output, and the control
%   input provides a scalar input
	Hx  	=	[0 1 0 0];	% Scalar output matrix for State-Space Model
	Hu		=	0;			% Output matrix for State-Space Model
	Gsel	=	[1;0;0;0];	% Control selector matrix for 
							% Scalar Transfer Function
							% (4 elements for longitudinal control;
							%  3 elements for lateral-directional control)
	Sys	=	ss(F,G,Hx,Hu);
	
%	Transient Response
%	==================
	if IC >= 1
		'Transient Response'
		xoo	=	[duo,dvo,dwo,dVo,dbetar,dalphar,dxo,dyo,dzo,...
				dpr,dqr,drr,dphir,dthetar,dpsir];
		uo	=	[dEr,dAr,dRr,dTo,dASr,dFr,dSr];

		tr	=	Trans(Sys,xoo,uo,delt,tf,LLD,SBA,LON,DIM);
	end

%	Static Response to Controls and Commands
%	=======================================
	if SR >= 1
		'Static Response'
		st	=	Static(F,G,Hstat,LON);
	end

%	Controllability and Observability
%	=================================
	if CO >= 1
		'Controllability and Observability'
		co	=	ConObs(F,G,Hobs,Gcon);
	end

%	Natural Frequencies, Damping Ratios, and Real Roots
%	===================================================
	if EVAL >= 1
		'Natural Frequencies, Damping Ratios, and Real Roots'
		nf	=	NatFreq(F,Sys);
        '[Note: The m-function "damp" incorrectly identifies natural frequencies and damping ratios for real modes.]'
        '[Real modes do not have natural frequencies and damping ratios.]'
	end
	
%	Stability and Modes of Motion
%	=============================
	if EVEC >= 1
		'Stability and Modes of Motion'
		sm	=	StabMode(F,Vt,LLD,LON,SBA,DIM);
	end

%	Zero-Pole-Gain Transfer Functions for State-Space Model
%	=======================================================
	if TRF >= 1
		'Transfer Functions'
		G			=	G * Gsel;
		Sys			=	ss(F,G,Hx,Hu);
		ZPKsys		=	zpk(Sys)
		'DC Gain'
		DCampRatio	=	dcgain(ZPKsys)
	end

%	Bode Plots of Zero-Pole-Gain Transfer Functions
%	===============================================
	if BODE >= 1
		'Bode Plots'
		figure
		bode(ZPKsys)
		grid
	end

%	Nyquist Plots of Zero-Pole-Gain Transfer Functions
%	==================================================
	if NYQUIST >= 1
		'Nyquist Plots'
		if	DCampRatio ~= 0
			ZPKabs	=	sign(DCampRatio) * ZPKsys;	% Impose positivity
		else
			ZPKabs	=	ZPKsys;
		end
		figure
		nyquist(ZPKabs)
		grid
	end

%	Nichols Charts of Zero-Pole-Gain Transfer Functions
%	===================================================
	if NICHOLS >= 1
		'Nichols Charts'
		figure
		if	DCampRatio ~= 0
			ZPKabs	=	sign(DCampRatio) * ZPKsys	% Impose positivity
		else
			ZPKabs	=	ZPKsys;
		end
		nichols(ZPKabs)
		grid
	end