Module: relativeODuKF

Executive Summary

This module filters position measurements that have been processed from planet images in order to estimate spacecraft relative position to an observed body in the inertial frame. The filter used is an unscented Kalman filter, and the images are first processed by houghCricles and pixelLineConverter in order to produce this filter’s measurements.

The module PDF Description contains further information on this module’s function, how to run it, as well as testing.

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.

Module I/O Messages

Msg Variable Name

Msg Type

Description

navStateOutMsg

NavTransMsgPayload

navigation translation output message

filtDataOutMsg

OpNavFilterMsgPayload

output filter data message

opNavInMsg

OpNavMsgPayload

opnav input message

Functions

void SelfInit_relODuKF(RelODuKFConfig *configData, int64_t moduleId)

This method creates the two moduel output messages.

Parameters:

  • configData – The configuration data associated with the OD filter

  • moduleId – The ID associated with the configData

Returns:

void

void Update_relODuKF(RelODuKFConfig *configData, uint64_t callTime, int64_t moduleId)

This method takes the relative position measurements and outputs an estimate of the spacecraft states in the intertial frame.

Parameters:

  • configData – The configuration data associated with the OD filter

  • callTime – The clock time at which the function was called (nanoseconds)

  • moduleId – The ID associated with the configData

Returns:

void

void Reset_relODuKF(RelODuKFConfig *configData, uint64_t callTime, int64_t moduleId)

This method resets the relative OD filter to an initial state and initializes the internal estimation matrices.

Parameters:

  • configData – The configuration data associated with the OD filter

  • callTime – The clock time at which the function was called (nanoseconds)

  • moduleId – The ID associated with the configData

Returns:

void

void relODuKFTwoBodyDyn(double state[ODUKF_N_STATES], double mu, double *stateDeriv)

Function for two body dynamics solvers in order to use in the RK4. Only two body dynamics is used currently, but SRP, Solar Gravity, spherical harmonics can be added here.

Parameters:

  • state – The starting state

  • muPlanet – Planet gravity constant

  • stateDeriv – State derivative vector

Returns:

double Next state

int relODuKFTimeUpdate(RelODuKFConfig *configData, double updateTime)

This method performs the time update for the relative OD kalman filter. It propagates the sigma points forward in time and then gets the current covariance and state estimates.

Parameters:

  • configData – The configuration data associated with the OD filter

  • updateTime – The time that we need to fix the filter to (seconds)

Returns:

void

int relODuKFMeasUpdate(RelODuKFConfig *configData)

This method performs the measurement update for the kalman filter. It applies the observations in the obs vectors to the current state estimate and updates the state/covariance with that information.

Parameters:

configData – The configuration data associated with the OD filter

Returns:

void

void relODuKFCleanUpdate(RelODuKFConfig *configData)

This method cleans the filter states after a bad upadate on the fly. It removes the potentially corrupted previous estimates and puts the filter back to a working state.

Parameters:

configData – The configuration data associated with the OD filter

Returns:

void

void relODStateProp(RelODuKFConfig *configData, double *stateInOut, double dt)

This method propagates a relative OD state vector forward in time. Note that the calling parameter is updated in place to save on data copies.

Parameters:

  • configData – The configuration data associated with the OD filter

  • stateInOut – The state that is propagated

  • dt – Time step (s)

Returns:

void

void relODuKFMeasModel(RelODuKFConfig *configData)

This method computes the measurement model. Given that the data is coming from the pixelLine Converter, the transformation has already taken place from pixel data to spacecraft position.

Parameters:

configData – The configuration data associated with the OD filter

Returns:

void

struct RelODuKFConfig
#include <relativeODuKF.h>

Top level structure for the relative OD unscented kalman filter. Used to estimate the spacecraft’s inertial position relative to a body.

Public Members

NavTransMsg_C navStateOutMsg

navigation output message

OpNavFilterMsg_C filtDataOutMsg

output filter data message

OpNavMsg_C opNavInMsg

opnav input message

size_t numStates

[-] Number of states for this filter

size_t countHalfSPs

[-] Number of sigma points over 2

size_t numObs

[-] Number of measurements this cycle

double beta

[-] Beta parameter for filter

double alpha

[-] Alpha parameter for filter

double kappa

[-] Kappa parameter for filter

double lambdaVal

[-] Lambda parameter for filter

double gamma

[-] Gamma parameter for filter

double switchMag

[-] Threshold for where we switch MRP set

double dt

[s] seconds since last data epoch

double timeTag

[s] Time tag for statecovar/etc

double gyrAggTimeTag

[s] Time-tag for aggregated gyro data

double aggSigma_b2b1[3]

[-] Aggregated attitude motion from gyros

double dcm_BdyGyrpltf[3][3]

[-] DCM for converting gyro data to body frame

double wM[2 * ODUKF_N_STATES + 1]

[-] Weighting vector for sigma points

double wC[2 * ODUKF_N_STATES + 1]

[-] Weighting vector for sigma points

double stateInit[ODUKF_N_STATES]

[-] State estimate to initialize filter to

double state[ODUKF_N_STATES]

[-] State estimate for time TimeTag

double statePrev[ODUKF_N_STATES]

[-] State estimate for time TimeTag at previous time

double sBar[ODUKF_N_STATES * ODUKF_N_STATES]

[-] Time updated covariance

double sBarPrev[ODUKF_N_STATES * ODUKF_N_STATES]

[-] Time updated covariance at previous time

double covar[ODUKF_N_STATES * ODUKF_N_STATES]

[-] covariance

double covarPrev[ODUKF_N_STATES * ODUKF_N_STATES]

[-] covariance at previous time

double covarInit[ODUKF_N_STATES * ODUKF_N_STATES]

[-] Covariance to init filter with

double xBar[ODUKF_N_STATES]

[-] Current mean state estimate

double obs[3]

[-] Observation vector for frame

double yMeas[3 * (2 * ODUKF_N_STATES + 1)]

[-] Measurement model data

double SP[(2 * ODUKF_N_STATES + 1) * ODUKF_N_STATES]

[-] sigma point matrix

double qNoise[ODUKF_N_STATES * ODUKF_N_STATES]

[-] process noise matrix

double sQnoise[ODUKF_N_STATES * ODUKF_N_STATES]

[-] cholesky of Qnoise

double measNoise[ODUKF_N_MEAS * ODUKF_N_MEAS]

[-] Measurement Noise

double noiseSF

[-] Scale factor for Measurement Noise

int planetIdInit

[-] Planet being navigated inital value

int planetId

[-] Planet being navigated as per measurement

uint32_t firstPassComplete

[-] Flag to know if first filter update

double postFits[3]

[-] PostFit residuals

double timeTagOut

[s] Output time-tag information

double maxTimeJump

[s] Maximum time jump to allow in propagation

OpNavMsgPayload opNavInBuffer

[-] ST sensor data read in from message bus

BSKLogger *bskLogger

BSK Logging.