if (propbool == 0):
# Using sgp to propagate
Advitiy.setgpsData(sensor.GPS(Advitiy))
if (propbool == 1):
# Use J2 to propagate
pos = Advitiy.getPos_J2()
vel = Advitiy.getVel_J2()
Advitiy.setPos_J2(propagate(pos, vel, time_J2, 0.5, drag=False)[0])
Advitiy.setVel_J2(propagate(pos, vel, time_J2, 0.5, drag=False)[1])
# Advitiy.setJ2Data(sensor.J2_propagator(Advitiy))
# Estimated quaternion
if (estbool == 0): # qBO is same as obtained by integrator
Advitiy.setQUEST(defblock.estimator(Advitiy))
# if (estbool == 1): #qBO is obtained using Quest/MEKF
# control torque
if (contcons == 0):
# getting default control torque (zero in our case)
Advitiy.setControl_b(defblock.controller(Advitiy))
if (contcons == 1):
# getting control torque by omega controller
# k = 0.001
torque_control[i * int(Nmodel / Ncontrol):(i + 1) * (int(Nmodel / Ncontrol)), :] \
= -0.001 * Advitiy.getW_BI_b()
# print(Advitiy.getW_BI_b())
def test_estimator(self): qBO = np.array([.414,0.5,-0.5,0.]) state = np.hstack((qBO,np.array([0.10050588,-0.05026119,-0.3014887]))) mySat = satellite.Satellite(state,128.05) self.assertTrue(np.allclose(defblock.estimator(mySat),qBO))