Module: 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, attitude and attitude rate of the asteroid’s body frame with respect to the inertial frame, and the attitude and attitude rate of the spacecraft with respect to the inertial frame.

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

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
rwInMsgs	RWConfigLogMsgPayload	Vector of reaction wheel input messages
thrusterInMsgs	THROutputMsgPayload	Vector of thruster input messages
navTransOutMsg	NavTransMsgPayload	Translational nav output message
navTransOutMsgC	NavTransMsgPayload	C-wrapped translational nav output message
navAttOutMsg	NavAttMsgPayload	Attitude nav output message
navAttOutMsgC	NavAttMsgPayload	C-wrapped attitude 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 complete state estimate for a spacecraft in proximity of a small body. The relative spacecraft position and velocity, spacecraft attitude and rate, 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 Module: simpleNav and Module: 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\\ \mathbf{x}_5\\ \mathbf{x}_6 \end{bmatrix}= \begin{bmatrix} {}^O\mathbf{r}_{S/O} \\ {}^O\dot{\mathbf{r}}_{S/O} \\ \boldsymbol{\sigma}_{AN} \\ {}^A\boldsymbol{\omega}_{AN} \\ \boldsymbol{\sigma}_{BN} \\ {}^B\boldsymbol{\omega}_{BN} \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 state estimate \(\hat{\mathbf{x}}_{k+1}\) and estimation error covariance \(P_{k+1}\) are then computed by propagating the equations below:

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

(5)\[\dot{P}_k = A_kP_k + P_kA_k^T + L_kQ_kL_k^T\]

The measurements are read into the module and the state and covariance are updated as follows:

(6)\[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}\]

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

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

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

(10)\[\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}\]

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

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

(13)\[\dot{\mathbf{x}}_5 = \dot{\boldsymbol{\sigma}}_{B/N} = \dfrac{1}{4} \Bigr [ \Bigr ( 1-||\mathbf{x}_5||^2 \Bigr ) [I_{3 \times 3}] + 2[\tilde{\mathbf{x}}_5] + 2\mathbf{x}_5\mathbf{x}_5^T \Bigr]\mathbf{x}_6\]

(14)\[\dot{\mathbf{x}}_6 = {}^B\dot{\boldsymbol{\omega}}_{B/N} = -[I_T]^{-1} \Bigr([\tilde{\mathbf{x}}_6][I_T]\mathbf{x}_6 + [I_W]{}^B\dot{\boldsymbol{\Omega}} + [\tilde{\mathbf{x}}_6][I_W]{}^B\boldsymbol{\Omega} -\sum_j^J{}^B\mathbf{L}_{T_j}\Bigr )\]

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 reaction wheels’ spin axes are aligned with the body-frame axes of the spacecraft

• Only three reaction wheels are used for attitude control

• The reaction wheels must be added in the order of the body-frame axes, i.e. 1-2-3

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

• 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

• IHubPntC_B, the inertia of the spacecraft

• IWheelPntC_B, the inertia of the reaction wheels

• 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. While the rwInMsgs and thrusterInMsgs are optional, BSK will throw a warning if they are not connected.

---

class SmallBodyNavEKF : public SysModel

#include <smallBodyNavEKF.h>

This module estimates relative spacecraft position and velocity with respect to the body, attitude and attitude rate of the body wrt. the inertial frame, and the attitude and attitude rate of the spacecraft with respect to 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

~SmallBodyNavEKF()

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

void addRWToFilter(Message<RWConfigLogMsgPayload> *tmpRWMsg)

Adds rw message.

This method is used to add a rw 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.

std::vector<ReadFunctor<RWConfigLogMsgPayload>> rwInMsgs

Reaction wheel speed and torque input messages.

std::vector<ReadFunctor<THROutputMsgPayload>> thrusterInMsgs

thruster input msg vector

Message<NavTransMsgPayload> navTransOutMsg

Translational nav output message.

Message<NavAttMsgPayload> navAttOutMsg

Attitude nav output message.

Message<SmallBodyNavMsgPayload> smallBodyNavOutMsg

Small body nav output msg - states and covariances.

Message<EphemerisMsgPayload> asteroidEphemerisOutMsg

Small body ephemeris output message.

NavTransMsg_C navTransOutMsgC = {}

C-wrapped Translational nav output message.

NavAttMsg_C navAttOutMsgC = {}

C-wrapped Attitude nav output message.

SmallBodyNavMsg_C smallBodyNavOutMsgC = {}

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

EphemerisMsg_C asteroidEphemerisOutMsgC = {}

C-wrapped 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.

Eigen::Matrix3d IHubPntC_B

sc inertia

Eigen::Matrix3d IWheelPntC_B

wheel inertia

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.

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 predict(uint64_t CurrentSimNanos)

Prediction step of Kalman filter.

This method performs the KF prediction step

Parameters

CurrentSimNanos –

Returns

void

void aprioriState(uint64_t CurrentSimNanos)

Computes the apriori state.

This method computes the apriori state estimate using euler integration

Parameters

CurrentSimNanos –

Returns

void

void aprioriCovar(uint64_t CurrentSimNanos)

Computes the apriori covariance.

This method compute the apriori estimation error covariance through euler integration

Parameters

CurrentSimNanos –

Returns

void

void checkMRPSwitching()

Checks the MRPs for switching.

This method checks the propagated MRP states to see if they exceed a norm of 1. If they do, the appropriate states are transferred to the shadow set and the covariance is updated.

Returns

void

void computeDynamicsMatrix()

Computes the new dynamics matrix, A_k.

This method computes the state dynamics matrix, A, for the next iteration

Returns

void

void measurementUpdate()

Computes the measurement update for the EKF.

This method performs the KF measurement update step

Returns

void

Private Members

NavTransMsgPayload navTransInMsgBuffer

Message buffer for input translational nav message.

NavAttMsgPayload navAttInMsgBuffer

Message buffer for input attitude nav message.

EphemerisMsgPayload asteroidEphemerisInMsgBuffer

Message buffer for asteroid ephemeris.

EphemerisMsgPayload sunEphemerisInMsgBuffer

Message buffer for sun ephemeris.

std::vector<RWConfigLogMsgPayload> rwConfigLogInMsgBuffer

Buffer for rw speed messages.

std::vector<THROutputMsgPayload> thrusterInMsgBuffer

Buffer for thruster force and torques.

uint64_t prevTime

Previous time, ns.

uint64_t numStates

Number of states.

Eigen::Vector3d thrust_B

Thrust expressed in body-frame components.

Eigen::Vector3d torque_B

Torque expressed in body-frame components.

Eigen::VectorXd x_hat_dot_k

Rate of change of state estimate.

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::MatrixXd P_dot_k

Rate of change of estimation error covariance.

Eigen::MatrixXd P_k1_

Apriori estimation error covariance.

Eigen::MatrixXd P_k1

Updated estimation error covariance.

Eigen::MatrixXd A_k

State dynamics matrix.

Eigen::MatrixXd L

Eigen::MatrixXd M

Eigen::MatrixXd H_k1

Jacobian of measurement model.

Eigen::MatrixXd I_full

numStates x numStates identity matrix

double mu_sun

Gravitational parameter of the sun.

Eigen::Matrix3d o_hat_3_tilde

Tilde matrix of the third asteroid orbit frame base vector.

Eigen::Vector3d o_hat_1

First asteroid orbit frame base vector.

Eigen::MatrixXd I

3 x 3 identity matrix

classicElements oe_ast

Orbital elements of the asteroid.

double F_dot

Time rate of change of true anomaly.

double F_ddot

Second time derivative of true anomaly.

Eigen::Matrix3d dcm_ON

DCM from the inertial frame to the small-body’s hill frame.

Eigen::Vector3d r_SO_O

Vector from the small body’s origin to the inertial frame origin in small-body hill frame components.

Eigen::Vector3d Omega_B

Speed of the reaction wheels in the spacecraft body frame.

Eigen::Vector3d Omega_dot_B

Accleration of the reaction wheels in the spacecraft body frame.