def isObservable(self, X, t, params):
"""
Method that decides if an attitude is observable for a given time and star coordinates.
:param X: State.
:param t: Time.
:param params: Dynamic parameters (star coordinates).
:return: Boolean indicating if it is visible.
"""
period_obs = 1.0/self._obs_rate
# It is assumed that the observations cannot be taken faster than obs_rate.
if self._last_obs == -1:
self._last_obs = t
elif self._last_obs == t: # It is still the same sampling interval
self._last_obs = t
elif self._last_obs + period_obs <= t:
self._last_obs = t
else:
return False
mrp1 = X[0]
mrp2 = X[1]
mrp3 = X[2]
mrp = np.array([mrp1, mrp2, mrp3])
star_number = params[0]
star_coord = self.getObserverCoordinates()
star_right_as = star_coord[star_number][2]
star_dec = star_coord[star_number][3]
cos_alpha = np.cos(star_right_as)
sin_alpha = np.sin(star_right_as)
cos_delta = np.cos(star_dec)
sin_delta = np.sin(star_dec)
# Inertial direction of the star (Unit vector!!!)
r_N = np.array([cos_delta*cos_alpha, cos_delta*sin_alpha, sin_delta])
# Body attitude B relative to inertial frame N
BN = attitudeKinematics.mrp2dcm(mrp)
# Body direction of the star (Unit vector!!!)
r_B = BN.dot(r_N)
st_nmbr = len(self._starTrackerDirecBody) # Number of Star Trackers
for i in range(0, st_nmbr):
b_ST = self._starTrackerDirecBody[i] # Direction of the camera of the star
fov_ST = self._fov[i]
angle = np.arccos(r_B.dot(b_ST)) # angle between the star and the normal to the camera
if angle <= fov_ST/2: # The star is observable
return True
return False
r_B_true_ST = np.zeros((nmbr_obs_ST, 3))
for i in range(0, nmbr_obs_ST):
star_observed = star_catalog_ST[observers_ST[i]]
star_RA = star_observed[2]
star_dec = star_observed[3]
cos_right_asc = np.cos(star_RA)
sin_right_asc = np.sin(star_RA)
cos_dec = np.cos(star_dec)
sin_dec = np.sin(star_dec)
r_N = np.array([cos_dec * cos_right_asc, cos_dec * sin_right_asc, sin_dec])
mrp = obs_true_states_ST[i, 0:3]
BN = attitudeKinematics.mrp2dcm(mrp)
r_B_true_ST[i, :] = BN.dot(r_N)
plt.figure()
plt.subplot(311)
plt.plot(obs_time_vec_ST, r_B_true_ST[:, 0], label='$x_{star-B}$ true')
plt.plot(obs_time_vec_ST,
observations_ST[:, 0],
'.',
label='$x_{star-B}$ observed')
plt.plot(obs_time_vec_ST,
observations_ST[:, 0] - r_B_true_ST[:, 0],
label='$x_{star-B}$ Error')
plt.ylabel("$x_{star-B}$", size=18)
plt.legend()
def isObservable(self, X, t, params):
"""
Method that decides if an attitude is observable for a given time and star coordinates.
:param X: State.
:param t: Time.
:param params: Dynamic parameters (star coordinates).
:return: Boolean indicating if it is visible.
"""
period_obs = 1.0 / self._obs_rate
# It is assumed that the observations cannot be taken faster than obs_rate.
if self._last_obs == -1:
self._last_obs = t
elif self._last_obs == t: # It is still the same sampling interval
self._last_obs = t
elif self._last_obs + period_obs <= t:
self._last_obs = t
else:
return False
mrp1 = X[0]
mrp2 = X[1]
mrp3 = X[2]
mrp = np.array([mrp1, mrp2, mrp3])
star_number = params[0]
star_coord = self.getObserverCoordinates()
star_right_as = star_coord[star_number][2]
star_dec = star_coord[star_number][3]
cos_alpha = np.cos(star_right_as)
sin_alpha = np.sin(star_right_as)
cos_delta = np.cos(star_dec)
sin_delta = np.sin(star_dec)
# Inertial direction of the star (Unit vector!!!)
r_N = np.array(
[cos_delta * cos_alpha, cos_delta * sin_alpha, sin_delta])
# Body attitude B relative to inertial frame N
BN = attitudeKinematics.mrp2dcm(mrp)
# Body direction of the star (Unit vector!!!)
r_B = BN.dot(r_N)
st_nmbr = len(self._starTrackerDirecBody) # Number of Star Trackers
for i in range(0, st_nmbr):
b_ST = self._starTrackerDirecBody[
i] # Direction of the camera of the star
fov_ST = self._fov[i]
angle = np.arccos(r_B.dot(
b_ST)) # angle between the star and the normal to the camera
if angle <= fov_ST / 2: # The star is observable
return True
return False
r_B_true_ST = np.zeros((nmbr_obs_ST, 3))
for i in range(0, nmbr_obs_ST):
star_observed = star_catalog_ST[observers_ST[i]]
star_RA = star_observed[2]
star_dec = star_observed[3]
cos_right_asc = np.cos(star_RA)
sin_right_asc = np.sin(star_RA)
cos_dec = np.cos(star_dec)
sin_dec = np.sin(star_dec)
r_N = np.array([cos_dec*cos_right_asc, cos_dec*sin_right_asc, sin_dec])
mrp = obs_true_states_ST[i,0:3]
BN = attitudeKinematics.mrp2dcm(mrp)
r_B_true_ST[i,:] = BN.dot(r_N)
plt.figure()
plt.subplot(311)
plt.plot(obs_time_vec_ST, r_B_true_ST[:,0], label='$x_{star-B}$ true')
plt.plot(obs_time_vec_ST, observations_ST[:,0], '.', label='$x_{star-B}$ observed')
plt.plot(obs_time_vec_ST, observations_ST[:,0]-r_B_true_ST[:,0], label='$x_{star-B}$ Error')
plt.ylabel("$x_{star-B}$", size=18)
plt.legend()
plt.subplot(312)
plt.plot(obs_time_vec_ST, r_B_true_ST[:,1], label='$y_{star-B}$ true')
plt.plot(obs_time_vec_ST, observations_ST[:,1], '.', label='$y_{star-B}$ observed')
plt.plot(obs_time_vec_ST, observations_ST[:,1]-r_B_true_ST[:,1], label='$y_{star-B}$ Error')
plt.ylabel("$y_{star-B}$", size=18)