%% Simulation of pendulum motion and FFT-analysis of doppler-shifted sound signal
% Alex Read,  July 1, 2019, University of Oslo, Department of Physics
% Simulate a pendulum with L=12 m that has an amplitude of about 1m. We will measure the velocity of the pendulum by the simulation of a doppler-shifted sound signal transmitted from the moving pendulum. A fast fourier-transform (FFT) analyses the signal received by the simulation of a stationary microphone positionned in the plane of the pendulum motion.
%
%% Periodic motion of pendulum
% The period T and angular frequency \omega \text{ } of the pendulum are given by:
% T = 2\pi\sqrt{g\over L}, \omega =\frac{2\pi }{T}\ldotp
% The position of the pendulum is given by
% \theta(t) = \theta_0 \cos{\omega t},
% where \theta_0 is angular amplitude of the pendulum (friction is not simulated). The maximum (tangential) velocity of the pendulum v_{max} is given by:
% v_{max} = L\,\dot{\theta}_{max} = L\,\omega\,\theta_0.
%

g = 9.819; %m/s^2 (in Oslo, see https://units.fandom.com/wiki/Gravity_of_Earth)
L = 12;
x = 1;
theta_0 = atan(x/L); theta_0_d = theta_0*180/pi;
omega = sqrt(g/L); T = 2*pi/omega;
v_max = L*omega*theta_0; 
fprintf('Period = %.2f s, angular amplitude = %.2f degrees\n',T,theta_0_d)
fprintf('Maximum tangential velocity = %.2f m/s\n',v_max)

%
%% Simulate motion of pendulum
% Now simulate 30 s of motion, sampling at 96 kHz. We use the x-component of the position, velocity, and acceleration (dimension in the plane of the pendulum motion). We project the motion, velocity, and acceleration on to the x-axis, but these are nearly equal to the tangential values for small-amplitude osccillations.

totalTime = 30;
sampleRate = 96E3;
dt = totalTime/sampleRate;
t = linspace(0,totalTime-dt,totalTime*sampleRate);
theta = theta_0*cos(omega*t);
x = theta.*cos(theta)*L;
thetaDot = -theta_0*omega*sin(omega*t);
vx = thetaDot.*cos(theta)*L;
ax = -theta_0*omega^2*cos(omega*t).*cos(theta)*L;
figure(1), hold off, plot(t,vx)
title('Velocity in x-direction'), xlabel('t (s)'), ylabel('v_x (m/s)')

%
%% Simulate sound signal with doppler-shift
% Now simulate the creation of a signal S with unit amplitude with frequency f=12 kHz picked up by a microphone beyond one end of the pendulum's motion. The signal will be sampled with frequency sampleRate. The Doppler-shift is caused by the changing of the position of the source with time (the extra 2*pi*f*x/c term).

Temp = 20; % temperature degrees C
c = 331.1 + 0.606*Temp; % lydhastighet tørr luft m/s
f = 12E3;
S = cos(2*pi*f.*(t+x/c));
fprintf('Velocity of sound in dry air at 20 C = %.2f m/s\n',c)

%
%% Frequency analysis of the simulated sound signal
% Partition the data into 0.5 s intervals \Delta t and perform a Fourier analysis to find the average frequency per interval. For the average frequency we use the frequency with the largest amplitude (in the first half of the frequency spectrum, since the upper half is a mirror copy of the frequency spectrum up to half of the sampling frequency).
% Note that the resolution \Delta f on the FFT-measured frequency is 1 over the length of the time interval:
% \Delta f=\frac{1}{\Delta t\text{ }}.
% Note also that the average velocity is measured over a time interval such that the time for a given velocity should be the average time in the interval (rather than the time at one of the edges of the interval).
% As an example, we show a plot of the absolute value of the amplitude as a function of frequency for the last time interval.
% The main result of this section is a plot of the reconstructed (by FFT) frequency f^' per time interval of the signal picked up by the simulated microphone. The reconstructed points are compared to the theoretical Doppler-shifted frequency, which is derived by comparing the wavelength of sound emitted from a source at rest to one that moves a distance \Delta x=v\text{ }T\text{ }during a time interval of one period T, where v is the Velocity of the source. Since the source is now closer to the observer (e.g. a microphone) this represents a change of the wavelength from \lambda to $\lambda^' = \lambda-vT =\lambda-v/f$ . Since the velocity of sound is constant we have
% c=\lambda f = \lambda^'f^' = (\lambda-v/f)f^' = (c-v)f^'/f,
% which can be solved for the observed frequency
% f^' = f{c\over c-v}.

timeInterval = 0.5;
Nintervals = floor(totalTime/timeInterval);
samplesPerInterval = floor(sampleRate*timeInterval);
fv = 2*(0:samplesPerInterval-1);

for ii = 1:Nintervals
    imin = 1 + (ii-1)*samplesPerInterval;
    imax = imin -1 + samplesPerInterval;
    Y = fft(S(imin:imax),samplesPerInterval);
    E = abs(Y);
    [ym,im]=max(E(1:end/2));
    finterval(ii) = fv(im);
end
figure(3), plot(fv,E), xlabel('Frequency (Hz)'), ylabel('|Amplitude|')
title('FFT for one time interval')

tinterval = linspace(timeInterval/2,totalTime-timeInterval/2,Nintervals);
figure(5), hold off, errorbar(tinterval,finterval,(1/timeInterval)*ones(size(finterval)),'*'), hold on
ylabel("Energy"), xlabel('Frequency (Hz)')
title('Doppler-shifted frequency')
ftheory = c./(c-vx)*f;
plot(t,ftheory,'-')
ylabel('Frequency (Hz)'),xlabel('Time (s)')
legend('FFT measurement','Theoretical')

%
%% Measured velocity and acceleration
% Now show velocity and acceleration derived from the frequency measurements. The velocity for a given observed frequency can be found by solving the previous equation for v:
% v = c\left(1-{f\over f^'}\right)
% Note that we have 1 less acceleration measurement since we use finite differences to compute acceleration. Since we in fact measure average acceleration \bar{a} we have to make sure that we show the average acceleration in the middle of the respective time interval.
% The uncertainty on the velocity is computed by propagating the uncertainty on the observed frequency through the expression above:
% |\Delta v| = c\,{f\over f^'}\,{\Delta f^'\over f^'}.
% Since the acceleration is derived by the difference between two consecutive  velocity measurements divided by the time interval, the uncertainty on the acceleration has a factor of sqrt(2):
% \bar{a} = {v_2-v_1\over \Delta t} and
% \Delta\bar{a} = {\sqrt{(\Delta v_1)^2+(\Delta v_1)^2}\over \Delta t} \approx {\Delta v_2\over \Delta t}\sqrt{2}.

vxm = c*(1-f./finterval);
dvxm = c*f./finterval.*(1/timeInterval)./finterval;
hold off, errorbar(tinterval,vxm,dvxm,'*')
title('Velocity measurement'), xlabel('Time (s)')
ylabel('Velocity (m/s')
hold on, plot(t,vx,'-')
legend('FFT measurement','Theory')

axm = diff(vxm)/timeInterval;
daxm = sqrt(2)*dvxm/timeInterval;
hold off, errorbar(timeInterval/2+tinterval(1:end-1),axm,daxm(1:end-1),'*')
hold on, plot(t,ax,'-')
title('Acceleration measurement'), xlabel('Time (s)')
ylabel('Acceleration (m/s^2)')
legend('FFT measurement','Theory')