3dpcp/.svn/pristine/d7/d79ec5edec92ff5c8f2f2e34cc9ecd8915a350a3.svn-base

76 lines
1.5 KiB
Text
Raw Normal View History

2012-09-16 12:33:11 +00:00
% script for interpolating data of RTS Hannover
function output = recordingtime_sync(input, ref)
[input_m, input_n] = size(input);
[ref_m, ref_n] = size(ref);
output = zeros(ref_m, input_n);
for i = 1 : ref_m
upper_index = find(input(:,1) > ref(i,1));
upper = input(upper_index(1),:);
if(upper_index(1) > 1)
lower = input((upper_index(1)-1),:);
else
lower = upper;
end
diff = upper - lower;
% normalise angel sym 0
if(diff(5) > pi)
diff(5) = diff(5) - 2*pi;
end
if(diff(5) < -pi)
diff(5) = diff(5) + 2*pi;
end
if(diff(6) > pi)
diff(6) = diff(6) - 2*pi;
end
if(diff(6) < -pi)
diff(6) = diff(6) + 2*pi;
end
if(diff(7) > pi)
diff(7) = diff(7) - 2*pi;
end
if(diff(7) < -pi)
diff(7) = diff(7) + 2*pi;
end
if(diff(1) == 0)
rel_time = 0;
else
rel_time = (ref(i,1) - lower(1,1)) / diff(1);
end
output(i,:) = lower + rel_time .* diff;
% normalise angel sym 0
if(output(i,5) > pi)
output(i,5) = output(i,5) - 2*pi;
end
if(output(i,5) < -pi)
output(i,5) = output(i,5) + 2*pi;
end
if(output(i,6) > pi)
output(i,6) = output(i,6) - 2*pi;
end
if(output(i,6) < -pi)
output(i,6) = output(i,6) + 2*pi;
end
if(output(i,7) > (2*pi))
output(i,7) = output(i,7) - 2*pi;
end
if(output(i,7) < 0.0)
output(i,7) = output(i,7) + 2*pi;
end
end