%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Test linfitsvd with ssm
% 
% Test with an harmonic hoscillator
% 
% L Ferraioli 11-10-2010
%
% $Id: test_matrix_linfitsvd_ssm.m,v 1.1 2010/11/22 16:57:54 luigi Exp $
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% set parameters

m = 2; % kg
k = 0.1; % kg s^-2
damp = 0.05; % kg s^-1

fAmp = 3;
NAmp = 1;
fs = 1;

cutbefore = 100*fs;
cutafter = 500*fs;

%% input force and readout noise
% prepare the inputs for 2 experiments

% input force
fc1 = ao(plist('waveform','sine wave','A',fAmp,'f',1e-1,'phi',0,'toff',cutbefore/fs,'nsecs',6e2,'fs',fs));
in1 = zeropad(fc1,plist('Position','post','N',cutafter));
% input readout noise
rn1 = ao(plist('tsfcn', 'randn(size(t))', 'fs', fs, 'nsecs', in.nsecs));
rn1 = rn1.*NAmp;

% input force
fc2 = ao(plist('waveform','sine wave','A',fAmp,'f',1e-2,'phi',0,'toff',cutbefore/fs,'nsecs',6e2,'fs',fs));
in2 = zeropad(fc2,plist('Position','post','N',cutafter));
% input readout noise
rn2 = ao(plist('tsfcn', 'randn(size(t))', 'fs', fs, 'nsecs', in.nsecs));
rn2 = rn2.*NAmp;

iplot(in1,in2)

%% Simulate the system

sys = ssm(plist('built-in', 'HARMONIC_OSC_1D', 'SYMBOLIC PARAMS',{'M', 'K', 'VBETA'}));
sys = sys.setParameters(plist('names',{'M', 'K', 'VBETA'},'values',[m k damp]));
sys.keepParameters();
sys.modifyTimeStep(plist('newtimestep',1/fs));

plsym = plist('AOS VARIABLE NAMES',{'COMMAND.Force','noise.readout'},...
  'RETURN OUTPUTS','HARMONIC_OSC_1D.position',...
  'AOS',[in1,rn1]);
out1 = simulate(sys,plsym);

plsym = plist('AOS VARIABLE NAMES',{'COMMAND.Force','noise.readout'},...
  'RETURN OUTPUTS','HARMONIC_OSC_1D.position',...
  'AOS',[in2,rn2]);
out2 = simulate(sys,plsym);

iplot(out1,out2)

%% set input data and run fit with ssm

mod = ssm(plist('built-in', 'HARMONIC_OSC_1D', 'SYMBOLIC PARAMS',{'M', 'K', 'VBETA'}));
mod = mod.setParameters(plist('names',{'M', 'K', 'VBETA'},'values',[1.8 0.2 0.1]));

FitParams = {'M', 'K', 'VBETA'};
InputNames = {{'COMMAND.Force'},{'COMMAND.Force'}};
OutputNames = {{'HARMONIC_OSC_1D.position'},{'HARMONIC_OSC_1D.position'}};
Input = collection(in1,in2);

o1 = matrix(out1);
o2 = matrix(out2);

plfit = plist(...
  'FitParams',FitParams,...
  'Model',mod,...
  'Input',Input,...
  'InputNames',InputNames,...
  'OutputNames',OutputNames,...
  'WhiteningFilter',[],...
  'tol',1,...
  'Nloops',5,... % number of fit iterations
  'Ncut',cutbefore); % number of data points to skip at the starting of the series to avoid whitening filter transient

pars = linfitsvd(o1,o2,plfit);

%% build smodel

symmod = smodel('1./(M.*((2.*i.*pi.*f).^2)+VBETA.*(2.*i.*pi.*f)+K)');
symmod.setParams({'M','VBETA','K'},{1.8,0.2,0.1});
symmod.setXvar('f');
symmod.setYunits('kg^-1 s^-2');
smod = matrix(symmod);

%% set input and fit

FitParams = {'M', 'K', 'VBETA'};
Input = collection(matrix(in1),matrix(in2));
o1 = matrix(out1);
o2 = matrix(out2);

plfit = plist(...
  'FitParams',FitParams,...
  'Model',smod,...
  'Input',Input,...
  'WhiteningFilter',[],...
  'tol',1,...
  'Nloops',5,... % number of fit iterations
  'Ncut',cutbefore); % number of data points to skip at the starting of the series to avoid whitening filter transient

pars = linfitsvd(o1,o2,plfit);