Module: relativeODuKF¶

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.

Functions

void SelfInit_relODuKF(RelODuKFConfig *configData, int64_t moduleId)¶

This method creates the two moduel output messages.

Return

void

Parameters

configData: The configuration data associated with the OD filter

void CrossInit_relODuKF(RelODuKFConfig *configData, int64_t moduleId)¶

This method performs the second stage of initialization for the OD filter. It’s primary function is to link the input messages that were created elsewhere.

Return

void

Parameters

configData: The configuration data associated with the OD filter

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.

Return

void

Parameters

configData: The configuration data associated with the OD filter
callTime: The clock time at which the function was called (nanoseconds)

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.

Return

void

Parameters

configData: The configuration data associated with the OD filter
callTime: The clock time at which the function was called (nanoseconds)

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.

Return

double Next state

Parameters

state: The starting 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.

Return

void

Parameters

configData: The configuration data associated with the OD filter
updateTime: The time that we need to fix the filter to (seconds)

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.

Return

void

Parameters

configData: The configuration data associated with the OD filter
updateTime: The time that we need to fix the filter to (seconds)

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.

Return

void

Parameters

configData: The configuration data associated with the OD filter

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.

Return

void

Parameters

stateInOut: The state that is propagated

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.

Return

void

Parameters

configData: The configuration data associated with the OD filter

struct OpNavMeas¶

#include <relativeODuKF.h>

Structure to gather the OpNav messages and content.

Public Members

char opNavInMsgName[MAX_STAT_MSG_LENGTH]¶: [-] Input message buffer from opNav measurement method

int32_t opNavInMsgId¶: [-] Input message Id from opNav measurement method

double noise[3 * 3]¶: [-] Per axis noise on the measurement

struct OpNavParsing¶

#include <relativeODuKF.h>

Structure to gather the OpNav messages and content.

Public Members

int numMethods¶: Number of opNav measurement methods.

OpNavMeas OpNav[MAX_ST_VEH_COUNT]¶: [-] Decoded data for both measurement methods

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

char navStateOutMsgName[MAX_STAT_MSG_LENGTH]¶: The name of the output message.

char filtDataOutMsgName[MAX_STAT_MSG_LENGTH]¶: The name of the output filter data message.

char opNavInMsgName[MAX_STAT_MSG_LENGTH]¶: [-] The name of the input RW speeds 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

OpNavFswMsg opNavInMsg¶: [-] ST sensor data read in from message bus

int32_t navStateOutMsgId¶: Id for the outgoing body estimate message

int32_t filtDataOutMsgId¶: [-] Id for the filter data output message

int32_t opNavInMsgId¶: [-] Id for the incoming mass properties message

BSKLogger *bskLogger¶: BSK Logging.