%-------------------------------------------------------------------------
%                         DEMONSTRATION PROGRAM-3
%           FOR  MMLE.M  MAXIMUM LIKELIHOOD IDENTIFICATION FUNCTION
%             UNSTABLE HELICOPTER MODEL - NEEDS PROCESS NOISE ALGO
'-----------------------------------------------------------------------------';
format compact,clc
diary ml_demo3.log

disp(ans);
disp('                           MMLE  DEMONSTRATION  No 3')
disp('         CH47 helicopter longitudinal stability and control derivatives.')
disp('                 Two inputs, four outputs. Phugoid mode unstable.')
disp('                      True parameters= [ 1 1 1 1 1 10 10]')
disp('                State noise covariance = diag[0 100 100 0]/1e6')
disp('                   Measurement noise covariance = I * 10^-4')
disp(ans)
%------------------------------------------------------------ SIMULATE DATA

%     THE "RESULTS" MATRIX PRODUCED BY MMLE.M USES PREF IF AVAILABLE TO IT
pref=[1 1 1 1 1 10 10]; %      GET TRUE SYSTEM PARAMETERS AND MATRICES
[a,phi,gam,c,d,f,x0,dt]=ml_p2ss3(pref); %

ndp=200;t=[0:ndp-1]*dt;
uydata=zeros(ndp,6);%
uydata(15:24,1)=ones(10,1);%                   PITCH PULSE    .1 INCH UNITS
uydata(5:14,2)=ones(10,1);%               COLLECTIVE PULSE    .1 INCH UNITS

%  UYDATA(:,3:6)= VEL(FT/S), SINK(FT/S), PITCH_RATE(DEG/s), PITCH_ANGLE(DEG)

uydata(:,3:6)=dlsim(phi,gam,c,d,uydata(:,1:2)); % NOISE-FREE RESPONSE

%              NOW FILTER STATE NOISE SEPARATELY TO BYPASS gam MATRIX
randn('seed',0);
rootqd=[0 0;pref(6) 0;0 pref(7);0 0]/1000;
statenoise=dlsim(phi,rootqd,c,d,randn(ndp,2));%     STATE NOISE

%  OUTPUT IS NOISE-FREE RESPONSE + STATE NOISE + MEASUREMENT NOISE, i.e.,
[m,n]=size(statenoise);
uydata(:,3:6)=uydata(:,3:6) + statenoise + .01*randn(m,n);

%----------------------------------------- SUFFICIENT INPUTS TO MMLE FOLLOW
%uydata -- WAS DEFINED ABOVE
pidq=[1]; %     pidq,pidm,pidf MUST BE VALID EVEN IF NOT USED
pidm=[1:5]; %  (STABILITY AND CONTROL DERIVS)
pidf=[1:7]; %   ditto & INCLUDE STATE NOISE

p0=0.7*pref; %      SETUP THE P0 VECTOR WITH 30% ERROR

opt=[0 1 5 5 .02 .01 .001 1];

gg0=eye(4)*1e-4; %             INITIAL GUESS OF INNOVATIONS COV. = MEAS COV
p2snam='ml_p2ss3'; %                              FILENAME OF P2SS FUNCTION
pert=1e-4;
linesearch=1; %       USE LINESEARCH OPTION TO HELP PROC. NOISE CONVERGENCE
%--------------------------------------------------------------------------
%                     SIMULATE RESPONSE FOR p0,  AND PLOT THIS AND THE DATA

uydata0=uydata;[a,phi,gam,c,d]=ml_p2ss3(p0);
uydata0(:,3:6)=dlsim(phi,gam,c,d,uydata0(:,1:2)); %         RESPONSE FOR p0

plot(t,[uydata uydata0]),clear uydata0
%--------------------------------------------------------------------------
mmle
diary off
%----------------------------------------------------------- end ml_demo3.m

