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

MOD: vtl matlab kf changes

Browse Source
This commit is contained in:
miguekf 2022-12-08 19:57:02 +01:00
parent 4cc75d419f
commit 033a87be4b
2 changed files with 86 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -3,16 +3,25 @@
sat_number=5; sat_number=5;
%% ################## Kalman filter initialization ###################################### %% ################## Kalman filter initialization ######################################
% covariances (static) % covariances (static)
kf_P_x = eye(8)*10.0; %TODO: use a real value. kf_P_x = eye(8); %TODO: use a real value.
kf_x = zeros(8, 1); kf_x = zeros(8, 1);
kf_R = zeros(2*sat_number, 2*sat_number); kf_R = zeros(2*sat_number, 2*sat_number);
kf_dt=0.1; kf_dt=0.1;
kf_F = eye(8, 8); % kf_F = eye(8, 8);
kf_F(1, 4) = kf_dt; % kf_F(1, 4) = kf_dt;
kf_F(2, 5) = kf_dt; % kf_F(2, 5) = kf_dt;
kf_F(3, 6) = kf_dt; % kf_F(3, 6) = kf_dt;
kf_F(7, 8) = kf_dt; % kf_F(7, 8) = kf_dt;
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,8); kf_H = zeros(2*sat_number,8);
kf_y = zeros(2*sat_number, 1); kf_y = zeros(2*sat_number, 1);
kf_yerr = zeros(2*sat_number, 1); kf_yerr = zeros(2*sat_number, 1);
@ -20,18 +29,41 @@ kf_yerr = zeros(2*sat_number, 1);
kf_S = zeros(2*sat_number, 2*sat_number); % kf_P_y innovation covariance matrix kf_S = zeros(2*sat_number, 2*sat_number); % kf_P_y innovation covariance matrix
%% State error Covariance Matrix Q (PVT) %% 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. 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-7 0
0 0 0 0 0 0 0 1e-4];%new_data.rx_pvt_var(i); %careful, values for V and T could not be adecuate.
%% %%
% Measurement error Covariance Matrix R assembling % % Measurement error Covariance Matrix R assembling
for chan=1:5 %neccesary quantities % for chan=1:5 %neccesary quantities
for t=1:length(navSolution.RX_time) % % It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error)
% 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, chan) = 40.0; %TODO: fill with real values. % kf_R(chan+sat_number, chan+sat_number) = 10.0;
kf_R(chan+sat_number, chan+sat_number) = 10.0; % end
end
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];
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));
%% ################## Kalman Tracking ###################################### %% ################## Kalman Tracking ######################################
% receiver solution from rtklib_solver % receiver solution from rtklib_solver
for t=1:length(navSolution.RX_time) for t=1:length(navSolution.RX_time)
@ -44,17 +76,7 @@ for t=1:length(navSolution.RX_time)
kf_x(5,t) = navSolution.vY(t); kf_x(5,t) = navSolution.vY(t);
kf_x(6,t) = navSolution.vZ(t); kf_x(6,t) = navSolution.vZ(t);
kf_x(7,t) = clk_bias_s(t); kf_x(7,t) = clk_bias_s(t);
kf_x(8,t) = 1e-6;%new_data.rx_dts(1); kf_x(8,t) = clk_drift(t);%new_data.rx_dts(1);
x_u=kf_x(1,t);
y_u=kf_x(2,t);
z_u=kf_x(3,t);
xDot_u=kf_x(4,t);
yDot_u=kf_x(5,t);
zDot_u=kf_x(6,t);
cdeltat_u=kf_x(7,t)*SPEED_OF_LIGHT_M_S;
cdeltatDot_u=1e-8*SPEED_OF_LIGHT_M_S;
else
kf_x(:,t)=corr_kf_state(:,t-1);
x_u=kf_x(1,t); x_u=kf_x(1,t);
y_u=kf_x(2,t); y_u=kf_x(2,t);
@ -62,23 +84,27 @@ for t=1:length(navSolution.RX_time)
xDot_u=kf_x(4,t); xDot_u=kf_x(4,t);
yDot_u=kf_x(5,t); yDot_u=kf_x(5,t);
zDot_u=kf_x(6,t); zDot_u=kf_x(6,t);
cdeltat_u=kf_x(7,t)*SPEED_OF_LIGHT_M_S; cdeltat_u(t)=kf_x(7,t)*SPEED_OF_LIGHT_M_S;
cdeltatDot_u=1e-8*SPEED_OF_LIGHT_M_S; cdeltatDot_u=kf_x(8,t)*SPEED_OF_LIGHT_M_S;
kf_P_x = eye(8); %TODO: use a real value.
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)*SPEED_OF_LIGHT_M_S;%
cdeltatDot_u=kf_x(8,t)*SPEED_OF_LIGHT_M_S;%
end end
% Kalman state prediction (time update) % Kalman state prediction (time update)
kf_x(:,t) = kf_F * kf_x(:,t); % state prediction 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 kf_P_x= kf_F * kf_P_x * kf_F' + kf_Q; % state error covariance prediction
%from error state variables to variables %from error state variables to variables
% From state variables definition % From state variables definition
% TODO: cast to type properly % TODO: cast to type properly
d = zeros(sat_number, 1);
rho_pri = zeros(sat_number, 1);
rhoDot_pri = zeros(sat_number, 1);
a_x = zeros(sat_number, 1);
a_y = zeros(sat_number, 1);
a_z = zeros(sat_number, 1);
c_pr_m=zeros(sat_number,length(navSolution.RX_time));
for chan=1:5 %neccesary quantities for chan=1:5 %neccesary quantities
d(chan)=(sat_posX_m(chan,t)-kf_x(1,t))^2; d(chan)=(sat_posX_m(chan,t)-kf_x(1,t))^2;
@ -86,7 +112,7 @@ for t=1:length(navSolution.RX_time)
d(chan)=d(chan)+(sat_posZ_m(chan,t)-kf_x(3,t))^2; d(chan)=d(chan)+(sat_posZ_m(chan,t)-kf_x(3,t))^2;
d(chan)=sqrt(d(chan)); d(chan)=sqrt(d(chan));
c_pr_m(chan,t)=d(chan)+clk_bias_s(t)*SPEED_OF_LIGHT_M_S; 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_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_y(chan,t)=-(sat_posY_m(chan,t)-kf_x(2,t))/d(chan);
@ -94,7 +120,7 @@ for t=1:length(navSolution.RX_time)
rhoDot_pri(chan,t)=(sat_velX(chan,t)-kf_x(4,t))*a_x(chan,t)... 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_velY(chan,t)-kf_x(5,t))*a_y(chan,t)...
+(sat_velZ(chan,t)-kf_x(6,t))*a_z(chan,t); +(sat_velZ(chan,t)-kf_x(6,t))*a_z(chan,t)+cdeltatDot_u;
end end
@ -110,7 +136,7 @@ for t=1:length(navSolution.RX_time)
%kf_y(i) = new_data.pr_m(i); % i-Satellite %kf_y(i) = new_data.pr_m(i); % i-Satellite
%kf_y(i+sat_number) = rhoDot_pri(i)/Lambda_GPS_L1; % i-Satellite %kf_y(i+sat_number) = rhoDot_pri(i)/Lambda_GPS_L1; % i-Satellite
kf_yerr(chan,t)=c_pr_m(chan,t)-sat_prg_m(chan,t);%-0.000157*SPEED_OF_LIGHT_M_S; kf_yerr(chan,t)=c_pr_m(chan,t)-sat_prg_m(chan,t);%-0.000157*SPEED_OF_LIGHT_M_S;
kf_yerr(chan+sat_number,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1)-rhoDot_pri(chan,t); kf_yerr(chan+sat_number,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1+cdeltatDot_u)-rhoDot_pri(chan,t);
end end
% DOUBLES DIFFERENCES % DOUBLES DIFFERENCES
@ -138,5 +164,5 @@ for t=1:length(navSolution.RX_time)
% kf_x(6) =kf_x(6) /SPEED_OF_LIGHT_M_S; % kf_x(6) =kf_x(6) /SPEED_OF_LIGHT_M_S;
% kf_x(7) =kf_x(7) /SPEED_OF_LIGHT_M_S; % kf_x(7) =kf_x(7) /SPEED_OF_LIGHT_M_S;
corr_kf_state(7,t)=corr_kf_state(7,t)/SPEED_OF_LIGHT_M_S; corr_kf_state(7,t)=corr_kf_state(7,t)/SPEED_OF_LIGHT_M_S;
corr_kf_state(8,t)=1e-8/SPEED_OF_LIGHT_M_S; corr_kf_state(8,t)=corr_kf_state(8,t)/SPEED_OF_LIGHT_M_S;% forced !!!!!
end end

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

@ -23,7 +23,7 @@ clearvars
% end % end
SPEED_OF_LIGHT_M_S=299792458.0; SPEED_OF_LIGHT_M_S=299792458.0;
Lambda_GPS_L1=0.1902937; Lambda_GPS_L1=0.1902937;
point_of_closure=6000; point_of_closure=5;
%% %%
samplingFreq=5000000; samplingFreq=5000000;
channels=6; channels=6;
@ -40,9 +40,9 @@ load observables\observable_raw.mat
% refSatData = SpirentSatData2struct('..\log_spirent\sat_data_V1A1_SPF_LD_05.csv'); % refSatData = SpirentSatData2struct('..\log_spirent\sat_data_V1A1_SPF_LD_05.csv');
rx_PRN=[28 4 17 15 27 9]; % for SPF_LD_05. 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'... load('PVT_raw.mat','sat_posX_m','sat_posY_m','sat_posZ_m','sat_velX','sat_velY'...
,'sat_velZ','sat_prg_m','clk_bias_s','sat_dopp_hz') ,'sat_velZ','sat_prg_m','clk_bias_s','clk_drift','sat_dopp_hz','user_clk_offset')
%%
vtlSolution = Vtl2struct('dump_vtl_file.csv'); vtlSolution = Vtl2struct('dump_vtl_file.csv');
%% calculate LOS Rx-sat %% calculate LOS Rx-sat
% for chan=1:5 % for chan=1:5
@ -102,6 +102,22 @@ legend('PRN 28','PRN 4','PRN 17','PRN 15','PRN 27','Location','eastoutside')
%% %%
%general_raw_plot %general_raw_plot
vtl_general_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 % time_reference_spirent_obs=129780;%s
% if(load_observables) % if(load_observables)