% idynopt.m : optimize inverse dynamics filtering

% By: Ton van den Bogert, University of Calgary
% Date: April 26, 1996
% Report problems to bogert@acs.ucalgary.ca

% Results will depend on the random numbers
% used to simulate noise.  The optimization procedure should
% be performed N (e.g. 10) times (without resetting the random number
% generator in between) in order to obtain mean and standard deviation
% for optimal filter parameters.

clear
global eval dt kin frc mom fmg n m I fCM FMhip FMknee FMankle Mhip Mknee Mankle

% the datafiles are sampled at 10000 samples/sec.
% to simulate typical video system:
interval = 50;		% interval=50 for 200 fps
dt = interval*0.0001;	% resulting sampling interval
noise = 0.0005;		% typical: 0.5 mm noise

% define the model parameters for segments thigh, shank, foot
m = [6.85 , 2.86, 1.00];
I = [0.14536126706365, 0.42996389275840E-01, 0.20000000000000E-01];
fCM = [0.43237250554324, 0.43349753694581, 0];

% there are 6001 samples, but the first part is not very interesting
first = 1000;
last = 6001;

% load data
disp('Loading data...');
load all.kin
kin = all(first:interval:last,:);
[n,ncol] = size(kin);
kin = kin + noise*randn(n,ncol);
load all.frc
frc = all(first:interval:last,:);
load all.mom
mom = -all(first:interval:last,:);
mom(:,2) = -mom(:,2);		% knee: extension should be positive
load all.fmg
fmg = all(first:interval:last,:);

% do analysis
eval = 0;
x0 = [15 15 15 15];		% initial guess of filter parameters
dx = [0.5 0.5 0.5 0.5];		% needed for Bremermann optimization
maxiter = 0;			% set to max. # of iterations
if (maxiter == 0)		% if zero, just do one inverse dynamics analysis
  F = idynfun(x0)
else
  optfcut = fmins('idynfun',x0)			% uses Simplex optimization
  %optfcut = bremer('idynfun',x0,dx,maxiter)	% uses Bremermann optimization
end

% show results
idynplt;