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
J_r.m
function [ result ] = J_r( delta )
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Function: Overloading function.
% Method1: I(3°¡3) - (1-cos(theta))*skew(delta)/norm(delta)^2 + ...
% (norm(delta)-sin(norm(delta)))*skew(delta)^2/norm(delta)^3
% Input1: delta: 3°¡1 vector
% Returns1: result: See the document
% Method2: See the document
% Input2: delta: 6+3N vector
% Returns2: result: See the document
% Author: Jingwei Song. 04/06/2016
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N= (size(delta,1)-3)/3;
if(size(delta,1) == 3)
function_mode = 1;
else
function_mode = 2;
end
if(function_mode == 1)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Function1
theta = norm(delta);
if(theta < 0.00000001)
result = eye(3,3);
else
result = eye(3,3) - (1-cos(theta))*skew(delta)/theta^2 + (theta-sin(theta))*skew(delta)^2/norm(delta)^3;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
else
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Function2
result = zeros(3+3*N,3+3*N);
s_theta = delta(1:3,1);
for i = 1 : N+1 % Set diagonal to J_r(s_theta)
result(3*i-2:3*i,3*i-2:3*i) = J_r(s_theta);
end
for i = 1 : N
s_temp = delta(3*i+1:3*i+3,1);
result(3*i+1:3*i+3,1:3) = K_r_x1x2(s_theta, s_temp);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end
end