clear;
fclose all;
randn('state',666); %make non-random (required for debugging)
PATH='C:\Work\Benisil';

%%Specify the imu error model and generate the path
[SenErrDef, IniErrDef, ObsErrDef, ini_pva, dt]=sys_path(PATH);
%copy the files for the second(lag state) and third(buffer) ins implementations
copyfile([PATH '\imu.bin'], [PATH '\imu1.bin']);
copyfile([PATH '\obs_pos.bin'], [PATH '\obs_pos1.bin']);
copyfile([PATH '\mnav.bin'], [PATH '\mnav1.bin']);
copyfile([PATH '\imuerr.bin'], [PATH '\imuerr1.bin']);

%%Open the files
F_IMU1=fopen([PATH '\imu.bin'],'rb');
F_IMU2=fopen([PATH '\imu1.bin'],'rb');
F_TRU1=fopen([PATH '\mnav.bin'],'rb');
F_TRU2=fopen([PATH '\mnav1.bin'],'rb');
F_IMUERR1=fopen([PATH '\imuerr.bin'],'rb');
F_IMUERR2=fopen([PATH '\imuerr1.bin'],'rb');
F_OBS_POS1=fopen([PATH '\obs_pos.bin'],'rb');
F_OBS_POS2=fopen([PATH '\obs_pos1.bin'],'rb');
F_RES1=fopen([PATH '\res1.bin'],'wb');
F_RES2=fopen([PATH '\res2.bin'],'wb');
F_RES3=fopen([PATH '\res3.bin'],'wb');

%%Program parameters
delay=11/dt; %delay for the fixed lag 
prp_per=0.5/dt; %propagation time for the discretized system
dx_upd_per=20/dt; %accumulator correction period 

%%Sensor error models
[dAimu, dBimu, Cimu, dDimu, sP0imu]=imu_modTI_v000(SenErrDef);  %errors models in discrete time
[Aimu, Qimu, Rimu]=dc2dc_v000(dAimu, dBimu*dBimu', dDimu*dDimu', dt, 1, []);  %models in continious time
%%%%%%%%Rimu=Rimu/dt; %%correction for the notation difference. (we need a standard notation for mems errors!!!!!)

%Initial covariance
P1=zeros(15);
P1(1:3,1:3)=IniErrDef.pos_sP*IniErrDef.pos_sP';
P1(4:6,4:6)=IniErrDef.vel_sP*IniErrDef.vel_sP';
P1(7:9,7:9)=IniErrDef.att_sP*IniErrDef.att_sP';
P1(10:15,10:15)=sP0imu*sP0imu';
P2=P1;

%imu error states
ximu1=zeros(6,1);
ximu2=zeros(6,1); %buffer ins

%INS registers
pos_n1=ini_pva(:,1);
att_n1=ini_pva(:,3);
Cbn1=euler2dcm_v000(att_n1);
vel_n1=Cbn1*ini_pva(:,2); %note path generator is defined for body frame velocity

pos_n2=pos_n1; %buffer ins
vel_n2=vel_n1;
Cbn2=Cbn1; 

%accumulators and control flags
avg_imu1=zeros(6,1);
avg_imu2=zeros(6,1);

pr_coun1=0;prp_coun1=0;
pr_coun2=0;prp_coun2=0;
dx_coun=0;
dx_flag=1;
dx_last_up=-1;

dx12_accum=zeros(15,1);
dx11_accum=zeros(15,1);
dP12_accum=zeros(15);
dP11_accum=zeros(15);
STM12=eye(15);
STM11=eye(15);
STM_upd=eye(15);

write_flag1=0;
write_flag2=0;

I=eye(15);

%%read the input data from the files
imu_data1=fread(F_IMU1,8,'double'); %garbage
imu_data1=fread(F_IMU1,8,'double');
imu_data2=fread(F_IMU2,8,'double'); %garbage
imu_data2=fread(F_IMU2,8,'double');
true_val1 = fread(F_TRU1, 10, 'double');
imu_err1=fread(F_IMUERR1, 7, 'double');
true_val2 = fread(F_TRU2, 10, 'double');
imu_err2=fread(F_IMUERR2, 7, 'double');

obs_pos1=fread(F_OBS_POS1, 4, 'double');
obs_pos2=fread(F_OBS_POS2, 4, 'double');

%%Start iterations
while (~feof(F_IMU2))
    if (~feof(F_IMU1)) %ins1 computations (innovation INS)
        imu1=imu_data1(3:8)-ximu1;
        [Cbn1, vel_n1, pos_n1]=strapdown_ned_dcm_v000(Cbn1, vel_n1, pos_n1, imu1(1:3), imu1(4:6), dt);
        avg_imu1=avg_imu1+imu1;
        
        pr_coun1=pr_coun1+1;
        prp_coun1=prp_coun1+1;
        dx_coun=dx_coun+1;
        
        if (mod(prp_coun1,prp_per)==0 && prp_coun1~=0) %propagate the covariance and estimates
            avg_imu1=avg_imu1/prp_coun1;
            
            %%Form the discrete time navigation model
            [STM1 Q1]=mdl_ned_dcm_v000(pos_n1, vel_n1, Cbn1, avg_imu1(1:3), Aimu, Qimu, Cimu, Rimu, prp_coun1*dt);
            
            %Propagate
            P1=STM1*P1*STM1'+Q1;
            STM12=STM1*STM12;
            STM11=STM1*STM11;
            STM_upd=STM1*STM_upd;
            prp_coun1=0;
            avg_imu1=zeros(6,1);
            
            %update IMU error states
            ximu1=STM1(10:15,10:15)*ximu1;
            
            write_flag1=1;
        end
        
        
        while (~isempty(obs_pos1) && (pr_coun1==obs_pos1(1)))
            %first check wheter the covariance is uptodate
            if (prp_coun1~=0)
                avg_imu1=avg_imu1/prp_coun1;
                [STM1 Q1]=mdl_ned_dcm_v000(pos_n1, vel_n1, Cbn1, avg_imu1(1:3), Aimu, Qimu, Cimu, Rimu, prp_coun1*dt);
                P1=STM1*P1*STM1'+Q1;
                STM12=STM1*STM12;
                STM11=STM1*STM11;
                STM_upd=STM1*STM_upd;
                prp_coun1=0;
                avg_imu1=zeros(6,1);
                ximu1=STM1(10:15,10:15)*ximu1;
                
            end
            
            H1=zeros(3,15); %position update
            H1(:,1:3)=eye(3);
            R1=ObsErrDef(1).sR*ObsErrDef(1).sR';
            inno1=pos_n1-obs_pos1(2:4);
            
            Rinno_inv1=inv(H1*P1*H1'+R1);
            K1=P1*H1'*Rinno_inv1;         
            
            %correct current states
            dx1=K1*inno1;
            pos_n1=pos_n1-dx1(1:3);
            vel_n1=vel_n1-dx1(4:6);
            mx_a=euler2dcm_v000(dx1(7:9));
            Cbn1=mx_a*Cbn1;
            ximu1=ximu1+dx1(10:15);
            %update cov
            P1=(I-K1*H1)*P1*(I-K1*H1)'+K1*R1*K1';

            %correct lagged states
            vr_a=STM12'*H1'*Rinno_inv1*inno1;
            dx12_accum=dx12_accum+vr_a;
            vr_a=STM11'*H1'*Rinno_inv1*inno1;
            dx11_accum=dx11_accum+vr_a;
            dP12_accum=dP12_accum+STM12'*H1'*Rinno_inv1*H1*STM12;
            dP11_accum=dP11_accum+STM11'*H1'*Rinno_inv1*H1*STM11;

            %prepare for the next cycle
            STM12=(I-K1*H1)*STM12;
            STM11=(I-K1*H1)*STM11;
            STM_upd=(I-K1*H1)*STM_upd;
            obs_pos1=fread(F_OBS_POS1, 4, 'double');
            
            write_flag1=1;
            
            %%dx_accumulator reset
            if ((dx_coun>dx_upd_per) && (dx_flag))
                STM11=eye(15);
                dx11_accum=zeros(15,1);
                dP11_accum=zeros(15);
                dx_flag=0;
                dx_coun=0;
                dx_last_up=pr_coun1;
            end
        end
        
        %True values
        true_val1 = fread(F_TRU1, 10, 'double');
        imu_err1=fread(F_IMUERR1, 7, 'double');
        
        %%%write the results
        %read the true values
        if (write_flag1)
            err1=zeros(15,1);
            err1(1:3)=pos_n1-true_val1(2:4);
            err1(4:6)=vel_n1-true_val1(5:7);
            err1(7:9)=-dcm2euler_v000(Cbn1*euler2dcm_v000(true_val1(8:10))');
            err1(10:15)=imu_err1(2:7)-ximu1;
            
            fwrite(F_RES1,[pr_coun1;err1;diag(P1).^0.5],'double');
            write_flag1=0;
            
        end
        
        %read the next imu output
        imu_data1=fread(F_IMU1,8,'double');
    end
        
    %INS2 and INS3 computations (lagged state, buffer ins)
    if (pr_coun1>=delay)
        imu2=imu_data2(3:8)-ximu2;
        [Cbn2, vel_n2, pos_n2]=strapdown_ned_dcm_v000(Cbn2, vel_n2, pos_n2, imu2(1:3), imu2(4:6), dt);
        avg_imu2=avg_imu2+imu2;
    
        pr_coun2=pr_coun2+1;
        prp_coun2=prp_coun2+1;
        
        if (mod(prp_coun2,prp_per)==0 && prp_coun2~=0) %propagate the covariance
            %ins 2
            avg_imu2=avg_imu2/prp_coun2;
            [STM2 Q2]=mdl_ned_dcm_v000(pos_n2, vel_n2, Cbn2, avg_imu2(1:3), Aimu, Qimu, Cimu, Rimu, prp_coun2*dt);
            STM2_inv=inv(STM2);
            P2=STM2*P2*STM2'+Q2;
            prp_coun2=0;
            avg_imu2=zeros(6,1);
            
            %update IMU error states
            ximu2=STM2(10:15,10:15)*ximu2;
            
            %ins 2
            dx12_accum=STM2_inv'*dx12_accum;
            dP12_accum=STM2_inv'*dP12_accum*STM2_inv;
            
            %STM
            STM12=STM12*STM2_inv;
            
            write_flag2=1;
        end
        
        while ~isempty(obs_pos2) && pr_coun2==obs_pos2(1)
            %first check wheter the covariance is uptodate
            if (prp_coun2~=0)
                %ins 2
                avg_imu2=avg_imu2/prp_coun2;
                [STM2 Q2]=mdl_ned_dcm_v000(pos_n2, vel_n2, Cbn2, avg_imu2(1:3), Aimu, Qimu, Cimu, Rimu, prp_coun2*dt);
                STM2_inv=inv(STM2);
                P2=STM2*P2*STM2'+Q2;
                prp_coun2=0;
                avg_imu2=zeros(6,1);

                %update IMU error states
                ximu2=STM2(10:15,10:15)*ximu2;

                %ins 2 (FL smoothed) correction values
                dx12_accum=STM2_inv'*dx12_accum;
                dP12_accum=STM2_inv'*dP12_accum*STM2_inv;

                %STM
                STM12=STM12*STM2_inv;
            end
            
            H2=zeros(3,15);
            H2(:,1:3)=eye(3);
            R2=ObsErrDef(1).sR*ObsErrDef(1).sR';
            inno2=pos_n2-obs_pos2(2:4);
                        
            Rinno_inv2=inv(H2*P2*H2'+R2);
            K2=P2*H2'*Rinno_inv2;

            %correct current states
            dx2=K2*inno2;
            pos_n2=pos_n2-dx2(1:3);
            vel_n2=vel_n2-dx2(4:6);
            mx_a=euler2dcm_v000(dx2(7:9));
            Cbn2=mx_a*Cbn2;
            ximu2=ximu2+dx2(10:15);
            P2=(I-K2*H2)*P2*(I-K2*H2)'+K2*R2*K2';

            %update smoother's accumulators
            STM2_inv=inv(I-K2*H2);
            STM12=STM12*STM2_inv;

            dx12_accum=dx12_accum-H2'*Rinno_inv2*inno2;
            dx12_accum=STM2_inv'*dx12_accum;
            
            dP12_accum=dP12_accum-H2'*Rinno_inv2*H2;
            dP12_accum=STM2_inv'*dP12_accum*STM2_inv;
            
            %prepare for the next cycle
            obs_pos2=fread(F_OBS_POS2, 4, 'double');
            write_flag2=1;
            
            if ((pr_coun2==dx_last_up) && (dx_flag==0))     %assign buffer to lagged accumulators to suppress the numerical precision problems (a simple stupid way to reduce the effect of implicit instability of fixed lag smoothers)
                STM12=STM11;
                dx12_accum=dx11_accum;
                dP12_accum=dP11_accum;
                dx_flag=1;
            end

        end
        
       
        %%%write the results
        true_val2 = fread(F_TRU2, 10, 'double');
        imu_err2=fread(F_IMUERR2, 7, 'double');
        if (write_flag2)
            %smoothed values
            dx12=P2*dx12_accum;
            pos_fls=pos_n2-dx12(1:3);
            vel_fls=vel_n2-dx12(4:6);
            mx_a=euler2dcm_v000(dx12(7:9));
            Cbn_fls=mx_a*Cbn2;
            ximu_fls=ximu2+dx12(10:15);
                        
            err2=zeros(15,1);
            err2(1:3)=pos_fls-true_val2(2:4);
            err2(4:6)=vel_fls-true_val2(5:7);
            err2(7:9)=-dcm2euler_v000(Cbn_fls*euler2dcm_v000(true_val2(8:10))');
            err2(10:15)=imu_err2(2:7)-ximu_fls;
            
            %smoothed covariance
            P_fls=(I-P2*dP12_accum)*P2;
            fwrite(F_RES2,[pr_coun2;err2;diag(P_fls).^0.5],'double');
            
            write_flag2=0;
            
%             %write the 2nd ins outputs (not needed at all, just for
%             %checking
%             %read the true values
%             err3=zeros(nstate,1);
%             err3(1:3)=pos2-true_val2(2:4);
%             err3(4:6)=vel2-true_val2(5:7);
%             err3(7:9)=-dcm2euler_v(Cbn2*euler2dcm_v(true_val2(8:10))');
%             err3(10:15)=true_val2(11:16)-imu_err_est2;
%             fwrite(F_RES3,[pr_coun2;err3;diag(P2).^0.5],'double');
        end
        
        %read the next imu output
        imu_data2=fread(F_IMU2,8,'double');
    end
end


fclose all;
figure;
res1=readbin_v000([PATH '\res1.bin'],31);
res2=readbin_v000([PATH '\res2.bin'],31);
plot(res1(1,:)/100,abs(res1(2,:))*6e6)
hold on
plot(res2(1,:)/100,abs(res2(2,:))*6e6,'r')
plot(res1(1,:)/100,(res1(17,:))*6e6,'--');
plot(res2(1,:)/100,(res2(17,:))*6e6,'r--');
title('lat. error (Note that after each update smt=filt for 4 seconds as fls period=11)');
ylabel('m');
xlabel('sec');
legend('filter err.', 'FLS err.','filter std', 'FLS std');
grid;
return;


figure;
yy=6;
plot(res1(1,:)/100,abs(res1(yy,:)))
hold on
plot(res2(1,:)/100,abs(res2(yy,:)),'r')
plot(res1(1,:)/100,(res1(16+yy,:)),'--');
plot(res2(1,:)/100,(res2(16+yy,:)),'r--');
xlabel('sec');
legend('filter', 'fls');
grid;
