disp('------------------------------')
disp('quat2angle( ) test')
f = 1e-10;
q = [0.5 0.5 0.5 0.5] + (rand(1,4)*2-1)*f;
q = q / norm(q)
vi = [1 2 3]';
disp('------------------------------')
disp('dcm = quat2dcm(q)')
dcm = quat2dcm(q)
[a3,a2,a1] = quat2angle(q,'ZYX');
c = cos(a3);
s = sin(a3);
r3 = [ c s 0;
-s c 0;
0 0 1];
c = cos(a2);
s = sin(a2);
r2 = [ c 0 -s;
0 1 0;
s 0 c];
c = cos(a1);
s = sin(a1);
r1 = [ 1 0 0;
0 c s;
0 -s c];
disp('dce = result of individual rotation matrix multiplies, ZYX')
dce = r1 * r2 * r3
vb = dcm * vi;
ve = dce * vi;
vq = quatmultiply(quatconj(q),[0 vi']);
vq = quatmultiply(vq,q);
vr = quatrotate(q,vi');
disp('------------------------------')
disp('vi = input inertial vector')
disp(vi')
disp('vb = dcm * vi')
disp(vb')
disp('ve = dce * vi')
disp(ve')
disp('vq = conj(q) * vi * q')
disp(vq(2:4))
disp('vr = quatrotate(q,vi'')')
disp(vr)
disp('q')
disp(q);
disp('angle2quat(angles) for ZYX')
disp(angle2quat(a3,a2,a1,'ZYX'))
disp('------------------------------')
disp('------------------------------')
[a3,a2,a1] = quat2angle(q,'XYZ');
c = cos(a3);
s = sin(a3);
r3 = [ 1 0 0;
0 c s;
0 -s c];
c = cos(a2);
s = sin(a2);
r2 = [ c 0 -s;
0 1 0;
s 0 c];
c = cos(a1);
s = sin(a1);
r1 = [ c s 0;
-s c 0;
0 0 1];
disp('dce = result of individual rotation matrix multiplies, XYZ')
dce = r1 * r2 * r3
vb = dcm * vi;
ve = dce * vi;
vq = quatmultiply(quatconj(q),[0 vi']);
vq = quatmultiply(vq,q);
vr = quatrotate(q,vi');
disp('------------------------------')
disp('vi = input inertial vector')
disp(vi')
disp('vb = dcm * vi')
disp(vb')
disp('ve = dce * vi')
disp(ve')
disp('vq = conj(q) * vi * q')
disp(vq(2:4))
disp('vr = quatrotate(q,vi'')')
disp(vr)
disp('q')
disp(q);
disp('angle2quat(angles) for XYZ')
disp(angle2quat(a3,a2,a1,'XYZ'))
disp('------------------------------')
Best Answer