function [m,I,x] = BiomchProb()
% Program to calculate parameter values for 3 DOF postural
% sway dynamic model

% Define kinematic and force/torque data
global g
g = 9.81;

t = (0:0.1:1)';
n = max(size(t));

A = 15.0*pi/180.0;
w = 2.0*pi;
q3 = A*cos(w*t);
u3 = -A*w*sin(w*t);
u3p = -A*w*w*cos(w*t);

F1 = [    611.476;
          512.738;
          207.096;
         -207.096;
         -512.738;
         -611.476;
         -512.738;
         -207.096;
          207.096;
          512.738;
          611.476];

F2 = [    850.545;
          738.384;
          553.093;
          553.093;
          738.384;
          850.545;
          738.384;
          553.093;
          553.093;
          738.384;
          850.545];

 T3 = [  -916.140;
         -741.672;
         -283.603;
          283.603;
          741.672;
          916.14;
          741.672;
          283.603;
         -283.603;
         -741.672;
         -916.14];

m = 70;
I = 20;
x = 0.875;

%zero = eqns(m,I,x,q3,u3,u3p,F1,F2,T3)
options = optimset('Display','Iter','MaxFunEvals',1000)
m0 = 0;
I0 = 0;
x0 = 0;
pold = [m0 I0 x0]';
lb = zeros(3,1);
ub = [];
p = lsqnonlin(@costfunc,pold,lb,ub,options,q3,u3,u3p,F1,F2,T3)

m = p(1);
I = p(2);
x = p(3);

%=======================================================================%
function zero = eqns(m,I,x,q3,u3,u3p,F1,F2,T3)
global g

u1p = 0;
u2p = 0;

zero(:,1) = F1 - m*(x*sin(q3).*u3.^2+u1p-x*cos(q3).*u3p);
zero(:,2) = F2 - m*(g+u2p-x*cos(q3).*u3.^2-x*sin(q3).*u3p);
zero(:,3) = T3 - I*u3p - m*x.*(x.*u3p-g*sin(q3)-sin(q3).*u2p-cos(q3).*u1p);

%=======================================================================%
function errors = costfunc(p,q3,u3,u3p,F1,F2,T3)
global g

m = p(1);
I = p(2);
x = p(3);
    
zero = eqns(m,I,x,q3,u3,u3p,F1,F2,T3);

errors = zero;