Module: inertialUKF

Executive Summary

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

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

NavAttMsgPayload

navigation output message

filtDataOutMsg

InertialFilterMsgPayload

name of the output filter data message

massPropsInMsg

VehicleConfigMsgPayload

spacecraft vehicle configuration input message

rwParamsInMsg

RWArrayConfigMsgPayload

reaction wheel parameter input message. Can be an empty message if no RW are included.

rwSpeedsInMsg

RWSpeedMsgPayload

reaction wheel speed input message. Can be an empty message if no RW are included.

gyrBuffInMsg

AccDataMsgPayload

rate gyro input message


Functions

void SelfInit_inertialUKF(InertialUKFConfig *configData, int64_t moduleId)

This method creates the two moduel output messages.

Parameters:
  • configData – The configuration data associated with the CSS WLS estimator

  • moduleId – The module identifier

Returns:

void

void Read_STMessages(InertialUKFConfig *configData)

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

Parameters:

configData – The configuration data associated with the CSS estimator

Returns:

void

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

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

Returns:

void

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.

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

Returns:

void

void inertialUKFAggGyrData(InertialUKFConfig *configData, double prevTime, double propTime, AccDataMsgPayload *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.

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

Returns:

void

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.

Parameters:
  • configData – The configuration data associated with the CSS estimator

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

Returns:

void

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.

Parameters:
  • configData – The configuration data associated with the CSS estimator

  • currentST – Current star tracker state

Returns:

void

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.

Parameters:

configData – The configuration data associated with the CSS estimator

Returns:

void

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.

Parameters:
  • configData – The configuration data associated with this module

  • stateInOut – The state that is propagated

  • dt – Time step (s)

Returns:

void

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.

Parameters:
  • configData – The configuration data associated with the CSS estimator

  • currentST – current star tracker state

Returns:

void

struct STMessage
#include <inertialUKF.h>

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

Public Members

STAttMsg_C stInMsg

star tracker input message

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

NavAttMsg_C navStateOutMsg

The name of the output message.

InertialFilterMsg_C filtDataOutMsg

The name of the output filter data message.

VehicleConfigMsg_C massPropsInMsg

[-] The name of the mass props message

RWArrayConfigMsg_C rwParamsInMsg

The name of the RWConfigParams input message.

RWSpeedMsg_C rwSpeedsInMsg

[-] The name of the input RW speeds message

AccDataMsg_C gyrBuffInMsg

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

&#8212; 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

STAttMsgPayload 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

int isFreshST[MAX_ST_VEH_COUNT]

[-] isWritten flag for STs

RWArrayConfigMsgPayload rwConfigParams

[-] struct to store message containing RW config parameters in body B frame

RWSpeedMsgPayload rwSpeeds

[-] Local reaction wheel speeds

RWSpeedMsgPayload rwSpeedPrev

[-] Local reaction wheel speeds

double speedDt

[s] The time difference between speeds

uint64_t timeWheelPrev

[ns] Previous wheel time-tag from msg

VehicleConfigMsgPayload localConfigData

[-] Vehicle configuration data

LowPassFilterData gyroFilt[3]

[-] Low-pass filters for input gyro data

STDataParsing STDatasStruct

[-] Id of the input message buffer

BSKLogger *bskLogger

BSK Logging.