ADCS.controller.mtq_w_rw_QPG module

class ADCS.controller.mtq_w_rw_QPG.MTQ_w_RW_QPG(est_sat, p_gain, d_gain, gamma, c_gain, h_target=array([0., 0., 0.]))

Bases: MTQ_w_RW_LP

Gyroscopically–Weighted Quadratic Torque Allocation for Mixed RW–MTQ ADCS.

This controller implements a state-dependent weighted bounded least-squares torque allocator for a spacecraft with reaction wheels (RWs) and magnetorquers (MTQs). It extends MTQ_w_RW_LP by introducing an anisotropic weighting matrix \(W(\boldsymbol{\omega})\) that depends on the instantaneous body angular velocity \(\boldsymbol{\omega}\). The weighting modifies the tracking error metric so that torque error along the spin axis can be penalized differently from error in the transverse plane.

The allocator computes actuator commands that minimize a weighted torque residual subject to actuator saturation bounds, using scipy.optimize.lsq_linear().

Combined Actuator Torque Model

Let the desired body torque be \(\boldsymbol{\tau}_{\mathrm{des}} \in \mathbb{R}^3\) and define the stacked actuator command vector

\[\begin{split}\boldsymbol{u} = \begin{bmatrix} \boldsymbol{u}_{\mathrm{rw}} \\ \boldsymbol{u}_{\mathrm{mtq}} \end{bmatrix} \in \mathbb{R}^{n_{\mathrm{rw}} + n_{\mathrm{mtq}}}.\end{split}\]

Reaction wheel torque is generated along wheel axes stacked in \(A_{\mathrm{rw}} \in \mathbb{R}^{3 \times n_{\mathrm{rw}}}\):

\[\boldsymbol{\tau}_{\mathrm{rw}} = A_{\mathrm{rw}} \boldsymbol{u}_{\mathrm{rw}}.\]

Magnetorquer torque is generated by commanding a dipole moment in the geomagnetic field \(\boldsymbol{B}\):

\[\boldsymbol{\tau}_{\mathrm{mtq}} = \boldsymbol{m} \times \boldsymbol{B} = -[\boldsymbol{B}]_{\times}\boldsymbol{m} = \left(-[\boldsymbol{B}]_{\times} A_{\mathrm{mtq}}\right)\boldsymbol{u}_{\mathrm{mtq}}.\]

The combined mapping is

\[A_{\mathrm{tot}} = \begin{bmatrix} A_{\mathrm{rw}} & -[\boldsymbol{B}]_{\times} A_{\mathrm{mtq}} \end{bmatrix}, \qquad \boldsymbol{\tau}_{\mathrm{ach}} = A_{\mathrm{tot}} \boldsymbol{u}.\]

Gyroscopically Weighted Error Metric

Define the unit spin axis (when \(\|\boldsymbol{\omega}\| > 0\)) as

\[\hat{\boldsymbol{\omega}} = \frac{\boldsymbol{\omega}}{\|\boldsymbol{\omega}\|}.\]

The weighting matrix is a rank-1 update to the identity:

\[W(\boldsymbol{\omega}) = I_{3 \times 3} + \gamma \left(\hat{\boldsymbol{\omega}} \hat{\boldsymbol{\omega}}^{\mathsf{T}}\right).\]

For \(\gamma > 0\), residual components parallel to \(\boldsymbol{\omega}\) are penalized more heavily than transverse components. For \(\gamma < 0\), the allocator relatively de-emphasizes spin-axis tracking in favor of transverse tracking. When \(\gamma = 0\) or \(\|\boldsymbol{\omega}\| \approx 0\), the weighting reduces to \(W = I\).

Weighted Bounded Least Squares Allocation

The allocation solves

\[\min_{\boldsymbol{u}} \;\left\|W(\boldsymbol{\omega})\left(A_{\mathrm{tot}}\boldsymbol{u}-\boldsymbol{\tau}_{\mathrm{des}}\right)\right\|_2^2\]

subject to box constraints

\[-u_{i,\max} \le u_i \le u_{i,\max}, \qquad \forall i.\]

This can be implemented by left-multiplying the mapping and target:

\[A_w = W A_{\mathrm{tot}}, \qquad \boldsymbol{b}_w = W \boldsymbol{\tau}_{\mathrm{des}},\]

and then solving

\[\min_{\boldsymbol{u}} \;\left\|A_w \boldsymbol{u} - \boldsymbol{b}_w\right\|_2^2\]

with the same bounds.

Effectiveness Metric

After solving for \(\boldsymbol{u}^*\), the achieved torque is \(\boldsymbol{\tau}_{\mathrm{ach}} = A_{\mathrm{tot}}\boldsymbol{u}^*\). An effectiveness scalar is computed by projecting \(\boldsymbol{\tau}_{\mathrm{ach}}\) onto the desired direction:

\[\hat{\boldsymbol{\tau}} = \frac{\boldsymbol{\tau}_{\mathrm{des}}}{\|\boldsymbol{\tau}_{\mathrm{des}}\|}, \qquad \alpha = \max\!\left(0,\frac{\boldsymbol{\tau}_{\mathrm{ach}}\cdot\hat{\boldsymbol{\tau}}}{\|\boldsymbol{\tau}_{\mathrm{des}}\|}\right).\]

param est_sat:

Estimated satellite model providing actuator instances and configuration.

type est_sat:

EstimatedSatellite

param p_gain:

Proportional gain used by the parent controller law.

type p_gain:

float

param d_gain:

Derivative gain used by the parent controller law.

type d_gain:

float

param gamma:

Weighting scalar that scales the penalty for torque residual parallel to the spin axis.

type gamma:

float

param c_gain:

Additional control gain used by the parent controller law.

type c_gain:

float

param h_target:

Target wheel momentum vector used by the parent mixed-actuation controller.

type h_target:

numpy.ndarray | list

allocate_max_torque_in_direction(tau_des, b_body, est_sat, omega)

Allocate bounded RW and MTQ commands using a gyroscopically weighted least-squares metric.

The method builds the combined actuator influence matrix

\[A_{\mathrm{tot}} = \begin{bmatrix} A_{\mathrm{rw}} & -[\boldsymbol{B}]_{\times} A_{\mathrm{mtq}} \end{bmatrix}\]

using wheel axes from RW and MTQ axes from MTQ provided by EstimatedSatellite.

The residual torque is

\[\boldsymbol{r}(\boldsymbol{u}) = A_{\mathrm{tot}}\boldsymbol{u} - \boldsymbol{\tau}_{\mathrm{des}}.\]

The weighting matrix is defined (when \(\|\boldsymbol{\omega}\| > 0\)) as

\[W(\boldsymbol{\omega}) = I_{3 \times 3} + \gamma \frac{\boldsymbol{\omega}\boldsymbol{\omega}^{\mathsf{T}}}{\boldsymbol{\omega}^{\mathsf{T}}\boldsymbol{\omega}}.\]

The optimization problem is

\[\min_{\boldsymbol{u}} \;\left\|W(\boldsymbol{\omega})\boldsymbol{r}(\boldsymbol{u})\right\|_2^2\]

subject to box constraints

\[-u_{i,\max} \le u_i \le u_{i,\max}, \qquad \forall i.\]

This is implemented by solving an equivalent bounded least-squares problem with

\[A_w = W(\boldsymbol{\omega})A_{\mathrm{tot}}, \qquad \boldsymbol{b}_w = W(\boldsymbol{\omega})\boldsymbol{\tau}_{\mathrm{des}}.\]

The solver is scipy.optimize.lsq_linear() with the Trust Region Reflective method.

After obtaining \(\boldsymbol{u}^*\), the achieved torque is \(\boldsymbol{\tau}_{\mathrm{ach}} = A_{\mathrm{tot}}\boldsymbol{u}^*\). The method returns an effectiveness scalar

\[\hat{\boldsymbol{\tau}} = \frac{\boldsymbol{\tau}_{\mathrm{des}}}{\|\boldsymbol{\tau}_{\mathrm{des}}\|}, \qquad \alpha = \max\!\left(0,\frac{\boldsymbol{\tau}_{\mathrm{ach}}\cdot\hat{\boldsymbol{\tau}}}{\|\boldsymbol{\tau}_{\mathrm{des}}\|}\right).\]

Parameters:

• tau_des (numpy.ndarray) – Desired body-frame torque vector \(\boldsymbol{\tau}_{\mathrm{des}}\) in N·m.

• b_body (numpy.ndarray) – Body-frame geomagnetic field vector \(\boldsymbol{B}\) in tesla.

• est_sat (EstimatedSatellite) – Estimated satellite model providing actuator instances and limits.

• omega (numpy.ndarray) – Body angular velocity vector \(\boldsymbol{\omega}\) in rad/s used to form the weighting matrix.

Returns:

Tuple of RW commands, MTQ commands, and effectiveness scalar \(\alpha\).

Return type:

tuple[numpy.ndarray, numpy.ndarray, float]

find_u(x_hat, sens, est_sat, os_hat, goal=None)

Compute actuator commands for detumble or goal-tracking operation.

If goal is No_Goal, the method computes a detumble and momentum-management command based on body rates and wheel momentum estimates:

• The geomagnetic field is estimated in the body frame from sensor measurements using the MTM readout model in the parent class.

• A rate damping torque is computed in the plane perpendicular to the field direction because MTQs cannot generate torque parallel to \(\boldsymbol{B}\).

• A momentum dumping torque is computed from the error between estimated system wheel momentum and the target vector h_target and projected into the MTQ-achievable plane.

• MTQ commands are computed and scaled to satisfy saturation. The scaling factor \(\alpha_{\mathrm{mtq}}\) is used to scale RW assistance to avoid net spin-up when MTQs cannot realize the intended damping and dumping torque.

If goal is not No_Goal, the method computes a PD-style desired torque using pointing error from error() and rate error relative to the reference generated by to_ref(). The quaternion alignment error is computed using vector_alignment_error(). A gyroscopic term based on the spacecraft inertia and wheel momentum is added, producing the final desired torque \(\boldsymbol{\tau}_{\mathrm{des}}\). This desired torque is allocated using allocate_max_torque_in_direction(), which applies the gyroscopically weighted least-squares metric.

Parameters:

• x_hat (numpy.ndarray) – Estimated state vector containing angular rate, attitude quaternion, and optionally wheel momentum states.

• sens (numpy.ndarray) – Sensor measurement vector used to estimate body magnetic field through the MTM readout model.

• est_sat (EstimatedSatellite) – Estimated satellite model providing actuators, inertia, and boresight.

• os_hat (Orbital_State) – Estimated orbital state used by goal reference generation and error models.

• goal (Goal | None) – Optional goal object providing desired pointing and reference rates.

Returns:

Command vector for all actuators in the order used by est_sat.actuators.

Return type:

numpy.ndarray

Parameters:

• est_sat (EstimatedSatellite)

• p_gain (float)

• d_gain (float)

• gamma (float)

• c_gain (float)

• h_target (ndarray | list)