load loaddata.mat

%using variables interpolated to the dvl time
nmax=max(size(auvE));
%n=317
n=1500

position = [0 0 .979 .935];

co = [0 0 1;
      0 0.5 0;
      1 0 0;
      0 0.75 0.75;
      0.75 0 0.75;
      0.75 0.75 0;
      0.25 0.25 0.25];
set(groot,'defaultAxesColorOrder',co)

%n=max(size(auvE));
VN=zeros(1,n);
VE=zeros(1,n);
VD=zeros(1,n);
Ndot=zeros(1,n);
Edot=zeros(1,n);
%Ddot=zeros(1,n);
lambdaApprox=zeros(1,n);
rangeApprox=zeros(1,n);


E=auvE(1:n)-auvE(nmax);  %<-- that's about where dock is
N=auvN(1:n)-auvN(nmax);
%D=auvD-auvD(n);
t=time(1:n)-time(1);



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:n
    %R=quatEuler2R(roll(i),pitch(i),yaw(i));
    R=Euler2DC(roll(i),pitch(i),yaw(i));
    Vdum=R'*[vx(i);vy(i);vz(i)];
    VN(i)=Vdum(1);
    VE(i)=Vdum(2);
    VD(i)=Vdum(3);
    if i>1
        dt=time(i)-time(i-1);
        Ndot(i-1)=(auvN(i)-auvN(i-1))/dt;  %<-- off by one time step?
        Edot(i-1)=(auvE(i)-auvE(i-1))/dt;
        %Ddot(i)=(auvZ(i)-auvZ(i-1))/dt;
    end
    %rangeApprox(i)=sqrt(E(i)^2+N(i)^2);
    rangeApprox(i)=sqrt(E(i)^2+N(i)^2);
    lambdaApprox(i)=atan2(-E(i),-N(i));
    
end
%lambdaApprox=minuspi2pi(lambdaApprox);
%lambdaApprox=zeroto2pi(lambdaApprox);
lambdaApprox=wrapTo2Pi(lambdaApprox);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
pings=zeros(1,n);
RDAT=zeros(1,n);
LambdaDAT=zeros(1,n);
fpings=zeros(1,n);
fRDAT=zeros(1,n);
fLambdaDAT=zeros(1,n);


f=86400;   %conversion factor for time
measuredrange=data.DAT.acoustic_contact_range.value;
mrt=f*data.DAT.acoustic_contact_range.time;
measuredlambda=data.DAT.azimuth_vehicleFrame.value;
mlt=f*data.DAT.azimuth_vehicleFrame.time;
filteredrange=data.TrackAcousticContact.horizontal_range_to_contact_lowpass.value;
mf1t=f*data.TrackAcousticContact.horizontal_range_to_contact_lowpass.time;
filteredlambda=data.TrackAcousticContact.heading_to_contact_lowpass.value;
mft2=f*data.TrackAcousticContact.heading_to_contact_lowpass.time;

for ii=2:n
C1=and(mrt>time(ii-1),mrt<=time(ii));
C2=and(mlt>time(ii-1),mlt<=time(ii));
if and(sum(C1)>0,sum(C2)>0)
    pings(ii)=1;
    RDAT(ii)=median(measuredrange(C1));
    LambdaDAT(ii)=angle1minus2(median(measuredlambda(C2)),-yaw(ii));
end
end

for ii=2:n
C1=and(mf1t>time(ii-1),mf1t<=time(ii));
C2=and(mft2>time(ii-1),mft2<=time(ii));
if and(sum(C1)>0,sum(C2)>0)
    fpings(ii)=1;
    fRDAT(ii)=median(filteredrange(C1));
    fLambdaDAT(ii)=angle1minus2(median(filteredlambda(C2)),0);
end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

figure(1)
plot(E,N)
axis equal
grid
set(gcf,'units','normalized')
set(gcf,'position',position);

figure(2)
plot(t,VN,'k',t,Ndot,'b',t,VE,'g',t,Edot,'c')    
title('dvlV(k)      diffV(b)');
grid
set(gcf,'units','normalized')
set(gcf,'position',position);

figure(3)
plot(t,rangeApprox,'.b',t,RDAT,'.r',t,fRDAT,'.g')
title('range calcuated from E N ')
set(gcf,'units','normalized')
set(gcf,'position',position);

lambdaApprox=wrapToPi(lambdaApprox);
LambdaDAT=wrapToPi(LambdaDAT);
fLambdaDAT=wrapToPi(fLambdaDAT);
figure(4)
plot(t,lambdaApprox*180/pi,'.k',t,LambdaDAT*180/pi,'.g',t,fLambdaDAT*180/pi,'.r')
title('lambda calculated from E N(k)  DAT(r)')
ylabel('degrees')
set(gcf,'units','normalized')
set(gcf,'position',position);

% figure(5)
% plot(t,LambdaDAT*180/pi,'g*',t,fLambdaDAT*180/pi,'.r')
% set(gcf,'units','normalized')
% set(gcf,'position',position);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% get data
% lambdameas= measured los
% Range = measured range to target
% deltaEdot= (Edock-Eveh)dot rotated into inertial frame  deltaEdot=-Eauv for fixed dock
% deltaNdot= (Ndock-Nveh)dot rotated into inertail frame

lambdameas=lambdaApprox;
Rmeas=rangeApprox;
deltaEdot=-VE;
deltaNdot=-VN;


%quick checks
Rmeasdotout=zeros(1,n);
lambdameasdot=zeros(1,n);
lambdadotcalc=zeros(1,n);
Rdotcalc=zeros(1,n);

for i=2:n
    dt=t(i)-t(i-1);
    Rmeasdotout(i)=(Rmeas(i)-Rmeas(i-1))/dt;
    lambdameasdot(i)=angle1minus2(lambdameas(i),lambdameas(i-1))/dt;
    clambda=cos(lambdameas(i));
    slambda=sin(lambdameas(i));
    lambdadotcalc(i)=(1/Rmeas(i))*(clambda*deltaEdot(i) -slambda*deltaNdot(i));
    Rdotcalc(i)=slambda*deltaEdot(i) +clambda*deltaNdot(i);
end
figure(5)
plot(t,Rmeasdotout,'.k',t,Rdotcalc,'.r')
title('verify Rdot calculation matches Rdot')
set(gcf,'units','normalized')
set(gcf,'position',position);
figure(6)
plot(t,lambdameasdot,'.k',t,lambdadotcalc,'.r')
title('verify lambdameasdot calculation matches lambdadotcalc')
set(gcf,'units','normalized')
set(gcf,'position',position);
% 


% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% estimator with states (Edock-E), (Ndock-N)
if 1
    deltaNhatout=zeros(1,n);
    deltaEhatout=zeros(1,n);
    lambdaout=zeros(1,n);
    Rout=zeros(1,n);
    Ediff=zeros(1,n);
    Ndiff=zeros(1,n);
    datE=zeros(1,n);
    datN=zeros(1,n);
    fdatE=zeros(1,n);
    fdatN=zeros(1,n);

    Xbar=[-N(1);-E(1)];
    Xhat=Xbar;
    
    k=.1;
    


for i=2:n
    
    dt=time(i)-time(i-1);
    
    Xbar=Xhat+dt*eye(2)*[deltaNdot(i-1);deltaEdot(i-1)];
    
    deltaNbar=Xbar(1);
    deltaEbar=Xbar(2);
    if pings(i)==1
        R=RDAT(i);
        L=LambdaDAT(i);
        clambda=cos(L);
        slambda=sin(L);
        lambdabar=atan2(deltaEbar,deltaNbar);
        Rbar=sqrt(deltaEbar^2+deltaNbar^2);
        
        K=[k 0;0 k];
        innov=[Rbar*(angle1minus2(L,lambdabar)) ;R-Rbar];
        Xhat=Xbar+K*[-slambda clambda;clambda slambda]*innov;       
        
        Ediff(i)=Xhat(2)-R*sin(L);
        Ndiff(i)=Xhat(1)-R*cos(L);
        
       datE(i)=RDAT(i)*sin(LambdaDAT(i));
       datN(i)=RDAT(i)*cos(LambdaDAT(i));
       
       fdatE(i)=fRDAT(i)*sin(fLambdaDAT(i));
       fdatN(i)=fRDAT(i)*cos(fLambdaDAT(i));
    else
        Xhat=Xbar;
    end

    lambdaout(i)=atan2(Xhat(2),Xhat(1));
    Rout(i)=sqrt(Xhat(1)^2+Xhat(2)^2);
    deltaNhatout(i)=Xhat(1);
    deltaEhatout(i)=Xhat(2);

    
end

figure(20)
lambdameas=wrapTo2Pi(lambdameas);
lambdaout=wrapTo2Pi(lambdaout);
LambdaDAT=wrapTo2Pi(LambdaDAT);
% plot(t,lambdameas,'b.',t,lambdaout,'k.',t,LambdaDAT,'.r')
% title('LOS:  EN(b)    EKF(k)    DAT(r)')
plot(t,lambdaout,'k.',t,LambdaDAT,'.r')
title('LOS:    EKF(k)    DAT(r)')
set(gcf,'units','normalized')
set(gcf,'position',position);

% figure(21)
% plot(t,Rmeas,'b.',t,Rout,'k.',t,RDAT,'.r')
% title('Range:  EN(b)    EKF(k)    DAT(r)')
% grid

figure(21)
plot(t,Rout,'k',t,fRDAT,'.r')
title('Range:   EKF(k)    filteredDAT(r)')
xlabel('time')
grid
set(gcf,'units','normalized')
set(gcf,'position',position);


figure(22)
plot(t,Ediff,'.')
title('Eastings:  EKF-AHRS')
set(gcf,'units','normalized')
set(gcf,'position',position);

figure(23)
plot(t,Ndiff,'.')
title('Northings:  EKF-AHRS')
set(gcf,'units','normalized')
set(gcf,'position',position);

dist=50;
Cplot=and(Rout<dist,Rout>0);
Ctime=and(t>350,t<650);
C1=and(Cplot,Ctime);
C=and(Ctime,C1);
figure(24)
plot(deltaEhatout(C),deltaNhatout(C),'k',datE(C),datN(C),'.k',fdatE(C),fdatN(C),'.r')
vv=[-10 10 -10 10];
grid
axis(vv)
title('EKF(k)        filtered-Range-and-Bearing (.r)')
xlabel('Eastings')
ylabel('Northings')
set(gcf,'units','normalized')
set(gcf,'position',position);

Cplot=and(Rout<dist,Rout>0);
Ctime=and(t>1100,t<1450);
C1=and(Cplot,Ctime);
C=and(Ctime,C1);
figure(25)
plot(deltaEhatout(C),deltaNhatout(C),'k',datE(C),datN(C),'.k',fdatE(C),fdatN(C),'.r')
vv=[-10 10 -10 10];
grid
axis(vv)
title('EKF(k)        filtered-Range-and-Bearing (.r)')
xlabel('Eastings')
ylabel('Northings')
set(gcf,'units','normalized')
set(gcf,'position',position);

Cplot=and(Rout<dist,Rout>0);
Ctime=and(t>2550,t<3000);
C1=and(Cplot,Ctime);
C=and(Ctime,C1);
figure(26)
plot(deltaEhatout(C),deltaNhatout(C),'k',datE(C),datN(C),'.k',fdatE(C),fdatN(C),'.r')
vv=[-10 10 -10 10];
grid
axis(vv)
title('EKF(k)        filtered-Range-and-Bearing (.r)')
xlabel('Eastings')
ylabel('Northings')
set(gcf,'units','normalized')
set(gcf,'position',position);



     


end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
