okeefeEKF

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

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

Return

void

Parameters

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

void CrossInit_okeefeEKF(okeefeEKFConfig *configData, int64_t moduleID)

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

Return

void

Parameters

  • configData: The configuration data associated with the CSS interface

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.

Return

void

Parameters

  • configData: The configuration data associated with the CSS estimator

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

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

Return

void

Parameters

  • configData: The configuration data associated with the CSS estimator

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

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.

Return

void

Parameters

  • configData: The configuration data associated with the CSS estimator

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

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.

Return

void

Parameters

  • configData: The configuration data associated with the CSS estimator

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

void sunlineStateSTMProp(double dynMat[SKF_N_STATES * SKF_N_STATES], double dt, double omega[SKF_N_STATES_HALF], double *stateInOut, double *prevstates, double *stateTransition)
void sunlineHMatrixYMeas(double states[SKF_N_STATES], 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)
void sunlineKalmanGain(double covarBar[SKF_N_STATES * SKF_N_STATES], double hObs[MAX_N_CSS_MEAS * SKF_N_STATES], double qObsVal, int numObs, double *kalmanGain)
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.

Return

void

Parameters

  • states: Updated states

  • dt: Time step

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

  • omega: Pointer to the rotation rate

void sunlineDynMatrix(double omega[SKF_N_STATES], double dt, double *dynMat)
void sunlineCKFUpdate(double xBar[SKF_N_STATES], double kalmanGain[SKF_N_STATES * MAX_N_CSS_MEAS], double covarBar[SKF_N_STATES * SKF_N_STATES], double qObsVal, int numObs, double yObs[MAX_N_CSS_MEAS], double hObs[MAX_N_CSS_MEAS * SKF_N_STATES], double *x, double *covar)
void okeefeEKFUpdate(double kalmanGain[SKF_N_STATES * MAX_N_CSS_MEAS], double covarBar[SKF_N_STATES * SKF_N_STATES], double qObsVal, int numObs, double yObs[MAX_N_CSS_MEAS], double hObs[MAX_N_CSS_MEAS * SKF_N_STATES], double *states, double *x, double *covar)
struct okeefeEKFConfig
#include <okeefeEKF.h>

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

Public Members

char navStateOutMsgName[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 cssDataInMsgName[MAX_STAT_MSG_LENGTH]

The name of the Input message

char cssConfigInMsgName[MAX_STAT_MSG_LENGTH]

[-] 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]
double xBar[SKF_N_STATES_HALF]

[-] State errors

double covarBar[SKF_N_STATES_HALF * SKF_N_STATES_HALF]

[-] Current time updated mean state estimate [-] 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]
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

Number of currently active CSS sensors

uint32_t numCSSTotal

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

double sensorUseThresh

Threshold below which we discount sensors

double eKFSwitch

Max covariance element after which the filter switches to an EKF

NavAttIntMsg outputSunline

Output sunline estimate data

CSSArraySensorIntMsg cssSensorInBuffer

[-] CSS sensor data read in from message bus

int32_t navStateOutMsgId

ID for the outgoing body estimate message

int32_t filtDataOutMsgId

[-] ID for the filter data output message

int32_t cssDataInMsgId

ID for the incoming CSS sensor message

int32_t cssConfigInMsgId

[-] ID associated with the CSS configuration data