diff --git a/src/utils/matlab/vtl/doppler_vtl_plot.m b/src/utils/matlab/vtl/doppler_vtl_plot.m new file mode 100644 index 000000000..bf175bd6c --- /dev/null +++ b/src/utils/matlab/vtl/doppler_vtl_plot.m @@ -0,0 +1,35 @@ +%% LOAD + + Carrier_Doppler_hz_sim=zeros(length(refSatData.GPS.SIM_time),channels); + + 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(3);%PRN 20 + Carrier_Doppler_hz_sim(i,3)=refSatData.GPS.series(i).doppler_shift(8);%PRN 17 + Carrier_Doppler_hz_sim(i,4)=refSatData.GPS.series(i).doppler_shift(6);%PRN 12 + Carrier_Doppler_hz_sim(i,5)=refSatData.GPS.series(i).doppler_shift(5);%PRN 9 + Carrier_Doppler_hz_sim(i,6)=refSatData.GPS.series(i).doppler_shift(2);%PRN 5 + Carrier_Doppler_hz_sim(i,7)=refSatData.GPS.series(i).doppler_shift(1);%PRN 4 + Carrier_Doppler_hz_sim(i,8)=refSatData.GPS.series(i).doppler_shift(7);%PRN 2 + end + +%% + + figure('Name','RX_Carrier_Doppler_hz'); +for channel_cnt=1:channels + subplot(3,3,channel_cnt) + plot(linspace(0,tFinal,length(Carrier_Doppler_hz(channel_cnt,:)')), Carrier_Doppler_hz(channel_cnt,:)','s') + xlim([0,tFinal]); + ylim([min(Carrier_Doppler_hz_sim(:,channel_cnt+1)),max(Carrier_Doppler_hz_sim(:,channel_cnt+1))]); + xlabel('') + ylabel('Doppler (Hz)') + xlabel('time from simulation init (seconds)') + grid on + hold on + legend(['PRN' num2str(rx_PRN(channel_cnt)) ' GNSS-SDR'],'Location','eastoutside')% bench + plot(refSatData.GPS.SIM_time/1000, Carrier_Doppler_hz_sim(:,channel_cnt+1)','.','DisplayName','SPIRENT reference') + plot(linspace(TTFF_sec+23,tFinal,length(sat_dopp_hz_filt(channel_cnt,:))),sat_dopp_hz_filt(channel_cnt,:),'o','DisplayName','filtered VTL') + hold off;grid minor +end + + diff --git a/src/utils/matlab/vtl/kf_prototype_model.m b/src/utils/matlab/vtl/kf_prototype_model.m new file mode 100644 index 000000000..602665755 --- /dev/null +++ b/src/utils/matlab/vtl/kf_prototype_model.m @@ -0,0 +1,280 @@ +%% vtl KF +load rocket_model_dynamics.mat +%% +sat_number=6; % limit by csv? +%% ################## Kalman filter initialization ###################################### +state_number=9; +% covariances (static) +kf_P_x = eye(state_number); %TODO: use a real value. +kf_x = zeros(state_number, 1); +kf_R = zeros(2*sat_number, 2*sat_number); +kf_dt=0.05; +% kf_F = eye(state_number, state_number); +% 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_F=[ 1 0 0 kf_dt 0 0 0 0 0 + 0 1 0 0 kf_dt 0 0 0 0 + 0 0 1 0 0 kf_dt 0 0 0 + 0 0 0 1 0 0 0 0 0 + 0 0 0 0 1 0 0 0 0 + 0 0 0 0 0 1 0 0 0 + 0 0 0 0 0 0 1 kf_dt kf_dt^2/2 + 0 0 0 0 0 0 0 1 kf_dt + 0 0 0 0 0 0 0 0 1]; + +% kf_F=[ 1 0 0 kf_dt 0 0 0 0 +% 0 1 0 0 kf_dt 0 0 0 +% 0 0 1 0 0 kf_dt 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 kf_dt +% 0 0 0 0 0 0 0 1]; +% +kf_H = zeros(2*sat_number,state_number); +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 + +%% 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 +index=0; +modelflag=0; +t_from_lunch=0; +for t=2:length(navSolution.RX_time) + + index=index+1; + %% State error Covariance Matrix Q (PVT) and R (MEASUREMENTS) + if (index < point_of_closure) + %% State error Covariance Matrix Q (PVT) + kf_Q = blkdiag(posx_var,posy_var,posz_var,velx_var,vely_var,velz_var,clk_bias_var,clk_drift_var,clk_d_drift_var); + %% + + % Measurement error Covariance Matrix R assembling + kf_R = blkdiag(eye(sat_number)*pr_var,eye(sat_number)*pr_dot_var); + + else + kf_Q = blkdiag(posx_var,posy_var,posz_var,velx_var,vely_var,velz_var,clk_bias_var,clk_drift_var,clk_d_drift_var); + for chan=1:sat_number + kf_R(chan,chan) = pr_var*sat_CN0_dBhz(chan,t)/200; + kf_R(chan+sat_number,chan+sat_number)= pr_dot_var*sat_CN0_dBhz(chan,t)/200; + end + end + + if(~modelflag) + clear x_u y_u z_u xDot_u yDot_u zDot_u cdeltatDot_u cdeltatDot_u_g cdeltat_u_g + if (index < point_of_closure) + disp('one step') + 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)*SPEED_OF_LIGHT_M_S; + kf_x(8,t) = clk_drift(t)*SPEED_OF_LIGHT_M_S;%new_data.rx_dts(1); + kf_x(9,t) = 1.0; + + 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); + + % Kalman state prediction (time update) + kf_P_x = eye(state_number)*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 + disp('feedback') + 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 + else + disp('MODEL') + t_from_lunch=t_from_lunch+3; + if(t_from_lunch>length(rocket_model_dynamics.vX)) + t_from_lunch=length(rocket_model_dynamics.vX); + end + kf_x(1,t) = rocket_model_dynamics.X(t_from_lunch); + kf_x(2,t) = rocket_model_dynamics.Y(t_from_lunch); + kf_x(3,t) = rocket_model_dynamics.Z(t_from_lunch); + kf_x(4,t) = rocket_model_dynamics.vX(t_from_lunch); + kf_x(5,t) = rocket_model_dynamics.vY(t_from_lunch); + kf_x(6,t) = rocket_model_dynamics.vZ(t_from_lunch); + 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); + kf_x(9,t) = 1.0; + + 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); + + % Kalman state prediction (time update) + kf_P_x = eye(state_number)*1; %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 + end + + for chan=1:sat_number %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); + end + + + for chan=1:sat_number % Measurement matrix H assembling + % It has st_nmbr columns (st_nmbr 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]. + + % Kalman stimation (measurement update) + for chan=1:sat_number % Measurement matrix H assembling + kf_yerr(chan,t)=c_pr_m(chan,t)-sat_prg_m(chan,t); + if (t100)>1) + modelflag=1; +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:sat_number %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); + end + + kf_H = zeros(2*sat_number,state_number); + + for chan=1:sat_number % Measurement matrix H assembling + % It has st_nmbr columns (st_nmbr 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; + disp("kf_yerr_g:") + disp(kf_yerr_g(:,t)) + disp('zDot_u:') + disp(zDot_u) + % Filtered pseudorange error measurement (in m) AND Filtered Doppler shift measurements (in Hz): + for chan=1:sat_number % 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! + % d_dt_clk_drift=(corr_kf_state(8,t)-corr_kf_state(8,t-1)); + % d_dt_clk_drift=0; + sat_dopp_hz_filt(chan,t)=(rhoDot_pri_filt(chan,t)-corr_kf_state(8,t))/Lambda_GPS_L1; + err_carrier_phase_rads_filt(chan,t) = trapz(kf_yerr_g(chan+sat_number,1:t)/Lambda_GPS_L1)*2*kf_dt; + carrier_freq_hz =GPS_L1_freq_hz+sat_dopp_hz_filt(chan,t); + % carrier_freq_rate_hz_s = 0; + err_code_phase_chips(chan,t) = (kf_yerr_g(chan,t))/SPEED_OF_LIGHT_M_S*1023e3; + end + +end \ No newline at end of file diff --git a/src/utils/matlab/vtl/pr_m_vtl_plot.m b/src/utils/matlab/vtl/pr_m_vtl_plot.m new file mode 100644 index 000000000..da96bb1a6 --- /dev/null +++ b/src/utils/matlab/vtl/pr_m_vtl_plot.m @@ -0,0 +1,49 @@ +%% LOAD + + Pseudorange_m_sim=zeros(length(refSatData.GPS.SIM_time),channels); + + for i=1:length(refSatData.GPS.SIM_time) + 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(3);%PRN 20 + Pseudorange_m_sim(i,3)=refSatData.GPS.series(i).pr_m(8);%PRN 17 + Pseudorange_m_sim(i,4)=refSatData.GPS.series(i).pr_m(6);%PRN 12 + Pseudorange_m_sim(i,5)=refSatData.GPS.series(i).pr_m(5);%PRN 9 + Pseudorange_m_sim(i,6)=refSatData.GPS.series(i).pr_m(2);%PRN 5 + Pseudorange_m_sim(i,7)=refSatData.GPS.series(i).pr_m(1);%PRN 4 + Pseudorange_m_sim(i,8)=refSatData.GPS.series(i).pr_m(7);%PRN 2 + end +%% + + figure('Name','RX_PseudoRange_m'); +for channel_cnt=1:channels + subplot(3,3,channel_cnt) + plot(linspace(0,tFinal,length(Pseudorange_m(channel_cnt,:)')), Pseudorange_m(channel_cnt,:)','s') + xlim([0,tFinal]); +% ylim([min(Pseudorange_m_sim(:,channel_cnt)),max(Pseudorange_m_sim(:,channel_cnt))]); + xlabel('') + ylabel('PR [m] (m)') + xlabel('time from simulation init (seconds)') + grid on + hold on + legend(['PRN' num2str(rx_PRN(channel_cnt)) ' GNSS-SDR'],'Location','eastoutside')% bench +% plot(refSatData.GPS.SIM_time/1000, Pseudorange_m_sim(:,channel_cnt)','.','DisplayName','SPIRENT reference') +% plot(navSolution.RX_time(1,:)-time_reference_spirent_obs, sat_dopp_hz_filt(1,:),'o','DisplayName','filtered VTL') + hold off;grid minor +end +%% + figure('Name','Rsim_PseudoRange_m'); +for channel_cnt=1:channels + subplot(3,3,channel_cnt) + plot(linspace(0,tFinal,length(Pseudorange_m(channel_cnt,:)')), Pseudorange_m(channel_cnt,:)','s') + xlim([0,tFinal]); + ylim([min(Pseudorange_m_sim(:,channel_cnt)),max(Pseudorange_m_sim(:,channel_cnt))]); + xlabel('') + ylabel('PR [m] (m)') + xlabel('time from simulation init (seconds)') + grid on + hold on + legend(['PRN' num2str(rx_PRN(channel_cnt)) ' GNSS-SDR'],'Location','eastoutside')% bench + plot(refSatData.GPS.SIM_time/1000, Pseudorange_m_sim(:,channel_cnt)','.','DisplayName','SPIRENT reference') +% plot(navSolution.RX_time(1,:)-time_reference_spirent_obs, sat_dopp_hz_filt(1,:),'o','DisplayName','filtered VTL') + hold off;grid minor +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 index b6f964ae1..9004ecb75 100644 --- a/src/utils/matlab/vtl/vtl_general_plot.m +++ b/src/utils/matlab/vtl/vtl_general_plot.m @@ -1,28 +1,17 @@ %% % 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) +% for i=1:length(refSatData.GPS.SIM_time) +% Pseudorange_m_sim(i,1)=refSatData.GPS.series(i).pr_m(9);%PRN 30 +% Pseudorange_m_sim(i,2)=refSatData.GPS.series(i).pr_m(3);%PRN 29 +% Pseudorange_m_sim(i,3)=refSatData.GPS.series(i).pr_m(5);%PRN 24 +% Pseudorange_m_sim(i,4)=refSatData.GPS.series(i).pr_m(2);%PRN 12 +% Pseudorange_m_sim(i,5)=refSatData.GPS.series(i).pr_m(8);%PRN 10 +% Pseudorange_m_sim(i,6)=refSatData.GPS.series(i).pr_m(6);%PRN 5 +% end +% end % ------------------------------------- % if(load_observables) % Rx_pseudo_all=figure('Name','RX_pr_m');plot(RX_time(1,:)-time_reference_spirent_obs, Pseudorange_m','s') @@ -51,7 +40,7 @@ end % hold off % grid on % end -%---VTL VELOCITY: GNSS SDR plot -------------------------------------- +%% ---VTL VELOCITY: GNSS SDR plot -------------------------------------- VTL_VEL=figure('Name','velocities'); subplot(2,2,1); plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.vX,'.'); @@ -62,7 +51,7 @@ plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.vX... % plot(navSolution.RX_time-navSolution.RX_time(1),kf_xerr(4,:),'.'); ylabel('vX (m/s)') xlabel('time U.A.') -ylim([-5 5]) +% ylim([-5 5]) title('Subplot 1: vX ') legend ('raw navSolution','raw kf state','SPIRENT reference','Location','eastoutside') @@ -75,7 +64,7 @@ plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.vY... ,'.','DisplayName','SPIRENT reference') ylabel('vY (m/s)') xlabel('time U.A.') -ylim([-5 5]) +% ylim([-5 5]) title('Subplot 1: vY ') legend ('raw navSolution','raw kf state','SPIRENT reference','Location','eastoutside') @@ -88,7 +77,7 @@ plot(refSolution.SIM_time/1000-TTFF_sec,refSolution.vZ... ,'.','DisplayName','SPIRENT reference') ylabel('vZ (m/s)') xlabel('time U.A.') -ylim([-5 5]) +% ylim([-5 5]) title('Subplot 1: vZ ') legend ('raw navSolution','raw kf state','SPIRENT reference','Location','eastoutside') @@ -96,7 +85,7 @@ sgtitle('velocities') %% --- VTL UTM centered POSITION: GNSS SDR plot -------------------------------------- -VTL_POS=figure('Name','VTL UTM COORD CENTERED IN 1^{ST} POSITION'); +VTL_POS=figure('Name','VTL UTM COORD '); subplot(2,2,1); plot(navSolution.RX_time-navSolution.RX_time(1),navSolution.X,'.'); hold on;grid on @@ -131,84 +120,10 @@ title('Subplot 1: Z ') legend ('raw navSolution','raw kf state','SPIRENT ref','Location','eastoutside') sgtitle('VTL UTM COORD') -%% -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'); - subplot(2,2,1) - 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','SPIRENT 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'); - subplot(2,2,2) - 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','SPIRENT 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'); - subplot(2,2,3) - 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','SPIRENT 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'); - subplot(2,2,4) - 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','SPIRENT 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); +subplot(2,3,1); plot(navSolution.RX_time-navSolution.RX_time(1),[navSolution.X-navSolution.X(1);... navSolution.Y-navSolution.Y(1) ;navSolution.Z-navSolution.Z(1)],... 'b.','DisplayName','RTKLIB solution'); @@ -219,13 +134,13 @@ plot(refSolution.SIM_time/1000-TTFF_sec,[refSolution.X-refSolution.X(spirent_ind refSolution.Y-refSolution.Y(spirent_index_TTFF) refSolution.Z-refSolution.Z(spirent_index_TTFF)],... 'r.','DisplayName','SPIRENT reference'); legend('Location','eastoutside'); -ylim([-200,200]) -xlim([0,120]) +% ylim([-200,200]) +xlim([0,tFinal]) ylabel('X Y Z (m)') xlabel('time [s]') title('Subplot 1: POSITION [m]') -subplot(2,2,2); +subplot(2,3,2); plot(navSolution.RX_time-navSolution.RX_time(1),[navSolution.vX;... navSolution.vY; navSolution.vZ],... 'b.','DisplayName','RTKLIB solution'); @@ -236,30 +151,41 @@ plot(refSolution.SIM_time/1000-TTFF_sec,[refSolution.vX... refSolution.vY refSolution.vZ],... 'r.','DisplayName','SPIRENT reference'); -xlim([0,120]) +xlim([0,tFinal]) ylabel('vX vY vZ (m/s)') xlabel('time [s]') title('Subplot 1: VELOCITIES [m/s]') -subplot(2,2,3); +subplot(2,3,3); plot(navSolution.RX_time-navSolution.RX_time(1),clk_bias_s*SPEED_OF_LIGHT_M_S,... 'b.','DisplayName','RTKLIB solution'); hold on;grid on plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(7,:),... 'k.','DisplayName','filt VTL'); -ylim([3019190, 3019700]) -xlim([0,120]) +% ylim([3633390, 3634580]) +xlim([0,tFinal]) ylabel('clk bias (m)') xlabel('time [s]') title('Subplot 1: clk bias [m]') -subplot(2,2,4); +subplot(2,3,4); plot(navSolution.RX_time-navSolution.RX_time(1),clk_drift*SPEED_OF_LIGHT_M_S,... 'b.','DisplayName','RTKLIB solution'); hold on;grid on plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(8,:),... 'k.','DisplayName','filt VTL'); -xlim([0,120]) +xlim([0,tFinal]) +ylabel('clk drift (m/s)') +xlabel('time [s]') +title('Subplot 1: clk drift [m/s]') + +subplot(2,3,5); +plot(navSolution.RX_time(1:end-1)-navSolution.RX_time(1),diff(clk_drift)/kf_dt*SPEED_OF_LIGHT_M_S,... + 'b.','DisplayName','RTKLIB solution'); +hold on;grid on +plot(navSolution.RX_time-navSolution.RX_time(1),corr_kf_state(9,:),... + 'k.','DisplayName','filt VTL'); +xlim([0,tFinal]) ylabel('clk drift (m/s)') xlabel('time [s]') title('Subplot 1: clk drift [m/s]') diff --git a/src/utils/matlab/vtl/vtl_prototype.m b/src/utils/matlab/vtl/vtl_prototype.m deleted file mode 100644 index c3abe3392..000000000 --- a/src/utils/matlab/vtl/vtl_prototype.m +++ /dev/null @@ -1,99 +0,0 @@ -% 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; -GPS_L1_freq_hz=1575.42e6; -point_of_closure=4060; - -%% ==================== 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=20;%m^2 -pr_dot_var=2;%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; -spirent_index_TTFF=416; - -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'); -% -load observables\observables_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','clk_drift','sat_dopp_hz','user_clk_offset') - -if(load_observables) - load observables\observables_raw.mat - refSatData = SpirentSatData2struct('..\log_spirent\sat_data_V1A1_SPF_LD_05.csv'); -end -%% -% vtlSolution = Vtl2struct('dump_vtl_file.csv'); -%% - -kf_prototype - -%% ====== 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 - -%% ============================================== ============================================== -%% -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') - diff --git a/src/utils/matlab/vtl/vtl_tracking.m b/src/utils/matlab/vtl/vtl_tracking.m deleted file mode 100644 index c78a70db6..000000000 --- a/src/utils/matlab/vtl/vtl_tracking.m +++ /dev/null @@ -1,80 +0,0 @@ -% ------------------------------------------------------------------------- -% -% 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; -%% -trkSolution=trk2struct('dump_trk_file.csv'); - -%% split by solution type -figure;sgtitle('real doppler') -for chan=0:4 -eval(['[indCH' num2str(chan) ',~]= find(trkSolution.dopp.real==chan);']) -eval(['Dopp_real_CH' num2str(chan) '=trkSolution.dopp.real(indCH' num2str(chan) ',2);']) -eval(['subplot(2,3,' num2str(chan+1) ');plot(Dopp_real_CH' num2str(chan) ')']) -end -figure;sgtitle('cmd doppler') -for chan=0:4 -eval(['[indCH' num2str(chan) ',~]= find(trkSolution.dopp.cmd==chan);']) -eval(['Dopp_cmd_CH' num2str(chan) '=trkSolution.dopp.cmd(indCH' num2str(chan) ',2);']) -eval(['subplot(2,3,' num2str(chan+1) ');plot(Dopp_cmd_CH' num2str(chan) ')']) -end - -%% -% for chan=0:4 -% load(['tracking\tracking_raw' num2str(chan) '.mat']); -% tracking_channel(chan+1).PRN=PRN; -% tracking_channel(chan+1).CN0_SNV_dB_Hz=CN0_SNV_dB_Hz; -% tracking_channel(chan+1).carrier_doppler_hz=carrier_doppler_hz; -% tracking_channel(chan+1).carrier_doppler_rate_hz=carrier_doppler_rate_hz; -% tracking_channel(chan+1).code_error_chips=code_error_chips; -% tracking_channel(chan+1).code_freq_chips=code_freq_chips; -% tracking_channel(chan+1).code_freq_rate_chips=code_freq_rate_chips; -% tracking_channel(chan+1).acc_carrier_phase_rad=acc_carrier_phase_rad; -% -% clearvars -except tracking_channel chan -% end -% %% -% figure -% for chan=1:5 -% subplot(2,3,chan); -% plot(tracking_channel(chan).carrier_doppler_hz); -% grid minor -% end -% sgtitle('carrier doppler hz channel') -% %% -% figure -% for chan=1:5 -% subplot(2,3,chan); -% plot(tracking_channel(chan).CN0_SNV_dB_Hz); -% grid minor -% end -% sgtitle('CN0 SNV dB Hz channel') -% -% %% -% figure -% for chan=1:5 -% subplot(2,3,chan); -% plot(tracking_channel(chan).acc_carrier_phase_rad); -% grid minor -% end -% sgtitle('acc carrier phase rad') \ No newline at end of file