Corrected the errors present in sensor.py

Author: rsriya

constants_1U.py

@@ -101,21 +101,22 @@
 SS_THRESHOLD = 0.5
 
 ADC_BIAS = np.array([0,0,0,0,0,0])
-ADC_COV = np.array([0,0,0,0,0,0])
+ADC_COV = 0.01*np.identity(6)
+
 #GPS (random values)
-GPS_POS_COV = np.array([0,0,0])
-GPS_VEL_COV = np.array([0,0,0])
-GPS_TIME_COV = np.array([0,0,0])
 GPS_POS_BIAS = np.array([0,0,0])
 GPS_VEL_BIAS = np.array([0,0,0])
-GPS_TIME_BIAS = 0
+GPS_TIME_BIAS = np.array([0])
+GPS_POS_COV = np.identity(3)
+GPS_VEL_COV = np.identity(3)
+GPS_TIME_COV = np.array([[0]])
 
 #Magnetometer (random values)
 MAG_BIAS = np.array([0,0,0])
-MAG_COV = np.array([0,0,0])
+MAG_COV = 1e-9*np.identity(3)
 
 #Gyroscope (random values)
 GYRO_F_BIAS = np.array([0,0,0])
-GYRO_F_COV = np.array([0,0,0])
+GYRO_F_COV = 1e-9*np.identity(3)
 
 k_detumbling = 4*np.pi*(1+sin(radians(Inclination-11)))*Jmin/TimePeriod    #gain constant in B_dot controller (from book by F. Landis Markley)

sensor.py

@@ -1,7 +1,6 @@
 from constants_1U import *
 import frames as fs
 import numpy as np
-from numpy import nultivariate_normal as mvg
 import qnv
 
 def ADC(sun_vector):
@@ -11,7 +10,7 @@ def ADC(sun_vector):
     #sunsensor gain : multiplying factor that converts dot product of sunvector with normal to voltage
     #output : 6 voltages, 1 per sunsensor
 
-    u=1/(SS_QUANTIZER-1)
+    u=1/(SS_QUANTIZER-1) #Reference for this
 
     m_normalVectors = np.zeros([6,3]) #matrix of normal vectors of sensors
     #S1 and S2 are opposite, S3 and S4 are opposite, S5 and S6 are opposite
@@ -31,7 +30,7 @@ def ADC(sun_vector):
         v=(u)*(round(v/u))*SS_GAIN #conversion from dot product to voltage
         v_output[iter] = v
 
-    v_output = v_output + mvg(ADC_BIAS,ADC_COV) #add error to true quantity
+    v_output = v_output + np.random.multivariate_normal(ADC_BIAS,ADC_COV) #add error to true quantity
 
     return v_output
 
@@ -81,9 +80,9 @@ def calc_SV(ss):
 
 def sunsensor(sat):
 
-    v_q_BO = sat.getQ_BO()    
-    v_s_o = sat.getSun_o() #obtain sunvector in orbit frame
-    v_s_b = qnv.quatRotate(v_q_BO,v_s_o) #obtain sunvector in body frame
+    v_q_BI = sat.getQ_BI()    
+    v_s_i = sat.getSun_i() #obtain sunvector in orbit frame
+    v_s_b = qnv.quatRotate(v_q_BI,v_s_i) #obtain sunvector in body frame
     ss_read = ADC(v_s_b) #find reading per sunsensor, error already incorporated in ADC code
     v_sun_b_m = calc_SV(ss_read) #calculate sunvector from sunsensor readings
 
@@ -101,9 +100,9 @@ def GPS(sat):
     time = sat.getTime()
 
     #add errors to true quantities
-    v_pos_m = v_pos + mvg(GPS_POS_BIAS,GPS_POS_COV)
-    v_vel_m = v_vel + mvg(GPS_VEL_BIAS,GPS_VEL_COV)
-    time_m = time + mvg(GPS_TIME_BIAS,GPS_TIME_COV)
+    v_pos_m = v_pos + np.random.multivariate_normal(GPS_POS_BIAS,GPS_POS_COV)
+    v_vel_m = v_vel + np.random.multivariate_normal(GPS_VEL_BIAS,GPS_VEL_COV)
+    time_m = time + np.random.multivariate_normal(GPS_TIME_BIAS,GPS_TIME_COV)
 
     return np.hstack([v_pos_m,v_vel_m,time_m])
 
@@ -112,16 +111,16 @@ def magnetometer(sat):
     #input : satellite class object
     #output : measured magnetic field
 
-    v_q_BO = sat.getQ_BO() 
-    v_B_o = sat.getMag_o() #obtain magnetic field in orbit frame
-    v_B_b = qnv.quatRotate(v_q_BO,v_B_o) #obtain magnetic field in body frame
-    v_B_m = v_B_b + mvg(MAG_BIAS,MAG_COV) #add errors
+    v_q_BI = sat.getQ_BI() 
+    v_B_i = sat.getMag_i() #obtain magnetic field in orbit frame
+    v_B_b = qnv.quatRotate(v_q_BI,v_B_i) #obtain magnetic field in body frame
+    v_B_m = v_B_b + np.random.multivariate_normal(MAG_BIAS,MAG_COV) #add errors
     return v_B_m
 
 def gyroscope(sat):
 
     v_w_BIB = sat.getW_BI_b()
     v_bias_var = sat.getGyroVarBias()
-    v_w_BIB_m = v_w_BIB + v_bias_var + mvg(GYRO_F_BIAS,GYRO_F_COV)
+    v_w_BIB_m = v_w_BIB + v_bias_var + np.random.multivariate_normal(GYRO_F_BIAS,GYRO_F_COV)
 
     return v_w_BIB_m

test_solver.py

@@ -0,0 +1,67 @@
+from solver import updateStateTimeRK4
+import numpy as np
+import frames
+import unittest
+import satellite
+from dynamics import x_dot_BO
+from constants_1U import G, M_EARTH, v_w_IO_o
+from ddt import data, ddt
+
+def f_constant(sat):
+	cons = np.array((1.,1.,1.,1.,1.,1.,1.))
+	return cons
+@ddt
+class TestSolver(unittest.TestCase):
+	
+	def test_constant(self):
+		t0 = 0.0
+		mySat = satellite.Satellite(np.array([1.0,0.,0.,0.,0.,0.,0.]),t0)
+		t = 10
+		mySat.setPos(np.array([1e6,0.,0.]))
+		mySat.setVel(np.array([5.0,5.0,0.0]))
+		h = 0.1
+		for i in range(0,int(t/h)):
+			updateStateTimeRK4(mySat,f_constant,h)
+			mySat.setTime(t0+(i+1)*h)
+		self.assertTrue(np.allclose([10.0,10.0,10.0],mySat.getW_BO_b()))
+
+	@data(0.001,0.005,0.05,0.1,0.5)	
+	def test_dynamics(self,value):
+		t0 = 0.
+		h = value
+		
+		v_q_BO = np.array([0.4,0.254,-0.508,0.71931912])
+		
+		v_w_BO_b = frames.wBIb2wBOb(np.array([0.1,-0.05,-0.3]),v_q_BO,v_w_IO_o)
+		
+		mySat1 = satellite.Satellite(np.hstack((v_q_BO, v_w_BO_b)),t0)
+		
+		mySat1.setPos(np.array([1e6,53e5,0.]))
+		mySat1.setVel(np.array([5.60,-5.0,0.0]))
+		mySat1.setDisturbance_b(np.array([10e-10,-4e-6,-3e-5]))
+		mySat1.setControl_b(np.array([1e-5,1e-5,-8e-4]))
+		
+		updateStateTimeRK4(mySat1,x_dot_BO,h)
+		x1 = mySat1.getState()
+
+		mySat2 = satellite.Satellite(np.hstack((v_q_BO, v_w_BO_b)),t0)
+		
+		mySat2.setPos(np.array([1e6,53e5,0.]))
+		mySat2.setVel(np.array([5.60,-5.0,0.0]))
+		mySat2.setDisturbance_b(np.array([10e-10,-4e-6,-3e-5]))
+		mySat2.setControl_b(np.array([1e-5,1e-5,-8e-4]))
+		
+		mySat2.setState(np.hstack((v_q_BO,v_w_BO_b)))		
+		k1 = h*x_dot_BO(mySat2)
+		mySat2.setState(np.hstack((v_q_BO,v_w_BO_b))+0.5*k1)
+		k2 = h*x_dot_BO(mySat2)
+		mySat2.setState(np.hstack((v_q_BO,v_w_BO_b))+0.5*(k2))
+		k3 = h*x_dot_BO(mySat2)
+		mySat2.setState(np.hstack((v_q_BO,v_w_BO_b))+k3)
+		k4 = h*x_dot_BO(mySat2)
+		
+		error_state = np.hstack((v_q_BO,v_w_BO_b)) + (1./6.)*(k1 + 2.*k2 + 2.*k3 + k4)
+		self.assertTrue(np.allclose(error_state,x1))
+	
+if __name__=="__main__":
+	unittest.main(verbosity=2)