ADCS.estimators.attitude_estimators.attitude_SRUAKF module

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

Bases: UAKF

Square Root Unscented Kalman Filter (SR-UKF) using choldate rank-1 updates.

This class implements a numerically stable Square Root UKF (SR-UKF) for spacecraft attitude estimation with an augmented state and reduced attitude error representation inherited from UAKF.

Instead of propagating the covariance matrix \(P\), this filter propagates an upper-triangular Cholesky factor \(S\) such that

\[P = S^\top S,\]

which improves numerical stability and helps keep \(P\) symmetric positive semi-definite under finite precision arithmetic.

This implementation uses the external C-extension choldate for fast rank-1 Cholesky updates and downdates, which is particularly important for the SR-UKF because the 0th sigma point can carry a negative covariance weight \(W_0^{(c)} < 0\), requiring a Cholesky downdate.

State and reduced error representation

Let the (possibly augmented) estimator state be \(\mathbf{x}\). In the common reduced-attitude-error formulation, the quaternion is not directly included as 4 DOF in the covariance; instead, a 3-parameter error vector \(\delta\boldsymbol{\theta}\in\mathbb{R}^3\) is used, so the reduced covariance dimension is \(L_x = N-1\) (unless quat_as_vec=True).

Process noise is represented by a covariance \(Q\) with square root \(S_Q\):

\[Q = S_Q^\top S_Q.\]

See also: - UAKF - EstimatedArray - EstimatedSatellite

make_pts_and_wts(pt0, which_sensors)

Generate augmented sigma points using the stored square root covariance.

This method generates sigma points for a UKF-style estimator, while exploiting the persistently tracked square-root factor \(S\) of the reduced covariance for efficiency.

It produces the standard UKF outputs:

• augmented dimension \(L\),

• sigma point container list pts of length \(2L+1\),

• mean weights \(W_i^{(m)}\),

• covariance weights \(W_i^{(c)}\),

• state-only sigma points array sig0 aligned with pts.

Unscented transform weights

Let \(L\) be the augmented dimension. With standard UKF parameters \(\alpha\), \(\beta\), \(\kappa\) (stored in the base class), define

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

The 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 point construction with square root factor

For the (reduced) state block of size \(L_x\), the covariance is

\[P_x = S^\top S,\]

where \(S\) is upper-triangular. If \(L_x = \dim(\mathbf{x})\) in the reduced space, sigma-point offsets are formed from the rows of \(S\) scaled by \(\gamma\):

\[\Delta X = \gamma S.\]

These offsets are applied to the mean state \(\bar{\mathbf{x}}\) using the estimator-specific state addition (which must respect quaternion error composition), via the inherited method add_to_state().

Augmentation blocks

The method supports an augmented space including:

• state error block (from \(S\)),

• control/process noise block (from control_cov()),

• sensor noise block (shape tracked but treated as zero contribution in this architecture),

• integration noise block (shape tracked but treated as zero contribution here).

For the control/process noise block with covariance \(Q_u\), sigma offsets are formed from a Cholesky factor \(L_u\) such that \(Q_u = L_u L_u^\top\).

Output alignment

The returned pts list stores each sigma point as a 4-list:

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

The array sig0 is a stacked view of the state component only, aligned in order with pts; sigma points whose perturbation is purely in noise blocks are padded with the mean state.

param pt0:

Current mean full state vector (augmented state representation).

type pt0:

numpy.ndarray

param which_sensors:

Boolean mask indicating which sensors are active.

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 list,

• wts_m are mean weights,

• wts_c are covariance weights,

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

rtype:

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

Parameters:

• pt0 (ndarray)

• which_sensors (List[bool])

update_core(u, sensors, os)

Perform one SR-UKF predict/update cycle using QR factorization and Cholesky downdates.

This method executes a full SR-UKF cycle in square-root form:

1. Time update (prediction): propagate sigma points through dynamics and compute the predicted mean and predicted square-root covariance \(S^-\).

2. Measurement update: predict measurements, compute innovation covariance square root \(S_{yy}\), compute the Kalman gain, update the mean.

3. Square-root covariance update: apply sequential Cholesky downdates using choldate.choldowndate() via weighted_cholupdate().

The method returns an EstimatedArray containing the updated full state and a reconstructed covariance \(P^+\).

Notation

Let sigma points be \(\chi_i\) with weights \(W_i^{(m)}\), \(W_i^{(c)}\). Let the propagated sigma points be \(\chi_i^-\) and the predicted mean be \(\bar{\mathbf{x}}^-\).

Let measurement sigma points be \(\gamma_i^- = h(\chi_i^-)\) with predicted mean \(\bar{\mathbf{y}}^-\).

Sigma point propagation

Dynamics propagation is performed using the satellite model RK4 integrator:

• noiseless_rk4()

for each sigma point, with control perturbations applied where relevant.

Predicted mean

The predicted reduced mean is formed by the unscented transform:

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

Quaternion handling

In reduced attitude-error mode (quat_as_vec=False), sigma points encode a 3-vector attitude error \(\delta\boldsymbol{\theta}\) (stored in the reduced state) which is mapped to a quaternion increment via vec3_to_quat() and composed with the propagated reference quaternion using quat_mult():

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

Predicted square-root covariance via QR

Let deviations be

\[\mathbf{X}_i = \chi_i^- - \bar{\mathbf{x}}^-.\]

Form the weighted deviation matrix excluding the 0th point:

\[A = \begin{bmatrix} \sqrt{W_1^{(c)}}\,\mathbf{X}_1 & \cdots & \sqrt{W_{2L}^{(c)}}\,\mathbf{X}_{2L} \end{bmatrix}^\top.\]

Augment with the process-noise square root (transposed to align shapes) and compute an upper-triangular factor via QR:

\[\begin{bmatrix} A & S_Q^\top \end{bmatrix} = QR,\qquad S^- \leftarrow R.\]

Because \(W_0^{(c)}\) may be negative, the 0th point is incorporated via a rank-1 update/downdate:

\[S^- \leftarrow \operatorname{cholop}\!\left(S^-, \mathbf{X}_0, W_0^{(c)}\right),\]

where \(\operatorname{cholop}\) is implemented by weighted_cholupdate().

Measurement square-root covariance

Similarly, with measurement deviations

\[\mathbf{Y}_i = \gamma_i^- - \bar{\mathbf{y}}^-,\]

and sensor noise covariance \(R = S_R^\top S_R\), compute

\[\begin{bmatrix} \sqrt{W_1^{(c)}}\,\mathbf{Y}_1 & \cdots & \sqrt{W_{2L}^{(c)}}\,\mathbf{Y}_{2L} & S_R^\top \end{bmatrix} = QR,\qquad S_{yy} \leftarrow R,\]

then incorporate \(\mathbf{Y}_0\) with the weighted rank-1 operation.

Cross-covariance and Kalman gain

The cross-covariance is computed as

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

The Kalman gain is obtained by solving

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

without explicitly forming \(P_{yy}\) by using triangular solves with \(S_{yy}\) (upper-triangular):

\[P_{yy} = S_{yy}^\top S_{yy},\]

and for each right-hand side using forward/backward substitution. In code, this corresponds to calls equivalent to scipy.linalg.solve_triangular().

Innovation and mean update

With measurement \(\mathbf{y}\), innovation

\[\boldsymbol{\nu} = \mathbf{y} - \bar{\mathbf{y}}^-,\]

and mean update

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

Square-root covariance update (downdate form)

The SR-UKF covariance update can be expressed in square-root downdate form. Let

\[U = S_{yy}\,K^\top.\]

Then

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

In square-root form, this is implemented by sequential Cholesky downdates:

\[S^+ \leftarrow \operatorname{choldowndate}(S^-, U).\]

This method performs that operation via weighted_cholupdate() with a negative weight.

Reconstruction and compatibility

For compatibility with other estimator infrastructure, the method reconstructs a covariance matrix

\[P^+ = S^{+\top} S^+,\]

symmetrizes it, and returns it in the resulting EstimatedArray.

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

Sensor activation logic

Active attitude sensors are inferred from the actual measurement vector by checking each sensor output slice for NaNs. The sensor-output mask is expanded using the inherited helper method _expand_sensor_mask().

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 innovation covariance square root is singular during triangular solves.

return:

Updated estimate container with fields:

• val: updated full augmented state,

• cov: reconstructed reduced covariance \(P^+\),

• int_cov: unchanged (inherited from internal state).

rtype:

EstimatedArray

Parameters:

• u (ndarray)

• sensors (ndarray)

• os (Orbital_State)

Return type:

EstimatedArray

weighted_cholupdate(mat, vec, wt)

Perform a weighted rank-1 Cholesky update/downdate using choldate.

This method updates an upper-triangular Cholesky factor \(S\) (named mat) to reflect adding or subtracting a weighted outer product of a vector \(\mathbf{v}\):

• For \(wt \ge 0\) (update):

  \[S_{\text{new}}^\top S_{\text{new}} = S^\top S + wt\, \mathbf{v}\mathbf{v}^\top.\]

• For \(wt < 0\) (downdate):

  \[S_{\text{new}}^\top S_{\text{new}} = S^\top S - |wt|\, \mathbf{v}\mathbf{v}^\top.\]

Implementation detail

The choldate routines operate in-place on the Cholesky factor and accept the scaled vector \(\sqrt{|wt|}\,\mathbf{v}\).

This wrapper provides:

• sign logic: choldate.cholupdate() if \(wt\ge 0\), choldate.choldowndate() if \(wt < 0\),

• handling of matrix inputs vec (multiple vectors) by applying sequential rank-1 operations.

Matrix input semantics

If vec is a 2D array with multiple vectors, the method applies

\[S \leftarrow \operatorname{cholop}(S, \mathbf{v}_1, wt),\; S \leftarrow \operatorname{cholop}(S, \mathbf{v}_2, wt),\;\ldots\]

where \(\operatorname{cholop}\) is an update or downdate depending on the sign of \(wt\).

param mat:

Upper-triangular Cholesky factor \(S\) to be modified.

type mat:

numpy.ndarray

param vec:

Update vector \(\mathbf{v}\) (shape (n,)) or a matrix of vectors (shape (k,n) or (n,k)) to be applied sequentially.

type vec:

numpy.ndarray

param wt:

Scalar weight controlling update (nonnegative) or downdate (negative).

type wt:

float

raises ValueError:

If vec has more than 2 dimensions.

raises numpy.linalg.LinAlgError:

If NaN/Inf appears in the updated factor.

return:

Updated upper-triangular Cholesky factor \(S_{\text{new}}\).

rtype:

numpy.ndarray

Parameters:

• mat (ndarray)

• vec (ndarray)

• wt (float)

Return type:

ndarray

Parameters:

• est_sat (EstimatedSatellite)

• J2000 (float)

• x_hat (ndarray)

• P_hat (ndarray)

• Q_hat (ndarray)

• dt (float)

• cross_term (bool)

• quat_as_vec (bool)