https://github.com/RomaTeng/EKF-SLAM-on-Manifold
Tip revision: 12d7d8d88c84161baed173e38d49dedb4adb2b96 authored by Teng Zhang on 26 February 2017, 15:09:17 UTC
Record my control paper
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