398 lines
16 KiB
Matlab
398 lines
16 KiB
Matlab
%using autocorrelation to estimate the current bmp within some fixed window
|
|
|
|
%load sensor files
|
|
%files = dir(fullfile('../../measurements/2017.06/mSensor/', '*.csv'));
|
|
%files = natsortfiles(dir(fullfile('../../measurements/2017.06/lgWear/', '*.csv')));
|
|
%files = dir(fullfile('../../measurements/wearR/', '*.csv'));
|
|
%files = dir(fullfile('../../measurements/peter_failed/', '*.csv'));
|
|
%files = dir(fullfile('../../measurements/2018.06/manfred/LGWatchR/', '*.csv'));
|
|
%files = dir(fullfile('../../measurements/2018.06/peter/Huawai/', '*.csv'));
|
|
%files = dir(fullfile('../../measurements/2018.06/peter/mSensor/', '*.csv'));
|
|
files = dir(fullfile('../../measurements/2018.06/frank/mSensorTest/', '*.csv'));
|
|
%files = dir(fullfile('../../measurements/2018.06/leon/mSensor/', '*.csv'));
|
|
|
|
%files_sorted = natsortfiles({files.name});
|
|
for file = files'
|
|
|
|
filename = [file.folder '/' file.name];
|
|
measurements = dlmread(filename, ';', 3, 0);
|
|
|
|
%load ground truth file
|
|
fid = fopen(filename);
|
|
fgetl(fid);
|
|
Str = fgetl(fid);
|
|
Key = 'Metronom: ';
|
|
Index = strfind(Str, Key);
|
|
gtDataRaw = sscanf(Str(Index(1) + length(Key):end), '%g', 1);
|
|
gtData = [];
|
|
gtFile = [];
|
|
if(isempty(gtDataRaw))
|
|
gtFile = extractAfter(Str, Key);
|
|
gtFile = strcat('../../measurements/2018.06/gt_toni/', gtFile);
|
|
f = fopen(gtFile);
|
|
gtDataRaw = textscan(f, '%f %s', 'Delimiter', ' ');
|
|
fclose(f);
|
|
[~,~,~,hours,minutes,seconds] = datevec(gtDataRaw{2}, 'HH:MM:SS.FFF');
|
|
gtData(:,1) = 1000*(60*minutes + seconds); %we do not use hours!
|
|
gtData(:,2) = gtDataRaw{1};
|
|
else
|
|
gtData = gtDataRaw;
|
|
end
|
|
|
|
%draw the raw acc data
|
|
m_idx = [];
|
|
m_idx = (measurements(:,2)==3); %Android App: 10, Sensor: 3, Normal Data: 2
|
|
m = measurements(m_idx, :);
|
|
|
|
%Interpolate to generate a constant sample rate to 250hz (4ms per sample)
|
|
sample_rate_ms = 4;%ms
|
|
[~, m_unique_idx] = unique(m(:,1)); %matlab requirs unique timestamps for interp
|
|
m = m(m_unique_idx, :);
|
|
t = m(:,1); %timestamps
|
|
t_interp = t(1):sample_rate_ms:t(length(t));
|
|
m_interp = interp1(t,m(:,3:5),t_interp);
|
|
|
|
%put all together again
|
|
m = [t_interp', t_interp', m_interp];
|
|
|
|
% figure(1);
|
|
% plot(m(:,1),m(:,3)) %x
|
|
% legend("x", "location", "eastoutside");
|
|
%
|
|
% figure(2);
|
|
% plot(m(:,1),m(:,4)) %yt
|
|
% legend("y", "location", "eastoutside");
|
|
%
|
|
% figure(3);
|
|
% plot(m(:,1),m(:,5)) %z
|
|
% legend("z", "location", "eastoutside");
|
|
%
|
|
% %magnitude
|
|
magnitude = sqrt(sum(m(:,3:5).^2,2));
|
|
% figure(5);
|
|
% plot(m(:,1), magnitude);
|
|
% legend("magnitude", "location", "eastoutside");
|
|
|
|
%waitforbuttonpress();
|
|
|
|
%save timestamps
|
|
timestamps = m(:,1);
|
|
data = m(:,3); %only z
|
|
|
|
%TODO: Different window sizes for periods under 16.3 s
|
|
window_size = 1024; %about 2 seconds using 2000hz, 16.3 s using 250hz
|
|
overlap = 256;
|
|
bpm_per_window_ms = [];
|
|
bpm_per_window = [];
|
|
bpm_3D = [];
|
|
ms_3D = [];
|
|
|
|
gtIdx = 1;
|
|
gtError_3D = [];
|
|
gtError_1D = [];
|
|
|
|
for i = window_size+1:1:length(data)
|
|
|
|
%wait until window is filled with new data
|
|
if(mod(i,overlap) == 0)
|
|
|
|
%set cur ground truth
|
|
if(length(gtData) > 1)
|
|
curTimestamp = timestamps(i) - timestamps(1);
|
|
while(curTimestamp > gtData(gtIdx,1) && gtIdx < length(gtData))
|
|
curGtBpm = gtData(gtIdx,2);
|
|
gtIdx = gtIdx + 1;
|
|
end
|
|
else
|
|
curGtBpm = gtData;
|
|
end
|
|
|
|
|
|
%measure periodicity of window and use axis with best periodicity
|
|
[corr_x, lag_x] = xcov(m(i-window_size:i,3), (window_size/2), "coeff");
|
|
[corr_y, lag_y] = xcov(m(i-window_size:i,4), (window_size/2), "coeff");
|
|
[corr_z, lag_z] = xcov(m(i-window_size:i,5), (window_size/2), "coeff");
|
|
|
|
%magnitude
|
|
[corr_mag, lag_mag] = xcov(magnitude(i-window_size:i), (window_size/2), "coeff");
|
|
|
|
%TODO: stichwort spatial autocorrelation
|
|
%figure(77);
|
|
%scatter3(timestamps(i-window_size:i), m(i-window_size:i,4), m(i-window_size:i,5));
|
|
|
|
%distanz zwischen den vektoren nehmen und in eine normale autocorrelation zu packen
|
|
%aufpassen wegen der norm, dass die richtung quasi nicht verloren geht.
|
|
%https://en.wikipedia.org/wiki/Lp_space
|
|
[corr_3D, lag_3D] = distCorr(m(i-window_size:i, 3:5), (window_size/2));
|
|
|
|
corr_x_pos = corr_x;
|
|
corr_y_pos = corr_y;
|
|
corr_z_pos = corr_z;
|
|
corr_mag_pos = corr_mag;
|
|
|
|
corr_x_pos(corr_x_pos<0)=0;
|
|
corr_y_pos(corr_y_pos<0)=0;
|
|
corr_z_pos(corr_z_pos<0)=0;
|
|
corr_mag_pos(corr_mag_pos<0)=0;
|
|
|
|
[peak_x, idx_x_raw] = findpeaks(corr_x_pos, 'MinPeakHeight', 0.1,'MinPeakDistance', 50, 'MinPeakProminence', 0.1);
|
|
[peak_y, idx_y_raw] = findpeaks(corr_y_pos, 'MinPeakHeight', 0.1,'MinPeakDistance', 50, 'MinPeakProminence', 0.1);
|
|
[peak_z, idx_z_raw] = findpeaks(corr_z_pos, 'MinPeakHeight', 0.1,'MinPeakDistance', 50, 'MinPeakProminence', 0.1);
|
|
[peak_mag, idx_mag_raw] = findpeaks(corr_mag_pos, 'MinPeakHeight', 0.1,'MinPeakDistance', 50, 'MinPeakProminence', 0.1);
|
|
[peak_3D, idx_3D_raw] = findpeaks(corr_3D, 'MinPeakHeight', 0.1,'MinPeakDistance', 50, 'MinPeakProminence', 0.1);
|
|
|
|
|
|
idx_x_raw = sort(idx_x_raw);
|
|
idx_y_raw = sort(idx_y_raw);
|
|
idx_z_raw = sort(idx_z_raw);
|
|
idx_mag_raw = sort(idx_mag_raw);
|
|
idx_3D_raw = sort(idx_3D_raw);
|
|
|
|
idx_x = findFalseDetectedPeaks(idx_x_raw, lag_x, corr_x);
|
|
idx_y = findFalseDetectedPeaks(idx_y_raw, lag_y, corr_y);
|
|
idx_z = findFalseDetectedPeaks(idx_z_raw, lag_z, corr_z);
|
|
idx_mag = findFalseDetectedPeaks(idx_mag_raw, lag_mag, corr_mag);
|
|
%idx_3D = findFalseDetectedPeaks(idx_3D_raw, lag_3D', corr_3D);
|
|
idx_3D = idx_3D_raw;
|
|
|
|
Dwindow = m(i-window_size:i,3);
|
|
Dwindow_mean_ts_diff = mean(diff(lag_3D(idx_3D) * sample_rate_ms)); %2.5 ms is the time between two samples at 400hz
|
|
Dwindow_mean_bpm = (60000 / (Dwindow_mean_ts_diff));
|
|
|
|
% figure(10);
|
|
% plot(lag_3D, corr_3D, lag_3D(idx_3D), corr_3D(idx_3D), 'r*', lag_3D(idx_3D_raw), corr_3D(idx_3D_raw), 'g*')
|
|
% hold ("on")
|
|
% m_label_ms = strcat(" mean ms: ", num2str(Dwindow_mean_ts_diff));
|
|
% m_label_bpm = strcat(" mean bpm: ", num2str(Dwindow_mean_bpm));
|
|
% title(strcat(" ", m_label_ms, " ", m_label_bpm));
|
|
% hold ("off");
|
|
|
|
Xwindow = m(i-window_size:i,3);
|
|
Xwindow_mean_ts_diff = mean(diff(lag_x(idx_x) * sample_rate_ms)); %2.5 ms is the time between two samples at 400hz
|
|
Xwindow_mean_bpm = (60000 / (Xwindow_mean_ts_diff));
|
|
|
|
% figure(11);
|
|
% plot(lag_x, corr_x, lag_x(idx_x), corr_x(idx_x), 'r*', lag_x(idx_x_raw), corr_x(idx_x_raw), 'g*') %z
|
|
% hold ("on")
|
|
% m_label_ms = strcat(" mean ms: ", num2str(Xwindow_mean_ts_diff));
|
|
% m_label_bpm = strcat(" mean bpm: ", num2str(Xwindow_mean_bpm));
|
|
% title(strcat(" ", m_label_ms, " ", m_label_bpm));
|
|
% hold ("off");
|
|
|
|
Ywindow = m(i-window_size:i,4);
|
|
Ywindow_mean_ts_diff = mean(diff(lag_y(idx_y) * sample_rate_ms));
|
|
Ywindow_mean_bpm = (60000 / (Ywindow_mean_ts_diff));
|
|
|
|
% figure(12);
|
|
% plot(lag_y, corr_y, lag_y(idx_y), corr_y(idx_y), 'r*', lag_y(idx_y_raw), corr_y(idx_y_raw), 'g*') %z
|
|
% hold ("on")
|
|
% m_label_ms = strcat(" mean ms: ", num2str(Ywindow_mean_ts_diff));
|
|
% m_label_bpm = strcat(" mean bpm: ", num2str(Ywindow_mean_bpm));
|
|
% title(strcat(" ", m_label_ms, " ", m_label_bpm));
|
|
% hold ("off");
|
|
|
|
Zwindow = m(i-window_size:i,5);
|
|
Zwindow_mean_ts_diff = mean(diff(lag_z(idx_z)* sample_rate_ms));
|
|
Zwindow_mean_bpm = (60000 / (Zwindow_mean_ts_diff));
|
|
|
|
% figure(13);
|
|
% plot(lag_z, corr_z, lag_z(idx_z), corr_z(idx_z), 'r*', lag_z(idx_z_raw), corr_z(idx_z_raw), 'g*') %z
|
|
% hold ("on")
|
|
% m_label_ms = strcat(" mean ms: ", num2str(Zwindow_mean_ts_diff));
|
|
% m_label_bpm = strcat(" mean bpm: ", num2str(Zwindow_mean_bpm));
|
|
% title(strcat(" ", m_label_ms, " ", m_label_bpm));
|
|
% hold ("off");
|
|
|
|
%magnitude
|
|
Mwindow = magnitude(i-window_size:i);
|
|
Mwindow_mean_ts_diff = mean(diff(lag_mag(idx_mag)* sample_rate_ms));
|
|
Mwindow_mean_bpm = (60000 / (Mwindow_mean_ts_diff));
|
|
|
|
% figure(14);
|
|
% plot(lag_mag, corr_mag, lag_mag(idx_mag), corr_mag(idx_mag), 'r*', lag_mag(idx_mag_raw), corr_mag(idx_mag_raw), 'g*') %z
|
|
% hold ("on")
|
|
% m_label_ms = strcat(" mean ms: ", num2str(Mwindow_mean_ts_diff));
|
|
% m_label_bpm = strcat(" mean bpm: ", num2str(Mwindow_mean_bpm));
|
|
% title(strcat(" ", m_label_ms, " ", m_label_bpm));
|
|
% hold ("off");
|
|
|
|
%breakpoints dummy for testing
|
|
if(length(idx_x) > length(idx_x_raw))
|
|
a = 0; %breakpointdummy
|
|
end
|
|
|
|
if(length(idx_y) > length(idx_y_raw))
|
|
a = 0; %breakpointdummy
|
|
end
|
|
|
|
if(length(idx_z) > length(idx_z_raw))
|
|
a = 0; %breakpointdummy
|
|
end
|
|
|
|
|
|
|
|
%Find the most proper axis. We use 3 quantities: mean of corr.
|
|
%value, sum of corr val. and number of peaks. Simple normalization
|
|
%to get the axis that fullfills the quantities the most.
|
|
idx_noZero_x = idx_x(lag_x(idx_x) ~= 0);
|
|
idx_noZero_y = idx_y(lag_x(idx_y) ~= 0);
|
|
idx_noZero_z = idx_z(lag_x(idx_z) ~= 0);
|
|
|
|
corr_mean_x = geomean(corr_x(idx_noZero_x(corr_x(idx_noZero_x)>0)));
|
|
corr_mean_y = geomean(corr_y(idx_noZero_y(corr_y(idx_noZero_y)>0)));
|
|
corr_mean_z = geomean(corr_z(idx_noZero_z(corr_z(idx_noZero_z)>0)));
|
|
|
|
corr_rms_x = rms(corr_x(idx_x(lag_x(idx_x) ~= 0)));
|
|
corr_rms_y = rms(corr_y(idx_y(lag_y(idx_y) ~= 0)));
|
|
corr_rms_z = rms(corr_z(idx_z(lag_z(idx_z) ~= 0)));
|
|
|
|
num_peaks_x = 1;%length(idx_x);
|
|
num_peaks_y = 1;%length(idx_y);
|
|
num_peaks_z = 1;%length(idx_z);
|
|
|
|
num_intersection_x = getNumberOfIntersections(corr_x, lag_x, 0.2);
|
|
num_intersection_y = getNumberOfIntersections(corr_y, lag_y, 0.2);
|
|
num_intersection_z = getNumberOfIntersections(corr_z, lag_z, 0.2);
|
|
|
|
quantity_matrix = [corr_mean_x corr_mean_y corr_mean_z;
|
|
corr_rms_x corr_rms_y corr_rms_z;
|
|
num_intersection_x num_intersection_y num_intersection_z];
|
|
|
|
quantity_matrix_percent(1,:) = quantity_matrix(1,:) ./ sum(quantity_matrix(1,:));
|
|
quantity_matrix_percent(2,:) = quantity_matrix(2,:) ./ sum(quantity_matrix(2,:));
|
|
quantity_matrix_percent(3,:) = quantity_matrix(3,:) ./ sum(quantity_matrix(3,:));
|
|
|
|
quantity_factors = sum(quantity_matrix_percent) / 3;
|
|
|
|
%TODO: Wenn ein quantity wert NaN ist, sind alle NaN...
|
|
quantity_x = quantity_factors(1);
|
|
quantity_y = quantity_factors(2);
|
|
quantity_z = quantity_factors(3);
|
|
|
|
%choose axis with sum(corr) nearest to 0
|
|
%{
|
|
corr_sum_xyz = [sum(corr_x) sum(corr_y) sum(corr_z)];
|
|
[~,idx_nearest_zero] = min(abs(corr_sum_xyz));
|
|
|
|
if(idx_nearest_zero == 1)
|
|
window_mean_ts_diff = Xwindow_mean_ts_diff;
|
|
window_mean_bpm = Xwindow_mean_bpm;
|
|
elseif(idx_nearest_zero == 2)
|
|
window_mean_ts_diff = Ywindow_mean_ts_diff;
|
|
window_mean_bpm = Ywindow_mean_bpm;
|
|
else
|
|
window_mean_ts_diff = Zwindow_mean_ts_diff;
|
|
window_mean_bpm = Zwindow_mean_bpm;
|
|
end
|
|
%}
|
|
|
|
%quantity_x = num_intersection_x;
|
|
%quantity_y = num_intersection_y;
|
|
%quantity_z = num_intersection_z;
|
|
|
|
if(quantity_x > quantity_y && quantity_x > quantity_z)
|
|
window_mean_ts_diff = Xwindow_mean_ts_diff;
|
|
window_mean_bpm = Xwindow_mean_bpm;
|
|
elseif(quantity_y > quantity_z)
|
|
window_mean_ts_diff = Ywindow_mean_ts_diff;
|
|
window_mean_bpm = Ywindow_mean_bpm;
|
|
else
|
|
window_mean_ts_diff = Zwindow_mean_ts_diff;
|
|
window_mean_bpm = Zwindow_mean_bpm;
|
|
end
|
|
|
|
|
|
if(isnan(window_mean_ts_diff) || isnan(window_mean_bpm))
|
|
%do nothing
|
|
else
|
|
gtError_1D = [gtError_1D, abs(window_mean_bpm - curGtBpm)];
|
|
bpm_per_window_ms = [bpm_per_window_ms, window_mean_ts_diff];
|
|
bpm_per_window = [bpm_per_window, window_mean_bpm];
|
|
end
|
|
|
|
|
|
%3D mean
|
|
if(isnan(Dwindow_mean_bpm))
|
|
%nothing
|
|
else
|
|
gtError_3D = [gtError_3D, abs(Dwindow_mean_bpm - curGtBpm)];
|
|
bpm_3D = [bpm_3D, Dwindow_mean_bpm];
|
|
ms_3D = [ms_3D, Dwindow_mean_ts_diff];
|
|
end
|
|
|
|
%TODO: if correlation value is lower then a treshhold, we are not conducting TODO: change to a real classification instead of a treshhold.
|
|
|
|
end
|
|
end
|
|
|
|
%TODO: smooth the results using a moving avg or 1d kalman filter.(transition for kalman could be adding the last measured value)
|
|
|
|
%remove the first 40% of the results, due to starting delays while recording.
|
|
%number_to_remove = round(abs(0.1 * length(bpm_per_window_ms)));
|
|
%num_all = length(bpm_per_window_ms);
|
|
%bpm_per_window_ms = bpm_per_window_ms(number_to_remove:num_all);
|
|
%bpm_per_window = bpm_per_window(number_to_remove:num_all);
|
|
|
|
mean_final_ms = mean(bpm_per_window_ms);
|
|
std_final_ms = std(bpm_per_window_ms);
|
|
|
|
mean_final_bpm = mean(bpm_per_window);
|
|
std_final_bpm = std(bpm_per_window);
|
|
|
|
mean_final_error_1D = mean(gtError_1D);
|
|
std_final_error_1D = std(gtError_1D);
|
|
|
|
mean_final_ms_3D = mean(ms_3D);
|
|
std_final_ms_3D = std(ms_3D);
|
|
|
|
mean_final_bpm_3D = mean(bpm_3D);
|
|
std_final_bpm_3D = std(bpm_3D);
|
|
|
|
mean_final_error_3D = mean(gtError_3D);
|
|
std_final_error_3D = std(gtError_3D);
|
|
|
|
fprintf('%s: mean = %f bpm (%f bpm) stddev = %f bpm (%f bpm) --- 1D\n', strrep(regexprep(filename,'^.*recording_',''),'.txt',''), mean_final_error_1D, mean_final_bpm, std_final_error_1D, std_final_bpm);
|
|
fprintf('%s: mean = %f bpm (%f bpm) stddev = %f bpm (%f bpm) --- 3D\n', strrep(regexprep(filename,'^.*recording_',''),'.txt',''), mean_final_error_3D, mean_final_bpm_3D, std_final_error_3D, std_final_bpm_3D);
|
|
|
|
end
|
|
|
|
|
|
% %1D fft - nicht so der brüller
|
|
% z_fft = fft(m(i-window_size:i,5));
|
|
% L = length(z_fft);
|
|
% Fs = 250;
|
|
% P2 = abs(z_fft/L);
|
|
% P1 = P2(1:L/2+1);
|
|
% P1(2:end-1) = 2*P1(2:end-1);
|
|
% f = Fs*(0:(L/2))/L; %nyquist frequence
|
|
%
|
|
% figure(66);
|
|
% plot(f, P1);
|
|
%
|
|
% %3D fft
|
|
% m_3D = m(i-window_size:i, 3);
|
|
% m_3D(:,:,2) = m(i-window_size:i, 4);
|
|
% m_3D(:,:,3) = m(i-window_size:i, 5);
|
|
%
|
|
% fft_3D = fftn(m_3D);
|
|
%
|
|
% %2D fft
|
|
% fft_xy = fft2(m(i-window_size:i, 3:4));
|
|
% fft_yz = fft2(m(i-window_size:i, 3:4));
|
|
%
|
|
% fft_test = fft(m(i-window_size:i, 3:5),[],2);
|
|
%
|
|
% figure(60);
|
|
% imagesc(abs(fftshift(fft_test)));
|
|
%
|
|
% figure(61);
|
|
% imagesc(abs(fftshift(fft_xy)));
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|