Module: sunlineSEKF

This module implements and tests a Switch Extended Kalman Filter in order to estimate the sunline direction.

More information can be found in the PDF Description

Functions

void SelfInit_sunlineSEKF(sunlineSEKFConfig *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

  • moduleID: The module identifier

void CrossInit_sunlineSEKF(sunlineSEKFConfig *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

  • moduleID: The module identifier

void Reset_sunlineSEKF(sunlineSEKFConfig *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)

  • moduleID: The module identifier

void Update_sunlineSEKF(sunlineSEKFConfig *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)

  • moduleID: The module identifier

void sunlineTimeUpdate(sunlineSEKFConfig *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(sunlineSEKFConfig *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[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH], double bVec[SKF_N_STATES], double dt, double *stateInOut, 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.

Return

void

Parameters

  • stateInOut:

  • dynMat:

  • bVec:

  • dt: time step

  • stateTransition:

void sunlineHMatrixYMeas(double states[EKF_N_STATES_SWITCH], size_t numCSS, double cssSensorCos[MAX_N_CSS_MEAS], double sensorUseThresh, double cssNHat_B[MAX_NUM_CSS_SENSORS * 3], 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.

Return

void

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

void sunlineKalmanGain(double covarBar[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH], double hObs[MAX_N_CSS_MEAS * EKF_N_STATES_SWITCH], double qObsVal, size_t numObs, double *kalmanGain)

This method computes the Kalman gain given the measurements.

Return

void

Parameters

  • covarBar: The time updated covariance

  • hObs: The H matrix filled with the observations

  • qObsVal: The observation noise

  • numObs: The number of observations

  • kalmanGain: Pointer to the Kalman Gain

void sunlineDynMatrix(double states[EKF_N_STATES_SWITCH], double bVec[SKF_N_STATES], 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

Return

void

Parameters

  • states: Updated states

  • bVec: b vector

  • dt: Time step

  • dynMat: Pointer to the Dynamic Matrix

void sunlineCKFUpdate(double xBar[EKF_N_STATES_SWITCH], double kalmanGain[EKF_N_STATES_SWITCH * MAX_N_CSS_MEAS], double covarBar[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH], double qObsVal, size_t numObs, double yObs[MAX_N_CSS_MEAS], double hObs[MAX_N_CSS_MEAS * EKF_N_STATES_SWITCH], double *x, double *covar)

This method computes the updated with a Classical Kalman Filter

Return

void

Parameters

  • xBar: The state after a time update

  • kalmanGain: The computed Kalman Gain

  • covarBar: The time updated covariance

  • qObsVal: The observation noise

  • numObs: 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

void sunlineSEKFUpdate(double kalmanGain[EKF_N_STATES_SWITCH * MAX_N_CSS_MEAS], double covarBar[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH], double qObsVal, size_t numObs, double yObs[MAX_N_CSS_MEAS], double hObs[MAX_N_CSS_MEAS * EKF_N_STATES_SWITCH], double *states, double *x, double *covar)

This method computes the updated with a Extended Kalman Filter

Return

void

Parameters

  • kalmanGain: The computed Kalman Gain

  • covarBar: The time updated covariance

  • qObsVal: The observation noise

  • numObs: 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

void sunlineSEKFSwitch(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

  • bVec_B: Pointer to b vector

  • states: Pointer to the states

  • covar: Pointer to the covariance

void sunlineSEKFComputeDCM_BS(double sunheading[SKF_N_STATES_HALF], double bVec[SKF_N_STATES_HALF], double *dcm)

compute the DCM

Return

void

Parameters

  • sunheading: array of sun heading measurement states

  • bVec: array of bVec values

  • dcm: DCM pointer to be returned

struct sunlineSEKFConfig
#include <sunlineSEKF.h>

Top level structure for the CSS-based Switch Extended Kalman Filter. Used to estimate the sun state in the vehicle body frame.

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 bVec_B[SKF_N_STATES_HALF]

[-] current vector of the b frame used to make Switch frame

double switchTresh

[-] Cosine of angle between singularity and S-frame. If close to 1, the threshold for switching frames is lower. If closer to 0.5 singularity is more largely avoided but switching is more frequent

double state[EKF_N_STATES_SWITCH]

[-] State estimate for time TimeTag

double x[EKF_N_STATES_SWITCH]

State errors

double xBar[EKF_N_STATES_SWITCH]

[-] Current mean state estimate

double covarBar[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH]

[-] Time updated covariance

double covar[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH]

[-] covariance

double stateTransition[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH]

[-] State transition Matrix

double kalmanGain[EKF_N_STATES_SWITCH * MAX_N_CSS_MEAS]

Kalman Gain

double dynMat[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH]

[-] Dynamics Matrix, A

double measMat[MAX_N_CSS_MEAS * EKF_N_STATES_SWITCH]

[-] Measurement Matrix, H

double W_BS[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH]

[-] Switch Matrix to bring states and covariance to new S-frame when switch occurs

double obs[MAX_N_CSS_MEAS]

[-] Observation vector for frame

double yMeas[MAX_N_CSS_MEAS]

[-] Linearized measurement model data

double postFits[MAX_N_CSS_MEAS]

[-] PostFit residuals

double procNoise[(EKF_N_STATES_SWITCH - 3) * (EKF_N_STATES_SWITCH - 3)]

[-] process noise matrix

double measNoise[MAX_N_CSS_MEAS * MAX_N_CSS_MEAS]

[-] Maximally sized obs noise matrix

double cssNHat_B[MAX_NUM_CSS_SENSORS * 3]

[-] CSS normal vectors converted over to body

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 massPropsInMsgId

[-] ID for the incoming mass properties message

int32_t cssConfInMsgId

[-] ID associated with the CSS configuration data

BSKLogger *bskLogger

BSK Logging.