exp_231205_scriptRectRot.m

Go to the documentation of this file.

%
% For LICENSE / TERMS OF USE, see the file LICENSE in the root directory of the repository.
 
clear; close all; clc;
 
%% add functions path and get data path
addpath(genpath('../../functions'));
datapath = getDataPath();
datapath = fullfile(datapath, "self_captured/231205_rotating_around_a_rectangle");
 
%% load data
data(1) = jumpReadData(fullfile(datapath, "sensor1", "S-24-A-13026_001.BIN"));
data(2) = jumpReadData(fullfile(datapath, "sensor2", "S-24-A-13036_001.BIN"));
data(3) = jumpReadData(fullfile(datapath, "sensor3", "S-24-A-13169_000.BIN"));
 
%% cut start and end
g_thres = 0.2;
for i=1:height(data)
    % first possible movment is 25 s after start
    [~, idx_start] = min(abs(data(i).tt.t - (data(i).tt.t(1) + seconds(25))));
 
    % find the actual movement
    norm_acc = vecnorm(data(i).tt.acc(idx_start:end, :),2,2);
    idx_start = idx_start + ...
        find(abs(norm_acc - mean(norm_acc,"all")) > g_thres, 1);
 
    % find the index 25s before
    [~, idx_start] = min(abs(data(i).tt.t - data(i).tt.t(idx_start) + seconds(25)));
 
    % define the last index (start index plus datarate times 153s [= experiment duration]
    idx_end = idx_start + data(i).mpu_rate * 153;
 
    % cut the timetable
    data(i).tt = data(i).tt(idx_start:idx_end,:);
end
 
%% Inertial Orientation Estimation with VQF
vqf = MyVqf();
IOE.vqf = vqf.filterOffline(data(2).tt.acc, data(2).tt.gyr, data(2).tt.mag);
 
%% animate
animateQuat(IOE.vqf.quat9D, "speedup",4);
 
figure;
stem(IOE.vqf.restDetected)