# estMapY.append(est_map_x[1, est_k])
# simYr.append(sim_yr[0, k])
# simYr2.append(sim_yr[1, k])
# gnssLinPosRbtPlot.append(dvl_linVelRbt[0, dvl_k])
# gnssLinPosRbtPlot2.append(dvl_linVelRbt[1, dvl_k])
# estRbtXPlot.append(est_rbt_x[0, est_k])
# estRbtXPlot2.append(est_rbt_x[1, est_k])
### Increment time ###
t_now = t_now + dt
""" Plots """
# Robot x vs y
genPlots.plotXY(simX, simY, linPosX, linPosY, estMapX, estMapY)
# # Plot Robot position vs time (map frame)
# # Set fonts properties
# plt.rc('text', usetex=True)
# plt.rc('font', family='serif')
# titleFontSize = 20
# defaultFontSize = 12
# markerSize = 0.5
# tt = np.linspace(0, endTime, 48001, endpoint= True)
# ttGnss = np.linspace(0, endTime, 48001, endpoint= True)
# ttImu = np.linspace(0, endTime, 1000, endpoint = True)
# ttEst = np.linspace(0, endTime, 48001, endpoint = True)
acBiasX.append(imu_acBias[0, imu_k])
acBiasY.append(imu_acBias[1, imu_k])
acBiasZ.append(imu_acBias[2, imu_k])
# Estimator parameters
estXMap.append(est_map_x[0, est_k])
estYMap.append(est_map_x[1, est_k])
estZMap.append(est_map_x[2, est_k])
estVelXRbt.append(est_rbt_x[6, est_k])
estVelYRbt.append(est_rbt_x[7, est_k])
estVelZRbt.append(est_rbt_x[8, est_k])
estAcBiasX.append(est_rbt_x[12, est_k])
estAcBiasY.append(est_rbt_x[13, est_k])
estAcBiasZ.append(est_rbt_x[14, est_k])
### Increment time ###
t_now = t_now + t.dt
""" Plots """
# Robot x vs y
genPlots.plotXY(simX, simY, linPosX, linPosY, estXMap, estYMap)
genPlots.plotMap(t_N, t.end, simX, simY, simZ, estXMap, estYMap, estZMap,
linPosX, linPosY, linPosZ)
genPlots.plotRbt(t_N, t.end, simRoll, simPitch, simYaw, simVelX, simVelY,
simVelZ, linVelXRbt, linVelYRbt, linVelZRbt, roll, pitch, yaw,
acBiasX, acBiasY, acBiasZ, estAcBiasX, estAcBiasY, estAcBiasZ,
estVelXRbt, estVelYRbt, estVelZRbt)
plt.show()
# if __name__ == "__main__":
# # execute only if run as a script
# main()
