ADCS.controller.mtq_w_rw_LP module

class ADCS.controller.mtq_w_rw_LP.MTQ_w_RW_LP(est_sat, p_gain, d_gain, c_gain, h_target=array([0., 0., 0.]))[source]

Bases: Controller

Linear–Programming–Based Torque Allocation for Mixed RW–MTQ ADCS.

This controller allocates attitude control effort between reaction wheels (RWs) and magnetorquers (MTQs) using a geometry-aware linear program (LP). The LP computes the maximum physically achievable torque colinear with a requested torque direction while enforcing hard actuator saturation limits and the MTQ torque-plane constraint.

The implementation is intended for use with an estimated satellite model EstimatedSatellite and goal objects derived from Goal.

Actuator Models

Desired body-frame control torque:

\[\boldsymbol{\tau}_{\mathrm{des}} \in \mathbb{R}^3, \qquad T_{\mathrm{des}} = \|\boldsymbol{\tau}_{\mathrm{des}}\|, \qquad \hat{\boldsymbol{\tau}} = \frac{\boldsymbol{\tau}_{\mathrm{des}}}{T_{\mathrm{des}}}.\]

Reaction wheels (for \(N_{\mathrm{rw}}\) wheels):

\[\boldsymbol{\tau}_{\mathrm{rw}} = \sum_{i=1}^{N_{\mathrm{rw}}} \boldsymbol{a}_i u_i = A_{\mathrm{rw}} \boldsymbol{u}_{\mathrm{rw}}, \qquad |u_i| \le u_{i,\max},\]

where \(\boldsymbol{a}_i\) are unit wheel axes.

Magnetorquers (for \(N_{\mathrm{mtq}}\) rods) produce a dipole moment \(\boldsymbol{m} = A_{\mathrm{mtq}} \boldsymbol{u}_{\mathrm{mtq}}\) and torque:

\[\boldsymbol{\tau}_{\mathrm{mtq}} = \boldsymbol{m} \times \boldsymbol{B} = -[\boldsymbol{B}]_\times A_{\mathrm{mtq}} \boldsymbol{u}_{\mathrm{mtq}}, \qquad \boldsymbol{\tau}_{\mathrm{mtq}} \perp \boldsymbol{B},\]

with the skew-symmetric matrix \([\boldsymbol{B}]_\times\) satisfying \([\boldsymbol{B}]_\times \boldsymbol{x} = \boldsymbol{B} \times \boldsymbol{x}\). The cross-product matrix is consistent with skewsym().

Combined actuator mapping:

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

LP Formulation

Directly enforcing \(A_{\mathrm{tot}} \boldsymbol{u} = \boldsymbol{\tau}_{\mathrm{des}}\) may be infeasible due to underactuation (MTQ plane) and saturation. Instead, introduce a scalar availability variable \(T_{\mathrm{avail}} \ge 0\) and solve:

\[\max_{\boldsymbol{u},\,T_{\mathrm{avail}}}\; T_{\mathrm{avail}} \quad \text{s.t.}\quad A_{\mathrm{tot}} \boldsymbol{u} - T_{\mathrm{avail}}\hat{\boldsymbol{\tau}} = \boldsymbol{0}, \quad -u_{k,\max} \le u_k \le u_{k,\max}, \quad T_{\mathrm{avail}} \ge 0.\]

Let \(T_{\max}\) be the optimizer value of \(T_{\mathrm{avail}}\).

  • If \(T_{\max} \ge T_{\mathrm{des}}\), the commanded effort is scaled to reproduce \(\boldsymbol{\tau}_{\mathrm{des}}\) exactly.

  • If \(T_{\max} < T_{\mathrm{des}}\), the controller applies the maximum feasible torque in the requested direction.

Define the scalar effectiveness metric:

\[\alpha = \frac{T_{\max}}{T_{\mathrm{des}}} \in [0,1].\]

Pointing and Torque-Free Momentum Management

In pointing mode (non-No_Goal), the controller:

  1. Computes a PD+gyro torque request using goal attitude error and angular-rate error. The goal error is obtained from error() and the reference rate from to_ref().

  2. Uses the LP allocator to achieve the largest feasible torque colinear with the request.

  3. If both RWs and MTQs are present and authority remains, attempts a torque-free secondary desaturation by selecting MTQ torque (projected into the MTQ plane) to reduce stored wheel momentum and commanding an equal-and-opposite RW torque so the net additional body torque is approximately zero.

In desaturation mode (No_Goal), the controller uses MTQ-only logic (B-dot style) with optional RW coordination to reduce rates and dump wheel momentum while respecting actuator limits.

allocate_max_torque_in_direction(tau_des, b_body, est_sat)[source]

Solve the LP to maximize feasible torque colinear with a desired torque direction.

This method implements the normalized LP:

\[\max_{\boldsymbol{u},\,T_{\mathrm{avail}}}\; T_{\mathrm{avail}} \quad \text{s.t.}\quad A_{\mathrm{tot}}\boldsymbol{u} = T_{\mathrm{avail}}\hat{\boldsymbol{\tau}}, \quad -u_{k,\max} \le u_k \le u_{k,\max}, \quad T_{\mathrm{avail}} \ge 0,\]

where \(\hat{\boldsymbol{\tau}} = \boldsymbol{\tau}_{\mathrm{des}}/\|\boldsymbol{\tau}_{\mathrm{des}}\|\). The combined map is:

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

The MTQ block uses the body magnetic field \(\boldsymbol{B}\) and enforces the plane constraint \(\boldsymbol{\tau}_{\mathrm{mtq}} \perp \boldsymbol{B}\) by construction.

Output scaling

Let \(T_{\max}\) be the optimizer value of \(T_{\mathrm{avail}}\) and \(T_{\mathrm{des}} = \|\boldsymbol{\tau}_{\mathrm{des}}\|\).

  • If \(T_{\max} \le T_{\mathrm{des}}\), the system is saturated and the method returns the maximizing command and \(\alpha = T_{\max}/T_{\mathrm{des}}\).

  • If \(T_{\max} > T_{\mathrm{des}}\), the method scales the maximizing command by \(T_{\mathrm{des}}/T_{\max}\) to match \(\boldsymbol{\tau}_{\mathrm{des}}\) exactly and returns \(\alpha = 1\).

Degenerate cases

If \(\|\boldsymbol{\tau}_{\mathrm{des}}\|\) is near zero, the method returns zeros and \(\alpha = 1\). If the LP solver fails (e.g., no torque achievable in the requested direction), the method returns zeros and \(\alpha = 0\).

param tau_des:

Desired body-frame torque vector \(\boldsymbol{\tau}_{\mathrm{des}}\).

type tau_des:

numpy.ndarray

param b_body:

Body-frame magnetic field vector \(\boldsymbol{B}\).

type b_body:

numpy.ndarray

param est_sat:

Estimated satellite providing the RW and MTQ actuator sets and limits.

type est_sat:

EstimatedSatellite

return:

Tuple (u_rw_cmd, u_mtq_cmd, alpha) where u_rw_cmd has shape (N_rw,), u_mtq_cmd has shape (N_mtq,), and alpha is the achievable torque fraction.

rtype:

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

Parameters:
Return type:

tuple[ndarray, ndarray, float]

find_u(x_hat, sens, est_sat, os_hat, goal=None)[source]

Compute actuator commands for either pointing or desaturation mode.

If goal is No_Goal (or None), this method routes to find_u_desaturate(). Otherwise it routes to find_u_pointing().

The returned vector is ordered in the same actuator ordering as est_sat.actuators and contains MTQ and RW command signals consistent with each actuator’s u_max bounds.

Parameters:

  • x_hat (numpy.ndarray) – Estimated state vector. The first 3 elements are body rates \(\boldsymbol{\omega}\) and elements 3:7 are the attitude quaternion. If present, elements 7:7+N_rw store RW scalar momentum states.

  • sens (numpy.ndarray) – Raw sensor vector used to estimate the body magnetic field via the MTM model.

  • est_sat (EstimatedSatellite) – Estimated satellite object providing hardware configuration and inertia.

  • os_hat (Orbital_State) – Estimated orbital state used by the goal to compute reference vectors and rates.

  • goal (Goal | None) – Pointing goal. If None, the controller uses No_Goal.

Returns:

Actuator command vector in est_sat.actuators ordering.

Return type:

numpy.ndarray

find_u_desaturate(x_hat, sens, est_sat, os_hat, goal=None)[source]

Compute actuator commands for desaturation mode (No_Goal).

This mode prioritizes: - Rate damping in the MTQ-achievable plane (perpendicular to \(\boldsymbol{B}\)). - Momentum dumping toward \(\boldsymbol{h}_{\mathrm{target}}\) when RWs exist.

Magnetic field estimation

The body magnetic field \(\boldsymbol{B}\) is reconstructed from MTM readings. If \(\|\boldsymbol{B}\|\) is near zero, no MTQ torque is possible and this method returns zeros.

Rate damping (B-dot style plane damping)

MTQs can only generate torque perpendicular to \(\boldsymbol{B}\). The method dampens body rates only in that plane. Let:

\[\hat{\boldsymbol{B}} = \frac{\boldsymbol{B}}{\|\boldsymbol{B}\|}, \qquad \boldsymbol{\omega}_{\perp} = \boldsymbol{\omega} - (\boldsymbol{\omega}\cdot\hat{\boldsymbol{B}})\hat{\boldsymbol{B}}.\]

The plane-damping torque is:

\[\boldsymbol{\tau}_{\mathrm{bdot}} = -k_{\omega}\boldsymbol{\omega}_{\perp}.\]

Momentum dumping

For RWs, the stored body-frame wheel momentum is:

\[\boldsymbol{h}_{\mathrm{rw}} = A_{\mathrm{rw}}\boldsymbol{h}_{\mathrm{rw,scalars}}.\]

The error relative to the target is:

\[\boldsymbol{h}_{\mathrm{err}} = \boldsymbol{h}_{\mathrm{rw}} - \boldsymbol{h}_{\mathrm{target}}.\]

A nominal dumping torque is computed (gain and sign follow the implementation):

\[\boldsymbol{\tau}_{\mathrm{dump}}^{\ast} = k_h \boldsymbol{h}_{\mathrm{err}}.\]

The commanded dump torque is then projected into the MTQ-achievable plane:

\[\boldsymbol{\tau}_{\mathrm{dump},\perp} = \boldsymbol{\tau}_{\mathrm{dump}}^{\ast} - (\boldsymbol{\tau}_{\mathrm{dump}}^{\ast}\cdot\hat{\boldsymbol{B}})\hat{\boldsymbol{B}}.\]

MTQ allocation and saturation scaling

The MTQ command is computed by a pseudoinverse of the effective MTQ torque map:

\[A_{\mathrm{mtq,eff}} = -[\boldsymbol{B}]_\times A_{\mathrm{mtq}}, \qquad \boldsymbol{u}_{\mathrm{mtq}} = A_{\mathrm{mtq,eff}}^{+}\boldsymbol{\tau}_{\mathrm{mtq,des}}.\]

If any MTQ channel exceeds its limit, a scalar scale factor \(\alpha_{\mathrm{mtq}} \in [0,1]\) is applied to keep commands feasible.

RW coordination

When MTQ authority is limited (small \(\alpha_{\mathrm{mtq}}\)), the RW torque request is scaled accordingly to avoid injecting uncompensated body torque. The RW command is produced using the RW torque-to-command mapping built in the constructor and then saturated to bounds.

param x_hat:

Estimated state vector containing body rates, quaternion, and optionally RW momentum states.

type x_hat:

numpy.ndarray

param sens:

Sensor vector used to estimate the body magnetic field from MTM measurements.

type sens:

numpy.ndarray

param est_sat:

Estimated satellite object providing actuators and inertia.

type est_sat:

EstimatedSatellite

param os_hat:

Estimated orbital state (not required by the current desaturation logic but included for interface compatibility).

type os_hat:

Orbital_State

param goal:

Optional goal (unused in this mode). Included for signature compatibility.

type goal:

Goal | None

return:

Actuator command vector in est_sat.actuators ordering for desaturation mode.

rtype:

numpy.ndarray

Parameters:
find_u_pointing(x_hat, sens, est_sat, os_hat, goal)[source]

Compute actuator commands for pointing with LP allocation and torque-free desaturation.

This method performs two stages:

Stage 1: LP torque allocation in the requested direction

A desired control torque is computed as PD feedback plus a gyroscopic compensation term:

\[\boldsymbol{\tau}_{\mathrm{pd}} = -k_p \boldsymbol{q}_{\mathrm{err}} - k_d(\boldsymbol{\omega} - \boldsymbol{\omega}_{\mathrm{ref}}),\]

where \(\boldsymbol{q}_{\mathrm{err}}\) is obtained from error(), and \(\boldsymbol{\omega}_{\mathrm{ref}}\) is the body-frame reference rate from to_ref() and rot_mat().

The gyroscopic term uses satellite inertia \(J\) and wheel momentum:

\[\boldsymbol{h}_{\mathrm{rw}} = A_{\mathrm{rw}}\boldsymbol{h}_{\mathrm{rw,scalars}}, \qquad \boldsymbol{\tau}_{\mathrm{gyro}} = \boldsymbol{\omega} \times (J\boldsymbol{\omega} + \boldsymbol{h}_{\mathrm{rw}}),\]

giving:

\[\boldsymbol{\tau}_{\mathrm{des}} = \boldsymbol{\tau}_{\mathrm{pd}} + \boldsymbol{\tau}_{\mathrm{gyro}}.\]

The body magnetic field \(\boldsymbol{B}\) is reconstructed from MTM readings using the sensor mapping built in the constructor. The LP allocator allocate_max_torque_in_direction() then returns a feasible command achieving \(\alpha \boldsymbol{\tau}_{\mathrm{des}}\) with \(\alpha \in [0,1]\).

Stage 2: torque-free momentum desaturation using leftover authority

If both RWs and MTQs are present, the controller attempts to reduce wheel momentum error without introducing additional net body torque. Define the body-frame wheel momentum error:

\[\boldsymbol{h}_{\mathrm{err}} = \boldsymbol{h}_{\mathrm{rw}} - \boldsymbol{h}_{\mathrm{target}}.\]

A desired dumping torque is:

\[\boldsymbol{\tau}_{\mathrm{dump}}^{\ast} = -k_c \boldsymbol{h}_{\mathrm{err}}.\]

Because MTQ torque must satisfy \(\boldsymbol{\tau}_{\mathrm{mtq}} \perp \boldsymbol{B}\), the dump torque is projected into the achievable plane:

\[\boldsymbol{\tau}_{\mathrm{mtq,sec}} = \boldsymbol{\tau}_{\mathrm{dump}}^{\ast} - (\boldsymbol{\tau}_{\mathrm{dump}}^{\ast} \cdot \hat{\boldsymbol{B}})\hat{\boldsymbol{B}}, \qquad \hat{\boldsymbol{B}} = \frac{\boldsymbol{B}}{\|\boldsymbol{B}\|}.\]

Torque-free cancellation is imposed by selecting the secondary wheel torque:

\[\boldsymbol{\tau}_{\mathrm{rw,sec}} = -\boldsymbol{\tau}_{\mathrm{mtq,sec}}.\]

The method maps these secondary torques into actuator commands via pseudoinverses, then computes the largest scalar \(\beta \in [0,1]\) such that the combined primary+secondary commands remain within hard bounds:

\[-u_{k,\max} \le u_k^{\mathrm{primary}} + \beta u_k^{\mathrm{sec}} \le u_{k,\max}.\]

This coupled scaling preserves the near-zero net secondary torque by avoiding independent clipping of RW and MTQ channels.

param x_hat:

Estimated state vector containing body rates, quaternion, and optionally RW momentum states.

type x_hat:

numpy.ndarray

param sens:

Sensor vector used to estimate the body magnetic field from MTM measurements.

type sens:

numpy.ndarray

param est_sat:

Estimated satellite object containing actuators, inertia, and boresight configuration.

type est_sat:

EstimatedSatellite

param os_hat:

Estimated orbital state used by the goal to define reference vectors and rates.

type os_hat:

Orbital_State

param goal:

Pointing goal providing reference vector and attitude error computation.

type goal:

Goal

return:

Actuator command vector in est_sat.actuators ordering, including any torque-free desaturation contribution that fits within remaining authority.

rtype:

numpy.ndarray

Parameters:
Return type:

ndarray

plot_capacity(ax, points, face_color, edge_color, alpha=0.25)[source]

Plot a convex capacity set (line, polygon, or 3D hull) from a point cloud.

The method determines the intrinsic rank (dimension) of the point set by SVD and renders: - Rank 1: a line segment between extreme projections. - Rank 2: a filled convex polygon in 3D via a 2D convex hull in the best-fit plane. - Rank 3: a 3D convex hull surface, plus silhouette edges.

Rank detection

Let \(P \in \mathbb{R}^{N \times 3}\) be the unique points and \(\bar{\boldsymbol{p}}\) be their centroid. Compute the SVD of centered points:

\[P_c = P - \mathbf{1}\bar{\boldsymbol{p}}^{\mathsf{T}} = U \Sigma V^{\mathsf{T}}.\]

The number of singular values above a tolerance defines the effective rank, which selects the plotting primitive.

param ax:

A Matplotlib 3D axis object used for plotting.

type ax:

matplotlib.axes.Axes

param points:

Point cloud in torque space with shape (N, 3).

type points:

numpy.ndarray

param face_color:

Face color for polygon or hull rendering.

type face_color:

str

param edge_color:

Edge color for line/polygon edges and hull silhouettes.

type edge_color:

str

param alpha:

Transparency value used for filled primitives.

type alpha:

float

return:

None

rtype:

None

Parameters:

  • points (ndarray)

  • face_color (str)

  • edge_color (str)

  • alpha (float)

plot_torques(tau_des, b_body, est_sat)[source]

Plot achievable torque envelopes and the allocated torque vectors in 3D.

This method: - Calls allocate_max_torque_in_direction() to obtain RW and MTQ commands and the effectiveness metric \(\alpha\). - Constructs representative point clouds of extreme actuator combinations (hypercube corners) for RWs and MTQs and draws their convex hulls. - Plots vectors for RW torque, MTQ torque, total achieved torque, and desired torque.

Torque reconstruction

RW torque is reconstructed as:

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

MTQ torque is reconstructed from dipole moment and magnetic field:

\[\boldsymbol{\tau}_{\mathrm{mtq}} = \boldsymbol{m} \times \boldsymbol{B}, \qquad \boldsymbol{m} = A_{\mathrm{mtq,axes}}\boldsymbol{u}_{\mathrm{mtq}}.\]

The achieved torque is:

\[\boldsymbol{\tau}_{\mathrm{ach}} = \boldsymbol{\tau}_{\mathrm{rw}} + \boldsymbol{\tau}_{\mathrm{mtq}}.\]

Hull plotting uses plot_capacity().

param tau_des:

Desired body-frame torque vector to visualize.

type tau_des:

numpy.ndarray

param b_body:

Body-frame magnetic field vector used for MTQ torque mapping.

type b_body:

numpy.ndarray

param est_sat:

Estimated satellite providing actuator configuration and limits.

type est_sat:

EstimatedSatellite

return:

None

rtype:

None

Parameters:
Return type:

None

Parameters:

  • est_sat (EstimatedSatellite)

  • p_gain (float)

  • d_gain (float)

  • c_gain (float)

  • h_target (ndarray | list)