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

fix: vtl kf in matlab version working

Browse Source
This commit is contained in:
miguekf 2022-12-08 20:50:56 +01:00
parent 9f8284a909
commit f7aac29e1d
2 changed files with 22 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -35,8 +35,8 @@ kf_Q = [ 1 0 0 0 0 0 0 0
0 0 0 1 0 0 0 0 0 0 0 1 0 0 0 0
0 0 0 0 1 0 0 0 0 0 0 0 1 0 0 0
0 0 0 0 0 1 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 1e-9 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. 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.
%% %%
% % Measurement error Covariance Matrix R assembling % % Measurement error Covariance Matrix R assembling
@ -75,8 +75,8 @@ for t=1:length(navSolution.RX_time)
kf_x(4,t) = navSolution.vX(t); kf_x(4,t) = navSolution.vX(t);
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)*SPEED_OF_LIGHT_M_S;
kf_x(8,t) = clk_drift(t);%new_data.rx_dts(1); kf_x(8,t) = clk_drift(t)*SPEED_OF_LIGHT_M_S;%new_data.rx_dts(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);
@ -84,9 +84,9 @@ 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(t)=kf_x(7,t)*SPEED_OF_LIGHT_M_S; cdeltat_u(t)=kf_x(7,t);
cdeltatDot_u=kf_x(8,t)*SPEED_OF_LIGHT_M_S; cdeltatDot_u=kf_x(8,t);
kf_P_x = eye(8); %TODO: use a real value. kf_P_x = eye(8)*100; %TODO: use a real value.
else else
kf_x(:,t)=corr_kf_state(:,t-1); kf_x(:,t)=corr_kf_state(:,t-1);
@ -96,15 +96,12 @@ 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(t)=kf_x(7,t)*SPEED_OF_LIGHT_M_S;% cdeltat_u(t)=kf_x(7,t);%
cdeltatDot_u=kf_x(8,t)*SPEED_OF_LIGHT_M_S;% cdeltatDot_u=kf_x(8,t);%
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 state variables definition
% TODO: cast to type properly
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;
@ -133,8 +130,6 @@ for t=1:length(navSolution.RX_time)
% Kalman estimation (measurement update) % Kalman estimation (measurement update)
for chan=1:5 % Measurement matrix H assembling for chan=1:5 % Measurement matrix H assembling
%kf_y(i) = new_data.pr_m(i); % 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+cdeltatDot_u)-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
@ -154,15 +149,12 @@ for t=1:length(navSolution.RX_time)
kf_S = kf_H * kf_P_x* kf_H' + kf_R; % innovation covariance matrix (S) 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_K = (kf_P_x * kf_H') * inv(kf_S); % Kalman gain
kf_xerr(:,t) = kf_K * (kf_yerr(:,t)); % Error state estimation kf_xerr(:,t) = kf_K * (kf_yerr(:,t)); % Error state estimation
%kf_x = kf_x(:,t) - kf_xerr(:,t); % updated state estimation (a priori + error)
kf_P_x = (eye(length(kf_P_x)) - kf_K * kf_H) * kf_P_x; % update state estimation error covariance matrix 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 corr_kf_state(:,t)=kf_x(:,t)-kf_xerr(:,t); %updated state estimation
% TODO: compare how KF state diverges from RTKLIB solution!
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) % States related tu USER clock adjust from m/s to s (by /SPEED_OF_LIGHT_M_S)
% kf_x(6) =kf_x(6) /SPEED_OF_LIGHT_M_S; % kf_x(6) =kf_x(6);
% kf_x(7) =kf_x(7) /SPEED_OF_LIGHT_M_S; % kf_x(7) =kf_x(7);
corr_kf_state(7,t)=corr_kf_state(7,t)/SPEED_OF_LIGHT_M_S;
corr_kf_state(8,t)=corr_kf_state(8,t)/SPEED_OF_LIGHT_M_S;% forced !!!!!
end end

16
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=5; point_of_closure=6000;
%% %%
samplingFreq=5000000; samplingFreq=5000000;
channels=6; channels=6;
@ -103,13 +103,13 @@ 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 % close all
figure;plot(kf_xerr(7,:),'.');title('clk bias err') % figure;plot(kf_xerr(7,:),'.');title('clk bias err')
figure;plot(kf_xerr(8,:),'.');title('clk drift err') % figure;plot(kf_xerr(8,:),'.');title('clk drift err')
figure;plot(kf_x(7,:),'.');title('clk bias state') % figure;plot(kf_x(7,:),'.');title('clk bias state')
figure;plot(kf_x(8,:),'.');title('clk drift 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(7,:),'.');title('clk bias corrected state')
figure;plot(corr_kf_state(8,:),'.');title('clk drift corrected state') % figure;plot(corr_kf_state(8,:),'.');title('clk drift corrected state')
%% %%
% close all % close all
% kferr_pos_all=[vtlSolution.kferr.X vtlSolution.kferr.Y vtlSolution.kferr.Z]; % kferr_pos_all=[vtlSolution.kferr.X vtlSolution.kferr.Y vtlSolution.kferr.Z];