function N=invRPY(M)

phi=atan2(M(2,1),M(1,1));

theta=atan2(-M(3,1),sqrt(M(3,2)^2+M(3,3)^2));

psi=atan2(M(3,2),M(3,3));
if (theta<=(pi/2)) | (theta<=(pi/2))
N=[phi,theta,psi];
end

end



