def plot_stateMeanPropagationError(k_vec, X_error_vec):
sigma_lim = 0.01
kf.plotSigma(k_vec, X_error_vec[:3, :], 4, dt, sigma_lim)
kf.checkArePlotsSaved('EKF', 'sigma_propagation_error', 'MRP State Propagation Error')
omega_lim = -1
kf.plotOmega(k_vec, X_error_vec[3:, :], 5, dt, omega_lim)
kf.checkArePlotsSaved('EKF', 'bias_propagation_error', 'Bias State Propagation Error')
def plot_groundTruth(X_ground_vec, k_vec, dt):
sigma_lim = 1
kf.plotSigma(k_vec, X_ground_vec[:3, :], 0, dt, sigma_lim)
kf.checkArePlotsSaved('EKF', 'sigma_true', 'True MRP')
omega_lim = -1
kf.plotOmega(k_vec, X_ground_vec[3:, :], 1, dt, omega_lim)
kf.checkArePlotsSaved('EKF', 'bias_true', 'True Angular Bias')
def plot_groundTruth(X_ground_vec, k_vec, dt):
sigma_lim = 1
kf.plotSigma(k_vec, X_ground_vec[:3, :], 0, dt, sigma_lim)
kf.checkArePlotsSaved('EKF', 'sigma_true', 'True MRP')
omega_lim = -1
kf.plotOmega(k_vec, X_ground_vec[3:, :], 1, dt, omega_lim)
kf.checkArePlotsSaved('EKF', 'bias_true', 'True Angular Bias')
def plot_stateMeanPropagationError(k_vec, X_error_vec):
sigma_lim = 0.01
kf.plotSigma(k_vec, X_error_vec[:3, :], 4, dt, sigma_lim)
kf.checkArePlotsSaved('EKF', 'sigma_propagation_error',
'MRP State Propagation Error')
omega_lim = -1
kf.plotOmega(k_vec, X_error_vec[3:, :], 5, dt, omega_lim)
kf.checkArePlotsSaved('EKF', 'bias_propagation_error',
'Bias State Propagation Error')
def plot_prefitResiduals(k_meas, E_y_ergo_vec, k_vec, dt):
def chi2inv_NIS():
# The values r1_y and r2_Y have been computed in Matlab
# for k = 2000, p = 3
r1_y = 2.8485
r2_y = 3.1560
return (r1_y, r2_y)
(r1_y, r2_y) = chi2inv_NIS()
idx_y = k_meas -1
E_yy_ergo = E_y_ergo_vec[idx_y::idx_y]
t_yy = dt * k_vec[idx_y::idx_y]
plt.figure(22)
plt.plot(t_yy, E_yy_ergo, 'bo')
plt.axhline(r1_y / k_meas, color='dodgerblue')
plt.axhline(r2_y/ k_meas, color='dodgerblue')
plt.xlabel('Time, sec')
plt.ylabel('NIS statistic, $\\bar{\epsilon}_y$')
plt.legend(['NIS$_K$','$r_1$', '$r_2$'])
kf.checkArePlotsSaved('EKF', 'NIS_test', 'NIS Estimation Results')
def plot_prefitResiduals(k_meas, E_y_ergo_vec, k_vec, dt):
def chi2inv_NIS():
# The values r1_y and r2_Y have been computed in Matlab
# for k = 2000, p = 3
r1_y = 2.8485
r2_y = 3.1560
return (r1_y, r2_y)
(r1_y, r2_y) = chi2inv_NIS()
idx_y = k_meas - 1
E_yy_ergo = E_y_ergo_vec[idx_y::idx_y]
t_yy = dt * k_vec[idx_y::idx_y]
plt.figure(22)
plt.plot(t_yy, E_yy_ergo, 'bo')
plt.axhline(r1_y / k_meas, color='dodgerblue')
plt.axhline(r2_y / k_meas, color='dodgerblue')
plt.xlabel('Time, sec')
plt.ylabel('NIS statistic, $\\bar{\epsilon}_y$')
plt.legend(['NIS$_K$', '$r_1$', '$r_2$'])
kf.checkArePlotsSaved('EKF', 'NIS_test', 'NIS Estimation Results')
def plotOmegaBiasEstimate(t, m2_error, s2_vec, nfig, omegaLim):
omegaLabel = 'Angular Rate [rad / s]'
plt.figure(nfig + 3)
kf.plotMeanErrorAndCovar(t, m2_error[0, :], s2_vec[0, :], kf.color_x, 'b', omegaLim, omegaLabel)
kf.checkArePlotsSaved('EKF', 'bias_plus_1', 'Estimate of state $\omega_{b1}$')
plt.figure(nfig + 4)
kf.plotMeanErrorAndCovar(t, m2_error[1, :], s2_vec[1, :], kf.color_y, 'g', omegaLim, omegaLabel)
kf.checkArePlotsSaved('EKF', 'bias_plus_2', 'Estimate of state $\omega_{b1}$')
plt.figure(nfig + 5)
kf.plotMeanErrorAndCovar(t, m2_error[2, :], s2_vec[2, :], kf.color_z, 'r', omegaLim, omegaLabel)
kf.checkArePlotsSaved('EKF', 'bias_plus_3', 'Estimate of state $\omega_{b1}$')
def plotSigmaEstimate(t, m1_error, s1_vec, nfig, sigmaLim):
sigmaLabel = 'MRP Attitude Set'
plt.figure(nfig)
kf.plotMeanErrorAndCovar(t, m1_error[0, :], s1_vec[0, :], kf.color_x, 'b', sigmaLim, sigmaLabel)
kf.checkArePlotsSaved('EKF', 'sigma_plus_1', 'Estimate of state $\sigma_1$')
plt.figure(nfig + 1)
kf.plotMeanErrorAndCovar(t, m1_error[1, :], s1_vec[1, :], kf.color_y, 'g', sigmaLim, sigmaLabel)
kf.checkArePlotsSaved('EKF', 'sigma_plus_2', 'Estimate of state $\sigma_2$')
plt.figure(nfig + 2)
kf.plotMeanErrorAndCovar(t, m1_error[2, :], s1_vec[2, :], kf.color_z, 'r', sigmaLim, sigmaLabel)
kf.checkArePlotsSaved('EKF', 'sigma_plus_3', 'Estimate of state $\sigma_3$')
def plotOmegaBiasEstimate(t, m2_error, s2_vec, nfig, omegaLim):
omegaLabel = 'Angular Rate [rad / s]'
plt.figure(nfig + 3)
kf.plotMeanErrorAndCovar(t, m2_error[0, :], s2_vec[0, :], kf.color_x, 'b',
omegaLim, omegaLabel)
kf.checkArePlotsSaved('EKF', 'bias_plus_1',
'Estimate of state $\omega_{b1}$')
plt.figure(nfig + 4)
kf.plotMeanErrorAndCovar(t, m2_error[1, :], s2_vec[1, :], kf.color_y, 'g',
omegaLim, omegaLabel)
kf.checkArePlotsSaved('EKF', 'bias_plus_2',
'Estimate of state $\omega_{b1}$')
plt.figure(nfig + 5)
kf.plotMeanErrorAndCovar(t, m2_error[2, :], s2_vec[2, :], kf.color_z, 'r',
omegaLim, omegaLabel)
kf.checkArePlotsSaved('EKF', 'bias_plus_3',
'Estimate of state $\omega_{b1}$')
def plotSigmaEstimate(t, m1_error, s1_vec, nfig, sigmaLim):
sigmaLabel = 'MRP Attitude Set'
plt.figure(nfig)
kf.plotMeanErrorAndCovar(t, m1_error[0, :], s1_vec[0, :], kf.color_x, 'b',
sigmaLim, sigmaLabel)
kf.checkArePlotsSaved('EKF', 'sigma_plus_1',
'Estimate of state $\sigma_1$')
plt.figure(nfig + 1)
kf.plotMeanErrorAndCovar(t, m1_error[1, :], s1_vec[1, :], kf.color_y, 'g',
sigmaLim, sigmaLabel)
kf.checkArePlotsSaved('EKF', 'sigma_plus_2',
'Estimate of state $\sigma_2$')
plt.figure(nfig + 2)
kf.plotMeanErrorAndCovar(t, m1_error[2, :], s1_vec[2, :], kf.color_z, 'r',
sigmaLim, sigmaLabel)
kf.checkArePlotsSaved('EKF', 'sigma_plus_3',
'Estimate of state $\sigma_3$')
def plot_chiSquare_NEES(k_vec, E_x_vec, dt):
(r1, r2) = kf.chi2inv(1)
kf.plotChiResults(k_vec, r1, r2, E_x_vec, 21, dt)
kf.checkArePlotsSaved('EKF', 'NEES_test', 'NIS Estimation Results')
def plot_gyroObs(omega_obs_vec, k_vec, dt):
omega_lim = -1
kf.plotOmega(k_vec, omega_obs_vec, 3, dt, omega_lim)
kf.checkArePlotsSaved('EKF', 'gyro_obs', 'Observed Angular Rates')
def plot_STmeas(Y_meas_vec, k_vec, dt):
sigma_lim = 1
kf.plotSigma(k_vec, Y_meas_vec, 2, dt, sigma_lim)
kf.checkArePlotsSaved('EKF', 'sigma_meas', 'Measured MRP')
def plot_chiSquare_NEES(k_vec, E_x_vec, dt):
(r1, r2) = kf.chi2inv(1)
kf.plotChiResults(k_vec, r1, r2, E_x_vec, 21, dt)
kf.checkArePlotsSaved('EKF', 'NEES_test', 'NIS Estimation Results')
def plot_gyroObs(omega_obs_vec, k_vec, dt):
omega_lim = -1
kf.plotOmega(k_vec, omega_obs_vec, 3, dt, omega_lim)
kf.checkArePlotsSaved('EKF', 'gyro_obs', 'Observed Angular Rates')
def plot_STmeas(Y_meas_vec, k_vec, dt):
sigma_lim = 1
kf.plotSigma(k_vec, Y_meas_vec, 2, dt, sigma_lim)
kf.checkArePlotsSaved('EKF', 'sigma_meas', 'Measured MRP')