Added some help and fixed two bugs.

dcm2aa.m

@@ -69,11 +69,13 @@
             r(:,theta_is_pi)   = normalize(r(:,theta_is_pi));
 
             normal      = ~theta_is_zero & ~theta_is_pi;
-            d           = 1./(2 * sin(theta(normal)));
-            s           = sum(normal);
-            r(1,normal) = d .* reshape(R(2,3,normal) - R(3,2,normal), [1 s]);
-            r(2,normal) = d .* reshape(R(3,1,normal) - R(1,3,normal), [1 s]);
-            r(3,normal) = d .* reshape(R(1,2,normal) - R(2,1,normal), [1 s]);
+            if any(normal)
+                d           = 1./(2 * sin(theta(normal)));
+                s           = sum(normal);
+                r(1,normal) = d .* reshape(R(2,3,normal) - R(3,2,normal), [1 s]);
+                r(2,normal) = d .* reshape(R(3,1,normal) - R(1,3,normal), [1 s]);
+                r(3,normal) = d .* reshape(R(1,2,normal) - R(2,1,normal), [1 s]);
+            end
 
         % Otherwise, codegen...
         else

example_vectors_and_rotations.m

@@ -1,9 +1,16 @@
 %% Examples for Vectors and Rotations Toolbox
 %
 % This file shows examples of using this toolbox to manipulate vectors and
-% rotations (in various parameterizations). The functions are consistent,
-% so once one knows how to use, e.g., a quaternion in one function, it is
-% the same everywhere.
+% rotations in various parameterizations. Once one knows how to use each
+% rotation and vector type, the remaining functions become intuitive.
+%
+% These examples assume you've read the readme already. If not, start
+% there.
+
+%% Rotation Conversions
+% 
+% We'll start with functions for rotating from one representation of a
+% rotation to another.
 
 %%
 % Let's suppose frame B is rotated from frame A by pi/4 about the y axis.
@@ -14,19 +21,19 @@
 r = [0; 1; 0];
 
 %%
-% And the angle of B with respect to (wrt) A:
+% And the angle of frame B with respect to (wrt) A:
 
 theta = pi/4;
 
 %%
 % If something were along the x axis in A, then we'd expect to see it
 % equally on the x and z axes of B. Let's make sure that happens by
 % converting the axis and angle to a direction cosine matrix (DCM) and
-% translate a vector known in A to the B frame.
+% converting a vector known in A to the B frame.
 
-r_A  = [1; 0; 0];               % a vector in the A frame
+v_A  = [1; 0; 0];               % a vector in the A frame
 R_BA = aa2dcm(theta, r)         % rotation matrix of B wrt A
-r_B  = R_BA * r_A               % the vector, as seen in B
+v_B  = R_BA * v_A               % the vector, as seen in B
 
 %%
 % That looks right. Of course, rotations about x, y, or z are common, so
@@ -43,7 +50,8 @@
 [theta, r] = dcm2aa(R_BA)
 
 %%
-% We can express the rotation as a quaternion too.
+% We can express the rotation as a rotation quaternion (Euler-Rodrigues 
+% symmetric parameters, with the scalar as the fourth element of q).
 
 q_BA = aa2q(theta, r) % quaternion of B wrt A
 
@@ -85,28 +93,125 @@
 R_BA = ea2dcm(ea_BA)
 
 %%
-% Generalized Rodrigues parameters are not as intuitive as the other forms,
-% but they have their uses. Let's create the Gibbs vector for this
-% rotation:
+% Modified Rodrigues parameters are not as intuitive as the other forms,
+% but they have their uses. Let's create them for this rotation:
 
-g_BA = aa2grp(theta, r, 0, 1)
+p_BA = aa2mrp(theta, r)
 
 %%
 % We could have used the quaterion as well:
 
-g_BA = q2grp(q_BA, 0, 1)
+g_BA = q2mrp(q_BA)
 
 %%
 % And then back to the axis and angle, DCM, and quaternion:
 
-[theta, r] = grp2aa(g_BA, 0, 1)
-R_BA = grp2dcm(g_BA, 0, 1)
-q_BA = grp2q(g_BA, 0, 1)
+[theta, r] = mrp2aa(g_BA)
+R_BA = mrp2dcm(g_BA)
+q_BA = mrp2q(g_BA)
+
+%%
+% We can also scale the MRPs by a constant factor. For instance, scaling
+% them by a factor of 4 causes the MRPs to approach the rotation vector for
+% small angles. For instance, let's look at three small rotations aboud
+% three different axes:
+
+theta = [0.01, 0.1, 0.5];
+r     = [   1,   0,   0; ...
+            0,   1,   0; ...
+            0,   0,   1];
+
+%%
+% The rotation vectors are:
+
+[theta(1) * r(:,1), theta(2) * r(:,2), theta(3) * r(:,3)]
+
+%%
+% Let's create the scaled MRPs for each of the three rotations (notice how
+% we can pass in 1-by-n and 3-by-n values and get the vectorized results).
+
+p = aa2mrp(theta, r, 4)
+
+%%
+% A scaling factor of 4 is clearly useful.
+%
+% Just like q and -q refer to the same rotation for quaternions, so too do
+% the modified Rodrigues parameters have an alternate, or "shadow",
+% representation.
+
+s = mrpalt(p, 4)
+
+%%
+% And we can convert back with the same function. It doesn't need to "know"
+% that it's now operating on the shadow set; the operations are the same.
+
+p = mrpalt(s, 4)
 
 %%
-% And the modified Rodrigues parameters:
+% In fact, neither really "is" the shadow set; they're both shadows of each
+% other. And we can convert them to, e.g., the same rotation quaternion:
+
+q1 = mrp2q(p, 4)
+q2 = mrp2q(s, 4)
+
+%%
+% (They differ by sign, but q and -q refer to the same rotation for 
+% rotation quaternions.)
+
+%% Operations with Rotations
+% 
+% Direction cosine matrices are very easy to use. For instance, multiple
+% rotations can be created by simply multiplying them together. If R_BA is
+% the rotation of frame B wrt frame A, and R_CB is the rotation of frame C
+% wrt frame B, then the composite rotation of C wrt A is:
+% 
+% R_CA = R_CB * R_BA
+% 
+% Let's check. Suppose B is rotated from A by a rotation of pi/3 about some
+% axis, and then C is rotated from B by 2*pi/3 about the same axis. The
+% composite rotation angle should be pi.
+
+Rx(2*pi/3) * Rx(pi/3)
+
+%%
+% Let's try that again, rotating around some random vector.
+
+r = randunit(3)
+% r = [1; 1; 0] / sqrt(2)
+% r = [1; 1; 1] / sqrt(3)
+R_BA = aa2dcm(pi/3,   r);
+R_CB = aa2dcm(2*pi/3, r);
+R_CA = R_CB * R_BA
+
+%%
+% What's the composite angle and axis of rotation?
+
+[theta, r] = dcm2aa(R_CA)
+
+%%
+% It's pi/3 + 2*pi/3 = pi about our original axis, like we'd expect.
+
+%%
+% For quaternions and modified Rodrigues parameters, the analogous 
+% arithmetic is encapsulated in qcomp and mrpcomp.
+
+q_BA = aa2q(  pi/3, r);
+q_CB = aa2q(2*pi/3, r);
+q_CA = qcomp(q_CB, q_BA)
+[theta, r] = q2aa(q_CA)
+
+p_BA = aa2mrp(  pi/3, r);
+p_CB = aa2mrp(2*pi/3, r);
+p_CA = mrpcomp(p_CB, p_BA)
+[theta, r] = mrp2aa(p_CA)
 
-mrp_BA = q2grp(q_BA, 1, 1)
-[theta, r] = grp2aa(mrp_BA, 1, 1)
-R_BA = grp2dcm(mrp_BA, 1, 1)
-q_BA = grp2q(mrp_BA, 1, 1)
+%% TODO
+% qrot
+% qinv
+% qpos
+% qdiff, qerr, mrpdiff, mrperr
+% qdot
+% qinterp
+% cross3, crs3, normalize, vmag, vmag2
+% rae2xyz, xyz2rae
+% vecplot

mrp2q.m

@@ -23,7 +23,7 @@
 
     % Set defaults so that for small angles, the scaled MRPs approach the
     % rotation vector.
-    if nargin < 3 || isempty(f), f = 1; end;
+    if nargin < 2 || isempty(f), f = 1; end;
 
     % Check dimensions.
     assert(nargin >= 1, ...
@@ -37,7 +37,7 @@
     q       = zeros(4, n, class(p));
     f2      = f * f;
     pm2     = p(1,:).*p(1,:) + p(2,:).*p(2,:) + p(3,:).*p(3,:);
-    c0      = 1 ./ (f2 + pm2);
+    c0      = 1  ./ (f2 + pm2);
     q(4,:)  = c0 .* (f2 - pm2);
     c0      = (2*f) * c0;
     q(1,:)  = c0 .* p(1,:);

readme.md

@@ -117,7 +117,8 @@ These conventions are consistent with the literature on using quaternions for ro
 2. Quaternion "composition" is backwards from quaternion multiplication in that toolbox.
 3. qinv should be used instead of quatconj.
 4. Functions in this toolbox are created to ensure that quaternions retain a unit 2-norm.
-5. The functions in this toolbox are both faster in MATLAB and better for C code generation.
+5. This toolbox expects multiple quaternions to be a 4-by-n matrix, whereas it is n-by-4 in that toolbox.
+6. The functions in this toolbox are both faster in MATLAB and better for C code generation.
 
 
 Euler Angles