% y = wgn(m,n,p) generates an m-by-n matrix of white Gaussian noise
% p specifies the power of y in decibels relative to a watt. The default load impedance is 1 ohm.
% check the help command with functions wgn and pinknoise

clear all; clc;
ntot = 1;   % number of repetitions of the calculation (the M of the class)
tfin = 1000; % total observation time length

for uu = 1:ntot % iterates over uu cycles if desired

% part 1) noise generation and visualization (in terms of output voltage)
t = linspace(0,tfin,160000); % generates time scale

vnmax = 2.5e-5; % rms of white gaussian noise (in terms of volts rms)
Pnmax = vnmax^2; % power on a load of 1 Ohm (needed by the wgn command, see next two rows)
PdB = 10*log10(Pnmax); % power in dB (this is what the wgn matlab function wants as input)
vn = wgn(1, length(t), PdB); % generation of wgn noisy signal in time

A = 2.5e-6; % amplitude of pink noise
vf = pinknoise(length(vn))*A; % generates pink noise in time

t2=linspace(0,tfin,10); % slower time scale for slow offset drift
vnmax2 = 0.03e-3; % os amplitude
coeff = rand(1,10)*vnmax2; % random os variations within amplitude
vo = polyfit(t2,coeff,5); % 5th order fitting coefficient
vo = t2.^5*vo(1)+t2.^4*vo(2)+t2.^3*vo(3)+t2.^2*vo(4)+t2.^1*vo(5)+t2.^0*vo(6); % 5th order fitting coefficient
vo = interp1(t2, vo, t); % interpolates slow noise on the same time scale as previous noise

S = 1e-3; % sensitivity of 1 mV/dps
rn = (vn+vf+vo)/S; % normalization


figure(5); hold all; grid on; box on; % plots sampled signal in time
stairs(t, rn);
stairs(t, vn/S);
stairs(t, vf/S);
stairs(t, vo/S);
xlabel('Time [s]');
ylabel('Sampled Rate [dps]');

figure(6); hold all; grid on; box on; % plots histogram of angular rates
hist(rn,20);
xlabel('Sampled Rate [dps]');
ylabel('Distribution');


% part 2) calculation of the angle
dt = diff(t); % calculates time differences

for i=1:length(t)-1
    a(i) = sum(rn(1:i).*dt(1:i)); % calculates integrated angle in time
end

figure(7); hold all; grid on; box on; % plots angle in time
plot(t(1:end-1), a);
xlabel('Time [s]');
ylabel('Sampled Angle [°]');

afin(uu) = a(end); % final angle value for this specific cycle uu


% part 3) calculation of AV
tau_min = tfin/length(t)*10; % sets minimum observation time (at least ten average per slot)
tau_max = t(end)/10; % sets maximum observation time (at least average over 10 slots)
npoint = 50; % number of observation time on which to plot the AV

tau=logspace(log10(tau_min), log10(tau_max), npoint); % definition of the observation time axis

for k=1:npoint % the loop calculates the average values along the slots, and then the AV
    M(k) = floor(t(end)/tau(k)); %n. of slots in which to divide the acquisition for the kth observation time
    slot_length(k) = floor((t(end)/M(k))/(tfin/length(t))); % length (in points) of the corresponding slot
    for j = 1:M(k)
        rm(j)=mean(rn(slot_length(k)*(j-1)+1 : slot_length(k)*j )); % mean values over the slots
        if ntot==1
            rmt(slot_length(k)*(j-1)+1:slot_length(k)*j)=rm(j)*ones(1, slot_length(k)); % calculates traces for the plot only if ntot==1 to avoid too data in the graphs
        end
    end
%     if ntot==1
%         figure(4); % plots the averages values for the different slots (only if ntot=1)
%         hold all;
%         plot(t, rmt);
%     end
    drm=diff(rm); % differences between mean values of adjacent slots
    clear rm; % cancels to save memory
    AV(k)= 1/(2*(M(k)-1)) * sum(drm.^2); % calculates AV
end

figure(9); hold all; % plots root AV of angular rate
loglog(tau(1:end-1), sqrt(AV(1:end-1)));
xlabel('Observation Time [s]');
ylabel('Allan variance [dps]');

figure(10); hold all; % plots root AV of angle
loglog(tau(1:end-1), sqrt(AV(1:end-1)).*(tau(1:end-1)));
xlabel('Observation Time [s]');
ylabel('Allan variance [°]');

end

% figure(8); hold all; grid on; box on;
% hist(afin);
% xlabel('Final Angle [°]');
% ylabel('Distribution');


