ADCS.controller.mtq_w_rw_QPC module

class ADCS.controller.mtq_w_rw_QPC.MTQ_w_RW_QPC(est_sat, p_gain, d_gain, c_gain, h_target=array([0., 0., 0.]))

Bases: MTQ_w_RW_LP

Constrained Quadratic–Programming Torque Allocation for Mixed RW–MTQ ADCS.

This controller performs constrained torque allocation for a spacecraft equipped with reaction wheels (RWs) and magnetorquers (MTQs). It extends the bounded least-squares allocation idea by enforcing an additional Lyapunov-style instantaneous power constraint that prevents the allocator from producing torque that increases rotational kinetic energy when the higher-level control law intends to dissipate it.

The class inherits from MTQ_w_RW_LP and uses a constrained least-squares formulation solved with scipy.optimize.minimize() using the SLSQP algorithm. If the constrained optimizer fails, it falls back to a geometry-aware scaling Linear Program implemented in solve_lp_scaling().

Combined Actuator Torque Model

Let the desired body torque be \(\boldsymbol{\tau}_{\mathrm{des}} \in \mathbb{R}^3\). Define the stacked 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 produced from the commanded dipole moment and 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}.\]

Constrained Least Squares Objective

The allocator seeks the command vector that minimizes torque tracking error while obeying actuator saturations:

\[\min_{\boldsymbol{u}} \;\frac{1}{2}\left\|A_{\mathrm{tot}}\boldsymbol{u}-\boldsymbol{\tau}_{\mathrm{des}}\right\|_2^2\]

subject to box bounds

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

Energy Gate Constraint

Let \(\boldsymbol{\omega}\) be the body angular velocity. The instantaneous mechanical power delivered by a control torque \(\boldsymbol{\tau}\) is

\[P = \boldsymbol{\omega}^{\mathsf{T}}\boldsymbol{\tau}.\]

This controller enforces a linear inequality constraint on the achieved torque to prevent destabilizing allocations when the requested torque implies damping:

\[\boldsymbol{\omega}^{\mathsf{T}}\left(A_{\mathrm{tot}}\boldsymbol{u}\right) \le \max\!\left(0,\boldsymbol{\omega}^{\mathsf{T}}\boldsymbol{\tau}_{\mathrm{des}}\right).\]

When \(\boldsymbol{\omega}^{\mathsf{T}}\boldsymbol{\tau}_{\mathrm{des}} < 0\), the constraint forbids achieved torque that injects energy (\(P>0\)). When \(\boldsymbol{\omega}^{\mathsf{T}}\boldsymbol{\tau}_{\mathrm{des}} \ge 0\), the gate is non-restrictive and the allocator can focus on tracking accuracy.

Post-hoc Effectiveness Metric

After computing \(\boldsymbol{u}^*\), the achieved torque \(\boldsymbol{\tau}_{\mathrm{ach}} = A_{\mathrm{tot}}\boldsymbol{u}^*\) is projected onto the desired direction to compute a scalar effectiveness:

\[\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 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, h)

Allocate bounded RW and MTQ commands with a linear power constraint.

This method constructs the combined actuator mapping

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

from actuator axes contained in EstimatedSatellite, using RW and MTQ.

It then solves a constrained least-squares problem

\[\min_{\boldsymbol{u}} \;\frac{1}{2}\left\|A_{\mathrm{tot}}\boldsymbol{u}-\boldsymbol{\tau}_{\mathrm{des}}\right\|_2^2\]

subject to actuator saturation bounds

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

and an instantaneous power gate

\[\boldsymbol{\omega}^{\mathsf{T}}\left(A_{\mathrm{tot}}\boldsymbol{u}\right) \le \max\!\left(0,\boldsymbol{\omega}^{\mathsf{T}}\boldsymbol{\tau}_{\mathrm{des}}\right).\]

The optimizer is implemented via scipy.optimize.minimize() with method SLSQP. If the constrained solve fails, the method falls back to a scaled LP feasibility solve using solve_lp_scaling().

After solving, it computes an effectiveness scalar

\[\hat{\boldsymbol{\tau}} = \frac{\boldsymbol{\tau}_{\mathrm{des}}}{\|\boldsymbol{\tau}_{\mathrm{des}}\|}, \qquad \alpha = \max\!\left(0,\frac{(A_{\mathrm{tot}}\boldsymbol{u}^*)\cdot\hat{\boldsymbol{\tau}}} {\|\boldsymbol{\tau}_{\mathrm{des}}\|}\right),\]

and returns separated RW and MTQ command vectors.

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 by the power constraint.

• h (numpy.ndarray) – Wheel momentum state vector used by the allocator state-dependent constraint terms.

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 either detumble or goal-tracking operation.

This method computes a commanded actuator vector based on the estimated state, sensor measurements, and an optional pointing goal.

When goal is No_Goal, the controller performs a detumble and momentum-management style behavior:

• The body angular rate is damped in the MTQ-achievable plane perpendicular to the geomagnetic field.

• A momentum dumping torque is computed from the error between current wheel momentum and h_target and is also projected into the MTQ-achievable plane.

• MTQ commands are computed and scaled to respect saturation. A scalar scaling \(\alpha_{\mathrm{mtq}}\) is derived from MTQ saturation.

• RW commands are scaled by \(\alpha_{\mathrm{mtq}}\) to avoid net body spin-up when MTQs cannot supply the intended damping or dumping torque.

When goal is not No_Goal, the controller computes a PD-style desired torque using pointing error and rate error derived from the goal reference generated by to_ref(). It adds the gyroscopic torque term \(\boldsymbol{\tau}_{\mathrm{gyro}} = \boldsymbol{\omega} \times (J\boldsymbol{\omega} + \boldsymbol{h})\) and then calls allocate_max_torque_in_direction() to allocate the requested torque under actuator bounds and the energy gate constraint.

Returned torque is the internally formed desired body torque for the goal-tracking branch and is not defined for the detumble branch in this implementation.

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:

Tuple of commanded actuator vector and the desired torque used for allocation in the goal-tracking branch.

Return type:

tuple[numpy.ndarray, numpy.ndarray | None]

solve_lp_scaling(tau_des, A_total, lb, ub)

Compute the maximum feasible scaling of a desired torque direction under box bounds.

This method solves a linear program that finds the largest scalar \(T_{\mathrm{available}} \ge 0\) such that a torque of magnitude \(T_{\mathrm{available}}\) can be generated along the unit direction \(\hat{\boldsymbol{\tau}}\) of the desired torque.

Let

\[\hat{\boldsymbol{\tau}} = \frac{\boldsymbol{\tau}_{\mathrm{des}}}{\|\boldsymbol{\tau}_{\mathrm{des}}\|}.\]

The LP introduces decision variables \(\boldsymbol{u}\) and \(T_{\mathrm{available}}\) and enforces

\[A_{\mathrm{tot}}\boldsymbol{u} = T_{\mathrm{available}}\hat{\boldsymbol{\tau}}.\]

The objective maximizes \(T_{\mathrm{available}}\):

\[\max_{\boldsymbol{u},\,T_{\mathrm{available}}}\; T_{\mathrm{available}}\]

subject to the actuator bounds

\[\boldsymbol{\ell} \le \boldsymbol{u} \le \boldsymbol{u},\]

where \(\boldsymbol{\ell}\) and \(\boldsymbol{u}\) are provided as lb and ub.

If \(T_{\mathrm{available}}\) exceeds the requested torque magnitude, the solution is scaled down to return a command that exactly achieves \(\boldsymbol{\tau}_{\mathrm{des}}\) while preserving direction.

The solver is implemented via scipy.optimize.linprog() using the highs backend.

Parameters:

• tau_des (numpy.ndarray) – Desired torque vector used to define \(\hat{\boldsymbol{\tau}}\).

• A_total (numpy.ndarray) – Combined actuator influence matrix \(A_{\mathrm{tot}}\).

• lb (numpy.ndarray) – Lower bounds for actuator commands.

• ub (numpy.ndarray) – Upper bounds for actuator commands.

Returns:

Tuple containing the achievable torque vector, the scaling factor, and the actuator command vector.

Return type:

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

Parameters:

• est_sat (EstimatedSatellite)

• p_gain (float)

• d_gain (float)

• c_gain (float)

• h_target (ndarray | list)