diff --git a/commonFunctions/sensors/NoiseAnalysis/Functions/NoiseAnalysis_pink.m b/commonFunctions/sensors/NoiseAnalysis/Functions/NoiseAnalysis_pink.m new file mode 100644 index 0000000000000000000000000000000000000000..a1fd16d2606398e88a0f156b9db0a8e8169bf07f --- /dev/null +++ b/commonFunctions/sensors/NoiseAnalysis/Functions/NoiseAnalysis_pink.m @@ -0,0 +1,155 @@ +function [sensor_vect] = NoiseAnalysis_pink(sensor_vect, sensor_number, track, plot_val) + +fprintf("Sensor analyzed: %s, track: %d and noise type: %s\n\n", sensor_vect(sensor_number).name, track, sensor_vect(sensor_number).noise_type); + +name = sensor_vect(sensor_number).name; +fs = sensor_vect(sensor_number).fs; + +b_left = sensor_vect(sensor_number).bounds(1); +b_right = sensor_vect(sensor_number).bounds(2); + + +%% Magic values +temp_val = 0.1; +butterOrder = 1; +fcut = 0.3; +white_variance = 15; + + +%% Noise extraction +A = importdata(name).data; +signal = A(:,track); + +cut_left = ceil(length(signal)*b_left); +cut_right = ceil(length(signal)*b_right); +signal = signal(cut_left:cut_right); + +filtered_signal = movmean(signal, fs); + +% Plots +bound = 15; +y_min = min(min(signal), min(filtered_signal)); +y_max = max(max(signal), max(filtered_signal)); +if plot_val + figure(2) + subplot(1,2,1), hold on, grid on, title("Unfiltered signal"), xlabel("Samples [-]"), ylabel("Value"), ylim([y_min y_max]) + plot(signal) + subplot(1,2,2), hold on, grid on, title("Filtered signal"), xlabel("Samples [-]"), ylabel("Value"), ylim([y_min y_max]) + plot(filtered_signal(bound:end-bound)) % removed boundaries +end + +% Noise extraction +noise = signal(bound:end-bound) - filtered_signal(bound:end-bound); + +% fft of noise +tf = length(noise)/fs; +t = 0:1/fs:tf; +[G_n,f] = dft(noise,t); + + +%% Peaks identification & pink noise creator +max_height = round(max(abs(G_n))*temp_val); + +if plot_val + figure(3) + subplot(1,2,1), hold on, hold on, grid on, title("Real noise"), xlabel("f [Hz]"), ylabel("|fft(X)|") + plot(f,abs(G_n)) + plot([0 max(f)], [max_height max_height], '.--') +end + +peaks_count = 0; +peaks_vect_val = []; +peaks_vect_f = []; +peaks_last = 0; +for ii = round(length(G_n)/2) : length(G_n)-1 + if abs(G_n(ii)) > max_height && abs(G_n(ii+1)) < abs(G_n(ii)) && ii-peaks_last>1 + peaks_count = peaks_count + 1; + peaks_vect_val = [peaks_vect_val abs(G_n(ii))]; + peaks_vect_f = [peaks_vect_f f(ii)]; + peaks_last = ii; + + if plot_val + plot(f(ii), abs(G_n(ii)), 'or') + end + end +end + +% Peaks creator as sine waves +factor = 2/(length(t)); % normalize the height +sine_vect = zeros(1,length(t)); +for ii = 1:length(peaks_vect_val) + sine_vect = sine_vect + factor*peaks_vect_val(ii)*sin(2*pi*peaks_vect_f(ii)*t + randn(1)); +end + + + +%% Noise cut for variance +% new_vect=zeros(1,length(G_n)); +% for ii = 1:length(G_n) +% if abs(G_n(ii)) < max_height +% new_vect(ii) = G_n(ii); +% else +% new_vect(ii) = 0; +% end +% end +% figure +% plot(f,abs(new_vect)) +% +% signal_new = idft(new_vect',f,t); +% variance1 = var(signal_new); + + +%% Pink noise creator +white_noise = sqrt(white_variance)*randn(size(t)); + +[b, a] = butter(butterOrder, fcut, 'low'); +pink_noise = filter(b, a, white_noise); +noise_modeled = sine_vect + pink_noise; + + +%% Final plots +[G_test,f] = dft(noise_modeled,t); +if plot_val + figure(3) + subplot(1,2,2), hold on, hold on, grid on, title("Modeled noise"), xlabel("f [Hz]"), ylabel("|fft(X)|") + plot(f,abs(G_test)) +end + +y_min = min(min(noise), min(noise)); +y_max = max(max(noise_modeled), max(noise_modeled)); + +if plot_val + figure(4) + subplot(1,2,1), hold on, grid on, title("Real noise"), xlabel("Samples [-]"), ylabel("Value"), ylim([y_min y_max]) + plot(noise) + subplot(1,2,2), hold on, grid on, title("Modeled noise"), xlabel("Samples [-]"), ylabel("Value"), ylim([y_min y_max]) + plot(noise_modeled) +end + + +%% Data + +if ~plot_val + tracks = sensor_vect(sensor_number).tracks; + if tracks(1) >= 6 + track = track - (tracks(1) - 2); + end + switch track + case 2 + sensor_vect(sensor_number).track1.peaks_vect_f = peaks_vect_f; + sensor_vect(sensor_number).track1.peaks_vect_val = factor*peaks_vect_val; + sensor_vect(sensor_number).track1.variance = white_variance*sensor_vect(sensor_number).factor; + case 3 + sensor_vect(sensor_number).track2.peaks_vect_f = peaks_vect_f; + sensor_vect(sensor_number).track2.peaks_vect_val = factor*peaks_vect_val; + sensor_vect(sensor_number).track2.variance = white_variance*sensor_vect(sensor_number).factor; + case 4 + sensor_vect(sensor_number).track3.peaks_vect_f = peaks_vect_f; + sensor_vect(sensor_number).track3.peaks_vect_val = factor*peaks_vect_val; + sensor_vect(sensor_number).track3.variance = white_variance*sensor_vect(sensor_number).factor; + end +end + + +end + diff --git a/commonFunctions/sensors/NoiseAnalysis/Functions/NoiseAnalysis_white.m b/commonFunctions/sensors/NoiseAnalysis/Functions/NoiseAnalysis_white.m new file mode 100644 index 0000000000000000000000000000000000000000..c7737a5b0b64f39979b3d67e04dc1eaf358556db --- /dev/null +++ b/commonFunctions/sensors/NoiseAnalysis/Functions/NoiseAnalysis_white.m @@ -0,0 +1,110 @@ +function [sensor_vect] = NoiseAnalysis_white(sensor_vect, sensor_number, track, plot_val) + +fprintf("Sensor analyzed: %s, track: %d and noise type: %s\n", sensor_vect(sensor_number).name, track, sensor_vect(sensor_number).noise_type); + +% data extrapolation +name = sensor_vect(sensor_number).name; +fs = sensor_vect(sensor_number).fs; + +b_left = sensor_vect(sensor_number).bounds(1); +b_right = sensor_vect(sensor_number).bounds(2); + +bound = 1; + +A = importdata(name).data; +signal = A(:,track); + +% portion to be analyzed +cut_left = ceil(length(signal)*b_left); +cut_right = ceil(length(signal)*b_right); +signal = signal(cut_left:cut_right); + +% filtered signal +filtered_signal = movmean(signal, fs); + +y_min = min(min(signal), min(filtered_signal)); +y_max = max(max(signal), max(filtered_signal)); +if plot_val + figure + subplot(1,2,1), hold on, grid on, title("Unfiltered signal"), xlabel("Samples [-]"), ylabel("Value"), ylim([y_min y_max]) + plot(signal) + subplot(1,2,2), hold on, grid on, title("Filtered signal"), xlabel("Samples [-]"), ylabel("Value"), ylim([y_min y_max]) + plot(filtered_signal(bound:end-bound)) % removed boundaries +end + +noise = signal(bound:end-bound) - filtered_signal(bound:end-bound); + +% fft of noise +tf = length(noise)/fs; +t = 0:1/fs:tf; +L = length(t)-1; + +[G_n,~] = dft(noise,t); +noise_variance = var(signal); + +% White noise - modeling +white_noise = sqrt(noise_variance)*randn(L, 1); + +y_min = min(min(noise), min(white_noise)); +y_max = max(max(noise), max(white_noise)); +if plot_val + figure + subplot(1,2,1), hold on, grid on, title("Extracted noise"), xlabel("Samples [-]"), ylabel("Value"), ylim([y_min y_max]) + plot(noise) % removed boundaries + subplot(1,2,2), hold on, grid on, title("Modeled noise"), xlabel("Samples [-]"), ylabel("Value"), ylim([y_min y_max]) + plot(white_noise) +end + +[G_wn,~] = dft(white_noise,t); + +% comparison between modeled noise and extracted +filtered_white_noise = movmean(white_noise, fs); + +[G_check,f] = dft(white_noise-filtered_white_noise,t); +y_max = round(max(max(abs(G_check)), max(abs(G_n)))); + +if plot_val + figure + subplot(1,3,1), axis square, hold on, grid on, title("|fft(X)| - Extracted noise"), xlabel("f (Hz)"), ylabel("|fft(X)|"), ylim([0, y_max]) + plot(f,abs(G_n),"LineWidth",1) + subplot(1,3,2), axis square, hold on, grid on, title("|fft(X)| - Modeled (+ filter)"), xlabel("f (Hz)"), ylabel("|fft(X)|"), ylim([0, y_max]) + plot(f,abs(G_check),"LineWidth",1) + subplot(1,3,3), axis square, hold on, grid on, title("|fft(X)| modeled white noise"), xlabel("f (Hz)"), ylabel("|fft(X)|"), ylim([0, y_max]) + plot(f,abs(G_wn),"LineWidth",1) +end + +% Variance check +fprintf("Variance of orginal noise: \t\t\t\t" + var(noise) + "\n") +fprintf("variance of modeled (+ filter) noise: \t" + var(white_noise-filtered_white_noise) + "\n") + +% Statistical analysis +[h,p] = ttest2(noise, filtered_white_noise); + +if h == 0 + disp('Signals are statistically similar'); +else + disp('Signals are not statistically similar'); +end + +% Mean squared error +mse = mean((noise - filtered_white_noise).^2); +disp(['MSE between the two noises: ', num2str(mse)]); +fprintf("\n") + +if ~plot_val + tracks = sensor_vect(sensor_number).tracks; + if tracks(1) >= 6 + track = track - (tracks(1) - 2); + end + switch track + case 2 + sensor_vect(sensor_number).track1 = noise_variance; + case 3 + sensor_vect(sensor_number).track2 = noise_variance; + case 4 + sensor_vect(sensor_number).track3 = noise_variance; + end +end + +end + diff --git a/commonFunctions/sensors/NoiseAnalysis/Functions/WindowViewer.m b/commonFunctions/sensors/NoiseAnalysis/Functions/WindowViewer.m new file mode 100644 index 0000000000000000000000000000000000000000..8cc5f8e19a4d6c539b9566fcfbc2eb1516fadda8 --- /dev/null +++ b/commonFunctions/sensors/NoiseAnalysis/Functions/WindowViewer.m @@ -0,0 +1,22 @@ +function [] = WindowViewer(name, track, b_left, b_right) + +A = importdata(name).data; +signal = A(:,track); + +cut_left = ceil(length(signal)*b_left); +cut_right = ceil(length(signal)*b_right); + +figure(1), subplot(1,2,1), hold on, grid on +plot(linspace(0,1,length(signal)), signal') +plot([b_left b_left], [min(signal) max(signal)], Color="red", LineWidth=1.5) +plot([b_right b_right], [min(signal) max(signal)], Color="red", LineWidth=1.5) +xlabel("Position [-]"), ylabel("Value") + +cut_signal = signal(cut_left:cut_right); + +subplot(1,2,2), hold on, grid on +plot(cut_signal) +xlabel("Samples [-]"), ylabel("Value") +hold off + +end diff --git a/commonFunctions/sensors/NoiseAnalysis/Functions/dft.m b/commonFunctions/sensors/NoiseAnalysis/Functions/dft.m new file mode 100644 index 0000000000000000000000000000000000000000..7902b049ba887937fa7cf9f7fe41feefa56826ec --- /dev/null +++ b/commonFunctions/sensors/NoiseAnalysis/Functions/dft.m @@ -0,0 +1,33 @@ +function [G,f] = dft(g,t,Nf) + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% DISCREET FOURIER TRANSFORM +% g = input signal +% t = input time axis +% Nf = number of frequency samples +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +if not(exist('Nf')) + Nf = length(t); +end +if size(g,1)==1 + g = g.'; +end + +G = fft(g,Nf,1); % transform along columns by default +G = fftshift(G,1); + +% frequency axis +dt = t(2)-t(1); +if mod(Nf,2) % odd number of samples + f = (-(Nf-1)/2:(Nf-1)/2)/Nf/dt; +else % even number of samples + f = (-Nf/2:Nf/2-1)/Nf/dt; +end + +% fft delay compensation +phase = -2*pi*f(:)*t(1); +w = exp(1i*phase(:))*ones(1,size(g,2)); +G = G.*w; + +end diff --git a/commonFunctions/sensors/NoiseAnalysis/NoiseCreator.mlx b/commonFunctions/sensors/NoiseAnalysis/NoiseCreator.mlx new file mode 100644 index 0000000000000000000000000000000000000000..4492785dca3564e01f78d8765e7e77b1bf62ea31 Binary files /dev/null and b/commonFunctions/sensors/NoiseAnalysis/NoiseCreator.mlx differ diff --git a/commonFunctions/sensors/NoiseAnalysis/main_noiseAnalysis.m b/commonFunctions/sensors/NoiseAnalysis/main_noiseAnalysis.m new file mode 100644 index 0000000000000000000000000000000000000000..833acbee00b1a18bb7d1d8358dfc3089a42cc307 --- /dev/null +++ b/commonFunctions/sensors/NoiseAnalysis/main_noiseAnalysis.m @@ -0,0 +1,93 @@ +%% Main script for noise modeling + +restoredefaultpath + + +%% Path + +matFolder = ""; +addpath(genpath("./Functions")) +addpath(genpath(matFolder)) + + +%% 1 - Single sensor test + +clear, clc +close all + +sensor_single.name = "motor_Motor_CCPressureData.csv"; +sensor_single.noise_type = "white"; +sensor_single.track = 2; +sensor_single.fs = 100; +sensor_single.bound_left = 0.35; +sensor_single.bound_right = 0.5; +sensor_single.bounds = [sensor_single.bound_left sensor_single.bound_right]; + + +%% 1.1 - Visualizer + +clc, close all + +WindowViewer(sensor_single.name, sensor_single.track, sensor_single.bound_left, sensor_single.bound_right) + + +%% 1.2 - Single sensor analysis + +clc, close all + +if sensor_single.noise_type == "white" + sensor_vect = NoiseAnalysis_white(sensor_single, 1, sensor_single.track, 1); +elseif sensor_single.noise_type == "pink" + sensor_vect = NoiseAnalysis_pink(sensor_single, 1, sensor_single.track, 1); +else + error("Noise type is neither 'white' nor 'pink'") +end + + +%% 2 - Multiple sensors analysis + +clear, clc +close all + +name = "Lyra_Port_sensor_vect"; +save_name = "Lyra_Port_sensor_vect_res"; + +try + load(name) +catch + error("Use mat_creator first") +end + +for sensor_num = 1:length(Lyra_Port_sensor_vect) + for track = Lyra_Port_sensor_vect(sensor_num).tracks + if Lyra_Port_sensor_vect(sensor_num).noise_type == "white" + Lyra_Port_sensor_vect = NoiseAnalysis_white(Lyra_Port_sensor_vect, sensor_num, track, 0); + elseif Lyra_Port_sensor_vect(sensor_num).noise_type == "pink" + Lyra_Port_sensor_vect = NoiseAnalysis_pink(Lyra_Port_sensor_vect, sensor_num, track, 0); + else + error("Noise type is neither 'white' nor 'pink'") + end + end +end + +save(save_name, name) + + +%% 3 - fs + +name = "payload_Payload_StaticPressureData.csv"; +track = 1; % timestamp track + +timestamp = importdata(name).data; +timestamp = timestamp(:,1); + +diff = zeros(1,length(timestamp)-1); +for ii = 1:length(timestamp)-1 + diff(ii) = timestamp(ii+1) - timestamp(ii); +end + +diff_mean = mean(diff); +fs = round(1/diff_mean * 1e6); + +fprintf("%s is sampled at %d Hz\n", name, fs) + diff --git a/commonFunctions/sensors/NoiseAnalysis/mat_creator.m b/commonFunctions/sensors/NoiseAnalysis/mat_creator.m new file mode 100644 index 0000000000000000000000000000000000000000..c24e4ede4d0d2a379b0c8ac5f2ea1bdbfeb3ec2f --- /dev/null +++ b/commonFunctions/sensors/NoiseAnalysis/mat_creator.m @@ -0,0 +1,63 @@ +%% new .mat for init Portugal October 2024 - Lyra + +clear, clc +close all + +Lyra_Port_sensor_vect(1).name = "main_Main_StaticPressureData1.csv"; +Lyra_Port_sensor_vect(1).info = "barometer1"; +Lyra_Port_sensor_vect(1).fs = 100; +Lyra_Port_sensor_vect(1).noise_type = "pink"; +Lyra_Port_sensor_vect(1).bounds = [0.5 0.6]; +Lyra_Port_sensor_vect(1).tracks = 2; +Lyra_Port_sensor_vect(1).factor = 0.01; + +Lyra_Port_sensor_vect(2).name = "main_Main_StaticPressureData2.csv"; +Lyra_Port_sensor_vect(2).info = "barometer2"; +Lyra_Port_sensor_vect(2).fs = 100; +Lyra_Port_sensor_vect(2).noise_type = "pink"; +Lyra_Port_sensor_vect(2).bounds = [0.6 0.7]; +Lyra_Port_sensor_vect(2).tracks = 2; +Lyra_Port_sensor_vect(2).factor = 0.01; + +Lyra_Port_sensor_vect(3).name = "main_Boardcore_LPS28DFWData.csv"; +Lyra_Port_sensor_vect(3).info = "barometer3"; +Lyra_Port_sensor_vect(3).fs = 50; +Lyra_Port_sensor_vect(3).noise_type = "white"; +Lyra_Port_sensor_vect(3).bounds = [0.13 0.23]; +Lyra_Port_sensor_vect(3).tracks = 2; +Lyra_Port_sensor_vect(3).factor = 0.01; + +Lyra_Port_sensor_vect(4).name = "main_Boardcore_LSM6DSRXData.csv"; +Lyra_Port_sensor_vect(4).info = "accelerometer"; +Lyra_Port_sensor_vect(4).fs = 440; +Lyra_Port_sensor_vect(4).noise_type = "white"; +Lyra_Port_sensor_vect(4).bounds = [0.35 0.5]; +Lyra_Port_sensor_vect(4).tracks = [2 3 4]; +Lyra_Port_sensor_vect(4).factor = 1000/9.81; + +Lyra_Port_sensor_vect(5).name = "main_Boardcore_LSM6DSRXData.csv"; +Lyra_Port_sensor_vect(5).info = "initial gyroscope"; +Lyra_Port_sensor_vect(5).fs = 440; +Lyra_Port_sensor_vect(5).noise_type = "white"; +Lyra_Port_sensor_vect(5).bounds = [0.35 0.5]; +Lyra_Port_sensor_vect(5).tracks = [6 7 8]; +Lyra_Port_sensor_vect(5).factor = 1000; + +Lyra_Port_sensor_vect(6).name = "motor_Motor_CCPressureData.csv"; +Lyra_Port_sensor_vect(6).info = "initial chamber pressure"; +Lyra_Port_sensor_vect(6).fs = 100; +Lyra_Port_sensor_vect(6).noise_type = "white"; +Lyra_Port_sensor_vect(6).bounds = [0.35 0.5]; +Lyra_Port_sensor_vect(6).tracks = 2; +Lyra_Port_sensor_vect(6).factor = 0.01; + +Lyra_Port_sensor_vect(7).name = "payload_Payload_StaticPressureData.csv"; +Lyra_Port_sensor_vect(7).info = "pitot (static + total)"; +Lyra_Port_sensor_vect(7).fs = 100; +Lyra_Port_sensor_vect(7).noise_type = "pink"; +Lyra_Port_sensor_vect(7).bounds = [0.3 0.47]; +Lyra_Port_sensor_vect(7).tracks = 2; +Lyra_Port_sensor_vect(7).factor = 0.01; + +save("Lyra_Port_sensor_vect") +