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 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.
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
.
Msg Variable Name |
Msg Type |
Description |
---|---|---|
navTransInMsg |
Translational nav input message |
|
navAttInMsg |
Attitude nav input message |
|
asteroidEphemerisInMsg |
Small body ephemeris input message |
|
sunEphemerisInMsg |
Sun ephemeris input message |
|
thrusterInMsgs |
Vector of thruster input messages |
|
cmdForceBodyInMsg |
Commanded force input |
|
navTransOutMsg |
Translational nav output message |
|
navTransOutMsgC |
C-wrapped translational nav output message |
|
smallBodyNavOutMsg |
Small body nav output msg - states and covariances |
|
smallBodyNavOutMsgC |
C-wrapped small body nav output msg - states and covariances |
|
asteroidEphemerisOutMsg |
Small body ephemeris output message |
|
asteroidEphemerisOutMsgC |
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 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:
The associated frame definitions may be found in the following table.
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.
The apriori state estimate \(\hat{\mathbf{x}}_{k+1}^-\) and STM \(\Phi(k+1,k)\) are propagated using the equations below:
The apriori estimation error covariance \(P_{k+1}^-\) is then computed by propagating the equations below:
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.
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.
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 kgmu_ast
, the gravitational constant of the small body in \(\text{m}^3/\text{s}^2\)Q
, the process noise covariancesR
, the measurement noise covariancex_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 : public SysModel¶
- #include <smallBodyNavEKF.h>
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
-
~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
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.
-
NavTransMsg_C navTransOutMsgC = {}¶
C-wrapped Translational 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.
-
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(uint64_t CurrentSimNanos)¶
Reads input messages.
This method is used to read the input messages.
- Parameters
CurrentSimNanos –
- 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 RK4 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(Eigen::VectorXd x_hat)¶
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
-
void computeEquationsOfMotion(Eigen::VectorXd x_hat, Eigen::MatrixXd Phi)¶
Computes the EOMs of the state and state transition matrix.
This method calculates the EOMs of the state vector and state transition matrix
- Parameters
x_hat –
Phi –
- 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<THROutputMsgPayload> thrusterInMsgBuffer¶
Buffer for thruster force and torques.
-
CmdForceBodyMsgPayload cmdForceBodyInMsgBuffer¶
Buffer for the commanded force input.
-
uint64_t prevTime¶
Previous time, ns.
-
uint64_t numStates¶
Number of states.
-
Eigen::Vector3d thrust_B¶
Thrust expressed in body-frame components.
-
Eigen::Vector3d cmdForce_B¶
External force 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 Phi_k¶
State transition matrix.
-
Eigen::MatrixXd Phi_dot_k¶
Rate of change of STM.
-
Eigen::MatrixXd L¶
-
Eigen::MatrixXd M¶
-
Eigen::MatrixXd H_k1¶
Jacobian of measurement model.
-
Eigen::MatrixXd I_full¶
numStates x numStates identity matrix
-
Eigen::VectorXd k1¶
k1 constant for RK4 integration
-
Eigen::VectorXd k2¶
k2 constant for RK4 integration
-
Eigen::VectorXd k3¶
k3 constant for RK4 integration
-
Eigen::VectorXd k4¶
k4 constant for RK4 integration
-
Eigen::MatrixXd k1_phi¶
k1 STM constant for RK4 integration
-
Eigen::MatrixXd k2_phi¶
k2 STM constant for RK4 integration
-
Eigen::MatrixXd k3_phi¶
k3 STM constant for RK4 integration
-
Eigen::MatrixXd k4_phi¶
k4 STM constant for RK4 integration
-
bool newMeasurements¶
-
double mu_sun¶
Keeps track of whether or not new measurements have been written.
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.
-
SmallBodyNavEKF()¶