%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% clean up the workspace, and generate some data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all ; close all ; clc
randn('seed',0);

N = 10;
m = 2
b = 3

x = (1:N)';
y = m*x + b + randn(N,1);
y([1 10]) = [15 15];

figure();
plot(x , y , 'o' , x , m*x+b , 'k');
xlim([0 N+1]);
xlabel('x');
ylabel('y');
grid on;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% fit a line to the data using OLS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
A = [x , ones(size(x))];
zols = A \ y;
mols = zols(1)
bols = zols(2)

hold on;
    plot(x , mols * x + bols , 'r');
hold off;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% fit a line to the data using IRWLS for l1 regression
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
ml1 = mols;
bl1 = bols;

kmax = 1000;
delta = 1e-6;
for k = 1:kmax
    W = diag(1./max(abs(A*[ml1;bl1] - y) , delta));
    zl1 = (A' * W * A) \ (A' * W * y);
    
    if norm(zl1 - [ml1;bl1]) < 1e-6
        break;
    end
    ml1 = zl1(1);
    bl1 = zl1(2);
end
ml1
bl1

hold on;
    plot(x , ml1 * x + bl1 , 'g');
hold off;
legend('data' , 'truth' , ...
       'OLS estimate' , 'WLS estimate' , ...
       'Location' , 'Northwest');