mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-11-15 14:25:00 +00:00
Code Issues Releases Wiki Activity

MOD: updated matlab vtl_kf

Browse Source
This commit is contained in:
miguekf 2022-12-11 02:11:12 +01:00
parent ad50a5b1e7
commit 9b740b454d
4 changed files with 205 additions and 193 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

87
src/utils/matlab/vtl/kf_prototype.m
View File

@ -28,36 +28,7 @@ kf_yerr = zeros(2*sat_number, 1);
% kf_xerr = zeros(8, 1); % kf_xerr = zeros(8, 1);
kf_S = zeros(2*sat_number, 2*sat_number); % kf_P_y innovation covariance matrix kf_S = zeros(2*sat_number, 2*sat_number); % kf_P_y innovation covariance matrix
%% State error Covariance Matrix Q (PVT) %% pre-allocate for speed
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
d = zeros(sat_number, 1); d = zeros(sat_number, 1);
rho_pri = zeros(sat_number, 1); rho_pri = zeros(sat_number, 1);
rhoDot_pri = zeros(sat_number, 1); rhoDot_pri = zeros(sat_number, 1);
@ -74,6 +45,20 @@ sat_dopp_hz_filt=zeros(sat_number,length(navSolution.RX_time));
% receiver solution from rtklib_solver % receiver solution from rtklib_solver
for t=2:length(navSolution.RX_time) 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 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) if (t<length(navSolution.RX_time)-point_of_closure)
kf_x(1,t) = navSolution.X(t); kf_x(1,t) = navSolution.X(t);
@ -114,40 +99,40 @@ for t=2:length(navSolution.RX_time)
for chan=1:5 %neccesary quantities for chan=1:5 %neccesary quantities
d(chan)=(sat_posX_m(chan,t)-x_u)^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_posY_m(chan,t)-y_u)^2;
d(chan)=d(chan)+(sat_posZ_m(chan,t)-z_u)^2; d(chan)=d(chan)+(sat_posZ_m(chan,t)-z_u)^2;
d(chan)=sqrt(d(chan)); d(chan)=sqrt(d(chan));
c_pr_m(chan,t)=d(chan)+cdeltat_u(t); c_pr_m(chan,t)=d(chan)+cdeltat_u(t);
a_x(chan,t)=-(sat_posX_m(chan,t)-x_u)/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_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); 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)... 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_velY(chan,t)-yDot_u)*a_y(chan,t)...
+(sat_velZ(chan,t)-zDot_u)*a_z(chan,t); +(sat_velZ(chan,t)-zDot_u)*a_z(chan,t);
end end
for chan=1:5 % Measurement matrix H assembling 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) % 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, 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; 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 end
% unobsv(t) = length(kf_F) - rank(obsv(kf_F,kf_H)); % unobsv(t) = length(kf_F) - rank(obsv(kf_F,kf_H));
% !!!! Limitaciones % !!!! Limitaciones
% obsv no se recomienda para el diseño de control, ya que calcular el rango de la matriz de observabilidad % 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 % 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]. % sistemas con más de unos cuantos estados. Este hecho está bien documentado en la sección III de [1].
% Kalman estimation (measurement update) % Kalman estimation (measurement update)
for chan=1:5 % Measurement matrix H assembling 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,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+sat_number,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1)-rhoDot_pri(chan,t);
end end
% DOUBLES DIFFERENCES % DOUBLES DIFFERENCES

3
src/utils/matlab/vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m
View File

@ -52,6 +52,7 @@ end
if(load_vtl) if(load_vtl)
vtlSolution = Vtl2struct('dump_vtl_file.csv'); vtlSolution = Vtl2struct('dump_vtl_file.csv');
end end
%% calculate LOS Rx-sat if advance_vtl_data_available=1 %% calculate LOS Rx-sat if advance_vtl_data_available=1
if(advance_vtl_data_available) if(advance_vtl_data_available)
@ -348,3 +349,5 @@ if(load_observables)
hold off hold off
grid on grid on
end end
%%
dopp_filtered_plotting

134
src/utils/matlab/vtl/vtl_general_plot.m
View File

@ -1,6 +1,56 @@
%% %%
% vtl_general_plot.m % 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 VELOCITY: GNSS SDR plot --------------------------------------
VTL_VEL=figure('Name','velocities'); VTL_VEL=figure('Name','velocities');
subplot(2,2,1); subplot(2,2,1);
@ -14,7 +64,7 @@ ylabel('vX (m/s)')
xlabel('time U.A.') xlabel('time U.A.')
ylim([-5 5]) ylim([-5 5])
title('Subplot 1: vX ') 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); subplot(2,2,2);
plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.vY,'.'); plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.vY,'.');
@ -27,7 +77,7 @@ ylabel('vY (m/s)')
xlabel('time U.A.') xlabel('time U.A.')
ylim([-5 5]) ylim([-5 5])
title('Subplot 1: vY ') 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); subplot(2,2,3);
plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.vZ,'.'); plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.vZ,'.');
@ -40,7 +90,7 @@ ylabel('vZ (m/s)')
xlabel('time U.A.') xlabel('time U.A.')
ylim([-5 5]) ylim([-5 5])
title('Subplot 1: vZ ') title('Subplot 1: vZ ')
legend ('raw navSolution','raw kf state','reference','Location','east') legend ('raw navSolution','raw kf state','reference','Location','eastoutside')
sgtitle('velocities') sgtitle('velocities')
@ -50,38 +100,104 @@ VTL_POS=figure('Name','VTL UTM COORD CENTERED IN 1^{ST} POSITION');
subplot(2,2,1); subplot(2,2,1);
plot(navSolution.X-navSolution.X(1),'.'); plot(navSolution.X-navSolution.X(1),'.');
hold on;grid on 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,:),'.'); plot(kf_xerr(1,:),'.');
ylabel('X (m)') ylabel('X (m)')
xlabel('time U.A.') xlabel('time U.A.')
ylim([-200 800]) ylim([-200 800])
title('Subplot 1: X ') 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); subplot(2,2,2);
plot(navSolution.Y-navSolution.Y(1),'.'); plot(navSolution.Y-navSolution.Y(1),'.');
hold on;grid on 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,:),'.'); plot(kf_xerr(2,:),'.');
ylabel('Y (m)') ylabel('Y (m)')
xlabel('time U.A.') xlabel('time U.A.')
ylim([-200 50]) ylim([-200 50])
title('Subplot 1: Y ') 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); subplot(2,2,3);
plot(navSolution.Z-navSolution.Z(1),'.'); plot(navSolution.Z-navSolution.Z(1),'.');
hold on;grid on 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,:),'.'); plot(kf_xerr(3,:),'.');
ylabel('Z (m)') ylabel('Z (m)')
xlabel('time U.A.') xlabel('time U.A.')
ylim([-350 50]) ylim([-350 50])
title('Subplot 1: Z ') 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') 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 %% STATE PLOT
VTL_STATE=figure('Name','VTL STATE'); VTL_STATE=figure('Name','VTL STATE');
subplot(2,2,1); subplot(2,2,1);

138
src/utils/matlab/vtl/vtl_prototype.m
View File

@ -23,8 +23,24 @@ clearvars
% end % end
SPEED_OF_LIGHT_M_S=299792458.0; SPEED_OF_LIGHT_M_S=299792458.0;
Lambda_GPS_L1=0.1902937; 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; samplingFreq=5000000;
channels=6; channels=6;
TTFF_sec=41.48; TTFF_sec=41.48;
@ -50,46 +66,10 @@ if(load_observables)
end end
%% %%
% vtlSolution = Vtl2struct('dump_vtl_file.csv'); % 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 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 ======================================================= %% ====== FILTERING =======================================================
% moving_avg_factor= 500; % moving_avg_factor= 500;
% LAT_FILT = movmean(navSolution.latitude,moving_avg_factor); % 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 vtl_general_plot
%% ============================================== ============================================== %% ============================================== ==============================================
time_reference_spirent_obs=129780;%s %%
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
if(load_observables) 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
Carrier_Doppler_hz_sim=zeros(length(refSatData.GPS.SIM_time),6); legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','Location','eastoutside')
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