https://github.com/RomaTeng/EKF-SLAM-on-Manifold
Raw File
Tip revision: 12d7d8d88c84161baed173e38d49dedb4adb2b96 authored by Teng Zhang on 26 February 2017, 15:09:17 UTC
Record my control paper
Tip revision: 12d7d8d
Exp.m
function [ result ] = Exp( x )

% use sparse matrix
N=size(x,1);

if N==3
    theta=norm(x);
    if theta==0
        result=eye(3);
    else     
        omega =x/theta;
        result=eye(3,3) + sin(theta) * skew(omega) + (1 - cos(theta))*skew(omega)^2;  
    end
        
else
    m=(N-3)/3;
    result=speye(3+m);
    result(1:3,1:3)=Exp( x(1:3));
    
    

end




    
    
    
end

back to top