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