sunlineUKF#

Executive Summary#

This module implements and tests a Unscented Kalman Filter in order to estimate the sunline direction.

More information 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

name of the navigation output message containing the estimated states

filtDataOutMsg

SunlineFilterMsgPayload

name of the output filter data message

cssDataInMsg

CSSArraySensorMsgPayload

name of the CSS sensor input message

cssConfigInMsg

CSSConfigMsgPayload

name of the CSS configuration input message

Class SunlineUKF#

class SunlineUKF : public SysModel#

Top level structure for the CSS-based unscented Kalman Filter. Used to estimate the sun state in the vehicle body frame.

Public Functions

void reset(uint64_t callTime) override#

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

Parameters:

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

Returns:

void

void updateState(uint64_t callTime) override#

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

Parameters:

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

Returns:

void

Public Members

Message<NavAttMsgPayload> navStateOutMsg#

The name of the output message

Message<SunlineFilterMsgPayload> filtDataOutMsg#

The name of the output filter data message

ReadFunctor<CSSArraySensorMsgPayload> cssDataInMsg#

The name of the Input message

ReadFunctor<CSSConfigMsgPayload> cssConfigInMsg#

[-] The name of the CSS configuration message

int numStates#

[-] Number of states for this filter

int countHalfSPs#

[-] Number of sigma points over 2

int 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 qObsVal#

[-] CSS instrument noise parameter

double dt#

[s] seconds since last data epoch

double timeTag#

[s] Time tag for statecovar/etc

double wM[2 * SKF_N_STATES + 1]#

[-] Weighting vector for sigma points

double wC[2 * SKF_N_STATES + 1]#

[-] Weighting vector for sigma points

double state[SKF_N_STATES]#

[-] State estimate for time TimeTag

double sBar[SKF_N_STATES * SKF_N_STATES]#

[-] Time updated covariance

double covar[SKF_N_STATES * SKF_N_STATES]#

[-] covariance

double xBar[SKF_N_STATES]#

[-] Current mean state estimate

double obs[MAX_N_CSS_MEAS]#

[-] Observation vector for frame

double yMeas[MAX_N_CSS_MEAS * (2 * SKF_N_STATES + 1)]#

[-] Measurement model data

double postFits[MAX_N_CSS_MEAS]#

[-] PostFit residuals

double SP[(2 * SKF_N_STATES + 1) * SKF_N_STATES]#

[-] sigma point matrix

double qNoise[SKF_N_STATES * SKF_N_STATES]#

[-] process noise matrix

double sQnoise[SKF_N_STATES * SKF_N_STATES]#

[-] cholesky of Qnoise

double qObs[MAX_N_CSS_MEAS * MAX_N_CSS_MEAS]#

[-] Maximally sized obs noise matrix

double cssNHat_B[MAX_NUM_CSS_SENSORS * 3]#

[-] CSS normal vectors converted over to body

double CBias[MAX_NUM_CSS_SENSORS]#

[-] CSS individual calibration coefficients

uint32_t numActiveCss#

&#8212; Number of currently active CSS sensors

uint32_t numCSSTotal#

[-] Count on the number of CSS we have on the spacecraft

double sensorUseThresh#

&#8212; Threshold below which we discount sensors

NavAttMsgPayload outputSunline#

&#8212; Output sunline estimate data

CSSArraySensorMsgPayload cssSensorInBuffer#

[-] CSS sensor data read in from message bus

BSKLogger bskLogger = {}#

BSK Logging.