def getNextIteration(self, W, B, uSun, Qtrue):
self.setBM(B, Qtrue)
self.setWM(W, Qtrue)
self.setuSunM(uSun)
Qnump = self.QM.vec(
) + self.dQ() * self.dt # calcul de la nouvelle orientation
self.QM = Quaternion(*Qnump[:, 0])
self.t += self.dt
def addition(self, reducedState): # reducedState is a 9-dim column vector (3x3-dim column vectors)
alpha = np.linalg.norm(reducedState[0:3])
if alpha > 1.0:
print(alpha, "large angle")
aplha = 1.0
if alpha < 1e-4: #alpha environ égal à 0
direction = np.array([0.0,0.0,0.0]) #quaternion nul avec une direction quelconque
else:
direction = reducedState[0:3, 0] / alpha
C, S = np.cos(alpha / 2), np.sin(alpha / 2)
QuatDelta = Quaternion(C, direction[0] * S, direction[1] * S, direction[2] * S)
return State(self.Q * QuatDelta, self.W + reducedState[3:6], self.I, self.gyroBias + reducedState[6:9])
def __init__(self,
gyroModel,
magneticModel,
sunSensorModel,
dt,
Q0=Quaternion(1, 0, 0, 0)):
self.gyroModel = gyroModel
self.magneticModel = magneticModel
self.sunSensorModel = sunSensorModel
self.dt = dt
self.t = 0
self.QM = Q0
self.BM = None
self.WM = None
self.uSunM = None
def getNextIteration(self, M, dw, J, B, I):
# M : moment magnétique des MC exprimé dans Rv
# dw : vecteur accélération angulaire des RI exprimé dans Rv
# J : Moment d'inertie des RI
# I : Tenseur d'inertie du satellite exprimé dans Rv
# B : Vecteur champ magnétique environnant, exprimé dans Rr
self.L += self.dL(M, dw, J,
B) * self.dt # calcul du nouveau moment cinétique
W = self.Q.V2R(np.dot(np.linalg.inv(I), self.Q.R2V(
self.L))) # Vecteur rotation du satellite dans Rr
Qnump = self.Q.vec(
) + self.dQ(W) * self.dt # calcul de la nouvelle orientation
Qnump /= np.linalg.norm(Qnump)
self.Q = Quaternion(*Qnump[:, 0])
self.t += self.dt
return W
def __init__(self, dt, L0, Q0=Quaternion(1, 0, 0, 0)):
self.t = 0 # temps écoulé
self.dt = dt # time increment
self.Q = Q0 # quaternion de rotation de Rv par rapport à Rr
self.L = L0 # Moment cinétique du satellite dans Rr
B = environnement.getEnvironment() # dans le référentiel géocentrique
# on récupère le prochain vecteur rotation (on fait ube étape dans la sim)
W = sim.getNextIteration(M, dw, J, B, I)
# Sauvegarder les valeurs de simulation actuelles:
stab.setAttitude(sim.Q)
stab.setRotation(W)
stab.setMagneticField(B)
# Enregistrement de variables pour affichage
Wr.append(np.linalg.norm(W))
qs.append(sim.Q)
# Prise de la commande de stabilisation
dw, M = stab.getCommand(Quaternion(Qt[0][0], Qt[1][0], Qt[2][0],
Qt[3][0])) # dans Rv
U, M = hardW.getRealCommand(dw, M)
# affichage de données toute les 10 itérations
if nbit % 10 == 0:
print("W :", str(W[:, 0]), "|| norm :", str(np.linalg.norm(W)),
"|| dw :", str(dw[:, 0]), "|| B :", str(B[:, 0]), "|| Q :",
str(sim.Q.axis()[:, 0]), "|| M :", str(np.linalg.norm(M)))
# Actualisation de l'affichage graphique
b_vector.axis = 1e6 * vp.vector(B[0][0], B[1][0], B[2][0])
satellite.rotate(angle=np.linalg.norm(W) * dt,
axis=vp.vector(W[0][0], W[1][0], W[2][0]),
origin=vp.vector(10, 10, 10))
# Rate : réalise 25 fois la boucle par seconde
sys.path.append('..')
import filter as flt
import numpy as np
import os as os
import shutil as shutil
from scao.quaternion import Quaternion
os.chdir("../../tst/sim")
sys.path.append(os.getcwd())
print(os.getcwd())
if (not os.path.isfile('conf.py')):
shutil.copy2('conf.default.py', 'conf.py')
from conf import *
dim = 6 # 3 pour les quaternions, 3 pour la rotation
q0 = Quaternion(1,0,0,0)
W0 = np.array([[1.0],[0.0],[0.0]])
P0 = np.eye(dim)
Qcov = np.eye(dim)
Rcov = np.eye(6) # ATTENTION, dimension de la mesure ici, pas de l'état
dt = 1
I = np.diag((m * (ly ** 2 + lz ** 2) / 3, m * (lx ** 2 + lz ** 2) / 3,
m * (lx ** 2 + ly ** 2) / 3)) # Tenseur inertie du satellite
ukf = flt.UKF(q0,W0,I,gyroModel[0],P0,Qcov,Rcov,dt)
state = ukf.curState
#print(state)
################################
# Pour un passage de boucle
r_coil = 75e-4
U_max = 5
mu_rel = 31
J = 1 # moment d'inertie des RW
# r_coil, r_wire, n_coils, mu_rel, U_max
mgt_parameters = r_coil, r_wire, n_windings, mu_rel, U_max
#####
hardW = Hardware(mgt_parameters, 'custom coil')
# Initialisation du champ magnétique:
orbite.setTime(t)
environnement.setPosition(orbite.getPosition())
B = environnement.getEnvironment() # dans le référentiel du satellite
# Simulateur
Q0 = Quaternion(1, 0, 0, 0)
sim = Simulator(dt, L0, Q0)
Qt = Quaternion(1, 0, 0, 0) # Quaternion objectif
#Monde mesuré
mWorld = MeasuredWorld(gyroModel, magneticModel, dt, Q0)
# Algortihmes de stabilisation
stab = SCAO(PIDRW(RW_P, RW_dP, RW_D), PIDMT(MT_P, MT_dP, MT_D), SCAOratio,
I, J) # stabilisateur
sns.set(context='talk', style='white')
plt.rcParams["figure.figsize"] = (8, 4)
#####################
# Boucle principale #
def runStatisticalInvestigation(numbreOfOrbites=1, Nstat=50, doSCA=False, useUKF=True):
alpha_values = []
beta_values = []
epsGyro_values = []
for kkk in tqdm(range(Nstat), total=Nstat):
epsilon_gyro = 10 ** -(random() * 4 + 3) # entre 10^-2 et 10^-6
epsGyro_values.append(epsilon_gyro)
standard_deviation_gyro = np.array([[epsilon_gyro], [epsilon_gyro], [epsilon_gyro]])
gyroModel = (biais_gyro, standard_deviation_gyro, scaling_factor_gyro, drift_gyro)
W0 = 0 * np.array([[2 * (random() - 0.5)] for i in range(3)]) # rotation initiale dans le référentiel R_r
t = 0
dw = np.array([[0.], [0.], [0.]]) # vecteur de l'accélération angulaire des RW
M = np.array([[0.], [0.], [0.]]) # vecteur du moment magnétique des bobines
I = np.diag((m * (ly ** 2 + lz ** 2) / 3, m * (lx ** 2 + lz ** 2) / 3,
m * (lx ** 2 + ly ** 2) / 3)) # Tenseur inertie du satellite
L0 = np.dot(I, W0)
# Environnement
orbite = Orbit(omega, i, e, r_p, mu, tau)
environnement = Environment(B_model)
# Hardware
#####
# paramètres hardware
n_windings = 500
r_wire = 125e-6
r_coil = 75e-4
U_max = 5
mu_rel = 31
J = 1 # moment d'inertie des RW
# r_coil, r_wire, n_coils, mu_rel, U_max
mgt_parameters = r_coil, r_wire, n_windings, mu_rel, U_max
#####
hardW = Hardware(mgt_parameters, 'custom coil')
# Initialisation du champ magnétique:
orbite.setTime(t)
environnement.setPosition(orbite.getPosition())
LocalMagField_Rr = environnement.getEnvironment() # dans le référentiel du satellite
# Simulateur
Q0 = Quaternion(1, 0, 0, 0)
sim = Simulator(dt, L0, Q0)
Qt = Quaternion(1, 0, 0, 0) # Quaternion objectif
# Monde mesuré
mWorld = MeasuredWorld(gyroModel, magneticModel, dt, Q0)
# Algortihmes de stabilisation
stab = SCAO(PIDRW(RW_P, RW_dP, RW_D), PIDMT(MT_P, MT_dP, MT_D), SCAOratio, I, J) # stabilisateur
############################
# Initialisation du filtre #
############################
dimState = 9 # 3 pour les quaternions, 3 pour la rotation, 3 pour les biais
dimObs = 6
Rcov = np.eye(dimObs) # ATTENTION, dimension de la mesure ici, pas de l'état
Rcov[0:3, 0:3] *= epsilon_gyro ** 2
Rcov[3:6, 3:6] *= epsilon_mag ** 2
CovRot = 1e-2 # Paramètre à régler pour le bon fonctionnement du filtre
P0 = np.eye(dimState) * 1e-3
Qcov = np.zeros((dimState, dimState))
Qcov[0:3, 0:3] = 1e-12 * np.eye(3) # uncertainty on the evolution of the attitude
Qcov[3:6, 3:6] = CovRot * np.eye(3) # uncertainty on the evolution of the velocity
Qcov[6:9, 6:9] = 1e-12 * np.eye(3) # uncertainty on evolution of the biais
ukf = flt.UKF(Q0, W0, gyroModel[0], P0, Qcov, Rcov, dt)
#####################
# Boucle principale #
#####################
output = {'t': [], 'M': [], 'U': []}
outputB = {'t': [], 'B': [], 'BM': []}
outputUKF = {'Qs': [], 'Qm': [], "Qc": [], 'Ws': [], "Wm": [], "Wc": [], "P": []}
timesteps = np.arange(t, numbreOfOrbites * orbite.getPeriod(), dt)
# for t in tqdm(timesteps):
for t in timesteps:
# on récupère la valeur actuelle du champ magnétique et on actualise l'affichage du champ B
orbite.setTime(t) # orbite.setTime(t)
environnement.setPosition(orbite.getPosition())
LocalMagField_Rr = environnement.getEnvironment() # dans le référentiel géocentrique
# on récupère le prochain vecteur rotation (on fait ube étape dans la sim)
W = sim.getNextIteration(M, dw, J, LocalMagField_Rr, I)
# Update monde mesuré
mWorld.getNextIteration(W, LocalMagField_Rr, sim.Q)
if useUKF:
# Correction des erreurs avec un filtre
ukf.errorCorrection(mWorld.WM, mWorld.BM, LocalMagField_Rr)
# Sauvegarder les valeurs de simulation actuelles: (valeurs mesurées)
stab.setAttitude(ukf.curState.Q)
stab.setRotation(ukf.curState.W)
else:
# Sauvegarder les valeurs de simulation actuelles: (valeurs mesurées)
stab.setAttitude(mWorld.QM)
stab.setRotation(mWorld.WM)
stab.setMagneticField(sim.Q.V2R(mWorld.BM)) # non recalé
# Prise de la commande de stabilisation
if doSCA:
dw, M = stab.getCommand(Qt) # dans Rv
U, M = hardW.getRealCommand(dw, M)
# output['t'].append(t)
# output['M'].append(M)
# output['U'].append(U)
outputUKF['Ws'].append(W)
# outputUKF['Wm'].append(mWorld.WM)
# outputUKF['Wc'].append(ukf.curState.W)
outputUKF['Qs'].append(sim.Q)
# outputUKF['Qm'].append(mWorld.QM)
# outputUKF['Qc'].append(ukf.curState.Q)
# outputUKF['P'].append(ukf.P)
# outputB['t'].append(t)
# outputB['B'].append(sim.Q.R2V(LocalMagField_Rr))
# outputB['BM'].append(mWorld.BM)
index = len(timesteps) // 10 # take the last 10%
angles = [abs(quat.angle()) for quat in outputUKF['Qs'][-index:]]
velocities = [np.linalg.norm(w) for w in outputUKF['Ws'][-index:]]
alpha_max = max(angles)
beta_max = max(velocities)
alpha_values.append(alpha_max)
beta_values.append(beta_max)
return alpha_values, beta_values, epsGyro_values