Module: inertialUKF¶

This module filters incoming star tracker measurements and reaction wheel data in order to get the best possible inertial attitude estimate. The filter used is an unscented Kalman filter using the Modified Rodrigues Parameters (MRPs) as a non-singular attitude measure. Measurements can be coming in from several camera heads.

More information on can be found in the PDF Description

Functions

void SelfInit_inertialUKF(InertialUKFConfig *configData, int64_t moduleId)¶

This method creates the two moduel output messages.

Return

void

Parameters

configData: The configuration data associated with the CSS WLS estimator
moduleId: The module identifier

void CrossInit_inertialUKF(InertialUKFConfig *configData, int64_t moduleId)¶

This method performs the second stage of initialization for the inertial 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 CSS interface
moduleId: The module identifier

void Read_STMessages(InertialUKFConfig *configData, int64_t moduleId)¶

This method reads in the messages from all available star trackers and orders them with respect to time of measurement

Return

void

Parameters

configData: The configuration data associated with the CSS estimator
moduleId: The module identifier

void Update_inertialUKF(InertialUKFConfig *configData, uint64_t callTime, int64_t moduleId)¶

This method takes the parsed CSS sensor data and outputs an estimate of the sun vector in the ADCS body frame

Return

void

Parameters

configData: The configuration data associated with the CSS estimator
callTime: The clock time at which the function was called (nanoseconds)
moduleId: The module identifier

void Reset_inertialUKF(InertialUKFConfig *configData, uint64_t callTime, int64_t moduleId)¶

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

Return

void

Parameters

configData: The configuration data associated with the CSS estimator
callTime: The clock time at which the function was called (nanoseconds)
moduleId: The module identifier

void inertialUKFAggGyrData(InertialUKFConfig *configData, double prevTime, double propTime, AccDataFswMsg *gyrData)¶

This method aggregates the input gyro data into a combined total quaternion rotation to push the state forward by. This information is stored in the main data structure for use in the propagation routines.

Return

void

Parameters

configData: The configuration data associated with the CSS estimator
prevTime: [s] Previous time step
propTime: The time that we need to fix the filter to (seconds)
gyrData: The gyro measurements that we are going to accumulate forward into time

int inertialUKFTimeUpdate(InertialUKFConfig *configData, double updateTime)¶

This method performs the time update for the inertial 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 CSS estimator
updateTime: The time that we need to fix the filter to (seconds)

int inertialUKFMeasUpdate(InertialUKFConfig *configData, int currentST)¶

This method performs the measurement update for the inertial 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 CSS estimator
currentST: Current star tracker state

void inertialUKFCleanUpdate(InertialUKFConfig *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 CSS estimator

void inertialStateProp(InertialUKFConfig *configData, double *stateInOut, double dt)¶

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

Return

void

Parameters

configData: The configuration data associated with this module
stateInOut: The state that is propagated
dt: Time step (s)

void inertialUKFMeasModel(InertialUKFConfig *configData, int currentST)¶

This method computes what the expected measurement vector is for each CSS that is present on the spacecraft. All data is transacted from the main data structure for the model because there are many variables that would have to be updated otherwise.

Return

void

Parameters

configData: The configuration data associated with the CSS estimator
currentST: current star tracker state

struct STMessage¶

#include <inertialUKF.h>

Star Tracker (ST) sensor container structure. Contains the msg input name and Id and sensor noise value.

Public Members

char stInMsgName[MAX_STAT_MSG_LENGTH]¶: [-] Input message buffer from MIRU

int32_t stInMsgId¶: [-] Input message Id from MIRU

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

struct STDataParsing¶

#include <inertialUKF.h>

Structure to gather the ST messages and content.

Public Members

int numST¶: Number of Star Trackers.

STMessage STMessages[MAX_ST_VEH_COUNT]¶: [-] Decoded MIRU data for both camera heads

struct InertialUKFConfig¶

#include <inertialUKF.h>

Top level structure for the Inertial unscented kalman filter. Used to estimate the spacecraft’s inertial attitude. Measurements are StarTracker data and gyro data.

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 massPropsInMsgName[MAX_STAT_MSG_LENGTH]¶: [-] The name of the mass props message

char rwParamsInMsgName[MAX_STAT_MSG_LENGTH]¶: The name of the RWConfigParams input message.

char rwSpeedsInMsgName[MAX_STAT_MSG_LENGTH]¶: [-] The name of the input RW speeds message

char gyrBuffInMsgName[MAX_STAT_MSG_LENGTH]¶: [-] Input message buffer from MIRU

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 * AKF_N_STATES + 1]¶: [-] Weighting vector for sigma points

double wC[2 * AKF_N_STATES + 1]¶: [-] Weighting vector for sigma points

double stateInit[AKF_N_STATES]¶: [-] State estimate to initialize filter to

double state[AKF_N_STATES]¶: [-] State estimate for time TimeTag

double statePrev[AKF_N_STATES]¶: [-] State estimate for time TimeTag at previous time

double sBar[AKF_N_STATES * AKF_N_STATES]¶: [-] Time updated covariance

double sBarPrev[AKF_N_STATES * AKF_N_STATES]¶: [-] Time updated covariance at previous time

double covar[AKF_N_STATES * AKF_N_STATES]¶: [-] covariance

double covarPrev[AKF_N_STATES * AKF_N_STATES]¶: [-] covariance at previous time

double covarInit[AKF_N_STATES * AKF_N_STATES]¶: [-] Covariance to init filter with

double xBar[AKF_N_STATES]¶: [-] Current mean state estimate

double obs[3]¶: [-] Observation vector for frame

double yMeas[3 * (2 * AKF_N_STATES + 1)]¶: [-] Measurement model data

double SP[(2 * AKF_N_STATES + 1) * AKF_N_STATES]¶: [-] sigma point matrix

double qNoise[MAX_ST_VEH_COUNT * AKF_N_STATES * AKF_N_STATES]¶: [-] process noise matrix

double sQnoise[MAX_ST_VEH_COUNT * AKF_N_STATES * AKF_N_STATES]¶: [-] cholesky of Qnoise

double IInv[3][3]¶: [(kg m^2)^-1] inverse of inertia tensor

uint32_t numUsedGyros¶: Number of currently active CSS sensors

uint32_t firstPassComplete¶: flag

double sigma_BNOut[3]¶: [-] Output MRP

double omega_BN_BOut[3]¶: [r/s] Body rate output data

double timeTagOut¶: [s] Output time-tag information

double maxTimeJump¶: [s] Maximum time jump to allow in propagation

STAttFswMsg stSensorIn[MAX_ST_VEH_COUNT]¶: [-] ST sensor data read in from message bus

int stSensorOrder[MAX_ST_VEH_COUNT]¶: [-] ST sensor data read in from message bus

uint64_t ClockTimeST[MAX_ST_VEH_COUNT]¶: [-] All of the ClockTimes for the STs

uint64_t ReadSizeST[MAX_ST_VEH_COUNT]¶: [-] All of the ReadSizes for the STs

RWArrayConfigFswMsg rwConfigParams¶: [-] struct to store message containing RW config parameters in body B frame

RWSpeedIntMsg rwSpeeds¶: [-] Local reaction wheel speeds

RWSpeedIntMsg rwSpeedPrev¶: [-] Local reaction wheel speeds

double speedDt¶: [s] The time difference between speeds

uint64_t timeWheelPrev¶: [ns] Previous wheel time-tag from msg

VehicleConfigFswMsg localConfigData¶: [-] Vehicle configuration data

LowPassFilterData gyroFilt[3]¶: [-] Low-pass filters for input gyro data

int32_t navStateOutMsgId¶: Id for the outgoing body estimate message

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

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

int32_t rwParamsInMsgId¶: [-] Id for the RWArrayConfigFswMsg ingoing message

int32_t rwSpeedsInMsgId¶: [-] Id for the incoming RW speeds

int32_t gyrBuffInMsgId¶: [-] Id of the input message buffer

STDataParsing STDatasStruct¶: [-] Id of the input message buffer

BSKLogger *bskLogger¶: BSK Logging.