Kalman Filtering Practical Work

Last Name:
First Name:
Group:

I. INTRODUCTION....................................................................................................................................................2
II. PRELIMINARY QUESTIONS: MEASUREMENT MODEL.............................................................................3
III. KALMAN FILTER ESTIMATION ALGORITHM.........................................................................................4
IV. PERFORMANCE ANALYSIS...........................................................................................................................7
A. DIAGONAL TRAJECTORY.........................................................................................................................................7
B. LINEAR TRAJECTORY..............................................................................................................................................8
C. ELLIPTICAL TRAJECTORY.......................................................................................................................................8
V. Conclusion................................................................................................................................................................10
I. Introduction
The purpose of this practical work is to show an implementation example of both Least Squares and
Kalman filtering algorithms applied to the satellite-based positioning of a mobile. To do so, let us
consider the two-dimensional navigation system composed of n fixed transmitters in the plane
providing a signal allowing the receiver to make distance measurements between the transmitter
and the receiver. We consider a mobile M(x,y) equipped with a receiver moving in that plane.

Figure Introduction.1 : Navigation in the plane.

The distance measurements are made by the receiver by determining the delay between the received
signal and a local replica of that signal. The transmitters are all synchronised. The range
measurements are modelled as follows:

 y (t )  ME  ct (t )  e (t )
 1 1 1


 y 2 (t )  ME2  ct (t )  e2 (t )

 
 y (t )  ME  ct (t )  e (t )

 n n n

where:
– t is the bias between the receiver clock and the transmitter clock.
– e1, e2, …, en are the distance measurement errors.

The following notations will be used throughout the practical work:

Vector of ME1  b(t )

X (t )   x (t ) b(t )
T
unknowns to y (t ) Measurement h: X  
determine function ME n  b(t )
Receiver clock bias b( t )  ct ( t )

Vector of Estimates 
Xˆ (t )  xˆ (t ) yˆ (t ) 
bˆ(t )
T Estimates

residual
X (t )  X (t )  Xˆ (t )

Measurement noise
R (with W=R-1)
covariance matrix

The goal of this practical work is then to implement Least Squares and Kalman filtering algorithms
in order to determine the position of the mobile, using both. A Matlab script is then used for that
purpose. The template provided for this practical work can be decomposed as given below.

PHASE
MATLAB SCRIPT HEADER / PROCESSING STEP
NB
%%%%%%%%%%%%%%%%%%%%%%%%
% constant declaration %
%%%%%%%%%%%%%%%%%%%%%%%%
1
Declaration of all the constants used in the script.
The trajectory of the mobile is configured in this code block.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% generation of the true trajectory %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2
The trajectory of the mobile is generated. This trajectory will serve as reference in order
to analyse the performance that can be achieved with the different algorithms
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% generation of the measurements %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
3 From the true trajectory (phase 2) and the position of the emitters (phase 1), the true
pseudoranges are generated. Noise and bias are also added to simulate propagation error
and receiver clock bias.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% determination of the position + clock bias through EKF %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
4 This part is dedicated to the position computation using the Extended Kalman Filter
(EKF) algorithm because of the non-linear property of the measurement equations.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Comparison of the results with respect to the true inputs %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
5
This part computes the errors made by the different algorithms with respect to the true
inputs.
%%%%%%%%%%%%%%%%%%%%%%%
% Display the results %
6 %%%%%%%%%%%%%%%%%%%%%%%
This part displays the results on several graphs.
Table Introduction.1 : Matlab Simulation Script Headlines.

II. Preliminary Questions: Measurement Model

This section is dedicated to the derivation of the measurement model used in the EKF algorithm.

 Question II.1: Express the distance measurements as a function of the vehicle coordinates and
the position of the emitters

Answer:
MEi   x Ei  x M  2   y Ei  y M  2

 Question II.2: Determine the minimal number of simultaneous measurements allowing the
instantaneous estimation of the position of the vehicle.

Answer:
 yi (t )  MEi  ct (t )  ei (t ) with MEi   x Ei  xM  2   y Ei  yM 2

The unknowns are the position of the receiver (xM, yM) and the receiver clock bias Δt. The minimal
number of simultaneous measurements needed to estimate the position of the vehicle is thus equal
to three.

 Question II.3: According to the previous results, describe the kind of KF algorithm that has to
be used to estimate the coordinates and the clock bias of the receiver.

Answer:
Iterative LS, Extended KF

III. KALMAN FILTER ESTIMATION ALGORITHM

Question IV.1: We model the trajectory (x, y) and the user clock bias b as random walks. We thus
assume that the jerk (1st order derivative of acceleration) along x and y is a white noise with
spectrum density level  x and  y . We also assume that the acceleration of the clock bias is a white
2 2

noise with spectrum density level  b2 . Give the expression of the continuous state transition model:
X (t )  F  t  X  t   W  t  . Determine the most adequate state vector X  t  to be used in the Kalman
Filter. Determine matrix F  t  .

Answer:

State vector: X  x v x a x y v y a y t dt
T

State transition:
o x  v x , v x  a x , a x  n x where nx is a centred white gaussian noise.
o Similar model for y, vy and ay.
o t  dt , dt  ndt where ndt is a centered gaussian white noise.

 x  0 1 0 0 0 0 0 0  x   0 
 v x  0 0 1 0 0 0 0 0  v x   0 
  
 a x  0 0 0 0 0 0 0 0  a x   n x 
  0     
0 0 0 1 0 0 0  y   0 
 y     
 v y  0 0 0 0 0 1 0 0  v y   0 
   0     
0 0 0 0 0 0 0  a y   n y 
a y  
   0 0 0 0 0 0 0 1  b   0 
b     
 b  0 0 0 0 0 0 0 0  b   nb 

MATRIZ DE TRANSICION
 Question IV.2: We assume that this state is characteristic of a continuous process observed at
discrete epochs with period Ts. In order to implement the Kalman filter, we first have to get the
relationships of question IV.1 discrete. Get the previously identified state transition equations of
the system discrete.

Answer:

Fk  I  F (t )  Ts

1 0 0 0 0 0 0 0  0 1 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0 0 1 0 0 0 0 0

0 0 1 0 0 0 0 0  0 0 0 0 0 0 0 0
   
0 0 0 1 0 0 0 0  0 0 0 0 1 0 0 0
Fk     Ts
0 0 0 0 1 0 0 0  0 0 0 0 0 1 0 0
   
0 0 0 0 0 1 0 0  0 0 0 0 0 0 0 0
0 0 0 0 0 0 1 0  0 0 0 0 0 0 0 1
   
0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0

1 Ts 0 0 0 0 0 0
0 1 Ts 0 0 0 0 0 

0 0 1 0 0 0 0 0
 
0 0 0 1 Ts 0 0 0
Fk  
0 0 0 0 1 Ts 0 0
 
0 0 0 0 0 1 0 0
0 0 0 0 0 0 1 Ts 
 
0 0 0 0 0 0 0 1 

State transition Matrix: Fk  exp F (t )  Ts   I  F (t )  Ts 

 F (t )  Ts  2  oT 2 
s
2!
2
State noise covariance matrix:  T
Qk  Q(t )  Ts  F (t )  Q(t )  Q (t )  F (t )  s  o Ts2
2
T
  
Measurement matrix: Yk  Y (t  k )
Measurement noise covariance matrix: Rk  R (t  k )

 Question IV.3: Give the expression of the covariance matrix Q of the state noise..

Answer:
0 0 0 0 0 0 0 0 
0 0 0 0 0 0 0 0 

0 0  2
nx 0 0 0 0 0 
 
0 0 0 0 0 0 0 0 
Qt  
0 0 0 0 0 0 0 0 
 
0 0 0 0 0  2
ny 0 0 
0 0 0 0 0 0 0 0 
 
0 0 0 0 0 0 0  nb
2

 And as Qk  Qt  Ts

0 0 0 0 0 0 0 0 
0 0 0 0 0 0 0 0 

0 0  2
nx Ts 0 0 0 0 0 
 
0 0 0 0 0 0 0 0 
Qk  
0 0 0 0 0 0 0 0 
 
0 0 0 0 0  2
ny Ts 0 0 
0 0 0 0 0 0 0 0 
 
0 0 0 0 0 0 0  nb
2
Ts 

 Question IV.4: Give the expression of the observation equation of the system. What kind of
Kalman filter should be implemented?

Answer:
Y  h X   E , where X   x b  and E   e1
 eN 
T T
y
There is a non-linear relationship between the state and the measurements. An extended Kalman
filter should then be used.

 Question IV.5: We assume that the errors e1, e2, …, en are random errors independent with each
other. Each errors ei is centred Gaussian white noise with standard deviation ei. Give the
expression of the measurement noise covariance matrix R.

Answer:
 12 0 0 
 
R 0  0 
 0 0  N2 
 
IV. Performance Analysis

This chapter is dedicated to the analysis of the estimation performance of the previously described
algorithm.

A. Diagonal Trajectory

 Question V.1: Present, for each figure, all quantities plotted and mentioned (in particular define
the standard deviations plotted and displayed).
Answer:

 Question V.2: Analyze the performance of the Extended Kalman filter for the diagonal
trajectory and indicate the KF tuning (without modifying the default EKF tuning: initialization,
state transition model, measurement model).
Answer:

 Question V.3: Describe and explain the behaviour of the estimation error. After a description
and a general explanation of the behaviour, explain why the error along x is larger than the error
along y. Explain why the estimation error increases with time.
Answer:

 Question V.4: Present and explain the behaviour of the innovations.

Answer:

 Question V.5: Analyze the convergence of the filter in the following conditions, considering the
state transition and the observation model tuning. Detail the influence of each of the two
parameters X̂ 0 0 ,  0 0 , and discuss the time of convergence.
 X̂ 0 0 far from true initial state,  0 0 large.
 X̂ 0 0 far from true initial state,  0 0 small.

Answer:

 Question V.6: Analyse the influence of matrix Q in the following cases:

  Different noise for a x , a y and b . Modify the values of the variance of
these noises and discuss their influence. In particular, try to otain an optimal
tuning of these values.
 Noise identical and thus correlated for a x and a y . We instruct the K.F. of the
fact that we know that the mobile is moving along the diagonal. Compare the
performance with the previous case.

Answer:

 Question V.7: Analyze the impact of matrix R on the estimation error and on the predicted
value of its standard deviation in the following cases:
 Standard deviation of applied noise (vector sigma) multiplied by 2
 R smaller than nominal case: R=1/4*diag(sigma.^2)
 R non proportional to the true covariance matrix

Answer:

B. Linear Trajectory
Linear trajectory along x.

 Question V.8: Analyze the performance of the EKF for the linear trajectory and indicate its
tuning.

Answer:

 Question V.9: Analyze the influence of matrix Q in the following cases:

 Noise different for a x , a y et b .
 Noise a y set to zero. Compare the performance with the previous case and
explain the difference.

Answer:

C. Elliptical Trajectory

 Question V.10: Analyze the performance of the EKF for the elliptical trajectory.

Answer:

 Question V.11: Is it possible to adapt the covariance matrix of the state noise Q to this new
trajectory ? Explain why and give an example of matrix if possible.

Answer:
Conclusion

 Question VI.1: Summarize the observations and conclusions concerning the properties of the
Kalman filter and draw a conclusion on its performance.

Answer: