def plotResults(directory):
np.set_printoptions(linewidth=150)
plt.rc('text', usetex=True)
plt.rc('font', family='serif')
global xtitles, vtitles, etitles, imu_titles, colors, data, pw, plotCov
xtitles = ['$p_x$', '$p_y$', '$p_z$', '$q_w$', '$q_x$', '$q_y$', '$q_z$']
vtitles = ['$v_x$', '$v_y$', '$v_z$']
etitles = ['$\phi$', r'$\theta$', '$\psi$']
imu_titles = [r"$acc_x$", r"$acc_y$", r"$acc_z$", r"$\omega_x$", r"$\omega_y$", r"$\omega_z$"]
colors = ['#1f77b4', '#ff7f0e', '#2ca02c', '#d62728', '#9467bd', '#8c564b', '#e377c2', '#7f7f7f', '#bcbd22', '#17becf']
plotCov = True
data = Log(directory)
pw = plotWindow()
plotPosition()
plotVelocity()
plotAttitude()
plotEuler()
plotLla()
plotIMU()
plotImuBias()
plotZVRes()
plotGnssRes()
pw.show()
def on_pushed_inspect(self):
'''When the inspect Button is clicked this function cuts the data accoring to the current
temporal and spatial borders. Then the data is prepared for plotting depending on the chosen
format finally plotWindow is called for display.'''
if self.DataLoaded == False:
messages.send_warning('No File loaded')
return
df = basic_stats.cut_timelines(self.TMP_FILE, self.INFO,
self.CSV_INFO_FILE)
representation = str(self.selectType.currentText())
datatype = str(self.selectSpec.currentText())
data2plot = genPlotData.makeData2Plot(df, representation, datatype,
self.INFO, self.PARAM_INFO_FILE)
if data2plot == None:
messages.send_warning('selection invalid')
return
else:
self.pW = plotWindow(data2plot, representation, datatype)
self.pW.exec_()
import matplotlib.pyplot as plt
import numpy as np
from plotWindow import plotWindow
np.set_printoptions(linewidth=150)
plt.rc('text', usetex=True)
plt.rc('font', family='serif')
pw = plotWindow()
LOG_WIDTH = 1 + 10 + 10 + 9 + 10 + 6
data = np.reshape(
np.fromfile('/tmp/ImuFactor.CheckDynamics.log', dtype=np.float64),
(-1, LOG_WIDTH))
t = data[:, 0]
y = data[:, 1:11]
yhat = data[:, 11:21]
dy = data[:, 21:30]
ycheck = data[:, 30:40]
u = data[:, 40:46]
error = ycheck - yhat
f = plt.figure()
plt.suptitle('Position')
for i in range(3):
plt.subplot(3, 1, i + 1)
plt.plot(t, y[:, i], label='y', linewidth=2)
plt.plot(t, yhat[:, i], label=r'$\hat{y}$')
def plotRawGPS(data, title="raw_gps"):
np.set_printoptions(linewidth=150)
plt.rc('text', usetex=True)
plt.rc('font', family='serif')
t = data[:, 0]
xhat0 = data[:, 1:8]
vhat0 = data[:, 8:11]
xhatf = data[:, 11:18]
vhatf = data[:, 18:21]
x = data[:, 21:28]
v = data[:, 28:31]
dthat0 = data[:, 31:33]
dthatf = data[:, 33:35]
dt = data[:, 35:37]
s = data[:, 37:52]
shatf = data[:, 52:67]
shat0 = data[:, 67:82]
xtitles = ['x', 'y', 'z', 'qw', 'qx', 'qy', 'qz']
vtitles = ['vx', 'vy', 'vz']
dttitles = [r'$\tau$', r'$\dot{\tau}$']
pw = plotWindow(title=title)
f = plt.figure(dpi=150)
plt.plot(-xhat0[:, 1], xhat0[:, 0], label=r"$\hat{x}_0$")
plt.plot(-xhatf[:, 1],
xhatf[:, 0],
'--',
linewidth=3,
label=r"$\hat{x}_f$")
plt.plot(-x[:, 1], x[:, 0], label="$x$")
plt.legend()
pw.addPlot("Position2D", f)
f = plt.figure(dpi=150)
for i in range(3):
plt.subplot(3, 1, i + 1)
plt.title(xtitles[i])
plt.plot(t, xhat0[:, i], label=r"$\hat{x}_0$")
plt.plot(t, xhatf[:, i], '--', linewidth=3, label=r"$\hat{x}_f$")
plt.plot(t, x[:, i], label="$x$")
plt.legend()
pw.addPlot("Position", f)
f = plt.figure(dpi=150)
for i in range(4):
plt.subplot(4, 1, i + 1)
plt.title(xtitles[i + 3])
plt.plot(t, xhat0[:, i + 3], label=r"$\hat{x}_0$")
plt.plot(t, xhatf[:, i + 3], '--', linewidth=3, label=r"$\hat{x}_f$")
plt.plot(t, x[:, i + 3], label="$x$")
plt.legend()
pw.addPlot("Attitude", f)
f = plt.figure(dpi=150)
for i in range(3):
plt.subplot(3, 1, i + 1)
plt.title(vtitles[i])
plt.plot(t, vhat0[:, i], label=r"$\hat{x}_0$")
plt.plot(t, vhatf[:, i], '--', linewidth=3, label=r"$\hat{x}_f$")
plt.plot(t, v[:, i], label="$x$")
plt.legend()
pw.addPlot("Velocity", f)
f = plt.figure()
for i in range(2):
plt.subplot(2, 1, i + 1)
plt.title(dttitles[i])
plt.plot(t, dthat0[:, i], label=r"$\hat{x}_0$")
plt.plot(t, dthatf[:, i], '--', linewidth=3, label=r"$\hat{x}_f$")
plt.plot(t, dt[:, i], label="$x$")
plt.legend()
pw.addPlot("Clock Bias", f)
f = plt.figure()
plt.suptitle("Switch Factor")
for i in range(15):
plt.subplot(15, 1, i + 1)
plt.plot(t, shat0[:, i], "-r", label=r"$\hat{x}_0$")
plt.plot(t, shatf[:, i], "-b", label=r"$\hat{x}_f$")
plt.plot(t, s[:, i], "-g", label=r"x")
plt.ylim(-0.1, 1.1)
if i == 0:
plt.legend(ncol=3)
pw.addPlot("Switch Factor", f)
pw.show()