%% Fitting Demo 
% 
% revision history:
% 04/16/24 Mark D. Shattuck <mds> fittingDemo.m


x=(0:.1:1)';  % Independent variable
N=length(x);  % number of points

plot(x,3*x-2,'.-')        % plot dependent variable
y=3*x-2+randn(size(x))/3; % add noise
plot(x,3*x-2,'-',x,y,'o') % plot w&w/o noise 
a=polyfit(x,y,0)          % fit to constant
a=polyfit(x,y,1)          % fit to line
a=polyfit(x,y,2)          % fit to parabola

a=polyfit(x,y,1)          % fit to line
plot(x,3*x-2,'-',x,y,'o',x,polyval(a,x)) % plot fit

%% plot different degrees
plot(x,3*x-2,'-',x,y,'o',x,polyval(polyfit(x,y,0),x))
plot(x,3*x-2,'-',x,y,'o',x,polyval(polyfit(x,y,1),x))
plot(x,3*x-2,'-',x,y,'o',x,polyval(polyfit(x,y,2),x))
plot(x,3*x-2,'-',x,y,'o',x,polyval(polyfit(x,y,3),x))
plot(x,3*x-2,'-',x,y,'o',x,polyval(polyfit(x,y,4),x))
plot(x,3*x-2,'-',x,y,'o',x,polyval(polyfit(x,y,5),x))
plot(x,3*x-2,'-',x,y,'o',x,polyval(polyfit(x,y,6),x))
plot(x,3*x-2,'-',x,y,'o',x,polyval(polyfit(x,y,7),x))
plot(x,3*x-2,'-',x,y,'o',x,polyval(polyfit(x,y,8),x))
plot(x,3*x-2,'-',x,y,'o',x,polyval(polyfit(x,y,9),x))
plot(x,3*x-2,'-',x,y,'o',x,polyval(polyfit(x,y,10),x))

%%
xx=(-.1:.01:1.1)'; % finer mesh for fit function
plot(x,3*x-2,'-',x,y,'o',xx,polyval(polyfit(x,y,10),xx))

%% errors
M=1000;                         % number of trials
a=zeros(M,2);                   % storage for fit params
yvar=zeros(M,1);                % storage for var(y)
for n=1:M;                      % loop over all trials 
  y=3*x-2+randn(size(x))/3;     % generate random data std(y)=1/3
  a(n,:)=polyfit(x,y,1);        % store fit
  yf=polyval(a(n,:),x);         % find fitted data
  yvar(n)=sum((y-yf).^2)/(N-2); % calculate var(y-yf)
end
plot(a)                         % plot all fit parameters 
%% compare

[3 -2;mean(a)]                  % compare to actual [3 -2]

sy=sqrt(mean(yvar));            % calc \sigma_y==error in y
[1/3;sy]                        % compare to 1/3

% compare to equation for fit error 
[std(a); ...                      
sy/sqrt(N)*sqrt(1/(mean(x.^2)-mean(x)^2))...
sy/sqrt(N)*sqrt(mean(x.^2)/(mean(x.^2)-mean(x)^2))]


%%
a=polyfit(x,y,1) % polyfit way

[x ones(size(x))]\y % linear algebra way

%% linear algebra explaination
syms a b
[x ones(size(x))]*[a;b]      % fit function for each point 
[x ones(size(x))]*[a;b]==y   % must fit N equation, but only 2 unk
[x ones(size(x))]\y          % matlab way for psuedo-inverse   
a=polyfit(x,y,1)             % same as polyfit
%edit polyfit                 % line 68 shows similar qr way

plot(x,3*x-2,'-',x,y,'o',x,polyval(polyfit(x,y,1),x),xx,[xx ones(size(xx))]*([x ones(size(x))]\y))

%% but can use any function that is linear in parameters
y=4*x.^2-2*sin(2*pi*x)+randn(size(x))/3;
plot(x,y)
Af=[x.^2 sin(2*pi*x)]\y
Af=lscov([x.^2 sin(2*pi*x)],y)    % alternative way

%% finer grid
plot(xx,4*xx.^2-2*sin(2*pi*xx),'-',x,y,'o',xx,[xx.^2 sin(2*pi*xx)]*Af)

%% 2D linear fit
[xx yy]=ndgrid(-1:.2:1,-1:.2:1);       % 2D grid

zz=2*xx.^2+xx.*yy-yy.^2;               % 2D function A=[2;1;-1]
plot3(xx,yy,zz,'k.','markersize',20)   % plot

%% add noise
zz=2*xx.^2+xx.*yy-yy.^2+randn(size(xx))/3;
plot3(xx,yy,zz,'k.','markersize',20)

%% use linear algebra to fit
Af=[xx(:).^2 xx(:).*yy(:) yy(:).^2]\zz(:)    % fit
zf=0*xx;                                     % pre-init
zf(:)=[xx(:).^2 xx(:).*yy(:) yy(:).^2]*Af;   % evaluate
surf(xx,yy,zf);                              % plot as surface

%% add points as well
surf(xx,yy,zf);
hold on;
plot3(xx,yy,zz,'k.','markersize',20);
hold off;
axis off

%% random grid
A=[2;1;-1];                    % exact coefficients
xx=2*rand(100,1)-1;            % random points in -1 to 1   
yy=2*rand(100,1)-1;
zz=[xx.^2 xx.*yy yy.^2]*A;     % exact function
plot3(xx,yy,zz,'k.','markersize',20)

%% Add noise
zz=[xx.^2 xx.*yy yy.^2]*A+randn(100,1)/3;
plot3(xx,yy,zz,'k.','markersize',20)

%% fit
Af=[xx.^2 xx.*yy yy.^2]\zz    % fit

[xs ys]=ndgrid(-1:.2:1,-1:.2:1); % grid for evaluation

zf=0*xs; % pre-init
zf(:)=[xs(:).^2 xs(:).*ys(:) ys(:).^2]*Af;  % evaluate

% plot
surf(xs,ys,zf);
hold on;
plot3(xx,yy,zz,'k.','markersize',20);
hold off;
axis off

%% finer grid
[xs ys]=ndgrid(-1:.1:1,-1:.1:1);
zf=0*xs;zf(:)=[xs(:).^2 xs(:).*ys(:) ys(:).^2]*Af;

% plot
surf(xs,ys,zf);
hold on;
plot3(xx,yy,zz,'k.','markersize',20);
hold off;
axis off

%% non-linear fit
x=(0:.1:1)';  % Independent variable
y=4*x.^2-2*sin(2*pi*x)+randn(size(x))/3; % same equation but fit for 2*pi

Yf=@(p) p(1)*x.^2-p(2)*sin(p(3)*x);  % autonomous fit function
X2=@(p) sum((Yf(p)-y).^2);           % autonomous X2=chi^2
p=fminsearch(X2,[1 1 3]);            % find min of X2

plot(x,y,'o',x,p(1)*x.^2-p(2)*sin(p(3)*x),x,4*x.^2-2*sin(2*pi*x))
plot(x,y,'o',x,Yf(p),x,Yf([4 2 2*pi]))

%% alternative way to include x and y
% easier to use Yf with different x values

Yf=@(p,x) p(1)*x.^2-p(2)*sin(p(3)*x);  % autonomous fit function
X2=@(p,x,y) sum((Yf(p,x)-y).^2);       % autonomous X2=chi^2
p=fminsearch(@(p) X2(p,x,y),[1 1 3]);  % find min of X2

plot(x,y,'o',x,p(1)*x.^2-p(2)*sin(p(3)*x),x,4*x.^2-2*sin(2*pi*x))
plot(x,y,'o',x,Yf(p,x),x,Yf([4 2 2*pi],x))


%% finer grid
xx=(-.1:.01:1.1)'; % finer mesh for fit function
plot(x,y,'o',xx,Yf(p,xx),xx,Yf([4 2 2*pi],xx))

%% Add weights
S=1/10*ones(size(x));      % most errors have std=1/10    
S(end)=5;                  % last point has std=5;
y=3*x-2+randn(size(x)).*S; % add noise with std S

X=[x ones(size(x))];       % X for X*A=y

Af=X\y;     % fit without considering S

% with weighting W=diag(1./S.^2) 
% minimize chi=(X*A-y)'*W*(X*A-y)=(X*A-y)'*sqrtm(W)*sqrtm(W)*(X*A-y)
% minimize e'*e, where e=sqrtm(W)*(X*A-y)
Awf=diag(1./S)*[x ones(size(x))]\diag(1./S)*y;        

% use matlab function lscov for more info 
[Awf1,stdA,mse,SS]=lscov([x ones(size(x))],y,1./S.^2);  

% alternative min chi=(X*A-y)'*W*(X*A-y)
% dchi/dA=(X*A-y)'*(W+W')*d(X*A-y)/dA=2*(X*A-y)'*W*X'==0
% 0==((X*A-y)'*W*X')'=X'*W*(X*A-y)
% X'*W*X*A=X'*W*y => A=inv(X'*W*X)*(X'*W*y)=X'*W*X\(X'*W*y)
W=diag(1./S.^2);
Awf2=X'*W*X\(X'*W*y);

plot(x,y,'o-',x,[x ones(size(x))]*Af,x,[x ones(size(x))]*Awf)