File ekf.hpp

Implementation of the Extended Kalman Filter (EKF).

License:

BSD 3-clause

Copyright

Copyright (c) 2020, New York University and Max Planck Gesellschaft

namespace mim_estimation

namespace standard_filters

template<class S, class M>
class EKF

#include <ekf.hpp>

This class implements an extended Kalman Filter (EKF).

This class implements a templated Extended Kalman Filter that receives a State class “S” and a measurement class “M”. Both templated class must have this following operator: -, +, =, <<. This operator must be eigen compatible. To achieve this in a compact way I strongly suggest inheriting directly from the class Eigen::Vector<N> (see Eigen documentation https://eigen.tuxfamily.org/dox/TopicCustomizing_InheritingMatrix.html).

The main sources of reading comes from

• [1] “S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics, 2000” (Chapter 3)

• [2] “http://wolfweb.unr.edu/~fadali/EE782/NumEvalQphi.pdf”

• [3] “https://en.wikipedia.org/wiki/Extended_Kalman_filter”

The notation used in this documentation is the one from “http://wolfweb.unr.edu/~fadali/EE782/NumEvalQphi.pdf”.

\[\begin{split}\begin{align} x_t = g(u_t, x_{t-1}) + \epsilon_t \\ z_t = h(x_t) + \delta_t \end{align}\end{split}\]

With \( x_t \) the system state, \( u_t \) the system control, \( g \) the process evolution, \( \epsilon_t \approx N(0, R_t) \) the process Gaussian noise, \( h \) the measurement evolution and \( \delta_t \approx N(0, Q_t) \) the measurement Gaussian noise. The first equation depicts the “process” in the code. The second equation represents the “measurement” in the code.

Using a taylor expansion of the previous equation we get:

\[\begin{split}\begin{align} x_t = g(u_t, \mu_{t-1}) + G_t \; * \; (x_{t-1} - \mu_{t-1}) + \epsilon_t \\ z_t = h(\mu_t) + H_t \; * \; (x_{t} - \mu_{t}) + \delta_t \end{align}\end{split}\]

With \( \mu_{t} \) the average of the estimated state of covariance \( \Sigma_t \). The equation of the EKF routine are defined with the “update_filter” function.

The mapping between the variable names and this piece of documentation is done next to each variable.

Public Functions

EKF(bool numerical_jac, bool is_discrete, double dt, unsigned lin_order = 2)

EKF, constructor of the class.

Parameters:

• numerical_jac – do we compute the Jacobians analytically or not?

• is_discrete – is the process discrete?

• dt – the sampling period of the EKF.

inline ~EKF()

Default destructor.

void updateFilter(bool update_on)

updateFilter: compute a pass of the extended Kalman filter

From here on we only have access to \( \mu_{t-1} \) and \( \Sigma_{t-1} \) the previous estimated state along time. First of all we need to check if the system is a discrete one (see constructor). If the system is continuous we need to build a discrete approximation of it. In order to do so we use the Van Loan’s method (see [2] slide 15).

• 1/ Compute the dynamic matrix of the continuous process \( M \)

  \[\begin{align} M = \left[ \left. \frac{-G_t}{0} \right| \frac{E_t R_t E_t^T}{G_t^T} \right] \end{align}\]
  With \( E_t \) being the jacobian of the noise. In [2], \( E_t \) is also considered as the control jacobian in a way.

• 2/ Obtain the matrix M exponential

  \[\begin{align} e^{M \Delta t} = \left[ \left. \frac{ e^{-G_t \Delta t} }{0} \right| \frac{ e^{-G_t \Delta t} F_{t, disc} }{ e^{G_t^T \Delta t} } \right] \end{align}\]

• 3/ Transpose the lower right corner to compute the discretized process noise covariance.

  \[\begin{align} R_{t, disc} = e^{G_t \Delta t} = (e^{G_t^T \Delta t})^T \end{align}\]

• 4/ Compute the discretized process jacobian as follow:

  \[\begin{align} F_{t, disc} = R_{t, disc} \;\;\; e^{M \Delta t}[\text{upper right corner}] \end{align}\]
  In the following we will discard the \( {disc} \) subscript in the expression of the process Jacobian and noise covariance for sake of clarity. We assume that the system is either discrete or already discretized.

When the system has been discretized (or is discrete) we compute the EKF with the following procedure. First we need to predict what is supposed to be the current estimated state \( \tilde{\mu}_{t} \) and its Covariance \( \tilde{\Sigma}_{t} \) according to the model and the current control:

\[\begin{split}\begin{align} \tilde{\mu}_{t} &= g(u_t, \mu_{t-1}) \\ \tilde{\Sigma}_{t} &= G_t \Sigma_{t-1} G_t^T + R_t \end{align}\end{split}\]

The second step consist in introducing the actual measurement when one occur.

\[\begin{split}\begin{align} K_{t} &= \tilde{\Sigma}_{t} H_t^T (H_t \tilde{\Sigma}_t H_t^T + Q_t)^{-1} \\ \mu_{t} &= \tilde{\mu}_{t} + K_t (z_t - h(\tilde{\mu}_t)) \\ \Sigma_{t} &= (I - K_t H_t) \tilde{\Sigma}_{t} \end{align}\end{split}\]

Parameters:

update_on – is the measurement new?

virtual Eigen::Matrix<double, N, 1> getFilterState(void) = 0

getFilterState

Returns:

the current state estimate \( \mu_{t} \)

void printDebug(void)

printDebug, print all the protected method of the class for debugging purposes

Public Static Attributes

static const int N = S::state_dim

State dimension

static const int P = S::noise_dim

Noise dimension

static const int K = M::meas_dim

Measurement dimension

Protected Functions

virtual Eigen::Matrix<double, N, 1> processModel(S &s) = 0

processModel, a model of the continuous/discrete state dynamics \( g(u_t, x_{t-1}) \)

Parameters:

s – \( [x_{t-1}^T \;\; u_t^T]^T \)

Returns:

the predicted state (Max: should not return anything and update state_pre_?)

virtual void formProcessJacobian(void) = 0

formProcessJacobian, compute the Jacobian of the process \( G_t \)

virtual void formProcessNoise(void) = 0

formProcessNoise, compute the covariance of the process noise \( R_t \)

virtual void formNoiseJacobian(void) = 0

formNoiseJacobian, compute the Jacobian of the process noise \( E_t \)

virtual Eigen::Matrix<double, K, 1> measModel(S &s) = 0

measModel, a model of the measurement dynamics \( h(x_t) \)

Parameters:

s – a state \( x_t \)

Returns:

the predicted measurement (Max: should not return anything and update meas_pred_?)

virtual void formMeasJacobian(void) = 0

formMeasJacobian, compute the Jacobian of the measurement \( H_t \)

virtual void formMeasNoise(void) = 0

formMeasJacobian, compute the measure covariance \( Q_t \)

virtual void formActualMeas(void) = 0

formMeasJacobian, compute the measurement from sensors \( z_t \)

void computeJacobians(Eigen::MatrixXd &G_t, Eigen::MatrixXd &H_t, double ds)

computeJacobians, compute the finite differentiation

Parameters:

• G_t – [inout] the process jacobian to compute

• H_t – [inout] the measurement jacobian

• ds – [inout] the finite differentiation delta \( \frac{f(x+ds) - f(x-ds)}{2ds} \)

bool check_jacobians(bool evaluate)

check_the_jacobians, this function computes the jacobian from the discrete model and from the continuous jacobian discretized and compare them

Parameters:

evaluate – [in] true, the function is computing the check/false it does nothing

Returns:

a boolean that says if everything is alright. for Van Loan discretization, \( M \)

Protected Attributes

S state_pre_

Predicted state ( \( \tilde{\mu} \), \( \tilde{\Sigma} \))

S state_post_

State of the filter. One variable is used for the previous ( \( \mu_{t-1} \), \( \Sigma_{t-1} \)) and current state ( \( \mu_{t} \), \( \Sigma_{t} \))

M meas_actual_

Current measurement ( \( z_{t} \))

M meas_pred_

Predicted measurement ( \( h(\tilde{\mu}_t)\) )

Eigen::MatrixXd VL_mat_

for Van Loan discretization, \( e^{M dt} \)

Eigen::MatrixXd exp_VL_mat_

discretized/discrete process Jacobian, \( G_{t,disc}/G_t \)

Eigen::MatrixXd proc_jac_disc_

user input process Jacobian, \( G_t \)

Eigen::MatrixXd proc_jac_

Jacobian of the process noise, \( E_t \)

Eigen::MatrixXd noise_jac_

Jacobian of the measurement, \( H_t \)

Eigen::MatrixXd meas_jac_

user input covariance of the process noise \( R_t \)

Eigen::MatrixXd proc_noise_

discretized/discrete covariance of the process noise \( R_{t,disc}/R_t \)

Eigen::MatrixXd proc_noise_disc_

user input covariance of the measure noise \( Q_t \)

Eigen::MatrixXd meas_noise_

covariance of the process noise \( Q_t \) assuming a discrete measurement

Eigen::MatrixXd meas_noise_disc_

Kalman filter gain matrix \( K_t \)

Eigen::MatrixXd gain_mat_

LDLT decomposition to invert some matrix

Eigen::LDLT<Eigen::MatrixXd> ldlt

Do we compute the Jacobian numerically or analytically?

bool numerical_jac_

Is the process discrete?

bool is_discrete_

sampling period

double dt_

order of the process dynamic linearization

unsigned lin_order_