Source code for ADCS.controller.bdot
__all__ = ["BDot"]
import numpy as np
from typing import List
from ADCS.CONOPS.goals import Goal
from ADCS.controller.controller import Controller
from ADCS.satellite_hardware.satellite.estimated_satellite import EstimatedSatellite
from ADCS.satellite_hardware.sensors import Sensor, MTM
from ADCS.satellite_hardware.actuators import Actuator, MTQ
from ADCS.orbits.orbital_state import Orbital_State
from ADCS.orbits.universal_constants import TimeConstants
from ADCS.helpers.math_helpers import limit
[docs]
class BDot(Controller):
r"""
Implements the B-Dot magnetic detumbling controller.
This class provides an implementation of the classical B-Dot control law,
which is commonly used during the early mission phase of a satellite to
reduce angular velocity after deployment. The controller relies exclusively
on magnetometer measurements and magnetorquer actuators, exploiting the
interaction between the spacecraft magnetic dipole moment and the Earth's
geomagnetic field.
The B-Dot controller is stateless with respect to the satellite attitude and
angular velocity estimates and instead uses the time derivative of the
measured magnetic field expressed in the body frame.
Control Law
The requested magnetic dipole moment is given by
.. math::
\mathbf{m}_{\mathrm{req}} = -K \, \dot{\mathbf{B}}
where
+----------------------+-------------------------------------------------+
| Symbol | Description |
+======================+=================================================+
| \mathbf{m} | Magnetic dipole moment (A m^2) |
+----------------------+-------------------------------------------------+
| K | Positive scalar control gain |
+----------------------+-------------------------------------------------+
| \mathbf{B} | Magnetic field vector in body frame (T) |
+----------------------+-------------------------------------------------+
The magnetic torque generated by the magnetorquers is
.. math::
\boldsymbol{\tau} = \mathbf{m} \times \mathbf{B}
Substituting the B-Dot control law yields
.. math::
\boldsymbol{\tau} = -K \left( \dot{\mathbf{B}} \times \mathbf{B} \right)
For a tumbling satellite, the dominant contribution to the magnetic field
derivative is due to body rotation:
.. math::
\dot{\mathbf{B}} \approx \boldsymbol{\omega} \times \mathbf{B}
which produces a dissipative torque opposing angular velocity components
perpendicular to the geomagnetic field.
Hardware Dependencies
This controller requires the following hardware interfaces to be defined in
:class:`~ADCS.satellite_hardware.satellite.estimated_satellite.EstimatedSatellite`:
1. At least one magnetometer of type
:class:`~ADCS.satellite_hardware.sensors.MTM`
2. At least one magnetorquer of type
:class:`~ADCS.satellite_hardware.actuators.MTQ`
Sensor and Actuator Mapping
Internally, the controller constructs:
* A sensor reconstruction matrix using the Moore–Penrose pseudoinverse to map
raw magnetometer measurements to a three-dimensional magnetic field vector
* An actuation matrix to map desired magnetic dipole moments to actuator
commands, allowing for redundant or non-orthogonal magnetorquers
"""
def __init__(self, est_sat: EstimatedSatellite, gain: float) -> None:
r"""
Initializes the B-Dot controller.
This constructor builds the required sensor and actuator mapping matrices,
initializes internal state variables for numerical differentiation of the
magnetic field, and determines actuator saturation limits based on the
available magnetorquers.
Sensor mapping is performed by constructing a reconstruction matrix that
maps raw magnetometer measurements to a three-dimensional magnetic field
vector. Actuator mapping is performed by constructing a pseudoinverse
matrix that maps desired magnetic dipole moments to actuator command
vectors.
:param est_sat: Estimated satellite object containing sensors and actuators
:type est_sat: ~ADCS.satellite_hardware.satellite.estimated_satellite.EstimatedSatellite
:param gain: Proportional B-Dot control gain K
:type gain: float
:return: None
:rtype: None
"""
self.gain = gain
# Sensor Reading Matrix
self.M_read, self.mtm_indices = self.build_sensor_matrix_pinv(sensors=est_sat.attitude_sensors+est_sat.rw_actuators, sensor_type=MTM)
# Actuation Matrix
self.M_act, self.mtq_indices = self.build_torque_to_u_matrix_pinv(actuators=est_sat.actuators, actuator_type=MTQ)
# State Storage for derivative
self.prev_B = np.zeros(3)
self.prev_time = None
self.initialized = False
# Storage of max torque limits
self.max_torque = self.find_max_torque(actuators=est_sat.actuators)
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) -> np.ndarray:
r"""
Computes the B-Dot control command.
This method evaluates the magnetic detumbling control law at the current
time step using raw magnetometer measurements and orbital timing
information.
Algorithm
1. Magnetic Field Reconstruction
Raw sensor measurements y are mapped to the body-frame magnetic field
vector using the sensor reconstruction matrix:
.. math::
\mathbf{B}_k = M_{\mathrm{read}} \, \mathbf{y}
2. Discrete-Time Magnetic Field Derivative
The time derivative of the magnetic field is approximated using a
backward finite difference:
.. math::
\dot{\mathbf{B}}_k \approx
\frac{\mathbf{B}_k - \mathbf{B}_{k-1}}{\Delta t}
where the time step \Delta t is computed from the J2000 timestamps
provided by :class:`~ADCS.orbits.orbital_state.Orbital_State` and
converted to seconds using
:class:`~ADCS.orbits.universal_constants.TimeConstants`.
3. Desired Magnetic Dipole Moment
The B-Dot control law is applied:
.. math::
\mathbf{m}_{\mathrm{req}} = -K \, \dot{\mathbf{B}}_k
4. Actuator Command Mapping
The desired dipole moment is converted to actuator commands using the
actuator mapping matrix:
.. math::
\mathbf{u} = M_{\mathrm{act}} \, \mathbf{m}_{\mathrm{req}}
5. Actuator Saturation
The actuator commands are limited according to magnetorquer
capabilities using
:func:`~ADCS.helpers.math_helpers.limit`.
:param x_hat: Estimated state vector, not used by the B-Dot controller
:type x_hat: np.ndarray
:param sens: Raw sensor measurement vector
:type sens: np.ndarray
:param est_sat: Estimated satellite object required by the controller interface
:type est_sat: ~ADCS.satellite_hardware.satellite.estimated_satellite.EstimatedSatellite
:param os_hat: Orbital state estimate providing timing information
:type os_hat: ~ADCS.orbits.orbital_state.Orbital_State
:return: Actuator command vector for magnetorquers
:rtype: np.ndarray
"""
y = np.asarray(sens).reshape(-1)
B_curr = self.M_read @ y
t_curr = os_hat.J2000
if not self.initialized or self.prev_time is None:
B_dot = np.zeros(3)
self.initialized = True
else:
dt = t_curr - self.prev_time
dt*=TimeConstants.cent2sec
if dt <= 1e-9:
B_dot = np.zeros(3)
else:
B_dot = (B_curr - self.prev_B)/dt
self.prev_B = B_curr
self.prev_time = t_curr
m_desired = -self.gain * B_dot
u_cmd = self.M_act @ m_desired
u_cmd = limit(u=u_cmd, umax=self.max_torque)
return u_cmd