Sensor-filer

%fil 1 = doppler

meanTp = round(mean(diff(rawTimes)))*1e-6;
fs = 1/meanTp(1); %samples per second
dt = 1/fs; % seconds per sample
from = 1000;
to = 30000;
Nfft = 2^(14);
N = to - from; %samples; % samples taken

RadarIF_I = allData.RadarIF_I -512;

radar_fft = fft(RadarIF_I(from:to),Nfft);
f = 1/(Nfft*dt)*(-Nfft/2:1:Nfft/2-1);

radar_fft(1) = 0;
[a, b] = max(abs(radar_fft));
peak = f(Nfft/2 + b);

vel = peak/160.9;

s_fft = fftshift(radar_fft);
s_fft = s_fft/max(s_fft);
figure;
plot(f, 20*log10(abs(s_fft)), 'r-');
title(['Magnitude with peak at: ' num2str(peak) ' Velocity: ' num2str(vel)]);
axis([-2000 2000 -70 10]);
xlabel('Frequency');
ylabel('magnitude');
grid on;


%fil 2 = radfreq

meanTp = round(mean(diff(rawTimes)))*1e-6;
fs = 1/meanTp(1); %samples per second
dt = 1/fs; % seconds per sample
N = 5000; %samples; % samples taken

Radar_I = allData.RadarIF_I - 512;
Radar_Q = allData.RadarIF_Q - 512;
%ffts
%RadarIF_I_fft = fftshift(fft(Radar_I(5000:12000),N));
%RadarIF_Q_fft = fftshift(fft(Radar_Q(5000:12000), N));
RadarIF_I_fft = fft(Radar_I(8000:13000),N);
RadarIF_I_fft = RadarIF_I_fft(1:N/2);

RadarIF_Q_fft = fft(Radar_Q(8000:13000), N);
RadarIF_Q_fft = RadarIF_Q_fft(1:N/2);

%Freq specs
dF = fs/N;
%f = -fs/2:dF:fs/2-dF;
f = (0:N/2-1)*fs/N;

%spectrum plot
figure;
plot(f, abs(RadarIF_I_fft), '-o',...
    f, abs(RadarIF_Q_fft), '-o'...
    );
xlabel('Frequency (in Hertz)');
title('Magnitude response');

%fil 3 = raspianalyze

%This script will import the binary data from files written by the C
% program on Raspberry Pi.
%
% The script uses the function raspiImport to do the actual import and
% conversion from binary data to numerical values. Make sure you have
% downloaded it as well.
%
% After the import function is finished, the data are written to the
% variable rawData. The number of samples is returned in a variable samples

% Definitions
channels = 5;   % Number of ADC channels used

% Open, import and close binary data file produced by Raspberry Pi
%% FIXME: Change this.
path = 'W:\adcData.bin';


% Run function to import all data from the binary file. If you change the
% name or want to read more files, you must change the function
% accordingly.
[rawData, nomTp] = raspiImport(path,channels);

% Plot all raw data and corresponding amplitude response
fh_raw = figure;    % fig handle
plot(rawData,'-o');
ylim([0, 4095]) % 12 bit ADC gives values in range [0, 4095]
xlabel('sample');
ylabel('conversion value');
legendStr = cell(1,channels);
for i = 1:channels
    legendStr{1,i} = ['ch. ' num2str(i)];
end
legend(legendStr,'location','best');
title('Raw ADC data');

%fil 4 = raspiimport

% raspiImport takes in a path string and imports a specified binary data file
% from this path. It returns the raw data as a matrix NUM_SAMPLES x
% NUM_CAHNNELS, and the sample period in seconds.
function [rawData, sample_period] = raspiImport(path,channels)

% Input argument handling
if nargin < 2
    channels = 1;   % Assume single channel as nothing was specified
    warning('Number of channels not defined, assuming single...');
end

% Open file
fidAdcData = fopen(path);
% Read binary data to local variables
sample_period = fread(fidAdcData, 1, 'double')*1.0e-06;
adcData = fread(fidAdcData,'uint16');
% Close files properly after import
fclose(fidAdcData);

lenAdcData = length(adcData);       % Total number of ADC data bytes
samples = lenAdcData/channels;      % Total number of samples

%reshape one-dimensional data array into NUM_SAMPLES x NUM_CHANNELS data matrix
rawData = reshape(adcData, channels, samples)';

end

%fil 5 = test_mic


fs = 16000;
r = 16;
%insert samplerange you want to crosscorrelate
p = 1:500000;



k = rawData(:,1)-2048;
k = interp(k,r);
l = rawData(:,3)-2048;
l = interp(l,r);
m = rawData(:,4)-2048;
m = interp(m,r);

%Removing DC components BEFORE crosscorrelation
m1 = k;
m2 = l;
m3 = m;

%Crosscorrealtion between all three mics
[acor1, lag1] = xcorr(m1, m2);
[~, I_1] = max( abs(acor1));
lagDiff1 = lag1(I_1);
timeDiff1 = lagDiff1/(fs);

[acor2, lag2] = xcorr(m1, m3);
[~, I2] = max( abs(acor2));
lagDiff2 = lag2(I2);
timeDiff2 = lagDiff2/(fs);

[acor3, lag3] = xcorr(m2, m3);
[~, I3] = max( abs(acor3));
lagDiff3 = lag3(I3);
timeDiff3 = lagDiff3/(fs);

%Incoming angle estimation
theta = atan(sqrt(3)*((timeDiff1 + timeDiff2)/(timeDiff1 - timeDiff2 - 2*timeDiff3)));
if (lagDiff3 < 0),
    theta = theta + pi;
elseif (lagDiff1 == lagDiff2 && lagDiff1 > 0),
        theta = -pi/2;
elseif (lagDiff1 == lagDiff2 && lagDiff1 < 0),
        theta = pi/2;
end

%plot relevant figures
figure;
subplot(3,1,1);
plot(lag1/fs, acor1);
title('Lag from m2 to m1');
xlabel(['sample difference: ' num2str(lagDiff1) ' Time difference: ' num2str(timeDiff1)]);

subplot(3,1,2);
plot(lag2/fs, acor2);
title('Lag from m3 to m1');
xlabel(['sample difference: ' num2str(lagDiff2) ' Time difference: ' num2str(timeDiff2)]);

subplot(3,1,3);
plot(lag3/fs, acor3);
title('Lag from m3 to m2');
xlabel(['sample difference: ' num2str(lagDiff3) ' Time difference: ' num2str(timeDiff3)]);

figure;
plot(p,m1, '-o',...
    p,m2, '-o',...
    p,m3, '-o'...
    );
title('Part of signals m1, m2 and m3 that are crosscorrelated');
xlabel('t [s]');
ylabel('12-bit Conversion value');
legend('Mic1','Mic2','Mic3');


figure;

plot([0 sqrt(3)/2],[1 -0.5], 'g--o',...
    [-sqrt(3)/2 0], [-0.5 1], 'b--o',...
    [sqrt(3)/2 -sqrt(3)/2], [-0.5 -0.5], 'r--o',...
    [0 10*cos(theta)], [0 10*sin(theta)]...
    );
axis([-1.5 1.5 -1.5 1.5]);
text(0, 1.2, 'm1');
text(-1, -0.7, 'm2');
text(1, -0.7, 'm3');
grid on;

%fil 6 = komm

load('project1-data.mat');
Fs = 1/(t(4)-t(3));
lengde = length(x);
%power
power = bandpower(x,Fs,[0 Fs/2]);
rRMS = rms(x)^2;

%power density
xdft = fft(x);
xdft = xdft(1:lengde/2+1);
psdx = (1/(Fs*lengde)) * abs(xdft).^2;
psdx(2:end-1) = 2*psdx(2:end-1);
freq = 0:Fs/length(x):Fs/2;

figure()
plot(freq,10*log10(psdx))
grid on
title('Periodogram Using FFT')
xlabel('Frequency (Hz)')
ylabel('Power/Frequency (dB/Hz)')

figure()
periodogram(x,rectwin(length(x)),length(x),Fs)

%autocorrelation
figure()
autocorr(x);