mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-11-15 14:25:00 +00:00
MOD: updated matlab vtl_kf
This commit is contained in:
miguekf
parent
ad50a5b1e7
commit
9b740b454d
@ -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;
|
||||
|
||||
@ -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
|
||||
end
|
||||
%%
|
||||
dopp_filtered_plotting
|
||||
@ -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);
|
||||
|
||||
@ -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')
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user