Mercurial > hg > octave-terminal
changeset 3224:1e7532b9173b
[project @ 1998-12-04 22:08:23 by jwe]
author | jwe |
---|---|
date | Fri, 04 Dec 1998 22:08:23 +0000 |
parents | 3ee04ff37b3e |
children | 7aae2c3636a7 |
files | scripts/quaternion/demoquat.m scripts/quaternion/qconj.m scripts/quaternion/qcoordinate_plot.m scripts/quaternion/qderiv.m scripts/quaternion/qderivmat.m scripts/quaternion/qinv.m scripts/quaternion/qmult.m scripts/quaternion/qtrans.m scripts/quaternion/qtransv.m scripts/quaternion/qtransvmat.m scripts/quaternion/quaternion.m |
diffstat | 11 files changed, 533 insertions(+), 0 deletions(-) [+] |
line wrap: on
line diff
new file mode 100644 --- /dev/null +++ b/scripts/quaternion/demoquat.m @@ -0,0 +1,233 @@ + +# Thanks to Mr. Charles Hall, Dr. Don Krupp and Dr. Larry Mullins at +# NASA's Marshall Space Flight Center for notes and instruction in +# use and conventions with quaternions. - A. S. Hodel + +function opt = demoquat() + +opt = 0; +quitopt = 5; +while(opt != quitopt) + opt = menu("Quaternion function demo (c) 1998 A. S. Hodel, a.s.hodel@eng.auburn.edu", + "quaternion construction/data extraction", + "simple quaternion functions", + "transformation functions", + "body-inertial frame demo", + "Quit"); + + switch(opt) + case(1), + printf("Quaternion construction/data extraction\n"); + help quaternion + prompt + cmd = "q = quaternion(1,2,3,4)"; + run_cmd + disp("This format stores the i,j,k parts of the quaternion first;") + disp("the real part is stored last.") + prompt + disp(" ") + disp("i, j, and k are all square roots of -1; however they do not") + disp("commute under multiplication (discussed further with the function") + disp("qmult). Therefore quaternions do not commute under multiplcation:") + disp(" q1*q2 != q2*q1 (usually)") + prompt + + disp("Quaternions as rotations: unit quaternion to represent") + disp("rotation of 45 degrees about the vector [1 1 1]") + cmd = "degrees = pi/180; q1 = quaternion([1 1 1],45*degrees)"; + run_cmd + prompt + cmd = "real_q = cos(45*degrees/2)"; + run_cmd + printf("The real part of the quaternion q(4) is cos(theta/2).\n----\n\n"); + cmd = "imag_q = sin(45*degrees/2)*[1 1 1]/norm([1 1 1])" + run_cmd + disp("The imaginary part of the quaternion is sin(theta/2)*unit vector"); + disp("The constructed quaternion is a unit quaternion."); + prompt + disp("Can also extract both forms of the quaternion:") + disp("Vector/angle form of 1i + 2j + 3k + 4:") + cmd = "[vv,th] = quaternion(q)"; + run_cmd + cmd = "vv_norm = norm(vv)"; + run_cmd + disp("Returns the eigenaxis as a 3-d unit vector"); + disp("Check values: ") + cmd = "th_deg = th*180/pi"; + run_cmd + disp("") + disp("This concludes the quaternion construction/extraction demo."); + prompt + + case(2), + printf("Simple quaternion functions\n"); + cmd = "help qconj"; + run_cmd + cmd = "degrees = pi/180; q1 = quaternion([1 1 1],45*degrees)"; + run_cmd + cmd = "q2 = qconj(q1)"; + run_cmd + disp("The conjugate changes the sign of the complex part of the") + printf("quaternion.\n\n"); + prompt + printf("\n\n\nMultiplication of quaternions:\n"); + cmd = "help qmult"; + run_cmd + cmd = "help qinv" + run_cmd + disp("Inverse quaternion: q*qi = qi*q = 1:") + cmd = "q1i = qinv(q1)"; + run_cmd + cmd = "one = qmult(q1,q1i)"; + run_cmd + + printf("Conclusion of simple quaternion functions"); + prompt + case(3), + printf("Transformation functions\n"); + disp("A problem with the discussion of coordinate transformations is that"); + disp("one must be clear on what is being transformed: does a rotation of"); + disp("theta degrees mean that you're rotating the VECTOR by theta degrees,"); + disp("also called the 'active convention,' or does it mean that you rotate "); + disp("the COORDINATE FRAME by theta degrees, also called the 'passive convention,' "); + disp("which is equivalent to rotating the VECTOR by (-theta) degrees. The"); + disp("functions in this demo use the active convention. I'll point out where"); + disp("this changes the code as the demo runs."); + disp(" -- The author"); + prompt + printf("\n\n"); + disp("Sequences of rotations:") + printf("\n\nRotation of a vector by 90 degrees about the reference z axis\n"); + cmd = "qz = quaternion([0 0 1], pi/2);"; + disp(cmd) ; eval(cmd); + printf("\n\nRotation of a vector by 90 degrees about the reference y axis\n"); + cmd="qy = quaternion([0 1 0], pi/2);"; + disp(cmd) ; eval(cmd); + printf("\n\nRotation of a vector by 90 degrees about the reference x axis\n"); + cmd="qx = quaternion([1 0 0], pi/2);"; + run_cmd + printf("\n\nSequence of three rotations: 90 degrees about x, then 90 degrees\n"); + disp("about y, then 90 degrees about z (all axes specified in the reference frame):"); + qchk = qmult(qz,qmult(qy,qx)); + cmd = "[vv,th] = quaternion(qchk), th_deg = th*180/pi"; + run_cmd + disp("The sequence of the three rotations above is equivalent to a single rotation") + disp("of 90 degrees about the y axis. Check:"); + cmd = "err = norm(qchk - qy)"; + run_cmd + + disp("Transformation of a quaternion by a quaternion:") + disp("The three quaternions above were rotations specified about") + disp("a single reference frame. It is often convenient to specify the"); + disp("eigenaxis of a rotation in a different frame (e.g., when computing"); + disp("the transformation rotation in terms of the Euler angles yaw-pitch-roll)."); + cmd = "help qtrans"; + run_cmd + disp("") + disp("NOTE: If the passive convention is used, then the above"); + disp("formula changes to v = qinv(q)*v*q instead of ") + disp("v = q*v*qinv(q).") + prompt + disp("") + disp("Example: Vectors in Frame 2 are obtained by rotating them from ") + disp(" from Frame 1 by 90 degrees about the x axis (quaternion qx)") + disp(" A quaternion in Frame 2 rotates a vector by 90 degrees about") + disp(" the Frame 2 y axis (quaternion qy). The equivalent rotation") + disp(" in the reference frame is:") + cmd = "q_eq = qtrans(qy,qx); [vv,th] = quaternion(q_eq)"; + run_cmd + disp("The rotation is equivalent to rotating about the reference z axis") + disp("by 90 degrees (quaternion qz)") + prompt + + disp("Transformation of a vector by a quaternion"); + cmd = "help qtransv"; + run_cmd + + disp("NOTE: the above formula changes if the passive quaternion ") + disp("is used; the cross product term is subtracted instead of added."); + prompt + disp("Example: rotate the vector [1,1,1] by 90 degrees about the y axis"); + cmd = "vec_r = qtransv([1,1,1],qy)"; + run_cmd + prompt + disp("Equivalently, one may multiply by qtransvmat:") + cmd = "help qtransvmat"; + run_cmd + disp("NOTE: the passive quaternion convention would use the transpose") + disp("(inverse) of the orthogonal matrix returned by qtransvmat."); + prompt + cmd = "vec_r_2 = qtransvmat(qy)*[1;1;1]; vec_err = norm(vec_r - vec_r_2)"; + run_cmd + + disp("") + disp("The last transformation function is the derivative of a quaternion") + disp("Given rotation rates about the reference x, y, and z axes."); + cmd = "help qderivmat"; + run_cmd + disp("") + disp("Example:") + disp("Frame is rotating about the z axis at 1 rad/s") + cmd = "Omega = [0,0,1]; Dmat = qderivmat(Omega)"; + run_cmd + disp("Notice that Dmat is skew symmetric, as it should be.") + disp("expm(Dmat*t) is orthogonal, so that unit quaternions remain") + disp("unit quaternions as the rotating frame precesses."); + disp(" ") + disp("This concludes the transformation demo."); + prompt; + case(4), + printf("Body-inertial frame demo: Look at the source code for\n"); + printf("demoquat.m and qcoordinate_plot.m to see how it's done.\n"); + + # i,j,k units + iv = quaternion(1,0,0,0); jv = quaternion(0,1,0,0); + kv = quaternion(0,0,1,0); + + # construct quaternion to desired view. + degrees = pi/180; daz = 45*degrees; del = -30*degrees; + qazimuth = quaternion([0,0,1],daz); + qelevation = quaternion([cos(daz),sin(daz),0],del); + qview = qmult(qelevation,qazimuth); + + # inertial frame i, j, k axes. + iif = iv; jf = qtrans(jv,iv); kf = qtrans(kv,iv); + + # rotation steps + th = 0:5:20; ov = ones(size(th)); myth = [th,max(th)*ov ; 0*ov,th]; + + # construct yaw-pitch-roll cartoon + for kk=1:length(myth(1,:)) + figure(kk) + thy = myth(1,kk); + thp = myth(2,kk); + + qyaw = quaternion([0,0,1],thy*pi/180); + [jvy,th] = quaternion(qtrans(jf,qyaw)); + qpitch = quaternion(jvy(1:3),thp*pi/180); + qb = qmult(qpitch, qyaw); + qi = quaternion([1 0 0],180*degrees); + + printf("yaw=%8.4f, pitch=%8.4f, \n qbi = (%8.4f)i + (%8.4e)j + (%8.4f)k + (%8.4f)\n",thy,thp, ... + qb(1), qb(2), qb(3), qb(4)); + [vv,th] = quaternion(qb); + printf(" = (vector) = [%8.4f %8.4f %8.4f], th=%5.2f deg\n", ... + vv(1), vv(2), vv(3), th*180/pi); + + qb = qmult(qb,qi); + title(sprintf("yaw=%5.2f deg, pitch=%5.2f deg",thy,thp)) + qcoordinate_plot(qi,qb,qview); + # uncomment the next four lines to get eps output + #gset terminal postscript eps + #eval(sprintf("gset output 'fig%d.eps'",kk)); + #replot + #gset terminal x11 + #prompt + endfor + case(quitopt), + printf("Exiting quaternion demo\n"); + otherwise, + error(sprintf("Illegal option %f",opt)); + endswitch +endwhile +endfunction
new file mode 100644 --- /dev/null +++ b/scripts/quaternion/qconj.m @@ -0,0 +1,11 @@ + +function retval = qconj(q) + # function retval = qconj(q) + # conjugate of a quaternion + # q = [w,x,y,z] = w*i + x*j + y*k + z + # qconj(q) = -w*i -x*j -y*k + z + + [a,b,c,d] = quaternion(q); + retval = quaternion(-a,-b,-c,d); +endfunction +
new file mode 100644 --- /dev/null +++ b/scripts/quaternion/qcoordinate_plot.m @@ -0,0 +1,64 @@ +function qcoordinate_plot(qf,qb,qv) +# function qcoordinate_plot(qf,qb,qv) +# plot in the current figure a set of coordinate axes as viewed from +# the orientation specified by quaternion qv. Inertial axes are +# also plotted +# qf: quaternion from reference (x,y,z) to inertial +# qb: quaternion from reference to body +# qv: quaternion from reference to view angle + +degrees = pi/180; +d180 = 180*degrees; + +# construct coordinate transformation to view frame +cm = qtransvmat(qv); +p1 = [-1,-1,1]; p2 = [-1,-1,-1]; p3 = [1,-1,-1]; p4 = [ 1,-1, 1]; +p5 = [-1, 1,1]; p6 = [ 1, 1, 1]; p7 = [1, 1,-1]; p8 = [-1, 1,-1]; +# outline positive quadrant +box1 = cm*[p4; p6; p5; p6; p7]'; +# outline rest of the box +box2 =cm*[p7; p8; p5; p1; p4; p3; p7; p3; p2; p1; p2; p8]'; + +# compute inertial to body rotation eigenaxis +# qb = qbf*qf => qbf = qb/qf +# +# need to use inverse quaternion to rotate axes +qbf = qinv(qmult(qb,qinv(qf))); + +[eaxv,th_eig] = quaternion(qbf); + +# draw 1/3 circle in x-y plane around a unit z axis +th = (0:-12:-120)*degrees*sign(th_eig); lth = length(th); +cpts = [0 0 0.1*cos(th) ; 0 0 0.1*sin(th); 0 1 1*ones(1,lth)]; + +# rotate the 1/3 circle around eigenaxis of inertial to body rotation +# qez = qe/qz = rotation to get from z axis to eigenaxis. +# This rotates the 1/3 circle from x-y plane to the plane normal to +# eigenaxis +qez = qmult(qbf,qinv(quaternion(0,0,1,0))); +eig_xm = qtransvmat(qez); +cpts = cm*eig_xm*cpts; + +# transform inertial and body quaternions to view coordinates (rotate +# by azimuth, elevation) +qfm = qtransvmat(qf); qbm = qtransvmat(qf); +qf = qmult(qv,qf); qb = qmult(qv,qb); + +# get coordinate axes in inertial and reference frame +jnk = qtransvmat(qf); ifv = jnk(:,1); jfv = jnk(:,2); kfv = jnk(:,3); +jnk = qtransvmat(qb); ibv = jnk(:,1); jbv = jnk(:,2); kbv = jnk(:,3); + +gset size square +axis([-2,2,-2,2]); +[vv,theta] = quaternion(qb); +xlabel(sprintf("rotate about eigenaxis %5.2f deg",th_eig/degrees)); +plot( [ibv(1),0],[ibv(3),0],"-@11;x (body);", ... + [0,jbv(1)],[0,jbv(3)],"-@21;y (body);", ... + [0,kbv(1)],[0,kbv(3)],"-@32;z (body);", ... + [ifv(1),0],[ifv(3),0],"-@13;x (inertial);", ... + [0,jfv(1)],[0,jfv(3)],"-@23;y (inertial);", ... + [0,kfv(1)],[0,kfv(3)],"-@34;z (inertial);", ... + cpts(1,:), cpts(3,:),".-6 ;eigenaxis;", ... + box2(1,:),box2(3,:),"-4;;", ... + box1(1,:),box1(3,:),"-5;;"); +endfunction
new file mode 100644 --- /dev/null +++ b/scripts/quaternion/qderiv.m @@ -0,0 +1,23 @@ +function Dmat = qderivmat(Omega) +# function Dmat = qderivmat(Omega) +# Derivative of a quaternion. +# Let Q be a quaternion to transform a vector from a fixed frame to +# a rotating frame. If the rotating frame is rotating about the +# [x,y,z] axes at angular rates [wx, wy, wz], then the derivative +# of Q is given by +# Q' = qderivmat(Omega)*Q +# +# If the passive convention is used (rotate the frame, not the vector), +# then Q' = -qderivmat(Omega)*Q; see the accompanying document +# quaternion.ps for details. + +Omega = vec(Omega); +if(length(Omega) != 3) + error("qderivmat: Omega must be a length 3 vector."); +endif + +Dmat = 0.5*[ 0.0, Omega(3), -Omega(2), Omega(1); ... + -Omega(3), 0.0, Omega(1), Omega(2); ... + Omega(2), -Omega(1), 0.0, Omega(3); ... + -Omega(1), -Omega(2), -Omega(3), 0.0 ]; +endfunction
new file mode 100644 --- /dev/null +++ b/scripts/quaternion/qderivmat.m @@ -0,0 +1,23 @@ +function Dmat = qderivmat(Omega) +# function Dmat = qderivmat(Omega) +# Derivative of a quaternion. +# Let Q be a quaternion to transform a vector from a fixed frame to +# a rotating frame. If the rotating frame is rotating about the +# [x,y,z] axes at angular rates [wx, wy, wz], then the derivative +# of Q is given by +# Q' = qderivmat(Omega)*Q +# +# If the passive convention is used (rotate the frame, not the vector), +# then Q' = -qderivmat(Omega)*Q; see the accompanying document +# quaternion.ps for details. + +Omega = vec(Omega); +if(length(Omega) != 3) + error("qderivmat: Omega must be a length 3 vector."); +endif + +Dmat = 0.5*[ 0.0, Omega(3), -Omega(2), Omega(1); ... + -Omega(3), 0.0, Omega(1), Omega(2); ... + Omega(2), -Omega(1), 0.0, Omega(3); ... + -Omega(1), -Omega(2), -Omega(3), 0.0 ]; +endfunction
new file mode 100644 --- /dev/null +++ b/scripts/quaternion/qinv.m @@ -0,0 +1,12 @@ +function retval = qinv(q) +# function b = qinv(q) +# return the inverse of a quaternion +# q = [w,x,y,z] = w*i + x*j + y*k + z +# qmult(q,qinv(q)) = 1 = [0 0 0 1] + +if(norm(q) != 0) + retval = qconj(q) /sum(q .* q); +else + error("qinv: zero quaternion passed!"); +end +endfunction
new file mode 100644 --- /dev/null +++ b/scripts/quaternion/qmult.m @@ -0,0 +1,21 @@ +function retval = qmult(a,b) + # function retval = qmult(a,b) + # multiply two quaternions + # [w,x,y,z] = w*i + x*j + y*k + z + # identities: + # i^2 = j^2 = k^2 = -1 + # ij = k jk = i + # ki = j kj = -i + # ji = -k ik = -j + + [a1,b1,c1,d1] = quaternion(a); + [a2,b2,c2,d2] = quaternion(b); + + ri = b1*c2 - c1*b2 + d1*a2 + a1*d2; + rj = c1*a2 - a1*c2 + d1*b2 + b1*d2; + rk = a1*b2 - b1*a2 + d1*c2 + c1*d2; + rr = -(a1*a2 + b1*b2 + c1*c2) + d1*d2; + + retval = quaternion(ri,rj,rk,rr); +endfunction +
new file mode 100644 --- /dev/null +++ b/scripts/quaternion/qtrans.m @@ -0,0 +1,13 @@ +function v = qtrans(v,q) +# function v = qtrans(v,q) +# transform the unit quaternion v by the unit quaternion q; +# v = [w x y z], q = transformation quaternion +# returns v = q*v/q + +if(!is_vector(v) | length(v) != 4) + error(sprintf("qtrans: v(%d,%d) must be a quaternion",rows(v),columns(v))) +elseif(!is_vector(q) | length(q) != 4) + error(sprintf("qtrans: q(%d,%d) must be a quaternion",rows(q),columns(q))) +endif + +v = qmult(q,qmult(v,qinv(q)));
new file mode 100644 --- /dev/null +++ b/scripts/quaternion/qtransv.m @@ -0,0 +1,23 @@ +function vi = qtransv(vb,qib) +# function vi = qtransv(vb,q) +# transform the 3-D vector v by the unit quaternion q; +# v = [w x y z], q = transformation quaternion +# returns vi = column vector +# vi = (2*real(q)^2 - 1)*vb + 2*imag(q)*(imag(q)'*vb) +# + 2*real(q)*cross(imag(q),vb) +# where imag(q) is a column vector of length 3. + +if(!is_vector(vb) | length(vb) != 3) + error(sprintf("qtransv: v(%d,%d) must be a 3-D vector",rows(vb),columns(vb))) +elseif(!is_vector(qib) | length(qib) != 4) + error(sprintf("qtransv: q(%d,%d) must be a quaternion",rows(qib),columns(qib))) +elseif(max(abs(imag(vb))) + max(abs(imag(qib))) != 0) + vb + qib + error("qtransv: input values must be real."); +endif + +qr = qib(4); qimag = vec(qib(1:3)); vb = vec(vb); +vi = (2*qr^2 - 1)*vb + 2*qimag*(qimag'*vb) + 2*qr*cross(qimag,vb); + +endfunction
new file mode 100644 --- /dev/null +++ b/scripts/quaternion/qtransvmat.m @@ -0,0 +1,24 @@ +function Aib = qtransvmat(qib) +# function Aib = qtransvmat(qib) +# construct 3x3 transformation matrix from quaternion qib +# Aib is equivalent to rotation of th radians about axis vv, where +# [vv,th] = quaternion(qib) + +if(!is_vector(qib) | length(qib) != 4) + error(sprintf("qtransvmat: q(%d,%d) must be a quaternion",rows(qib),columns(qib))) +elseif(max(abs(imag(qib))) != 0) + qib + error("qtransvmat: input values must be real."); +endif + +Aib = [ (2.*(qib(1)^2 + qib(4)^2) -1.), ... + (2.*(qib(1)*qib(2)-qib(3)*qib(4))), ... + (2.*(qib(1)*qib(3)+qib(2)*qib(4))); ... + (2.*(qib(1)*qib(2)+qib(3)*qib(4))), ... + (2.*(qib(2)*qib(2)+qib(4)*qib(4))-1.), ... + (2.*(qib(2)*qib(3)-qib(1)*qib(4))); ... + (2.*(qib(1)*qib(3)-qib(2)*qib(4))), ... + (2.*(qib(2)*qib(3)+qib(1)*qib(4))), ... + (2.*(qib(3)*qib(3)+qib(4)*qib(4))-1.)]; +endfunction +
new file mode 100644 --- /dev/null +++ b/scripts/quaternion/quaternion.m @@ -0,0 +1,86 @@ +function [a,b,c,d] = quaternion(w,x,y,z) +# quaternion: construct or extract a quaternion +# w = a*i + b*j + c*k + d from given data. +# +# calling formats: +# [a,b,c,d] = quaternion(w) -or- +# [vv,theta] = quaternion(w) +# w = quaternion(a,b,c,d) +# w = quaternion(vv,theta) + +switch(nargin) +case(1), # extract information + if(!(is_vector(w) & length(w) == 4) ) + error("input vector must be of length 4)") + endif + # extract data + switch(nargout) + case(4), + a = w(1); + b = w(2); + c = w(3); + d = w(4); + case(2), + if(abs(norm(w)-1) > 1e-12) + warning(sprintf("quaternion: ||w||=%e, setting=1 for vv, theta",norm(w))) + w = w/norm(w); + endif + [a,b,c,d] = quaternion(w); + theta = acos(d)*2; + if(abs(theta) > pi) + theta = theta - sign(theta)*pi; + endif + sin_th_2 = norm([a b c]); + + if(sin_th_2 != 0) + vv = [a,b,c]/sin_th_2; + else + vv = [a b c]; + endif + a = vv; + b = theta; + otherwise, + usage("[a,b,c,d] = quaternion(w) or [vv,theta] = quaternion(w)"); + endswitch + +case(2), + if(nargout != 1) + usage("w = quaterion(vv,theta)"); + endif + vv = w; + theta = x; + + if(!is_vector(vv) | length(vv) != 3) + error("vv must be a length three vector") + elseif(!is_scalar(theta)) + error("theta must be a scalar"); + elseif(norm(vv) == 0) + error("quaternion: vv is zero.") + elseif(abs(norm(vv)-1) > 1e-12) + warning("quaternion: ||vv|| != 1, normalizing") + vv = vv/norm(vv); + endif + + if(abs(theta) > 2*pi) + warning("quaternion: |theta| > 2 pi, normalizing") + theta = rem(theta,2*pi); + endif + vv = vv*sin(theta/2); + d = cos(theta/2); + a = quaternion(vv(1), vv(2), vv(3), d); + +case(4), + if(nargout != 1) + usage("w = quaterion(a,b,c,d)"); + endif + if ( !(is_scalar(w) & is_scalar(x) & is_scalar(y) & is_scalar(z)) ) + error("input values must be scalars.") + endif + a = [w x y z]; + +otherwise, + error("usage: [a,b,c,d] = quaternion(w), a = quaternion(w,x,y,z)") + +endswitch + +endfunction