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

&#8212; Threshold below which we discount sensors

NavAttMsgPayload outputHeading

&#8212; Output heading estimate data

OpNavMsgPayload opnavInBuffer

&#8212; message buffer

BSKLogger *bskLogger

BSK Logging.