#
# ISC License
#
# Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder
#
# Permission to use, copy, modify, and/or distribute this software for any
# purpose with or without fee is hereby granted, provided that the above
# copyright notice and this permission notice appear in all copies.
#
# THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
# WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
# MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
# ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
# WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
# ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
# OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
#
r"""
Overview
--------
``OpNavScenarios/plotting/OpNav_Plotting.py`` contains the plotting routines. None of the files are saved,
but are shown when the scenario is run with python. Saving is left to the user's discretion.
"""
import matplotlib as mpl
import matplotlib.pyplot as plt
import numpy as np
# import scipy.optimize
from Basilisk.utilities import macros as mc
from Basilisk.utilities import unitTestSupport
from matplotlib.patches import Ellipse
color_x = 'dodgerblue'
color_y = 'salmon'
color_z = 'lightgreen'
m2km = 1.0 / 1000.0
ns2min = 1/60.*1E-9
mpl.rcParams.update({'font.size' : 8 })
# If a specific style for plotting wants to be used
try:
plt.style.use("myStyle")
params = {'axes.labelsize': 8, 'axes.titlesize': 8, 'legend.fontsize': 8, 'xtick.labelsize': 7,
'ytick.labelsize': 7, 'text.usetex': True}
mpl.rcParams.update(params)
except:
pass
# def fit_sin(tt, yy):
# """
# Fit sin to the input time sequence, and return fitting parameters "amp", "omega", "phase", "offset", "freq", "period" and "fitfunc"
# """
# tt = np.array(tt)
# yy = np.array(yy)
# ff = np.fft.fftfreq(len(tt), (tt[1]-tt[0])) # assume uniform spacing
# Fyy = abs(np.fft.fft(yy))
# guess_freq = abs(ff[np.argmax(Fyy[1:])+1]) # excluding the zero frequency "peak", which is related to offset
# guess_amp = np.std(yy) * 2.**0.5
# guess_offset = np.mean(yy)
# guess = np.array([guess_amp, 2.*np.pi*guess_freq, 0., guess_offset])
#
# def sinfunc(t, A, w, p, c): return A * np.sin(w*t + p) + c
# popt, pcov = scipy.optimize.curve_fit(sinfunc, tt, yy, p0=guess)
# A, w, p, c = popt
# f = w/(2.*np.pi)
# fitfunc = lambda t: A * np.sin(w*t + p) + c
# return {"amp": A, "omega": w, "phase": p, "offset": c, "freq": f, "period": 1./f, "fitfunc": fitfunc, "maxcov": np.max(pcov), "rawres": (guess,popt,pcov)}
def show_all_plots():
plt.show()
def clear_all_plots():
plt.close("all")
def omegaTrack(rError, covar):
colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10.)
colorList = []
for i in range(10):
colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)])
t = rError[:, 0] * ns2min
plt.figure(num=2210101, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.plot(t , rError[:, 1]*180./np.pi, color = colorList[2] , label= 'Error')
plt.plot(t, 3*np.sqrt(covar[:, 0,0])*180./np.pi, color=colorList[8], linestyle = '--', label=r'Covar (3-$\sigma$)')
plt.plot(t, -3*np.sqrt(covar[:, 0,0])*180./np.pi, color=colorList[8], linestyle = '--')
plt.legend(loc='best')
plt.ylabel(r'$\mathbf{\omega}_{' + str(2) +r'}$ Error in $\mathcal{C}$ ($^\circ$/s)')
plt.ylim([-0.07,0.07])
plt.xlabel('Time (min)')
# plt.savefig('RateCam1.pdf')
plt.figure(num=2210201, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.plot(t , rError[:, 2]*180./np.pi, color = colorList[2])
plt.plot(t, 3*np.sqrt(covar[:, 1,1])*180./np.pi, color=colorList[8], linestyle = '--')
plt.plot(t, -3*np.sqrt(covar[:, 1,1])*180./np.pi, color=colorList[8], linestyle = '--')
plt.ylabel(r'$\mathbf{\omega}_{' + str(3) +r'}$ Error in $\mathcal{C}$ ($^\circ$/s)')
plt.ylim([-0.07,0.07])
plt.xlabel('Time (min)')
# plt.savefig('RateCam2.pdf')
def vecTrack(ref, track, covar):
colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10.)
colorList = []
for i in range(10):
colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)])
rError = np.copy(ref)
rError[:,1:] -= track[:,1:]
errorDeg = np.zeros([len(rError[:, 0]), 2])
covDeg = np.zeros([len(rError[:, 0]), 2])
for i in range(len(errorDeg[:, 0])):
errorDeg[i, 0] = rError[i, 0]
covDeg[i, 0] = rError[i, 0]
errorDeg[i, 1] = np.arccos(np.dot(ref[i, 1:4], track[i, 1:4]))*180./np.pi
covDeg[i, 1] = np.arccos(np.dot(ref[i, 1:4], track[i, 1:4]))
covarVec = np.array(
[track[i, 1] + np.sqrt(covar[i, 0,0]), track[i, 2] + np.sqrt(covar[i, 1,1]),
track[i, 3] + np.sqrt(covar[i, 2,2])])
covarVec = covarVec / np.linalg.norm(covarVec)
covDeg[i, 1] = 3 * np.arccos(np.dot(covarVec, track[i, 1:4]))*180./np.pi
t = ref[:, 0] * ns2min
plt.figure(num=101011, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.plot(t , errorDeg[:, 1], color = colorList[1] , label= "Off-point")
plt.plot(t, covDeg[:,1], color=colorList[5], linestyle = '--', label=r'Covar (3-$\sigma$)')
plt.legend(loc='upper right')
plt.ylabel(r'Mean $\hat{\mathbf{h}}$ Error in $\mathcal{C}$ ($^\circ$)')
plt.xlabel('Time (min)')
plt.ylim([0,2])
# plt.savefig('HeadingDeg.pdf')
plt.figure(num=10101, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.plot(t , rError[:, 1], color = colorList[2] , label= 'Error')
plt.plot(t, 3*np.sqrt(covar[:, 0,0]), color=colorList[8], linestyle = '--', label=r'Covar (3-$\sigma$)')
plt.plot(t, -3*np.sqrt(covar[:, 0,0]), color=colorList[8], linestyle = '--')
plt.legend()
plt.ylabel(r'$\hat{\mathbf{h}}_{' + str(1) +r'}$ Error in $\mathcal{C}$ (-)')
plt.ylim([-0.04,0.04])
plt.xlabel('Time (min)')
# plt.savefig('HeadingCam1.pdf')
plt.figure(num=10201, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.plot(t , rError[:, 2], color = colorList[2] )
plt.plot(t, 3*np.sqrt(covar[:, 1,1]), color=colorList[8], linestyle = '--')
plt.plot(t, -3*np.sqrt(covar[:, 1,1]), color=colorList[8], linestyle = '--')
plt.ylabel(r'$\hat{\mathbf{h}}_{' + str(2) +r'}$ Error in $\mathcal{C}$ (-)')
plt.ylim([-0.04,0.04])
plt.xlabel('Time (min)')
# plt.savefig('HeadingCam2.pdf')
plt.figure(num=10301, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.plot(t , rError[:, 3], color = colorList[2])
plt.plot(t, 3*np.sqrt(covar[:, 2,2]), color=colorList[8], linestyle = '--')
plt.plot(t, -3*np.sqrt(covar[:, 2,2]), color=colorList[8], linestyle = '--')
plt.ylabel(r'$\hat{\mathbf{h}}_{' + str(3) +r'}$ Error in $\mathcal{C}$ (-)')
plt.ylim([-0.04,0.04])
plt.xlabel('Time (min)')
# plt.savefig('HeadingCam3.pdf')
def plot_faults(dataFaults, valid1, valid2):
colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10.)
colorList = []
for i in range(10):
colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)])
# for i in range(len(dataFaults[:,0])):
# if (dataFaults[i,0]*ns2min%1 != 0):
# valid1[i, 1] = np.nan
# valid2[i, 1] = np.nan
plt.figure(10101, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.xlabel('Time (min)')
plt.ylabel('Detected Fault')
plt.scatter(valid1[:,0]* ns2min, valid1[:,1], color = colorList[5], alpha = 0.1, label = "Limb")
plt.scatter(valid2[:,0]* ns2min, valid2[:,1], color = colorList[8], alpha = 0.1, label = "Circ")
plt.scatter(dataFaults[:,0]* ns2min, dataFaults[:,1], marker = '.', color = colorList[2], label = "Faults")
plt.legend()
# plt.savefig('FaultsDetected.pdf')
return
def diff_methods(vec1, meth1, meth2, val1, val2):
colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10.)
colorList = []
for i in range(10):
colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)])
validIdx1 = []
for i in range(len(val1[:,0])):
if np.abs(val1[i,1] - 1) < 0.01:
validIdx1.append(i)
validIdx2 = []
for i in range(len(val2[:,0])):
if np.abs(val2[i,1] - 1) < 0.01:
validIdx2.append(i)
diff1 = np.full([len(validIdx1), 4], np.nan)
diffNorms1 = np.full([len(validIdx1), 2], np.nan)
diff2 = np.full([len(validIdx2), 4], np.nan)
diffNorms2 = np.full([len(validIdx2), 2], np.nan)
for i in range(len(validIdx1)):
diff1[i,0] = vec1[validIdx1[i],0]
diff1[i,1:] = vec1[validIdx1[i],1:] - meth1[validIdx1[i],1:]
diffNorms1[i,0] = vec1[validIdx1[i],0]
diffNorms1[i,1] = np.linalg.norm(vec1[validIdx1[i],1:]) - np.linalg.norm(meth1[validIdx1[i],1:])
for i in range(len(validIdx2)):
diff2[i,0] = vec1[validIdx2[i],0]
diff2[i,1:] = vec1[validIdx2[i],1:] - meth2[validIdx2[i],1:]
diffNorms2[i,0] = vec1[validIdx2[i],0]
diffNorms2[i,1] = np.linalg.norm(vec1[validIdx2[i],1:]) - np.linalg.norm(meth2[validIdx2[i],1:])
plt.figure(1, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.xlabel('Time')
plt.plot(diff1[:, 0] * ns2min, diff1[:, 1] * m2km, color = colorList[1], label=r"$\mathbf{r}_\mathrm{Limb}$")
plt.plot(diff1[:, 0] * ns2min, diff1[:, 2] * m2km, color = colorList[5])
plt.plot(diff1[:, 0] * ns2min, diff1[:, 3] * m2km, color = colorList[8])
plt.plot(diff2[:, 0] * ns2min, diff2[:, 1] * m2km, color=colorList[1], label=r"$\mathbf{r}_\mathrm{Circ}$", linestyle ='--', linewidth=2)
plt.plot(diff2[:, 0] * ns2min, diff2[:, 2] * m2km, color=colorList[5], linestyle ='--', linewidth=2)
plt.plot(diff2[:, 0] * ns2min, diff2[:, 3] * m2km, color=colorList[8], linestyle ='--', linewidth=2)
plt.legend()
plt.ylabel(r"$\mathbf{r}_{\mathrm{true}} - \mathbf{r}_{\mathrm{opnav}}$ (km)")
plt.xlabel("Time (min)")
# plt.savefig('MeasErrorComponents.pdf')
plt.figure(2, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.xlabel('Time')
plt.plot(diff1[:, 0] * ns2min, diffNorms1[:,1] * m2km, color = colorList[1])
plt.plot(diff2[:, 0] * ns2min, diffNorms2[:,1] * m2km, color = colorList[1], linestyle="--", linewidth=2)
plt.ylabel(r"$|\mathbf{r}_{\mathrm{true}}|$ - $|\mathbf{r}_{\mathrm{opnav}}|$ (km)")
plt.xlabel("Time (min)")
# plt.savefig('MeasErrorNorm.pdf')
def diff_vectors(vec1, vec2, valid, string):
assert len(vec1[0,:]) == len(vec2[0,:]), print("Vectors need to be the same size")
colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10.)
colorList = []
for i in range(10):
colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)])
validIdx = []
for i in range(len(valid[:,0])):
if np.abs(valid[i,1] - 1) < 0.01:
validIdx.append(i)
diff = np.full([len(validIdx), 4], np.nan)
diffNorms = np.full([len(validIdx), 2], np.nan)
m2km2 = m2km*m2km
for i in range(len(validIdx)):
diff[i,0] = vec1[validIdx[i],0]
diff[i,1:] = vec1[validIdx[i],1:] - vec2[validIdx[i],1:]
diffNorms[i,0] = vec1[validIdx[i],0]
diffNorms[i,1] = np.linalg.norm(vec1[validIdx[i],1:]) - np.linalg.norm(vec2[validIdx[i],1:])
# plt.figure(1, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.figure(1, figsize=(3.5, 2.), facecolor='w', edgecolor='k')
plt.xlabel('Time')
plt.plot(diff[:, 0] * ns2min, diff[:, 1] * m2km2, color = colorList[1], label=r"$x_\mathrm{"+string+"}$")
plt.plot(diff[:, 0] * ns2min, np.mean(diff[:, 1]) * m2km2 * np.ones(len(diff[:, 0])), color = colorList[1], linestyle = '--')
plt.plot(diff[:, 0] * ns2min, diff[:, 2] * m2km2, color = colorList[5], label=r"$y_\mathrm{"+string+"}$")
plt.plot(diff[:, 0] * ns2min, np.mean(diff[:, 2]) * m2km2 * np.ones(len(diff[:, 0])), color = colorList[5], linestyle = '--')
plt.plot(diff[:, 0] * ns2min, diff[:, 3] * m2km2, color = colorList[8], label=r"$z_\mathrm{"+string+"}$")
plt.plot(diff[:, 0] * ns2min, np.mean(diff[:, 3]) * m2km2 * np.ones(len(diff[:, 0])), color = colorList[8], linestyle = '--')
plt.legend()
plt.ylabel(r"$\mathbf{r}_{\mathrm{true}} - \mathbf{r}_{\mathrm{opnav}}$ (km)")
plt.xlabel("Time (min)")
##plt.savefig('MeasErrorComponents.pdf')
# plt.figure(2, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.figure(2, figsize=(3.5, 2.), facecolor='w', edgecolor='k')
plt.xlabel('Time')
plt.plot(diff[:, 0] * ns2min, diffNorms[:,1] * m2km2, color = colorList[1])
plt.plot(diff[:, 0] * ns2min, np.mean(diffNorms[:,1]) * m2km2 * np.ones(len(diff[:, 0])), color = colorList[1], linestyle="--")
plt.ylabel(r"$|\mathbf{r}_{\mathrm{true}}|$ - $|\mathbf{r}_{\mathrm{opnav}}|$ (km)")
plt.xlabel("Time (min)")
#plt.savefig('MeasErrorNorm.pdf')
return
def nav_percentages(truth, states, covar, valid, string):
numStates = len(states[0:,1:])
colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10.)
colorList = []
for i in range(10):
colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)])
validIdx = []
for i in range(len(valid[:,0])):
if np.abs(valid[i,1] - 1) < 0.01:
validIdx.append(i)
diffPos = np.full([len(validIdx), 2], np.nan)
diffVel = np.full([len(validIdx), 2], np.nan)
covarPos = np.full([len(validIdx), 2], np.nan)
covarVel = np.full([len(validIdx), 2], np.nan)
m2km2 = m2km
for i in range(len(validIdx)):
diffPos[i,0] = states[validIdx[i],0]
diffPos[i,1] = np.linalg.norm(states[validIdx[i],1:4] - truth[validIdx[i],1:4])/np.linalg.norm(truth[validIdx[i],1:4])*100
diffVel[i,0] = states[validIdx[i],0]
diffVel[i,1] = np.linalg.norm(states[validIdx[i],4:7] - truth[validIdx[i],4:7])/np.linalg.norm(truth[validIdx[i],4:7])*100
covarPos[i,0] = states[validIdx[i],0]
posVec = np.sqrt(np.array([covar[validIdx[i],1], covar[validIdx[i],1 + 6+1], covar[validIdx[i],1 + 2*(6+1)]]))
covarPos[i,1] = 3*np.linalg.norm(posVec)/np.linalg.norm(truth[validIdx[i],1:4])*100
covarVel[i,0] = states[validIdx[i],0]
velVec = np.sqrt(np.array([covar[validIdx[i],1 + 3*(6+1)], covar[validIdx[i],1 + 4*(6+1)], covar[validIdx[i],1 + 5*(6+1)]]))
covarVel[i,1] = 3*np.linalg.norm(velVec)/np.linalg.norm(truth[validIdx[i],4:7])*100
plt.figure(101, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
# plt.figure(101, figsize=(3.5, 2), facecolor='w', edgecolor='k')
plt.plot(diffPos[:, 0] * ns2min, diffPos[:, 1] , color = colorList[1], label = "Error")
plt.plot(covarPos[:, 0] * ns2min, covarPos[:,1], color = colorList[8], linestyle = '--', label=r'Covar ($3\sigma$)')
plt.legend(loc='upper right')
plt.ylabel(r"$\mathbf{r}_\mathrm{"+string+r"}$ errors ($\%$)")
plt.xlabel("Time (min)")
# plt.ylim([0,3.5])
#plt.savefig('PercentErrorPos.pdf')
plt.figure(102, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
# plt.figure(102, figsize=(3.5, 2.), facecolor='w', edgecolor='k')
plt.plot(diffVel[:, 0] * ns2min, diffVel[:, 1], color = colorList[1])
plt.plot(covarVel[:, 0] * ns2min, covarVel[:,1], color = colorList[8], linestyle = '--')
plt.ylabel(r"$\dot{\mathbf{r}}_\mathrm{"+string+ r"}$ errors ($\%$)")
plt.xlabel("Time (min)")
# plt.ylim([0,15])
#plt.savefig('PercentErrorVel.pdf')
RMSPos = np.sqrt(sum(diffPos[:, 1] ** 2)/len(diffPos[:, 1]))
RMSPosCov = np.sqrt(sum((covarPos[:, 1]) ** 2)/len(covarPos[:, 1]))
RMSVel = np.sqrt(sum(diffVel[:, 1] ** 2)/len(diffVel[:, 1]))
RMSVelCov = np.sqrt(sum((covarVel[:, 1]) ** 2)/len(covarVel[:, 1]))
print('-------- Pos RMS '+ string + '--------')
print(RMSPos)
print(RMSPosCov)
print('------------------------')
print('-------- Vel RMS '+ string + '--------')
print(RMSVel)
print(RMSVelCov)
print('------------------------')
# RMS Errors Computed for heading and rate
# unitTestSupport.writeTeXSnippet('RMSPos_'+ string, str(round(RMSPos,3)), os.path.dirname(__file__))
# unitTestSupport.writeTeXSnippet('RMSPosCov_'+ string, str(round(RMSPosCov,3)),os.path.dirname(__file__))
# unitTestSupport.writeTeXSnippet('RMSVel_'+ string, str(round(RMSVel,3)), os.path.dirname(__file__))
# unitTestSupport.writeTeXSnippet('RMSVelCov_'+ string, str(round(RMSVelCov,3)),os.path.dirname(__file__))
return
def plot_orbit(r_BN):
plt.figure()
plt.xlabel('$R_x$, km')
plt.ylabel('$R_y$, km')
plt.plot(r_BN[:, 1] * m2km, r_BN[:, 2] * m2km, color_x)
plt.scatter(0, 0)
plt.title('Spacecraft Orbit')
return
def plot_rw_motor_torque(timeData, dataUsReq, dataRW, numRW):
colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10.)
colorList = []
for i in range(10):
colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)])
plt.figure(22, figsize=(3, 1.8), facecolor='w', edgecolor='k')
for idx in range(1, numRW):
plt.plot(timeData, dataUsReq[:, idx],
'--',
color=colorList[idx*2],
label=r'$\hat u_{s,' + str(idx) + '}$')
plt.plot(timeData, dataRW[idx - 1][:, 1],
color=colorList[idx*2],
label='$u_{s,' + str(idx) + '}$')
plt.legend(loc='lower right')
#plt.savefig('RWMotorTorque.pdf')
plt.xlabel('Time (min)')
plt.ylabel('RW Motor Torque (Nm)')
def plot_TwoOrbits(r_BN, r_BN2):
colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10.)
colorList = []
for i in range(10):
colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)])
# fig = plt.figure(5, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
fig = plt.figure(5, figsize=(3.5, 2.), facecolor='w', edgecolor='k')
ax = fig.add_subplot(projection='3d')
ax.set_xlabel(r'$R_x$, km')
ax.set_ylabel(r'$R_y$, km')
ax.set_zlabel(r'$R_z$, km')
ax.plot(r_BN[:, 1] * m2km, r_BN[:, 2] * m2km, r_BN[:, 3] * m2km, color=colorList[1], label="True")
for i in range(len(r_BN2[:,0])):
if np.abs(r_BN2[i, 1])>0 or np.abs(r_BN2[i, 2])>0:
ax.scatter(r_BN2[i, 1] * m2km, r_BN2[i, 2] * m2km, r_BN2[i, 3] * m2km, color=colorList[8], label="Measured")
ax.scatter(0, 0, color='r')
# ax.set_title('Orbit and Measurements')
return
def plot_attitude_error(timeLineSet, dataSigmaBR):
colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10.)
colorList = []
for i in range(10):
colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)])
plt.figure(5555, figsize=(3, 1.8), facecolor='w', edgecolor='k')
plt.rcParams["font.size"] = "8"
fig = plt.gcf()
ax = fig.gca()
vectorData = unitTestSupport.pullVectorSetFromData(dataSigmaBR)
sNorm = np.array([np.linalg.norm(v) for v in vectorData])
plt.plot(timeLineSet, sNorm,
color=colorList[0],
)
plt.xlabel('Time (min)')
plt.xlim([40, 100])
plt.ylabel(r'Attitude Error Norm $|\sigma_{C/R}|$')
#plt.savefig('AttErrorNorm.pdf')
ax.set_yscale('log')
def plot_rate_error(timeLineSet, dataOmegaBR):
colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10.)
colorList = []
for i in range(10):
colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)])
plt.figure(666666, figsize=(3, 1.8), facecolor='w', edgecolor='k')
plt.rcParams["font.size"] = "8"
styleList = ['-', '--', ':']
for idx in range(1, 4):
plt.plot(timeLineSet, dataOmegaBR[:, idx],
color=colorList[idx*2],
label=r'$\omega_{BR,' + str(idx) + '}$',
linestyle= styleList[idx-1] )
plt.legend(loc='lower right')
plt.xlim([40, 100])
plt.xlabel('Time (min)')
#plt.savefig('RateErrorControl.pdf')
plt.ylabel('Rate Tracking Error (rad/s) ')
return
def plot_rw_cmd_torque(timeData, dataUsReq, numRW):
plt.figure()
for idx in range(1, 4):
plt.plot(timeData, dataUsReq[:, idx],
'--',
color=unitTestSupport.getLineColor(idx, numRW),
label=r'$\hat u_{s,' + str(idx) + '}$')
plt.legend(loc='lower right')
plt.xlabel('Time (min)')
#plt.savefig('RWMotorTorque.pdf')
plt.ylabel('RW Motor Torque (Nm)')
def plot_rw_speeds(timeData, dataOmegaRW, numRW):
plt.figure()
for idx in range(1, numRW + 1):
plt.plot(timeData, dataOmegaRW[:, idx] / mc.RPM,
color=unitTestSupport.getLineColor(idx, numRW),
label=r'$\Omega_{' + str(idx) + '}$')
plt.legend(loc='upper right')
plt.xlabel('Time [min]')
plt.ylabel('RW Speed (RPM) ')
def plotStateCovarPlot(x, Pflat):
colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10.)
colorList = []
for i in range(10):
colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)])
numStates = len(x[0,:])-1
P = np.zeros([len(Pflat[:,0]),numStates,numStates])
t= np.zeros(len(Pflat[:,0]))
for i in range(len(Pflat[:,0])):
t[i] = x[i, 0]*ns2min
if not np.isnan(Pflat[i,1]):
Plast = Pflat[i,1:].reshape([numStates,numStates])
P[i, :, :] = Plast
x0 = x[i,7:10]
if numStates == 6 or numStates == 3:
P[i,:,:] = Pflat[i,1:].reshape([numStates,numStates])
if numStates == 9 :
plt.figure(10, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.plot(t , x[:, 1]*m2km, label='$r_1$', color = colorList[1])
plt.plot(t , 3 * np.sqrt(P[:, 0, 0])*m2km, '--', label=r'Covar (3-$\sigma$)', color = colorList[8])
plt.plot(t ,- 3 * np.sqrt(P[:, 0, 0])*m2km, '--', color = colorList[8])
plt.legend(loc='best')
plt.ylabel('Position Error (km)')
plt.grid()
plt.figure(11, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.plot(t , x[:, 4]*m2km, color = colorList[1])
plt.plot(t , 3 * np.sqrt(P[:, 3, 3])*m2km, '--', color = colorList[8])
plt.plot(t ,- 3 * np.sqrt(P[:, 3, 3])*m2km, '--', color = colorList[8])
plt.ylabel('Rate Error (km/s)')
plt.grid()
plt.figure(12, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.plot(t , x[:, 7], color = colorList[1])
plt.plot(t , x0[0] + 3 * np.sqrt(P[:, 6, 6]), '--', color = colorList[8])
plt.plot(t , x0[0] - 3 * np.sqrt(P[:, 6, 6]), '--', color = colorList[8])
plt.title('First bias component (km/s)')
plt.grid()
plt.figure(13, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.plot(t , x[:, 2]*m2km, color = colorList[1])
plt.plot(t , 3 * np.sqrt(P[:, 1, 1])*m2km, '--', color = colorList[8])
plt.plot(t ,- 3 * np.sqrt(P[:, 1, 1])*m2km, '--', color = colorList[8])
plt.title('Second pos component (km)')
plt.grid()
plt.figure(14, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.plot(t , x[:, 5]*m2km, color = colorList[1])
plt.plot(t , 3 * np.sqrt(P[:, 4, 4])*m2km, '--', color = colorList[8])
plt.plot(t ,- 3 * np.sqrt(P[:, 4, 4])*m2km, '--', color = colorList[8])
plt.xlabel('Time (min)')
plt.title('Second rate component (km/s)')
plt.grid()
plt.figure(15, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.plot(t , x[:, 8], color = colorList[1])
plt.plot(t , x0[1] + 3 * np.sqrt(P[:, 7, 7]), '--', color = colorList[8])
plt.plot(t , x0[1] - 3 * np.sqrt(P[:, 7, 7]), '--', color = colorList[8])
plt.title('Second bias component (km/s)')
plt.grid()
plt.figure(16, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.plot(t , x[:, 3]*m2km, color = colorList[1])
plt.plot(t , 3 * np.sqrt(P[:, 2, 2])*m2km, '--', color = colorList[8])
plt.plot(t ,-3 * np.sqrt(P[:, 2, 2])*m2km, '--', color = colorList[8])
plt.xlabel('Time (min)')
plt.title('Third pos component (km)')
plt.grid()
plt.figure(17, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.plot(t , x[:, 6]*m2km, color = colorList[1])
plt.plot(t , 3 * np.sqrt(P[:, 5, 5])*m2km, '--', color = colorList[8])
plt.plot(t , -3 * np.sqrt(P[:, 5, 5])*m2km, '--', color = colorList[8])
plt.xlabel('Time (min)')
plt.title('Third rate component (km/s)')
plt.grid()
plt.figure(18, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.plot(t , x[:, 9], color = colorList[1])
plt.plot(t , x0[2] + 3 * np.sqrt(P[:, 8, 8]), '--', color = colorList[8])
plt.plot(t , x0[2] - 3 * np.sqrt(P[:, 8, 8]), '--', color = colorList[8])
plt.title('Third bias component (km/s)')
plt.grid()
if numStates == 6 or numStates ==3:
plt.figure(20, figsize=(2.4, 1.4), facecolor='w', edgecolor='k')
plt.plot(t , x[:, 1]*m2km, label='State Error', color = colorList[1])
plt.plot(t , 3 * np.sqrt(P[:, 0, 0])*m2km, '--', label=r'Covar (3-$\sigma$)', color = colorList[8])
plt.plot(t ,- 3 * np.sqrt(P[:, 0, 0])*m2km, '--', color = colorList[8])
plt.legend(loc='best')
plt.ylabel(r'$r_1$ Error (km)')
#plt.savefig('Filterpos1.pdf')
if numStates == 6:
plt.figure(21, figsize=(2.4, 1.4), facecolor='w', edgecolor='k')
plt.plot(t , x[:, 4]*m2km, color = colorList[1])
plt.plot(t , 3 * np.sqrt(P[:, 3, 3])*m2km, '--', color = colorList[8])
plt.plot(t ,- 3 * np.sqrt(P[:, 3, 3])*m2km, '--', color = colorList[8])
plt.ylabel(r'$v_1$ Error (km/s)')
#plt.savefig('Filtervel1.pdf')
if numStates == 6 or numStates ==3:
plt.figure(22, figsize=(2.4, 1.4), facecolor='w', edgecolor='k')
plt.plot(t , x[:, 2]*m2km, color = colorList[1])
plt.plot(t , 3 * np.sqrt(P[:, 1, 1])*m2km, '--', color = colorList[8])
plt.plot(t ,- 3 * np.sqrt(P[:, 1, 1])*m2km, '--', color = colorList[8])
plt.ylabel(r'$r_2$ Error (km)')
#plt.savefig('Filterpos2.pdf')
if numStates == 6:
plt.figure(23, figsize=(2.4, 1.4), facecolor='w', edgecolor='k')
plt.plot(t , x[:, 5]*m2km, color = colorList[1])
plt.plot(t , 3 * np.sqrt(P[:, 4, 4])*m2km, '--', color = colorList[8])
plt.plot(t ,- 3 * np.sqrt(P[:, 4, 4])*m2km, '--', color = colorList[8])
plt.ylabel(r'$v_2$ Error (km/s)')
#plt.savefig('Filtervel2.pdf')
if numStates == 6 or numStates ==3:
plt.figure(24, figsize=(2.4, 1.4), facecolor='w', edgecolor='k')
plt.plot(t , x[:, 3]*m2km, color = colorList[1])
plt.plot(t , 3 * np.sqrt(P[:, 2, 2])*m2km, '--', color = colorList[8])
plt.plot(t ,-3 * np.sqrt(P[:, 2, 2])*m2km, '--', color = colorList[8])
plt.xlabel('Time (min)')
plt.ylabel(r'$r_3$ Error (km)')
#plt.savefig('Filterpos3.pdf')
if numStates == 6:
plt.figure(25, figsize=(2.4, 1.4), facecolor='w', edgecolor='k')
plt.plot(t , x[:, 6]*m2km, color = colorList[1])
plt.plot(t , 3 * np.sqrt(P[:, 5, 5])*m2km, '--', color = colorList[8])
plt.plot(t , -3 * np.sqrt(P[:, 5, 5])*m2km, '--', color = colorList[8])
plt.xlabel('Time (min)')
plt.ylabel(r'$v_3$ Error (km/s)')
#plt.savefig('Filtervel3.pdf')
def centerXY(centerPoints, size):
t= centerPoints[:, 0]*ns2min
subtime = []
subX = []
subY = []
center = np.full([len(t), 3], np.nan)
for i in range(len(centerPoints[:,0])):
if centerPoints[i, 1:3].any() > 1E-10:
subtime.append(centerPoints[i, 0]*ns2min)
center[i,1:] = centerPoints[i,1:3] - size/2 + 0.5
subX.append(center[i,1])
subY.append(center[i, 2])
colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10.)
colorList = []
for i in range(10):
colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)])
# xCurve = fit_sin(subtime, subX)
# yCurve = fit_sin(subtime, subY)
#
# print "Periods in Errors are , " + str(xCurve["period"]) + " and " + str(yCurve["period"]) + " s"
# plt.figure(100, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.figure(100, figsize=(3.5, 2.), facecolor='w', edgecolor='k')
plt.plot(subtime, subX, ".", label = "X-center", color = colorList[8])
# plt.plot(t, xCurve["fitfunc"](t), "r", label="X-curve")
plt.plot(subtime, subY, ".", label = "Y-center", color = colorList[1])
# plt.plot(t, yCurve["fitfunc"](t), "b", label="Y-curve")
plt.legend(loc='best')
plt.ylabel("Pixels")
plt.title('First unit Vec component in C frame (-)')
#plt.savefig('CentersXY.pdf')
def pixelAndPos(x, r, centers, size):
t= x[:, 0]*ns2min
r_norm = np.zeros([len(r[:,0]), 5])
r_norm[:,0] = r[:,0]
centerPoints = np.full([len(t), 3], np.nan)
maxDiff = np.zeros(3)
for i in range(3):
values = []
for j in range(len(x[:, 0])):
if not np.isnan(x[j,i+1]):
values.append(x[j,i+1])
try:
maxDiff[i] = np.max(np.abs(values))
except ValueError:
maxDiff[i] =1
for i in range(len(x[:,0])):
r_norm[i,1:4] = r[i,1:]/np.linalg.norm(r[i,1:]) * maxDiff
r_norm[i,4] = np.linalg.norm(r[i,1:])
if centers[i, 1:3].any() > 1E-10:
centerPoints[i,1:] = centers[i,1:3] - size/2
for i in range(2):
values = []
for j in range(len(centerPoints[:, 0])):
if not np.isnan(centerPoints[j, i + 1]):
values.append(centerPoints[j, i + 1])
try:
maxCenter = np.max(np.abs(values))
except ValueError:
maxCenter = 1
centerPoints[:, i+1]/=maxCenter/maxDiff[i]
colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10.)
colorList = []
for i in range(10):
colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)])
# plt.figure(200, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.figure(200, figsize=(3.5, 2.), facecolor='w', edgecolor='k')
plt.plot(t, x[:, 1], ".", label='Truth - Meas', color = colorList[1])
plt.plot(t, r_norm[:, 1], label = "True Pos", color = colorList[5])
plt.plot(t, centerPoints[:,1], ".", label = "X-center", color = colorList[8])
# plt.plot(t, x1["fitfunc"](t), "r--", label = "Fit")
plt.legend(loc='best')
plt.title('First unit Vec component in C frame (-)')
# plt.figure(201, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.figure(201, figsize=(3.5, 2.), facecolor='w', edgecolor='k')
plt.plot(t, x[:, 2], ".", color = colorList[1])
plt.plot(t, r_norm[:, 2], color = colorList[5])
plt.plot(t, centerPoints[:,2], ".", label = "X-center", color = colorList[8])
# plt.plot(t, x2["fitfunc"](t), "r--")
plt.title('Second unit Vec component in C frame (-)')
# plt.figure(202, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.figure(202, figsize=(3.5, 2.), facecolor='w', edgecolor='k')
plt.plot(t, x[:, 3], ".", color = colorList[1])
plt.plot(t, r_norm[:, 3], color = colorList[5])
# plt.plot(t, x3["fitfunc"](t), "r--")
plt.xlabel('Time (min)')
plt.title('Third unit Vec component in C frame (-)')
# plt.figure(203, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.figure(203, figsize=(3.5, 2.), facecolor='w', edgecolor='k')
plt.plot(t, x[:, 4]*m2km, ".", color = colorList[1])
plt.xlabel('Time (min)')
plt.title('Norm in C frame (km)')
def imgProcVsExp(true, centers, radii, size):
colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10.)
colorList = []
for i in range(10):
colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)])
t= true[:, 0]*ns2min
centerline = np.ones([len(t),2])*(size/2+0.5)
found = 0
for i in range(len(t)):
if np.abs(centers[i,1]) <1E-10 and np.abs(centers[i,2]) < 1E-10:
centers[i,1:3] = np.array([np.nan,np.nan])
radii[i,1] = np.nan
else:
found = 1
if found == 0:
centerline[i,:] = np.array([np.nan,np.nan])
plt.figure(301, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
# plt.figure(301, figsize=(3.5, 2.), facecolor='w', edgecolor='k')
plt.rcParams["font.size"] = "8"
plt.plot(t, true[:, 1], "+", label='Truth Xpix', color = colorList[1])
plt.plot(t, centerline[:,0], "--", label='X-center', color = colorList[9])
plt.plot(t, centers[:, 1], '.', label = "ImagProc Xpix", color = colorList[5], alpha=0.7)
plt.legend(loc='best')
try:
plt.ylim([centerline[-1,1]-15, centerline[-1,1]+15])
except ValueError:
pass
plt.ylabel('X (px)')
plt.xlabel('Time (min)')
#plt.savefig('Xpix.pdf')
plt.figure(302, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
# plt.figure(302, figsize=(3.5, 2.), facecolor='w', edgecolor='k')
plt.plot(t, true[:, 2], "+", label='Truth Ypix', color = colorList[1])
plt.plot(t, centerline[:,1], "--", label='Y-center', color = colorList[9])
plt.plot(t, centers[:, 2], '.', label = "ImagProc Ypix", color = colorList[5], alpha=0.7)
plt.legend(loc='best')
try:
plt.ylim([centerline[-1,1]-15, centerline[-1,1]+15])
except ValueError:
pass
plt.ylabel('Y (px)')
plt.xlabel('Time (min)')
#plt.savefig('Ypix.pdf')
plt.figure(312, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
# plt.figure(312, figsize=(3.5, 2.), facecolor='w', edgecolor='k')
plt.plot(t, true[:, 3], "+", label=r'Truth $\rho$', color = colorList[1])
plt.plot(t, radii[:, 1],'.', label = r"ImagProc $\rho$", color = colorList[5], alpha=0.7)
plt.legend(loc='best')
plt.ylabel(r'$\rho$ (px)')
plt.xlabel('Time (min)')
#plt.savefig('Rhopix.pdf')
plt.figure(303, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
# plt.figure(303, figsize=(3.5, 2.), facecolor='w', edgecolor='k')
plt.plot(t, true[:, 1] - centers[:, 1], ".", label=r'$\mathrm{X}_\mathrm{true} - \mathrm{X}_\mathrm{hough}$', color = colorList[1])
plt.legend(loc='best')
plt.ylabel('X error (px)')
plt.grid()
#plt.savefig('Xerror.pdf')
plt.figure(304, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
# plt.figure(304, figsize=(3.5, 2.), facecolor='w', edgecolor='k')
plt.plot(t, true[:, 2] - centers[:, 2], ".", label=r'$\mathrm{Y}_\mathrm{true} - \mathrm{Y}_\mathrm{hough}$', color = colorList[1])
plt.legend(loc='best')
plt.ylabel('Y error (px)')
plt.grid()
#plt.savefig('Yerror.pdf')
plt.figure(305, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
# plt.figure(305, figsize=(3.5, 2.), facecolor='w', edgecolor='k')
plt.plot(t, true[:, 3] - radii[:, 1], ".", label=r'$\mathrm{\rho}_\mathrm{true} - \mathrm{\rho}_\mathrm{hough}$', color = colorList[1])
plt.legend(loc='best')
plt.ylabel('Radius error (px)')
plt.xlabel("Time (min)")
plt.grid()
#plt.savefig('Rhoerror.pdf')
def plotPostFitResiduals(Res, noise):
MeasNoise = np.zeros([len(Res[:,0]), 3])
Noiselast = np.zeros([3,3])
colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10.)
colorList = []
for i in range(10):
colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)])
t= np.zeros(len(Res[:,0]))
for i in range(len(Res[:,0])):
t[i] = Res[i, 0]*ns2min
if isinstance(noise, float):
MeasNoise[i, :] = np.array([3*np.sqrt(noise),3*np.sqrt(noise),3*np.sqrt(noise)])
else:
if not np.isnan(noise[i, 1]):
Noiselast = noise[i, 1:].reshape([3, 3])
MeasNoise[i, :] = np.array([3*np.sqrt(Noiselast[0,0]), 3*np.sqrt(Noiselast[1,1]), 3*np.sqrt(Noiselast[2,2])])/5
if np.isnan(noise[i, 1]):
MeasNoise[i, :] = np.array([3*np.sqrt(Noiselast[0,0]), 3*np.sqrt(Noiselast[1,1]), 3*np.sqrt(Noiselast[2,2])])/5
# Don't plot zero values, since they mean that no measurement is taken
for j in range(len(Res[0,:])-1):
if -1E-10 < Res[i,j+1] < 1E-10:
Res[i, j+1] = np.nan
if len(Res[0,:])-1 == 3:
plt.figure(401, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.plot(t , Res[:, 1]*m2km, ".", label='Residual', color = colorList[1])
plt.plot(t , MeasNoise[:,0]*m2km, '--', label=r'Noise ($3\sigma$)', color = colorList[8])
plt.plot(t , -MeasNoise[:,0]*m2km, '--', color = colorList[8])
plt.legend(loc=2)
max = np.amax(MeasNoise[:,0])
if max >1E-15:
plt.ylim([-2*max*m2km, 2*max*m2km])
plt.ylabel(r'$r_1$ Measured (km)')
plt.xlabel("Time (min)")
plt.grid()
#plt.savefig('Res1.pdf')
plt.figure(402, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.plot(t , Res[:, 2]*m2km, ".", color = colorList[1])
plt.plot(t , MeasNoise[:,1]*m2km, '--', color = colorList[8])
plt.plot(t , -MeasNoise[:,1]*m2km, '--', color = colorList[8])
max = np.amax(MeasNoise[:,1])
if max >1E-15:
plt.ylim([-2*max*m2km, 2*max*m2km])
plt.ylabel(r'$r_2$ Measured (km)')
plt.xlabel("Time (min)")
plt.grid()
#plt.savefig('Res2.pdf')
plt.figure(403, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.plot(t , Res[:, 3]*m2km, ".", color = colorList[1])
plt.plot(t , MeasNoise[:,2]*m2km, '--', color = colorList[8])
plt.plot(t , -MeasNoise[:,2]*m2km, '--', color = colorList[8])
max = np.amax(MeasNoise[:,2])
if max >1E-15:
plt.ylim([-2*max*m2km, 2*max*m2km])
plt.ylabel(r'$r_3$ Measured (km)')
plt.xlabel("Time (min)")
plt.grid()
#plt.savefig('Res3.pdf')
if len(Res[0,:])-1 == 6:
plt.figure(405, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.plot(t , Res[:, 1], "b.")
plt.plot(t , MeasNoise[:,0], 'r--')
plt.plot(t , -MeasNoise[:,0], 'r--')
plt.legend(loc='best')
plt.title('X-pixel component')
plt.grid()
plt.figure(406, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.plot(t , Res[:, 4], "b.")
plt.plot(t , MeasNoise[:,0], 'r--')
plt.plot(t , -MeasNoise[:,0], 'r--')
plt.title('X-bias component')
plt.grid()
plt.figure(407, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.plot(t , Res[:, 2], "b.")
plt.plot(t , MeasNoise[:,1], 'r--')
plt.plot(t , -MeasNoise[:,1], 'r--')
plt.title('Y-pixel component')
plt.grid()
plt.figure(408, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.plot(t , Res[:, 5], "b.")
plt.plot(t , MeasNoise[:,1], 'r--')
plt.plot(t , -MeasNoise[:,1], 'r--')
plt.xlabel('Time (min)')
plt.title('Y-bias component')
plt.grid()
plt.figure(409, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.plot(t , Res[:, 3], "b.")
plt.plot(t , MeasNoise[:,2], 'r--')
plt.plot(t , -MeasNoise[:,2], 'r--')
plt.xlabel('Time (min)')
plt.ylabel('Rho-component')
plt.grid()
plt.figure(410, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.plot(t , Res[:, 6], "b.")
plt.plot(t , MeasNoise[:,2], 'r--')
plt.plot(t , -MeasNoise[:,2], 'r--')
plt.xlabel('Time (min)')
plt.ylabel('Rho-bias component')
plt.grid()
def plot_cirlces(centers, radii, validity, resolution):
circleIndx = []
for i in range(len(centers[:,0])):
if validity[i, 1] == 1:
circleIndx.append(i)
colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(len(circleIndx)+1)
colorList = []
for i in range(len(circleIndx)):
colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)])
plt.figure(500, figsize=(3, 3), facecolor='w', edgecolor='k')
ax = plt.gca()
for i in range(len(circleIndx)):
if i%30 ==0:
ell_xy = Ellipse(xy=(centers[circleIndx[i],1], centers[circleIndx[i],2]),
width=radii[circleIndx[i],1], height=radii[circleIndx[i],1],
angle=0, linestyle='-', linewidth=3, color=colorList[i], alpha=0.7)
ell_xy.set(facecolor='none')
ax.add_patch(ell_xy)
ax.invert_yaxis()
ax.set_xlim(0, resolution[0])
ax.set_ylim(resolution[1],0)
plt.xlabel('X-axis (px)')
plt.ylabel('Y-axis (px)')
plt.axis("equal")
#plt.savefig('Circles.pdf')
def plot_limb(limbPoints, numLimb, validity, resolution):
indx = []
time = []
for i in range(len(limbPoints[:,0])):
if validity[i, 1] == 1:
indx.append(i)
numBerPoints = []
colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(len(indx)+1)
colorList = []
for i in range(len(indx)):
colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)])
numBerPoints.append(numLimb[indx[i], 1])
time.append(numLimb[indx[i], 0]*1E-9/60)
limbX = np.zeros([len(indx), 2*2000])
limbY = np.zeros([len(indx), 2*2000])
for i in range(len(indx)):
for j in range(1, int(numLimb[indx[i], 1])):
if np.abs(limbPoints[indx[i], 2*j] + limbPoints[indx[i], 2*j-1])>1E-1:
limbX[i,j] = limbPoints[indx[i], 2*j-1]
limbY[i,j] = limbPoints[indx[i], 2*j]
plt.figure(600, figsize=(3, 3), facecolor='w', edgecolor='k')
ax = plt.gca()
for i in range(len(indx)):
if i%30 ==0:
ax.scatter(limbX[i,:int(numLimb[indx[i],1])-1], limbY[i,:int(numLimb[indx[i],1])-1], color=colorList[i], marker='.', alpha=0.2)
ax.invert_yaxis()
ax.set_xlim(0, resolution[0])
ax.set_ylim(resolution[1],0)
plt.xlabel('X-axis (px)')
plt.ylabel('Y-axis (px)')
plt.axis("equal")
#plt.savefig('Limbs.pdf')
plt.figure(601, figsize=(2.7, 1.6), facecolor='w', edgecolor='k')
plt.plot(time, numBerPoints, color=colorList[1])
plt.xlabel('Time (min)')
plt.ylabel('Limb Size (px)')
#plt.savefig('LimbPoints.pdf')
[docs]class AnimatedCircles(object):
"""An animated scatter plot using matplotlib.animations.FuncAnimation."""
def __init__(self, size, centers, radii, validity):
self.sensorSize = size
self.idx = 0
self.i = 0
# Setup the figure and axes...
self.fig, self.ax = plt.subplots(num='Circles Animation')#, figsize=(10, 10), dpi=80, facecolor='w')
self.ax.invert_yaxis()
self.ax.set_xlim(0, self.sensorSize[0])
self.ax.set_ylim(self.sensorSize[1],0)
self.ax.set_aspect("equal")
# Then setup FuncAnimation.
self.setData(centers, radii, validity)
self.ani = mpl.animation.FuncAnimation(self.fig, self.update, interval=100,
init_func=self.setup_plot, frames=len(self.circleIndx), blit=True)
self.ani.save('../../TestImages/circlesAnimated.gif', writer='imagemagick', fps=60)
def setData(self, centers, radii, validity):
self.circleIndx = []
for i in range(len(centers[:,0])):
if validity[i, 1] == 1:
self.circleIndx.append(i)
self.centers = np.zeros([len(self.circleIndx), 3])
self.radii = np.zeros([len(self.circleIndx), 2])
for i in range(len(self.circleIndx)):
self.centers[i,0] = centers[self.circleIndx[i],0]
self.centers[i, 1:] = centers[self.circleIndx[i], 1:3]
self.radii[i,0] = centers[self.circleIndx[i],0]
self.radii[i, 1] = radii[self.circleIndx[i], 1]
self.colorList = mpl.cm.get_cmap("inferno").colors
[docs] def setup_plot(self):
"""Initial drawing of the scatter plot."""
self.scat = self.ax.scatter(self.centers[0,1], self.centers[0,2], c=self.colorList[0], s=self.radii[0,1])
# For FuncAnimation's sake, we need to return the artist we'll be using
# Note that it expects a sequence of artists, thus the trailing comma.
return self.scat,
def data_stream(self, i):
while self.idx < len(self.circleIndx):
xy = np.array([[self.sensorSize[0]/2+0.5, self.sensorSize[0]/2+0.5],[self.centers[i,1], self.centers[i,2]],[self.centers[i,1], self.centers[i,2]]])
s = np.array([1, 1, self.radii[i,1]])
c = [self.colorList[-1], self.colorList[i],self.colorList[i]]
yield np.c_[xy[:,0], xy[:,1], s[:]], c
[docs] def update(self, i):
"""Update the scatter plot."""
data, color = next(self.data_stream(i))
# Set x and y data...
self.scat.set_offsets(data[:, :2])
# Set sizes...
self.scat.set_sizes((data[:, 2]/2.)**2.)
# Set colors..
# self.scat.set_array(data[:, 3])
self.scat.set_edgecolor(color)
self.scat.set_facecolor("none")
# We need to return the updated artist for FuncAnimation to draw..
# Note that it expects a sequence of artists, thus the trailing comma.
return self.scat,
def StateErrorCovarPlot(x, Pflat, FilterType, show_plots):
nstates = int(np.sqrt(len(Pflat[0,:])-1))
mpl.rc("figure", figsize=(3, 1.8))
colorsInt = int(len(mpl.cm.get_cmap().colors)/10)
colorList = []
for i in range(10):
colorList.append(mpl.cm.get_cmap().colors[i*colorsInt])
P = np.zeros([len(Pflat[:,0]),nstates,nstates])
t= np.zeros(len(Pflat[:,0]))
for i in range(len(Pflat[:,0])):
t[i] = x[i, 0]*ns2min
P[i,:,:] = Pflat[i,1:37].reshape([nstates,nstates])
for j in range(len(P[0,0,:])):
P[i,j,j] = np.sqrt(P[i,j,j])
for i in range(3):
if i ==0:
plt.figure(num=None, facecolor='w', edgecolor='k')
plt.plot(t , x[:, i+1], color = colorList[0], label='Error')
plt.plot(t , 3 * P[:, i, i],color = colorList[-1], linestyle = '--', label=r'Covar (3-$\sigma$)')
plt.plot(t , -3 * P[:, i, i], color = colorList[-1], linestyle = '--')
plt.legend(loc='best')
plt.ylabel('$d_' + str(i+1) + '$ Error (-)')
plt.ylim([-0.1,0.1])
plt.xlabel('Time (min)')
unitTestSupport.saveFigurePDF('StateCovarHeading'+ FilterType + str(i) , plt, './')
if show_plots:
plt.show()
plt.close("all")
else:
plt.figure(num=None, facecolor='w', edgecolor='k')
plt.plot(t , x[:, i+1], color = colorList[0])
plt.plot(t , 3 * P[:, i, i],color = colorList[-1], linestyle = '--')
plt.plot(t , -3 * P[:, i, i], color = colorList[-1], linestyle = '--')
plt.ylabel('$d_' + str(i+1) + '$ Error (-)')
plt.xlabel('Time (min)')
plt.ylim([-0.1,0.1])
unitTestSupport.saveFigurePDF('StateCovarHeading'+ FilterType + str(i) , plt, './')
if show_plots:
plt.show()
plt.close("all")
if nstates>3:
for i in range(3,nstates):
if i == 3:
plt.figure(num=None, facecolor='w', edgecolor='k')
plt.plot(t, x[:, i + 1], color = colorList[0], label='Error')
plt.plot(t, 3 * P[:, i, i], color = colorList[-1],linestyle = '--', label=r'Covar (3-$\sigma$)')
plt.plot(t, -3 * P[:, i, i], color = colorList[-1],linestyle = '--')
plt.ylim([-0.0007,0.0007])
# plt.legend(loc='best')
if nstates == 5:
plt.ylabel(r'$\omega_' + str(i - 1) + r'$ Error (-)')
else:
plt.ylabel(r'$d\'_' + str(i-2) + r'$ Error (-)')
plt.xlabel('Time (min)')
unitTestSupport.saveFigurePDF('StateCovarRate' + FilterType + str(i), plt, './')
if show_plots:
plt.show()
plt.close("all")
else:
plt.figure(num=None, facecolor='w', edgecolor='k')
plt.plot(t, x[:, i + 1], color = colorList[0])
plt.plot(t, 3 * P[:, i, i], color = colorList[-1],linestyle = '--')
plt.plot(t, -3 * P[:, i, i], color = colorList[-1],linestyle = '--')
plt.ylim([-0.0007,0.0007])
if nstates == 5:
plt.ylabel(r'$\omega_' + str(i - 1) + r'$ Error (-)')
else:
plt.ylabel(r'$d\'_' + str(i-2) + r'$ Error (-)')
plt.xlabel('Time (min)')
unitTestSupport.saveFigurePDF('StateCovarRate' + FilterType + str(i), plt, './')
if show_plots:
plt.show()
plt.close("all")
plt.close("all")
def PostFitResiduals(Res, covar_B, FilterType, show_plots):
mpl.rc("figure", figsize=(3, 1.8))
colorsInt = int(len(mpl.cm.get_cmap().colors) / 10)
colorList = []
for i in range(10):
colorList.append(mpl.cm.get_cmap().colors[i * colorsInt])
MeasNoise = np.zeros([len(Res[:, 0]), 3])
prevNoise = np.zeros(3)
t = np.zeros(len(Res[:, 0]))
constantVal = np.array([np.nan] * 4)
for i in range(len(Res[:, 0])):
t[i] = Res[i, 0] * ns2min
if np.abs(covar_B[i, 1]) < 1E-15 and prevNoise[0] == 0:
MeasNoise[i,:] = np.full(3, np.nan)
elif np.abs(covar_B[i, 1]) < 1E-15 and prevNoise[0] != 0:
MeasNoise[i,:] = prevNoise
else:
MeasNoise[i,0] = 3 * np.sqrt(covar_B[i, 1])
MeasNoise[i,1] = 3 * np.sqrt(covar_B[i, 1 + 4])
MeasNoise[i,2] = 3 * np.sqrt(covar_B[i, 1 + 8])
prevNoise = MeasNoise[i,:]
# Don't plot constant values, they mean no measurement is taken
if i > 0:
for j in range(1, 4):
constantRes = np.abs(Res[i, j] - Res[i - 1, j])
if constantRes < 1E-10 or np.abs(constantVal[j - 1] - Res[i, j]) < 1E-10:
constantVal[j - 1] = Res[i, j]
Res[i, j] = np.nan
for i in range(3):
if i == 0:
plt.figure(num=None, dpi=80, facecolor='w', edgecolor='k')
plt.plot(t, Res[:, i + 1], color=colorList[0], linestyle='', marker='.', label='Residual')
plt.plot(t, MeasNoise[:,i], color=colorList[-1], linestyle="--", label=r'Noise (3-$\sigma$)')
plt.plot(t, -MeasNoise[:,i], color=colorList[-1], linestyle="--", )
plt.legend(loc='best')
plt.ylabel('$r_' + str(i + 1) + '$ (-)')
plt.xlabel('Time (min)')
plt.ylim([-2*MeasNoise[-1,i], 2*MeasNoise[-1,i]])
unitTestSupport.saveFigurePDF('PostFit' + FilterType + str(i), plt, './')
if show_plots:
plt.show()
plt.close("all")
else:
plt.figure(num=None, dpi=80, facecolor='w', edgecolor='k')
plt.plot(t, Res[:, i + 1], color=colorList[0], linestyle='', marker='.')
plt.plot(t, MeasNoise[:,i], color=colorList[-1], linestyle="--")
plt.plot(t, -MeasNoise[:,i], color=colorList[-1], linestyle="--", )
plt.ylabel('$r_' + str(i + 1) + '$ (-)')
plt.xlabel('Time min')
plt.ylim([-2*MeasNoise[-1,i], 2*MeasNoise[-1,i]])
unitTestSupport.saveFigurePDF('PostFit' + FilterType + str(i), plt, './')
if show_plots:
plt.show()
plt.close("all")
plt.close("all")
[docs]class AnimatedLimb(object):
"""An animated scatter plot using matplotlib.animations.FuncAnimation."""
def __init__(self, size, limb, centers, validity):
self.stream = self.data_stream()
self.sensorSize = size
self.idx = 0
self.i = 0
# Setup the figure and axes...
self.fig, self.ax = plt.subplots(num='Limb Animation') # , figsize=(10, 10), dpi=80, facecolor='w')
self.ax.invert_yaxis()
self.ax.set_xlim(0, self.sensorSize[0])
self.ax.set_ylim(self.sensorSize[1], 0)
self.ax.set_aspect("equal")
# Then setup FuncAnimation.
self.setData(limb, validity)
# Setup the figure and axes...
self.fig, self.ax = plt.subplots()
# Then setup FuncAnimation.
self.ani = mpl.animation.FuncAnimation(self.fig, self.update, interval=100,
init_func=self.setup_plot, blit=True)
[docs] def setup_plot(self):
"""Initial drawing of the scatter plot."""
self.scat = self.ax.scatter(self.centers[0,1], self.centers[0,2], c=self.colorList[0], s=self.radii[0,1])
# For FuncAnimation's sake, we need to return the artist we'll be using
# Note that it expects a sequence of artists, thus the trailing comma.
return self.scat,
def setData(self, centers, limbPoints, validity):
self.pointIndx = []
for i in range(len(limbPoints[:,0])):
if validity[i, 1] == 1:
self.pointIndx.append(i)
self.centers = np.zeros([len(self.pointIndx), 2])
self.points = np.zeros([len(self.pointIndx), 2*1000])
for i in range(len(self.pointIndx)):
self.centers[i, 1:] = centers[self.pointIndx[i], 1:2]
self.points[i,0] = limbPoints[self.pointIndx[i],1:]
self.colorList = mpl.cm.get_cmap("inferno").colors
[docs] def data_stream(self, i):
"""Generate a random walk (brownian motion). Data is scaled to produce
a soft "flickering" effect."""
limb = []
while self.idx < len(self.circleIndx):
xy = np.array([[self.sensorSize[0]/2+0.5, self.sensorSize[0]/2+0.5],[self.centers[i,1], self.centers[i,2]]])
for j in range(len(self.points[i,:])/2):
if self.points[i,j] >1E-1 or self.points[i,j+1] >1E-1:
limb.append([self.points[i,j], self.points[i,j+1]])
c = [self.colorList[-1], self.colorList[i], self.colorList[i]]
yield np.c_[xy[:,0], xy[:,1], limb[:,0], limb[:,1]], c
[docs] def update(self, i):
"""Update the scatter plot."""
data, color = next(self.data_stream(i))
# Set x and y data...
self.scat.set_offsets(data[:, :2])
# Set sizes...
self.scat.set_offsets(data[:, 2:4])
# Set colors..
# self.scat.set_array(data[:, 3])
self.scat.set_edgecolor(color)
self.scat.set_facecolor("none")
# We need to return the updated artist for FuncAnimation to draw..
# Note that it expects a sequence of artists, thus the trailing comma.
return self.scat