headingSuKF¶

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

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

Return

void

Parameters

configData: The configuration data associated with the heading estimator

void CrossInit_headingSuKF(HeadingSuKFConfig *configData, int64_t moduleID)¶

This method performs the second stage of initialization for the heading filter. It’s primary function is to link the input messages that were created elsewhere.

Return

void

Parameters

configData: The configuration data associated with the heading filter

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

Return

void

Parameters

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

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.

Return

void

Parameters

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

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.

Return

void

Parameters

configData: The configuration data associated with the heading estimator
updateTime: The time that we need to fix the filter to (seconds)

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.

Return

void

Parameters

configData: The configuration data associated with the heading estimator
updateTime: The time that we need to fix the filter to (seconds)

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.

Return

void

Parameters

stateInOut: The state that is propagated

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.

Return

void

Parameters

configData: The configuration data associated with the heading estimator

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.

Return

void

Parameters

covarBar: The time updated covariance
hObs: The H matrix filled with the observations
s2_B: Pointer to the second frame vector
states: Pointer to the states
covar: Pointer to the covariance

struct HeadingSuKFConfig¶

#include <headingSuKF.h>

Data structure for heading Switch unscented kalman filter estimator. Please see the _Documentation folder for details on how this Kalman Filter Functions.

Public Members

char opnavOutMsgName[MAX_STAT_MSG_LENGTH]¶: The name of the output message

char filtDataOutMsgName[MAX_STAT_MSG_LENGTH]¶: The name of the output filter data message

char opnavDataInMsgName[MAX_STAT_MSG_LENGTH]¶: The name of the input opnav data 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¶: [-] OpNav instrument noise parameter

double dt¶: [s] seconds since last data epoch

double timeTag¶: [s] Time tag for statecovar/etc

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

double obs[OPNAV_MEAS]¶: [-] Current mean state estimate [-] 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

NavAttIntMsg outputHeading¶: Output heading estimate data

OpNavFswMsg opnavInBuffer¶

int32_t opnavDataOutMsgId¶: ID for the outgoing body estimate message

int32_t filtDataOutMsgId¶: [-] ID for the filter data output message

int32_t opnavDataInMsgId¶