__all__ = ["MTQ_w_RW"]
import numpy as np
from typing import List
from ADCS.CONOPS.goals import Goal
from ADCS.controller import Controller
from ADCS.controller.helpers.quaternion_math import vector_alignment_error
from ADCS.satellite_hardware.satellite.estimated_satellite import EstimatedSatellite
from ADCS.orbits.orbital_state import Orbital_State
from ADCS.satellite_hardware.actuators import Actuator, MTQ, RW
from ADCS.satellite_hardware.sensors import MTM
from ADCS.helpers.math_helpers import rot_mat, normalize, skewsym, limit, norm
[docs]
class MTQ_w_RW(Controller):
r"""
Hybrid magnetic torquer and reaction wheel attitude controller.
This controller implements a three-axis attitude determination and control
system that combines reaction wheels (RW) for precise torque generation
with magnetic torquers (MTQ) for continuous reaction wheel momentum
unloading. The formulation follows the principles of redundant reaction
wheel momentum management described by Hogan and Schaub [1]_.
Reaction wheels provide high-bandwidth torque authority, while magnetic
torquers exploit the geomagnetic field to generate torques orthogonal to
the magnetic field direction. Momentum unloading is performed continuously
to prevent reaction wheel saturation.
The total commanded body torque is decomposed as:
.. math::
\boldsymbol{\tau}_{\text{total}} =
\boldsymbol{\tau}_{\text{rw}} +
\boldsymbol{\tau}_{\text{mag}}.
Attitude control torque is computed using a proportional-derivative
feedback law with gyroscopic compensation:
.. math::
\boldsymbol{\tau}_{\text{att}} =
-K_p \boldsymbol{q}_{\text{err}}
-K_d (\boldsymbol{\omega} - \boldsymbol{\omega}_{\text{ref}})
+ \boldsymbol{\tau}_{\text{gyro}}.
Gyroscopic coupling from the spacecraft body and reaction wheels is:
.. math::
\boldsymbol{\tau}_{\text{gyro}} =
\boldsymbol{\omega} \times
(J \boldsymbol{\omega} + \boldsymbol{h}_{\text{rw}}).
Reaction wheel momentum unloading torque is defined as:
.. math::
\boldsymbol{\tau}_{\text{dump}} =
-K_c (\boldsymbol{h}_{\text{rw}} - \boldsymbol{h}_{\text{tgt}}).
Magnetic torque is generated by a commanded dipole moment interacting
with the geomagnetic field:
.. math::
\boldsymbol{\tau}_{\text{mag}} =
\boldsymbol{m} \times \boldsymbol{B} =
-[B]_{\times} A_{\text{mtq}} \boldsymbol{u}_{\text{mtq}}.
The remaining torque demand is allocated to reaction wheels:
.. math::
\boldsymbol{\tau}_{\text{rw}} =
\boldsymbol{\tau}_{\text{att}} -
\boldsymbol{\tau}_{\text{mag}}.
Parameters
----------
est_sat : :class:`~ADCS.satellite_hardware.satellite.estimated_satellite.EstimatedSatellite`
Estimated satellite model containing inertia properties, actuator
geometry, and sensor configuration.
p_gain : float
Proportional gain :math:`K_p` applied to the quaternion error.
d_gain : float
Derivative gain :math:`K_d` applied to angular rate error.
c_gain : float
Momentum dumping gain :math:`K_c`.
h_target : numpy.ndarray
Desired body-frame reaction wheel momentum target vector.
Raises
------
ValueError
If any component of ``h_target`` exceeds the maximum allowable
reaction wheel momentum defined by
:class:`~ADCS.satellite_hardware.actuators.RW`.
Notes
-----
This controller assumes that reaction wheel momentum states are stored
contiguously in the estimated state vector beginning at index 7 and that
the ordering matches the internal actuator index ordering.
References
----------
.. [1] E. A. Hogan and H. Schaub,
Three-Axis Attitude Control Using Redundant Reaction Wheels
with Continuous Momentum Dumping,
Journal of Guidance, Control, and Dynamics,
Vol. 44, No. 3, 2021.
"""
def __init__(self, est_sat: EstimatedSatellite, p_gain: float, d_gain: float, c_gain: float, h_target: np.ndarray) -> None:
r"""
Initialize the hybrid MTQ and reaction wheel controller.
This method precomputes pseudo-inverse allocation matrices for
reaction wheels and magnetic torquers, extracts actuator limits,
and validates the desired reaction wheel momentum target.
Parameters
----------
est_sat : :class:`~ADCS.satellite_hardware.satellite.estimated_satellite.EstimatedSatellite`
Estimated satellite object providing actuator, sensor, and
inertia configuration.
p_gain : float
Proportional attitude control gain.
d_gain : float
Derivative angular rate control gain.
c_gain : float
Reaction wheel momentum unloading gain.
h_target : numpy.ndarray
Desired reaction wheel momentum target vector in body frame.
Returns
-------
None
"""
self.p_gain = p_gain
self.d_gain = d_gain
self.c_gain = c_gain
self.M_mtm_read, self.mtm_indices = self.build_sensor_matrix_pinv(sensors=est_sat.sensors+est_sat.rw_actuators, sensor_type=MTM)
self.M_rw_act, self.rw_indices = self.build_torque_to_u_matrix_pinv(actuators=est_sat.actuators, actuator_type=RW)
self.M_mtq_act, self.mtq_indices = self.build_torque_to_u_matrix_pinv(actuators=est_sat.actuators, actuator_type=MTQ)
self.A_mtq = self.build_u_to_torque_matrix_pinv(actuators=est_sat.actuators, actuator_type=MTQ)
self.rw_max_h = np.asarray([rw.h_max for rw in est_sat.actuators if isinstance(rw, RW)])
self.max_torque = self.find_max_torque(actuators=est_sat.actuators)
if np.any(self.rw_max_h < h_target):
raise ValueError("Target momentum cannot be higher than reaction wheel maximum momentum!")
self.h_target = h_target
self.n_actuators = len(est_sat.actuators)
[docs]
def find_u(self, x_hat: np.ndarray, sens: np.ndarray, est_sat: EstimatedSatellite, os_hat: Orbital_State, goal: Goal | None = None) -> np.ndarray:
r"""
Compute actuator command vector for reaction wheels and magnetic torquers.
This method evaluates the estimated spacecraft state, reconstructs
reaction wheel momentum, computes attitude control torque, performs
gyroscopic compensation, and allocates torque between reaction wheels
and magnetic torquers while enforcing actuator limits.
Reaction wheel momentum in the body frame is reconstructed as:
.. math::
\boldsymbol{h}_{\text{rw}} =
\sum_i h_i \hat{\boldsymbol{a}}_i.
Attitude control torque is computed as:
.. math::
\boldsymbol{\tau}_{\text{att}} =
-K_p \boldsymbol{q}_{\text{err}}
-K_d (\boldsymbol{\omega} - \boldsymbol{\omega}_{\text{ref}})
+ \boldsymbol{\tau}_{\text{gyro}}.
Magnetic torquer allocation uses a pseudo-inverse mapping:
.. math::
\boldsymbol{u}_{\text{mtq}} =
M_{\text{mag}}^{+} \boldsymbol{\tau}_{\text{dump}}.
Reaction wheel commands are computed from the remaining torque demand:
.. math::
\boldsymbol{u}_{\text{rw}} =
M_{\text{rw}} \boldsymbol{\tau}_{\text{rw}}.
Parameters
----------
x_hat : numpy.ndarray
Estimated spacecraft state vector containing angular rates,
quaternion attitude, and reaction wheel momentum states.
sens : numpy.ndarray
Raw sensor measurement vector used to reconstruct the magnetic
field via MTM pseudo-inverse mapping.
est_sat : :class:`~ADCS.satellite_hardware.satellite.estimated_satellite.EstimatedSatellite`
Satellite model containing inertia and actuator configuration.
os_hat : :class:`~ADCS.orbits.orbital_state.Orbital_State`
Estimated orbital state providing geomagnetic field information.
goal : :class:`~ADCS.CONOPS.goals.Goal`, optional
Guidance goal object providing reference attitude and angular
rate.
Returns
-------
numpy.ndarray
Full actuator command vector ordered according to
``est_sat.actuators``.
"""
w = x_hat[0:3]
q = x_hat[3:7]
if goal is None:
goal = Goal()
goal_vector_eci, w_ref_eci = goal.to_ref(os0=os_hat)
sens_clean = sens.copy()
sens_clean[np.isnan(sens_clean)] = 0.0
b_body = np.asarray(self.M_mtm_read @ sens_clean, float).reshape(3,)
boresight = est_sat.get_boresight(goal.boresight_name)
q_err_vec = goal.error(q=q, body_boresight=boresight, os0=os_hat)
# q_err_vec = vector_alignment_error(q=q, eci_goal=goal_vector_eci, body_boresight=est_sat.boresight)
R_b2i = rot_mat(q)
w_ref_body = R_b2i.T @ w_ref_eci
w_err = w - w_ref_body
# PD Control Law
tau_att = -self.p_gain*q_err_vec - self.d_gain*w_err
# Momentum Management
h_vals = x_hat[7:]
rw_axes = np.vstack([
np.asarray(rw.axis, float).reshape(3,)
for rw in est_sat.actuators
if isinstance(rw, RW)
])
h_vals = x_hat[7:]
h_rw_body = h_vals @ rw_axes
# Gyroscopic Compensation
J = est_sat.J_0
tau_gyro = np.cross(w, J @ w + h_rw_body)
tau_att += tau_gyro
# Momentum dumping
delta_h = h_rw_body - self.h_target
tau_dump = -self.c_gain * delta_h
# Allocation
B_skew = skewsym(b_body)
M_mag_eff = -B_skew @ self.A_mtq # (3, N_MTQ)
u_mtq = np.linalg.pinv(M_mag_eff) @ tau_dump
u_mtq = limit(u=u_mtq, umax=self.max_torque) #TODO: Proper limiting of Actuators
tau_mag_actual = M_mag_eff @ u_mtq
tau_rw_req = tau_att - tau_mag_actual
u_rw = self.M_rw_act @ tau_rw_req
u_rw = limit(u=u_rw, umax=self.max_torque)
u_total = u_mtq+u_rw
return u_total