src/sensor/rotation_vector/design/lib/quat2euler.m

   1 function euler = quat2euler(q)
   2 % quat2euler
   3 %
   4 % Copyright (c) 2014 Samsung Electronics Co., Ltd.
   5 %
   6 % Licensed under the Apache License, Version 2.0 (the "License");
   7 % you may not use this file except in compliance with the License.
   8 % You may obtain a copy of the License at
   9 %
  10 % http://www.apache.org/licenses/LICENSE-2.0
  11 %
  12 % Unless required by applicable law or agreed to in writing, software
  13 % distributed under the License is distributed on an "AS IS" BASIS,
  14 % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  15 % See the License for the specific language governing permissions and
  16 % limitations under the License.
  17
  18 % Converts a quaternion orientation to ZYX Euler angles where phi is a
  19 % rotation around X, theta around Y and psi around Z.
  20
  21     R(1,1,:) = 2.*q(:,1).^2-1+2.*q(:,2).^2;
  22     R(2,1,:) = 2.*(q(:,2).*q(:,3)-q(:,1).*q(:,4));
  23     R(3,1,:) = 2.*(q(:,2).*q(:,4)+q(:,1).*q(:,3));
  24     R(3,2,:) = 2.*(q(:,3).*q(:,4)-q(:,1).*q(:,2));
  25     R(3,3,:) = 2.*q(:,1).^2-1+2.*q(:,4).^2;
  26
  27     phi = atan2(R(3,2,:), R(3,3,:) );
  28     theta = -atan(R(3,1,:) ./ sqrt(1-R(3,1,:).^2) );
  29     psi = atan2(R(2,1,:), R(1,1,:) );
  30
  31     euler = [phi(1,:)' theta(1,:)' psi(1,:)'];
  32 end