This Extended Kalman Filter library is powerful and very simple to use, but a Kalman filter is very difficult to debug. So, it is very important to follow a procedure to be sure that everything is right (code and equations). This example suggests a procedure to follow and shows how to use the library. If you are not familiar with the Kalman filter, please read this article [02].

The first thing to do is to find out the state vector you want to estimate and the inputs of the system. After this, find the non-linear process function that describes the evolution of the state vector through time, that is :

where is the process noise vector due to uncertainty and process modeling errors.

Then, find the non-linear relation between your state vector and the measure vector .

where is the measure noise vector.

**In this example :**

A plane flies in a 2D space where the x axis is the distance traveled by the plane and y axis is its altitude. This system can be represented by the following continuous equations:

where is the plane's weight (1000 kg)

is the drag coefficient (0.35 N/m²/s²)

is the lift force (3.92 N/m²/s²)

is the gravitational acceleration (9.8 m/s)

(the input) is the motor's thrust

The discrete equation is:

where and are the random variables which represent the process noise.

A station on the ground (at the origin) mesures the angle between the plane and the ground (x axis) and the distance between the plane and the station. These measures are based on the following equation:

where and are the random variables which represent the process noise.

Calculate the jacobian matrix A, W, H and V where : A is an *n* by *n* jacobian matrix of partial derivatives, defined as follow :

W is an *n* by *nv* jacobian matrix of partial derivatives, defined as follow :

H is an *m* by *n* jacobian matrix of partial derivatives, defined as follow :

V is an *m* by *nv* jacobian matrix of partial derivatives, defined as follow :

is the number of element in state vector

is the number of measure

is the number of process noise random variables

is the number of measure noise random variables

**In this example:**

Set initial estimation of the state vector. After, set the covariance matrix P which represents the covariance of the error of the state vector estimation. Then, set the covariance Q and R which represent the covariance matrix of process noise and measurement noise, respectively.

**In this example:**

The first estimation of the state vector is based on the first measures and the covariance matrix are the following:

Now, it's time to create the first version of your Kalman filter. You should not try to optimize it at this step, just create your filter and validate it. This library allows you to optimize your filter, but in your first implementation, code only the basic functions. So, code functions called `makeProcess()`

, `makeMeasure()`

, `makeA()`

, `makeH()`

, `makeQ()`

, `makeR()`

, `makeV()`

and `makeW()`

. These functions will set the value of each matrix.

The first thing to do is to create your Kalman filter class.

class cPlaneEKF_sp : public Kalman::EKFilter<double,1> { public: cPlaneEKF_sp(); protected: void makeA(); void makeH(); void makeV(); void makeR(); void makeW(); void makeQ(); void makeProcess(); void makeMeasure(); double Period, Mass, Bfriction, Portance, Gravity; };

In this example, our Kalman filter inherits from the Extended Kalman Filter, because it's a non-linear problem ( and are non-linear functions) The first two template parameters are respectively the floating point type used by the filter (`float`

or `double`

) and the beginning index of vectors and matrices (0 or 1). There are three other template parameters to the `EKFilter`

template class. They are explained in the next section, but they can be safely set to their default values in the first version of the filter, which are false, false and true to disable optimizations and enable bound-checking.

You should declare each functions named previously in this class. You can declare variables too.

After, code the class constructor. You can call the function `setDim()`

here or you will call it manually in your `main()`

function after you created the filter object. The function `setDim()`

sets the number of states, the number of inputs, the number of process noise random variables, the number of measures and the number of measurement noise random variables. It can be used by advanced users to implement a Variable-Dimension Extended Kalman Filter (an EKF whose dimensions may change from one iteration to the other).

cPlaneEKF_sp::cPlaneEKF_sp() { setDim(4, 1, 2, 2, 2); Period = 0.2; Gravity = 9.8; Bfriction = 0.35; Portance = 3.92; Mass = 1000; }

In the function `makeProcess()`

, you should use a temporary vector to store the new state vector like this :

void cPlaneEKF_sp::makeProcess() { Vector x_(x.size()); x_(1) = x(1) + x(2)*Period + (Period*Period)/2*(u(1)/Mass - Bfriction/Mass*x(2)*x(2)); x_(2) = x(2) + (u(1)/Mass - Bfriction/Mass*x(2)*x(2))*Period; x_(3) = x(3) + x(4)*Period + (Period*Period)/2*(Portance/Mass*x(2)*x(2)-Gravity); x_(4) = x(4) + (Portance/Mass*x(2)*x(2)-Gravity)*Period; x.swap(x_); }

In the function `makeMeasure()`

, you update directly the measures vector . These are the predicted measures.

Then, you code all other functions `makeX()`

like this:

void cPlaneEKF_sp::makeA() { A(1,1) = 1.0; A(1,2) = Period - Period*Period*Bfriction/Mass*x(2); A(1,3) = 0.0; A(1,4) = 0.0; A(2,1) = 0.0; A(2,2) = 1 - 2*Period*Bfriction/Mass*x(2); A(2,3) = 0.0; A(2,4) = 0.0; A(3,1) = 0.0; A(3,2) = Period*Period*Portance/Mass*x(2); A(3,3) = 1.0; A(3,4) = Period; A(4,1) = 0.0; A(4,2) = 2*Period*Portance/Mass*x(2); A(4,3) = 0.0; A(4,4) = 1.0; }

void cPlaneEKF_sp::makeW() { W(1,1) = 0.0; W(1,2) = 0.0; W(2,1) = 1.0; W(2,2) = 0.0; W(3,1) = 0.0; W(3,2) = 0.0; W(4,1) = 0.0; W(4,2) = 1.0; }

void cPlaneEKF_sp::makeQ() { Q(1,1) = 0.01*0.01; Q(1,2) = 0.01*0.01/10.0; Q(2,1) = 0.01*0.01/10.0; Q(2,2) = 0.01*0.01; }

void cPlaneEKF_sp::makeH() { H(1,1) = -x(3)/(x(1)*x(1)+x(3)*x(3)); H(1,2) = 0.0; H(1,3) = x(1)/(x(1)*x(1)+x(3)*x(3)); H(1,4) = 0.0; H(2,1) = x(1)/sqrt(x(1)*x(1)+x(3)*x(3)); H(2,2) = 0.0; H(2,3) = x(3)/sqrt(x(1)*x(1)+x(3)*x(3)); H(2,4) = 0.0; }

void cPlaneEKF_sp::makeV() { V(1,1) = 1.0; V(1,2) = 0.0; V(2,1) = 0.0; V(2,2) = 1.0; }

void cPlaneEKF_sp::makeR() { R(1,1) = 0.01*0.01; R(1,2) = 0.0; R(2,1) = 0.0; R(2,2) = 50*50; }

Now, your filter is ready to be used. In this example, the measures and the inputs have been calculated by the `generation.m`

Matlab script. It's a good idea to test your filter with fixed measures and inputs if you want to validate it.

After you create the filter object, you should call the `setDim()`

function before calling the `init()`

function. In this example, the `setDim()`

function is called in the class constructor. The `init()`

function sets the initial state and the initial covariance matrix.

WARNING : The vectors passed to the `init()`

function become unusable when `init()`

returns ! Never use those vectors after the call.

cPlaneEKF filter; static const double _P0[] = {100.0*100.0, 0.0, 0.0, 0.0, 0.0, 10.0*10.0, 0.0, 0.0, 0.0, 0.0, 25.0*25.0, 0.0, 0.0, 0.0, 0.0, 10.0*10.0};

//Initiale estimate cout<<"angle: "<<Measure(1,1)<<"rayon: "<<Measure(2,1)<<endl; x(1) = cos(Measure(1,1))*Measure(2,1); x(2) = 60; x(3) = sin(Measure(1,1))*Measure(2,1); x(4) = 0; filter.init(x, P0);

for (i = 2; i <= NTRY; ++i) { // filter for(j = 1; j <= m; j++) z(j) = Measure(j,i); Vector u(1, F(i)); filter.step(u, z); cout << "xp(" << ":," << i<<") = " << filter.getX()<<endl; dataOutput<<"trajectory_udu(" << ":," << i<<") = " << filter.getX()<<endl; }

Call the function `step()`

for each iteration and pass the new inputs and the new measures.

When your Kalman filter works properly, you can optimize it in many simple ways.

- If matrix Q is always diagonal, set the OQ template parameter to
`true`

. This will minimize some calculations. Also, you will only need to fill in diagonal elements of Q, since the other values will never be read. - If both matrices V and R are always diagonal, set the OVR template parameter to
`true`

. This will minimize some calculations. Also, you will only need to fill in diagonal elements of V and R, since the other values will never be read. - If some matrix values are constant, use the
`makeBaseX()`

function instead of the`makeX()`

function to fill these values. These functions are called only once at the beginning instead of once per iteration. You can used the`makeBaseX()`

function to set values that never change in a matrix and just set the other values in the`makeX()`

function. - If complex calculations are needed for more than one of
`makeA()`

,`makeW()`

,`makeQ()`

and`makeProcess()`

functions, then use the function`makeCommonProcess()`

to do those calculations and save them in member variables of your own subclass. This function is always called before the others. - If complex calculations are needed for more than one of
`makeH()`

,`makeV()`

,`makeR()`

,`makeMeasure()`

and`makeDZ()`

functions, then use the function`makeCommonMeasure()`

to do those calculations and save them in member variables of your own subclass. This function is always called before the others. - While writing a
`makeX()`

or a`makeBaseX()`

function, there can be some execution paths where the function does not modify any matrix (for example, if there is some condition, then modify the matrix, else don't). If this is the case, then each non-mutating execution path should call`NoModification()`

before returning, so that some calculations can be avoided. - When your filter works properly, set the debug template parameter to
`false`

to disable bound-checking.

**In this example:**

This example have been optimized in many ways:

- The matrices V and R are diagonal, so the OVR template parameter is set to true. Then, we just have to set the diagonal values of those matrices.
- The matrices V, R, W and Q never change, so we use
`makeBaseX()`

functions instead of`makeX()`

functions. - Constants values of A and H have been moved from the
`makeX()`

functions to`makeBaseX()`

functions. - The filter works properly, so we don't need bound-checking. Let's set the debug template parameter to
`false`

.

So, the final result for this example is:

#ifndef PLANE_H #define PLANE_H #include <kfilter/ekfilter.hpp> class cPlaneEKF : public Kalman::EKFilter<double,1,false,true,false> { public: cPlaneEKF(); protected: void makeBaseA(); void makeBaseH(); void makeBaseV(); void makeBaseR(); void makeBaseW(); void makeBaseQ(); void makeA(); void makeH(); void makeProcess(); void makeMeasure(); double Period, Mass, Bfriction, Portance, Gravity; }; typedef cPlaneEKF::Vector Vector; typedef cPlaneEKF::Matrix Matrix; #endif

// -------------- plane.cpp - Example of Extended Kalman filter ------------------------// // // This file is part of kfilter. // kfilter is a C++ variable-dimension extended kalman filter library. // // Copyright (C) 2004 Vincent Zalzal, Sylvain Marleau // Copyright (C) 2001, 2004 Richard Gourdeau // Copyright (C) 2004 GRPR and DGE's Automation sector // École Polytechnique de Montréal // // Code adapted from algorithms presented in : // Bierman, G. J. "Factorization Methods for Discrete Sequential // Estimation", Academic Press, 1977. // // This library is free software; you can redistribute it and/or // modify it under the terms of the GNU Lesser General Public // License as published by the Free Software Foundation; either // version 2.1 of the License, or (at your option) any later version. // // This library is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // Lesser General Public License for more details. // // You should have received a copy of the GNU Lesser General Public // License along with this library; if not, write to the Free Software // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA // --------------------------- Example of Extended Kalman filter ------------------------// /* % A plane flights in a 2D space where the x axis is the distance traveled % by the plane and y axis is its altitude. This system can be represented % by the fallowing equations: % (This is just an example) % % xpp = F/m - bx/m * xp^2 % ypp = p/m * xp^2 - g % % where m is the plane's weight (1000 kg) % bx is the drag coefficient (0.35 N/m²/s²) % p is the lift force (3.92 N/m²/s²) % g is the gravitational acceleration (9.8 m/s²) % F is the motor's thrust % % A station on the ground (at the origin) mesures the angle between the % plane and the ground (x axis) and the distance between the plane and the station. % These measures are based and the fallowing equations: % % theta = atan2(y,x) % r = sqrt(x^2+y^2) % % The variance error matrix of the mesures is: % % R = [0.01^2 0 % 0 50^2] % % V = [1 0; % 0 1]; % % The variance error matrix of the plane's model is: WQW' % % Q = [0.01^2 0; % 0 0.01^2]; % % W = [0 0; % 1 0; % 0 0; % 0 1]; % */ #include "plane.h" #include <cmath> #include <iostream> using namespace std; cPlaneEKF::cPlaneEKF() { setDim(4, 1, 2, 2, 2); Period = 0.2; Gravity = 9.8; Bfriction = 0.35; Portance = 3.92; Mass = 1000; } void cPlaneEKF::makeBaseA() { A(1,1) = 1.0; // A(1,2) = Period - Period*Period*Bfriction/Mass*x(2); A(1,3) = 0.0; A(1,4) = 0.0; A(2,1) = 0.0; // A(2,2) = 1 - 2*Period*Bfriction/Mass*x(2); A(2,3) = 0.0; A(2,4) = 0.0; A(3,1) = 0.0; // A(3,2) = Period*Period*Portance/Mass*x(2); A(3,3) = 1.0; A(3,4) = Period; A(4,1) = 0.0; // A(4,2) = 2*Period*Portance/Mass*x(2); A(4,3) = 0.0; A(4,4) = 1.0; } void cPlaneEKF::makeA() { // A(1,1) = 1.0; A(1,2) = Period - Period*Period*Bfriction/Mass*x(2); // A(1,3) = 0.0; // A(1,4) = 0.0; // A(2,1) = 0.0; A(2,2) = 1 - 2*Period*Bfriction/Mass*x(2); // A(2,3) = 0.0; // A(2,4) = 0.0; // A(3,1) = 0.0; A(3,2) = Period*Period*Portance/Mass*x(2); // A(3,3) = 1.0; // A(3,4) = Period; // A(4,1) = 0.0; A(4,2) = 2*Period*Portance/Mass*x(2); // A(4,3) = 0.0; // A(4,4) = 1.0; } void cPlaneEKF::makeBaseW() { W(1,1) = 0.0; W(1,2) = 0.0; W(2,1) = 1.0; W(2,2) = 0.0; W(3,1) = 0.0; W(3,2) = 0.0; W(4,1) = 0.0; W(4,2) = 1.0; } void cPlaneEKF::makeBaseQ() { Q(1,1) = 0.01*0.01; Q(1,2) = 0.01*0.01/10.0; Q(2,1) = 0.01*0.01/10.0; Q(2,2) = 0.01*0.01; } void cPlaneEKF::makeBaseH() { // H(1,1) = -x(3)/(x(1)*x(1)+x(3)*x(3)); H(1,2) = 0.0; // H(1,3) = x(1)/(x(1)*x(1)+x(3)*x(3)); H(1,4) = 0.0; // H(2,1) = x(1)/sqrt(x(1)*x(1)+x(3)*x(3)); H(2,2) = 0.0; // H(2,3) = x(3)/sqrt(x(1)*x(1)+x(3)*x(3)); H(2,4) = 0.0; } void cPlaneEKF::makeH() { H(1,1) = -x(3)/(x(1)*x(1)+x(3)*x(3)); // H(1,2) = 0.0; H(1,3) = x(1)/(x(1)*x(1)+x(3)*x(3)); // H(1,4) = 0.0; H(2,1) = x(1)/sqrt(x(1)*x(1)+x(3)*x(3)); // H(2,2) = 0.0; H(2,3) = x(3)/sqrt(x(1)*x(1)+x(3)*x(3)); // H(2,4) = 0.0; } void cPlaneEKF::makeBaseV() { V(1,1) = 1.0; V(2,2) = 1.0; } void cPlaneEKF::makeBaseR() { R(1,1) = 0.01*0.01; R(2,2) = 50*50; } void cPlaneEKF::makeProcess() { Vector x_(x.size()); x_(1) = x(1) + x(2)*Period + (Period*Period)/2*(u(1)/Mass - Bfriction/Mass*x(2)*x(2)); x_(2) = x(2) + (u(1)/Mass - Bfriction/Mass*x(2)*x(2))*Period; x_(3) = x(3) + x(4)*Period + (Period*Period)/2*(Portance/Mass*x(2)*x(2)-Gravity); x_(4) = x(4) + (Portance/Mass*x(2)*x(2)-Gravity)*Period; x.swap(x_); } void cPlaneEKF::makeMeasure() { z(1)=atan2(x(3), x(1)); z(2)=sqrt(x(1)*x(1)+x(3)*x(3)); }

[01] Bierman, G. J. "Factorization Methods for Discrete Sequential
Estimation", Academic Press, 1977.

[02] Welch, G. and Bishop, G. "An Introduction to the %Kalman Filter", http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html

kfilter

Author(s): Vincent Zalzal, Sylvain Marleau, Richard Gourdeau

autogenerated on Wed Jul 23 04:33:42 2014

Author(s): Vincent Zalzal, Sylvain Marleau, Richard Gourdeau

autogenerated on Wed Jul 23 04:33:42 2014