def gyroscope(sat):
v_w_BIB = sat.getW_BI_b()
#print("v_w_BIB1",v_w_BIB)
v_bias_var_k = sat.getGyroVarBias()
v_bias_var_k1 = v_bias_var_k + sigma_u*(h**0.5)*mvg(GYRO_F_BIAS,np.eye(3))
v_w_BIB_m = v_w_BIB + 0.5*(v_bias_var_k1+v_bias_var_k) + mvg(GYRO_F_BIAS,np.eye(3))*(sigma_v**2/h+(sigma_u**2)*h/12)**0.5#mvg(GYRO_F_BIAS,GYRO_F_COV) ##v_bias_var needs to be corrected
sat.setGyroVarBias(v_bias_var_k1)
return v_w_BIB_m
def ADC(sun_vector):
#works as ADC, quantizes the readings of sensor, for more you can refer to documentation of this code.
#input: sun vector in body frame, normal vectors of each sunsensor, sunsensor gain, quantizer
#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)
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
m_normalVectors[0, :] = v_S1
m_normalVectors[1, :] = v_S2
m_normalVectors[2, :] = v_S3
m_normalVectors[3, :] = v_S4
m_normalVectors[4, :] = v_S5
m_normalVectors[5, :] = v_S6
v_output = np.zeros([6]) #vector of output of each sensor
for iter in range(0, 6):
v = np.dot(sun_vector, m_normalVectors[iter, :])
if v < 0:
v = 0 #if v < 0 that means the sunvector is making obtuse angle with the sensor which means the sensor is in dark
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
return v_output
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)
return v_w_BIB_m
def GPS(sat):
#model the GPS
#input : satellite class object
#output : measured position, velocity, time
#to add more outputs, add them to the hstack command and define any new constants in the constants file
v_pos = sat.getPos()
v_vel = sat.getVel()
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)
return np.hstack([v_pos_m, v_vel_m, time_m])
def magnetometer(sat):
#model the magnetometer
#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
return v_B_m
def sample(self, points, num=1):
n = len(points)
if self.num_points > 0:
kyy = self.gen_covmat(points, points)
kyx = self.gen_covmat(points, self.x)
kxy = self.gen_covmat(self.x, points)
kxx = np.linalg.inv(self.kxx)
covmat = kyy - kyx * kxx * kxy
y = np.transpose([self.y])
mean = kyx * kxx * y
mean = np.array(np.transpose(mean))[0]
else:
covmat = self.gen_covmat(points, points)
mean = np.zeros(n)
return mvg(mean, covmat, num)
def sample(self, points, num=1):
n = len(points)
if self.num_points > 0:
kyy = self.gen_covmat(points, points)
kyx = self.gen_covmat(points, self.x)
kxy = self.gen_covmat(self.x, points)
kxx = np.linalg.inv(self.kxx)
covmat = kyy - kyx*kxx*kxy
y = np.transpose([self.y])
mean = kyx*kxx*y
mean = np.array(np.transpose(mean))[0]
else:
covmat = self.gen_covmat(points, points)
mean = np.zeros(n)
return mvg(mean, covmat, num)
def sample(self, points, num=1):
n = len(points)
if self.num_points > 0:
kyy = self.gen_covmat(points, points)
kyx = self.gen_covmat(points, self.x) # points is the point where we will evaulate its vaule y is row x is col
kxy = self.gen_covmat(self.x, points)
kxx = np.linalg.inv(self.kxx)
covmat = kyy - kyx*kxx*kxy
y = np.transpose([self.y])
mean = kyx*kxx*y
mean = np.array(np.transpose(mean))[0]
else:
covmat = self.gen_covmat(points, points)
mean = np.zeros(n)
return mvg(mean, covmat, num)
def sample(self, points, num=1):
n = len(points)
if self.num_points > 0:
kyy = self.gen_covmat(points, points)
kyx = self.gen_covmat(
points, self.x
) # points is the point where we will evaulate its vaule y is row x is col
kxy = self.gen_covmat(self.x, points)
kxx = np.linalg.inv(self.kxx)
covmat = kyy - kyx * kxx * kxy
y = np.transpose([self.y])
mean = kyx * kxx * y
mean = np.array(np.transpose(mean))[0]
else:
covmat = self.gen_covmat(points, points)
mean = np.zeros(n)
return mvg(mean, covmat, num)
"""
import sensor
import satellite as sat
from constants import *
import matplotlib.pyplot as plt
from numpy.random import multivariate_normal as mvg
sat = satellite.Satellite(v_state0, v_glob_est_state0, v_loc_est_state0, t0)
N = 10000
h = 0.1
gyroVarBias0 = np.zeros(3)
sigma_u = 0.0001
sat.setGyroVarBias(gyroVarBias0)
var_bias = np.zeros((N, 3))
time = np.zeros(N)
t = 0
for i in range(N):
v_bias_var_k = sat.getGyroVarBias()
v_bias_var_k1 = v_bias_var_k + sigma_u * (h**0.5) * mvg(
GYRO_F_BIAS, np.eye(3))
var_bias[i, :] = v_bias_var_k1
t = t + h
time[i] = t
sat.setGyroVarBias(v_bias_var_k1)
plt.plot(time, var_bias)
