From 9b740b454d099539a2c1ceff7f3e60fd99089d36 Mon Sep 17 00:00:00 2001
From: miguekf <angel.glopez@alumnos.upm.es>
Date: Sun, 11 Dec 2022 02:11:12 +0100
Subject: [PATCH] MOD: updated matlab vtl_kf

---
 src/utils/matlab/vtl/kf_prototype.m           | 121 +++++++--------
 .../vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m    |   5 +-
 src/utils/matlab/vtl/vtl_general_plot.m       | 134 +++++++++++++++--
 src/utils/matlab/vtl/vtl_prototype.m          | 138 +++---------------
 4 files changed, 205 insertions(+), 193 deletions(-)

diff --git a/src/utils/matlab/vtl/kf_prototype.m b/src/utils/matlab/vtl/kf_prototype.m
index 806c2139b..6367f121c 100644
--- a/src/utils/matlab/vtl/kf_prototype.m
+++ b/src/utils/matlab/vtl/kf_prototype.m
@@ -28,36 +28,7 @@ kf_yerr = zeros(2*sat_number, 1);
 % kf_xerr = zeros(8, 1);
 kf_S = zeros(2*sat_number, 2*sat_number); % kf_P_y innovation covariance matrix
 
-%% State error Covariance Matrix Q (PVT)
-kf_Q = [    100     0     0     0     0     0     0     0
-            0     100     0     0     0     0     0     0
-            0     0     100     0     0     0     0     0
-            0     0     0     10     0     0     0     0
-            0     0     0     0     10     0     0     0
-            0     0     0     0     0     10     0     0
-            0     0     0     0     0     0     500     0
-            0     0     0     0     0     0     0     1500];%careful, values for V and T could not be adecuate.
-%%
-
-% % Measurement error Covariance Matrix R assembling
-% for chan=1:5 %neccesary quantities
-%     % It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error)
-%     kf_R(chan, chan) = 40.0; %TODO: fill with real values.
-%     kf_R(chan+sat_number, chan+sat_number) = 10.0;
-% end
-
-kf_R=[  3     0     0     0     0     0     0     0     0     0
-        0     3     0     0     0     0     0     0     0     0
-        0     0     3     0     0     0     0     0     0     0
-        0     0     0     3     0     0     0     0     0     0
-        0     0     0     0     3     0     0     0     0     0
-        0     0     0     0     0     30     0     0     0     0
-        0     0     0     0     0     0     30     0     0     0
-        0     0     0     0     0     0     0     30     0     0
-        0     0     0     0     0     0     0     0     30     0
-        0     0     0     0     0     0     0     0     0     30];
-    
-% pre-allocate for speed     
+%% pre-allocate for speed     
 d = zeros(sat_number, 1);
 rho_pri = zeros(sat_number, 1);
 rhoDot_pri = zeros(sat_number, 1);
@@ -73,7 +44,21 @@ sat_dopp_hz_filt=zeros(sat_number,length(navSolution.RX_time));
 %% ################## Kalman Tracking ######################################
 % receiver solution from rtklib_solver
 for t=2:length(navSolution.RX_time)
-   
+
+    %% State error Covariance Matrix Q (PVT) and R (MEASUREMENTS)
+    if (t<length(navSolution.RX_time)-point_of_closure)
+        %% State error Covariance Matrix Q (PVT)
+        kf_Q = eye(8);%new_data.rx_pvt_var(i); %careful, values for V and T could not be adecuate.
+        %%
+
+        % Measurement error Covariance Matrix R assembling
+        kf_R = blkdiag(eye(sat_number)*40,eye(sat_number)*10);
+
+    else
+        kf_Q = blkdiag(eye(3)*pos_var,eye(3)*vel_var,clk_bias_var,clk_drift_var);
+        kf_R = blkdiag(eye(sat_number)*pr_var,eye(sat_number)*pr_dot_var);
+    end
+
     clear x_u y_u z_u xDot_u yDot_u zDot_u cdeltatDot_u cdeltatDot_u_g cdeltat_u_g
     if (t<length(navSolution.RX_time)-point_of_closure)
         kf_x(1,t) = navSolution.X(t);
@@ -112,44 +97,44 @@ for t=2:length(navSolution.RX_time)
         kf_P_x= kf_F * kf_P_x * kf_F' + kf_Q;% state error covariance prediction
     end
 
-    
+
     for chan=1:5 %neccesary quantities
-            d(chan)=(sat_posX_m(chan,t)-x_u)^2;
-            d(chan)=d(chan)+(sat_posY_m(chan,t)-y_u)^2;
-            d(chan)=d(chan)+(sat_posZ_m(chan,t)-z_u)^2;
-            d(chan)=sqrt(d(chan));
-            
-            c_pr_m(chan,t)=d(chan)+cdeltat_u(t);
-            
-            a_x(chan,t)=-(sat_posX_m(chan,t)-x_u)/d(chan);
-            a_y(chan,t)=-(sat_posY_m(chan,t)-y_u)/d(chan);
-            a_z(chan,t)=-(sat_posZ_m(chan,t)-z_u)/d(chan);
-            
-            rhoDot_pri(chan,t)=(sat_velX(chan,t)-xDot_u)*a_x(chan,t)...
-                +(sat_velY(chan,t)-yDot_u)*a_y(chan,t)...
-                +(sat_velZ(chan,t)-zDot_u)*a_z(chan,t);
+        d(chan)=(sat_posX_m(chan,t)-x_u)^2;
+        d(chan)=d(chan)+(sat_posY_m(chan,t)-y_u)^2;
+        d(chan)=d(chan)+(sat_posZ_m(chan,t)-z_u)^2;
+        d(chan)=sqrt(d(chan));
+
+        c_pr_m(chan,t)=d(chan)+cdeltat_u(t);
+
+        a_x(chan,t)=-(sat_posX_m(chan,t)-x_u)/d(chan);
+        a_y(chan,t)=-(sat_posY_m(chan,t)-y_u)/d(chan);
+        a_z(chan,t)=-(sat_posZ_m(chan,t)-z_u)/d(chan);
+
+        rhoDot_pri(chan,t)=(sat_velX(chan,t)-xDot_u)*a_x(chan,t)...
+            +(sat_velY(chan,t)-yDot_u)*a_y(chan,t)...
+            +(sat_velZ(chan,t)-zDot_u)*a_z(chan,t);
     end
-    
-    
-    
+
+
+
     for chan=1:5 % Measurement matrix H assembling
-            % It has 8 columns (8 states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error)
-            kf_H(chan, 1) = a_x(chan,t); kf_H(chan, 2) = a_y(chan,t); kf_H(chan, 3) = a_z(chan,t); kf_H(chan, 7) = 1.0;
-            kf_H(chan+sat_number, 4) = a_x(chan,t); kf_H(chan+sat_number, 5) = a_y(chan,t); kf_H(chan+sat_number, 6) = a_z(chan,t); kf_H(chan+sat_number, 8) = 1.0;
+        % It has 8 columns (8 states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error)
+        kf_H(chan, 1) = a_x(chan,t); kf_H(chan, 2) = a_y(chan,t); kf_H(chan, 3) = a_z(chan,t); kf_H(chan, 7) = 1.0;
+        kf_H(chan+sat_number, 4) = a_x(chan,t); kf_H(chan+sat_number, 5) = a_y(chan,t); kf_H(chan+sat_number, 6) = a_z(chan,t); kf_H(chan+sat_number, 8) = 1.0;
     end
-    
-%     unobsv(t) = length(kf_F) - rank(obsv(kf_F,kf_H));
-% !!!! Limitaciones
-% obsv no se recomienda para el diseño de control, ya que calcular el rango de la matriz de observabilidad 
-% no se recomienda para las pruebas de observabilidad. Ob será numéricamente singular para la mayoría de los 
-% sistemas con más de unos cuantos estados. Este hecho está bien documentado en la sección III de [1].
+
+    %     unobsv(t) = length(kf_F) - rank(obsv(kf_F,kf_H));
+    % !!!! Limitaciones
+    % obsv no se recomienda para el diseño de control, ya que calcular el rango de la matriz de observabilidad
+    % no se recomienda para las pruebas de observabilidad. Ob será numéricamente singular para la mayoría de los
+    % sistemas con más de unos cuantos estados. Este hecho está bien documentado en la sección III de [1].
 
     % Kalman estimation (measurement update)
     for chan=1:5 % Measurement matrix H assembling
-            kf_yerr(chan,t)=c_pr_m(chan,t)-sat_prg_m(chan,t);%-0.000157*SPEED_OF_LIGHT_M_S;
-            kf_yerr(chan+sat_number,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1)-rhoDot_pri(chan,t);
+        kf_yerr(chan,t)=c_pr_m(chan,t)-sat_prg_m(chan,t);%-0.000157*SPEED_OF_LIGHT_M_S;
+        kf_yerr(chan+sat_number,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1)-rhoDot_pri(chan,t);
     end
-    
+
     % DOUBLES DIFFERENCES
     % kf_yerr = zeros(2*sat_number, 1);
     % for (int32_t i = 1; i < sat_number; i++) % Measurement vector
@@ -160,13 +145,13 @@ for t=2:length(navSolution.RX_time)
     %     kf_yerr(i+sat_number)=kf_y(i+sat_number)-(new_data.doppler_hz(i)-new_data.doppler_hz(i-1));
     % }
     % kf_yerr.print("DOUBLES DIFFERENCES");
-    
+
     % Kalman filter update step
     kf_S = kf_H * kf_P_x* kf_H' + kf_R; % innovation covariance matrix (S)
     kf_K = (kf_P_x * kf_H') * inv(kf_S); % Kalman gain
     kf_xerr(:,t) = kf_K * (kf_yerr(:,t)); % Error state estimation
     kf_P_x = (eye(length(kf_P_x)) - kf_K * kf_H) * kf_P_x; % update state estimation error covariance matrix
-    
+
     if (t<length(navSolution.RX_time)-point_of_closure)
         corr_kf_state(:,t)=kf_xpri(:,t)-kf_xerr(:,t)+kf_x(:,t); %updated state estimation
     else
@@ -178,11 +163,11 @@ for t=2:length(navSolution.RX_time)
     x_u=corr_kf_state(1,t);
     y_u=corr_kf_state(2,t);
     z_u=corr_kf_state(3,t);
-    xDot_u=corr_kf_state(4,t);  
-    yDot_u=corr_kf_state(5,t);  
-    zDot_u=corr_kf_state(6,t);  
-    cdeltat_u_g=corr_kf_state(7,t);  
-    cdeltatDot_u_g=corr_kf_state(8,t);  
+    xDot_u=corr_kf_state(4,t);
+    yDot_u=corr_kf_state(5,t);
+    zDot_u=corr_kf_state(6,t);
+    cdeltat_u_g=corr_kf_state(7,t);
+    cdeltatDot_u_g=corr_kf_state(8,t);
 
     for chan=1:5 %neccesary quantities
         d(chan)=(sat_posX_m(chan,t)-x_u)^2;
diff --git a/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m b/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m
index fa4abe1f3..42626a591 100644
--- a/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m
+++ b/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m
@@ -52,6 +52,7 @@ end
 if(load_vtl)
 vtlSolution = Vtl2struct('dump_vtl_file.csv');
 end
+
 %% calculate LOS Rx-sat if advance_vtl_data_available=1
 
 if(advance_vtl_data_available)
@@ -347,4 +348,6 @@ if(load_observables)
     plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,1)','.','DisplayName','reference')
     hold off
     grid on
-end
\ No newline at end of file
+end
+%%
+dopp_filtered_plotting
\ No newline at end of file
diff --git a/src/utils/matlab/vtl/vtl_general_plot.m b/src/utils/matlab/vtl/vtl_general_plot.m
index 3cf6ef538..ea760bedc 100644
--- a/src/utils/matlab/vtl/vtl_general_plot.m
+++ b/src/utils/matlab/vtl/vtl_general_plot.m
@@ -1,6 +1,56 @@
 %%
 % vtl_general_plot.m
 %%
+time_reference_spirent_obs=129780;%s
+
+if(load_observables)
+
+    Carrier_Doppler_hz_sim=zeros(length(refSatData.GPS.SIM_time),6);
+
+    for i=1:length(refSatData.GPS.SIM_time)
+        Carrier_Doppler_hz_sim(i,1)=refSatData.GPS.series(i).doppler_shift(4);%PRN 28
+        Carrier_Doppler_hz_sim(i,2)=refSatData.GPS.series(i).doppler_shift(1);%PRN 4
+        Carrier_Doppler_hz_sim(i,3)=refSatData.GPS.series(i).doppler_shift(6);%PRN 17
+        Carrier_Doppler_hz_sim(i,4)=refSatData.GPS.series(i).doppler_shift(7);%PRN 15
+        Carrier_Doppler_hz_sim(i,5)=refSatData.GPS.series(i).doppler_shift(8);%PRN 27
+        Carrier_Doppler_hz_sim(i,6)=refSatData.GPS.series(i).doppler_shift(9);%PRN 9
+
+        Pseudorange_m_sim(i,1)=refSatData.GPS.series(i).pr_m(4);%PRN 28
+        Pseudorange_m_sim(i,2)=refSatData.GPS.series(i).pr_m(1);%PRN 4
+        Pseudorange_m_sim(i,3)=refSatData.GPS.series(i).pr_m(6);%PRN 17
+        Pseudorange_m_sim(i,4)=refSatData.GPS.series(i).pr_m(7);%PRN 15
+        Pseudorange_m_sim(i,5)=refSatData.GPS.series(i).pr_m(8);%PRN 27
+        Pseudorange_m_sim(i,6)=refSatData.GPS.series(i).pr_m(9);%PRN 9
+    end
+end
+% -------------------------------------
+% if(load_observables)
+%     Rx_pseudo_all=figure('Name','RX_pr_m');plot(RX_time(1,:)-time_reference_spirent_obs, Pseudorange_m','s')
+%     xlim([0,140]);
+%     xlabel('')
+%     ylabel('Pseudorange (m)')
+%     xlabel('time from simulation init (seconds)')
+%     grid on
+%     hold on
+%     plot(refSatData.GPS.SIM_time/1000, Pseudorange_m_sim','.')
+%     plot(navSolution.RX_time(1,:)-time_reference_spirent_obs, pr_m_filt,'o')
+%     legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','PRN 9','Location','eastoutside')
+%     hold off
+%     grid on
+% 
+%     Rx_pseudo_one=figure('Name','RX_pr_m');plot(RX_time(1,:)-time_reference_spirent_obs, Pseudorange_m(1,:)','s')
+%     xlim([0,140]);
+%     xlabel('')
+%     ylabel('Pseudorange (m)')
+%     xlabel('time from simulation init (seconds)')
+%     grid on
+%     hold on
+%     legend('PRN 28 GNSS-SDR','Location','eastoutside')
+%     plot(refSatData.GPS.SIM_time/1000, Pseudorange_m_sim(:,1)','.','DisplayName','reference')
+%     plot(navSolution.RX_time(1,:)-time_reference_spirent_obs, pr_m_filt(1,:),'o','DisplayName','filtered VTL')
+%     hold off
+%     grid on
+% end
 %---VTL VELOCITY: GNSS SDR plot --------------------------------------
 VTL_VEL=figure('Name','velocities');
 subplot(2,2,1);
@@ -14,7 +64,7 @@ ylabel('vX (m/s)')
 xlabel('time U.A.')
 ylim([-5 5])
 title('Subplot 1: vX ')
-legend ('raw navSolution','raw kf state','reference','Location','east')
+legend ('raw navSolution','raw kf state','reference','Location','eastoutside')
 
 subplot(2,2,2);
 plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.vY,'.');
@@ -27,7 +77,7 @@ ylabel('vY (m/s)')
 xlabel('time U.A.')
 ylim([-5 5])
 title('Subplot 1: vY ')
-legend ('raw navSolution','raw kf state','reference','Location','east')
+legend ('raw navSolution','raw kf state','reference','Location','eastoutside')
 
 subplot(2,2,3);
 plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.vZ,'.');
@@ -40,7 +90,7 @@ ylabel('vZ (m/s)')
 xlabel('time U.A.')
 ylim([-5 5])
 title('Subplot 1: vZ ')
-legend ('raw navSolution','raw kf state','reference','Location','east')
+legend ('raw navSolution','raw kf state','reference','Location','eastoutside')
 
 sgtitle('velocities') 
 
@@ -50,38 +100,104 @@ VTL_POS=figure('Name','VTL UTM COORD CENTERED IN 1^{ST} POSITION');
 subplot(2,2,1);
 plot(navSolution.X-navSolution.X(1),'.');
 hold on;grid on
-plot(corr_kf_state(1,:)-corr_kf_state(1,1),'.');
+plot(corr_kf_state(1,3:end)-corr_kf_state(1,3))
 plot(kf_xerr(1,:),'.');
 ylabel('X (m)')
 xlabel('time U.A.')
 ylim([-200 800])
 title('Subplot 1: X ')
-legend ('raw navSolution','raw kf state','kferr','Location','east')
+legend ('raw navSolution','raw kf state','kferr','Location','eastoutside')
 
 subplot(2,2,2);
 plot(navSolution.Y-navSolution.Y(1),'.');
 hold on;grid on
-plot(corr_kf_state(2,:)-corr_kf_state(2,1),'.');
+plot(corr_kf_state(2,3:end)-corr_kf_state(2,3))
 plot(kf_xerr(2,:),'.');
 ylabel('Y (m)')
 xlabel('time U.A.')
 ylim([-200 50])
 title('Subplot 1: Y ')
-legend ('raw navSolution','raw kf state','kferr','Location','east')
+legend ('raw navSolution','raw kf state','kferr','Location','eastoutside')
 
 subplot(2,2,3);
 plot(navSolution.Z-navSolution.Z(1),'.');
 hold on;grid on
-plot(corr_kf_state(3,:)-corr_kf_state(3,1),'.');
+plot(corr_kf_state(3,3:end)-corr_kf_state(3,3))
 plot(kf_xerr(3,:),'.');
 ylabel('Z (m)')
 xlabel('time U.A.')
 ylim([-350 50])
 title('Subplot 1: Z ')
-legend ('raw navSolution','raw kf state','kferr','Location','east')
+legend ('raw navSolution','raw kf state','kferr','Location','eastoutside')
 
 sgtitle('VTL UTM COORD CENTERED IN 1^{ST} POSITION') 
 %%
+if(load_observables)
+%     Rx_Dopp_all=figure('Name','RX_Carrier_Doppler_hz');plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz','s')
+%     xlim([0,140]);
+%     xlabel('')
+%     ylabel('Doppler (Hz)')
+%     xlabel('time from simulation init (seconds)')
+%     grid on
+%     hold on
+%     plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim','.')
+%     plot(navSolution.RX_time(1,:)-time_reference_spirent_obs, sat_dopp_hz_filt,'o')
+%     legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','PRN 9','Location','eastoutside')
+%     hold off
+%     grid on
+
+    Rx_Dopp_28=figure('Name','RX_Carrier_Doppler_hz');plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(1,:)','s')
+    xlim([0,140]);
+    ylim([-2340,-2220]);
+    xlabel('')
+    ylabel('Doppler (Hz)')
+    xlabel('time from simulation init (seconds)')
+    grid on
+    hold on
+    legend('PRN 28 GNSS-SDR','Location','eastoutside')
+    plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,1)','.','DisplayName','reference')
+    plot(navSolution.RX_time(1,:)-time_reference_spirent_obs, sat_dopp_hz_filt(1,:),'o','DisplayName','filtered VTL')
+    hold off;grid minor
+
+    Rx_Dopp_4=figure('Name','RX_Carrier_Doppler_hz');plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(2,:)','s')
+    xlim([0,140]);  
+    ylim([2540,2640]);
+    xlabel('')
+    ylabel('Doppler (Hz)')
+    xlabel('time from simulation init (seconds)')
+    grid on
+    hold on
+    legend('PRN 4 GNSS-SDR','Location','eastoutside')
+    plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,2)','.','DisplayName','reference')
+    plot(navSolution.RX_time(1,:)-time_reference_spirent_obs, sat_dopp_hz_filt(2,:),'o','DisplayName','filtered VTL')
+    hold off;grid minor
+
+        Rx_Dopp_17=figure('Name','RX_Carrier_Doppler_hz');plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(3,:)','s')
+    xlim([0,140]);  
+    ylim([-1800,-1730]);
+    xlabel('')
+    ylabel('Doppler (Hz)')
+    xlabel('time from simulation init (seconds)')
+    grid on
+    hold on
+    legend('PRN 17 GNSS-SDR','Location','eastoutside')
+    plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,3)','.','DisplayName','reference')
+    plot(navSolution.RX_time(1,:)-time_reference_spirent_obs, sat_dopp_hz_filt(3,:),'o','DisplayName','filtered VTL')
+    hold off;grid minor
+
+        Rx_Dopp_15=figure('Name','RX_Carrier_Doppler_hz');plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(4,:)','s')
+    xlim([0,140]);  
+    ylim([-2680,-2620]);
+    xlabel('')
+    ylabel('Doppler (Hz)')
+    xlabel('time from simulation init (seconds)')
+    grid on
+    hold on
+    legend('PRN 15 GNSS-SDR','Location','eastoutside')
+    plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,4)','.','DisplayName','reference')
+    plot(navSolution.RX_time(1,:)-time_reference_spirent_obs, sat_dopp_hz_filt(4,:),'o','DisplayName','filtered VTL')
+    hold off;grid minor
+end
 %% STATE PLOT
 VTL_STATE=figure('Name','VTL STATE');
 subplot(2,2,1);
diff --git a/src/utils/matlab/vtl/vtl_prototype.m b/src/utils/matlab/vtl/vtl_prototype.m
index e29ce4208..19b6ec5ce 100644
--- a/src/utils/matlab/vtl/vtl_prototype.m
+++ b/src/utils/matlab/vtl/vtl_prototype.m
@@ -23,8 +23,24 @@ clearvars
 % end
 SPEED_OF_LIGHT_M_S=299792458.0;
 Lambda_GPS_L1=0.1902937;
-point_of_closure=3000;
-%%
+point_of_closure=6000;
+
+%% ==================== VARIANCES =============================
+pos_var=100;%m^2
+vel_var=10;%m^2/s^2
+clk_bias_var=40;%m^2
+clk_drift_var=1500;%m^2/s^2
+pr_var=10;%m^2
+pr_dot_var=30;%m^2/s^2
+
+% CARLES PAPER LTE GNSS VTL
+% pos_var=2;%m^2
+% vel_var=0.2;%m^2/s^2
+% clk_bias_var=1e-7;%m^2
+% clk_drift_var=1e-4;%m^2/s^2
+% pr_var=20;%m^2
+% pr_dot_var=3;%m^2/s^2
+%% ============================================================
 samplingFreq=5000000;
 channels=6;
 TTFF_sec=41.48;
@@ -50,46 +66,10 @@ if(load_observables)
 end
 %%
 % vtlSolution = Vtl2struct('dump_vtl_file.csv');
-%% calculate LOS Rx-sat
-
-% for chan=1:5
-%     for t=1:length(navSolution.RX_time)
-%         d(chan)=(sat_posX_m(chan,t)-navSolution.X(t))^2;
-%         d(chan)=d(chan)+(sat_posY_m(chan,t)-navSolution.Y(t))^2;
-%         d(chan)=d(chan)+(sat_posZ_m(chan,t)-navSolution.Z(t))^2;
-%         d(chan)=sqrt(d(chan));
-%         
-%         c_pr_m(chan,t)=d(chan)+clk_bias_s(t)*SPEED_OF_LIGHT_M_S;
-%         
-%         a_x(chan,t)=-(sat_posX_m(chan,t)-navSolution.X(t))/d(chan);
-%         a_y(chan,t)=-(sat_posY_m(chan,t)-navSolution.Y(t))/d(chan);
-%         a_z(chan,t)=-(sat_posZ_m(chan,t)-navSolution.Z(t))/d(chan);
-%     end
-% end
-% 
-% %%
-
-% %%
-% for chan=1:5
-%     for t=1:length(navSolution.RX_time)
-%         rhoDot_pri(chan,t)=(sat_velX(chan,t)-navSolution.vX(t))*a_x(chan,t)...
-%             +(sat_velY(chan,t)-navSolution.vY(t))*a_y(chan,t)...
-%             +(sat_velZ(chan,t)-navSolution.vZ(t))*a_z(chan,t);
-%         
-%         kf_yerr(chan,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1)-rhoDot_pri(chan,t);
-%     end
-% end
-
 %%
 
 kf_prototype
 
-%%
-figure;plot(kf_yerr(1:5,:)');title('c pr m-error');xlabel('t U.A');ylabel('pr m [m]');grid minor
-legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','Location','eastoutside')
-figure;plot(kf_yerr(6:10,:)');title('c pr m DOT-error');xlabel('t U.A');ylabel('pr m dot [m/s]');grid minor
-legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','Location','eastoutside')
-
 %% ====== FILTERING =======================================================
 % moving_avg_factor= 500;
 % LAT_FILT = movmean(navSolution.latitude,moving_avg_factor);
@@ -109,81 +89,9 @@ legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','Location','eastoutside')
 vtl_general_plot
 
 %% ============================================== ==============================================
-time_reference_spirent_obs=129780;%s
-
-if(load_observables)
-
-    Carrier_Doppler_hz_sim=zeros(length(refSatData.GPS.SIM_time),6);
-
-    for i=1:length(refSatData.GPS.SIM_time)
-        Carrier_Doppler_hz_sim(i,1)=refSatData.GPS.series(i).doppler_shift(4);%PRN 28
-        Carrier_Doppler_hz_sim(i,2)=refSatData.GPS.series(i).doppler_shift(1);%PRN 4
-        Carrier_Doppler_hz_sim(i,3)=refSatData.GPS.series(i).doppler_shift(6);%PRN 17
-        Carrier_Doppler_hz_sim(i,4)=refSatData.GPS.series(i).doppler_shift(7);%PRN 15
-        Carrier_Doppler_hz_sim(i,5)=refSatData.GPS.series(i).doppler_shift(8);%PRN 27
-        Carrier_Doppler_hz_sim(i,6)=refSatData.GPS.series(i).doppler_shift(9);%PRN 9
-
-        Pseudorange_m_sim(i,1)=refSatData.GPS.series(i).pr_m(4);%PRN 28
-        Pseudorange_m_sim(i,2)=refSatData.GPS.series(i).pr_m(1);%PRN 4
-        Pseudorange_m_sim(i,3)=refSatData.GPS.series(i).pr_m(6);%PRN 17
-        Pseudorange_m_sim(i,4)=refSatData.GPS.series(i).pr_m(7);%PRN 15
-        Pseudorange_m_sim(i,5)=refSatData.GPS.series(i).pr_m(8);%PRN 27
-        Pseudorange_m_sim(i,6)=refSatData.GPS.series(i).pr_m(9);%PRN 9
-    end
-
-
-    Rx_Dopp_all=figure('Name','RX_Carrier_Doppler_hz');plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz','s')
-    xlim([0,140]);
-    xlabel('')
-    ylabel('Doppler (Hz)')
-    xlabel('time from simulation init (seconds)')
-    grid on
-    hold on
-    plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim','.')
-    plot(navSolution.RX_time(1,:)-time_reference_spirent_obs, sat_dopp_hz_filt,'o')
-    legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','PRN 9','Location','eastoutside')
-    hold off
-    grid on
-
-    Rx_Dopp_one=figure('Name','RX_Carrier_Doppler_hz');plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(1,:)','s')
-    xlim([0,140]);
-    ylim([-2340,-2220]);
-    xlabel('')
-    ylabel('Doppler (Hz)')
-    xlabel('time from simulation init (seconds)')
-    grid on
-    hold on
-    legend('PRN 28 GNSS-SDR','Location','eastoutside')
-    plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,1)','.','DisplayName','reference')
-    plot(navSolution.RX_time(1,:)-time_reference_spirent_obs, sat_dopp_hz_filt(1,:),'o','DisplayName','filtered VTL')
-    
-    hold off
-    grid on
-% -------------------------------------
-%     Rx_pseudo_all=figure('Name','RX_pr_m');plot(RX_time(1,:)-time_reference_spirent_obs, Pseudorange_m','s')
-%     xlim([0,140]);
-%     xlabel('')
-%     ylabel('Pseudorange (m)')
-%     xlabel('time from simulation init (seconds)')
-%     grid on
-%     hold on
-%     plot(refSatData.GPS.SIM_time/1000, Pseudorange_m_sim','.')
-%     plot(navSolution.RX_time(1,:)-time_reference_spirent_obs, pr_m_filt,'o')
-%     legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','PRN 9','Location','eastoutside')
-%     hold off
-%     grid on
-% 
-%     Rx_pseudo_one=figure('Name','RX_pr_m');plot(RX_time(1,:)-time_reference_spirent_obs, Pseudorange_m(1,:)','s')
-%     xlim([0,140]);
-%     xlabel('')
-%     ylabel('Pseudorange (m)')
-%     xlabel('time from simulation init (seconds)')
-%     grid on
-%     hold on
-%     legend('PRN 28 GNSS-SDR','Location','eastoutside')
-%     plot(refSatData.GPS.SIM_time/1000, Pseudorange_m_sim(:,1)','.','DisplayName','reference')
-%     plot(navSolution.RX_time(1,:)-time_reference_spirent_obs, pr_m_filt(1,:),'o','DisplayName','filtered VTL')
-%     hold off
-%     grid on
-end
+%%
+figure;plot(navSolution.RX_time-navSolution.RX_time(1),kf_yerr(1:5,:)');title('c pr m-error');xlabel('t U.A');ylabel('pr m [m]');grid minor
+legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','Location','eastoutside')
+figure;plot(navSolution.RX_time-navSolution.RX_time(1),kf_yerr(6:10,:)');title('c pr m DOT-error');xlabel('t U.A');ylabel('pr m dot [m/s]');grid minor
+legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','Location','eastoutside')