% computeShipMotion.m
% compare motcorr Wplat to computed wplat from INS heave & angles for SeaState doy 275 hr 04

clear; close all;
motcorrLoad; % load PSD motcorr variables jd, uplat, vplat, wplat, etc.
skqLoad;     % load skq 1Hz jd_skq, roll_skq, pitch_skq, heading_skq, heave_skq
% skip time shift - this file of ship data already corrected
% jd_skq = jd_skq + 20.75/86400;  % ship UTC time lags psd GPS time by 20.75 sec
roll_skq = roll_skq*pi/180;
pitch_skq = -pitch_skq*pi/180;     % right handed convention
heading_skq = -heading_skq*pi/180; % right handed convention
heading_skq = unwrap(heading_skq);

% specify filter for derivative of heave and angles
pwelch(heave_skq,[],[],[],1);                   % check heave psd
Nf = 50;  Fpass = 0.45; Fstop = 0.48; Fs=1;     % filter parameters
d = designfilt('differentiatorfir','FilterOrder',Nf,...
    'PassbandFrequency',Fpass,'StopbandFrequency',Fstop,...
    'SampleRate',Fs);
fvtool(d,'MagnitudeDisplay','zero-phase','Fs',Fs); % check filter response

% apply differentiator filter to ship heave and angles
w_skq = filter(d,heave_skq);
ratex = filter(d,roll_skq);
ratey = filter(d,pitch_skq);
ratez = filter(d,heading_skq);
omega = [ratex,ratey,ratez];    % ship rotational rates in rad/sec
angles = [roll_skq,pitch_skq,heading_skq];

% trim time vector and filtered signals to correct for signal delay and edge transients
% arrays will be trimmed from 3600 to 3550 elements for delay = Nf/2 = 25
delay = mean(grpdelay(d));
jd_filt = jd_skq(1:end-delay); w_skq(1:delay) = []; % correct for delay
jd_filt(1:delay) = []; w_skq(1:delay) = [];         % remove transient
omega(1:2*delay,:) = [];    % trim anglular rate array by same amount as w_skq
angles(1:2*delay,:) = [];

% plot heave and w_skq, showing peaks and zero crossings - verify derivative filter is working
[pkp,lcp] = findpeaks(heave_skq);
zcp = zeros(size(lcp));
[pkm,lcm] = findpeaks(-heave_skq);
zcm = zeros(size(lcm));
figure; subplot(2,1,1)
plot(jd_skq,heave_skq,jd_skq([lcp;lcm]),[pkp;-pkm],'or')
xlabel('DOY'); ylabel('Displacement (m)'); grid on;
subplot(2,1,2)
plot(jd_filt,w_skq,jd_skq([lcp;lcm]),[zcp;zcm],'or')
xlabel('DOY'); ylabel('wplat (m/s)'); grid on;

% uvw velocities at ship center in fixed frame - assuming u,v = 0  at center of ship
uvw_skq = [zeros(length(w_skq),1),zeros(length(w_skq),1),w_skq];

% displacement vector x,y,z (right handed) from rotation center to top of tower
R = ones(length(omega),1)*[55,0,17];

% compute estimated uvw velocities at top of tower using ship heave and angular rates
uvw_rot = cross(omega,R);
uvw_tower = neaqs_trans(uvw_rot',angles',1); uvw_tower = uvw_tower';

% plot velocities from ship INS and psd motcorr routine
close all;
figure; plot(jd_filt,uvw_tower(:,3),'b',jd,wplat,'r');
ylabel('W plat');grid; legend('ship','motcorr');
figure; plot(jd_filt,uvw_tower(:,2),'b',jd,vplat,'r');
ylabel('V plat');grid; legend('ship','motcorr');
figure; plot(jd_filt,uvw_tower(:,1),'b',jd,uplat,'r');
ylabel('U plat');grid; legend('ship','motcorr');
