% projectileMotionFit.m
% W. Rose 20240201
% Estimate parameters of projectile motion
% Uses data generated by projectileMotion.m.
% See Collin Pecora, Matlab Answers 2024-02-01.
% See Warburton & Wang, 2004. 

clear

% Get data from file
%load('data0.mat');  % data with no noise
load('data1.mat');  % data with noise added
expnum=1;           % experiment to fit, =1, 2, or 3
% Select columns 1-5 or 6-10 or 11-15 for fitting.
measdat=reshape([data{5*expnum-4:5*expnum}],[],5);

% Prepare measured data to pass to fitting function
t=measdat(:,1);                     % measurement times
vxm=measdat(:,4);                   % measured vx
vym=measdat(:,5);                   % measured vy
% Find range by finding index when y is closest to 0. 
% Skip first 3 elements of y() when searching, to avoid finding y(t=0).
[~,idx]=min(abs(measdat(4:end,3))); % index of element with y closest to 0
measrng=measdat(idx+3,2);           % range
% Fit
p0=[100,45,.1];                     % initial guess [v0,theta,b]
opts=optimoptions(@fmincon,Display='Notify');
f = @(p)wSSE(p,[t,vxm,vym],measrng);
p=fmincon(f,p0,[],[],[],[],[1,0,.001],[1e4,90,100],[],opts);

% Fit is complete. Compute RMS error in velocity.
vxe=vxFcn(p(1),p(2),p(3),t);            % vx estimate (m/s)
vye=vyFcn(p(1),p(2),p(3),t);            % vy estimate (m/s)
rmserr=sqrt(sum((vxe-vxm).^2+(vye-vym).^2)/(length(vxm)+length(vym)));
% Display results on console
estrng=rangeFcn(p(1),p(2),p(3));        % range estimate (m)
fprintf('Fit: v0=%.1f m/s, theta=%.1f deg, b=%.3f /s.\n',p)
fprintf('Fit: Vel.rmse=%.1f m/s, rangeErr=%.1f m.\n',rmserr,estrng-measrng)
% Plot results
figure
subplot(211), plot(t,vxm,'r*',t,vxe,'-b')
grid on; xlabel('Time (s)'); ylabel('Vel.(m/s)'); 
title('Vx meas. & pred.'); legend('Meas','Pred')
subplot(212), plot(t,vym,'r*',t,vye,'-b')
grid on; xlabel('Time (s)'); ylabel('Vel.(m/s)'); 
title('Vy meas. & pred.'); legend('Meas','Pred')

function sse=wSSE(p,measdata,measrng)
v0=p(1);
theta=p(2);
b=p(3);
t=measdata(:,1);
vxm=measdata(:,2);
vym=measdata(:,3);
vxe=vxFcn(v0,theta,b,t);    % estimated vx
vye=vyFcn(v0,theta,b,t);    % estimated vy
estrng=rangeFcn(v0,theta,b);  % estimated range
sigmap=1;
sigmav=10;
sse=(measrng-estrng)^2/sigmap^2+...
    sum((vxm-vxe).^2)/sigmav^2+...
    sum((vym-vye).^2)/sigmav^2;
end

function x1 = xFcn(v0,theta,b,t)
%XFCN horizontal position function
% Inputs
%   v0      initial velocity (m/s)
%   theta   angle (deg)
%   b       air resistance (1/s)
%   t       time (s, scalar or vector)
% Output
%   x1     horiz position (m, scalar or vector, like t)
% See W&W 2004, eq. 3.  
x1=(v0*cosd(theta)/b)*(1-exp(-b*t));
end

function v1x = vxFcn(v0,theta,b,t)
%VXFCN horizontal velocity function
% Inputs
%   v0      initial velocity (m/s)
%   theta   angle (deg)
%   b       air resistance (1/s)
%   t       time (s, scalar or vector)
% Output
%   vx1     horiz veloc (m/s, scalar or vector, like t)
% See W&W 2004, eq. 2.
v1x=v0*cosd(theta)*exp(-b*t);
end

function y1 = yFcn(v0,theta,b,t)
%VYFCN vertical velocity function
% Inputs
%   v0      initial velocity (m/s)
%   theta   angle (deg)
%   b       air resistance (1/s)
%   t       time (s, scalar or vector)
% Output
%   vy1      vert veloc (m/s, scalar or vector, like t)
% See W&W 2004, eq. 4.
g=9.81;     % accel (m/s^2)
y1=((v0*sind(theta)+g/b)/b)*(1-exp(-b*t))-g*t/b;
end

function v1y = vyFcn(v0,theta,b,t)
%VYFCN vertical velocity function
% Inputs
%   v0      initial velocity (m/s)
%   theta   angle (deg)
%   b       air resistance (1/s)
%   t       time (s, scalar or vector)
% Output
%   vy1      vert veloc (m/s, scalar or vector, like t)
% See W&W 2004, eq. 4.
g=9.81;     % accel (m/s^2)
v1y=(v0*sind(theta)+g/b)*exp(-b*t)-g/b;
end

function R = rangeFcn(v0,theta,b)
%RANGEFCN range function
% Inputs
%   v0      initial velocity (m/s)
%   theta   angle (deg)
%   b       air resistance (1/s)
% Output
%   R       range (m)
% See W&W 2004, eqs. 20,21.
g=9.81;     % accel (m/s^2)
u0=v0*cosd(theta);
w0=v0*sind(theta);
z=-(1+b*w0/g)*exp(-(1+b*w0/g));
R=u0/b*(1+lambertw(z)/(1+b*w0/g));
end
    