% *****************************************************************
% Using the Moore-Penrose Pseudoinverse to remove known disturbances from
% measured data
%
% all vectors are column vectors.
% *****************************************************************
close all; clear all;

% *****************************************************************
% Example: A measured signal has been polluted with power-line hum,
% at a fundamental frequency, 3rd and 5th harmonic, with unknown
% amplitude and phase.
% *****************************************************************
m=1000; % number of measured points
fs=22000; % sampling rate
fHum=50; % known 50 Hz power line hum frequency
index=(1:m)';

% create power-line hum terms
hum=0.87654*sin(2*pi*index*fHum/fs+0.12345);
hum3rd=0.65432*sin(2*pi*index*3*fHum/fs+0.45678);
hum5th=0.3321*sin(2*pi*index*5*fHum/fs+0.78901);

% create actual data
idealData=0.1*randn(m, 1);

% create measured data - deteriorated by hum
measuredData=idealData+hum+hum3rd+hum5th;

% *****************************************************************
% Processing to remove the hum
% *****************************************************************
% We know frequency and number of harmonics
% We DO NOT the amplitude and phase of each term.
% note: a*sin(ometaT)+b*cos(omegaT) is equivalent to c*sin(omegaT+d)
% where either [a, b] or [c, d] are the unknown parameters.

basis=[sin(2*pi*index*1*fHum/fs), cos(2*pi*index*1*fHum/fs), ...
       sin(2*pi*index*3*fHum/fs), cos(2*pi*index*3*fHum/fs), ...
       sin(2*pi*index*5*fHum/fs), cos(2*pi*index*5*fHum/fs)];

% *****************************************************************
% Moore-Penrose Pseudoinverse: Least-squares fit between the basis
% waveforms and the measured signal.
% *****************************************************************
leastSquaresCoeff=pinv(basis)*measuredData;
reconstructedHum=basis*leastSquaresCoeff;

correctedData=measuredData-reconstructedHum;

% *****************************************************************
% Plot measured signal and reconstructed hum
% *****************************************************************
figure(); hold on;
plot(measuredData);
plot(reconstructedHum, 'r', 'LineWidth', 2);
legend({'measured data', 'reconstructed hum signal'});

% *****************************************************************
% Plot the remainder after subtracting the reconstructed hum.
% Compare with ideal result
% *****************************************************************
figure(); hold on;
plot(idealData,  'k', 'LineWidth', 2);
plot(correctedData);
plot(idealData-correctedData, 'r', 'LineWidth', 3);
legend({'ideal result', 'measured and corrected', 'error'})