% Listing 22.2 for Example 22.2 of the book on
% "Principles of System Identification: Theory and Practice"

% Arun K. Tangirala
% December 06, 2015
% Freely distributed under no warranty

%% Data generating process
mod_dgp = idpoly(1,[0 0 2 -1],1,1,[1 -1.1 0.24],'Ts',1);
uk = idinput(2044,'prbs',[0 1/4],[-1 1]);
ykdet = sim(mod_dgp,uk);
mod_dgp.Noisevariance = var(ykdet)/10;
yk = sim(mod_dgp,uk,simOptions('AddNoise',true));

%% Generate outliers
% Range of values for outlier
max_outl = round(2*max(abs(yk))); min_outl = -round(2*max(abs(yk)));
N_outl = 30;
yk_outl = randi([min_outl max_outl],N_outl,1);

% Add outliers at random locations
outl_loc = randperm(length(yk),N_outl);
ykc = yk; ykc(outl_loc) = yk(outl_loc) + yk_outl;
ykc2 = ykc; ykc2(outl_loc) = NaN;

%% Estimate model without treatment
dataset = iddata(ykc,uk,1);
Ztrain = detrend(dataset,0);

% Assume model orders and delays are known
nb = 2; nf = 2; nk = 2;
mod_oe = oe(Ztrain,[nb nf nk]);

%% Estimate missing data and re-estimate model
datae = misdata(iddata(ykc2,uk,1));
Ztrain_clean = detrend(datae,0);

mod_oe2 = oe(Ztrain_clean,[nb nf nk]);

%% Estimate model using least trimmed squares
% Initial guess
mod_oe = oe(Ztrain,[nb nf nk]);
theta0 = [mod_oe.b(nk+1:end) mod_oe.f(2:end)]';

% Optimization using LTS
T = round(0.97*length(yk));
thetahat = fminsearch(@(theta) trimsq(theta,Ztrain,[nb nf nk]',T),theta0);
mod_oe3 = idpoly(1,[zeros(1,nk) thetahat(1:nb)'],1,1,[1 thetahat(nb+1:end)'],'Ts',1);