%This implements the speedometer example.
%The true speed X is a Wiener process. GLITCH: May be negative at times!
%The observed signal Y is a noisy odometer: the summation Y of X plus noise.
%conv(cumsum(X),[1,-1]) does not equal X--end effects. Need to truncate estimates.
clear;sigma=1;M=249;M1=M+1;%M:Zero-pad lengths of X and Y for convolution with H.
X=cumsum(randn(1,M));%True speed
Y=cumsum(X)+sigma*randn(1,M);%Noisy odometer
figure,subplot(211),plot(X),title('True speed')
subplot(212),plot(Y),title('Noisy odometer')
H=[1,-1];FH=fft(H,M1);SX=sum(X.^2);FY=fft(Y,M1);%DTFT(H)=denominator of Eq. (9.89).
R=conv(Y,H);%Estimate speed from noisy odometer. Estimate is noisy.
figure,subplot(211),plot(X),title('True speed')
subplot(212),plot(R(1:M)),title('Unfiltered estimate of speed. Noisy estimate.')
Omega=2*pi*[0:M1/2 M1/2-1:-1:1]/M1;%Omega. Implement Wiener filters using Eq. (9.93)
FZ=FY.*FH./(1+abs(FH).*abs(FH).*Omega.*Omega*0.4);Z=real(ifft(FZ));%Stochastic Wiener filter
FW=FY.*FH./(1+abs(FH).*abs(FH)*0.1);W=real(ifft(FW));%Deterministic Wiener filter
%Omit first 3 and last 3 values of of estimates (end effects-see above).
figure,subplot(211),plot(X),title('True speed')
subplot(212),plot(Z(4:246)),title('Stochastic Wiener filter estimate is smoothed.')
figure,subplot(211),plot(X),title('True speed')
subplot(212),plot(W(4:246)),title('Deterministic Wiener filter estimate is noisy.')

