From 74a838339305c534d307face37d8115e34a12793 Mon Sep 17 00:00:00 2001
From: miguekf <angel.glopez@alumnos.upm.es>
Date: Sat, 10 Dec 2022 22:01:29 +0100
Subject: [PATCH] MOD: matlab utils

---
 src/utils/matlab/vtl/kf_prototype.m           | 166 ++++++---
 .../vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m    | 321 +++++++++---------
 src/utils/matlab/vtl/vtl_general_plot.m       | 160 ++++-----
 src/utils/matlab/vtl/vtl_prototype.m          | 128 ++++---
 4 files changed, 416 insertions(+), 359 deletions(-)

diff --git a/src/utils/matlab/vtl/kf_prototype.m b/src/utils/matlab/vtl/kf_prototype.m
index 78bed097d..806c2139b 100644
--- a/src/utils/matlab/vtl/kf_prototype.m
+++ b/src/utils/matlab/vtl/kf_prototype.m
@@ -29,14 +29,14 @@ kf_yerr = zeros(2*sat_number, 1);
 kf_S = zeros(2*sat_number, 2*sat_number); % kf_P_y innovation covariance matrix
 
 %% State error Covariance Matrix Q (PVT)
-kf_Q = [    1     0     0     0     0     0     0     0
-            0     1     0     0     0     0     0     0
-            0     0     1     0     0     0     0     0
-            0     0     0     1     0     0     0     0
-            0     0     0     0     1     0     0     0
-            0     0     0     0     0     1     0     0
-            0     0     0     0     0     0     1e-9     0
-            0     0     0     0     0     0     0     1e-7];%new_data.rx_pvt_var(i); %careful, values for V and T could not be adecuate.
+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
@@ -57,17 +57,24 @@ kf_R=[  3     0     0     0     0     0     0     0     0     0
         0     0     0     0     0     0     0     0     30     0
         0     0     0     0     0     0     0     0     0     30];
     
-        d = zeros(sat_number, 1);
-    rho_pri = zeros(sat_number, 1);
-    rhoDot_pri = zeros(sat_number, 1);
-    a_x = zeros(sat_number, 1);
-    a_y = zeros(sat_number, 1);
-    a_z = zeros(sat_number, 1);
-    c_pr_m=zeros(sat_number,length(navSolution.RX_time));
+% pre-allocate for speed     
+d = zeros(sat_number, 1);
+rho_pri = zeros(sat_number, 1);
+rhoDot_pri = zeros(sat_number, 1);
+a_x = zeros(sat_number, 1);
+a_y = zeros(sat_number, 1);
+a_z = zeros(sat_number, 1);
+c_pr_m=zeros(sat_number,length(navSolution.RX_time));
+
+pr_m_filt=zeros(sat_number,length(navSolution.RX_time));
+rhoDot_pri_filt=zeros(sat_number,length(navSolution.RX_time));
+sat_dopp_hz_filt=zeros(sat_number,length(navSolution.RX_time));
+
 %% ################## Kalman Tracking ######################################
 % receiver solution from rtklib_solver
-for t=1:length(navSolution.RX_time)
-    
+for t=2:length(navSolution.RX_time)
+   
+    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);
         kf_x(2,t) = navSolution.Y(t);
@@ -77,7 +84,7 @@ for t=1:length(navSolution.RX_time)
         kf_x(6,t) = navSolution.vZ(t);
         kf_x(7,t) = clk_bias_s(t)*SPEED_OF_LIGHT_M_S;
         kf_x(8,t) = clk_drift(t)*SPEED_OF_LIGHT_M_S;%new_data.rx_dts(1);
-        
+
         x_u=kf_x(1,t);
         y_u=kf_x(2,t);
         z_u=kf_x(3,t);
@@ -86,38 +93,41 @@ for t=1:length(navSolution.RX_time)
         zDot_u=kf_x(6,t);
         cdeltat_u(t)=kf_x(7,t);
         cdeltatDot_u=kf_x(8,t);
-        kf_P_x  = eye(8)*100; %TODO: use a real value.
 
+        % Kalman state prediction (time update)
+        kf_P_x  = eye(8)*100; %TODO: use a real value.
+        kf_xpri(:,t) = kf_F * (kf_x(:,t)-kf_x(:,t-1));% state prediction
+        kf_P_x= kf_F * kf_P_x * kf_F' + kf_Q;% state error covariance prediction
     else
-        kf_x(:,t)=corr_kf_state(:,t-1);    
-        x_u=kf_x(1,t);
-        y_u=kf_x(2,t);
-        z_u=kf_x(3,t);
-        xDot_u=kf_x(4,t);
-        yDot_u=kf_x(5,t);
-        zDot_u=kf_x(6,t);
-        cdeltat_u(t)=kf_x(7,t);% 
-        cdeltatDot_u=kf_x(8,t);% 
+        x_u=corr_kf_state(1,t-1);
+        y_u=corr_kf_state(2,t-1);
+        z_u=corr_kf_state(3,t-1);
+        xDot_u=corr_kf_state(4,t-1);
+        yDot_u=corr_kf_state(5,t-1);
+        zDot_u=corr_kf_state(6,t-1);
+        cdeltat_u(t)=corr_kf_state(7,t-1);
+        cdeltatDot_u=corr_kf_state(8,t-1);
+
+        % Kalman state prediction (time update)
+        kf_P_x= kf_F * kf_P_x * kf_F' + kf_Q;% state error covariance prediction
     end
-    % Kalman state prediction (time update)
-    kf_x(:,t) = kf_F * kf_x(:,t);                        % state prediction
-    kf_P_x= kf_F * kf_P_x * kf_F' + kf_Q;  % state error covariance prediction
+
     
     for chan=1:5 %neccesary quantities
-            d(chan)=(sat_posX_m(chan,t)-kf_x(1,t))^2;
-            d(chan)=d(chan)+(sat_posY_m(chan,t)-kf_x(2,t))^2;
-            d(chan)=d(chan)+(sat_posZ_m(chan,t)-kf_x(3,t))^2;
+            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)-kf_x(1,t))/d(chan);
-            a_y(chan,t)=-(sat_posY_m(chan,t)-kf_x(2,t))/d(chan);
-            a_z(chan,t)=-(sat_posZ_m(chan,t)-kf_x(3,t))/d(chan);
+            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)-kf_x(4,t))*a_x(chan,t)...
-                +(sat_velY(chan,t)-kf_x(5,t))*a_y(chan,t)...
-                +(sat_velZ(chan,t)-kf_x(6,t))*a_z(chan,t)+cdeltatDot_u;
+            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
     
     
@@ -128,10 +138,16 @@ for t=1:length(navSolution.RX_time)
             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].
+
     % 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+cdeltatDot_u)-rhoDot_pri(chan,t);
+            kf_yerr(chan+sat_number,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1)-rhoDot_pri(chan,t);
     end
     
     % DOUBLES DIFFERENCES
@@ -146,15 +162,61 @@ for t=1:length(navSolution.RX_time)
     % 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
-    corr_kf_state(:,t)=kf_x(:,t)-kf_xerr(:,t); %updated state estimation
-      
-    err_clk_b=kf_xerr(7,t)
-    err_clk_d=kf_xerr(8,t)
-    % States related tu USER clock adjust from m/s to s (by /SPEED_OF_LIGHT_M_S)
-    % kf_x(6) =kf_x(6);
-    % kf_x(7) =kf_x(7);
+    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
+        corr_kf_state(:,t)=corr_kf_state(:,t-1)-kf_xerr(:,t); %updated state estimation
+    end
+
+    %%  ################## Geometric Transformation ######################################
+
+    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);  
+
+    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_g;
+
+        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)+cdeltatDot_u_g;
+    end
+
+    kf_H = zeros(2*sat_number,8);
+
+    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;
+    end
+
+    %  Re-calculate error measurement vector with the most recent data available: kf_delta_y=kf_H*kf_delta_x
+    kf_yerr_g=kf_H*kf_xerr;
+    %  Filtered pseudorange error measurement (in m) AND Filtered Doppler shift measurements (in Hz):
+    for chan=1:5  % Measurement vector
+        pr_m_filt(chan,t)=sat_prg_m(chan,t)+kf_yerr_g(chan,t);% now filtered
+        rhoDot_pri_filt(chan,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1+corr_kf_state(8,t))-kf_yerr_g(chan+sat_number,t);
+        %convert rhoDot_pri to doppler shift!
+        sat_dopp_hz_filt(chan,t)=(rhoDot_pri_filt(chan,t)-corr_kf_state(8,t))/Lambda_GPS_L1;
+
+    end
 end
\ No newline at end of file
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 291c7cf4f..fa4abe1f3 100644
--- a/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m
+++ b/src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m
@@ -29,7 +29,7 @@ channels=6;
 TTFF_sec=41.48;
 
 %% ============================ PARSER TO STRUCT ============================
-plot_skyplot=0;
+plot_skyplot=0; % not yet implemented
 plot_reference=1;
 load_observables=1;
 advance_vtl_data_available=1;
@@ -146,168 +146,163 @@ vZ_FILT = movmean(navSolution.vZ,moving_avg_factor);
 %%
 general_raw_plot
 %%
-
-%---VTL VELOCITY: GNSS SDR plot --------------------------------------
-VTL_VEL=figure('Name','velocities and heigh');
-subplot(2,2,1);
-plot(vtlSolution.rtklibpvt.vX,'.');
-hold on;grid on
-plot(vtlSolution.kfpvt.vX,'.');
-plot(vtlSolution.kferr.vX,'.');
-ylabel('vX (m/s)')
-xlabel('time U.A.')
-ylim([-5 5])
-title('Subplot 1: vX ')
-legend ('raw RTKlib','raw kf','kferr','Location','east')
-
-subplot(2,2,2);
-plot(vtlSolution.rtklibpvt.vY,'.');
-hold on;grid on
-plot(vtlSolution.kfpvt.vY,'.');
-plot(vtlSolution.kferr.vY,'.');
-ylabel('vY (m/s)')
-xlabel('time U.A.')
-ylim([-5 5])
-title('Subplot 1: vY ')
-legend ('raw RTKlib','raw kf','kferr','Location','east')
-
-subplot(2,2,3);
-plot(vtlSolution.rtklibpvt.vZ,'.');
-hold on;grid on
-plot(vtlSolution.kfpvt.vZ,'.');
-plot(vtlSolution.kferr.vZ,'.');
-ylabel('vZ (m/s)')
-xlabel('time U.A.')
-ylim([-5 5])
-title('Subplot 1: vZ ')
-legend ('raw RTKlib','raw kf','kferr','Location','east')
-
-subplot(2,2,4);
-plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.height,'.');
-hold on;grid on
-plot(navSolution.RX_time-navSolution.RX_time(1),HEIGH_FILT,'r.');
-ylabel('HEIGH (m)')
-xlabel('time from First FIX in (seconds)')
-title('Subplot 4: HEIGH')
-legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','southeast')
-
-sgtitle('velocities and heigh') 
-
-% --- VTL UTM centered POSITION: GNSS SDR  plot --------------------------------------
-
-VTL_POS=figure('Name','VTL UTM COORD CENTERED IN 1^{ST} POSITION');
-subplot(2,2,1);
-plot(vtlSolution.rtklibpvt.X-vtlSolution.rtklibpvt.X(1),'.');
-hold on;grid on
-plot(vtlSolution.kfpvt.X-vtlSolution.kfpvt.X(1),'.');
-plot(vtlSolution.kferr.X,'.');
-ylabel('X (m)')
-xlabel('time U.A.')
-ylim([-200 800])
-title('Subplot 1: X ')
-legend ('raw RTKlib','raw kf','kferr','Location','east')
-
-subplot(2,2,2);
-plot(vtlSolution.rtklibpvt.Y-vtlSolution.rtklibpvt.Y(1),'.');
-hold on;grid on
-plot(vtlSolution.kfpvt.Y-vtlSolution.kfpvt.Y(1),'.');
-plot(vtlSolution.kferr.Y,'.');
-ylabel('Y (m)')
-xlabel('time U.A.')
-ylim([-200 50])
-title('Subplot 1: Y ')
-legend ('raw RTKlib','raw kf','kferr','Location','east')
-
-subplot(2,2,3);
-plot(vtlSolution.rtklibpvt.Z-vtlSolution.rtklibpvt.Z(1),'.');
-hold on;grid on
-plot(vtlSolution.kfpvt.Z-vtlSolution.kfpvt.Z(1),'.');
-plot(vtlSolution.kferr.Z,'.');
-ylabel('Z (m)')
-xlabel('time U.A.')
-ylim([-350 50])
-title('Subplot 1: Z ')
-legend ('raw RTKlib','raw kf','kferr','Location','east')
-
-sgtitle('VTL UTM COORD CENTERED IN 1^{ST} POSITION') 
-%%
-% --- 'VTL errPV correction --------------------------------------
-
-VTL_errPV=figure('Name','VTL errPV correction');
-subplot(2,3,1);
-plot(vtlSolution.rtklibpvt.X-vtlSolution.rtklibpvt.X(1),'.');
-hold on;grid on
-plot(vtlSolution.kfpvt.X-vtlSolution.kfpvt.X(1),'.');
-plot(vtlSolution.kferr.X,'.');
-plot(vtlSolution.kfpvt.X-vtlSolution.rtklibpvt.X(1)+vtlSolution.kferr.X,'.');
-ylabel('X (m)')
-xlabel('time U.A.')
-ylim([-200 800])
-title('Subplot 1: X ')
-legend ('raw RTKlib','raw kf','kferr','added err+raw','Location','eastoutside')
-
-subplot(2,3,2);
-plot(vtlSolution.rtklibpvt.Y-vtlSolution.rtklibpvt.Y(1),'.');
-hold on;grid on
-plot(vtlSolution.kfpvt.Y-vtlSolution.kfpvt.Y(1),'.');
-plot(vtlSolution.kferr.Y,'.');
-plot(vtlSolution.kfpvt.Y-vtlSolution.rtklibpvt.Y(1)+vtlSolution.kferr.Y,'.');
-ylabel('Y (m)')
-xlabel('time U.A.')
-ylim([-200 50])
-title('Subplot 1: Y ')
-legend ('raw RTKlib','raw kf','kferr','added err+raw','Location','eastoutside')
-
-subplot(2,3,3);
-plot(vtlSolution.rtklibpvt.Z-vtlSolution.rtklibpvt.Z(1),'.');
-hold on;grid on
-plot(vtlSolution.kfpvt.Z-vtlSolution.kfpvt.Z(1),'.');
-plot(vtlSolution.kferr.Z,'.');
-plot(vtlSolution.kfpvt.Z-vtlSolution.rtklibpvt.Z(1)+vtlSolution.kferr.Z,'.');
-ylabel('Z (m)')
-xlabel('time U.A.')
-ylim([-350 50])
-title('Subplot 1: Z ')
-legend ('raw RTKlib','raw kf','kferr','added err+raw','Location','eastoutside')
-
-subplot(2,3,4);
-plot(vtlSolution.rtklibpvt.vX,'.');
-hold on;grid on
-plot(vtlSolution.kfpvt.vX,'.');
-plot(vtlSolution.kferr.vX,'.');
-plot(vtlSolution.kfpvt.vX+vtlSolution.kferr.vX,'.');
-ylabel('vX (m/s)')
-xlabel('time U.A.')
-ylim([-5 5])
-title('Subplot 1: vX ')
-legend ('raw RTKlib','raw kf','kferr','added err+raw','Location','eastoutside')
-
-subplot(2,3,5);
-plot(vtlSolution.rtklibpvt.vY,'.');
-hold on;grid on
-plot(vtlSolution.kfpvt.vY,'.');
-plot(vtlSolution.kferr.vY,'.');
-plot(vtlSolution.kfpvt.vY+vtlSolution.kferr.vY,'.');
-ylabel('vY (m/s)')
-xlabel('time U.A.')
-ylim([-5 5])
-title('Subplot 1: vY ')
-legend ('raw RTKlib','raw kf','kferr','added err+raw','Location','eastoutside')
-
-subplot(2,3,6);
-plot(vtlSolution.rtklibpvt.vZ,'.');
-hold on;grid on
-plot(vtlSolution.kfpvt.vZ,'.');
-plot(vtlSolution.kferr.vZ,'.');
-plot(vtlSolution.kfpvt.vZ+vtlSolution.kferr.vZ,'.');
-ylabel('vZ (m/s)')
-xlabel('time U.A.')
-ylim([-5 5])
-title('Subplot 1: vZ ')
-legend ('raw RTKlib','raw kf','kferr','added err+raw','Location','eastoutside')
-
-
-sgtitle('VTL errPV correction') 
+if(load_vtl)
+    %---VTL VELOCITY: GNSS SDR plot --------------------------------------
+    VTL_VEL=figure('Name','velocities and heigh');
+    subplot(2,2,1);
+    plot(vtlSolution.rtklibpvt.vX,'.');
+    hold on;grid on
+    plot(vtlSolution.kfpvt.vX,'.');
+    plot(vtlSolution.kferr.vX,'.');
+    ylabel('vX (m/s)')
+    xlabel('time U.A.')
+    ylim([-5 5])
+    title('Subplot 1: vX ')
+    legend ('raw RTKlib','VTL_engine kf','kferr','Location','east')
+    
+    subplot(2,2,2);
+    plot(vtlSolution.rtklibpvt.vY,'.');
+    hold on;grid on
+    plot(vtlSolution.kfpvt.vY,'.');
+    plot(vtlSolution.kferr.vY,'.');
+    ylabel('vY (m/s)')
+    xlabel('time U.A.')
+    ylim([-5 5])
+    title('Subplot 1: vY ')
+    legend ('raw RTKlib','VTL_engine kf','kferr','Location','east')
+    
+    subplot(2,2,3);
+    plot(vtlSolution.rtklibpvt.vZ,'.');
+    hold on;grid on
+    plot(vtlSolution.kfpvt.vZ,'.');
+    plot(vtlSolution.kferr.vZ,'.');
+    ylabel('vZ (m/s)')
+    xlabel('time U.A.')
+    ylim([-5 5])
+    title('Subplot 1: vZ ')
+    legend ('raw RTKlib','VTL_engine kf','kferr','Location','east')
+    
+    subplot(2,2,4);
+    plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.height,'.');
+    hold on;grid on
+    plot(navSolution.RX_time-navSolution.RX_time(1),HEIGH_FILT,'r.');
+    ylabel('HEIGH (m)')
+    xlabel('time from First FIX in (seconds)')
+    title('Subplot 4: HEIGH')
+    legend ('raw',['moving avg:' num2str(moving_avg_factor)],'Location','southeast')
+    
+    sgtitle('velocities and heigh')
+    
+    % --- VTL UTM centered POSITION: GNSS SDR  plot --------------------------------------
+    
+    VTL_POS=figure('Name','VTL UTM COORD CENTERED IN 1^{ST} POSITION');
+    subplot(2,2,1);
+    plot(vtlSolution.rtklibpvt.X-vtlSolution.rtklibpvt.X(1),'.');
+    hold on;grid on
+    plot(vtlSolution.kfpvt.X-vtlSolution.kfpvt.X(1),'.');
+    plot(vtlSolution.kferr.X,'.');
+    ylabel('X (m)')
+    xlabel('time U.A.')
+    ylim([-200 800])
+    title('Subplot 1: X ')
+    legend ('raw RTKlib','VTL_engine kf','kferr','Location','east')
+    
+    subplot(2,2,2);
+    plot(vtlSolution.rtklibpvt.Y-vtlSolution.rtklibpvt.Y(1),'.');
+    hold on;grid on
+    plot(vtlSolution.kfpvt.Y-vtlSolution.kfpvt.Y(1),'.');
+    plot(vtlSolution.kferr.Y,'.');
+    ylabel('Y (m)')
+    xlabel('time U.A.')
+    ylim([-200 50])
+    title('Subplot 1: Y ')
+    legend ('raw RTKlib','VTL_engine kf','kferr','Location','east')
+    
+    subplot(2,2,3);
+    plot(vtlSolution.rtklibpvt.Z-vtlSolution.rtklibpvt.Z(1),'.');
+    hold on;grid on
+    plot(vtlSolution.kfpvt.Z-vtlSolution.kfpvt.Z(1),'.');
+    plot(vtlSolution.kferr.Z,'.');
+    ylabel('Z (m)')
+    xlabel('time U.A.')
+    ylim([-350 50])
+    title('Subplot 1: Z ')
+    legend ('raw RTKlib','VTL_engine kf','kferr','Location','east')
+    
+    sgtitle('VTL UTM COORD CENTERED IN 1^{ST} POSITION')
+    %%
+    % --- 'VTL errPV correction --------------------------------------
+    
+    VTL_errPV=figure('Name','VTL errPV correction');
+    subplot(2,3,1);
+    plot(vtlSolution.rtklibpvt.X-vtlSolution.rtklibpvt.X(1),'.');
+    hold on;grid on
+    plot(vtlSolution.kfpvt.X-vtlSolution.kfpvt.X(1),'.');
+    plot(vtlSolution.kferr.X,'.');
+    ylabel('X (m)')
+    xlabel('time U.A.')
+    ylim([-200 800])
+    title('Subplot 1: X ')
+    legend ('raw RTKlib','VTL_engine kf','kferr','Location','eastoutside')
+    
+    subplot(2,3,2);
+    plot(vtlSolution.rtklibpvt.Y-vtlSolution.rtklibpvt.Y(1),'.');
+    hold on;grid on
+    plot(vtlSolution.kfpvt.Y-vtlSolution.kfpvt.Y(1),'.');
+    plot(vtlSolution.kferr.Y,'.');
+    ylabel('Y (m)')
+    xlabel('time U.A.')
+    ylim([-200 50])
+    title('Subplot 1: Y ')
+    legend ('raw RTKlib','VTL_engine kf','kferr','Location','eastoutside')
+    
+    subplot(2,3,3);
+    plot(vtlSolution.rtklibpvt.Z-vtlSolution.rtklibpvt.Z(1),'.');
+    hold on;grid on
+    plot(vtlSolution.kfpvt.Z-vtlSolution.kfpvt.Z(1),'.');
+    plot(vtlSolution.kferr.Z,'.');
+    ylabel('Z (m)')
+    xlabel('time U.A.')
+    ylim([-350 50])
+    title('Subplot 1: Z ')
+    legend ('raw RTKlib','VTL_engine kf','kferr','Location','eastoutside')
+    
+    subplot(2,3,4);
+    plot(vtlSolution.rtklibpvt.vX,'.');
+    hold on;grid on
+    plot(vtlSolution.kfpvt.vX,'.');
+    plot(vtlSolution.kferr.vX,'.');
+    ylabel('vX (m/s)')
+    xlabel('time U.A.')
+    ylim([-5 5])
+    title('Subplot 1: vX ')
+    legend ('raw RTKlib','VTL_engine kf','kferr','Location','eastoutside')
+    
+    subplot(2,3,5);
+    plot(vtlSolution.rtklibpvt.vY,'.');
+    hold on;grid on
+    plot(vtlSolution.kfpvt.vY,'.');
+    plot(vtlSolution.kferr.vY,'.');
+    ylabel('vY (m/s)')
+    xlabel('time U.A.')
+    ylim([-5 5])
+    title('Subplot 1: vY ')
+    legend ('raw RTKlib','VTL_engine kf','kferr','Location','eastoutside')
+    
+    subplot(2,3,6);
+    plot(vtlSolution.rtklibpvt.vZ,'.');
+    hold on;grid on
+    plot(vtlSolution.kfpvt.vZ,'.');
+    plot(vtlSolution.kferr.vZ,'.');
+    ylabel('vZ (m/s)')
+    xlabel('time U.A.')
+    ylim([-5 5])
+    title('Subplot 1: vZ ')
+    legend ('raw RTKlib','VTL_engine kf','kferr','Location','eastoutside')
+    
+    
+    sgtitle('VTL errPV correction')
+end
 %% ============================================== ==============================================
 time_reference_spirent_obs=129780;%s
 if(load_observables)
diff --git a/src/utils/matlab/vtl/vtl_general_plot.m b/src/utils/matlab/vtl/vtl_general_plot.m
index ea0924865..3cf6ef538 100644
--- a/src/utils/matlab/vtl/vtl_general_plot.m
+++ b/src/utils/matlab/vtl/vtl_general_plot.m
@@ -4,37 +4,43 @@
 %---VTL VELOCITY: GNSS SDR plot --------------------------------------
 VTL_VEL=figure('Name','velocities');
 subplot(2,2,1);
-plot(navSolution.vX,'.');
+plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.vX,'.');
 hold on;grid on
-plot(kf_x(4,:),'.');
-plot(kf_xerr(4,:),'.');
+plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(4,:),'.');
+plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.vX...
+    ,'.','DisplayName','reference')
+% plot(navSolution.RX_time-navSolution.RX_time(1),kf_xerr(4,:),'.');
 ylabel('vX (m/s)')
 xlabel('time U.A.')
 ylim([-5 5])
 title('Subplot 1: vX ')
-legend ('raw navSolution','raw kf state','kferr','Location','east')
+legend ('raw navSolution','raw kf state','reference','Location','east')
 
 subplot(2,2,2);
-plot(navSolution.vY,'.');
+plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.vY,'.');
 hold on;grid on
-plot(kf_x(5,:),'.');
-plot(kf_xerr(5,:),'.');
+plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(5,:),'.');
+% plot(navSolution.RX_time-navSolution.RX_time(1),kf_xerr(5,:),'.');
+plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.vY...
+    ,'.','DisplayName','reference')
 ylabel('vY (m/s)')
 xlabel('time U.A.')
 ylim([-5 5])
 title('Subplot 1: vY ')
-legend ('raw navSolution','raw kf state','kferr','Location','east')
+legend ('raw navSolution','raw kf state','reference','Location','east')
 
 subplot(2,2,3);
-plot(navSolution.vZ,'.');
+plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.vZ,'.');
 hold on;grid on
-plot(kf_x(6,:),'.');
-plot(kf_xerr(6,:),'.');
+plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(6,:),'.');
+% plot(navSolution.RX_time-navSolution.RX_time(1),kf_xerr(6,:),'.');
+plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.vZ...
+    ,'.','DisplayName','reference')
 ylabel('vZ (m/s)')
 xlabel('time U.A.')
 ylim([-5 5])
 title('Subplot 1: vZ ')
-legend ('raw navSolution','raw kf state','kferr','Location','east')
+legend ('raw navSolution','raw kf state','reference','Location','east')
 
 sgtitle('velocities') 
 
@@ -44,7 +50,7 @@ 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(kf_x(1,:)-kf_x(1,1),'.');
+plot(corr_kf_state(1,:)-corr_kf_state(1,1),'.');
 plot(kf_xerr(1,:),'.');
 ylabel('X (m)')
 xlabel('time U.A.')
@@ -55,7 +61,7 @@ legend ('raw navSolution','raw kf state','kferr','Location','east')
 subplot(2,2,2);
 plot(navSolution.Y-navSolution.Y(1),'.');
 hold on;grid on
-plot(kf_x(2,:)-kf_x(2,1),'.');
+plot(corr_kf_state(2,:)-corr_kf_state(2,1),'.');
 plot(kf_xerr(2,:),'.');
 ylabel('Y (m)')
 xlabel('time U.A.')
@@ -66,7 +72,7 @@ legend ('raw navSolution','raw kf state','kferr','Location','east')
 subplot(2,2,3);
 plot(navSolution.Z-navSolution.Z(1),'.');
 hold on;grid on
-plot(kf_x(3,:)-kf_x(3,1),'.');
+plot(corr_kf_state(3,:)-corr_kf_state(3,1),'.');
 plot(kf_xerr(3,:),'.');
 ylabel('Z (m)')
 xlabel('time U.A.')
@@ -76,80 +82,50 @@ legend ('raw navSolution','raw kf state','kferr','Location','east')
 
 sgtitle('VTL UTM COORD CENTERED IN 1^{ST} POSITION') 
 %%
-% % --- 'VTL errPV correction --------------------------------------
-% 
-% VTL_errPV=figure('Name','VTL errPV correction');
-% subplot(2,3,1);
-% plot(navSolution.X-navSolution.X(1),'.');
-% hold on;grid on
-% plot(vtlSolution.kfpvt.X-vtlSolution.kfpvt.X(1),'.');
-% plot(vtlSolution.kferr.X,'.');
-% plot(vtlSolution.kfpvt.X-navSolution.X(1)+vtlSolution.kferr.X,'.');
-% ylabel('X (m)')
-% xlabel('time U.A.')
-% ylim([-200 800])
-% title('Subplot 1: X ')
-% legend ('raw RTKlib','raw kf','kferr','added err+raw','Location','eastoutside')
-% 
-% subplot(2,3,2);
-% plot(navSolution.Y-navSolution.Y(1),'.');
-% hold on;grid on
-% plot(vtlSolution.kfpvt.Y-vtlSolution.kfpvt.Y(1),'.');
-% plot(vtlSolution.kferr.Y,'.');
-% plot(vtlSolution.kfpvt.Y-navSolution.Y(1)+vtlSolution.kferr.Y,'.');
-% ylabel('Y (m)')
-% xlabel('time U.A.')
-% ylim([-200 50])
-% title('Subplot 1: Y ')
-% legend ('raw RTKlib','raw kf','kferr','added err+raw','Location','eastoutside')
-% 
-% subplot(2,3,3);
-% plot(navSolution.Z-navSolution.Z(1),'.');
-% hold on;grid on
-% plot(vtlSolution.kfpvt.Z-vtlSolution.kfpvt.Z(1),'.');
-% plot(vtlSolution.kferr.Z,'.');
-% plot(vtlSolution.kfpvt.Z-navSolution.Z(1)+vtlSolution.kferr.Z,'.');
-% ylabel('Z (m)')
-% xlabel('time U.A.')
-% ylim([-350 50])
-% title('Subplot 1: Z ')
-% legend ('raw RTKlib','raw kf','kferr','added err+raw','Location','eastoutside')
-% 
-% subplot(2,3,4);
-% plot(navSolution.vX,'.');
-% hold on;grid on
-% plot(vtlSolution.kfpvt.vX,'.');
-% plot(vtlSolution.kferr.vX,'.');
-% plot(vtlSolution.kfpvt.vX+vtlSolution.kferr.vX,'.');
-% ylabel('vX (m/s)')
-% xlabel('time U.A.')
-% ylim([-5 5])
-% title('Subplot 1: vX ')
-% legend ('raw RTKlib','raw kf','kferr','added err+raw','Location','eastoutside')
-% 
-% subplot(2,3,5);
-% plot(navSolution.vY,'.');
-% hold on;grid on
-% plot(vtlSolution.kfpvt.vY,'.');
-% plot(vtlSolution.kferr.vY,'.');
-% plot(vtlSolution.kfpvt.vY+vtlSolution.kferr.vY,'.');
-% ylabel('vY (m/s)')
-% xlabel('time U.A.')
-% ylim([-5 5])
-% title('Subplot 1: vY ')
-% legend ('raw RTKlib','raw kf','kferr','added err+raw','Location','eastoutside')
-% 
-% subplot(2,3,6);
-% plot(navSolution.vZ,'.');
-% hold on;grid on
-% plot(vtlSolution.kfpvt.vZ,'.');
-% plot(vtlSolution.kferr.vZ,'.');
-% plot(vtlSolution.kfpvt.vZ+vtlSolution.kferr.vZ,'.');
-% ylabel('vZ (m/s)')
-% xlabel('time U.A.')
-% ylim([-5 5])
-% title('Subplot 1: vZ ')
-% legend ('raw RTKlib','raw kf','kferr','added err+raw','Location','eastoutside')
-% 
-% 
-% sgtitle('VTL errPV correction') 
\ No newline at end of file
+%% STATE PLOT
+VTL_STATE=figure('Name','VTL STATE');
+subplot(2,2,1);
+plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(1:3,:)'-corr_kf_state(1:3,3)','.');
+hold on;grid on
+plot(refSolution.SIM_time/1000-TTFF_sec,[refSolution.X-refSolution.X(1)...
+    refSolution.Y-refSolution.Y(1) refSolution.Z-refSolution.Z(1)],...
+    '.','DisplayName','reference');
+ylim([-200,200])
+xlim([0,120])
+ylabel('X Y Z (m)')
+xlabel('time U.A.')
+title('Subplot 1: POSITION ')
+
+subplot(2,2,2);
+plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(4:6,:)','.');
+hold on;grid on
+plot(refSolution.SIM_time/1000-TTFF_sec,[refSolution.vX...
+    refSolution.vY refSolution.vZ],...
+    '.','DisplayName','reference');
+xlim([0,120])
+ylabel('vX vY vZ (m/s)')
+xlabel('time U.A.')
+title('Subplot 1: VELOCITIES ')
+
+subplot(2,2,3);
+plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(7,:),'.');
+ylim([3019190, 3019700])
+hold on;grid on
+plot(navSolution.RX_time-navSolution.RX_time(1),clk_bias_s*SPEED_OF_LIGHT_M_S,'.');
+xlim([0,120])
+ylabel('clk bias (m)')
+xlabel('time U.A.')
+title('Subplot 1: clk bias')
+legend('vtl','rtklib')
+
+subplot(2,2,4);
+plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(8,:),'.');
+hold on;grid on
+plot(navSolution.RX_time-navSolution.RX_time(1),clk_drift*SPEED_OF_LIGHT_M_S,'.');
+xlim([0,120])
+ylabel('clk drift (m/s)')
+legend('vtl','rtklib')
+xlabel('time U.A.')
+title('Subplot 1: clk drift ')
+
+sgtitle('VTL STATE') 
\ No newline at end of file
diff --git a/src/utils/matlab/vtl/vtl_prototype.m b/src/utils/matlab/vtl/vtl_prototype.m
index 8bd495974..e29ce4208 100644
--- a/src/utils/matlab/vtl/vtl_prototype.m
+++ b/src/utils/matlab/vtl/vtl_prototype.m
@@ -23,15 +23,17 @@ clearvars
 % end
 SPEED_OF_LIGHT_M_S=299792458.0;
 Lambda_GPS_L1=0.1902937;
-point_of_closure=6000;
+point_of_closure=3000;
 %%
 samplingFreq=5000000;
 channels=6;
 TTFF_sec=41.48;
 
-%% ============================ PARSER TO STRUCT ============================
 plot_skyplot=0;
 plot_reference=1;
+load_observables=1;
+
+%% ============================ PARSER TO STRUCT ============================
 
 navSolution = GnssSDR2struct('PVT_raw.mat');
 refSolution = SpirentMotion2struct('..\log_spirent\motion_V1_SPF_LD_05.csv');
@@ -41,8 +43,13 @@ load observables\observable_raw.mat
 rx_PRN=[28 4 17 15 27 9]; % for SPF_LD_05.
 load('PVT_raw.mat','sat_posX_m','sat_posY_m','sat_posZ_m','sat_velX','sat_velY'...
         ,'sat_velZ','sat_prg_m','clk_bias_s','clk_drift','sat_dopp_hz','user_clk_offset')
+
+if(load_observables)
+    load observables\observable_raw.mat
+    refSatData = SpirentSatData2struct('..\log_spirent\sat_data_V1A1_SPF_LD_05.csv');
+end
 %%
-vtlSolution = Vtl2struct('dump_vtl_file.csv');
+% vtlSolution = Vtl2struct('dump_vtl_file.csv');
 %% calculate LOS Rx-sat
 
 % for chan=1:5
@@ -78,13 +85,11 @@ vtlSolution = Vtl2struct('dump_vtl_file.csv');
 kf_prototype
 
 %%
-figure;plot(kf_yerr(1:5,:)');title('c_pr_m-error');xlabel('t U.A');ylabel('pr_m [m]');grid minor
+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
+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')
-%%
-% figure;plot([a_x(1,:);a_y(1,:);a_z(1,:)]');
-% figure;plot([vtlSolution.LOSx vtlSolution.LOSy vtlSolution.LOSz])
+
 %% ====== FILTERING =======================================================
 % moving_avg_factor= 500;
 % LAT_FILT = movmean(navSolution.latitude,moving_avg_factor);
@@ -102,64 +107,83 @@ legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','Location','eastoutside')
 %%
 %general_raw_plot
 vtl_general_plot
-%%
-% close all
-% figure;plot(kf_xerr(7,:),'.');title('clk bias err')
-% figure;plot(kf_xerr(8,:),'.');title('clk drift err')
-% figure;plot(kf_x(7,:),'.');title('clk bias state')
-% figure;plot(kf_x(8,:),'.');title('clk drift state')
-% figure;plot(corr_kf_state(7,:),'.');title('clk bias corrected state')
-% figure;plot(corr_kf_state(8,:),'.');title('clk drift corrected state')
-%%
-% close all
-% kferr_pos_all=[vtlSolution.kferr.X vtlSolution.kferr.Y vtlSolution.kferr.Z];
-% kferr_vel_all=[vtlSolution.kferr.vX vtlSolution.kferr.vY vtlSolution.kferr.vZ];
-% figure;plot(kferr_pos_all,'.');title('original pos err') 
-% figure;plot(kf_xerr(1:3,:)','.');title('calc pos err')
-% figure;plot(kferr_vel_all,'.');title('original vel err')
-% figure;plot(kf_xerr(4:6,:)','.');title('calc vel err')
+
 %% ============================================== ==============================================
-% time_reference_spirent_obs=129780;%s
-% if(load_observables)
-% %     figure;plot(Carrier_phase_cycles','.')
-% %     figure;plot(Pseudorange_m','.')
-%         %%%
-%     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
-%         
-%     end
-%     
-%     
-%     Rx_Dopp_all=figure('Name','RX_Carrier_Doppler_hz');plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz','.')
+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('Doppler (Hz)')
+%     ylabel('Pseudorange (m)')
 %     xlabel('time from simulation init (seconds)')
 %     grid on
 %     hold on
-%     legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','PRN 9','Location','eastoutside')
-%     plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim','.')
+%     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_Dopp_one=figure('Name','RX_Carrier_Doppler_hz');plot(RX_time(1,:)-time_reference_spirent_obs, Carrier_Doppler_hz(1,:)','.')
+% 
+%     Rx_pseudo_one=figure('Name','RX_pr_m');plot(RX_time(1,:)-time_reference_spirent_obs, Pseudorange_m(1,:)','s')
 %     xlim([0,140]);
-%     ylim([-2340,-2220]);
 %     xlabel('')
-%     ylabel('Doppler (Hz)')
+%     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, Carrier_Doppler_hz_sim(:,1)','.','DisplayName','reference')
+%     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
\ No newline at end of file
+end
+