%
% 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:
Z1 = [ 1, 27, 33, 45, 12, 16, 83, 67, 54, 39, 23, 6, 14, 15, 19, 31, 37, 44, 56, 60 ]; Z1 = Z1(:); 
T1 = 1:length(Z1);
ph = plot( T1, Z1, 'o' ); hold on; 

cols = ['grkmc'];

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

p = 3; % lets start with the quadradic fit

H1 = F(:,1:p); % this is H1 

% Form the (left pseudoinverse) matrix and estimate the coefficients 
xhat1 = (H1' * H1) \ (H1' * Z1); % these are the least square coefficients
fprintf('The first estimate of the polynomial coefficients (lowest to highest) is:\n');
xhat1 

figure(gcf); plot( T1, H1 * xhat1, '-b' ); 

% Some auxillary matrices: 
% R1 is the identity matrix 
P1Inv = H1' * H1;

% We now get a new measurement:
T2 = length(T1)+1;
Z2 = 25;

% Recursivly estimate the new polynomial coefficients form the Kalman filter matrix K
%
H2  = [ 1 T2 T2^2 ]; 
tmp = H2 * ( P1Inv \ H2' ) + 1; % this is a scalar
K2  = (1/tmp) * ( P1Inv \ H2' );

xhat2 = xhat1 + K2 * ( Z2 - H2 * xhat1 );

fprintf('The second estimate of the polynomial coefficients (lowest to highest) is:\n');
xhat2

figure(gcf); plot( T2, Z2, 'o', 'markersize', 10, 'markerfacecolor', 'b' ); 
plot( [T1,T2], [ H1 * xhat2; H2 * xhat2 ], '-r' ); 

axis tight;