%
% Written by:
% -- 
% John L. Weatherwax                2005-08-14
% 
% email: wax@alum.mit.edu
% 
% Please send comments and especially bug reports to the
% above email address.
% 
%-----

clear; close all; clc; 
clear functions; 

% the given data:
Z = [ 1, 27, 33, 45, 12, 16, 83, 67, 54, 39, 23, 6, 14, 15, 19, 31, 37, 44, 56, 60 ]; Z = Z(:); 
T = 1:length(Z);
ph = plot( T, Z, 'o' ); 

cols = ['grkmc'];

% Compute the standard deviation of each point 
% -- the first point is 10 times better than the last point 
%
pointSTD = linspace( 1, 10, length(Z) );
N = diag( pointSTD ); % the error's as a matrix
S = N' * N; % square of the deterministic measurement error 

% This is the coefficient matrix 
F = fliplr( gallery( 'vander', T ) );

mses = [];
phs = [ph];
for p = 1:4, % index p => polynomial of order p-1
  H = F(:,1:p);
  % Form the H (weighted left pseudoinverse) matrix:
  HWL = ( H' * ( S \ H ) ) \ ( H' * ( S \ Z ) ); % these are the least square coefficients
  HWL = flipud( HWL ); % put this in the order needed for polyval

  figure(gcf); hold on;
  
  ZHat = polyval( HWL, T ); 
  % compute the mean square error:
  ms = mean( ( Z - ZHat(:) ).^2 );
  mses = [mses,ms];

  h = plot( T, ZHat, cols( mod(p-1,length(cols))+1 ) );
  plot( T, ZHat+pointSTD, [cols( mod(p-1,length(cols))+1 ),'-.'] );
  plot( T, ZHat-pointSTD, [cols( mod(p-1,length(cols))+1 ),'-.'] );
  phs = [ phs, h ];
end

axis tight; 
legend( phs, 'raw data','constant','first degree','second degree','third degree');
xlabel('time'); ylabel('z');

% print the mean square error of each estimate: 
for p=1:4, 
  fprintf('MSE(p= %3d)= %10.6f\n', [ p-1, mses(p) ] );
end