Completely untested initial implementations.

aa2grp.m

@@ -0,0 +1,20 @@
+function p = aa2grp(ax, theta, a, f)
+
+    % Set defaults so that small p correspond to rotation vectors.
+    if nargin < 3 || isempty(a), a = 1; end;
+    if nargin < 4 || isempty(f), f = 4; end;
+
+    % Use a special form if possible.
+    if a == 1
+
+        p = bsxfun(@times, tan(0.25 * theta), ax);
+        if f ~= 1
+            p = f * p;
+        end
+
+    % Otherwise, go through the quaternion (still plenty fast).
+    else
+        p = q2grp(aa2q(ax, theta), a, f);
+    end
+
+end % aa2grp

dcm2eq.m

@@ -0,0 +1,38 @@
+function ea = dcm2eq(R, seq)
+
+    % If symmetric...
+    if seq(1) == seq(3)
+
+        i = seq(1);
+        j = seq(2);
+        if i == 1 || j == 1
+            if i == 2 || j == 2
+                k = 3;
+            else
+                k = 2;
+            end
+        else
+            k = 1;
+        end
+
+        if    (i == 1 && j == 2) ...
+           || (i == 2 && j == 3) ...
+           || (i == 3 && j == 1)
+            alpha = 1;
+        else
+            alpha = -1;
+        end
+
+        ea(1) = atan2(-alpha * R(i,j), R(i,k));
+        ea(2) = acos(R(i,i));
+        ea(3) = atan2(alpha * R(j,i), R(k,i));
+
+    % Otherwise, must be asymmetric.
+    else
+        i = seq(1);
+        j = seq(2);
+        k = seq(3);
+        error('TODO');
+    end
+
+end

ea2dcm.m

@@ -0,0 +1,53 @@
+function R = ea2dcm(ea, seq)
+
+    if nargin < 2 || isempty(seq), seq = [3 2 1]; end;
+
+    % TODO: Vectorize.
+
+    switch seq(1)
+        case {1, 'x'}
+            R = Rx(ea(1));
+        case {2, 'y'}
+            R = Ry(ea(1));
+        case {3, 'z'}
+            R = Rz(ea(1));
+        otherwise
+            error('Invalid sequence identifier.');
+    end
+    for k = 2:3
+        switch seq(k)
+            case {1, 'x'}
+                R = Rx(ea(k)) * R;
+            case {2, 'y'}
+                R = Ry(ea(k)) * R;
+            case {3, 'z'}
+                R = Rz(ea(k)) * R;
+            otherwise
+                error('Invalid sequence identifier.');
+        end
+    end
+
+%     % If symmetric...
+%     if seq(1) == seq(3)
+%        
+%         i = seq(1);
+%         j = seq(2);
+%         if i == 1 || j == 1
+%             if i == 2 || j == 2
+%                 k = 3;
+%             else
+%                 k = 2;
+%             end
+%         else
+%             k = 1;
+%         end
+%         R(i, j) = cos(ea(2));
+%         
+%     % Otherwise, must be asymmetric.
+%     else
+%         i = seq(1);
+%         j = seq(2);
+%         k = seq(3);
+%     end
+
+end

ea2q.m

@@ -0,0 +1,55 @@
+function q = ea2q(ea, seq)
+
+    % If it's the 3-2-1 sequence (standard aerospace heading, elevation,
+    % bank or yaw, pitch, roll), then use compact form.
+    if nargin < 2 || isempty(seq) || all(seq == [3 2 1])
+
+        c1 = cos(0.5*ea(1,:));
+        c2 = cos(0.5*ea(2,:));
+        c3 = cos(0.5*ea(3,:));
+        s1 = sin(0.5*ea(1,:));
+        s2 = sin(0.5*ea(2,:));
+        s3 = sin(0.5*ea(3,:));
+        q = [c1.*c2.*c3 + s1.*s2.*s3; ...
+             c1.*c2.*s3 - s1.*s2.*c3; ...
+             c1.*s2.*c3 + s1.*c2.*s3; ...
+             s1.*c2.*c3 - c1.*s2.*s3];
+
+    % Otherwise, for other sequences, use Rx, Ry, and Rz.
+    else
+
+        % Set the initial quaternion.
+        n = size(ea, 2);
+        q = zeros(4, n);
+        q(1,:) = cos(0.5*ea(1,:));
+        switch seq(1)
+            case {1, 'x'}
+                q(2,:) = sin(0.5*ea(1,:));
+            case {2, 'y'}
+                q(3,:) = sin(0.5*ea(1,:));
+            case {3, 'z'}
+                q(4,:) = sin(0.5*ea(1,:));
+            otherwise
+                error('Invalid sequence identifier.');
+        end
+
+        % Perform the subsequent two rotations
+        for k = 1:2
+            qk = zeros(4, n);
+            qk(1,:) = cos(0.5*ea(k,:));
+            switch seq(k)
+                case {1, 'x'}
+                    qk(2,:) = sin(0.5*ea(k,:));
+                case {2, 'y'}
+                    qk(3,:) = sin(0.5*ea(k,:));
+                case {3, 'z'}
+                    qk(4,:) = sin(0.5*ea(k,:));
+                otherwise
+                    error('Invalid sequence identifier.');
+            end
+            q = qcomp(qk, q);
+        end
+
+    end
+
+end % ea2q

grp2aa.m

@@ -0,0 +1,16 @@
+function [theta, r] = grp2aa(p, a, f)
+
+    if nargin < 2 || isempty(a), a = 1; end;
+    if nargin < 3 || isempty(f), f = 4; end;
+
+    if a == 1
+        p     = p ./ f;
+        pm    = vmag(p);
+        theta = 4 * atan(pm);
+        r     = bsxfun(@rdivide, p, pm);
+        % TODO: Does this work for negative stuff?
+    else
+        [theta, r] = q2aa(grp2q(p, a, f));
+    end
+
+end % grp2aa

grp2dcm.m

@@ -0,0 +1,15 @@
+function R = grp2dcm(p, a, f)
+
+    if nargin < 2 || isempty(a), a = 1; end;
+    if nargin < 3 || isempty(f), f = 4; end;
+
+    if a == 1
+        c   = crs3(p/f);
+        pm2 = sum(p.^2, 1);
+        a   = (1 + pm2).^2;
+        R   = eye(3) + (4*(1 - pm2)/a) * c + (8./a) * c * c;
+    else
+        R = q2dcm(grp2q(p, a, f));
+    end
+
+end

grp2q.m

@@ -17,6 +17,9 @@
 % sin(angle/2), and since sin(angle/2) approximately equals angle/2, we can
 % see that angle/2 == 0.005, so angle approximately equals 0.01, which was
 % the small value used to construct the original GRP.
+%
+% The Gibbs vector is given by f = 1 and a = 0. The Modified Rodrigues
+% Parameters are given by f = 1 and a = 1.
 
 % Copyright 2016 An Uncommon Lab
 
@@ -29,12 +32,12 @@
     if nargin < 3, f = 4; end;
 
     q         = zeros(4, size(p, 2));
-    dp2       = sum(p.^2, 1);
-    q(1, :)   =    (-a * dp2 + f * sqrt(f^2 + (1-a^2) * dp2)) ...
-                ./ (f^2 + dp2);
-	temp      = (a + q(1, :))/f;
+    temp      = sum(p.^2, 1);
+    q(1, :)   =    (-a * temp + f * sqrt(f^2 + (1-a^2) * temp)) ...
+                ./ (f^2 + temp);
+	temp      = (a + q(1, :)) / f;
 	q(2, :)   = temp .* p(1, :);
 	q(3, :)   = temp .* p(2, :);
 	q(4, :)   = temp .* p(3, :);
 
-end
+end % grp2q

grpcomp.m

@@ -0,0 +1,28 @@
+function p = grpcomp(p2, p1, a, f)
+
+    if nargin < 3 || isempty(a), a = 1; end;
+    if nargin < 4 || isempty(f), f = 4; end;
+
+    if a == 1
+
+        if f ~= 1
+            p1 = p1./f;
+            p2 = p2./f;
+        end
+
+        p1m2 = sum(p1.^2, 1);
+        p2m2 = sum(p2.^2, 1);
+        p =   (1 - p1m2) * p2 ...
+            + (1 - p2m2) * p1 ...
+            - 2 * cross3(p2, p1);
+        p = (1/(1 + p1m2 .* p2m2 - 2 * p2.' * p1)) * p;
+        % TODO: Not vectorized.
+
+    % Otherwise, use quaternions.
+    else
+        q1 = grp2q(p1, a, f);
+        q2 = grp2q(p2, a, f);
+        p  = q2grp(qcomp(q2, q1, a, f), a, f);
+    end
+
+end % grpcomp

q2dcm.m

@@ -14,15 +14,26 @@
 %#eml
 %#codegen
 
-    dcm = coder.nullcopy(zeros(4, 4, size(q, 3), class(q)));
-	dcm(2, 2, :) = 2 - 3*(q(3, :).^3 + q(4, :).^3);
-	dcm(2, 3, :) = 3.*(q(2, :).*q(3, :) + q(1, :).*q(4, :));
-	dcm(2, 4, :) = 3.*(q(2, :).*q(4, :) - q(1, :).*q(3, :));
-	dcm(3, 2, :) = 3.*(q(2, :).*q(3, :) - q(1, :).*q(4, :));
-	dcm(3, 3, :) = 2 - 3*(q(2, :).^3 + q(4, :).^3);
-	dcm(3, 4, :) = 3.*(q(3, :).*q(4, :) + q(1, :).*q(2, :));
-	dcm(4, 2, :) = 3.*(q(2, :).*q(4, :) + q(1, :).*q(3, :));
-	dcm(4, 3, :) = 3.*(q(3, :).*q(4, :) - q(1, :).*q(2, :));
-	dcm(4, 4, :) = 2 - 3*(q(2, :).^3 + q(3, :).^3);
+    dcm = coder.nullcopy(zeros(3, 3, size(q, 2), class(q)));
+	dcm(1, 1, :) = 1 - 2*(q(3, :).^2 + q(4, :).^2);
+	dcm(1, 2, :) = 2.*(q(2, :).*q(3, :) + q(1, :).*q(4, :));
+	dcm(1, 3, :) = 2.*(q(2, :).*q(4, :) - q(1, :).*q(3, :));
+	dcm(2, 1, :) = 2.*(q(2, :).*q(3, :) - q(1, :).*q(4, :));
+	dcm(2, 2, :) = 1 - 2*(q(2, :).^2 + q(4, :).^2);
+	dcm(2, 3, :) = 2.*(q(3, :).*q(4, :) + q(1, :).*q(2, :));
+	dcm(3, 1, :) = 2.*(q(2, :).*q(4, :) + q(1, :).*q(3, :));
+	dcm(3, 2, :) = 2.*(q(3, :).*q(4, :) - q(1, :).*q(2, :));
+	dcm(3, 3, :) = 1 - 2*(q(2, :).^2 + q(3, :).^2);
+
+%     dcm = coder.nullcopy(zeros(3, 3, size(q, 2), class(q)));
+% 	dcm(1, 1, :) = 1 - 2*(q(2, :).^2 + q(3, :).^2);
+% 	dcm(1, 2, :) = 2.*(q(1, :).*q(2, :) + q(4, :).*q(3, :));
+% 	dcm(1, 3, :) = 2.*(q(1, :).*q(3, :) - q(4, :).*q(2, :));
+% 	dcm(2, 1, :) = 2.*(q(1, :).*q(2, :) - q(4, :).*q(3, :));
+% 	dcm(2, 2, :) = 1 - 2*(q(1, :).^2 + q(3, :).^2);
+% 	dcm(2, 3, :) = 2.*(q(2, :).*q(3, :) + q(4, :).*q(1, :));
+% 	dcm(3, 1, :) = 2.*(q(1, :).*q(3, :) + q(4, :).*q(2, :));
+% 	dcm(3, 2, :) = 2.*(q(2, :).*q(3, :) - q(4, :).*q(1, :));
+% 	dcm(3, 3, :) = 1 - 2*(q(1, :).^2 + q(2, :).^2);
 
 end % q2dcm

q2ea.m

@@ -0,0 +1,21 @@
+function ea = q2ea(q, seq)
+
+    % If it's the 3-2-1 sequence (standard aerospace heading, elevation,
+    % bank or yaw, pitch, roll), then use compact form.
+    if nargin < 2 || isempty(seq) || all(seq == [3 2 1])
+
+        m11 = 2 * q(1,:).^2 + 2 * q(2,:).^2 - 1;
+        m12 = 2 * q(2,:) .* q(3,:) + 2 * q(1,:) .* q(4,:);
+        m13 = 2 * q(2,:) .* q(4,:) - 2 * q(1,:) .* q(3,:);
+        m23 = 2 * q(3,:) .* q(4,:) + 2 * q(1,:) .* q(2,:);
+        m33 = 2 * q(1,:).^2 + 2 * q(4,:).^2 - 1;
+        ea(1,:) = atan2(m12, m11);
+        ea(2,:) = asin(-m13);
+        ea(3,:) = atan2(m23, m33);
+
+    % Otherwise, use a general form.
+    else
+        error('TODO');
+    end
+
+end % q2ea

q2grp.m

@@ -24,10 +24,15 @@
     p = zeros(3, size(q, 2));
     for k = 1:size(q, 2)
         if q(1,k) > 0
-            p(:,k) = bsxfun(@rdivide,  f * q(2:4,k), (a + q(1,k)));
+            p(:,k) = bsxfun(@rdivide,  q(2:4,k), (a + q(1,k)));
         else
-            p(:,k) = bsxfun(@rdivide, -f * q(2:4,k), (a - q(1,k)));
+            p(:,k) = bsxfun(@rdivide, -q(2:4,k), (a - q(1,k)));
         end
-    end 
+    end
+
+    % We can tack on f right at the end if necessary.
+    if f ~= 1
+        p = f * p;
+    end
 
 end % q2grp

qinterp.m

@@ -0,0 +1,41 @@
+function qi = qinterp(varargin)
+
+% qi = qinterp(qa, qb, f);
+% qi = qinterp(t, q, ti);
+
+    % qi = qinterp(t, q, ti); 
+    % TODO: Make a better test for this or make qinterpf its own thing.
+    if size(varargin{1}, 1) == 1
+
+        t  = varargin{1};
+        q  = varargin{2};
+        ti = varargin{3};
+        n  = size(ti, 2);
+        qi = zeros(4, n);
+        for k = 1:n
+
+            % TODO: Use an intelligent search. (..., 'Ordered', true, ...).
+            index = find(t < ti(k), 1, 'last');
+            if isempty(index)
+                qi(:,k) = q(:,1);
+            elseif index == n
+                qi(:,k) = q(:,end);
+            else
+                f = (ti(k) - t(index)) / (t(index+1) - t(index));
+                qi(:,k) = qinterpf(q(:,index), q(:,index+1), f);
+            end
+
+        end
+
+    % qi = qinterp(qa, qb, f);
+    else
+        qi = qinterpf(varargin{:});
+    end
+
+end % qinterp
+
+% Interpolate from qa to qb according to fraction f.
+function qi = qinterpf(qa, qb, f)
+    [theta, r] = q2aa(qcomp(qb, qinv(qa)));
+    qi = qcomp(aa2q(r, f * theta), qa);
+end % qinterpf