%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Test ssm/parameterDiff
% 
% Test with an harmonic hoscillator
% 
% L Ferraioli 11-10-2010
%
% $Id: test_ssm_parametersDiff.m,v 1.4 2010/12/02 17:55:01 adrien Exp $
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
close all
clc
clear
%% set parameters

m = 2; % kg
k = 0.1; % kg s^-2
damp = 0.05; % kg s^-1
fs = 1;
freq = logspace(-2.5,log10(fs/2-0.0001),300);

%% get the system ssm and smodel

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

smod = smodel('1 ./ (M.*((2.*i.*pi.*f).^2) + VBETA.*(2.*i.*pi.*f) + K)');
% smod = smodel('1 ./ (M.*((i.*f).^2) + VBETA.*(i.*f) + K)');
smod.setParams({'M', 'K', 'VBETA'},{m,k,damp});
smod.setXvar('f');
smod.setXvals(freq);
smod.setYunits('kg^-1 s^-2');


%% get derivatives

lsys = parameterDiff(sys,plist('names',{'M', 'K', 'VBETA'},'values',1e-8*[m,k,damp]));
lsys.modifyTimeStep(plist('newtimestep',1/fs));

dsmod_M = diff(smod,plist('var','M'));
dsmod_K = diff(smod,plist('var','K'));
dsmod_VBETA = diff(smod,plist('var','VBETA'));

%% compare frequency response
pl = plist('inputs',{'COMMAND.force'},...
  'outputs',{'HARMONIC_OSC_1D.position'},...
  'f',freq);
sys_none = bode(keepParameters(sys),pl);
smod_none = eval(smod);
iplot(sys_none,smod_none)

pl = plist('inputs',{'COMMAND.force'},...
  'outputs',{'HARMONIC_OSC_1D_DIFF_M.position'},...
  'f',freq);
sys_M = bode(lsys,pl);
smod_M = eval(dsmod_M);
iplot(sys_M,smod_M)


pl = plist('inputs',{'COMMAND.force'},...
  'outputs',{'HARMONIC_OSC_1D_DIFF_K.position'},...
  'f',freq);
sys_K = bode(lsys,pl);
smod_K = eval(dsmod_K);
iplot(sys_K,smod_K)

pl = plist('inputs',{'COMMAND.force'},...
  'outputs',{'HARMONIC_OSC_1D_DIFF_VBETA.position'},...
  'f',freq);
sys_VBETA = bode(lsys,pl);
smod_VBETA = eval(dsmod_VBETA);
iplot(sys_VBETA,smod_VBETA)