|
3224
|
1 |
|
|
2 # Thanks to Mr. Charles Hall, Dr. Don Krupp and Dr. Larry Mullins at |
|
|
3 # NASA's Marshall Space Flight Center for notes and instruction in |
|
|
4 # use and conventions with quaternions. - A. S. Hodel |
|
|
5 |
|
|
6 function opt = demoquat() |
|
|
7 |
|
|
8 opt = 0; |
|
|
9 quitopt = 5; |
|
|
10 while(opt != quitopt) |
|
|
11 opt = menu("Quaternion function demo (c) 1998 A. S. Hodel, a.s.hodel@eng.auburn.edu", |
|
|
12 "quaternion construction/data extraction", |
|
|
13 "simple quaternion functions", |
|
|
14 "transformation functions", |
|
|
15 "body-inertial frame demo", |
|
|
16 "Quit"); |
|
|
17 |
|
|
18 switch(opt) |
|
|
19 case(1), |
|
|
20 printf("Quaternion construction/data extraction\n"); |
|
|
21 help quaternion |
|
|
22 prompt |
|
|
23 cmd = "q = quaternion(1,2,3,4)"; |
|
|
24 run_cmd |
|
|
25 disp("This format stores the i,j,k parts of the quaternion first;") |
|
|
26 disp("the real part is stored last.") |
|
|
27 prompt |
|
|
28 disp(" ") |
|
|
29 disp("i, j, and k are all square roots of -1; however they do not") |
|
|
30 disp("commute under multiplication (discussed further with the function") |
|
|
31 disp("qmult). Therefore quaternions do not commute under multiplcation:") |
|
|
32 disp(" q1*q2 != q2*q1 (usually)") |
|
|
33 prompt |
|
|
34 |
|
|
35 disp("Quaternions as rotations: unit quaternion to represent") |
|
|
36 disp("rotation of 45 degrees about the vector [1 1 1]") |
|
|
37 cmd = "degrees = pi/180; q1 = quaternion([1 1 1],45*degrees)"; |
|
|
38 run_cmd |
|
|
39 prompt |
|
|
40 cmd = "real_q = cos(45*degrees/2)"; |
|
|
41 run_cmd |
|
|
42 printf("The real part of the quaternion q(4) is cos(theta/2).\n----\n\n"); |
|
|
43 cmd = "imag_q = sin(45*degrees/2)*[1 1 1]/norm([1 1 1])" |
|
|
44 run_cmd |
|
|
45 disp("The imaginary part of the quaternion is sin(theta/2)*unit vector"); |
|
|
46 disp("The constructed quaternion is a unit quaternion."); |
|
|
47 prompt |
|
|
48 disp("Can also extract both forms of the quaternion:") |
|
|
49 disp("Vector/angle form of 1i + 2j + 3k + 4:") |
|
|
50 cmd = "[vv,th] = quaternion(q)"; |
|
|
51 run_cmd |
|
|
52 cmd = "vv_norm = norm(vv)"; |
|
|
53 run_cmd |
|
|
54 disp("Returns the eigenaxis as a 3-d unit vector"); |
|
|
55 disp("Check values: ") |
|
|
56 cmd = "th_deg = th*180/pi"; |
|
|
57 run_cmd |
|
|
58 disp("") |
|
|
59 disp("This concludes the quaternion construction/extraction demo."); |
|
|
60 prompt |
|
|
61 |
|
|
62 case(2), |
|
|
63 printf("Simple quaternion functions\n"); |
|
|
64 cmd = "help qconj"; |
|
|
65 run_cmd |
|
|
66 cmd = "degrees = pi/180; q1 = quaternion([1 1 1],45*degrees)"; |
|
|
67 run_cmd |
|
|
68 cmd = "q2 = qconj(q1)"; |
|
|
69 run_cmd |
|
|
70 disp("The conjugate changes the sign of the complex part of the") |
|
|
71 printf("quaternion.\n\n"); |
|
|
72 prompt |
|
|
73 printf("\n\n\nMultiplication of quaternions:\n"); |
|
|
74 cmd = "help qmult"; |
|
|
75 run_cmd |
|
|
76 cmd = "help qinv" |
|
|
77 run_cmd |
|
|
78 disp("Inverse quaternion: q*qi = qi*q = 1:") |
|
|
79 cmd = "q1i = qinv(q1)"; |
|
|
80 run_cmd |
|
|
81 cmd = "one = qmult(q1,q1i)"; |
|
|
82 run_cmd |
|
|
83 |
|
|
84 printf("Conclusion of simple quaternion functions"); |
|
|
85 prompt |
|
|
86 case(3), |
|
|
87 printf("Transformation functions\n"); |
|
|
88 disp("A problem with the discussion of coordinate transformations is that"); |
|
|
89 disp("one must be clear on what is being transformed: does a rotation of"); |
|
|
90 disp("theta degrees mean that you're rotating the VECTOR by theta degrees,"); |
|
|
91 disp("also called the 'active convention,' or does it mean that you rotate "); |
|
|
92 disp("the COORDINATE FRAME by theta degrees, also called the 'passive convention,' "); |
|
|
93 disp("which is equivalent to rotating the VECTOR by (-theta) degrees. The"); |
|
|
94 disp("functions in this demo use the active convention. I'll point out where"); |
|
|
95 disp("this changes the code as the demo runs."); |
|
|
96 disp(" -- The author"); |
|
|
97 prompt |
|
|
98 printf("\n\n"); |
|
|
99 disp("Sequences of rotations:") |
|
|
100 printf("\n\nRotation of a vector by 90 degrees about the reference z axis\n"); |
|
|
101 cmd = "qz = quaternion([0 0 1], pi/2);"; |
|
|
102 disp(cmd) ; eval(cmd); |
|
|
103 printf("\n\nRotation of a vector by 90 degrees about the reference y axis\n"); |
|
|
104 cmd="qy = quaternion([0 1 0], pi/2);"; |
|
|
105 disp(cmd) ; eval(cmd); |
|
|
106 printf("\n\nRotation of a vector by 90 degrees about the reference x axis\n"); |
|
|
107 cmd="qx = quaternion([1 0 0], pi/2);"; |
|
|
108 run_cmd |
|
|
109 printf("\n\nSequence of three rotations: 90 degrees about x, then 90 degrees\n"); |
|
|
110 disp("about y, then 90 degrees about z (all axes specified in the reference frame):"); |
|
|
111 qchk = qmult(qz,qmult(qy,qx)); |
|
|
112 cmd = "[vv,th] = quaternion(qchk), th_deg = th*180/pi"; |
|
|
113 run_cmd |
|
|
114 disp("The sequence of the three rotations above is equivalent to a single rotation") |
|
|
115 disp("of 90 degrees about the y axis. Check:"); |
|
|
116 cmd = "err = norm(qchk - qy)"; |
|
|
117 run_cmd |
|
|
118 |
|
|
119 disp("Transformation of a quaternion by a quaternion:") |
|
|
120 disp("The three quaternions above were rotations specified about") |
|
|
121 disp("a single reference frame. It is often convenient to specify the"); |
|
|
122 disp("eigenaxis of a rotation in a different frame (e.g., when computing"); |
|
|
123 disp("the transformation rotation in terms of the Euler angles yaw-pitch-roll)."); |
|
|
124 cmd = "help qtrans"; |
|
|
125 run_cmd |
|
|
126 disp("") |
|
|
127 disp("NOTE: If the passive convention is used, then the above"); |
|
|
128 disp("formula changes to v = qinv(q)*v*q instead of ") |
|
|
129 disp("v = q*v*qinv(q).") |
|
|
130 prompt |
|
|
131 disp("") |
|
|
132 disp("Example: Vectors in Frame 2 are obtained by rotating them from ") |
|
|
133 disp(" from Frame 1 by 90 degrees about the x axis (quaternion qx)") |
|
|
134 disp(" A quaternion in Frame 2 rotates a vector by 90 degrees about") |
|
|
135 disp(" the Frame 2 y axis (quaternion qy). The equivalent rotation") |
|
|
136 disp(" in the reference frame is:") |
|
|
137 cmd = "q_eq = qtrans(qy,qx); [vv,th] = quaternion(q_eq)"; |
|
|
138 run_cmd |
|
|
139 disp("The rotation is equivalent to rotating about the reference z axis") |
|
|
140 disp("by 90 degrees (quaternion qz)") |
|
|
141 prompt |
|
|
142 |
|
|
143 disp("Transformation of a vector by a quaternion"); |
|
|
144 cmd = "help qtransv"; |
|
|
145 run_cmd |
|
|
146 |
|
|
147 disp("NOTE: the above formula changes if the passive quaternion ") |
|
|
148 disp("is used; the cross product term is subtracted instead of added."); |
|
|
149 prompt |
|
|
150 disp("Example: rotate the vector [1,1,1] by 90 degrees about the y axis"); |
|
|
151 cmd = "vec_r = qtransv([1,1,1],qy)"; |
|
|
152 run_cmd |
|
|
153 prompt |
|
|
154 disp("Equivalently, one may multiply by qtransvmat:") |
|
|
155 cmd = "help qtransvmat"; |
|
|
156 run_cmd |
|
|
157 disp("NOTE: the passive quaternion convention would use the transpose") |
|
|
158 disp("(inverse) of the orthogonal matrix returned by qtransvmat."); |
|
|
159 prompt |
|
|
160 cmd = "vec_r_2 = qtransvmat(qy)*[1;1;1]; vec_err = norm(vec_r - vec_r_2)"; |
|
|
161 run_cmd |
|
|
162 |
|
|
163 disp("") |
|
|
164 disp("The last transformation function is the derivative of a quaternion") |
|
|
165 disp("Given rotation rates about the reference x, y, and z axes."); |
|
|
166 cmd = "help qderivmat"; |
|
|
167 run_cmd |
|
|
168 disp("") |
|
|
169 disp("Example:") |
|
|
170 disp("Frame is rotating about the z axis at 1 rad/s") |
|
|
171 cmd = "Omega = [0,0,1]; Dmat = qderivmat(Omega)"; |
|
|
172 run_cmd |
|
|
173 disp("Notice that Dmat is skew symmetric, as it should be.") |
|
|
174 disp("expm(Dmat*t) is orthogonal, so that unit quaternions remain") |
|
|
175 disp("unit quaternions as the rotating frame precesses."); |
|
|
176 disp(" ") |
|
|
177 disp("This concludes the transformation demo."); |
|
|
178 prompt; |
|
|
179 case(4), |
|
|
180 printf("Body-inertial frame demo: Look at the source code for\n"); |
|
|
181 printf("demoquat.m and qcoordinate_plot.m to see how it's done.\n"); |
|
|
182 |
|
|
183 # i,j,k units |
|
|
184 iv = quaternion(1,0,0,0); jv = quaternion(0,1,0,0); |
|
|
185 kv = quaternion(0,0,1,0); |
|
|
186 |
|
|
187 # construct quaternion to desired view. |
|
|
188 degrees = pi/180; daz = 45*degrees; del = -30*degrees; |
|
|
189 qazimuth = quaternion([0,0,1],daz); |
|
|
190 qelevation = quaternion([cos(daz),sin(daz),0],del); |
|
|
191 qview = qmult(qelevation,qazimuth); |
|
|
192 |
|
|
193 # inertial frame i, j, k axes. |
|
|
194 iif = iv; jf = qtrans(jv,iv); kf = qtrans(kv,iv); |
|
|
195 |
|
|
196 # rotation steps |
|
|
197 th = 0:5:20; ov = ones(size(th)); myth = [th,max(th)*ov ; 0*ov,th]; |
|
|
198 |
|
|
199 # construct yaw-pitch-roll cartoon |
|
|
200 for kk=1:length(myth(1,:)) |
|
|
201 figure(kk) |
|
|
202 thy = myth(1,kk); |
|
|
203 thp = myth(2,kk); |
|
|
204 |
|
|
205 qyaw = quaternion([0,0,1],thy*pi/180); |
|
|
206 [jvy,th] = quaternion(qtrans(jf,qyaw)); |
|
|
207 qpitch = quaternion(jvy(1:3),thp*pi/180); |
|
|
208 qb = qmult(qpitch, qyaw); |
|
3235
|
209 qi = quaternion([1, 0, 0],180*degrees); |
|
3224
|
210 |
|
|
211 printf("yaw=%8.4f, pitch=%8.4f, \n qbi = (%8.4f)i + (%8.4e)j + (%8.4f)k + (%8.4f)\n",thy,thp, ... |
|
|
212 qb(1), qb(2), qb(3), qb(4)); |
|
|
213 [vv,th] = quaternion(qb); |
|
|
214 printf(" = (vector) = [%8.4f %8.4f %8.4f], th=%5.2f deg\n", ... |
|
|
215 vv(1), vv(2), vv(3), th*180/pi); |
|
|
216 |
|
|
217 qb = qmult(qb,qi); |
|
|
218 title(sprintf("yaw=%5.2f deg, pitch=%5.2f deg",thy,thp)) |
|
|
219 qcoordinate_plot(qi,qb,qview); |
|
|
220 # uncomment the next four lines to get eps output |
|
|
221 #gset terminal postscript eps |
|
|
222 #eval(sprintf("gset output 'fig%d.eps'",kk)); |
|
|
223 #replot |
|
|
224 #gset terminal x11 |
|
|
225 #prompt |
|
|
226 endfor |
|
|
227 case(quitopt), |
|
|
228 printf("Exiting quaternion demo\n"); |
|
|
229 otherwise, |
|
|
230 error(sprintf("Illegal option %f",opt)); |
|
|
231 endswitch |
|
|
232 endwhile |
|
|
233 endfunction |
