ADCS.controller.helpers.trajectory module

class ADCS.controller.helpers.trajectory.Trajectory(t, x, u, K, S, use_disturbance_estimation=False)

Bases: object

Container for trajectory optimization results produced by the ALTRO planner.

This class stores discrete-time trajectories of states, controls, feedback gains, and cost-to-go values, and provides interpolation utilities for real-time tracking control.

The reference trajectory defines a nominal solution \((\mathbf{x}^\ast(t), \mathbf{u}^\ast(t))\) to a nonlinear optimal control problem. A time-varying LQR feedback law is applied as

\[\mathbf{u}(t) = \mathbf{u}^\ast(t) - \mathbf{K}(t)\,\delta\mathbf{x}(t)\]

where \(\delta\mathbf{x}\) is a reduced-dimension error state that replaces the quaternion with a minimal 3D attitude error.

The class supports both row-major and column-major storage layouts and automatically detects dimensions.

Parameters:

• t (numpy.ndarray) – Time stamps associated with the trajectory samples.

• x (numpy.ndarray) – State trajectory array, either time-major or state-major.

• u (numpy.ndarray) – Control trajectory array, either time-major or control-major.

• K (numpy.ndarray) – Feedback gain matrices for TVLQR tracking.

• S (numpy.ndarray) – Cost-to-go values along the trajectory.

• use_disturbance_estimation (bool) – Flag enabling augmented error state with disturbance estimation.

compute_tracking_control(t, x_current)

Compute the tracking control input at a given time and state.

The control law is

\[\mathbf{u} = \mathbf{u}^\ast(t) - \mathbf{K}(t)\,\delta\mathbf{x}\]

where \(\delta\mathbf{x}\) is the reduced error state computed from the current and reference states.

Parameters:

• t (float) – Query time.

• x_current (numpy.ndarray) – Current full state vector.

Returns:

Control input computed by the tracking controller.

Return type:

numpy.ndarray

get_control_at(t)

Interpolate the reference control at a given time.

The control trajectory is interpolated linearly between neighboring time samples.

Parameters:

t (float) – Query time.

Returns:

Interpolated reference control vector.

Return type:

numpy.ndarray

get_gain_at(t)

Retrieve the feedback gain matrix at a given time.

The gain matrix \(\mathbf{K}(t)\) maps the reduced error state to control corrections. Multiple storage conventions are supported, including time-major, control-major, and flattened formats.

Parameters:

t (float) – Query time.

Returns:

Feedback gain matrix corresponding to the query time.

Return type:

numpy.ndarray

get_plotting_data()

Return trajectory data packaged for plotting.

Returns:

Dictionary containing time, state, control, and cost arrays.

Return type:

dict

get_state_at(t)

Interpolate the reference state at a given time.

Linear interpolation is applied between neighboring samples. Quaternion components are renormalized to preserve unit norm.

Parameters:

t (float) – Query time.

Returns:

Interpolated reference state vector.

Return type:

numpy.ndarray

get_state_input_gain(t)

Return reference state, control, and gain at a given time.

This method provides a unified interface for tracking controllers.

Parameters:

t (float) – Query time.

Returns:

Tuple containing reference state, reference control, gain matrix, and a placeholder value.

Return type:

Tuple[numpy.ndarray, numpy.ndarray, numpy.ndarray, None]

is_valid_time(t)

Check whether a time lies within the trajectory bounds.

Parameters:

t (float) – Query time.

Returns:

True if the time is within the trajectory interval.

Return type:

bool

plot_eci_trajectory(body_axis=array([0, 0, 1]), stride=1, show=True)

Plot the trajectory of a body-fixed axis expressed in the ECI frame.

The body axis is rotated into the inertial frame using the quaternion trajectory, producing a 3D curve on the unit sphere.

Parameters:

• body_axis (numpy.ndarray) – Body-frame axis to visualize.

• stride (int) – Subsampling stride for plotting.

• show (bool) – Flag indicating whether to display the plot immediately.

Returns:

None.

Return type:

None

to_simulation_results(satellite, target=None, w_target=None, boresight=None, *, include_cost=False)

Parameters:

• satellite (Satellite)

• target (ndarray | None)

• w_target (ndarray | None)

• boresight (ndarray | None)

• include_cost (bool)

Return type:

SimulationResults

update_disturbance_estimate(dist_torque)

Update the internal disturbance torque estimate.

This method is used when disturbance estimation is enabled in the tracking controller.

Parameters:

dist_torque (numpy.ndarray) – Estimated disturbance torque in the body frame.

Returns:

None.

Return type:

None

controls: ndarray[tuple[int, ...], dtype[float64]]

costs: ndarray[tuple[int, ...], dtype[float64]]

ctrl_dim: int

end_time: float

gains: ndarray[tuple[int, ...], dtype[float64]]

n_steps: int

start_time: float

state_dim: int

states: ndarray[tuple[int, ...], dtype[float64]]

times: ndarray[tuple[int, ...], dtype[float64]]