Source code for ADCS.satellite_hardware.disturbances.prop_disturbance
__all__ = ["Prop_Disturbance"]
import numpy as np
from typing import TYPE_CHECKING
from ADCS.satellite_hardware.disturbances.disturbance import Disturbance
from ADCS.satellite_hardware.errors.noise import Noise
from ADCS.orbits.orbital_state import Orbital_State
from ADCS.helpers.math_helpers import normalize, normed_vec_jac, normed_vec_hess
from ADCS.orbits.universal_constants import EarthConstants
[docs]
class Prop_Disturbance(Disturbance):
r"""
**Propulsion-Induced Disturbance Torque Model**
This class models **propulsion-related disturbance torques** acting on a spacecraft.
Such torques typically arise from imperfect thrust alignment, residual thrust during
coast phases, asymmetric propellant flow, or unmodeled reaction forces during
maneuver execution.
The propulsion disturbance is modeled as a **constant nominal torque** with an
optional **additive stochastic noise** component. Unlike environmental disturbances
(e.g., gravity-gradient or aerodynamic drag), this torque does **not depend on the
spacecraft attitude or orbital state**.
**Mathematical Model**
The propulsion disturbance torque is expressed as:
.. math::
\mathbf{T}_{\mathrm{prop}}(t)
=
\mathbf{T}_0 + \mathbf{n}(t),
where:
+------------------+----------------------------------------------+
| Symbol | Description |
+==================+==============================================+
| :math:`\mathbf{T}_0` | Nominal constant propulsion torque [N·m] |
+------------------+----------------------------------------------+
| :math:`\mathbf{n}(t)` | Zero-mean stochastic noise torque [N·m] |
+------------------+----------------------------------------------+
The noise term :math:`\mathbf{n}(t)` is generated by a user-supplied noise model
of type :class:`~ADCS.satellite_hardware.errors.noise.Noise`.
**Attitude Independence**
Since :math:`\mathbf{T}_{\mathrm{prop}}` does not depend on the attitude quaternion
:math:`\mathbf{q}`, all attitude derivatives vanish:
.. math::
\frac{\partial \mathbf{T}_{\mathrm{prop}}}{\partial \mathbf{q}} = \mathbf{0}_{3\times4},
\qquad
\frac{\partial^2 \mathbf{T}_{\mathrm{prop}}}{\partial \mathbf{q}^2}
= \mathbf{0}_{3\times4\times4}.
This makes the propulsion disturbance especially simple to include in linearized
or second-order attitude dynamics formulations.
:param torque_nominal: Nominal constant propulsion disturbance torque vector.
:type torque_nominal: :class:`numpy.ndarray`
:param noise: Noise model providing stochastic torque perturbations.
:type noise: :class:`~ADCS.satellite_hardware.errors.noise.Noise`
"""
def __init__(self, torque_nominal: np.ndarray, noise: Noise = None, estimate_dist: bool = False):
r"""
Initialize the propulsion disturbance model.
This constructor sets the nominal propulsion disturbance torque and optionally
attaches a stochastic noise model. If no noise model is provided, a default
instance of :class:`~ADCS.satellite_hardware.errors.noise.Noise` is used.
The internal disturbance state is initialized as:
.. math::
\mathbf{T}_{\mathrm{prop}}(0) = \mathbf{T}_0.
:param torque_nominal: Constant nominal propulsion torque vector [N·m],
shape ``(3,)``.
:type torque_nominal: :class:`numpy.ndarray`
:param noise: Noise model instance generating additive torque noise.
:type noise: :class:`~ADCS.satellite_hardware.errors.noise.Noise` | None
:param estimate_dist: Flag indicating whether this disturbance should be
included in the disturbance estimation state.
:type estimate_dist: :class:`bool`
:return: None
:rtype: None
"""
self.torque_nominal = torque_nominal
if noise:
self.noise = noise
else:
self.noise = Noise()
self.current_torque = self.torque_nominal.copy()
super().__init__(estimate_dist=estimate_dist)
[docs]
def update(self) -> None:
r"""
Update the current propulsion disturbance torque.
This method draws a new noise realization from the noise model and updates
the current disturbance torque according to:
.. math::
\mathbf{T}_{\mathrm{prop}}(t)
=
\mathbf{T}_0 + \mathbf{n}(t),
where :math:`\mathbf{n}(t)` is provided by
:meth:`~ADCS.satellite_hardware.errors.noise.Noise.get_noise`.
This method should typically be called once per simulation or control
update step.
:return: None
:rtype: None
"""
self.current_torque = self.torque_nominal + self.noise.get_noise()
[docs]
def torque(self, x: np.ndarray, os: Orbital_State) -> np.ndarray:
r"""
Return the **current propulsion disturbance torque**.
This method simply returns the internally stored disturbance torque that
was most recently updated by :meth:`update`.
The torque is independent of:
- the attitude quaternion in ``x``
- the orbital state ``os``
These parameters are accepted only to maintain interface consistency with
other disturbance models derived from
:class:`~ADCS.satellite_hardware.disturbances.disturbance.Disturbance`.
:param x: Full spacecraft state (unused).
:type x: :class:`numpy.ndarray`
:param os: Orbital state provider (unused).
:type os: :class:`~ADCS.orbits.orbital_state.Orbital_State`
:return: Current propulsion disturbance torque [N·m], shape ``(3,)``.
:rtype: :class:`numpy.ndarray`
"""
return self.current_torque
[docs]
def torque_qjac(self) -> np.ndarray:
r"""
Compute the **Jacobian of the propulsion disturbance torque** with respect
to the attitude quaternion.
Because the propulsion disturbance torque is constant and does not depend
on the attitude quaternion :math:`\mathbf{q}`, the Jacobian is identically zero:
.. math::
\frac{\partial \mathbf{T}_{\mathrm{prop}}}{\partial \mathbf{q}}
=
\mathbf{0}_{3\times4}.
:return: Quaternion Jacobian matrix of zeros, shape ``(3, 4)``.
:rtype: :class:`numpy.ndarray`
"""
return np.zeros((3, 4))
[docs]
def torque_qqHess(self) -> np.ndarray:
r"""
Compute the **Hessian of the propulsion disturbance torque** with respect
to the attitude quaternion.
Since :math:`\mathbf{T}_{\mathrm{prop}}` is constant, all second-order
derivatives vanish:
.. math::
\frac{\partial^2 \mathbf{T}_{\mathrm{prop}}}{\partial \mathbf{q}^2}
=
\mathbf{0}_{3\times4\times4}.
:return: Quaternion Hessian tensor of zeros, shape ``(3, 4, 4)``.
:rtype: :class:`numpy.ndarray`
"""
return np.zeros((3, 4, 4))