From ce964eea67a82f68f53e5f29ed6810bea51e2b35 Mon Sep 17 00:00:00 2001
From: miguekf <angel.glopez@alumnos.upm.es>
Date: Thu, 8 Dec 2022 13:58:54 +0100
Subject: [PATCH] ADD: new matlab routines for vtl

---
 src/utils/matlab/vtl/kf_prototype.m     | 142 ++++++++++++++++++++++
 src/utils/matlab/vtl/vtl_general_plot.m | 155 ++++++++++++++++++++++++
 src/utils/matlab/vtl/vtl_prototype.m    | 149 +++++++++++++++++++++++
 3 files changed, 446 insertions(+)
 create mode 100644 src/utils/matlab/vtl/kf_prototype.m
 create mode 100644 src/utils/matlab/vtl/vtl_general_plot.m
 create mode 100644 src/utils/matlab/vtl/vtl_prototype.m

diff --git a/src/utils/matlab/vtl/kf_prototype.m b/src/utils/matlab/vtl/kf_prototype.m
new file mode 100644
index 000000000..a113e4357
--- /dev/null
+++ b/src/utils/matlab/vtl/kf_prototype.m
@@ -0,0 +1,142 @@
+%% vtl KF
+%%
+sat_number=5;
+%% ################## Kalman filter initialization ######################################
+% covariances (static)
+kf_P_x  = eye(8)*10.0; %TODO: use a real value.
+kf_x    = zeros(8, 1);
+kf_R    = zeros(2*sat_number, 2*sat_number);
+kf_dt=0.1;
+kf_F = eye(8, 8);
+kf_F(1, 4) = kf_dt;
+kf_F(2, 5) = kf_dt;
+kf_F(3, 6) = kf_dt;
+kf_F(7, 8) = kf_dt;
+
+kf_H = zeros(2*sat_number,8);
+kf_y = zeros(2*sat_number, 1);
+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 = eye(8);%new_data.rx_pvt_var(i); %careful, values for V and T could not be adecuate.
+%%
+
+% Measurement error Covariance Matrix R assembling
+for chan=1:5 %neccesary quantities
+    for t=1:length(navSolution.RX_time)
+        % 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
+end
+
+%% ################## Kalman Tracking ######################################
+% receiver solution from rtklib_solver
+for t=1:length(navSolution.RX_time)
+    
+    if (t<length(navSolution.RX_time)-point_of_closure)
+        kf_x(1,t) = navSolution.X(t);
+        kf_x(2,t) = navSolution.Y(t);
+        kf_x(3,t) = navSolution.Z(t);
+        kf_x(4,t) = navSolution.vX(t);
+        kf_x(5,t) = navSolution.vY(t);
+        kf_x(6,t) = navSolution.vZ(t);
+        kf_x(7,t) = clk_bias_s(t);
+        kf_x(8,t) = 1e-6;%new_data.rx_dts(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=kf_x(7,t)*SPEED_OF_LIGHT_M_S;
+        cdeltatDot_u=1e-8*SPEED_OF_LIGHT_M_S;
+    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=kf_x(7,t)*SPEED_OF_LIGHT_M_S;
+        cdeltatDot_u=1e-8*SPEED_OF_LIGHT_M_S;
+    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
+    %from error state variables to variables
+    % From state variables definition
+    % TODO: cast to type properly
+    
+    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));
+    
+    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)=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)-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);
+            
+            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);
+    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;
+    end
+    
+    % Kalman estimation (measurement update)
+    for chan=1:5 % Measurement matrix H assembling
+            %kf_y(i) = new_data.pr_m(i); % i-Satellite
+            %kf_y(i+sat_number) = rhoDot_pri(i)/Lambda_GPS_L1; % i-Satellite
+            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
+    % {
+    %     kf_y(i)=new_data.pr_m(i)-new_data.pr_m(i-1);
+    %     kf_yerr(i)=kf_y(i)-(rho_pri(i)+rho_pri(i-1));
+    %     kf_y(i+sat_number)=(rhoDot_pri(i)-rhoDot_pri(i-1))/Lambda_GPS_L1;
+    %     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_x = kf_x(:,t) - kf_xerr(:,t);                                      % updated state estimation (a priori + error)
+    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
+    % TODO: compare how KF state diverges from RTKLIB solution!
+    
+    
+    % States related tu USER clock adjust from m/s to s (by /SPEED_OF_LIGHT_M_S)
+    % kf_x(6) =kf_x(6) /SPEED_OF_LIGHT_M_S;
+    % kf_x(7) =kf_x(7) /SPEED_OF_LIGHT_M_S;
+    corr_kf_state(7,t)=corr_kf_state(7,t)/SPEED_OF_LIGHT_M_S;
+    corr_kf_state(8,t)=1e-8/SPEED_OF_LIGHT_M_S;
+end
\ 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
new file mode 100644
index 000000000..ea0924865
--- /dev/null
+++ b/src/utils/matlab/vtl/vtl_general_plot.m
@@ -0,0 +1,155 @@
+%%
+% vtl_general_plot.m
+%%
+%---VTL VELOCITY: GNSS SDR plot --------------------------------------
+VTL_VEL=figure('Name','velocities');
+subplot(2,2,1);
+plot(navSolution.vX,'.');
+hold on;grid on
+plot(kf_x(4,:),'.');
+plot(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')
+
+subplot(2,2,2);
+plot(navSolution.vY,'.');
+hold on;grid on
+plot(kf_x(5,:),'.');
+plot(kf_xerr(5,:),'.');
+ylabel('vY (m/s)')
+xlabel('time U.A.')
+ylim([-5 5])
+title('Subplot 1: vY ')
+legend ('raw navSolution','raw kf state','kferr','Location','east')
+
+subplot(2,2,3);
+plot(navSolution.vZ,'.');
+hold on;grid on
+plot(kf_x(6,:),'.');
+plot(kf_xerr(6,:),'.');
+ylabel('vZ (m/s)')
+xlabel('time U.A.')
+ylim([-5 5])
+title('Subplot 1: vZ ')
+legend ('raw navSolution','raw kf state','kferr','Location','east')
+
+sgtitle('velocities') 
+
+%% --- VTL UTM centered POSITION: GNSS SDR  plot --------------------------------------
+
+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(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')
+
+subplot(2,2,2);
+plot(navSolution.Y-navSolution.Y(1),'.');
+hold on;grid on
+plot(kf_x(2,:)-kf_x(2,1),'.');
+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')
+
+subplot(2,2,3);
+plot(navSolution.Z-navSolution.Z(1),'.');
+hold on;grid on
+plot(kf_x(3,:)-kf_x(3,1),'.');
+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')
+
+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
diff --git a/src/utils/matlab/vtl/vtl_prototype.m b/src/utils/matlab/vtl/vtl_prototype.m
new file mode 100644
index 000000000..cba492250
--- /dev/null
+++ b/src/utils/matlab/vtl/vtl_prototype.m
@@ -0,0 +1,149 @@
+% VTL prototype
+% -------------------------------------------------------------------------
+%
+% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
+% This file is part of GNSS-SDR.
+%
+% Copyright (C) 2010-2019  (see AUTHORS file for a list of contributors)
+% SPDX-License-Identifier: GPL-3.0-or-later
+%
+% -------------------------------------------------------------------------
+%
+%%
+clc
+close all
+clearvars 
+
+% if ~exist('gps_l1_ca_read_pvt_raw_dump.m', 'file')
+%     addpath('./libs')
+% end
+% 
+% if ~exist('cat2geo.m', 'file')
+%     addpath('./libs/geoFunctions')
+% end
+SPEED_OF_LIGHT_M_S=299792458.0;
+Lambda_GPS_L1=0.1902937;
+point_of_closure=6000;
+%%
+samplingFreq=5000000;
+channels=6;
+TTFF_sec=41.48;
+
+%% ============================ PARSER TO STRUCT ============================
+plot_skyplot=0;
+plot_reference=1;
+
+navSolution = GnssSDR2struct('PVT_raw.mat');
+refSolution = SpirentMotion2struct('..\log_spirent\motion_V1_SPF_LD_05.csv');
+%
+load observables\observable_raw.mat
+% refSatData = SpirentSatData2struct('..\log_spirent\sat_data_V1A1_SPF_LD_05.csv');
+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','sat_dopp_hz')
+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')
+%%
+% 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);
+% LON_FILT = movmean(navSolution.longitude,moving_avg_factor);
+% HEIGH_FILT = movmean(navSolution.height,moving_avg_factor);
+% 
+% X_FILT = movmean(navSolution.X,moving_avg_factor);
+% Y_FILT = movmean(navSolution.Y,moving_avg_factor);
+% Z_FILT = movmean(navSolution.Z,moving_avg_factor);
+% 
+% vX_FILT = movmean(navSolution.vX,moving_avg_factor);
+% vY_FILT = movmean(navSolution.vY,moving_avg_factor);
+% vZ_FILT = movmean(navSolution.vZ,moving_avg_factor);
+% 
+%%
+%general_raw_plot
+vtl_general_plot
+%% ============================================== ==============================================
+% 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','.')
+%     xlim([0,140]);
+%     xlabel('')
+%     ylabel('Doppler (Hz)')
+%     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','.')
+%     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,:)','.')
+%     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')
+%     hold off
+%     grid on
+% end
\ No newline at end of file