ADCS.estimators.attitude_estimators.attitude_UAKF module

class ADCS.estimators.attitude_estimators.attitude_UAKF.UAKF(est_sat, J2000, x_hat, P_hat, Q_hat, dt=1.0, cross_term=False, quat_as_vec=False)

Bases: Attitude_Estimator

Unscented Kalman Filter (UKF) with an error-state attitude representation.

This class implements an Unscented Kalman Filter (UKF) for spacecraft attitude estimation and associated bias/parameter states. It inherits the augmented-state bookkeeping (full-state storage, reduced covariance representation, and satellite synchronization) from Attitude_Estimator.

The implementation uses an error-state attitude representation by default (quat_as_vec=False), in which the quaternion is stored in the full state vector but the covariance is stored in a minimal 3-parameter attitude-error coordinate (e.g., \(\delta\boldsymbol{\theta}\in\mathbb{R}^3\)). If quat_as_vec=True, the quaternion is included as a 4-vector in the covariance.

State and covariance structure

Let the full augmented state be

\[\begin{split}\mathbf{x} = \begin{bmatrix} \boldsymbol{\omega} \\ \mathbf{q} \\ \mathbf{z} \end{bmatrix},\end{split}\]

where \(\boldsymbol{\omega}\in\mathbb{R}^3\) is the angular rate, \(\mathbf{q}\in\mathbb{R}^4\) is the unit quaternion, and \(\mathbf{z}\) stacks remaining physical/bias/disturbance states.

In reduced error-state mode, the filter internally works with an error vector

\[\begin{split}\delta\mathbf{x} = \begin{bmatrix} \delta\boldsymbol{\omega} \\ \delta\boldsymbol{\theta} \\ \delta\mathbf{z} \end{bmatrix},\end{split}\]

with \(\delta\boldsymbol{\theta}\in\mathbb{R}^3\) representing attitude error, typically mapped to a quaternion increment \(\delta\mathbf{q}\) and composed multiplicatively with a reference quaternion. The full state always stores \(\mathbf{q}\) explicitly.

Unscented transform overview

For an augmented dimension \(L\), the UKF constructs sigma points \(\chi_i\) around the current mean using

\[\lambda = \alpha^2 (L + \kappa) - L,\qquad \gamma = \sqrt{L + \lambda}.\]

Mean and covariance weights are

\[W_0^{(m)} = \frac{\lambda}{L+\lambda},\qquad W_0^{(c)} = \frac{\lambda}{L+\lambda} + (1-\alpha^2+\beta),\]

\[W_i^{(m)} = W_i^{(c)} = \frac{1}{2(L+\lambda)},\quad i=1,\ldots,2L.\]

Sigma points are propagated through the nonlinear dynamics

\[\chi_i^- = f(\chi_i, \mathbf{u}, \mathbf{w}_i),\]

where propagation is performed by noiseless_rk4() with appropriate injected control perturbations. Predicted measurements are

\[\gamma_i^- = h(\chi_i^-),\]

computed using sensor_readings() (with an ErrorMode selecting bias/noise behavior).

Predicted means are

\[\bar{\mathbf{x}}^- = \sum_{i=0}^{2L} W_i^{(m)} \chi_i^-,\qquad \bar{\mathbf{y}}^- = \sum_{i=0}^{2L} W_i^{(m)} \gamma_i^-.\]

Covariances are

\[P^- = \sum_{i=0}^{2L} W_i^{(c)}(\chi_i^- - \bar{\mathbf{x}}^-)(\chi_i^- - \bar{\mathbf{x}}^-)^\top + Q,\]

\[P_{yy} = \sum_{i=0}^{2L} W_i^{(c)}(\gamma_i^- - \bar{\mathbf{y}}^-)(\gamma_i^- - \bar{\mathbf{y}}^-)^\top + R,\]

\[P_{y x} = \sum_{i=0}^{2L} W_i^{(c)}(\gamma_i^- - \bar{\mathbf{y}}^-)(\chi_i^- - \bar{\mathbf{x}}^-)^\top.\]

The gain is formed as

\[K = P_{x y} P_{yy}^{-1},\]

and the mean/covariance are updated using innovation \(\boldsymbol{\nu} = \mathbf{y} - \bar{\mathbf{y}}^-\):

\[\bar{\mathbf{x}}^+ = \bar{\mathbf{x}}^- + K\boldsymbol{\nu},\qquad P^+ = P^- - K P_{yy} K^\top.\]

Quaternion error composition

In reduced attitude-error mode, attitude is updated multiplicatively:

\[\delta\mathbf{q} = \operatorname{vec3\_to\_quat}(\delta\boldsymbol{\theta}),\qquad \mathbf{q}^+ = \mathbf{q}^- \otimes \delta\mathbf{q},\]

using vec3_to_quat() and quat_mult(). If quat_as_vec=True, the quaternion is renormalized via normalize(), and covariance is transformed using state_norm_jac().

add_to_state(state, add)

Apply an error perturbation to a base state on the attitude manifold.

This method implements the sigma-point “addition” operator. In Euclidean state components, it is additive. For quaternion attitude, it is multiplicative when quat_as_vec=False.

Single-point (vector) case

For a single base state \(\mathbf{x}\) and perturbation \(\delta\mathbf{x}\), the update is:

• If quat_as_vec=True (additive quaternion with renormalization):

  \[\mathbf{x}^+ = \mathbf{x} + \delta\mathbf{x},\qquad \mathbf{q}^+ \leftarrow \frac{\mathbf{q}^+}{\lVert \mathbf{q}^+ \rVert}.\]

  Quaternion normalization uses normalize().

• If quat_as_vec=False (multiplicative quaternion):

  \[\boldsymbol{\omega}^+ = \boldsymbol{\omega} + \delta\boldsymbol{\omega},\]
  \[\delta\mathbf{q} = \operatorname{vec3\_to\_quat}(\delta\boldsymbol{\theta}),\qquad \mathbf{q}^+ = \mathbf{q} \otimes \delta\mathbf{q},\]
  \[\mathbf{z}^+ = \mathbf{z} + \delta\mathbf{z}.\]

  Quaternion composition uses quat_mult() and the vector-to-quaternion map uses vec3_to_quat().

Batch (matrix) case

If add is a 2D array (sigma-point perturbations), the same operations are applied row-wise. For quat_as_vec=True, quaternion rows are normalized with matrix_row_normalize().

param state:

Base state vector (1D) or base full state vector (1D) used for all sigma points.

type state:

numpy.ndarray

param add:

Perturbation/error vector(s), shape (n,) or (k,n).

type add:

numpy.ndarray

return:

Updated state vector(s) after applying the perturbation on the manifold.

rtype:

numpy.ndarray

Parameters:

• state (ndarray)

• add (ndarray)

Return type:

ndarray

determine_covariances_to_use(state_cov, sens_cov, control_cov, int_cov)

Determine which covariance blocks are included in sigma-point augmentation.

The UKF may be implemented with an augmented state that includes process noise and/or measurement noise as additional random variables. This method encodes the estimator’s policy for which covariance blocks contribute to the augmented dimension \(L\) used in sigma point generation.

In this implementation, the returned list corresponds to:

\[[\text{state},\ \text{sensor},\ \text{control},\ \text{process}],\]

determining whether each covariance block is included in the sigma-point construction in make_pts_and_wts().

Parameters:

• state_cov (numpy.ndarray) – State covariance matrix \(P\) (reduced or full according to quat_as_vec).

• sens_cov (numpy.ndarray) – Sensor noise covariance matrix \(R\) for active sensors.

• control_cov (numpy.ndarray) – Control noise covariance associated with actuation commands.

• int_cov (numpy.ndarray) – Process (integration) noise covariance \(Q\).

Returns:

A 4-element boolean list indicating whether to include [state, sensor, control, process] covariance blocks.

Return type:

list[bool]

make_pts_and_wts(pt0, which_sensors)

Build augmented sigma points and Unscented Transform weights.

This method constructs sigma points in an augmented space comprised of selected covariance blocks (state, sensor noise, control noise, process noise). The inclusion policy is determined by determine_covariances_to_use().

Augmented dimension

Let covariance blocks be:

• state covariance \(P_x\),

• sensor covariance \(R\),

• control covariance \(Q_u\),

• process covariance \(Q\).

With inclusion flags \(b_j\in\{0,1\}\), the augmented dimension is

\[L = \sum_{j} b_j\, n_j,\]

where \(n_j\) is the dimension of the corresponding block.

Sigma point generation

Define

\[\lambda = \alpha^2(L+\kappa) - L,\qquad \gamma = \sqrt{L + \lambda},\qquad \text{scale} = L + \lambda.\]

For each included block with covariance \(P\), the method forms the scaled Cholesky factor:

\[\text{scale}\,P = LL^\top,\]

using numpy.linalg.cholesky(), then creates offsets

\[\Delta_i \in \{\pm L_{:,k}\}_{k=1}^{n},\]

yielding \(2n\) offsets per block.

For the state block (error-state), offsets are mapped onto the manifold using add_to_state(), ensuring multiplicative quaternion perturbations when quat_as_vec=False.

For non-state blocks (noise blocks), offsets are inserted into the sigma-point container list structure as additive perturbations on those blocks only.

Weights

Mean and covariance weights follow the standard UKF formulas:

\[W_0^{(m)} = \frac{\lambda}{L+\lambda},\qquad W_0^{(c)} = \frac{\lambda}{L+\lambda} + (1-\alpha^2+\beta),\]

\[W_i^{(m)} = W_i^{(c)} = \frac{1}{2(L+\lambda)},\quad i=1,\ldots,2L.\]

Output formats

The method returns:

• pts: a list of length \(2L+1\) where each entry is:

  \[[\text{full\_state},\ \text{sens\_noise},\ \text{control\_noise},\ \text{int\_noise}].\]

• sig0: a stacked array of the state component only, aligned with pts. Points not perturbed in the state block are padded with the mean state.

param pt0:

Current mean full state vector.

type pt0:

numpy.ndarray

param which_sensors:

Boolean mask indicating which sensors are active/valid.

type which_sensors:

list[bool]

return:

Tuple (L, pts, wts_m, wts_c, sig0) where:

• L is the augmented dimension,

• pts is the sigma-point container list,

• wts_m are mean weights (length \(2L+1\)),

• wts_c are covariance weights (length \(2L+1\)),

• sig0 is the stacked state-only sigma-point array aligned with pts.

rtype:

tuple[int, list, numpy.ndarray, numpy.ndarray, numpy.ndarray]

Parameters:

• pt0 (ndarray)

• which_sensors (List[bool])

new_post_state(pre_rest_state, post_dynstate, int_err, quatref)

Construct posterior reduced and full states after propagation with integration error.

This helper reconstructs, for a given sigma point, both:

• post_state: the reduced error-state representation used internally by the UKF,

• full_state: the full augmented state with quaternion suitable for satellite models.

Integration error injection

The integration error vector int_err is interpreted in the reduced error-state coordinates and partitioned into:

• a head portion applied to the dynamic state on the manifold via add_to_state(),

• a tail portion applied additively to the remaining bias/parameter states.

If state_len is the satellite dynamic-state length (including quaternion), the reduced “head length” used is

\[n_h = \text{state\_len} - 1 + \mathbf{1}[\text{quat\_as\_vec}],\]

matching the implementation.

Full-state reconstruction

After obtaining the reduced posterior vector post_state, the method constructs an auxiliary reference state containing quatref and then re-applies the reduced perturbation using add_to_state() to produce a full state with a quaternion.

param pre_rest_state:

Bias/parameter states before propagation (assumed constant through dynamics).

type pre_rest_state:

numpy.ndarray

param post_dynstate:

Propagated dynamic state (rate + quaternion + other dynamic entries).

type post_dynstate:

numpy.ndarray

param int_err:

Integration error/process-noise vector for this sigma point (reduced coordinates).

type int_err:

numpy.ndarray

param quatref:

Reference quaternion used for sigma-point attitude error mapping.

type quatref:

numpy.ndarray

return:

Tuple (post_state, full_state) where post_state is the reduced error-state vector and full_state is the full augmented state vector.

rtype:

tuple[numpy.ndarray, numpy.ndarray]

Parameters:

• pre_rest_state (ndarray)

• post_dynstate (ndarray)

• int_err (ndarray)

• quatref (ndarray)

reunite_states(dynstate, rest_state, quatref)

Reassemble the reduced error-state vector from propagated dynamics and static components.

This helper constructs a reduced state representation used internally by the filter for sigma-point bookkeeping.

Quaternion mapping

If quat_as_vec=True, the quaternion is treated as part of the additive state vector and the method simply concatenates:

\[\begin{split}\mathbf{x}_{red} = \begin{bmatrix}\mathbf{x}_{dyn} \\ \mathbf{x}_{rest}\end{bmatrix}.\end{split}\]

If quat_as_vec=False, the propagated dynamic state dynstate contains the full quaternion \(\mathbf{q}\). The method converts this quaternion into a 3-vector attitude error relative to a reference quaternion \(\mathbf{q}_{ref}\) by computing:

\[\delta\mathbf{q} = \mathbf{q}_{ref}^{-1} \otimes \mathbf{q},\]

using quat_inv() and quat_mult(), then mapping \(\delta\mathbf{q}\) to a 3-vector:

\[\delta\boldsymbol{\theta} = \operatorname{quat\_to\_vec3}(\delta\mathbf{q}),\]

via quat_to_vec3().

The returned reduced vector is arranged as:

\[[\boldsymbol{\omega},\ \delta\boldsymbol{\theta},\ \text{(other dyn)},\ \text{rest\_state}].\]

param dynstate:

Dynamic part of the state (rate + attitude + other dynamic entries); contains a full quaternion.

type dynstate:

numpy.ndarray

param rest_state:

Static/bias/parameter portion of the state appended after the dynamic portion.

type rest_state:

numpy.ndarray

param quatref:

Reference quaternion used to compute the attitude error (ignored if quat_as_vec=True).

type quatref:

numpy.ndarray

return:

Reduced error-state vector consistent with the estimator’s internal representation.

rtype:

numpy.ndarray

Parameters:

• dynstate (ndarray)

• rest_state (ndarray)

• quatref (ndarray)

Return type:

ndarray

sat_match(est_sat, state)

Synchronize an EstimatedSatellite instance with a raw full state vector.

Sigma-point propagation and measurement prediction require the satellite model to reflect the sigma point’s specific biases/parameters and attitude. This method constructs an EstimatedArray compatible container, inserts the provided raw state entries at indices defined by use, and then updates the satellite model via match_estimate().

Parameters:

• est_sat (EstimatedSatellite) – Satellite model instance to be updated for sigma-point propagation.

• state (numpy.ndarray) – Full augmented state vector to apply.

Returns:

None.

Return type:

None

update_core(u, sensors, os)

Perform one UKF predict/update cycle in reduced error-state coordinates.

This method implements a complete UKF step:

1. Determine active sensors from the provided measurement vector (NaN check), then expand to an output-level mask via _expand_sensor_mask().

2. Build augmented sigma points via make_pts_and_wts().

3. Propagate each sigma point through nonlinear dynamics using noiseless_rk4().

4. Predict measurements for each propagated sigma point using sensor_readings() with ErrorMode selecting bias inclusion.

5. Compute predicted mean/covariances using unscented weights.

6. Solve for Kalman gain, apply innovation update, and map the reduced attitude error back to a full quaternion state.

Orbital interpolation (CG5)

This implementation computes intermediate orbital states between prev_os and the current Orbital_State using coefficients from CG5:

\[\mathbf{o}_j = \operatorname{average}(\mathbf{o}_{prev}, \mathbf{o}, c_j),\qquad j=0,\ldots,4,\]

via average(), where \(c_j\) are the CG5 coefficients.

Unscented prediction and update

Let propagated reduced sigma points be \(\chi_i^-\) and predicted measurement sigma points be \(\gamma_i^-\). Predicted means:

\[\bar{\mathbf{x}}^- = \sum_{i=0}^{2L} W_i^{(m)} \chi_i^-,\qquad \bar{\mathbf{y}}^- = \sum_{i=0}^{2L} W_i^{(m)} \gamma_i^-.\]

Deviations:

\[\mathbf{X}_i = \chi_i^- - \bar{\mathbf{x}}^-,\qquad \mathbf{Y}_i = \gamma_i^- - \bar{\mathbf{y}}^-.\]

Covariances:

\[P^- = \sum_{i=0}^{2L} W_i^{(c)} \mathbf{X}_i\mathbf{X}_i^\top + Q,\]

\[P_{yy} = \sum_{i=0}^{2L} W_i^{(c)} \mathbf{Y}_i\mathbf{Y}_i^\top + R,\]

\[P_{y x} = \sum_{i=0}^{2L} W_i^{(c)} \mathbf{Y}_i\mathbf{X}_i^\top.\]

Gain and update (note the implementation’s array orientation):

\[K = P_{yy}^{-1} P_{y x},\]

and with innovation \(\boldsymbol{\nu}=\mathbf{y}-\bar{\mathbf{y}}^-\):

\[\bar{\mathbf{x}}^+ = \bar{\mathbf{x}}^- + \boldsymbol{\nu}^\top K,\]

\[P^+ = P^- - K^\top P_{yy} K.\]

Quaternion reconstruction

In reduced attitude-error mode (quat_as_vec=False), the updated reduced vector contains an attitude error \(\delta\boldsymbol{\theta}\) which is mapped to a quaternion increment and composed with the propagated reference quaternion:

\[\delta\mathbf{q} = \operatorname{vec3\_to\_quat}(\delta\boldsymbol{\theta}),\qquad \mathbf{q}^+ = \mathbf{q}_{ref} \otimes \delta\mathbf{q},\]

using vec3_to_quat() and quat_mult().

If quat_as_vec=True, the quaternion is renormalized with normalize() and the covariance is transformed by the normalization Jacobian from state_norm_jac():

\[P^+ \leftarrow J^\top P^+ J.\]

param u:

Control input vector.

type u:

numpy.ndarray

param sensors:

Stacked array of actual sensor measurements.

type sensors:

numpy.ndarray

param os:

Current orbital environment state.

type os:

Orbital_State

raises numpy.linalg.LinAlgError:

If the measurement covariance matrix is singular during gain computation.

return:

Updated estimate container with updated full state and reduced covariance.

rtype:

EstimatedArray

Parameters:

• u (ndarray)

• sensors (ndarray)

• os (Orbital_State)

Return type:

EstimatedArray

Parameters:

• est_sat (EstimatedSatellite)

• J2000 (float)

• x_hat (ndarray)

• P_hat (ndarray)

• Q_hat (ndarray)

• dt (float)

• cross_term (bool)

• quat_as_vec (bool)