Kalman Filter algorithms (EKF,IEKF,UKF) are centralized in one single virtual class, This class contains the system state vector and the system covariance matrix, as well as a generic method to execute one complete iteration of the selected algorithm. In practice, solving a specific problem requires deriving a new class from this virtual class and implementing a few methods such as transforming the state vector through the transition model, or computing the Jacobian of the observation model linearized at some given value of the state space.
A set of parameters that are problem-independent can be changed in the member of this class, where the most important parameter is the selection of the KF algorithm. Currently it can be chosen from the following ones:
- Naive EKF: The basic EKF algorithm.
- EKF a la Davison: A far more efficient implementation that processes each dimension of the observation vector sequentially (Based on the description given in [1]).
- Iterative Kalman Filter (IKF): This method re-linearizes the Jacobians around increasingly more accurate values of the state vector.
Another implementation of Bayesian filtering in the MRPT C++ library are
2 Writing a KF class for a specific problem
2.1 Deriving a new class
First a new class must be derived from It should be declared a public method as an entry point for the user, which saves the input data (observations,actions,...) and calls the protected method of the parent class.
There are two fundamental types of systems the user can build by deriving a new class:
- A normal problem: The size of the state vector remains unchanged over time. This size must be returned by the virtual method "get_vehicle_size()".
- A mapping-like problem: The size of the state vector grows as new map elements are incorporated over time. In this case the first "get_vehicle_size()" elements of the
state vector correspond to the state of the vehicle/robot/camera/... and the rest of the state vector is a whole number of "get_feature_size()" sub-vectors, each describing one map element (landmark, feature,...).
2.2 The internal flow of the algorithm
The method CKalmanFilterCapable::runOneKalmanIteration() will sequentially call each of the virtual methods, according to the following order:
- During the KF prediction stage:
- OnGetAction.
- OnTransitionModel.
- OnTransitionJacobian.
- OnTransitionNoise.
- OnNormalizeStateVector: This can be optionally implemented if required for the concrete problem.
- OnGetObservations: This is the ideal place for generating observations in those applications where it requires an estimate of the current state (e.g. in MonoSLAM, to predict where each landmark will be found onto the image). At this point the internal state vector and covariance contain the "prior distribution", i.e. updated through the transition model. This is also the place where data association must be solved (in mapping problems).
- During the KF update stage:
- OnObservationModelAndJacobians: This method will be called just once in naive EKF, or several times in the EKF a la Davison.
- OnNormalizeStateVector.
- If the system implements a mapping problem, and the data association returned by "OnGetObservations" indicates the existence of unmapped observations, then the next method will be invoked for each of these new features:
- OnInverseObservationModel.
- OnPostIteration: A placeholder for any code the user wants to execute after each iteration (e.g. logging, visualization,...).
3 An example
An example of a KF implementation can be found in the samples directory (samples/bayesianTracking ) for the problem of tracking a vehicle from noisy range and bearing data. Here it will be derived the required equations to be implemented, as well as how they are actually implemented in this example. Note that this problem is also implemented as a Particle Filter in the same example in order to visualize side-to-side the performance of both approaches.
3.1 Problem Statement
The problem of range-bearing tracking is that of estimating the vehicle state:
where x and y are the Cartesian coordinates of the vehicle, and vx and vy are the linear velocities. Thus, we'll use a simple constant velocity model, where the transition function is:
We will consider the time interval between steps Δt as the action u of the system. The observation vector consists of the bearing and range of the vehicle from some point (arbitrarily set to the origin):
zb = atan2(y,x)
Then, it is straightforward to obtain the required Jacobian of the transition function:
and the Jacobian of the observation model:
3.2 Implementation
The most important implemented methods are detailed below. For further details refer to the source code of the "bayesianTracking" example.
3.2.1 The transition model
The constant-velocities model is implemented simply as:
/** Implements the transition model \f$ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) \f$
* \param in_u The vector returned by OnGetAction.
* \param inout_x At input has \f$ \hat{x}_{k-1|k-1} \f$, at output must have \f$ \hat{x}_{k|k-1} \f$.
* \param out_skip Set this to true if for some reason you want to skip the prediction step (to do not modify either the vector or the covariance). Default:false
*/
void CRangeBearing::OnTransitionModel(
const CVectorTemplate<KFTYPE>& in_u,
CVectorTemplate<KFTYPE>& inout_x,
bool &out_skipPrediction )
{
// in_u[0] : Delta time
// in_out_x: [0]:x [1]:y [2]:vx [3]: vy
inout_x[0] += in_u[0] * inout_x[2];
inout_x[1] += in_u[0] * inout_x[3];
}3.2.2 The transition model Jacobian
/** Implements the transition Jacobian \f$ \frac{\partial f}{\partial x} \f$
* \param out_F Must return the Jacobian.
* The returned matrix must be \f$N \times N\f$ with N being either the size of the whole state vector or get_vehicle_size().
*/
void CRangeBearing::OnTransitionJacobian(
CMatrixTemplateNumeric<KFTYPE> & F
)
{
F.unit(4);
F(0,2) = m_deltaTime;
F(1,3) = m_deltaTime;
}3.2.3 The observation innovation and the observation Jacobian
/** Implements the observation prediction \f$ h_i(x) \f$ and the Jacobians \f$ \frac{\partial h_i}{\partial x} \f$ and (when applicable) \f$ \frac{\partial h_i}{\partial y_i} \f$.
* \param in_z This is the same matrix returned by OnGetObservations(), passed here for reference.
* \param in_data_association The vector returned by OnGetObservations(), passed here for reference.
* \param in_full If set to true, all the Jacobians and predictions must be computed at once. Otherwise, just those for the observation in_obsIdx.
* \param in_obsIdx If in_full=false, the row of the observation (in in_z and in_data_association) whose innovation & Jacobians are to be returned now.
* \param out_innov The difference between the expected observation and the real one: \f$ \tilde{y} = z - h(x) \f$. It must be a vector of length O*N (in_full=true) orO (in_full=false), with O=get_observation_size() and N=number of rows in in_z.
* \param out_Hx One or a vertical stack of \f$ \frac{\partial h_i}{\partial x} \f$.
* \param out_Hy An empty matrix, or one or a vertical stack of \f$ \frac{\partial h_i}{\partial y_i} \f$.
*
* out_Hx must consist of 1 (in_full=false) or N (in_full=true) vertically stacked \f$O \times V\f$ matrices, and out_Hy must consist of 1 or N \f$O \times F\f$ matrices, with:
* - N: number of rows in in_z (number of observed features, or 1 in general).
* - O: get_observation_size()
* - V: get_vehicle_size()
* - F: get_feature_size()
*/
void CRangeBearing::OnObservationModelAndJacobians(
const CMatrixTemplateNumeric<KFTYPE> &in_z,
const vector_int &in_data_association,
const bool &in_full,
const int &in_obsIdx,
CVectorTemplate<KFTYPE> &ytilde,
CMatrixTemplateNumeric<KFTYPE> &out_Hx,
CMatrixTemplateNumeric<KFTYPE> &out_Hy
)
{
// predicted bearing:
KFTYPE x = m_xkk[0];
KFTYPE y = m_xkk[1]; KFTYPE h_bear = atan2(y,x);
KFTYPE h_range = sqrt(square(x)+square(y)); ytilde.resize(2);
ytilde[0] = MATH::angleTo2PIRange( in_z(0,0) - h_bear );
ytilde[1] = in_z(0,1) - h_range; out_Hx.setSize(2,4);
out_Hx.zeros();
out_Hx(0,0) = -y/(square(x)+square(y));
out_Hx(0,1) = 1/(x*(1+square(y/x))); out_Hx(1,0) = x/sqrt(square(x)+square(y));
out_Hx(1,1) = y/sqrt(square(x)+square(y)); out_Hy.setSize(0,0); // Not used
}
Here just give the source code, you need to carry out all the necessary methods.
#pragma once
#include <mrpt/core.h>
using namespace mrpt;
using namespace mrpt::bayes;
using namespace mrpt::gui;
using namespace mrpt::math;
using namespace mrpt::slam;
using namespace mrpt::utils;
using namespace mrpt::random;
using namespace std;
// a new class must be derived from mrpt::bayes::CKalmanFilterCapable
class CRangeBearingEKF : public CKalmanFilterCapable
{
public:
CRangeBearingEKF(void);
~CRangeBearingEKF(void);
// call runOneKalmanIteration： prediction + update
void doProcess( double DeltaTime, double observationRange, double observationBearing );
void getState( CVectorTemplate<KFTYPE> &xkk, CMatrixTemplateNumeric<KFTYPE> &pkk)
{
xkk = m_xkk;
pkk = m_pkk;
}
protected:
float m_obsBearing,m_obsRange;
float m_deltaTime;
/** Must return the action vector u.
* param out_u The action vector which will be passed to OnTransitionModel
*/
void OnGetAction( CVectorTemplate<KFTYPE> &out_u );
/** Implements the transition model x{k|k-1} = f(x{k-1|k-1}, u_k ), that is the vehicle model
* \param in_u: The vector returned by OnGetAction.
* \param inout_x: At input has f(x{k-1|k-1}, u_k ), at output must have x{k|k-1}.
* \param out_skip Set this to true if for some reason you want to skip the prediction step (to do not modify either the vector or the covariance). Default:false
*/
void OnTransitionModel(
const CVectorTemplate<KFTYPE>& in_u,
CVectorTemplate<KFTYPE>& inout_x,
bool &out_skipPrediction
);
/** Implements the transition Jacobian partial f/ partial x, that is F
* \param out_F: Must return the Jacobian.
* The returned matrix must be N*N with N being either the size of the whole state vector or get_vehicle_size().
*/
void OnTransitionJacobian(
CMatrixTemplateNumeric<KFTYPE> & out_F
);
/** Implements the transition noise covariance Q_k
* \param out_Q: Must return the covariance matrix.
* The returned matrix must be of the same size than the Jacobian from OnTransitionJacobian
*/
void OnTransitionNoise(
CMatrixTemplateNumeric<KFTYPE> & out_Q
);
/** This is called between the KF prediction step and the update step to allow the application to employ
* the prior distribution of the system state in the detection of observations (data association), where applicable.
*
* \param out_z: N*O matrix, with O being get_observation_size() and N the number of "observations":
* how many observed landmarks for a map, or just one if not applicable.
* \param out_R2: A vector of size O (O being get_observation_size()) with the *variances* of the sensor noise for each
* of the observation components. This is constant for all the observations (where N>1), and in the naive Kalman method
* the value TKF_options::veryLargeR2 is used for unobserved map elements.
* \param out_data_association: An empty vector or, where applicable, a vector where the i'th element corresponds to
* the position of the observation in the i'th row of out_z within the system state vector, or -1 if it is a new map
* element and we want to insert it at the end of this KF iteration.
* This method will be called just once for each complete KF iteration.
*/
void OnGetObservations(
CMatrixTemplateNumeric<KFTYPE> &out_z,
CVectorTemplate<KFTYPE> &out_R2,
vector_int &out_data_association
);
/** Implements the observation prediction z(k+1|k) partial h_i}\partial x
*
* \param in_z: This is the same matrix returned by OnGetObservations(), passed here for reference.
* \param in_data_association The vector returned by OnGetObservations(), passed here for reference.
* \param in_full If set to true, all the Jacobians and predictions must be computed at once. Otherwise, just those for the observation in_obsIdx.
* \param in_obsIdx If in_full=false, the row of the observation (in in_z and in_data_association) whose innovation & Jacobians are to be returned now.
* \param out_innov The difference between the expected observation and the real one: \f$ \tilde{y} = z - h(x) \f$. It must be a vector of length O*N (in_full=true) or O (in_full=false), with O=get_observation_size() and N=number of rows in in_z.
* \param out_Hx One or a vertical stack of \f$ \frac{\partial h_i}{\partial x} \f$.
* \param out_Hy An empty matrix, or one or a vertical stack of \f$ \frac{\partial h_i}{\partial y_i} \f$.
*
* out_Hx must consist of 1 (in_full=false) or N (in_full=true) vertically stacked \f$O \times V\f$ matrices, and out_Hy must consist of 1 or N \f$O \times F\f$ matrices, with:
* - N: number of rows in in_z (number of observed features, or 1 in general).
* - O: get_observation_size()
* - V: get_vehicle_size()
* - F: get_feature_size()
*/
void OnObservationModelAndJacobians(
const CMatrixTemplateNumeric<KFTYPE> &in_z,
const vector_int &in_data_association,
const bool &in_full,
const int &in_obsIdx,
CVectorTemplate<KFTYPE> &out_innov,
CMatrixTemplateNumeric<KFTYPE> &out_Hx,
CMatrixTemplateNumeric<KFTYPE> &out_Hy
);
/** Must return the dimension of the "vehicle state": either the full state vector or the "vehicle" part if applicable.
*/
size_t get_vehicle_size() const { return 4; /* x y vx vy*/ }
// get_feature_size Is not implemented since this is not a "mapping" problem. Default method will return 0.
/** Must return the dimension of each observation (eg, 2 for pixel coordinates, 3 for 3D coordinates,etc).
*/
size_t get_observation_size() const { return 2; /* range+bearing */ }
/** @}
*/
};
