def test_gyroscope(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) sat.setGyroVarBias(np.array((-0.1, 0.1, 0.1))) self.assertTrue(np.allclose(sensor.gyroscope(sat),np.array((-4.0999,4.9575,0.1))))
def estimator(sat):
sat.setGyroVarBias(np.array([0,0,0]))
v_mag_o=sat.getMag_o()
v_mag_b_m=sat.getMag_b_m_c()
w_m_k=sensor.gyroscope(sat)
w_oio=-v_w_IO_o
delta_b=testfunction.delta_b_calc(b,b_e)
w_bob=testfunction.w_bob_calc(w_m_k,q_kk,w_oio,b_e)
#print(w_bob)
phi=testfunction.phi_calc(w_bob)
#print(phi)
delta_x=testfunction.delta_x_calc(q_kk,delta_b)
#print(delta_x)
q_k1k=testfunction.propogate_quaternion(w_bob,q_kk)
#print(q_k1k)
x_k1k=testfunction.propogate_state_vector(phi,delta_x)
#print(x_k1k)
P_k1k=testfunction.propogate_covariance(phi,P_k)
#print(P_k1k)
v_mag_b_e=testfunction.calc_v_mag_b_e(v_mag_b_m,v_mag_o,q_k1k)
#print(v_mag_b_e)
y=testfunction.calc_y(v_mag_b_m,v_mag_b_e)
#print(y)
M_m=testfunction.calc_M_m(v_mag_b_e,q_k1k)
#print(M_m)
K=testfunction.calc_K(P_k1k,M_m,R)
#print("K=")
#print(K)
x_k1k1=testfunction.update_state_vector(K,y,x_k1k,M_m)
#print("x=" )
#print(x_k1k1)
P_k1k1=testfunction.update_covariance(I6,K,M_m,P_k1k,R)
#print("p=")
#print(P_k1k1)
q_k1k1=testfunction.update_quaternion(x_k1k,q_kk)
#print("qk1k1")
#print(q_k1k1)
return q_k1k1, P_k1k1, x_k1k1
#print("result")
# 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
if (estbool == 0): # qBO is same as obtained by integrator
m_sgp_output = np.genfromtxt('sgp_output.csv', delimiter=",")
m_magnetic_field_i = np.genfromtxt('mag_output_i.csv', delimiter=",")
#Advitiy = satellite.Satellite(v_state0,t0)
position = np.zeros((end, 3))
velocity = np.zeros((end, 3))
w_est = velocity.copy()
w_gyro = velocity.copy()
w_true = velocity.copy()
q = np.zeros((end, 4))
RMSE = np.zeros((end, 3))
p = np.zeros((end, 6, 6))
x = np.zeros((end, 6))
xmod = np.zeros((end))
for i in range(end):
Advitiy.setGyroVarBias(np.array([0.001, 0.001, 0.001]))
w_m_k = sensor.gyroscope(Advitiy)
#w_t_k=satellite.getW_BO_b(Advitiy)
#w_t_bib=satellite.getW_BI_b(Advitiy)
Advitiy.setMag_i(m_magnetic_field_i[i, 1:4] * 1e-9)
Advitiy.setMag_b_m_c(sensor.magnetometer(Advitiy))
Advitiy.setPos(m_sgp_output[i, 1:4] * 1e-6)
Advitiy.setVel(m_sgp_output[i, 4:7] * 1e-6)
position[i] = Advitiy.getPos()
velocity[i] = Advitiy.getVel()
#print(i)
b_e = b - x[i - 1, 3:6]
#print(b_e)
q[i, :], p[i:], f = MEKFmaincode.estimator(Advitiy)
x[i, :] = f
RMSE[i, :] = np.sqrt(((b - f[3:6])**2) / end)
q_kk = q[i, :]