1. Discrete-time Kalman
Filter
Linear Quadratic Estimation
University of Macau
Manuel Maria Beja da Costa
July 2016
Abstract
The purpose of this paper is to present the results
obtained from simulation tests performed on a
discrete-time Kalman filter. The filter was
implemented on Matlab with the intention of serving
as an estimation tool for the localizatio of an
underwater vehicle.
1 Introduction
Kalman filtering (or Linear Quadratic Estimation) is
an algorithm that takes time-varying measurements
and a control vector and provides, in the end, an es-
timate of the desired and previously unknown, vari-
ables. We design a filter fit for underwater applica-
tion, capable of estimating the position of a single
moving target, the distance to a reference point and
the magnitude of a possible bias when provided read-
ings from the sensors. In the present case these read-
ings will be both the direction and the velocity of the
target.
The trajectory used for the simulation is an ideal
one. This enables the filter to have more accurate
measurements which leads to better results.
2 State Space Modeling
2.1 Governing Equations
To govern the state space, the following equations
were used:
x[k + 1] = A[k]x[k] + B[k]u[k] (1)
y[k + 1] = C[k + 1]x[k + 1], (2)
where k is an integer, incremented every iteration.
Initially, x[k] and y[k] were established as follows:
x[k] =








sx[k]
sy[k]
sz[k]
vRx
vRy
vRz








y[k] =


dx
dy
dz

 , (3)
vR[k] represents the real velocity of the target which
may be different from the velocity measured by the
body for there may be bias to be considered. Even
though vR[k] is a measurement, it is not contem-
plated on y[k] but on
u[k] =


vx
vy
vz

 , (4)
leaving y[k] with only the direction d[k]. Now, by
knowing that
vR[k] = vB[k] + b, (5)
we are able to modify x[k] in a way that makes possi-
ble the estimation of the bias b. The resulting states
vector follows as:
x[k] =








sx[k]
sy[k]
sz[k]
bx
by
bz








. (6)
2.2 Direction and Linearity
Having,
d[k] =
s[k]
s[k]
(7)
brings up an important issue, linearity. With y[k] as
stated on (3), and considering (2), we conclude that
it is impossible for (7) to be written in the form of
(2). It is clear now that the problem is non linear,
which is implicit in the definition of direction.
Our approach to this obstacle was to add one more
state to x[k], such that
x[k] =










sx[k]
sy[k]
sz[k]
bx
by
bz
r[k]










, (8)
where
1

2. r[k] = s[k] , (9)
and we also made
y[k] = 0, (10)
Now, if we set
C[k] =


1 0 0 0 0 0 −dx[k]
0 1 0 0 0 0 −dy[k]
0 0 1 0 0 0 −dz[k]

 . (11)
we will have the following equation:
0 =


1 0 0 0 0 0 −dx[k]
0 1 0 0 0 0 −dy[k]
0 0 1 0 0 0 −dz[k]

 ∗










sx[k]
sy[k]
sz[k]
bx
by
bz
r[k]










. (12)
We have thus restored linearity in our system.
2.3 Matrix A and B
Considering a new state was added, the respective
parameters have to be taken into account on both
matrices A and B.
It is known from (7) that
d[k + 1] =
s[k + 1]
r[k + 1]
, (13)
Since d[k + 1] is a unit vector and r[k + 1] is a scalar,
(13) can be written as
r[k + 1] = d[k + 1]T
s[k + 1]. (14)
Now, to estimate s[k + 1] we resort to the following
equation:
s[k + 1] = s[k] + vR[k]T. (15)
Replacing (14) on (15) yelds
r[k + 1] = d[k]T
(s[k] + vR[k]T). (16)
Using (5) and (13) we achieve the following equation
r[k+1] = d[k]T
r[k]d[k]+d[k+1]T
vb
[k]T+d[k+1]T
bT
(17)
We now have all the parameters for both matrix A
and B.
AT
[k] =










1 0 0 0 0 0 0
0 1 0 0 0 0 0
0 0 1 0 0 0 0
T 0 0 1 0 0 dx[k + 1]T
0 T 0 0 1 0 dy[k + 1]T
0 0 T 0 0 1 dz[k + 1]T
0 0 0 0 0 0 dT
[k + 1]d[k]










(18)
B[k] =










T 0 0
0 T 0
0 0 T
0 0 0
0 0 0
0 0 0
dx[k + 1]T dy[k + 1]T dz[k + 1]T










(19)
3 Trajectory
Even though a random trajectory can be used, there
are some requirements in order to have the best per-
formance from the filter. Firstly, a trajectory com-
posed by three sine functions was set up. We came
to the conclusion that this was not a good option,
as the trajectory showed a rather periodic behaviour,
whereby all the directions where not being excited
enough in order to obtain the best performance from
the filter. To overcome this problem we were pro-
vided with a trajectory designed by Eng. Joel Reis
that was carefully set up to meet all the requirements
needed for a good filtering. Such trajectory can be
seen on Fig.1.
Figure 1: Representation of the trajectory in three di-
mensions.
2

3. 4 The Kalman Filter
4.1 Environment Setup
We started by setting up the variables necessary to
describe the trajectory mentioned on Section 3.
After, using the definition of derivative, which is
approximated as
vB =
s[k + 1] − s[k]
T
, (20)
we got to the values of vB. Also, from (7), we ob-
tained d[k] and r[k]. The simulations were done with
the number of k iterations set to 1000 and T was set
to 1s. The remaining variables have been discussed
in the previous sections.
4.2 The Kalman Filter Equations
The following equations represent the Kalman filter
implementation:
¯x[k] = A[k − 1]ˆx[k − 1] + B[k − 1]u[k − 1] (21)
P−
[k] = A[k − 1]P[k − 1]AT
[k − 1] + Q[k − 1] (22)
K[k] = P−
[k]CT
[k] C[k]P−
[k]CT
[k] + R
−1
(23)
¯x[k] = ˆx[k] + K[k](y[k] − C[k]ˆx[k]) (24)
P[k] = (I − K[k]C[k])P−
[k], (25)
Both (21) and (22) are related to the prediction
(”a priori”) phase of the estimation. The remain-
ing equations, (23), (24) and (25), are related to the
updated (”a posteriori”) phase of the estimation. Be-
ing, respectively, the predicted state estimate and the
predicted estimate covariance. The remaining equa-
tions, (23), (24) and (25), are related to the update
(”a posteriori”) phase of the estimation and repre-
sent, respectively, the optimal Kalman gain, the up-
dated state estimate and updated estimate covari-
ance.
4.3 Noise and Error
We address now the matrices mentioned on the pre-
vious subsection. Starting with P−
which represents
the predicted estimate covariance, then there is P
which is the updated estimate covariance. P is a di-
agonal matrix and was initially set as:
P[0] =










10000 0 0 0 0 0 0
0 10000 0 0 0 0 0
0 0 10000 0 0 0 0
0 0 0 10 0 0 0
0 0 0 0 10 0 0
0 0 0 0 0 10 0
0 0 0 0 0 0 10000










.
(26)
To represent the process noise there is matrix Q,
through which is possible to regulate the uncertainty,
depending on how good the model is. R is the mea-
surement error covariance matrix and is representing
the noise over the measurements. It can, just like Q,
be adjusted according to the quality expected from
the measured values. The parameters chosen for both
matrix Q and R were the following:
Q[0] =












(10
−
4
) 0 0 0 0 0 0
0 (10
−
4
) 0 0 0 0 0
0 0 (10
−
4
) 0 0 0 0
0 0 0 (10
−
4
) 0 0 0
0 0 0 0 (10
−
4
) 0 0
0 0 0 0 0 (10
−
4
) 0
0 0 0 0 0 0 9












(27)
and
R[0] =


10 0 0
0 10 0
0 0 10

 . (28)
All the parameters were carefully chosen in order to
obtain the best results possible from the simulations
and therefore minimize the estimation error.
A very important part of this section is the noise
that was added to d[k]. Our goal here was to both
bring the simulation closer to what is a real envi-
ronment but also to enhance the performance of the
filter by exciting the parameters in d[k]. The noise
was induced, not through addition but with the tran-
sition of the parameters of d[k] to spherical coor-
dinates(azimuth, elevation). This transition allowed
us to add to the parameters a random variable with
Gaussian distribution and zero mean values between
[−1o
, 1o
]. The parameters were then converted back
to Cartesian coordinates. The improvement verified
on the results was considerable.
5 Results and Discussion
On this Section we present the plots with the results
from the simulations ran on the previously described
filter.
3

4. 0 100 200 300 400 500 600 700 800 900 1000
Time
-200
-150
-100
-50
0
50
Realposition-Estimatedposition
x
y
z
Figure 2: Position error for 1000 iterations.
500 550 600 650 700 750 800 850 900 950 1000
Time
-1
-0.5
0
0.5
1
1.5
2
2.5
3
3.5
4
Realposition-Estimatedposition
x
y
z
Figure 3: Position error for 500 iterations.
4

5. 0 100 200 300 400 500 600 700 800 900 1000
Time
-0.2
0
0.2
0.4
0.6
0.8
1
1.2
1.4
1.6
Realbias-Estimatedbias
x
y
z
Figure 4: Velocity bias error for 1000 iterations.
500 550 600 650 700 750 800 850 900 950 1000
Time
-0.01
-0.005
0
0.005
0.01
0.015
0.02
0.025
Realbias-Estimatedbias
x
y
z
Figure 5: Velocity bias error for 500 iterations.
5

6. 0 100 200 300 400 500 600 700 800 900 1000
Time
-20
0
20
40
60
80
100
120
140
Realrange-Estimatedrange
Figure 6: Range error for 1000 iterations.
500 550 600 650 700 750 800 850 900 950 1000
Time
-0.5
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
Realrange-Estimatedrange
Figure 7: Range error for 500 iterations.
6

7. On figures 2, 4 and 6 we have the error throughout
1000 iterations. On figures 3, 5, and 7, we have the
error throughout 500 iterations. We can see, after
looking at the plots presented above, that the results
presented by the implemented Kalman filter are con-
verging to the real values. We present two possible
ways of improving the results obtained. One of them
would be to use the Extended Kalman filter as it is
a better option to face the non linearity encountered
on Section 2.2. The other way of improving the ob-
tained results would be to find even better parameters
for the matrices P, Q and R, since they were man-
ually chosen, it will always be possible to find more
adequate ones.
6 Conclusion
Taking the results under considerations we can state
the implementation of the filter was a success. We
can state that the filter guarantees the stability and
convergence of the error dynamics. We could not
state that same for the Extended Kalman filter even
though it could have been a better option for reasons
stated above. To conclude, we can state that the filter
encounter some limitations when used for underwater
applications.
7 Acknowledgements
We would like to thank Prof. Carlos Silvestre, Prof.
Rui Martins and Eng. Joel Reis for the opportunity,
challenge and support given to us during the develop-
ment of this project. Thank you as well to Dr. David
Cabecinhas, Eng. Daniel Viegas and Eng. Wei Xie
for the constant help provided to us throughout the
time we spent in the Laboratory.
7