yaw roll pitch matrix

http://planning.cs.uiuc.edu/node102.html

http://planning.cs.uiuc.edu/node103.html