modest.substates package

Submodules

modest.substates.attitude module

class modest.substates.attitude.Attitude(attitudeQuaternion=Quaternion(1.0, 0.0, 0.0, 0.0), attitudeErrorCovariance=array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]), gyroBias=array([0., 0., 0.]), gyroBiasCovariance=array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]), t=0, covarianceStorage='covariance', useUnitVector=True, storeLastStateVectors=0)

Bases: modest.substates.substate.SubState

Estimates the attitude of a vehicle in three dimensions, along with three gyro bias states.

This class contains a six-state attitude estimator: three attitude states and three gyro bias states.

This class can function as a stand-alone class, or it can function as a SubState of the modest.modularfilter.ModularFilter class. The functions required for use as a SubState are defined first after __init__, then functions specific to this class are defined next.

The state uses quaternions to store attitude, which avoids issues of gimbal lock and increases numerical stability over other approaches, such as Euler angles. The quaternion itself is not treated as a part of the state vector. Rather, the state vector includes three attitude “error states,” which are updated at each measurement, then used to correct the attitude quaternion. After each correction, the error states are set back to zero.

The algorithms used for the state update mostly come from the book “Fundamentals of Spacecraft Attitude Determination and Control” (FSADC) by Markley and Crassidis. Chapter, section and page numbers will be referenced where appropriate.

Parameters:

• attitudeQuaternion (pyquaternion.Quaternion) – object containing the initial attitude estimate. This variable gets stored as qHat.
• attitudeErrorCovariance (numpy.ndarray) – A 3x3 numpy array containing the covariance of the current attitude estimate. This matrix is used to form the upper diagonal part of PHat.
• gyroBias (numpy.ndarray) – A 3 dimensional numpy array containing the estimate of gyro bias. This array is stored as bHat.
• gyroBiasCovariance (numpy.ndarray) – A 3x3 numpy array containing the estimate of covariance of gyro bias. This array is used to form the lower diagonal part of PHat.

RaDecMeasurementMatrices(source, measurement)

RaDecMeasurementMatrices computes measurement update matrices based which relate the state vector directly to measured angle of arrivals.

There are multiple ways to relate an angle measurement to the attitude state vector. This function computes measurement matricies which relate the attitude state vector directly to the measured angles of arrival, or bearing angles of the measured signal. This measurement matrix is particularly well suited to cases in which the measured quantity is two bearing angles (for instance an azimuth and elevation), while the unit vector measurement update is well suited to vector measurements (for instance tri-axis magnetometer measurements).

The measurement matricies computed by this method are described by the following equations. Given the following measurement vector of azimuth and elevation (\(\alpha\) and \(\beta\)):

\[\begin{split}\mathbf{y} = \begin{bmatrix} \alpha \\ \beta \end{bmatrix}\end{split}\]

The measurement matrices are given by:

\[\begin{split}\mathbf{H} = \begin{bmatrix} \hat{\alpha} & 0 & 1 & 0 & 0 & 0 \\ -\hat{\beta} & 1 & 0 & 0 & 0 & 0 \end{bmatrix}\end{split}\]

\[\begin{split}\boldsymbol{\delta}\mathbf{y} = \begin{bmatrix} \alpha\\ \beta \end{bmatrix} - \mathbf{H} \mathbf{\hat{x}}^{-}\end{split}\]

\[\begin{split}\boldsymbol{R} = \begin{bmatrix} \sigma_\alpha^2 & 0 \\ 0 & \sigma_\beta^2 \\ \end{bmatrix}\end{split}\]

Parameters:

• source – The signal source object associated with the measurement. The measurement may be of any class, but it is generally expected to behave something like a PointSource type object. Specifically, it is expected to have a RaDec() method which returns the right ascension and declination (i.e. bearing angles) to the source.
• measurement (dict) – A dict containing the measurements in the standard measurement format. This function assumes that the dict will contain measurements of bearing angle.

Returns:

A dictionary containing the measurement matrix, \(\mathbf{H}\), the measurement residual vector, \(\boldsymbol{\delta}\mathbf{y}\), and the measurement residual variance matrix, \(\mathbf{S}\)

Return type:

(dict)

RaDecRoll()

RaDecRoll returns the current attitude in terms of right ascension, declination, and roll

This function essentially computes the euler angles (1-2-3) from the current attitude quaternion #qHat and returns them in reverse order (3-2-1). This is helpful because the attitude of spacecraft is commonly expressed in terms of RA-Dec-Roll, rather than roll-pitch-yaw which is more standard in other aerospace applications.

Note

See Wikipedia’s article on the equatorial coordinate system for more details.

Returns:A list containing the three angles Return type:(list)

errorStateTimeUpdateMatrix(myOmega, deltaT)

errorStateTimeUpdateMatrix produces a time-update matrix for the attitude error state

This function the discrete-time error-state transition matrix. This is the matrix which propagates the attitude error state covariance and gyro bias covariance forward in time based on time ellapsed and angular velocity estimate.

The error-state transition matrix is defined as follows:

\[\begin{split}\boldsymbol{\Phi} = \begin{bmatrix} \boldsymbol{\Phi}_{11} & \boldsymbol{\Phi}_{12} \\ \boldsymbol{\Phi}_{21} & \boldsymbol{\Phi}_{22} \\ \end{bmatrix}\end{split}\]

where

\[\boldsymbol{\Phi}_{11} = \mathbf{I}_{3 \times 3} - \left[\mathbf{\hat{\omega}}_k^- \times \right] \frac {\textrm{sin}(||\mathbf{\hat{\omega}}_k^-]|| \Delta t)} {||\mathbf{\hat{\omega}}_k^-||} + \left[\mathbf{\hat{\omega}}_k^+ \times \right]^2 \frac {1 - \textrm{cos}(1 - ||\mathbf{\hat{\omega}}_k^-|| \Delta t)} {||\mathbf{\hat{\omega}}_k^-||^2}\]

\[\boldsymbol{\Phi}_{12} = \left[\mathbf{\hat{\omega}}_k^- \times \right] \frac {1 - \textrm{cos}(1 - ||\mathbf{\hat{\omega}}_k^-|| \Delta t)} {||\mathbf{\hat{\omega}}_k^-||^2} - \mathbf{I}_{3\times 3}\Delta t - \left[\mathbf{\hat{\omega}}_k^- \times \right]^2 \frac {||\mathbf{\hat{\omega}}_k^-|| \Delta t - \textrm{sin}(||\mathbf{\hat{\omega}}_k^-|| \Delta t)} {||\mathbf{\hat{\omega}}_k^-||^3}\]

\[\boldsymbol{\Phi}_{21} = \mathbf{0}_{3 \times 3}\]

\[\boldsymbol{\Phi}_{22} = \mathbf{I}_{3\times3}\]

See Fundamentals of Spacecraft Attitude Determination and Control, Section 6.2.4, page 258, equation 6.83 for more details and derivation.

Parameters:

• myOmega (numpy.ndarray) – The angular velocity estimate used to update the attitude quaternion (\(\mathbf{\hat{\omega}}_k^-\))
• deltaT (float) – The amount of time elapsed for the time-update, used for numerical integration of kinematics equation (\(\Delta T\))

Returns:

The error-state time update matrix, \(\boldsymbol{\Phi}\)

Return type:

(numpy.ndarray)

eulerAngles(t=None)

eulerAngles computes the Euler angles (roll, pitch and yaw) based on the current attitude.

This function computes the Euler angles (or, technically the “Tait-Bryan angles”), i.e. roll, pitch and yaw from the current attitude quaternion #qHat.

Note

See Wikipedia’s article on the equatorial coordinate system for more details.

See also

eulerSTD()

Parameters:t (float) – Optional, get euler angles at a specific time (not yet implemented) Returns:A list containing the three angles Return type:(list)

eulerSTD()

eulerSTD computes the Euler angle (roll, pitch and yaw) standard deviation based on the current covariance matrix.

Note

See Wikipedia’s article on the equatorial coordinate system for more details.

See also

eulerAngles()

Returns:A list containing the three angle standard deviations Return type:(list)

getMeasurementMatrices(measurement, source=None, useUnitVector=None)

getMeasurementMatrices computes and returns measurement update matrices

This function receives a dictionary containing a measurement, along with an object that contains the source model of the measurement. If the source is a Signals.PointSource type signal, then it generates unit-vector attitude measurement type matrices. Otherwise, the function returns dicts populated with None.

Parameters:

• measurement (dict) – A dictionary containing measurement information
• source (`modest.modularfilter.signalsources.signalsource.SignalSource) – The source object that produced the measurement

Returns:

A dictionary containing the measurement matrices H, R, and dY

Return type:

(dict)

See also

getMeasurementMatrices()

Note

This function is one of mandatory functions required for Attitude to function as a sub-state of ModularFilter.

processNoiseMatrix(deltaT, omegaVar, biasVar)

processNoiseMatrix generates a the process noise matrix

This function generates the process noise matrix for time update of attitude error covariance and gyro bias covariance. The process noise matrix is a function propagation time, angular velocity noise, and gyro bias noise. It is defined as follows:

\[\begin{split}\mathbf{Q} = \begin{bmatrix} \left(\sigma_v^2 \Delta T + \frac{1}{3}\sigma_u^2 \Delta T^3\right) \mathbf{I}_{3\times 3} & -\left( \frac{1}{2} \sigma_u^2 \Delta T^2 \right) \mathbf{I}_{3\times 3} \\ -\left( \frac{1}{2} \sigma_u^2 \Delta T^2 \right) \mathbf{I}_{3\times 3}& \left( \sigma_u^2 \Delta T \right) \mathbf{I}_{3\times 3} \end{bmatrix}\end{split}\]

where \(\sigma_v^2\) is the angular velocity noise (i.e. gyro measurement noise) and \(\sigma_u^2\) is the gyro bias process noise.

See Fundamentals of Spacecraft Attitude Determination and Control, Section 6.2.4, page 260, equation 6.93 for derivation and more details.

Parameters:

• deltaT (float) – The amount of time corresponding to the time update (\(\Delta T\))
• omegaVar (float) – The variance of the angular velocity (gyro) measurement (\(\sigma_v^2\))
• biasVar (float) – The variance of the gias bias process noise, indicates how much the gyro bias changes over time (\(\sigma_u^2\))

Returns:

The comibined 6x6 process noise matrix (\(\mathbf{Q}\))

Return type:

(numpy.ndarray)

quaternionTimeUpdateMatrix(myOmega, deltaT)

quaternionTimeUpdateMatrix produces a time-update matrix for the attitude quaternion

This function produces a 4x4 matrix which, when multiplied by an attitude quaternion, rotates the quaternion by an amount corresponding to the angular velocity and time ellapsed. The attitude quaternion is updated as follows:

\[\hat{\mathbf{q}}_k^- \approx{} \bar{\Theta}(\hat{\boldsymbol{\omega}}_j^+, \Delta T) \hat{\boldsymbol{q}}_j^+`\]

where

\[\begin{split}\bar{\Theta}( \hat{\boldsymbol{\omega}}_j^+, \Delta T ) = \begin{bmatrix} \textrm{cos} \left(\frac{1}{2} ||\hat{\boldsymbol{\omega}}_j^+|| \Delta t \right) I_3 - \left[\boldsymbol{\hat{\Psi}}_k^+ \times \right] & \left[\boldsymbol{\hat{\Psi}}_k^+ \times \right] \\ - \left[\boldsymbol{\hat{\Psi}}_k^+ \times \right] & \textrm{cos} \left(\frac{1}{2} ||\mathbf{\hat{\omega}}_j^+|| \Delta t \right) \end{bmatrix}\end{split}\]

and

\[\left[\boldsymbol{\hat{\Psi}}_k^+ \times \right] = \frac{ \textrm{sin}\left(\frac{1}{2} || \mathbf{\hat{\omega}}_j^+ || \Delta t \right) \mathbf{\hat{\omega}}_j^+ }{ || \mathbf{\hat{\omega}}_j^+ || }\]

The matrix returned by this function is \(\bar{\Theta}(\mathbf{\hat{\omega}}_j^+, \Delta T)\).

See Fundamentals of Spacecraft Attitude Determination and Control, Section 6.2.2, page 251, equation 6.60 for more details.

Parameters:

• myOmega (numpy.ndarray) – The angular velocity estimate used to update the attitude quaternion (\(\mathbf{\hat{\omega}}_k^-\))
• deltaT (float) – The amount of time elapsed for the time-update, used for numerical integration of kinematics equation (\(\Delta T\))

Returns:

The quaternion time-update matrix \(\bar{\Theta}(\mathbf{\hat{\omega}}_j^+, \Delta T)\)

Return type:

(numpy.ndarray)

sidUnitVec(RaDec)

sidUnitVec generates a unit vector from two angles

This function computes the unit vector in siderial coordinates from a measurement of right ascension and declination. This is a “helper” function; it doesn’t really need to be included in the class and could be moved to a seperate library probably.

Parameters:RaDec (dict) – A dictionary containing the two angles, labeled “RA” and “DEC”, in radians Returns:A unit vector generated from the angles given Return type:(numpy.ndarray)

skewSymmetric(vector)

skewSymmetric generates a skew-symmetric matrix from a 3x1 vector

This function generates a skew symmetric matrix from a 3x1 vector. It is a “helper” function and doesn’t actually need to be a member function. It could (should?) be moved to its own library.

Parameters:vector (list) – The vector Returns:The skew symmetric matrix Return type:(numpy.ndarray)

storeStateVector(svDict)

storeStateVector is responsible for taking an updated version of the state vector, and storing it in the class variables.

This function is designed to receive a time or measurement updated state vector and covariance, and store it. This function is used by :class:`~modest.modularfilter.ModularFilter~ to store a jointly updated state.

Depending on whether the state vector is the result of a time update (aPriori=True) or a measurement update (aPriori=False), the function may disregard the value of the attitude error state. This is because this class handles the time-update of #qHat internally, so the updated attitude error state is only relevant after a measurement update.

Parameters:svDict (dict) – A dictionary containing the current state vector information, including the state vector, the covariance, the time, and whether the state vector is “a priori” or “a posteriori.”

See also

storeStateVector()

Note

This function is one of mandatory functions required for Attitude to function as a sub-state of ModularFilter.

timeUpdate(dT, dynamics=None)

timeUpdate returns the time-update matrices, and handles the internal time update of the attitude estimate #qHat.

This function generates the time-update matrices F and Q, to be used for a time update, either locally or jointly as part of a State.ModularFilter.

This function looks for angular velocity (omega) and bias variance in the dynamics dict, and uses these to construct the time update matrices. If these are not included in the dynamics dict, then the function assumes these values to be zero.

This function also updates the attitude quaternion internally. It does not update the covariance matrix however; this must be done externally.

Parameters:dynamics (dict) – A dict containing information about the dynamics. Returns:A dict containing the state transition matrix (“F”) and the process noise matrix (“Q”) Return type:(dict)

See also

timeUpdate()

Note

This function is one of mandatory functions required for Attitude to function as a sub-state of ModularFilter.

unitVectorMeasurmentMatrices(source, measurement)

unitVectorMeasurmentMatrices generates measurement matrices for a angle measurement of a point source.

This function generates the set of measurement matrices \(\mathbf{H}\), \(\mathbf{dY}\), and \(\mathbf{R}\) corresponding to an inferred unit vector measurement from a set of two angle measurments (local right ascension and declination of a point source).

The measurement matrices are a function of the measurement itself, the source from which the measurement originated, and the current estimate of attitude, qHat. They are defined as follows:

\[\mathbf{H}_k[\mathbf{\hat{x}}^{-}_k] = A(\mathbf{\hat{q}}_k^- ) = \mathbf{u}^{(s)}_{N} \times\]

\[\mathbf{dY} = \mathbf{y}_{\mathbf{\alpha}} - A(\mathbf{\hat{q}}_k^- ) \mathbf{u}^{(s)}_{N}\]

\[\mathbf{R} = \mathbf{I}_{3\times 3} \sigma^2_{\mathbf{\alpha}}\]

The measured unit vector is a unit vector computed from the measured angles, using sidUnitVec().

See Fundamentals of Spacecraft Attitude Determination and Control, Section 6.2.4, page 257, Table 6.3 for more details and derivation.

Note

Currently, the measurement noise matrix is an identity matrix mutiplied by the angle measurement error. This is my interpretation of Section 6.2.3 in FSADC. However, if we derive the measurement noise matrix using the usual EKF method, we get a different result which results in an unstable estimator. I’m not convinced I understand why this method is right, or why the EKF method is unstable, but it works, so we’re using it for now.

Parameters:

• source – The signal source object associated with the measurement. The measurement may be of any class, but it is generally expected to behave something like a PointSource type object. Specifically, it is expected to have a RaDec() method which returns the right ascension and declination (i.e. bearing angles) to the source.
• measurement (dict) – A dict containing the measurements in the standard measurement format. This function assumes that the dict will contain measurements of bearing angle.

Returns:

A dictionary containing the measurement matrix, \(\mathbf{H}\), the measurement residual vector, \(\boldsymbol{\delta}\mathbf{y}\), and the measurement residual variance matrix, \(\mathbf{S}\)

Return type:

(dict)

PHat = None

(covarianceContainer) Current joint covariance matrix.

Upper 3x3 diagonal contains covariance of the attitude estimate (related to qHat), while lower 3x3 diagonal contains the covariance of the gyro bias bHat.

bHat = None

(numpy.ndarray) Current estimate of gyro bias

lastMeasID = None

Last measurement used to generate measurement matrices

lastMeasMat = None

Last set of measurement matrices This allows class to avoid redundant computation of the same set of measurement matrices.

lastSourceID = None

Last signal used to generate measurement matrices

qHat = None

Current estimate of attitude, stored as a Quaternion object.

Mathematically generally referred to as \(\mathbf{\hat{q}}^{-}_{k}\) for the a priori value, or \(\mathbf{\hat{q}}^{+}_{k}\) for the a posteriori

Type:(pyquaternion.Quaternion)

useUnitVector = None

(bool) Determines whether the unit vector is used for the measurement matrix, or the right ascension declination measurement model

modest.substates.correlationvector module

class modest.substates.correlationvector.CorrelationVector(trueSignal, filterOrder, dT, t=0, correlationVector=None, correlationVectorCovariance=None, signalTDOA=0, TDOAVar=0, aPriori=True, centerPeak=True, peakFitPoints=1, processNoise=1e-12, measurementNoiseScaleFactor=1, peakLockThreshold=1, covarianceStorage='covariance', internalNavFilter=None, navProcessNoise=1, tdoaStdDevThreshold=None, velStdDevThreshold=None, tdoaNoiseScaleFactor=None, velocityNoiseScaleFactor=None, storeLastStateVectors=0, vInitial=None, aInitial=None, gradInitial=None, peakEstimator='EK')

Bases: modest.substates.substate.SubState

buildDeepTimeUpdateMatrices(dT, dynamics, h)

buildFLMatrices(peakShift, h)

buildTimeUpdateMatrices(deltaT, dynamics, h)

computeSignalTDOA(c, P)

estimateSignalTDOA_EK(h, P)

estimateSignalTDOA_UT(h, P, useMean=True)

getMeasurementMatrices(measurement, source=None)

getMeasurementMatrices returns matrices needed to perform a measurement update

The getMeasurementMatrices() method is responsible for returning the EKF measurement update matrices. Specifically, it returns the measurement matrix \(\mathbf{H}\), the measurement noise matrix \(\mathbf{R}\), and the measurement residual vector \(\mathbf{\delta}\mathbf{y}\), folloing the standard Extended Kalman Filter measurement update equations:

\[\mathbf{\delta y} = \mathbf{y} - h(\mathbf{x}_k^-, \mathbf{w}_k)\]

\[\mathbf{H}_k^-= \frac{\partial h}{ \partial \mathbf{x}}\]

\[\mathbf{S}_k^- = \mathbf{H}_k \mathbf{P}_k^- \mathbf{H}_k^T + \mathbf{R}\]

\[\mathbf{K}_k^- = \mathbf{P}_k^- \mathbf{H}_k^T \mathbf{S}_k^{-1}\]

Because these matrices are nescessarily specific to the type of substate being updated, there is no default implementation in the SubState class. Rather, each derived class must implement this method as appropriate for the measurement of the state being modeled. Additionally, the measurement update may be different depending on which signal source is generating the measurement, so the substate must have measurement matrix generation implemented for all the expected signal sources.

In addition, some substates may require additional operations to occur at a measurement update. For instance, if a substate includes auxillary values (for instance, the attitude quaternion derived from the attitude error state), it may need to be updated seperately after the global state vector has been updated. In this case, the local implementation of the timeUpdate() function is the place to do these updates.

Note that there is no time associated with the measurement; the filter assumes that the measurement is occuring at the current time. Therefore it is the user’s responsibility to time-update the state to the current time before doing the measurement update.

Parameters:

• measurement (dict) – A dictionary containing the measurement value(s)
• source (str) – A key uniquely identifying the source of origin of the measurement

Returns:

(dict) A dictionary containing, at minimum, the following items:

• ”F”: The state time-update matrix
• ”Q”: The process noise matrix

getTOAMeasurementMatrices(measurement, corrVec)

initializeRealTimePlot(plotHandle=None, axisHandle=None)

static peakFinder(x, y)

static peakFinderJacobian(x, y)

static quadraticFit(x, y)

realTimePlot(normalized=True)

static sincDiff(x)

speedOfLight()

storeStateVector(svDict)

storeStateVector stores the most recent value of the state vector.

The storeStateVector method is responsible for storing a dictionary containing the most recent state estimate. In SubState implementation, the functionality is minimal: the new dictionary is simply appeneded to the list of state vector estimates. However, in some derived classes, it may be nescessary to implement additional functionality. This is particularly true if there are derived quantities that need to be calculated from the updated state vector (for instance, calculating the attitude quaternion from the attitude error states). Also in some cases, the actual value of the state vector may need to be “tweaked” by the SubState derived class.

If an alternative implementation is written for a derived class, it should still call this implementation, or at least make sure that it stores the current state estimate in stateVectorHistory.

Parameters:svDict (dict) – A dictionary containing the current state estimate.

timeUpdate(dT, dynamics=None)

timeUpdate returns time-update matrices

The timeUpdate() method is responsible for returning the EKF time update matrices. Specifically, it returns the state update matrix \(\mathbf{F}\) and the process noise matrix \(\mathbf{Q}\), following the standard Extended Kalman Filter time update equations:

\[\mathbf{x}_k^- = \mathbf{F}\mathbf{x}_j^+\]

\[\mathbf{P}_k^- = \mathbf{F} \mathbf{P}_k^- \mathbf{F}^T + \mathbf{Q}\]

Because these matrices are nescessarily specific to the type of substate being updated, there is no default implementation in the SubState class. Rather, each derived class must implement this method as appropriate for the dynamics of the state being modeled.

In addition, some substates may require additional operations to occur at a time update. For instance, if a substate includes auxillary values (for instance, the attitude quaternion derived from the attitude error state), it may need to be time-updated seperately from the other states. In this case, the local implementation of the #timeUpdate function is the place to do these updates.

Parameters:

• dT (float) – The ellapsed time over which the time update occurs
• dynamics (dict) – A dictionary containing any dynamics infomation which may be needed to update the state, for instance, measured accelerations or angular velocities.

Returns:

(dict) A dictionary containing, at minimum, the following items:

• ”F”: The state time-update matrix
• ”Q”: The process noise matrix

navProcessNoise = None

This is the default process noise that is injected into the navigation states as noise in the derivative of the highest order state.

peakEstimator = None

String that determines which algorithm is used to estimate peak. Use either EK (extended Kalman Filter) or UK (Unscented)

modest.substates.oneDimensionalPositionVelocity module

class modest.substates.oneDimensionalPositionVelocity.oneDPositionVelocity(objectID, stateVectorHistory, covarianceStorage='covariance', biasState=True, artificialBiasMeas=True, biasStateTimeConstant=0.9, biasStateProcessNoiseVar=0.001, biasMeasVar=1, storeLastStateVectors=0)

Bases: modest.substates.substate.SubState

getMeasurementMatrices(measurement, source=None)

getMeasurementMatrices returns matrices needed to perform a measurement update

The getMeasurementMatrices() method is responsible for returning the EKF measurement update matrices. Specifically, it returns the measurement matrix \(\mathbf{H}\), the measurement noise matrix \(\mathbf{R}\), and the measurement residual vector \(\mathbf{\delta}\mathbf{y}\), folloing the standard Extended Kalman Filter measurement update equations:

\[\mathbf{\delta y} = \mathbf{y} - h(\mathbf{x}_k^-, \mathbf{w}_k)\]

\[\mathbf{H}_k^-= \frac{\partial h}{ \partial \mathbf{x}}\]

\[\mathbf{S}_k^- = \mathbf{H}_k \mathbf{P}_k^- \mathbf{H}_k^T + \mathbf{R}\]

\[\mathbf{K}_k^- = \mathbf{P}_k^- \mathbf{H}_k^T \mathbf{S}_k^{-1}\]

Because these matrices are nescessarily specific to the type of substate being updated, there is no default implementation in the SubState class. Rather, each derived class must implement this method as appropriate for the measurement of the state being modeled. Additionally, the measurement update may be different depending on which signal source is generating the measurement, so the substate must have measurement matrix generation implemented for all the expected signal sources.

In addition, some substates may require additional operations to occur at a measurement update. For instance, if a substate includes auxillary values (for instance, the attitude quaternion derived from the attitude error state), it may need to be updated seperately after the global state vector has been updated. In this case, the local implementation of the timeUpdate() function is the place to do these updates.

Note that there is no time associated with the measurement; the filter assumes that the measurement is occuring at the current time. Therefore it is the user’s responsibility to time-update the state to the current time before doing the measurement update.

Parameters:

• measurement (dict) – A dictionary containing the measurement value(s)
• source (str) – A key uniquely identifying the source of origin of the measurement

Returns:

(dict) A dictionary containing, at minimum, the following items:

• ”F”: The state time-update matrix
• ”Q”: The process noise matrix

storeStateVector(svDict)

storeStateVector stores the most recent value of the state vector.

The storeStateVector method is responsible for storing a dictionary containing the most recent state estimate. In SubState implementation, the functionality is minimal: the new dictionary is simply appeneded to the list of state vector estimates. However, in some derived classes, it may be nescessary to implement additional functionality. This is particularly true if there are derived quantities that need to be calculated from the updated state vector (for instance, calculating the attitude quaternion from the attitude error states). Also in some cases, the actual value of the state vector may need to be “tweaked” by the SubState derived class.

If an alternative implementation is written for a derived class, it should still call this implementation, or at least make sure that it stores the current state estimate in stateVectorHistory.

Parameters:svDict (dict) – A dictionary containing the current state estimate.

timeUpdate(dT, dynamics=None)

timeUpdate returns time-update matrices

The timeUpdate() method is responsible for returning the EKF time update matrices. Specifically, it returns the state update matrix \(\mathbf{F}\) and the process noise matrix \(\mathbf{Q}\), following the standard Extended Kalman Filter time update equations:

\[\mathbf{x}_k^- = \mathbf{F}\mathbf{x}_j^+\]

\[\mathbf{P}_k^- = \mathbf{F} \mathbf{P}_k^- \mathbf{F}^T + \mathbf{Q}\]

Because these matrices are nescessarily specific to the type of substate being updated, there is no default implementation in the SubState class. Rather, each derived class must implement this method as appropriate for the dynamics of the state being modeled.

In addition, some substates may require additional operations to occur at a time update. For instance, if a substate includes auxillary values (for instance, the attitude quaternion derived from the attitude error state), it may need to be time-updated seperately from the other states. In this case, the local implementation of the #timeUpdate function is the place to do these updates.

Parameters:

• dT (float) – The ellapsed time over which the time update occurs
• dynamics (dict) – A dictionary containing any dynamics infomation which may be needed to update the state, for instance, measured accelerations or angular velocities.

Returns:

(dict) A dictionary containing, at minimum, the following items:

• ”F”: The state time-update matrix
• ”Q”: The process noise matrix

modest.substates.substate module

class modest.substates.substate.SubState(stateDimension=None, stateVectorHistory=None, storeLastStateVectors=0, objectID='')

Bases: object

This is an abstract base class for objects used as sub-states in State.ModularFilter.

SubState is an abstract base class that specifies the methods which are required for an object to function as a sub-state of State.ModularFilter.

Some of these methods are implemented and most likely do not need to be reimplemented in a derived class implementation (for example the #dimension and #covariance methods.

Other methods may have a rudimentary implementation that may be suitable for some derived classes, but not others, depending on the specific functionality of the derived class (for instance #getStateVector and #storeStateVector).

Finally, some methods are specifically tagged as abstract methods and are not implemented at all. These methods must be implemented in the derived class. This is usually because there is no way to implement even a rudimentary version of what the method is supposed to do without having some knowledge of what kind of substate the derived class contains (for instance timeUpdate() and getMeasurementMatrices()).

In any case, the documentation for each method of SubState contains a generalized description of what functionality the implementation should provide in a derived class.

covariance()

covariance() returns the SubState covariance matrix

The covariance method returns the covariance of the estimate of the substate.

todo: Currently, this method only returns the covariance of the most recent state estimate. Ideally, there should be an optional time parameter which would allow the user to get the covaraince matrix at a specified time (or the closest to that specified time).

Returns:Returns the covaraince matrix Return type:covarianceContainer

dimension()

dimension returns the dimension of the sub-state vector

The dimension method returns the dimension of the sub-state vector estimated by the SubState. This is the dimension as seen by the ModularFilter estimator.

The default implementation is to return the class variable __dimension__, which is saved at initialization. This is designated as a “protected” variable, and should not change during the course of the SubState’s lifetime. If child class overwrites this implementation, care should be taken to ensure that the value returned by #dimension does not change over SubState object lifetime.

For SubState objects with auxilary states, or other quantities related to the state vector but not directly estimated by the ModularFilter, #dimension should not count these states as part of the total dimension.

Returns:The dimension of state vector Return type:int

getMeasurementMatrices(measurement, source=None)

getMeasurementMatrices returns matrices needed to perform a measurement update

The getMeasurementMatrices() method is responsible for returning the EKF measurement update matrices. Specifically, it returns the measurement matrix \(\mathbf{H}\), the measurement noise matrix \(\mathbf{R}\), and the measurement residual vector \(\mathbf{\delta}\mathbf{y}\), folloing the standard Extended Kalman Filter measurement update equations:

\[\mathbf{\delta y} = \mathbf{y} - h(\mathbf{x}_k^-, \mathbf{w}_k)\]

\[\mathbf{H}_k^-= \frac{\partial h}{ \partial \mathbf{x}}\]

\[\mathbf{S}_k^- = \mathbf{H}_k \mathbf{P}_k^- \mathbf{H}_k^T + \mathbf{R}\]

\[\mathbf{K}_k^- = \mathbf{P}_k^- \mathbf{H}_k^T \mathbf{S}_k^{-1}\]

Because these matrices are nescessarily specific to the type of substate being updated, there is no default implementation in the SubState class. Rather, each derived class must implement this method as appropriate for the measurement of the state being modeled. Additionally, the measurement update may be different depending on which signal source is generating the measurement, so the substate must have measurement matrix generation implemented for all the expected signal sources.

In addition, some substates may require additional operations to occur at a measurement update. For instance, if a substate includes auxillary values (for instance, the attitude quaternion derived from the attitude error state), it may need to be updated seperately after the global state vector has been updated. In this case, the local implementation of the timeUpdate() function is the place to do these updates.

Note that there is no time associated with the measurement; the filter assumes that the measurement is occuring at the current time. Therefore it is the user’s responsibility to time-update the state to the current time before doing the measurement update.

Parameters:

• measurement (dict) – A dictionary containing the measurement value(s)
• source (str) – A key uniquely identifying the source of origin of the measurement

Returns:

(dict) A dictionary containing, at minimum, the following items:

• ”F”: The state time-update matrix
• ”Q”: The process noise matrix

getStateVector(t=None)

getStateVector returns the most recent value of the state vector

The getStateVector method is responsible for returning a dictionary object containing, at minimim, the following items:

• ‘stateVector’: A length dimension array containing the most recent state vector estimate
• ‘covariance’: A (dimension x dimension) array containing the most recent covariance matrix
• ‘aPriori’: A boolean indicating if the most recent estimate is the
• result of a time update (aPriori=True) or a measurement update (aPriori=False)

This function can be used as-is, or can be overloaded to perform additional tasks specific to the substate.

Parameters:t (int) – This is an optional argument if the user wants a state vector from a time other than the current time. Returns:A dictionary containing the state vector, covariance matrix, and aPriori status Return type:dict

initializeRealTimePlot(plotHandle=None, axisHandle=None)

initializeTimeHistoryPlot(plotHandle=None)

objectID()

realTimePlot(normalized=True, substateRange=None)

storeStateVector(svDict)

storeStateVector stores the most recent value of the state vector.

The storeStateVector method is responsible for storing a dictionary containing the most recent state estimate. In SubState implementation, the functionality is minimal: the new dictionary is simply appeneded to the list of state vector estimates. However, in some derived classes, it may be nescessary to implement additional functionality. This is particularly true if there are derived quantities that need to be calculated from the updated state vector (for instance, calculating the attitude quaternion from the attitude error states). Also in some cases, the actual value of the state vector may need to be “tweaked” by the SubState derived class.

If an alternative implementation is written for a derived class, it should still call this implementation, or at least make sure that it stores the current state estimate in stateVectorHistory.

Parameters:svDict (dict) – A dictionary containing the current state estimate.

timeHistoryPlot()

timeUpdate(dT, dynamics=None)

timeUpdate returns time-update matrices

The timeUpdate() method is responsible for returning the EKF time update matrices. Specifically, it returns the state update matrix \(\mathbf{F}\) and the process noise matrix \(\mathbf{Q}\), following the standard Extended Kalman Filter time update equations:

\[\mathbf{x}_k^- = \mathbf{F}\mathbf{x}_j^+\]

\[\mathbf{P}_k^- = \mathbf{F} \mathbf{P}_k^- \mathbf{F}^T + \mathbf{Q}\]

Because these matrices are nescessarily specific to the type of substate being updated, there is no default implementation in the SubState class. Rather, each derived class must implement this method as appropriate for the dynamics of the state being modeled.

In addition, some substates may require additional operations to occur at a time update. For instance, if a substate includes auxillary values (for instance, the attitude quaternion derived from the attitude error state), it may need to be time-updated seperately from the other states. In this case, the local implementation of the #timeUpdate function is the place to do these updates.

Parameters:

• dT (float) – The ellapsed time over which the time update occurs
• dynamics (dict) – A dictionary containing any dynamics infomation which may be needed to update the state, for instance, measured accelerations or angular velocities.

Returns:

(dict) A dictionary containing, at minimum, the following items:

• ”F”: The state time-update matrix
• ”Q”: The process noise matrix

nextSubstateObjectID = 0

storeLastStateVectors = None

(int) Determines how far back the state vector history may go. If zero, then the entire state vector history is stored

Module contents