State estimation modules maintain probabilistic estimates of quantities that cannot be measured directly. This page covers thrust center-of-mass estimation and small-body proximity navigation filters. For attitude-specific filters (sunline EKF, inertial UKF, CSS WLS), see Attitude determination. For optical navigation filters, see Optical navigation.
Thrust CM estimation (thrustCMEstimation)
Estimates the location of the spacecraft’s center of mass (CM) by processing the integral feedback torque produced by mrpFeedback during steady-state attitude hold. A gimbaled thruster is periodically articulated to generate linearly independent torque measurements, and a sequential weighted least-squares algorithm fuses them to converge on the CM location.
Measurements are only accepted when the attitude and rate errors fall below a convergence threshold ε:
ε = sqrt(|σ_B/R|² + |ω_B/R|²)
Sequential least-squares update
At each accepted measurement step:
K_n = P_n C_nᵀ (C_n P_n C_nᵀ + R)⁻¹
x_{n+1} = x_n + K_n (y_n - C_n x_n)
P_{n+1} = (I - K_n C_n) P_n
x_n = r_C/B is the CM location estimate, C_n = [t̃] is the skew-symmetric matrix of the thrust vector, and y_n = Z + [t̃] r_T/B with Z the negative integral feedback torque.
Messages
| Variable | Type | Direction | Description |
|---|
thrusterConfigBInMsg | THRConfigMsgPayload | Input | Thruster geometry and thrust level |
intFeedbackTorqueInMsg | CmdTorqueBodyMsgPayload | Input | Integral feedback torque from mrpFeedback |
attGuidInMsg | AttGuidMsgPayload | Input | Attitude and rate errors (for convergence check) |
vehConfigInMsg | VehicleConfigMsgPayload | Input | (optional) True CM for verification |
vehConfigOutMsg | VehicleConfigMsgPayload | Output | Updated vehicle configuration with estimated CM |
cmEstDataOutMsg | CMEstDataMsgPayload | Output | Estimated state, covariance, and residuals |
Parameters
| Parameter | Default | Description |
|---|
attitudeTol | 0 | Convergence threshold ε below which a measurement is accepted |
r_CB_B | [0, 0, 0] | Initial guess for CM location in body frame [m] |
P0 | [0, 0, 0] | Diagonal elements of the initial state covariance |
R0 | [0, 0, 0] | Diagonal elements of the measurement noise covariance |
Configuration example
from Basilisk.fswAlgorithms import thrustCMEstimation
cmEstimator = thrustCMEstimation.ThrustCMEstimation()
cmEstimator.ModelTag = "cmEstimator"
cmEstimator.attitudeTol = 1e-4 # convergence threshold
cmEstimator.r_CB_B = [0.01, -0.025, 0.04] # [m] initial CM guess
cmEstimator.P0 = [0.0025, 0.0025, 0.0025] # initial covariance
cmEstimator.R0 = [1e-9, 1e-9, 1e-9] # measurement noise
cmEstimator.thrusterConfigBInMsg.subscribeTo(thrConfigMsg)
cmEstimator.intFeedbackTorqueInMsg.subscribeTo(
mrpFeedbackWrap.intFeedbackTorqueOutMsg)
cmEstimator.attGuidInMsg.subscribeTo(attGuidMsg)
scSim.AddModelToTask(taskName, cmEstimator)
The thruster must be articulated in at least two linearly independent directions before the CM estimate converges. A single static thrust measurement cannot resolve all three components of the CM location.
Pair thrustCMEstimation with thrusterPlatformReference to continuously articulate the gimbal in a way that dumps RW momentum while generating measurement diversity for the CM estimator. See the scenarioSepMomentumManagement example for an end-to-end setup.
Small-body navigation EKF (smallBodyNavEKF)
A hybrid Extended Kalman Filter for spacecraft proximity operations around a small body. The filter jointly estimates:
- Spacecraft position and velocity relative to the small body, expressed in the small body’s Hill frame
- Small body attitude (MRPs) and spin rate relative to the inertial frame
This state vector is 12-dimensional and is intended to support POMDP planners for autonomous proximity operations.
State vector
X = [ r_S/O (3) — relative position in small-body Hill frame
ṙ_S/O (3) — relative velocity
σ_A/N (3) — small-body attitude (MRP)
ω_A/N (3) — small-body spin rate ]
Dynamics
The filter propagates relative position and velocity under two-body gravity, third-body solar gravity perturbation, solar radiation pressure (SRP), thruster forces, and reaction-wheel torques. The small body’s spin rate is modeled as constant. MRP switching is enforced to avoid the MRP singularity.
Messages
| Variable | Type | Direction | Description |
|---|
navTransInMsg | NavTransMsgPayload | Input | Spacecraft translational state |
navAttInMsg | NavAttMsgPayload | Input | Spacecraft attitude |
asteroidEphemerisInMsg | EphemerisMsgPayload | Input | Small body position and velocity |
sunEphemerisInMsg | EphemerisMsgPayload | Input | Sun position for SRP model |
thrusterInMsgs | THROutputMsgPayload | Input | Thruster output messages (vector) |
cmdForceBodyInMsg | cmdForceBodyMsgPayload | Input | Commanded body force |
navTransOutMsg | NavTransMsgPayload | Output | Estimated translational state |
smallBodyNavOutMsg | SmallBodyNavMsgPayload | Output | Full state estimate and covariance |
asteroidEphemerisOutMsg | EphemerisMsgPayload | Output | Estimated small body ephemeris |
...OutMsgC) are also available for use with C-based downstream modules.
Configuration example
from Basilisk.fswAlgorithms import smallBodyNavEKF
import numpy as np
nav = smallBodyNavEKF.SmallBodyNavEKF()
nav.ModelTag = "smallBodyNavEKF"
nav.A_sc = 1.0 # [m^2] spacecraft cross-sectional area
nav.M_sc = 400.0 # [kg] spacecraft mass
nav.mu_ast = 4.0 # [m^3/s^2] asteroid gravitational parameter
# 12x12 process noise covariance (position, velocity, MRP, omega)
nav.Q = np.diag([1e-6]*3 + [1e-9]*3 + [1e-8]*3 + [1e-10]*3).tolist()
# 12x12 measurement noise covariance
nav.R = np.diag([10.0]*3 + [0.1]*3 + [1e-4]*3 + [1e-6]*3).tolist()
# initial state estimate
nav.x_hat_k = [1000.0, 0.0, 0.0, # [m] relative position
0.0, 0.5, 0.0, # [m/s] relative velocity
0.0, 0.0, 0.0, # [-] asteroid MRP
0.0, 0.0, 0.001] # [rad/s] asteroid spin rate
nav.navTransInMsg.subscribeTo(simpleNavMsg)
nav.navAttInMsg.subscribeTo(navAttMsg)
nav.asteroidEphemerisInMsg.subscribeTo(asteroidEphemMsg)
nav.sunEphemerisInMsg.subscribeTo(sunEphemMsg)
scSim.AddModelToTask(taskName, nav)
Small-body navigation UKF (smallBodyNavUKF)
An Unscented Kalman Filter variant of the small-body navigation filter. It uses the same 12-element state vector and message interfaces as smallBodyNavEKF but applies the UKF sigma-point propagation for potentially better accuracy in highly nonlinear dynamics regimes near irregular small bodies.
Both smallBodyNavEKF and smallBodyNavUKF use simplified “measurements” written by simpleNav and planetNav rather than real sensor models. They are designed to provide a representative navigation solution for POMDP planners during autonomous proximity operations development.