align_wander_v000

%input is defined in "b", ref is defined in "n" (e.g inp=[g*sin(45) g*cos(45) 0], ref=[0 0 1])
%Cbn_est=(I-S(e))Cbn, e=E[inp_err]

function [Cnb E]=align_wander(inp, ref)

%Compute the dcm matrix
Cnb=align_vector(inp,ref);

if (nargout==2) %Compute the error matrix 
    E=zeros(3);
    ep=0.001;
    
    for i=1:3
        %ith perturbation
        inp_err=inp;
        inp_err(i)=inp_err(i)+ep;
        Cnb_err=align_vector(inp_err,ref);
        err=Cnb_err*Cnb'-eye(3);
        E(:,i)=[-err(2,3);err(1,3);-err(1,2)]/ep;
    end
    
end






function Cnb=align_vector(inp, ref)
%normalize
inp=inp/norm(inp);
ref=ref/norm(ref);

%angles
dotp=dot(ref,inp);
crosv=cross(ref,inp);
crosp=norm(crosv);


if (crosp==0) %inp and ref is already aligned
    if (dotp>=0) %no rotation is necessary assert(dotp==1)
        Cnb=eye(3);
    else %assert(dotp==-1)
        %Find a vector that is ortogonal to ref
        if (ref(1)^2>ref(2)^2)
            vr_a=[-ref(3);0;ref(1)];
        else
            vr_a=[0;-ref(3);ref(2)];
        end
        vr_a=vr_a/norm(vr_a)*pi;
        
        %rotate around computed orthogonal vector
        Cnb=rot2dcm_v000(vr_a);
    end
else
    Cnb=eye(3)+skew(crosv)+((1-dotp)/crosp^2)*skew(crosv)*skew(crosv);
end

return;