ADCS.estimators.attitude_estimators.attitude_MEKF module

1. Construct the tangent-space estimate

The MEKF estimates the physical state

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

\(\hat{\mathbf{q}}\) remains a four-element unit quaternion in the nominal state, but its uncertainty is represented by the three-element right attitude error

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

The EstimatorState covariance is therefore stored by Covariance in tangent coordinates. The selected quaternion_mode is quaternion_vector by default, or rotation_vector when requested; both are three-parameter charts.

class MEKF(AttitudeEstimator):
    def __init__(self, satellite, state, *, dt, quaternion_mode, ...):
        super().__init__(
            satellite, state, dt=dt,
            covariance_coordinates="tangent",
            correction_mode=quaternion_mode,
            measurement_quaternion_mode=quaternion_mode,
            ...,
        )

2. Propagate the nominal state

The deterministic spacecraft model propagates the nominal physical state from \(t_k\) to \(t_{k+1}\):

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

The quaternion is integrated and normalized as part of the nominal state; the covariance remains in the three-dimensional local attitude chart. propagate_state() performs the physical propagation without mutating the prior estimate.

predicted = propagate_state(
    prior, self.satellite, control, step,
    orbital_state_start, orbital_state_end,
    midpoint_orbital_state=midpoint_orbital_state,
)

3. Linearize the tangent error model and discretize process noise

The local MEKF error evolves as

\[\delta\dot{\mathbf{x}} = \mathbf{F}_k\delta\mathbf{x}+\mathbf{w}_k, \qquad \mathbb{E}[\mathbf{w}_k\mathbf{w}_k^T]=\mathbf{Q}_{c,k}.\]

The process model maps the spacecraft dynamics into the selected tangent chart, including the motion of that chart as the nominal quaternion changes. Van Loan discretization produces

\[\mathbf{\Phi}_k\approx e^{\mathbf{F}_k\Delta t}, \qquad \mathbf{Q}_{d,k}^{\mathrm{VL}}= \int_0^{\Delta t}e^{\mathbf{F}_k\tau} \mathbf{Q}_{c,k}e^{\mathbf{F}_k^T\tau}\,d\tau.\]

Held actuator-command noise contributes the zero-order-hold term

\[\mathbf{Q}_{d,k}^{u}=\mathbf{\Gamma}_k\mathbf{Q}_{u,k}\mathbf{\Gamma}_k^T, \qquad \mathbf{\Gamma}_k=\int_0^{\Delta t}e^{\mathbf{F}_k\tau} \mathbf{B}_k\,d\tau, \qquad \mathbf{Q}_{d,k}^{\mathrm{model}}= \mathbf{Q}_{d,k}^{\mathrm{VL}}+\mathbf{Q}_{d,k}^{u}.\]

A caller-supplied discrete covariance in EstimatorState.int_cov is added to \(\mathbf{Q}_{d,k}^{\mathrm{model}}\) during prediction.

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

4. Predict the tangent covariance

The Covariance operation predicted_linear() applies the linearized covariance prediction directly in \(\mathbb{R}^{6+n_h}\):

\[\mathbf{P}_{k+1}^- = \mathbf{\Phi}_k\mathbf{P}_k^+\mathbf{\Phi}_k^T +\mathbf{Q}_{d,k}^{model}+\mathbf{Q}_{d,k}^{state}.\]

No four-element quaternion covariance is constructed during this step.

predicted_covariance = prior.covariance.predicted_linear(
    transition, process_noise
)
predicted.covariance = predicted_covariance
predicted.process_noise = Covariance(
    process_noise,
    form=prior.process_noise.form,
    coordinates=prior.process_noise.coordinates,
)

5. Select measurements and construct the local innovation

MeasurementStack selects finite, enabled, and scheduled sources, then predicts the active measurements:

\[\hat{\mathbf{z}}_k=h(\hat{\mathbf{x}}_k^-,\mathbf{o}_k).\]

Additive measurements use \(\mathbf{r}=\mathbf{z}-\hat{\mathbf{z}}\). Quaternion measurements use a right relative quaternion and convert it into the selected three-coordinate chart:

\[\delta\mathbf{q}_{meas}= (\hat{\mathbf{q}}^-)^{-1}\otimes\mathbf{q}_{meas}, \qquad \mathbf{r}_q=\phi(\delta\mathbf{q}_{meas})\in\mathbb{R}^3.\]

candidate = stack.active_mask(measurements, enabled=enabled, ...)
predicted_measurements = stack.predict(
    self._state, orbital_state, active_mask=candidate
)
active = stack.active_mask(
    measurements, enabled=candidate,
    predicted=predicted_measurements, ...
)
residual = stack.residual(
    measurements, predicted_measurements, active,
    quaternion_mode=self.measurement_quaternion_mode,
)

6. Build \(\mathbf{H}\) and \(\mathbf{R}\)

The measurement Jacobian is expressed in the same tangent coordinates as the covariance:

\[\mathbf{H}_k= \left.\frac{\partial h}{\partial\mathbf{x}}\right|_{\hat{\mathbf{x}}_k^-}, \qquad \mathbf{R}_k=\mathbb{E}[\mathbf{v}_k\mathbf{v}_k^T].\]

For a quaternion source, the measurement covariance is projected from the four stored coefficients into the same three-coordinate right-error chart. Thus \(\mathbf{H}\) has tangent-state columns and quaternion residuals have three rows.

measurement_jacobian = stack.jacobian(
    self._state, orbital_state, active,
    quaternion_mode=self.measurement_quaternion_mode,
    coordinates="tangent",
)
measurement_noise = stack.covariance(
    self._state, active,
    quaternion_mode=self.measurement_quaternion_mode,
)

7. Compute the innovation covariance, gain, and tangent correction

For \(m\) active residual elements,

\[\mathbf{S}_k=\mathbf{H}_k\mathbf{P}_k^-\mathbf{H}_k^T+\mathbf{R}_k, \qquad \mathbf{K}_k=\mathbf{P}_k^-\mathbf{H}_k^T\mathbf{S}_k^{-1}, \qquad \delta\mathbf{x}_k=\mathbf{K}_k\mathbf{r}_k.\]

The correction has tangent-state dimension \(6+n_h\); its attitude block is a three-vector. The Joseph covariance update is

\[\mathbf{P}_{k,raw}^+= (\mathbf{I}-\mathbf{K}\mathbf{H})\mathbf{P}^- (\mathbf{I}-\mathbf{K}\mathbf{H})^T +\mathbf{K}\mathbf{R}\mathbf{K}^T.\]

gain, joseph_covariance = prior.covariance.updated_linear(
    measurement_jacobian, measurement_noise, joseph=True
)
correction = gain @ residual

8. Apply the multiplicative correction and reset the covariance

The attitude part of \(\delta\mathbf{x}\) is converted into a unit delta quaternion and composed on the right of the nominal quaternion:

\[\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).\]

Since the tangent origin has moved, the covariance is transported using the chart-specific reset Jacobian:

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

plus() and transport_covariance() own the quaternion chart operations, so the MEKF remains valid for either supported three-parameter quaternion_mode.

reset_jacobian = prior.retraction_jacobian(
    correction,
    quaternion_mode=self.correction_mode,
    quaternion_order="right",
)
corrected_covariance = prior.transport_covariance(
    joseph_covariance,
    correction,
    quaternion_mode=self.correction_mode,
    quaternion_order="right",
)
corrected = prior.plus(
    correction,
    quaternion_mode=self.correction_mode,
    quaternion_order="right",
)
corrected.covariance = corrected_covariance

class ADCS.estimators.attitude_estimators.attitude_MEKF.MEKF(satellite, state, *, dt, unmodeled_dynamics_psd=0.0, quaternion_mode='quaternion_vector')

Bases: AttitudeEstimator

Multiplicative EKF with a three-element right attitude error.

Parameters:

• satellite (Any)

• state (EstimatorState)

• dt (float)

• unmodeled_dynamics_psd (Any)

• quaternion_mode (str)