phidot = [1,sind(phi)*tand(theta),cosd(phi)*tand(theta)]*[p;q;r];
    thetadot = [0,cosd(phi),-sind(phi)]*[p;q;r];
    psidot = [0,sind(phi)*secd(theta),cosd(phi)*secd(theta)]*[p;q;r];