Module: 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 estimatorcallTime
: 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 estimatorcallTime
: 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 estimatorupdateTime
: 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 estimatorupdateTime
: 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 covariancehObs
: The H matrix filled with the observationss2_B
: Pointer to the second frame vectorstates
: Pointer to the statescovar
: Pointer to the covariance
-
struct
HeadingSuKFConfig
¶ Public Members
The name of the output message
-
char
filtDataOutMsgName
[MAX_STAT_MSG_LENGTH
]¶ The name of the output filter data message
The name of the input opnav data message
-
char
cameraConfigMsgName
[MAX_STAT_MSG_LENGTH
]¶ The name of the camera config 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
]¶
-
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
ID for the outgoing body estimate message
-
int32_t
filtDataOutMsgId
¶ [-] ID for the filter data output message
[-] ID for the opNav data in put message
-
int32_t
cameraConfigMsgID
¶ [-] The ID associated with the incoming camera config message