smallBodyNavEKF#

Executive Summary#

This module provides a navigation solution for a spacecraft about a small body. A hybrid extended Kalman filter estimates relative spacecraft position and velocity with respect to the small body and the attitude and attitude rate of the asteroid’s body frame with respect to the inertial frame.

This module is only meant to provide a somewhat representative autonomous small body proximity operations navigation solution for POMDP solvers. 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.

../../../../../_images/moduleIOSmallBodyNavigation.svg — Figure 1: `smallBodyNavEKF()` 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
navAttInMsg	NavAttMsgPayload	Attitude nav input message
asteroidEphemerisInMsg	EphemerisMsgPayload	Small body ephemeris input message
sunEphemerisInMsg	EphemerisMsgPayload	Sun ephemeris input message
thrusterInMsgs	THROutputMsgPayload	Vector of thruster input messages
cmdForceBodyInMsg	cmdForceBodyMsgPayload	Commanded force input
navTransOutMsg	NavTransMsgPayload	Translational nav output message
navTransOutMsgC	NavTransMsgPayload	C-wrapped translational nav output message
smallBodyNavOutMsg	SmallBodyNavMsgPayload	Small body nav output msg - states and covariances
smallBodyNavOutMsgC	SmallBodyNavMsgPayload	C-wrapped small body nav output msg - states and covariances
asteroidEphemerisOutMsg	EphemerisMsgPayload	Small body ephemeris output message
asteroidEphemerisOutMsgC	EphemerisMsgPayload	C-wrapped small body ephemeris output message

Detailed Module Description#

General Function#

The smallBodyNavEKF() module provides a state estimate for a spacecraft in proximity of a small body. The relative spacecraft position and velocity and small body attitude and rate are estimated by the filter. The filter assumes full observability of each state. The “measurements” are typically messages written out by simpleNav and planetNav modules. However, future developers can implement measurement models that adhere to the required I/O format. The full state vector may be found below:

(1)#\[\begin{split}\mathbf{X} = \begin{bmatrix} \mathbf{x}_1\\ \mathbf{x}_2\\ \mathbf{x}_3\\ \mathbf{x}_4 \end{bmatrix}= \begin{bmatrix} {}^O\mathbf{r}_{S/O} \\ {}^O\dot{\mathbf{r}}_{S/O} \\ \boldsymbol{\sigma}_{AN} \\ {}^A\boldsymbol{\omega}_{AN} \end{bmatrix}\end{split}\]

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

Frame Definitions#
Frame Description	Frame Definition
Small Body Hill Frame	\(O: \{\hat{\mathbf{o}}_1, \hat{\mathbf{o}}_2, \hat{\mathbf{o}}_3\}\)
Small Body Body Frame	\(A: \{\hat{\mathbf{a}}_1, \hat{\mathbf{a}}_2, \hat{\mathbf{a}}_3\}\)
Spacecraft Body Frame	\(B: \{\hat{\mathbf{b}}_1, \hat{\mathbf{b}}_2, \hat{\mathbf{b}}_3\}\)
J2000 Inertial Frame	\(N: \{\hat{\mathbf{n}}_1, \hat{\mathbf{n}}_2, \hat{\mathbf{n}}_3\}\)

Initialization#

Algorithm#

This module employs a hybrid extended Kalman filter (EKF) to estimate the relevant states. First, \(\hat{\mathbf{x}}_0\) and \(P_0\) are initialized by the user. The dynamics matrix \(A_0\) is initialized to identity by the module.

(2)#\[\hat{\mathbf{x}}_k = \hat{\mathbf{x}}_0\]

(3)#\[P_k = P_0\]

The apriori state estimate \(\hat{\mathbf{x}}_{k+1}^-\) and STM \(\Phi(k+1,k)\) are propagated using the equations below:

(4)#\[\dot{\hat{\mathbf{x}}}_{k} = f(\hat{\mathbf{x}}_k, \mathbf{u}_k, w_k, t_k)\]

(5)#\[\dot{\Phi}(k+1,k) = A_{k+1}\Phi(k+1,k)\]

The apriori estimation error covariance \(P_{k+1}^-\) is then computed by propagating the equations below:

(6)#\[P_{k+1}^- = \Phi(k+1, k) P_k^+ \Phi(k+1, k)^T + L_kQ_kL_k^T\]

The measurements are read into the module and the state and covariance are updated using the equation below. If no new measurements are present, the filter skips this step and writes out the apriori state estimate and covariance.

(7)#\[K_{k+1} = P_{k+1}^-H_{k+1}^T(H_{k+1}P_{k+1}^-H_{k+1}^T + M_{k+1}R_{k+1}M_{k+1}^T)^{-1}\]

(8)#\[\hat{\mathbf{x}}_{k+1}^+ = \hat{\mathbf{x}}_{k+1}^- + K_{k+1}[y_{k+1} - h(\hat{\mathbf{x}}_{k+1}^-, v_{k+1}, t_{k+1})]\]

(9)#\[P_{k+1}^+ = (I-K_{k+1}H_{k+1})P_{k+1}^-(I-K_{k+1}H_{k+1})^T+K_{k+1}M_{k+1}R_{k+1}M_{k+1}^TK_{k+1}^T\]

The dynamics for each element of \(f(\hat{\mathbf{x}}_k, \mathbf{u}_k, w_k, t_k)\) may be found below. The relative position and velocity dynamics are described in detail by Takahashi and Scheeres. The equations for attitude dynamics are described in detail in Chapters 3 and 4 of Analytical Mechanics of Space Systems. We assume that the small body rotates at a constant rate.

(10)#\[\dot{\mathbf{x}}_1 = ^O\dot{\mathbf{r}}_{S/O} = \mathbf{x}_2\]

(11)#\[\begin{split}\begin{split} \dot{\mathbf{x}}_2 = ^O\ddot{\mathbf{r}}_{S/O} = -\ddot{F}[\tilde{\hat{\mathbf{o}}}_3]\mathbf{x}_1 - 2\dot{F}[\tilde{\hat{\mathbf{o}}}_3]\mathbf{x}_2 - \dot{F}^2[\tilde{\hat{\mathbf{o}}}_3][\tilde{\hat{\mathbf{o}}}_3]\mathbf{x}_1- \dfrac{\mu_a \mathbf{x}_1}{||\mathbf{x}_1||^3} + \dfrac{\mu_s(3{}^O\hat{\mathbf{d}}{}^O\hat{\mathbf{d}}^T-[I_{3 \times 3}])\mathbf{x}_1}{d^3} \\ + C_{SRP}\dfrac{P_0(1+\rho)A_{sc}}{M_{sc}}\dfrac{(1\text{AU})^2}{d^2}\hat{\mathbf{o}}_1 + \sum_i^I\dfrac{{}^O\mathbf{F}_i}{M_{sc}} + \sum_j^J\dfrac{{}^O\mathbf{F}_j}{M_{sc}} \end{split}\end{split}\]

(12)#\[\dot{\mathbf{x}}_3 = \dot{\boldsymbol{\sigma}}_{A/N} = \dfrac{1}{4} \Bigr [ \Bigr ( 1-||\mathbf{x}_3||^2 \Bigr ) [I_{3 \times 3}] + 2[\tilde{\mathbf{x}}_3] + 2\mathbf{x}_3\mathbf{x}_3^T \Bigr]\mathbf{x}_4\]

(13)#\[\dot{\mathbf{x}}_4 = {}^A\dot{\boldsymbol{\omega}}_{A/N} = \mathbf{0}\]

Note that the MRP switching is checked following the procedure outlined in Karlgaard.

The derivation of the state dynamics matrix \(A\) is not shown here for brevity.

Module Assumptions and Limitations#

The module assumptions and limitations are listed below:

The spacecraft’s attitude and rate are perfectly known

The small body’s position and velocity in the inertial frame are perfectly known

Please refer to the cited works for specific assumptions about the filter dynamics

The matrix \(H_{k+1}\) is identity

User Guide#

The user then must set the following module variables:

A_sc, the area of the spacecraft in \(\text{m}^2\)
M_sc, the mass of the spacecraft in kg
mu_ast, the gravitational constant of the small body in \(\text{m}^3/\text{s}^2\)
Q, the process noise covariances
R, the measurement noise covariance
x_hat_k to initialize \(x_0\)
P_k to initialize \(P_0\)

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

Class SmallBodyNavEKF#

class SmallBodyNavEKF : public SysModel#

This module estimates relative spacecraft position and velocity with respect to the body and attitude and attitude rate of the body wrt. the inertial frame.

Public Functions

SmallBodyNavEKF()#: This is the constructor for the module class. It sets default variable values and initializes the various parts of the model

void reset(uint64_t currentSimNanos)#

Resets module.

This method is used to reset the module and checks that required input messages are connect.

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

void addThrusterToFilter(Message<THROutputMsgPayload> *tmpThrusterMsg)#

Adds thruster message.

This method is used to add a thruster to the filter.

Returns:: void

Public Members

ReadFunctor<NavTransMsgPayload> navTransInMsg#: Translational nav input message.

ReadFunctor<NavAttMsgPayload> navAttInMsg#: Attitude nav input message.

ReadFunctor<EphemerisMsgPayload> asteroidEphemerisInMsg#: Small body ephemeris input message.

ReadFunctor<EphemerisMsgPayload> sunEphemerisInMsg#: Sun ephemeris input message.

ReadFunctor<CmdForceBodyMsgPayload> cmdForceBodyInMsg#: Command force body in message.

std::vector<ReadFunctor<THROutputMsgPayload>> thrusterInMsgs#: thruster input msg vector

Message<NavTransMsgPayload> navTransOutMsg#: Translational nav output message.

Message<SmallBodyNavMsgPayload> smallBodyNavOutMsg#: Small body nav output msg - states and covariances.

Message<EphemerisMsgPayload> asteroidEphemerisOutMsg#: Small body ephemeris output message.

BSKLogger bskLogger#: — BSK Logging

double C_SRP#: SRP scaling coefficient.

double P_0#: SRP at 1 AU.

double rho#: Surface reflectivity.

double A_sc#: Surface area of the spacecraft.

double M_sc#: Mass of the spacecraft.

double mu_ast#: Gravitational constant of the asteroid.

Eigen::MatrixXd Q#: Process Noise.

Eigen::MatrixXd R#: Measurement Noise.

Eigen::VectorXd x_hat_k#: Current state estimate.

Eigen::MatrixXd P_k#: Current estimation error covariance.