Module: 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

Functions

void SelfInit_headingSuKF(HeadingSuKFConfig *configData, int64_t moduleID)¶

This method initializes the configData for the heading estimator. It checks to ensure that the inputs are sane and then creates the output message

Parameters

configData – The configuration data associated with the heading estimator
moduleID – The module identifier

Returns

void

void Update_headingSuKF(HeadingSuKFConfig *configData, uint64_t callTime, int64_t moduleID)¶

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

Parameters

configData – The configuration data associated with the heading estimator
callTime – The clock time at which the function was called (nanoseconds)
moduleID – The module identifier

Returns

void

void Reset_headingSuKF(HeadingSuKFConfig *configData, uint64_t callTime, int64_t moduleID)¶

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

Parameters

configData – The configuration data associated with the heading estimator
callTime – The clock time at which the function was called (nanoseconds)
moduleID – The module identifier

Returns

void

void headingSuKFTimeUpdate(HeadingSuKFConfig *configData, double updateTime)¶

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

Returns

void

void headingSuKFMeasUpdate(HeadingSuKFConfig *configData, double updateTime)¶

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

Returns

void

void headingStateProp(double *stateInOut, double *b_vec, double dt)¶

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

Parameters

stateInOut – The state that is propagated
b_Vec – pointer to b vector
dt – time step

Returns

void

void headingSuKFMeasModel(HeadingSuKFConfig *configData)¶

This method computes what the expected measurement vector is for each opnave measurement. 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 heading estimator
Returns: void

void headingSuKFComputeDCM_BS(double heading[HEAD_N_STATES], double bVec[HEAD_N_STATES], double *dcm)¶

void headingSuKFSwitch(double *bVec_B, double *states, double *covar)¶

This method computes the dcms necessary for the switch between the two frames. It the switches the states and the covariance, and sets s2 to be the new, different vector of the body frame.

Parameters

bVec_B – Pointer to b-vector
states – Pointer to the states
covar – Pointer to the covariance

Returns

void

struct HeadingSuKFConfig¶

#include <headingSuKF.h>

Top level structure for the SuKF heading module data.

Public Members

OpNavMsg_C opnavDataOutMsg¶: output message

HeadingFilterMsg_C filtDataOutMsg¶: output message

OpNavMsg_C opnavDataInMsg¶: input message

CameraConfigMsg_C 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¶: — Threshold below which we discount sensors

NavAttMsgPayload outputHeading¶: — Output heading estimate data

OpNavMsgPayload opnavInBuffer¶: — message buffer

BSKLogger *bskLogger¶: BSK Logging.