positionODuKF#

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. The filter used is a square root unscented Kalman filter, and the images are first processed by image processing and a measurement model in order to produce this filter’s measurements.

The module PDF Description contains further information on this module’s mathematical framework.

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
navTransOutMsg	NavTransMsgPayload	navigation translation output message
opNavFilterMsg	FilterMsgPayload	output filter data message containing states and covariances
opNavResidualMsg	FilterResidualMsgPayload	output measurement data message containing residuals
cameraPosMsg	CameraLocalizationMsgPayload.h	opnav input message containing the position vector towards the target

Module models#

The measurement model for the filter is functionally contained in the cameraTriangulation module. The message read in therefore contains the predicted measurement:

\[H[X] = X[0:3] = \mathbf{r}\]

The dynamics modules used are point mass, single body gravity, propagated with and RK4 integrator

\[F[X] = \dot{X} = - \frac{\mu}{| X[0:3] |^3}X[0:3] = - \frac{\mu}{r^3}\mathbf{r}\]

where \(r\) is the position of the spacecraft center of masss relative to the central body of gravitational parameter \(\mu\).

Module assumptions and limitations#

The module inherits all assumptions made while implementing a Kalman filter:

Observability considerations
Gaussian covariances
Linearity limits
and more

Otherwise, the limitations are governed by the measurement model and dynamics models relative to the required performance.

User Guide#

This section is to outline the steps needed to setup a positionODSRuKF converter in Python.

Import the module:

from Basilisk.fswAlgorithms import positionODSRuKF

Create an instantiation of converter class:

positionOD = positionODuKF.PositionODuKF()

Setup SRuKF general parameters:

positionOD.alpha = 0.02
positionOD.beta = 2.0

Setup SRuKF measurement parameters, measurement noise Standard Deviation is given in meters:

positionOD.muCentral = 3000*1E9
positionOD.measNoiseScaling = 1
positionOD.measNoiseSD = 100 #m

Setup initial state and covariances:

positionOD.stateInitial = [[1000.*1e3], [0], [0], [0.], [-1.*1e3], [0.]]
positionOD.covarInitial =[ [10., 0., 0., 0., 0., 0.],
                         [0., 10., 0., 0., 0., 0.],
                         [0., 0., 10., 0., 0., 0.],
                         [0., 0., 0., 0.01, 0., 0.],
                         [0., 0., 0., 0., 0.01, 0.],
                         [0., 0., 0., 0., 0., 0.01]]

Setup process noise with units of meters and meters/s:

sigmaPos = 1E2
sigmaVel = 1
positionOD.processNoise = [[sigmaPos, 0., 0., 0., 0., 0.],
                  [0., sigmaPos, 0., 0., 0., 0.],
                  [0., 0., sigmaPos, 0., 0., 0.],
                  [0., 0., 0., sigmaVel, 0., 0.],
                  [0., 0., 0., 0., sigmaVel, 0.],
                  [0., 0., 0., 0., 0., sigmaVel]]

Subscribe to the messages, primarily the measurement message:

positionOD.opNavHeadingMsg.subscribeTo(cameraTriangulation.cameraLocalizationOutMsg)

Class PositionODuKF#

class PositionODuKF : public SysModel#

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

Public Functions

void reset(uint64_t currentSimNanos) override#

Reset the position-based OD filter to an initial state and initializes the internal estimation matrices.

Parameters:: currentSimNanos – The clock time at which the function was called (nanoseconds)
Returns:: void

void updateState(uint64_t currentSimNanos) override#

Take the relative position measurements and outputs an estimate of the spacecraft states in the inertial frame.

Parameters:: currentSimNanos – The clock time at which the function was called (nanoseconds)
Returns:: void

Public Members

Message<FilterResidualsMsgPayload> opNavResidualMsg#: Variables are named closely to the reference document : “The Square-root unscented Kalman Filter for state and parameter-estimation” by van der Merwe and Wan

Eigen::MatrixXd processNoise#: [-] process noise matrix

Eigen::MatrixXd measurementNoise#: [-] Measurement Noise

Eigen::VectorXd stateInitial#: [-] State estimate for time TimeTag at previous time

Eigen::MatrixXd sBarInitial#: [-] Time updated covariance at previous time

Eigen::MatrixXd covarInitial#: [-] covariance at previous time

double measNoiseScaling = 1#: [-] Scale factor that can be applied on the measurement noise to over/under weight

double measNoiseSD = 0.1#: [km] Constant measurement noise