Skip to content

Commit

Permalink
Delete comment
Browse files Browse the repository at this point in the history
  • Loading branch information
@trinhgiahuy
trinhgiahuy authored Oct 23, 2022
1 parent 1ba63ad commit e314a44
Show file tree
Hide file tree
Showing 3 changed files with 10 additions and 352 deletions.
243 changes: 2 additions & 241 deletions 1_measure_files/cell_array_parser_vn200.m
View file
Original file line number Diff line number Diff line change
@@ -1,10 +1,4 @@
function data = cell_array_parser_vn200(fileID, vn200)
% creates a nested structure from a cell array
%fileID='2016-10-16-20-57-38.txt';
%fileID='2016-11-03-16-22-42.txt';
%fileID='/Users/heikki/Projektit/tut/dualimu.txt';
% load the data in form of cell array
%data_cell=importdata(fileID);

if ~exist('vn200', 'var')
vn200 = false;
Expand Down Expand Up @@ -38,259 +32,26 @@
end
end

% most data to arrays
% gps_seq = table2array(T(:,1));
% gps_stamp = table2array(T(:,2));
% imu_seq = table2array(T(:,3));
% ang_vel1 = table2array(T(:,7));
% ang_vel2 = table2array(T(:,8));
% ang_vel3 = table2array(T(:,9));
% quat1 = table2array(T(:,10));
% quat2 = table2array(T(:,11));
% quat3 = table2array(T(:,12));
% quat4 = table2array(T(:,13));
% acc1 = table2array(T(:,14));
% acc2 = table2array(T(:,15));
% acc3 = table2array(T(:,16));
% pos1 = table2array(T(:,28+of));
% pos2 = table2array(T(:,29+of));
% pos3 = table2array(T(:,30+of));
% imu_on = table2array(T(:,26+of));
% id = table2array(T(:,32+of));
%
% gps_itow = table2array(T(:,17));
% pozyx_seq = table2array(T(:,27+of));
% lat = table2array(T(:,4));
% lon = table2array(T(:,5));
% alt = table2array(T(:,6));
% grnd_speed = 1e-2.*table2array(T(:,22));
% hdg = 1e-5.*table2array(T(:,23));
% cov_speed = 1e-2.*table2array(T(:,24));
% cov_hdg = 1e-5.*table2array(T(:,25));
%
% uwb = table2array(T(:,31+of));

if (vn200)
v_n = table2array(T(:,18));
v_e = table2array(T(:,19));
v_d = table2array(T(:,20));


% gpstime = table2array(T(:,26));
% dtime = table2array(T(:,27));
% dtheta_x = table2array(T(:,28));
% dtheta_y = table2array(T(:,29));
% dtheta_z = table2array(T(:,30));
% dvel_x = table2array(T(:,31));
% dvel_y = table2array(T(:,32));
% dvel_z = table2array(T(:,33));
% if (of > 8)
% gpsvel_n = table2array(T(:,34));
% gpsvel_e = table2array(T(:,35));
% gpsvel_d = table2array(T(:,36));
% gpslat = table2array(T(:,37));
% gpslon = table2array(T(:,38));
% gpsalt = table2array(T(:,39));
% gpsfix = table2array(T(:,40));
% end

else
v_n = 1e-2.*table2array(T(:,18));
v_e = 1e-2.*table2array(T(:,19));
v_d = 1e-2.*table2array(T(:,20));
end

% last = NaN;
% for k=1:length(imu_seq)
% if (~isnan(last))
% imuseq = imu_seq(k);
% if (~isnan(imuseq))
% while (abs(imuseq-last) > 100)
% imuseq = imuseq + 65536;
% end
% imu_seq(k) = imuseq;
% last = imuseq;
% else
% imu_seq(k) = NaN;
% end
% else
% last = imu_seq(k);
% %imu_seq(k) = imu_seq(k);
% end
% end
%
% [xData, yData] = prepareCurveData( imu_seq(:), gps_stamp(:) );

% Set up fittype and options.
ft = fittype( 'poly1' );

% Fit model to data.
% [mtifit, ~] = fit( xData, yData, ft );
%
% mtits = imu_seq(:).*mtifit.p1+mtifit.p2-0.015;

% for n=1:(size(T,2)-(32+of))/12
%
% tmp = table2array(T(:,[33+of+(n-1)*12,35+of+(n-1)*12:44+of+(n-1)*12]));
% %mtw(n).id2 = table2array(T(:,34+(n-1)*12));
% %mtw(n).seq = table2array(T(:,33+(n-1)*12));
% %mtw(n).ang_vel(1) = table2array(T(:,35+(n-1)*12));
% %mtw(n).ang_vel(2) = table2array(T(:,36+(n-1)*12));
% %mtw(n).ang_vel(3) = table2array(T(:,37+(n-1)*12));
% %mtw(n).quat(1) = table2array(T(:,38+(n-1)*12));
% %mtw(n).quat(2) = table2array(T(:,39+(n-1)*12));
% %mtw(n).quat(3) = table2array(T(:,40+(n-1)*12));
% %mtw(n).quat(4) = table2array(T(:,41+(n-1)*12));
% %mtw(n).acc(1) = table2array(T(:,42+(n-1)*12));
% %mtw(n).acc(2) = table2array(T(:,43+(n-1)*12));
% %mtw(n).acc(3) = table2array(T(:,44+(n-1)*12));
% mtw(:,:,n) = tmp;
%
% last = NaN;
% for k=1:size(mtw,1)
% if (~isnan(last))
% imuseq = mtw(k,1,n);
% if (~isnan(imuseq))
% while (abs(imuseq-last) > 100 && imuseq < last)
% imuseq = imuseq + 65536;
% end
% mtw(k,1,n) = imuseq;
% last = imuseq;
% else
% mtw(k,1,n) = NaN;
% end
% else
% last = mtw(k,1,n);
% %mtw(k,1,n) = imu_seq(k);
% end
% end
%
% [xData, yData] = prepareCurveData( mtw(:,1,n), gps_stamp(:) );
%
% % Fit model to data.
% [mtwfit, ~] = fit( xData, yData, ft );
%
% mtwts(n,:) = mtw(:,1,n).*mtwfit.p1+mtwfit.p2;
% end


uwbtags = containers.Map;

% number of rows in the table max(size(T))
for k=1:max(size(T))
% place data to struct


% if (vn200)
% data(k).time.gpstime = gpstime(k);
% data(k).d.time = dtime(k);
% data(k).d.theta(1) = dtheta_x(k);
% data(k).d.theta(2) = dtheta_y(k);
% data(k).d.theta(3) = dtheta_z(k);
% data(k).d.vel(1) = dvel_x(k);
% data(k).d.vel(2) = dvel_y(k);
% data(k).d.vel(3) = dvel_z(k);
% if (of > 8)
% data(k).gps.raw_v_n = gpsvel_n(k);
% data(k).gps.raw_v_e = gpsvel_e(k);
% data(k).gps.raw_v_d = gpsvel_d(k);
% data(k).gps.raw_lat = gpslat(k);
% data(k).gps.raw_lon = gpslon(k);
% data(k).gps.raw_alt = gpsalt(k);
% data(k).gps.fix = gpsfix(k);
% end
% end

% data(k).time.gps_seq = gps_seq(k);
% data(k).time.ros_stamp = gps_stamp(k);
% data(k).time.imu_seq = imu_seq(k);
% data(k).time.gps_itow = gps_itow(k);
% data(k).time.pozyx_seq = pozyx_seq(k);
% data(k).gps.lat = lat(k);
% data(k).gps.lon = lon(k);
% data(k).gps.alt = alt(k);

data(k).gps.v_n = v_n(k);
data(k).gps.v_e = v_e(k);
data(k).gps.v_d = v_d(k);

% data(k).gps.grnd_speed = grnd_speed(k);
% data(k).gps.hdg = hdg(k);
% data(k).gps.cov_speed = cov_speed(k);
% data(k).gps.cov_hdg = cov_hdg(k);
% data(k).imu.ang_vel(1) = ang_vel1(k);
% data(k).imu.ang_vel(2) = ang_vel2(k);
% data(k).imu.ang_vel(3) = ang_vel3(k);
% data(k).imu.quat(1) = quat1(k); %q_1 pr q_2
% data(k).imu.quat(2) = quat2(k); %q_3
% data(k).imu.quat(3) = quat3(k); %q_1 pr q_2
% data(k).imu.quat(4) = quat4(k); %q_0
% data(k).imu.acc(1) = acc1(k); % A_x
% data(k).imu.acc(2) = acc2(k); % A_y
% data(k).imu.acc(3) = acc3(k); % A_z

% data(k).imu.ts = mtits(k);
%
% data(k).pozyx.pos(1) = pos1(k);
% data(k).pozyx.pos(2) = pos2(k);
% data(k).pozyx.pos(3) = pos3(k);
% data(k).pozyx.imu_on = imu_on(k);

% data(k).imu.id = id(k);

% if (strcmp(char(uwb(k)),'NaN') == 0)
%
% ranges = strsplit(char(uwb(k)), '|');
% for n=1:size(ranges,2)-1
% fields = strsplit(char(ranges(n)), ',');
% vals = struct();
% for i=1:size(fields,2)
% %fields(i)
% parts = strsplit(char(fields(i)), '=');
% if (strcmp(char(parts(1)), 's') || strcmp(char(parts(1)), 't'))
% vals.(char(parts(1))) = parts(2);
% else
% vals.(char(parts(1))) = str2double(char(parts(2)));
% end
% end
% if (isKey(uwbtags, vals.('t')))
% data(k).uwb.tag(uwbtags(char(vals.('t')))) = vals;
% else
% key = uwbtags.size(1)+1;
% uwbtags(char(vals.('t'))) = key;
% data(k).uwb.tag(key) = vals;
% end
% end
% end

% for each additional mtw

% for n=1:(size(T,2)-(32+of))/12
%
% data(k).mtw(n).id2 = table2array(T(k,34+(n-1)*12));
% data(k).mtw(n).seq = mtw(k,1,n);
% data(k).mtw(n).ang_vel(1) = mtw(k,2,n);
% data(k).mtw(n).ang_vel(2) = mtw(k,3,n);
% data(k).mtw(n).ang_vel(3) = mtw(k,4,n);
% data(k).mtw(n).quat(1) = mtw(k,5,n);
% data(k).mtw(n).quat(2) = mtw(k,6,n);
% data(k).mtw(n).quat(3) = mtw(k,7,n);
% data(k).mtw(n).quat(4) = mtw(k,8,n);
% data(k).mtw(n).acc(1) = mtw(k,9,n);
% data(k).mtw(n).acc(2) = mtw(k,10,n);
% data(k).mtw(n).acc(3) = mtw(k,11,n);
%
% data(k).mtw(n).ts = mtwts(n,k);
% end


end

toc

end
% formatSpec = '%s %d %2.1f %s\n';
% fileID = fopen('Oct20_celldata.dat','w');
% [nrows,ncols] = size(data);
% for row = 1:nrows
% fprintf(fileID,formatSpec,data{row,:});
% end
end
104 changes: 3 additions & 101 deletions 1_measure_files/features_OK.m
View file
Original file line number Diff line number Diff line change
@@ -1,117 +1,19 @@
function [] = features_OK(data,subjectId)
% % Walking/running data processing
% data = cell_array_parser_vn200(fileID,'vn200');
% data = cell_array_parser_vn200('2017-11-15-11-39-39-gpsreferenced.txt','vn200')
% data(k): time, d, gps, imu, pozyx
%t0=200000;
t0=1;
tend=max(size(data));
%rad2deg=180/pi;
%tend=250000;
%fsmgaitv2.m

for k=t0:tend
%k=i-t0+1;

V_n(k)=data(k).gps.v_n;
V_e(k)=data(k).gps.v_e;
V_d(k)=data(k).gps.v_d;

% IMU horizontal speed
V_lon(k) = sqrt(V_n(k)^2 + V_e(k)^2);

% V_gps_n(k)=data(k).gps.raw_v_n;
% V_gps_e(k)=data(k).gps.raw_v_e;
% V_gps_d(k)=data(k).gps.raw_v_d;

%Speed(k)=sqrt(V_n(k)^2+V_e(k)^2);
% %Hdg(k)=atan2(V_n(k),V_e(k));
% wx(k)=data(k).imu.ang_vel(1); % rad/sec
% wy(k)=data(k).imu.ang_vel(2); % rad/sec
% wz(k)=data(k).imu.ang_vel(3); % rad/sec

%q0(k)=data(k).imu.quat(1);
%q1(k)=data(k).imu.quat(2);
%q2(k)=data(k).imu.quat(3);
%q3(k)=data(k).imu.quat(4);

%acc_n(k)=data(k).imu.acc(1);
%acc_e(k)=data(k).imu.acc(2);
%acc_d(k)=data(k).imu.acc(3);

% delVx(k) = data(k).d.vel(1);
% delVy(k) = data(k).d.vel(2);
% delVz(k) = data(k).d.vel(3);
% delTx(k) = data(k).d.theta(1);
% delTy(k) = data(k).d.theta(2);
% delTz(k) = data(k).d.theta(3);

% Ground track
%GrndTrack(k)=rad2deg*atan2(V_e(k),V_n(k));
% Lateral velocity constraint V_y = 0 (straight line running?)
%V_lat(k) = V_e(k)*cosd(GrndTrack(k)) - V_n(k)*sind(GrndTrack(k));


% GPS horizontal speed
%V_gps_lon(k) = sqrt(V_gps_n(k)^2 + V_gps_e(k)^2);

% Longitudinal acceleration
%A_lon(k) = sqrt(acc_n(k)^2 + acc_e(k)^2);
% Euler angle computation
%dcm_xsens = quat2dcm( [q0(k) q1(k) q2(k) q3(k)] );
%[yaw(k) pitch(k) roll(k)] = quat2angle( [q3(k) q0(k) q1(k) q2(k)] );
% [yaw(k) pitch(k) roll(k)] = quat2angle( [q0(k) q1(k) q2(k) q3(k)] );
%V_ref(k,:)= data(k).d.vel;
end

subjOutputDir=['O',num2str(subjectId),'_out\'];

save([subjOutputDir,'vel_imu.mat'],'V_d','V_lon');

%%save([subjOutputDir,'euler.mat'], 'yaw', 'pitch', 'roll');
%%save([subjOutputDir,'acc.mat'], 'acc_n', 'acc_e','acc_d','A_lon');

% Necessary: V_d, V_lon

% save('ang_vel.mat', 'wx', 'wy', 'wz')
% save('quat.mat', 'q0', 'q1', 'q2','q3')
% save('delV.mat', 'delVx', 'delVy', 'delVz')
% save('delT.mat', 'delTx', 'delTy', 'delTz')

% figure(1)
% plot(t0:tend,A_lon(t0:tend))
% hold
% plot(t0:tend,acc_d(t0:tend))
% title('Longitudinal and vertical acceleration')
% xlabel('time, *0.005 sec')
% ylabel('acceleration, m/s^2')
% legend('longitudinal acceleration','vertical acceleration')
% grid
%
% figure(2)
% plot(t0:tend,V_lon(t0:tend))
% hold
% plot(t0:tend,V_d(t0:tend))
% plot(t0:tend,V_gps_lon(t0:tend),':')
% title('Longitudinal and vertical velocity')
% xlabel('time, *0.005 sec')
% ylabel('Longitudinal and vertical velocity')
% legend('Longitudinal velocity INS', 'Vertical velocity', 'Longitudinal velocity GPS')
% grid
%
% figure(3)
% plot(t0:tend,GrndTrack(t0:tend))
% title('Ground track')
% xlabel('time, *0.005 sec')
% ylabel('Ground track, deg')
% grid
%
% figure(4)
% plot(t0:tend,rad2deg*yaw(t0:tend), t0:tend,rad2deg*pitch(t0:tend), t0:tend,rad2deg*roll(t0:tend))
% title('Upper body posture')
% xlabel('time, *0.005 sec')
% ylabel('yaw, pitch, roll, deg')
% legend('yaw', 'pitch', 'roll')
% grid
%
% figure(5)
% plot(t0:tend,V_ref(t0:tend,1), t0:tend,V_ref(t0:tend,2), t0:tend,V_ref(t0:tend,3))

end
Loading

0 comments on commit e314a44

Please sign in to comment.