def test_magnetometer(self): qBO = np.array([.0,0.,0.,1.]) v_w_BI_b = np.array([-3.9999, 4.8575, 0]) v_w_BO_b = v_w_BI_b + qnv.quatRotate(qBO,v_w_IO_o) state = np.hstack((qBO,v_w_BO_b)) sat = satellite.Satellite(state,12.05) v_pos_i = np.array([1e6,-2.03,-3.0]) v_vel_i = np.array([2.0e3,2.8,-73.2]) sat.setPos(v_pos_i) sat.setVel(v_vel_i) qOI = np.array([-0.70697614, -0.01353643, 0.70697824, 0.01353791]) Magi = qnv.quatRotate(qnv.quatInv(qOI),np.array((2.3e-6,3.4e-6,3.1e-6))) sat.setMag_i(Magi) self.assertTrue(np.allclose(sensor.magnetometer(sat),np.array((2.3e-6,3.4e-6,3.1e-6))))
# sensor reading
if (sensbool == 0):
# getting default sensor reading (zero noise in our case)
Advitiy.setSun_b_m(defblock.sunsensor(Advitiy))
Advitiy.setMag_b_m_p(Advitiy.getMag_b_m_c())
Advitiy.setMag_b_m_c(defblock.magnetometer(Advitiy))
Advitiy.setgpsData(defblock.gps(Advitiy))
Advitiy.setOmega_m(defblock.gyroscope(Advitiy))
# Advitiy.setJ2Data(defblock.J2_propagator(Advitiy))
if (sensbool == 1):
# getting sensor reading from models
Advitiy.setSun_b_m(sensor.sunsensor(Advitiy))
Advitiy.setMag_b_m_p(Advitiy.getMag_b_m_c())
Advitiy.setMag_b_m_c(sensor.magnetometer(Advitiy))
Advitiy.setOmega_m(sensor.gyroscope(Advitiy))
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
q_true_bo = testfunction.propogate_quaternion(w_true_bob, q_true_bo)
q_true_bo = q_true_bo / np.linalg.norm(q_true_bo)
w_true[i, :] = w_true_bob
q_true[i, :] = q_true_bo
Advitiy.setW_BO_b(w_true_bob)
Advitiy.setQ_BO(q_true_bo)
#N_u=np.random.multivariate_normal(np.array([0,0,0]),01e-4*np.eye(3))
#gyroVarBias[i,:] = gyroVarBias[i,:] + sigma_u*((0.1*model_step)**0.5)*N_u#
#Advitiy.setGyroVarBias(gyroVarBias[i,:])
Advitiy.setMag_i(m_magnetic_field_i[i, 1:4] * 1e-9)
mag, v_n_m = sensor.magnetometer(Advitiy)
Advitiy.setMag_b_m_c(mag)
Advitiy.setPos(m_sgp_output[i, 1:4] * 1e-6)
Advitiy.setVel(m_sgp_output[i, 4:7] * 1e-6)
#print(m_sgp_output[i,:])
q[i, :], p[i, :], x[i, :] = MEKFmaincode.estimator(Advitiy)
w_bias[i, :] = Advitiy.getGyroVarBias()
w_est_bias[i, :] = Advitiy.getGyroEstBias()
#print(w_est_bias[i,:])
#RMSE[i,:]=np.sqrt(((b-f[3:6])**2)/end)
q_kk = q[i, :]
P_k = p[i, :]
w_m_k = sensor.gyroscope(Advitiy)
w_gyro[i, :] = testfunction.w_bob_calc(