%  Daniel Kawano, Rose-Hulman Institute of Technology
%  Last modified:  Dec 29, 2017

function ydot = iphone_orientation_ODEs(t, y, omega1, omega2, omega3, tdata)

%  Interpolate the angular velocity data to obtain the values needed to
%  evaluate the state equations at the current integration step:

omega1 = interp1(tdata, omega1, t);         %  rad/s
omega2 = interp1(tdata, omega2, t);         %  rad/s
omega3 = interp1(tdata, omega3, t);         %  rad/s

%  Pre-allocate space for the state equations:

ydot = zeros(3,1);

%  For convenience, define the state variables:

psi = y(1);         %  rad
theta = y(2);       %  rad
phi = y(3);         %  rad

%  Establish the state equations:

ydot = [0, sin(phi)/cos(theta), cos(phi)/cos(theta);
        0, cos(phi), -sin(phi);
        1, sin(phi)*tan(theta), cos(phi)*tan(theta)]*[omega1, omega2, omega3]';