From 74a838339305c534d307face37d8115e34a12793 Mon Sep 17 00:00:00 2001 From: miguekf Date: Sat, 10 Dec 2022 22:01:29 +0100 Subject: [PATCH 1/2] MOD: matlab utils --- src/utils/matlab/vtl/kf_prototype.m | 166 ++++++--- .../vtl/pvt_raw_plotting_SPIRENT_GnssSDR.m | 321 +++++++++--------- src/utils/matlab/vtl/vtl_general_plot.m | 160 ++++----- src/utils/matlab/vtl/vtl_prototype.m | 128 ++++--- 4 files changed, 416 insertions(+), 359 deletions(-) diff --git a/src/utils/matlab/vtl/kf_prototype.m b/src/utils/matlab/vtl/kf_prototype.m index 78bed097d..806c2139b 100644 --- a/src/utils/matlab/vtl/kf_prototype.m +++ b/src/utils/matlab/vtl/kf_prototype.m @@ -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 Date: Sat, 10 Dec 2022 22:01:46 +0100 Subject: [PATCH 2/2] ADD: python utils --- src/utils/python/analysis.py | 67 ++++++++++++++++++++++ src/utils/python/vtl_analysis.py | 96 ++++++++++++++++++++++++++++++++ 2 files changed, 163 insertions(+) create mode 100644 src/utils/python/analysis.py create mode 100644 src/utils/python/vtl_analysis.py diff --git a/src/utils/python/analysis.py b/src/utils/python/analysis.py new file mode 100644 index 000000000..c1252451b --- /dev/null +++ b/src/utils/python/analysis.py @@ -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() + diff --git a/src/utils/python/vtl_analysis.py b/src/utils/python/vtl_analysis.py new file mode 100644 index 000000000..b3ed141f4 --- /dev/null +++ b/src/utils/python/vtl_analysis.py @@ -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() \ No newline at end of file