%
% Differential equations for the system dac13.m
% (c) Marc Bodson, June 18, 2020
%
function xdot=dac13f(t,x)
global kp ap d1 om1 d3 gi111 gi112 gi311 gi312 g11 g12 g31 g32 b a
%
% Extract states
%
y=x(1);al1d=x(2);al3d=x(3);th11=x(4);th12=x(5);
al1=x(6);x12f=x(7);th31=x(8);th32=x(9);
th11nav=x(10);th12nav=x(11);dal1nav=x(12);
x12fnav=x(13);th31nav=x(14);th32nav=x(15);
%
% Algebraic equations
%
d=d1*cos(al1d)+d3*cos(al3d);
al3=3*al1+th32;
u=th11*cos(al1)+th31*cos(al3);
y11=y*cos(al1);y12=-y*sin(al1);
y31=y*cos(al3);y32=-y*sin(al3);
x11=gi111*y11+gi112*y12;x12=-gi112*y11+gi111*y12;
x31=gi311*y31+gi312*y32;x32=-gi312*y31+gi311*y32;
x11nav=(th11nav-d1*cos(dal1nav))/2;x12nav=d1*sin(dal1nav)/2;
x31nav=(th31nav-d3*cos(3*dal1nav+th32nav))/2;
x32nav=d3*sin(3*dal1nav+th32nav)/2;
%
% Differential equations
%
d1est=abs(d1);d3est=abs(d3);
xdot(1,1)=-ap*y+kp*(u-d);xdot(2,1)=om1;xdot(3,1)=3*om1;
xdot(4,1)=-g11*2*x11;xdot(5,1)=-g12*2*(x12-x12f)/d1est;
xdot(6,1)=th12;xdot(7,1)=-b*x12f+(b-a)*x12;xdot(8,1)=-g31*2*x31;
xdot(9,1)=-g32*2*x32/d3est;xdot(10,1)=-g11*2*x11nav;
xdot(11,1)=-g12*2*(x12nav-x12fnav)/d1est;xdot(12,1)= th12nav-om1;
xdot(13,1)=-b*x12fnav+(b-a)*x12nav;xdot(14,1)=-g31*2*x31nav;
xdot(15,1)=-g32*2*x32nav/d3est;