Module: okeefeEKF

Executive Summary

This module implements and tests a Extended Kalman Filter in order to estimate the sunline 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

navStateOutMsg

NavAttMsgPayload

name of the navigation output message containing the estimated states

filtDataOutMsg

SunlineFilterMsgPayload

name of the output filter data message

cssDataInMsg

CSSArraySensorMsgPayload

name of the CSS sensor input message

cssConfigInMsg

CSSConfigMsgPayload

name of the CSS configuration input message

Functions

void SelfInit_okeefeEKF(okeefeEKFConfig *configData, int64_t moduleID)

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

Parameters:

  • configData – The configuration data associated with the CSS WLS estimator

  • moduleID – The module identifier

Returns:

void

void Reset_okeefeEKF(okeefeEKFConfig *configData, uint64_t callTime, int64_t moduleID)

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

Parameters:

  • configData – The configuration data associated with the CSS estimator

  • callTime – The clock time at which the function was called (nanoseconds)

  • moduleID – The module identifier

Returns:

void

void Update_okeefeEKF(okeefeEKFConfig *configData, uint64_t callTime, int64_t moduleID)

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

Parameters:

  • configData – The configuration data associated with the CSS estimator

  • callTime – The clock time at which the function was called (nanoseconds)

  • moduleID – The module identifier

Returns:

void

void sunlineTimeUpdate(okeefeEKFConfig *configData, double updateTime)

This method performs the time update for the sunline kalman filter. It calls for the updated Dynamics Matrix, as well as the new states and STM. It then updates the covariance, with process noise.

Parameters:

  • configData – The configuration data associated with the CSS estimator

  • updateTime – The time that we need to fix the filter to (seconds)

Returns:

void

void sunlineMeasUpdate(okeefeEKFConfig *configData, double updateTime)

This method performs the measurement update for the sunline 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 CSS estimator

  • updateTime – The time that we need to fix the filter to (seconds)

Returns:

void

void sunlineStateSTMProp(double dynMat[SKF_N_STATES_HALF * SKF_N_STATES_HALF], double dt, double omega[SKF_N_STATES_HALF], double *stateInOut, double *prevstates, double *stateTransition)

This method propagates a sunline state vector forward in time. Note that the calling parameter is updated in place to save on data copies. This also updates the STM using the dynamics matrix.

Parameters:

  • dynMat – dynamic matrix

  • dt – time step

  • omega – angular velocity

  • stateInOut – pointer to a state array

  • prevstates – pointer to previous states

  • stateTransition – pointer to state transition matrix

Returns:

void

void sunlineHMatrixYMeas(double states[SKF_N_STATES_HALF], size_t numCSS, double cssSensorCos[MAX_N_CSS_MEAS], double sensorUseThresh, double cssNHat_B[MAX_NUM_CSS_SENSORS * 3], double CBias[MAX_NUM_CSS_SENSORS], double *obs, double *yMeas, int *numObs, double *measMat)

This method computes the H matrix, defined by dGdX. As well as computing the innovation, difference between the measurements and the expected measurements. This methods modifies the numObs, measMat, and yMeas.

Parameters:

  • states –

  • numCSS – The total number of CSS

  • cssSensorCos – The list of the measurements from the CSSs

  • sensorUseThresh – Thresh The Threshold below which the measuremnts are not read

  • cssNHat_B – The normals vector for each of the CSSs

  • obs – Pointer to the observations

  • yMeas – Pointer to the innovation

  • numObs – Pointer to the number of observations

  • measMat – Point to the H measurement matrix

  • CBias – Vector of biases

Returns:

void

void sunlineKalmanGainOkeefe(double covarBar[SKF_N_STATES_HALF * SKF_N_STATES_HALF], double hObs[MAX_N_CSS_MEAS * SKF_N_STATES_HALF], double qObsVal, int numObsInt, double *kalmanGain)

This method computes the Kalman gain given the measurements.

Parameters:

  • covarBar – The time updated covariance

  • hObs – The H matrix filled with the observations

  • qObsVal – The observation noise

  • numObsInt – The number of observations

  • kalmanGain – Pointer to the Kalman Gain

Returns:

void

void sunlineRateCompute(double states[SKF_N_STATES_HALF], double dt, double prev_states[SKF_N_STATES_HALF], double *omega)

This method computes the rotation rate of the spacecraft by using the two previous state estimates.

Parameters:

  • states – Updated states

  • dt – Time step

  • prev_states – The states saved from previous step for this purpose

  • omega – Pointer to the rotation rate

Returns:

void

void sunlineDynMatrixOkeefe(double omega[SKF_N_STATES_HALF], double dt, double *dynMat)

This method computes the dynamics matrix, which is the derivative of the dynamics F by the state X, evaluated at the reference state. It takes in the configure data and updates this A matrix pointer called dynMat

Parameters:

  • omega – The rotation rate

  • dt – Time step

  • dynMat – Pointer to the Dynamic Matrix

Returns:

void

void sunlineCKFUpdateOkeefe(double xBar[SKF_N_STATES_HALF], double kalmanGain[SKF_N_STATES_HALF * MAX_N_CSS_MEAS], double covarBar[SKF_N_STATES_HALF * SKF_N_STATES_HALF], double qObsVal, int numObsInt, double yObs[MAX_N_CSS_MEAS], double hObs[MAX_N_CSS_MEAS * SKF_N_STATES_HALF], double *x, double *covar)

This method computes the updated with a Classical Kalman Filter

Parameters:

  • xBar – The state after a time update

  • kalmanGain – The computed Kalman Gain

  • covarBar – The time updated covariance

  • qObsVal – The observation noise

  • numObsInt – The amount of CSSs that get measurements

  • yObs – The y vector after receiving the measurements

  • hObs – The H matrix filled with the observations

  • x – Pointer to the state error for modification

  • covar – Pointer to the covariance after update

Returns:

void

void okeefeEKFUpdate(double kalmanGain[SKF_N_STATES_HALF * MAX_N_CSS_MEAS], double covarBar[SKF_N_STATES_HALF * SKF_N_STATES_HALF], double qObsVal, int numObsInt, double yObs[MAX_N_CSS_MEAS], double hObs[MAX_N_CSS_MEAS * SKF_N_STATES_HALF], double *states, double *x, double *covar)

This method computes the updated with a Extended Kalman Filter

Parameters:

  • kalmanGain – The computed Kalman Gain

  • covarBar – The time updated covariance

  • qObsVal – The observation noise

  • numObsInt – The amount of CSSs that get measurements

  • yObs – The y vector after receiving the measurements

  • hObs – The H matrix filled with the observations

  • states – Pointer to the states

  • x – Pointer to the state error for modification

  • covar – Pointer to the covariance after update

Returns:

void

struct okeefeEKFConfig
#include <okeefeEKF.h>

Data structure for CSS Extended kalman filter estimator without gyros measurements.

Public Members

NavAttMsg_C navStateOutMsg

The name of the output message

SunlineFilterMsg_C filtDataOutMsg

The name of the output filter data message

CSSArraySensorMsg_C cssDataInMsg

The name of the Input message

CSSConfigMsg_C cssConfigInMsg

[-] The name of the CSS configuration message

double qObsVal

[-] CSS instrument noise parameter

double qProcVal

[-] Process noise parameter

double dt

[s] seconds since last data epoch

double timeTag

[s] Time tag for statecovar/etc

double state[SKF_N_STATES_HALF]

[-] State estimate for time TimeTag

double prev_states[SKF_N_STATES_HALF]

[-] State estimate for previous time TimeTag

double omega[SKF_N_STATES_HALF]

[-] Rotation rate vector

double x[SKF_N_STATES_HALF]

[-] State errors

double xBar[SKF_N_STATES_HALF]

[-] Current time updated mean state estimate

double covarBar[SKF_N_STATES_HALF * SKF_N_STATES_HALF]

[-] Time updated covariance

double covar[SKF_N_STATES_HALF * SKF_N_STATES_HALF]

[-] covariance

double stateTransition[SKF_N_STATES_HALF * SKF_N_STATES_HALF]

[-] covariance

double kalmanGain[SKF_N_STATES_HALF * MAX_N_CSS_MEAS]

Kalman Gain

double dynMat[SKF_N_STATES_HALF * SKF_N_STATES_HALF]

[-] Dynamics Matrix, A

double measMat[MAX_N_CSS_MEAS * SKF_N_STATES_HALF]

[-] Measurement Matrix, H

double obs[MAX_N_CSS_MEAS]

[-] Observation vector for frame

double yMeas[MAX_N_CSS_MEAS]

[-] Linearized measurement model data

double procNoise[SKF_N_STATES_HALF * SKF_N_STATES_HALF]

[-] process noise matrix

double measNoise[MAX_N_CSS_MEAS * MAX_N_CSS_MEAS]

[-] Maximally sized obs noise matrix

double postFits[MAX_N_CSS_MEAS]

[-] PostFit residuals

double cssNHat_B[MAX_NUM_CSS_SENSORS * 3]

[-] CSS normal vectors converted over to body

double CBias[MAX_NUM_CSS_SENSORS]

[-] CSS individual calibration coefficients

uint32_t numStates

[-] Number of states for this filter

size_t numObs

[-] Number of measurements this cycle

uint32_t numActiveCss

&#8212; Number of currently active CSS sensors

uint32_t numCSSTotal

[-] Count on the number of CSS we have on the spacecraft

double sensorUseThresh

&#8212; Threshold below which we discount sensors

double eKFSwitch

&#8212; Max covariance element after which the filter switches to an EKF

NavAttMsgPayload outputSunline

&#8212; Output sunline estimate data

CSSArraySensorMsgPayload cssSensorInBuffer

[-] CSS sensor data read in from message bus

BSKLogger *bskLogger

BSK Logging.