Fs = 5000; % Sampling frequency
t= [0:1/Fs:1];
p = 40; % Linear predictive order
An = 0.0; % Noise amplitude

%
% Least-squares FIR filter that preserves constants
f1 = 60; f2 = 180;
x=1.5 + sin(2*pi*f1*t) + sin(2*pi*f2*t) + An*randn(size(t));
corr = xcorr(x,x,p,'biased');
R = toeplitz(corr(p+1:2*p+1));
b = R\ones(p+1,1);
b = b/sum(b);
boxcar = ones(1,p+1)/(p+1);

y = filter(b,1,x);
yave = filter(boxcar,1,x);
L = 200;
subplot(211);
plot(t(1:L),x(1:L),t(1:L),yave(1:L),'g',t(1:L),y(1:L),'r');
subplot(212);
N = 256;
f = [0:N/2]*Fs/N;
H = fft(b, N);
Hboxcar = fft(boxcar, N);
semilogy(f,abs(H(1:N/2+1)),'r',f,abs(Hboxcar(1:N/2+1)),'g');
grid
ax = axis;
line([f1 f1],[ax(3) ax(4)]);line([f2 f2],[ax(3) ax(4)]);
