ADCS.estimators.attitude_estimators.attitude_SRUKF module¶

1. Construct the square-root tangent-state filter

SRUKF uses the same physical state, right tangent attitude error, and automatic actuator-noise prediction augmentation as UKF:

\[\hat{\mathbf{x}}_k = [\hat{\boldsymbol{\omega}}_k,\hat{\mathbf{q}}_k,\hat{\mathbf{h}}_k]^T, \qquad \delta\mathbf{x}_k = [\delta\boldsymbol{\omega}_k, \delta\boldsymbol{\theta}_k,\delta\mathbf{h}_k]^T \in\mathbb{R}^{6+n_h}.\]

The defining difference is the covariance representation. Instead of storing \(\mathbf{P}\) directly, Covariance stores an upper factor \(\mathbf{S}\) such that

\[\mathbf{P}=\mathbf{S}^T\mathbf{S}.\]

The constructor converts both the state covariance and process-noise covariance to form="sqrt" before entering the shared UKF lifecycle.

result = state.copy()
result.covariance = state.covariance.copy(form="sqrt")
result.process_noise = state.process_noise.copy(form="sqrt")
super().__init__(
    satellite, result, dt=dt,
    quaternion_mode=quaternion_mode,
    alpha=alpha, beta=beta, kappa=kappa,
    ...,
)

2. Generate tangent sigma points from the square-root factor

For tangent dimension \(n=6+n_h\),

\[\lambda=\alpha^2(n+\kappa)-n, \qquad \gamma=\sqrt{n+\lambda}, \qquad \mathbf{P}=\mathbf{S}^T\mathbf{S}.\]

The sigma offsets are rows of the upper factor:

\[\boldsymbol{\xi}_0=\mathbf{0}, \qquad \boldsymbol{\xi}_i=\pm\gamma\,\mathbf{S}_{i,:}^T, \qquad \mathbf{X}_i=\hat{\mathbf{x}}\boxplus\boldsymbol{\xi}_i.\]

The mean and covariance weights are the standard unscented weights. No square-root factor is expanded into a full covariance to generate points.

points, offsets, mean_weights, covariance_weights = self._sigma_states(
    prior
)

3. Propagate sigma points and form the prediction

Each sigma point is independently propagated:

\[\mathbf{X}_{i,k+1}^- = f(\mathbf{X}_{i,k}^+,\mathbf{u}_k, \mathbf{o}_k,\mathbf{o}_{k+1},\Delta t).\]

As in the UKF, process noise is constructed separately and added after the propagated sigma-point deviations have been computed.

propagated_points = [
    propagate_state(
        point, self.satellite, control, step,
        orbital_state_start, orbital_state_end,
        midpoint_orbital_state=midpoint_orbital_state,
    )
    for point in points
]

After propagation, compute the manifold mean and square-root predicted covariance.

The predicted state is the weighted manifold mean:

\[\sum_i W_i^{(m)} (\mathbf{X}_{i,k+1}^-\boxminus\hat{\mathbf{x}}_{k+1}^-)=\mathbf{0}.\]

With \(\mathbf{d}_{i,k}^x\) denoting the local sigma deviations, the covariance is mathematically

\[\mathbf{P}_{k+1}^- = \sum_i W_i^{(c)}\mathbf{d}_{i,k}^x(\mathbf{d}_{i,k}^x)^T +\mathbf{Q}_{d,k}^{model}+\mathbf{Q}_{d,k}^{state}.\]

In SRUKF, Covariance forms its upper factor using QR factorization and rank updates/downdates, preserving \(\mathbf{P}=\mathbf{S}^T\mathbf{S}\) as the public representation.

predicted = self._state_mean(propagated_points, mean_weights)
transition, process_noise = discretize_process_noise(
    prior, self.satellite, control, orbital_state_start, step,
    final_state=predicted,
    quaternion_mode=self.correction_mode,
    quaternion_order="right",
    unmodeled_dynamics_psd=self.unmodeled_dynamics_psd,
)
deviations = np.vstack([
    point.minus(predicted,
        quaternion_mode=self.correction_mode,
        quaternion_order="right")
    for point in propagated_points
])
predicted.covariance = prior.covariance.predicted_unscented(
    deviations, covariance_weights, process_noise
)

4. Predict measurements and form their statistics

Active sigma points are passed through the measurement stack:

\[\mathbf{Z}_{i,k}=h(\mathbf{X}_{i,k}^-,\mathbf{o}_k).\]

Additive measurements use a weighted Euclidean mean. Quaternion measurements use a weighted right-error manifold mean in the selected three-parameter chart:

\[\sum_i W_i^{(m)} (\mathbf{Z}_{i,k}\boxminus\hat{\mathbf{z}}_k)=\mathbf{0}.\]

sigma_measurements = [
    stack.predict(point, orbital_state, active_mask=active)
    for point in points
]
predicted_measurement = self._measurement_mean(
    stack, sigma_measurements, active, mean_weights
)

Then form \(\mathbf{P}_{zz}\) and \(\mathbf{P}_{xz}\) from the measurement and state deviations.

Measurement deviations are expressed in residual coordinates and state deviations in tangent coordinates:

\[\mathbf{d}_{i,k}^z=\mathbf{Z}_{i,k}\boxminus\hat{\mathbf{z}}_k, \qquad \mathbf{P}_{zz}=\sum_i W_i^{(c)}\mathbf{d}_{i,k}^z (\mathbf{d}_{i,k}^z)^T+\mathbf{R}_k,\]

\[\mathbf{P}_{xz}=\sum_i W_i^{(c)}\mathbf{d}_{i,k}^x (\mathbf{d}_{i,k}^z)^T.\]

The square-root form computes the measurement covariance using QR and weighted Cholesky updates where needed; no measurement Jacobian is formed.

measurement_deviations = np.vstack([
    stack.residual(
        measurement, predicted_measurement, active,
        quaternion_mode=self.measurement_quaternion_mode,
    )
    for measurement in sigma_measurements
])
measurement_noise = stack.covariance(
    prior, active,
    quaternion_mode=self.measurement_quaternion_mode,
)

5. Compute the square-root unscented gain and correction

The gain is obtained from the weighted cross-covariance and measurement covariance:

\[\mathbf{K}_k=\mathbf{P}_{xz}\mathbf{P}_{zz}^{-1}, \qquad \mathbf{r}_k=\mathbf{z}_k\boxminus\hat{\mathbf{z}}_k, \qquad \delta\mathbf{x}_k=\mathbf{K}_k\mathbf{r}_k.\]

updated_unscented solves with the stored square-root representation and returns the posterior covariance in square-root form.

gain, posterior_covariance = prior.covariance.updated_unscented(
    state_deviations,
    measurement_deviations,
    covariance_weights,
    measurement_noise,
)
innovation = stack.residual(
    measurements, predicted_measurement, active,
    quaternion_mode=self.measurement_quaternion_mode,
)
correction = gain @ innovation

6. Retract the state and preserve square-root covariance storage

The tangent correction is applied as a right multiplicative quaternion update:

\[\mathbf{x}_k^+=\mathbf{x}_k^-\boxplus\delta\mathbf{x}_k, \qquad \mathbf{q}_k^+=\hat{\mathbf{q}}_k^- \otimes\phi^{-1}(\delta\boldsymbol{\theta}_k).\]

The posterior covariance is transported to the new tangent origin. Because the input covariance is square-root form, the transformed result preserves the upper-factor representation:

\[\mathbf{P}_k^+= \mathbf{J}_{reset}\mathbf{P}_{k,raw}^+\mathbf{J}_{reset}^T, \qquad \mathbf{P}_k^+=\mathbf{S}_k^{+T}\mathbf{S}_k^+.\]

corrected = prior.plus(
    correction,
    quaternion_mode=self.correction_mode,
    quaternion_order="right",
)
corrected.covariance = prior.transport_covariance(
    posterior_covariance,
    correction,
    quaternion_mode=self.correction_mode,
    quaternion_order="right",
)

class ADCS.estimators.attitude_estimators.attitude_SRUKF.SRUKF(satellite, state, *, dt, unmodeled_dynamics_psd=0.0, quaternion_mode='quaternion_vector', alpha=1.0, beta=2.0, kappa=0.0)[source]¶

Bases: UKF

UKF with square-root covariance storage.

This filter uses the same tangent-state and automatic control-noise sigma points, manifold means, and additive process and measurement noise as UKF. Its covariance is always retained in Covariance(form="sqrt") form, so the shared unscented covariance operations use square-root QR factorizations and Cholesky updates/downdates.

Parameters:

satellite (Any)
state (EstimatorState)
dt (float)
unmodeled_dynamics_psd (Any)
quaternion_mode (str)
alpha (float)
beta (float)
kappa (float)

reset(state)[source]¶

Reset while preserving square-root covariance storage.

Parameters:: state (EstimatorState)
Return type:: EstimatorState