diff --git a/src/utils/matlab/vtl/kf_prototype.m b/src/utils/matlab/vtl/kf_prototype.m index f6f52a4da..78bed097d 100644 --- a/src/utils/matlab/vtl/kf_prototype.m +++ b/src/utils/matlab/vtl/kf_prototype.m @@ -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 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. + 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. %% % % 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(5,t) = navSolution.vY(t); kf_x(6,t) = navSolution.vZ(t); - kf_x(7,t) = clk_bias_s(t); - kf_x(8,t) = clk_drift(t);%new_data.rx_dts(1); + 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); @@ -84,9 +84,9 @@ for t=1:length(navSolution.RX_time) 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; - kf_P_x = eye(8); %TODO: use a real value. + cdeltat_u(t)=kf_x(7,t); + cdeltatDot_u=kf_x(8,t); + kf_P_x = eye(8)*100; %TODO: use a real value. else 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); 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;% + cdeltat_u(t)=kf_x(7,t);% + cdeltatDot_u=kf_x(8,t);% 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 - %from error state variables to variables - % From state variables definition - % TODO: cast to type properly for chan=1:5 %neccesary quantities 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) 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+sat_number,t)=(sat_dopp_hz(chan,t)*Lambda_GPS_L1+cdeltatDot_u)-rhoDot_pri(chan,t); 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_K = (kf_P_x * kf_H') * inv(kf_S); % Kalman gain 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 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) - % kf_x(6) =kf_x(6) /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(8,t)=corr_kf_state(8,t)/SPEED_OF_LIGHT_M_S;% forced !!!!! + % kf_x(6) =kf_x(6); + % kf_x(7) =kf_x(7); end \ No newline at end of file diff --git a/src/utils/matlab/vtl/vtl_prototype.m b/src/utils/matlab/vtl/vtl_prototype.m index 8a07f510c..8bd495974 100644 --- a/src/utils/matlab/vtl/vtl_prototype.m +++ b/src/utils/matlab/vtl/vtl_prototype.m @@ -23,7 +23,7 @@ clearvars % end SPEED_OF_LIGHT_M_S=299792458.0; Lambda_GPS_L1=0.1902937; -point_of_closure=5; +point_of_closure=6000; %% samplingFreq=5000000; channels=6; @@ -103,13 +103,13 @@ 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 +% 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];