Module: pixelLineBiasUKF

Executive Summary

This module filters position measurements that have been processed from planet images in order to estimate spacecraft relative position to an observed body in the inertial frame. It is similar to the relativeOD filter except that it estimates measurement bias in pixels and therefore integrates the pixel and line transformation in the measurement model. This means this module reads in circle data directly.

The module PDF Description contains further information on this module’s function, how to run it, as well as testing.

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	NavTransMsgPayload	navigation translation output message
filtDataOutMsg	PixelLineFilterMsgPayload	output filter data message
circlesInMsg	OpNavCirclesMsgPayload	input messages with circles information
cameraConfigInMsg	CameraConfigMsgPayload	camera config input message
attInMsg	NavAttMsgPayload	attitude input message

Functions

void SelfInit_pixelLineBiasUKF(PixelLineBiasUKFConfig *configData, int64_t moduleId)

This method creates the two moduel output messages.

Parameters:

configData – The configuration data associated with the OD filter
moduleId – The ID associated with the configData

Returns:

void

void Update_pixelLineBiasUKF(PixelLineBiasUKFConfig *configData, uint64_t callTime, int64_t moduleId)

This method takes the relative position measurements and outputs an estimate of the spacecraft states in the intertial frame.

Parameters:

configData – The configuration data associated with the OD filter
callTime – The clock time at which the function was called (nanoseconds)
moduleId – The ID associated with the configData

Returns:

void

void Reset_pixelLineBiasUKF(PixelLineBiasUKFConfig *configData, uint64_t callTime, int64_t moduleId)

This method resets the relative OD filter to an initial state and initializes the internal estimation matrices.

Parameters:

configData – The configuration data associated with the OD filter
callTime – The clock time at which the function was called (nanoseconds)
moduleId – The ID associated with the configData

Returns:

void

void pixelLineBiasUKFTwoBodyDyn(double state[PIXLINE_N_STATES], double mu, double *stateDeriv)

int pixelLineBiasUKFTimeUpdate(PixelLineBiasUKFConfig *configData, double updateTime)

This method performs the time update for the relative OD kalman filter. It propagates the sigma points forward in time and then gets the current covariance and state estimates.

Parameters:

configData – The configuration data associated with the OD filter
updateTime – The time that we need to fix the filter to (seconds)

Returns:

void

int pixelLineBiasUKFMeasUpdate(PixelLineBiasUKFConfig *configData)

This method performs the measurement update for the 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 OD filter
Returns:: void

void pixelLineBiasUKFCleanUpdate(PixelLineBiasUKFConfig *configData)

This method cleans the filter states after a bad upadate on the fly. It removes the potentially corrupted previous estimates and puts the filter back to a working state.

Parameters:: configData – The configuration data associated with the OD filter
Returns:: void

void relODStateProp(PixelLineBiasUKFConfig *configData, double *stateInOut, double dt)

This method propagates a relative OD state vector forward in time. Note that the calling parameter is updated in place to save on data copies.

Parameters:

configData – The configuration data associated with the OD filter
stateInOut – The state that is propagated
dt – Time step (s)

Returns:

void

void pixelLineBiasUKFMeasModel(PixelLineBiasUKFConfig *configData)

This method computes the measurement model. Given that the data is coming from the pixelLine Converter, the transformation has already taken place from pixel data to spacecraft position.

Parameters:: configData – The configuration data associated with the OD filter
Returns:: void

struct PixelLineBiasUKFConfig

#include <pixelLineBiasUKF.h>

Top level structure for the relative OD unscented kalman filter. Used to estimate the spacecraft’s inertial position relative to a body.

Public Members

NavTransMsg_C navStateOutMsg: navigation translation output message

PixelLineFilterMsg_C filtDataOutMsg: output filter data message

OpNavCirclesMsg_C circlesInMsg: [-] input messages with circles information

CameraConfigMsg_C cameraConfigInMsg: camera config input message

NavAttMsg_C attInMsg: attitude input message

int moduleId: module ID

size_t numStates: [-] Number of states for this filter

size_t countHalfSPs: [-] Number of sigma points over 2

size_t 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 switchMag: [-] Threshold for where we switch MRP set

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

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

double gyrAggTimeTag: [s] Time-tag for aggregated gyro data

double aggSigma_b2b1[3]: [-] Aggregated attitude motion from gyros

double dcm_BdyGyrpltf[3][3]: [-] DCM for converting gyro data to body frame

double wM[2 * PIXLINE_N_STATES + 1]: [-] Weighting vector for sigma points

double wC[2 * PIXLINE_N_STATES + 1]: [-] Weighting vector for sigma points

double stateInit[PIXLINE_N_STATES]: [-] State estimate to initialize filter to

double state[PIXLINE_N_STATES]: [-] State estimate for time TimeTag

double statePrev[PIXLINE_N_STATES]: [-] State estimate for time TimeTag at previous time

double sBar[PIXLINE_N_STATES * PIXLINE_N_STATES]: [-] Time updated covariance

double sBarPrev[PIXLINE_N_STATES * PIXLINE_N_STATES]: [-] Time updated covariance at previous time

double covar[PIXLINE_N_STATES * PIXLINE_N_STATES]: [-] covariance

double covarPrev[PIXLINE_N_STATES * PIXLINE_N_STATES]: [-] covariance at previous time

double covarInit[PIXLINE_N_STATES * PIXLINE_N_STATES]: [-] Covariance to init filter with

double xBar[PIXLINE_N_STATES]: [-] Current mean state estimate

double obs[PIXLINE_N_MEAS]: [-] Observation vector for frame

double yMeas[PIXLINE_N_MEAS * (2 * PIXLINE_N_STATES + 1)]: [-] Measurement model data

double SP[(2 * PIXLINE_N_STATES + 1) * PIXLINE_N_STATES]: [-] sigma point matrix

double qNoise[PIXLINE_N_STATES * PIXLINE_N_STATES]: [-] process noise matrix

double sQnoise[PIXLINE_N_STATES * PIXLINE_N_STATES]: [-] cholesky of Qnoise

double measNoise[PIXLINE_N_MEAS * PIXLINE_N_MEAS]: [-] Measurement Noise

int planetIdInit: [-] Planet being navigated inital value

int planetId: [-] Planet being navigated as per measurement

uint32_t firstPassComplete: [-] Flag to know if first filter update

double postFits[3]: [-] PostFit residuals

double timeTagOut: [s] Output time-tag information

double maxTimeJump: [s] Maximum time jump to allow in propagation

OpNavCirclesMsgPayload circlesInBuffer: [-] ST sensor data read in from message bus

CameraConfigMsgPayload cameraSpecs: [-] Camera specs for nav

NavAttMsgPayload attInfo: [-] Att info for frame transformation

BSKLogger *bskLogger: BSK Logging.