From 09841f2bacb1a9802f90a0305b0bfdf45d208563 Mon Sep 17 00:00:00 2001
From: miguekf <angel.glopez@alumnos.upm.es>
Date: Sat, 10 Dec 2022 22:01:46 +0100
Subject: [PATCH] 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