ADCS.controller.mtq_w_rw_QPW module

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

Bases: MTQ_w_RW_LP

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

This controller implements a weighted bounded least-squares (WLS) torque allocator for a spacecraft with reaction wheels (RWs) and magnetorquers (MTQs). It is designed to behave between a strict direction-prioritizing allocator and a pure Euclidean-error allocator by penalizing torque direction error more strongly than torque magnitude error.

The class inherits from MTQ_w_RW_LP and solves a box-constrained weighted least-squares problem 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}.\]

Direction-Weighted Error Decomposition

Let the unit desired torque direction be

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

Define projection operators onto the parallel and perpendicular subspaces:

\[P_{\parallel} = \hat{\boldsymbol{\tau}}\hat{\boldsymbol{\tau}}^{\mathsf{T}}, \qquad P_{\perp} = I_{3 \times 3} - P_{\parallel}.\]

For a torque residual

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

the parallel and perpendicular components are

\[\boldsymbol{e}_{\parallel} = P_{\parallel}\boldsymbol{e}, \qquad \boldsymbol{e}_{\perp} = P_{\perp}\boldsymbol{e}.\]

Weighted Bounded Least Squares Allocation

Choose weights \(w_{\parallel} > 0\) and \(w_{\perp} > 0\) with \(w_{\perp} \gg w_{\parallel}\) to penalize directional error more strongly. The weighted objective is

\[\min_{\boldsymbol{u}} \;w_{\parallel}\|\boldsymbol{e}_{\parallel}(\boldsymbol{u})\|_2^2 + w_{\perp}\|\boldsymbol{e}_{\perp}(\boldsymbol{u})\|_2^2.\]

This can be written as a standard weighted least-squares norm by defining the square-root weighting matrix

\[W = \sqrt{w_{\parallel}}\,P_{\parallel} + \sqrt{w_{\perp}}\,P_{\perp},\]

which yields the equivalent problem

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

The actuator saturation bounds are enforced as

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

Implementation Parameters

The default configuration uses \(w_{\parallel}=1\) and \(w_{\perp}=100\). This biases the solution to align the achieved torque direction with \(\boldsymbol{\tau}_{\mathrm{des}}\) even when exact magnitude tracking is infeasible due to saturation or MTQ underactuation.

Effectiveness Metric

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

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

Allocate bounded RW and MTQ commands using direction-weighted least squares.

The method builds the combined actuator influence matrix

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

from RW and MTQ axes provided by EstimatedSatellite.

Let the torque residual be

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

Using the unit desired direction \(\hat{\boldsymbol{\tau}}\) and projection operators

\[P_{\parallel} = \hat{\boldsymbol{\tau}}\hat{\boldsymbol{\tau}}^{\mathsf{T}}, \qquad P_{\perp} = I_{3 \times 3} - P_{\parallel},\]

define the square-root weighting matrix

\[W = \sqrt{w_{\parallel}}\,P_{\parallel} + \sqrt{w_{\perp}}\,P_{\perp}.\]

The allocation solves

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

subject to actuator bounds

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

The implementation uses \(w_{\parallel}=1\) and \(w_{\perp}=100\) and solves the equivalent bounded least-squares problem

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

via scipy.optimize.lsq_linear().

The achieved torque is \(\boldsymbol{\tau}_{\mathrm{ach}} = A_{\mathrm{tot}}\boldsymbol{u}^*\). An effectiveness scalar is computed as

\[\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.

Returns:

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

Return type:

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

Parameters:

• est_sat (EstimatedSatellite)

• p_gain (float)

• d_gain (float)

• c_gain (float)

• h_target (ndarray | list)