headingSuKF#

Executive Summary#

This module implements and tests a Switch Unscented Kalman Filter in order to estimate an arbitrary heading direction. 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

opnavDataOutMsg

OpNavMsgPayload

output message with opnav information

filtDataOutMsg

HeadingFilterMsgPayload

output message with filter state data information

opnavDataInMsg

OpNavMsgPayload

optical navigation input message

cameraConfigInMsg

CameraConfigMsgPayload

(optional) camera configuration input message

Class HeadingSuKF#

class HeadingSuKF : public SysModel#

Top level structure for the SuKF heading module data.

Public Functions

void reset(uint64_t callTime) override#

This method resets the heading 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 heading 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<OpNavMsgPayload> opnavDataOutMsg#

output message

Message<HeadingFilterMsgPayload> filtDataOutMsg#

output message

ReadFunctor<OpNavMsgPayload> opnavDataInMsg#

input message

ReadFunctor<CameraConfigMsgPayload> cameraConfigInMsg#

(optional) input message

int putInCameraFrame#

[-] If camera message is found output the result to the camera frame as well as the body and inertial frame

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#

[-] OpNav instrument noise parameter

double rNorm#

[-] OpNav measurment norm

double dt#

[s] seconds since last data epoch

double timeTag#

[s] Time tag for statecovar/etc

double noiseSF#

[-] Scale factor for noise

double bVec_B[HEAD_N_STATES]#

[-] current vector of the b frame used to make frame

double switchTresh#

[-] Threshold for switching frames

double stateInit[HEAD_N_STATES_SWITCH]#

[-] State to initialize filter to

double state[HEAD_N_STATES_SWITCH]#

[-] State estimate for time TimeTag

double wM[2 * HEAD_N_STATES_SWITCH + 1]#

[-] Weighting vector for sigma points

double wC[2 * HEAD_N_STATES_SWITCH + 1]#

[-] Weighting vector for sigma points

double sBar[HEAD_N_STATES_SWITCH * HEAD_N_STATES_SWITCH]#

[-] Time updated covariance

double covarInit[HEAD_N_STATES_SWITCH * HEAD_N_STATES_SWITCH]#

[-] covariance to init to

double covar[HEAD_N_STATES_SWITCH * HEAD_N_STATES_SWITCH]#

[-] covariance

double xBar[HEAD_N_STATES_SWITCH]#

[-] Current mean state estimate

double obs[OPNAV_MEAS]#

[-] Observation vector for frame

double yMeas[OPNAV_MEAS * (2 * HEAD_N_STATES_SWITCH + 1)]#

[-] Measurement model data

double postFits[OPNAV_MEAS]#

[-] PostFit residuals

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

[-] sigma point matrix

double qNoise[HEAD_N_STATES_SWITCH * HEAD_N_STATES_SWITCH]#

[-] process noise matrix

double sQnoise[HEAD_N_STATES_SWITCH * HEAD_N_STATES_SWITCH]#

[-] cholesky of Qnoise

double qObs[OPNAV_MEAS * OPNAV_MEAS]#

[-] Maximally sized obs noise matrix

double sensorUseThresh#

&#8212; Threshold below which we discount sensors

NavAttMsgPayload outputHeading#

&#8212; Output heading estimate data

OpNavMsgPayload opnavInBuffer#

&#8212; message buffer

BSKLogger bskLogger = {}#

BSK Logging.