76 lines
1.5 KiB
Text
76 lines
1.5 KiB
Text
|
% 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
|