6.4. Filters¶

Filters, in astrodynamics, are generally used to estimate the state of a system, based on a model description and measurement data. Tudat provides a few filters, based on the Kalman filter concept, that you can use for these or any other purpose you see fit. The filtering techinques currently offered in Tudat are:

Linear Kalman Filter (LKF): an extremely effective and versatile procedure for combining noisy sensor outputs to estimate the state of a linear system with uncertain dynamics

Extended Kalman Filter (EKF): extends the LKF to non-linear applications, by using a first-order Taylor series approximation of the system around the current estimate

Unscented Kalman Filter (UKF): similar to the EKF, but addresses the non-linearity by using a deterministic sampling approach (instead of using system and measurement Jacobians)

You can find more information on the mathematical background of these filters in virtually any guidance, navigation and control book. However, the references used to define the filters in Tudat are:

LKF: E. Mooij, AE4870B - Re-entry Systems, Lecture Notes, Delft University of Technology, 2016.

EKF: Ogata, K., Discrete-Time Control Systems, 2nd ed. Pearson Education Asia, 2002.

UKF: Wan, E. and Van Der Merwe, R., “The Unscented Kalman Filter for Nonlinear Estimation,” in Adaptive Systems for Signal Processing, Communications, and Control Symposium. Institute of Electrical and Electronics Engineers, 2000, pp. 153–158.

On this page, you will find a description of how to create a Kalman filter object, either based on the linear, extended, or unscented principle. In general, you will notice that the creation of a filter, requires the input of two template arguments. These are called IndependentVariableType and DependentVariableType, and they respectively denote the type (i.e., double, long double, etc.) for the independent variable (usually time) and the dependent variables. Note that the dependent variable is internally defined as an Eigen::Matrix< DependentVariableType, Eigen::Dynamic, 1 > object (which corresponds to a vector of unspecified length).

6.4.1. Linear Kalman Filter¶

class LinearKalmanFilter¶

std::shared_ptr< LinearKalmanFilter< IndependentVariableType, DependentVariableType > > linearFilter =
   std::make_shared< LinearKalmanFilter< IndependentVariableType, DependentVariableType > >(
               stateTransitionMatrix,
               controlMatrix,
               measurementMatrix,
               systemUncertainty,
               measurementUncertainty,
               filteringTimeStep,
               initialTime,
               initialEstimatedStateVector,
               initialEstimatedStateCovarianceMatrix );

In the code above, the inputs are given by:

stateTransitionMatrixFunction: a function returning the state transition matrix, as a function of time, state and control input

controlMatrixFunction: a function returning the control matrix as a function of time, state and control input

measurementMatrixFunction: a function returning the measurement matrix as a function of time, state and control input

systemUncertainty: a matrix defining the uncertainty in modeling of the system

measurementUncertainty: a matrix defining the uncertainty in modeling of the measurements

filteringTimeStep: a scalar representing the value of the constant filtering time step

initialTime: a scalar representing the value of the initial time

initialStateVector: a vector representing the initial (estimated) state of the system; it is used as first a-priori estimate of the state vector

initialCovarianceMatrix: a matrix representing the initial (estimated) covariance of the system; it is used as first a-priori estimate of the covariance matrix

integrator: a pointer to integrator to be used to propagate state

Note

Currently, the creation of the LKF is only supported in the method above. A class such as LinearKalmanFilterSettings has not yet been implemented.

6.4.2. Extended Kalman Filter¶

class ExtendedKalmanFilter¶

std::shared_ptr< FilterBase< IndependentVariableType, DependentVariableType > > filterObject;
filterObject = createFilter( filterSettings,
                             systemFunction,
                             measurementFunction,
                             stateJacobianFunction,
                             stateNoiseJacobianFunction,
                             measurementJacobianFunction,
                             measurementNoiseJacobianFunction );

Here, the inputs are as follows:

filterSettings: a pointer to a ExtendedKalmanFilterSettings object, described at the end of this section

systemFunction: a function returning the state as a function of time and state vector; this can be a differential equation if the integratorSettings is set (i.e., if it is not a nullptr)

measurementFunction: a function returning the measurement as a function of time and state

stateJacobianFunction: a function returning the Jacobian of the system w.r.t. the state; its input values can be time and state vector

stateNoiseJacobianFunction: a function returning the Jacobian of the system function w.r.t. the system noise; its input values can be time and state vector

measurementJacobianFunction: a function returning the Jacobian of the measurement function w.r.t. the state; its input values can be time and state vector

measurementNoiseJacobianFunction: a function returning the Jacobian of the measurement function w.r.t. the measurement noise. The input values can be time and state vector

The filter settings above, can be defined for an extended Kalman filter as:

class ExtendedKalmanFilterSettings¶

std::shared_ptr< FilterSettings< IndependentVariableType, DependentVariableType > > filterSettings =
   std::make_shared< ExtendedKalmanFilterSettings< IndependentVariableType, DependentVariableType > >(
               systemUncertainty,
               measurementUncertainty,
               filteringTimeStep,
               initialTime,
               initialStateVector,
               initialCovarianceMatrix,
               integratorSettings );

where each element is defined as:

systemUncertainty: a matrix defining the uncertainty in modeling of the system

measurementUncertainty: a matrix defining the uncertainty in modeling of the measurements

filteringTimeStep: a scalar representing the value of the constant filtering time step

initialTime: a scalar representing the value of the initial time

initialStateVector: a vector representing the initial (estimated) state of the system; it is used as first a-priori estimate of the state vector

initialCovarianceMatrix: a matrix representing the initial (estimated) covariance of the system; it is used as first a-priori estimate of the covariance matrix

integratorSettings: a pointer to integration settings defining the integrator to be used to propagate the state

6.4.3. Unscented Kalman Filter¶

class UnscentedKalmanFilter¶

std::shared_ptr< FilterBase< IndependentVariableType, DependentVariableType > > filterObject;
filterObject = createFilter( filterSettings,
                             systemFunction,
                             measurementFunction )

Here, the inputs are as follows:

filterSettings: a pointer to a UnscentedKalmanFilterSettings object, described at the end of this section

systemFunction: a function returning the state as a function of time and state vector; this can be a differential equation if the integratorSettings is set (i.e., if it is not a nullptr)

measurementFunction: a function returning the measurement as a function of time and state

The filter settings above, can be defined for an extended Kalman filter as:

class UnscentedKalmanFilterSettings¶

std::shared_ptr< FilterSettings< IndependentVariableType, DependentVariableType > > filterSettings =
   std::make_shared< UnscentedKalmanFilterSettings< IndependentVariableType, DependentVariableType > >(
               systemUncertainty,
               measurementUncertainty,
               filteringTimeStep,
               initialTime,
               initialStateVector,
               initialCovarianceMatrix,
               integratorSettings,
               constantValueReference,
               customConstantParameters );

where each element is defined as:

systemUncertainty: a matrix defining the uncertainty in modeling of the system

measurementUncertainty: a matrix defining the uncertainty in modeling of the measurements

filteringTimeStep: a scalar representing the value of the constant filtering time step

initialTime: a scalar representing the value of the initial time

initialStateVector: a vector representing the initial (estimated) state of the system; it is used as first a-priori estimate of the state vector

initialCovarianceMatrix: a matrix representing the initial (estimated) covariance of the system; it is used as first a-priori estimate of the covariance matrix

integratorSettings: a pointer to integration settings defining the integrator to be used to propagate the state

constantValueReference: reference to be used for the values of the \(\alpha\) and \(\kappa\) parameters; this variable has to be part of the ConstantParameterReferences enumeration (custom parameters are supported)

customConstantParameters: values of the constant parameters \(\alpha\) and \(\kappa\), input as a pair, in case the custom_parameters enumeration is used in the previous field

The supported values for constantValueReference are based on UKF literature, and correspond to the values of \(\alpha\) and \(\kappa\) shown in the table below (where \(N\) denotes the length of the state vector). Note that the values of these two parameters have the following meaning:

\(\alpha\) is used to distribute the sigma points around the a-priori estimate

\(\kappa\) is a secondary scaling parameter, also used to distribute the sigma points around the a-priori estimate, but it has a smaller influence

`constantValueReference`	\(\alpha\)	\(\kappa\)
`reference_Wan_and_Van_der_Merwe` (default)	0.003	0.0
`reference_Lisano_and_Born_and_Axelrad`	1.0	3.0 - \(N\)
`reference_Challa_and_Moore_and_Rogers`	0.001	0.1
`customConstantParameters`	–	–

6.4.4. Using a Kalman Filter Object¶

To use the filters after their creation, you will need to update the system with the data corresponding to the new time step. This can be done by using the function updateFilter, which takes one inputs:

currentMeasurementVector: a vector denoting the current external measurement which will be used to correct the estimated a-priori state (thus to obtain the a-posteriori estimate)

Note that the filter object comes equipped with two noise generators. These produce random Gaussian noise based on the system and measuremenet uncertainty properties input by you. They can be retrieved with the commands produceSystemNoise and produceMeasurementNoise. You will find cases of how to use the noise and the other features mentioned on this page, in:

/Tudat/Mathematics/Filters/UnitTests

where a few examples for each filtering technique are shown. One of the test cases shown for the extended (EKF) and unscented (UKF) filters is also shown in an example application, which you can find in Kalman Filter for State Estimation.

The last element that should be discussed is the control system. It may be the case that you also need a control input together with the time and state. This can be added with the creation of a ControlWrapper of the type explained in the next section, Adding a Control Input.

These are the only major steps that you will need to take to keep the filter running. At the end of the estimation process, however, you can retrieve, however, retireve the history of the estimated states and covariance matrices by using the functions getEstimatedStateHistory and getEstimatedCovarianceHistory, respectively.

6.4.5. Adding a Control Input¶

It may be that for your application, your system needs to be controlled. This can be achieved by creating a control system (ControlSystem, or ControlWrapper in the unit tests), that provides the state function (and possibly the state Jacobian) with the current commanded vector.

The way this is done is by adding an extra input to the state function, i.e.,

Eigen::Vector3d stateFunction( const double currentTime, const Eigen::Vector3d& currentState, const Eigen::Vector3d& currentControl )

The last input of the stateFunction is another vector which denotes the commanded control vector. Note how this function, however is incompatible with the definition of systemFunction, i.e., the input to the ExtendedKalmanFilter and UnscentedKalmanFilter classes. In fact, this should be of type

std::function< DependentVector( const IndependentVariableType, const DependentVector& ) >

which one has two inputs (a double and an Eigen::Vector3d, in our case). The extra vector can be added by using the very handy std::bind command. This function allows us to bind the output of a function as an input to another function. Thus, by using the control class mentioned above, one can replace the systemFunction input with:

std::bind( &stateFunction, std::placeholders::_1, std::placeholders::_2, std::bind( &ControlSystem::getCurrentStateVector, controlSystem ) )

where the controlSystem object is of type ControlSystem. Using this method, if the control system is regularly updated (possibly every time step), the control vector will be automatically retrieved and parsed by the filter.

In Kalman Filter for State Estimation you will find an example where a control system is added to both an EKF and a UKF, which are used to estimate the state of a simple estimation application.

Tip

In general, you can apply the principle above to feed any other variable or object to the state and/or measurement functions.