Module: smallBodyNavUKF

Executive Summary

This module provides a navigation solution for a spacecraft about a small body. An unscented Kalman filter estimates relative spacecraft position and velocity with respect to the small body, and the non-Keplerian acceleration term.

This module is only meant to test the possibility of estimating pairs of position and acceleration which may be post processed to estimate the small body gravity field. Therefore, realistic measurement modules do not exist to support this module, and not every source of uncertainty in the problem is an estimated parameter. Future work will build upon this filter.

Message Connection Descriptions

The following table lists all the module input and output messages. The module msg connection is set by the user from python. The msg type contains a link to the message structure definition, while the description provides information on what this message is used for.

Figure 1: smallBodyNavUKF() Module I/O Illustration

Note that this C++ FSW module provides both C- and C++-wrapped output messages. The regular C++ wrapped output messages end with the usual ...OutMsg. The C wrapped output messages have the same payload type, but end with ...OutMsgC.

Module I/O Messages

Msg Variable Name	Msg Type	Description
navTransInMsg	NavTransMsgPayload	Translational nav input message
asteroidEphemerisInMsg	EphemerisMsgPayload	Small body ephemeris input message
smallBodyNavUKFOutMsg	SmallBodyNavUKFMsgPayload	Small body nav output msg - states and covariances
smallBodyNavUKFOutMsgC	SmallBodyNavUKFMsgPayload	C-wrapped small body nav output msg - states and covariances

Detailed Module Description

General Function

The smallBodyNavUKF() module provides a complete translational state estimate for a spacecraft in proximity of a small body. The relative spacecraft position and velocity, and the non-Keplerian acceleration (e.g. inhomogeneous gravity field) are estimated by the filter. The filter assumes full observability of the relative position and that the small body state is perfectly known. Future work may consider augmenting the estimates and using more realistic measurements. The full state vector may be found below:

(1)\[\begin{split}\mathbf{x} = \begin{bmatrix} {}^A\mathbf{r}_{S/A} \\ {}^A\mathbf{v}_{S/A} \\ {}^A\mathbf{a} \\ \end{bmatrix}\end{split}\]

The associated frame definitions may be found in the following table.

Frame Definitions

Frame Description	Frame Definition
Small Body Fixed Frame	\(A: \{\hat{\mathbf{a}}_1, \hat{\mathbf{a}}_2, \hat{\mathbf{a}}_3\}\)
J2000 Inertial Frame	\(N: \{\hat{\mathbf{n}}_1, \hat{\mathbf{n}}_2, \hat{\mathbf{n}}_3\}\)

Initialization

The module start by initializing the weights for the unscented transform

(2)\[w^{[0]}_{m}=\kappa / (\kappa + N),\>\>\>\>\>\> w^{[0]}_{c}=w^{[0]}_{m}+1-\alpha^2+\beta,\>\>\>\>\>\> w^{[i]}_{m}=w^{[i]}_{c}=1/(2N+\kappa) \>\> i\neq 0\]

Algorithm

This module employs an unscented Kalman filter (UKF) Wan and Van Der Merwe to estimate the relevant states. The UKF relies on the unscented transform (UT) to compute the non-linear transformation of a Gaussian distribution. Let consider a random variable \(\mathbf{x}\) of dimension \(N\) modelled as a Gaussian distribution with mean \(\hat{\mathbf{x}}\) and covariance \(P\). The UT computes numerically the resulting mean and covariance of \(\mathbf{y}=\mathbf{f}(\mathbf{x})\) by creating \(2N+1\) samples named sigma points as

(3)\[\pmb{\chi}^{[i]} = \hat{\mathbf{x}} \pm \left(\sqrt{(N+\kappa) P}\right)_{|i|},\>\> i = -N...N\]

where \(|i|\) denotes the columns of the matrix. Then, transform each sigma point as \(\pmb{\xi}^{[i]}=\mathbf{f}(\pmb{\chi}^{[i]})\). Finally, compute the mean and covariance of \(\mathbf{y}=\mathbf{f}(\mathbf{x})\) as

(4)\[\hat{\mathbf{y}} = \sum^{N}_{i=-N}w^{[i]}_{m}\pmb{\xi}^{[i]}\]

(5)\[R = \sum^{N}_{i=-N}w^{[i]}_{c}(\pmb{\xi}^{[i]} - \hat{\mathbf{y}})(\pmb{\xi}^{[i]} - \hat{\mathbf{y}})^T\]

In the small body scenario under consideration, there are two transformation functions. The process propagation, assumed as simple forward Euler integration, as

(6)\[\begin{split}\mathbf{x}_{k+1}=\Delta t \begin{bmatrix} 0_{3\times3} & I & 0_{3\times3}\\ -{}^A\Omega^2_{A/N} & -2{}^A\Omega_{A/N} & I\\ 0_{3\times3} & 0_{3\times3} & 0_{3\times3}\\ \end{bmatrix}\mathbf{x}_{k}+ \Delta t \begin{bmatrix} 0_{3\times1}\\ -\mu \mathbf{r}_k / ||\mathbf{r}_k||^3 \\ 0_{3\times1}\\ \end{bmatrix}\end{split}\]

Note that \({}^A\Omega_{A/N}\) is the cross-product matrix associated to the small body rotation rate. The state to measurements transformation is

(7)\[\begin{split}\mathbf{y}_{k+1}=\begin{bmatrix} I & 0_{3\times3} & 0_{3\times3}\\ \end{bmatrix}\mathbf{x}_{k+1}\end{split}\]

Under the previous considerations, the UKF estimation is as follows:

1) Compute the a-priori state estimation \(\hat{\mathbf{x}}^{-}_{k+1}\) and \(P^{-}_{k+1}\) through the UT to the propagation function. Add the process noise uncertainty \(P_{proc}\)

(8)\[P^{-}_{k+1}\leftarrow P^{-}_{k+1} + P_{proc}\]

2) Compute the a-priori measurements \(\hat{\mathbf{y}}^{-}_{k+1}\) and \(R^{-}_{k+1}\) through the UT to the state to measurements transformation function. Add the measurements uncertainty :math`R_{meas}`

(9)\[R^{-}_{k+1}\leftarrow R^{-}_{k+1} + R_{meas}\]

3. Compute the cross-correlation matrix between state and measurements and the Kalman gain

(10)\[H = \sum^{N}_{i=-N}w^{[i]}_{c}(\pmb{\chi}^{[i]} - \hat{\mathbf{x}}^{-}_{k+1})(\pmb{\xi}^{[i]} - \hat{\mathbf{y}}^{-}_{k+1})^T\]

(11)\[K = H (R^{-}_{k+1})^{-1}\]

4. Update the state estimation with the incoming measurement

(12)\[\mathbf{x}_{k+1} = \mathbf{x}^{-}_{k+1} + K(\mathbf{y}_{k+1} - \hat{\mathbf{y}}^{-}_{k+1})\]

(13)\[P_{k+1} = P^{-}_{k+1} - KR^{-}_{k+1}K^T\]

These steps are based on Wan and Van Der Merwe (see algorithm 3.1). The weights selection can be consulted there but it is the one described in the initialization step. The filter hyper-parameters are \(\{\alpha, \beta, \kappa\}\). Note that \(\kappa\) is equivalent to \(\lambda\) in the original publication.

Module Assumptions and Limitations

The module assumptions and limitations are listed below:

• The reference frame within the filter is rigidly attached to the small body

• The filter assumes the small body mass and rotational state are perfectly known

• The small body has a uniform rotation around its principal inertia axis

• Currently, the prediction and update rates occur at the same frequency

• The position measurement are produced by simpleNavMeas, thus being referred to the J2000 inertial frame. The position is internally translated to the asteroid fixed frame coordinates.

User Guide

The user then must set the following module variables:

• mu_ast, the gravitational constant of the small body in \(\text{m}^3/\text{s}^2\)

• P_proc, the process noise covariances

• R_meas, the measurement noise covariance

• x_hat_k to initialize the state estimation

• P_k to initialize the state estimation covariance

The user could opt to set the following module variables (initialized by default):

• alpha, filter hyper-parameter (2 by default)

• beta, filter hyper-parameter (0 by default)

• kappa, filter hyper-parameter (\(10^{-3}\) by default)

The user must connect to each input message described in Table 1.

---

class SmallBodyNavUKF : public SysModel

#include <smallBodyNavUKF.h>

This module estimates relative spacecraft position, velocity with respect to the body, and the non-Keplerian acceleration perturbing the spacecraft motion, using an unscented Kalman filter (UKF)

Public Functions

SmallBodyNavUKF()

This is the constructor for the module class. It sets default variable values and initializes the various parts of the model

~SmallBodyNavUKF()

Module Destructor

void SelfInit()

Self initialization for C-wrapped messages.

Initialize C-wrapped output messages

void Reset(uint64_t CurrentSimNanos)

Resets module.

This method is used to reset the module, check that required input messages are connect and compute weigths.

Returns:

void

void UpdateState(uint64_t CurrentSimNanos)

Updates state.

This is the main method that gets called every time the module is updated.

Returns:

void

Public Members

ReadFunctor<NavTransMsgPayload> navTransInMsg

Translational nav input message.

ReadFunctor<EphemerisMsgPayload> asteroidEphemerisInMsg

Small body ephemeris input message.

Message<SmallBodyNavUKFMsgPayload> smallBodyNavUKFOutMsg

Small body nav UKF output msg - states and covariances.

SmallBodyNavUKFMsg_C smallBodyNavUKFOutMsgC = {}

C-wrapped Small body nav UKF output msg - states and covariances.

BSKLogger bskLogger

&#8212; BSK Logging

double mu_ast

Gravitational constant of the small body.

Eigen::MatrixXd P_proc

Process noise covariance.

Eigen::MatrixXd R_meas

Measurement noise covariance.

Eigen::VectorXd x_hat_k

Current state estimate.

Eigen::MatrixXd P_k

Current state estimation covariance.

double alpha

UKF hyper-parameter.

double beta

UKF hyper-parameter.

double kappa

UKF hyper-parameter.

Eigen::Matrix3d dcm_AN

Small body dcm.

Eigen::Vector3d omega_AN_A

Small body angular velocity.

Private Functions

void readMessages()

Reads input messages.

This method is used to read the input messages.

Returns:

void

void writeMessages(uint64_t CurrentSimNanos)

Writes output messages.

This method writes the output messages

Returns:

void

void processUT(uint64_t CurrentSimNanos)

Process unscented transform.

This method does the UT to the initial distribution to compute the a-priori state

Parameters:

CurrentSimNanos –

Returns:

void

void measurementUT()

Measurements unscented transform.

This method does the UT to the a-priori state to compute the a-priori measurements

Returns:

void

void kalmanUpdate()

Computes the state and covariance update.

This method collects the measurements and updates the estimation

Returns:

void

Private Members

NavTransMsgPayload navTransInMsgBuffer

Message buffer for input translational nav message.

EphemerisMsgPayload asteroidEphemerisInMsgBuffer

Message buffer for asteroid ephemeris.

uint64_t prevTime

Previous time, ns.

uint64_t numStates

Number of states.

uint64_t numMeas

Number of measurements.

uint64_t numSigmas

Number of sigma points.

Eigen::VectorXd x_hat_k1_

Apriori state estimate for time k+1.

Eigen::VectorXd x_hat_k1

Update state estimate for time k+1.

Eigen::VectorXd wm_sigma

Mean sigma weights for UT.

Eigen::VectorXd wc_sigma

Covariance sigma weights for UT.

Eigen::VectorXd y_hat_k1_

Apriori measurement estimate for time k+1.

Eigen::MatrixXd P_k1_

Apriori state covariance estimate for time k+1.

Eigen::MatrixXd P_k1

Update state covariance estimate for time k+1.

Eigen::MatrixXd X_sigma_k1_

Apriori state sigma points for time k+1.

Eigen::MatrixXd R_k1_

Apriori measurements covariance estimate for time k+1.

Eigen::MatrixXd Y_sigma_k1_

Apriori measurements sigma points for time k+1.

Eigen::MatrixXd H

State-measurements cross-correlation matrix.

Eigen::MatrixXd K

Kalman gain matrix.