Cody

Problem 44895. Determine roll pitch yaw angles from a 3D rotation matrix

Solution 1799851

Submitted on 29 Apr 2019 by William
• Size: 59
• This is the leading solution.
This solution is locked. To view this solution, you need to provide a solution of the same size or smaller.

Test Suite

Test Status Code Input and Output
1   Pass
% compute random SO(3) for count = 1:5 rpy = (rand(1,3)-0.5)*pi; r = rpy(1); p = rpy(2); y = rpy(3); rpy2r = @(r,p,y) reshape([cos(p).*cos(y),cos(p).*sin(y),-sin(p),-cos(r).*sin(y)+cos(y).*sin(p).*sin(r),cos(r).*cos(y)+sin(p).*sin(r).*sin(y),cos(p).*sin(r),sin(r).*sin(y)+cos(r).*cos(y).*sin(p),-cos(y).*sin(r)+cos(r).*sin(p).*sin(y),cos(p).*cos(r)],[3,3]); R_correct = rpy2r(r, p, y); % user's estimate rpy = your_fcn_name(R_correct); r = rpy(1); p = rpy(2); y = rpy(3); err = R_correct' * rpy2r(r,p,y) - eye(3,3); assert(norm(err) < 10*eps) end

2   Pass
rpy = (rand(1,3)-0.5)*pi; r = rpy(1); p = pi/2; y = rpy(3); rpy2r = @(r,p,y) reshape([cos(p).*cos(y),cos(p).*sin(y),-sin(p),-cos(r).*sin(y)+cos(y).*sin(p).*sin(r),cos(r).*cos(y)+sin(p).*sin(r).*sin(y),cos(p).*sin(r),sin(r).*sin(y)+cos(r).*cos(y).*sin(p),-cos(y).*sin(r)+cos(r).*sin(p).*sin(y),cos(p).*cos(r)],[3,3]); R_correct = rpy2r(r, p, y); % user's estimate rpy = your_fcn_name(R_correct); r = rpy(1); p = rpy(2); y = rpy(3); err = R_correct' * rpy2r(r,p,y) - eye(3,3); assert(norm(err) < 10*eps)