How to Use This Extended Kalman Filter Library

8/10/2019 How to Use This Extended Kalman Filter Library

http://slidepdf.com/reader/full/how-to-use-this-extended-kalman-filter-library 1/14

How to Use this Extended Kalman Filter Library?

Introduction

This Extended Kalman Filter library is powerful and very simple to use, but a Kalman filter is verydifficult 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 thelibrary. If you are not familiar with the Kalman filter, please read this article [02].

Step 1 : Find the mathematical model of the system

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 statevector 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 itsaltitude. 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 followingequation:

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

Step 2 : Calculate Jacobian matrix

Calculate the jacobian matrix A, W, H and V where : A is an n by n jacobian matrix of partialderivatives, 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 vectoris the number of measureis the number of process noise random variablesis the number of measure noise random variables

In this example:



Step 3 : Initial conditions and covariance matrix

Set initial estimation of the state vector. After, set the covariance matrix P which represents thecovariance of the error of the state vector estimation. Then, set the covariance Q and R whichrepresent 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 arethe following:

Step 4 : Implementation of the Kalman filter



Now, it's time to create the first version of your Kalman filter. You should not try to optimize it at thisstep, just create your filter and validate it. This library allows you to optimize your filter, but in yourfirst 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 : public Kalman::EKFilter<double,1> {

public:

cPlaneEKF();

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 thefloating 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. Theyare explained in the next section, but they can be safely set to their default values in the first versionof 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 ofstates, the number of inputs, the number of process noise random variables, the number ofmeasures 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 maychange from one iteration to the other).

cPlaneEKF::cPlaneEKF()

{

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 likethis :

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_);

}

In the function makeMeasure(), you update directly the measures vector . These are thepredicted measures.

void cPlaneEKF::makeMeasure()

{

z(1)=atan2(x(3), x(1));

z(2)=sqrt(x(1)*x(1)+x(3)*x(3));

}

Then, you code all other functions makeX() like this:

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::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::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::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::makeV()

{

V(1,1) = 1.0;

V(1,2) = 0.0;

V(2,1) = 0.0;

V(2,2) = 1.0;

}



void cPlaneEKF::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 measuresand 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.

Step 5 : Optimization

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

1. If matrix Q is always diagonal, set the OQ template parameter to true. This will minimizesome calculations. Also, you will only need to fill in diagonal elements of Q, since the othervalues will never be read.

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

3. If some matrix values are constant, use the makeBaseX() function instead ofthe 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.4. 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 membervariables of your own subclass. This function is always called before the others.

5. If complex calculations are needed for more than oneof makeH(), makeV(), makeR(), makeMeasure() and makeDZ() functions, then use the

function makeCommonMeasure() to do those calculations and save them in membervariables of your own subclass. This function is always called before the others.

6. While writing a makeX() or a makeBaseX() function, there can be some execution pathswhere the function does not modify any matrix (for example, if there is some condition, thenmodify the matrix, else don't). If this is the case, then each non-mutating execution pathshould call NoModification() before returning, so that some calculations can be avoided.

7. When your filter works properly, set the debug template parameter to false to disablebound-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 insteadof 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 templateparameter to false.

So, the final result for this example is:



#ifndef PLANE_H

#define PLANE_H

#include "kalman/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.

//

http://kalman.sourceforge.net/doc/a00018.html

http://kalman.sourceforge.net/doc/a00018.html



// 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 thestation.

% 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));

}

References

[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

http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html




Date post:	02-Jun-2018
Category:	Documents
Upload:	hilmi-abdullah
View:	251 times
Download:	1 times

How to Use This Extended Kalman Filter Library

Documents