mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-11-15 14:25:00 +00:00
Merge branch 'vtl' of https://github.com/gnss-sdr/gnss-sdr-vtl into vtl
This commit is contained in:
M.A.Gomez
commit
7fdc452715
@ -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
|
||||
@ -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)
|
||||
|
||||
@ -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')
|
||||
%% 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')
|
||||
@ -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
|
||||
end
|
||||
|
||||
|
||||
67
src/utils/python/analysis.py
Normal file
67
src/utils/python/analysis.py
Normal file
@ -0,0 +1,67 @@
|
||||
#!/usr/bin/env python3
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
Created on Wed Nov 23 12:45:37 2022
|
||||
|
||||
@author: miguel
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
import h5py
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
#%%
|
||||
f = h5py.File('PVT_raw.mat','r')
|
||||
#data = f.get('rateQualityOutTrim/date')
|
||||
#data = np.array(data)
|
||||
print(f.keys()) # gives the name of the variables stored
|
||||
RX_time = f.get('RX_time')
|
||||
|
||||
vel_x = f.get('vel_x')
|
||||
vel_y = f.get('vel_y')
|
||||
vel_z = f.get('vel_z')
|
||||
|
||||
pos_x = f.get('pos_x')
|
||||
pos_y = f.get('pos_y')
|
||||
pos_z = f.get('pos_z')
|
||||
|
||||
#kf_state1=f.get('vtl_kf_state_1')
|
||||
|
||||
#%%
|
||||
for keys in f:
|
||||
keys=f.get(keys)
|
||||
#%%
|
||||
#plt.plot(kf_state1[:,0]-kf_state1[0,0],'o',label='kf_state1')
|
||||
|
||||
#plt.show()
|
||||
#%%
|
||||
plt.scatter(RX_time,pos_x)
|
||||
plt.show()
|
||||
plt.scatter(RX_time,pos_y-pos_y[0])
|
||||
plt.show()
|
||||
plt.scatter(RX_time,pos_z-pos_z[0])
|
||||
plt.show()
|
||||
|
||||
#%%
|
||||
plt.scatter(RX_time,pos_x-pos_x[0])
|
||||
#plt.ylim([4863484, 4863591])
|
||||
plt.ylim([-20,110])
|
||||
plt.ylabel('X [m]')
|
||||
plt.show()
|
||||
plt.scatter(RX_time,pos_y-pos_y[0])
|
||||
plt.ylabel('Y [m]')
|
||||
plt.ylim([-85, 5])
|
||||
plt.show()
|
||||
plt.scatter(RX_time,pos_z-pos_z[0])
|
||||
plt.ylabel('Z [m]')
|
||||
plt.ylim([-110,25])
|
||||
plt.show()
|
||||
|
||||
#%%
|
||||
plt.scatter(RX_time,vel_x)
|
||||
plt.show()
|
||||
plt.scatter(RX_time,vel_y-vel_y[0])
|
||||
plt.show()
|
||||
plt.scatter(RX_time,vel_z-vel_z[0])
|
||||
plt.show()
|
||||
|
||||
96
src/utils/python/vtl_analysis.py
Normal file
96
src/utils/python/vtl_analysis.py
Normal file
@ -0,0 +1,96 @@
|
||||
#!/usr/bin/env python3
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
Created on Sat Nov 26 22:18:25 2022
|
||||
|
||||
@author: miguel
|
||||
"""
|
||||
#%%
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
#%%
|
||||
with open('dump_vtl_file.csv') as f:
|
||||
rawdatos=np.genfromtxt(f,dtype=str,delimiter=",")
|
||||
#%%
|
||||
idx_kf_err = np.where(rawdatos[:,0]=='kf_xerr') [0]# se indexa
|
||||
idx_kf_state = np.where(rawdatos[:,0]=='kf_state')[0]
|
||||
idx_rtklib = np.where(rawdatos[:,0]=='rtklib_state')[0]
|
||||
#%%
|
||||
kf_err=rawdatos[idx_kf_err,1:].astype(float)
|
||||
kf_estate=rawdatos[idx_kf_state,1:].astype(float)
|
||||
rtklib=rawdatos[idx_rtklib,1:].astype(float)
|
||||
|
||||
#%%
|
||||
plt.close()
|
||||
plt.plot(kf_err[:,0],'o',label='kf_err')
|
||||
plt.plot(kf_estate[:,0]-kf_estate[0,0],'o',label='kf_estate')
|
||||
plt.plot(rtklib[:,0]-rtklib[0,0],'o',label='rtklib')
|
||||
plt.xlabel('time U.A.')
|
||||
plt.ylabel('X [m]')
|
||||
plt.legend()
|
||||
plt.show()
|
||||
|
||||
plt.plot(kf_err[:,1],'o',label='kf_err')
|
||||
plt.plot(kf_estate[:,1]-kf_estate[0,1],'o',label='kf_estate')
|
||||
plt.plot(rtklib[:,1]-rtklib[0,1],'o',label='rtklib')
|
||||
plt.xlabel('time U.A.')
|
||||
plt.ylabel('Y [m]')
|
||||
plt.legend()
|
||||
plt.show()
|
||||
|
||||
plt.plot(kf_err[:,2],'o',label='kf_err')
|
||||
plt.plot(kf_estate[:,2]-kf_estate[0,2],'o',label='kf_estate')
|
||||
plt.plot(rtklib[:,2]-rtklib[0,2],'o',label='rtklib')
|
||||
plt.xlabel('time U.A.')
|
||||
plt.ylabel('Z [m]')
|
||||
plt.legend()
|
||||
plt.show()
|
||||
|
||||
#%%
|
||||
plt.close()
|
||||
plt.plot(kf_estate[:,0]-kf_estate[0,0]+kf_err[:,0],'o',label='kf_corr_estate')
|
||||
plt.plot(rtklib[:,0]-rtklib[0,0],'o',label='rtklib')
|
||||
plt.xlabel('time U.A.')
|
||||
plt.ylabel('X [m]')
|
||||
plt.title('X corrected')
|
||||
plt.legend()
|
||||
plt.show()
|
||||
|
||||
plt.plot(kf_estate[:,1]-kf_estate[0,1]+kf_err[:,1],'o',label='kf_corr_estate')
|
||||
plt.plot(rtklib[:,1]-rtklib[0,1],'o',label='rtklib')
|
||||
plt.xlabel('time U.A.')
|
||||
plt.ylabel('Y [m]')
|
||||
plt.title('Y corrected')
|
||||
plt.legend()
|
||||
plt.show()
|
||||
|
||||
plt.plot(kf_estate[:,2]-kf_estate[0,2]+kf_err[:,2],'o',label='kf_corr_estate')
|
||||
plt.plot(rtklib[:,2]-rtklib[0,2],'o',label='rtklib')
|
||||
plt.xlabel('time U.A.')
|
||||
plt.ylabel('Z [m]')
|
||||
plt.title('Z corrected')
|
||||
plt.legend()
|
||||
plt.show()
|
||||
|
||||
#%%
|
||||
plt.close()
|
||||
plt.plot(kf_err[:,3],'o',label='kf_err')
|
||||
plt.plot(kf_estate[:,3]-kf_estate[0,3],'o',label='kf_estate')
|
||||
plt.plot(rtklib[:,3]-rtklib[0,3],'o',label='rtklib')
|
||||
plt.xlabel('time U.A.')
|
||||
plt.legend()
|
||||
plt.show()
|
||||
|
||||
plt.plot(kf_err[:,4],'o',label='kf_err')
|
||||
plt.plot(kf_estate[:,4]-kf_estate[0,4],'o',label='kf_estate')
|
||||
plt.plot(rtklib[:,4]-rtklib[0,4],'o',label='rtklib')
|
||||
plt.xlabel('time U.A.')
|
||||
plt.legend()
|
||||
plt.show()
|
||||
|
||||
plt.plot(kf_err[:,5],'o',label='kf_err')
|
||||
plt.plot(kf_estate[:,5]-kf_estate[0,5],'o',label='kf_estate')
|
||||
plt.plot(rtklib[:,5]-rtklib[0,5],'o',label='rtklib')
|
||||
plt.xlabel('time U.A.')
|
||||
plt.legend()
|
||||
plt.show()
|
||||
Loading…
Reference in New Issue
Block a user