elo3

clear all; close all;
%refactor w pythonie
load("accel.txt")
ax=accel(:,1);
ay=accel(:,2);
az=accel(:,3);
time = accel(:,4) ./1000;
% clear all;
%close all;
%clc;

% data = readtable('accel_logs.csv');

% ax = table2array(data(:,1));
% % ay = table2array(data(:,2));
% az = table2array(data(:,2));


% for i = 1:(length(ax))
%    ax(i) = strrep(ax(i), ',' , '.');
%    ay(i) = strrep(ay(i), ',' , '.');
%    az(i) = strrep(az(i), ',' , '.');
% end

% ax = str2double(ax);
% ay = str2double(ay);
% az = str2double(az);

% time = table2array(data(:,4)) ./ 1000;
%time = table2array(data(:,4));


Vx = zeros(5589,1);
Vy = zeros(5589,1);
Vz = zeros(5589,1);

for i = 1:(length(time)-1)
   Vx(i) = ax(i+1) * (time(i+1) - time(i));
   Vy(i) = ay(i+1) * (time(i+1) - time(i));
   Vz(i) = az(i+1) * (time(i+1) - time(i));
end

%plot3(Vx,Vy,Vz)

% temp_Vxy = zeros(5589,1);
% for i = 1: length(Vx)
%      temp_Vxy(i) = sqrt((Vx(i))^2 + (Vy(i))^2);

% end

V = zeros(5589,1);
% for i = 1: length(Vx)
%      V(i) = sqrt((temp_Vxy(i))^2 + (Vz(i))^2);
% end

for i = 1:length(V)
    V(i) = norm([Vx(i), Vy(i), Vz(i)]);
end
plot(time,V)

x = zeros(5589,1);
z = zeros(5589,1);
y = zeros(5589,1);

for i = 1:(length(time)-1)
   x(i) = Vx(i+1) * (time(i+1) - time(i));
   y(i) = Vy(i+1) * (time(i+1) - time(i));
   z(i) = Vz(i+1) * (time(i+1) - time(i));
end

dist = zeros(5589,1);
for i = 1:length(dist)
    dist(i) = norm([x(i), y(i), z(i)]);
end

trapz(time,Vx)
trapz(time,Vy)
trapz(time,Vz)
%plot(time,x);
%plot(time,y);
%plot(time,z);

totalDistance = cumsum(dist);

sum(dist)

plot(time,totalDistance)

totalDistance(end)

t0 = 1;
t1 = 2;

%for i

%end