Scribd
Upload
14 views

Projet

Uploaded by

Assimi Dembélé
Copyright
© © All Rights Reserved
Available Formats
Download as PDF, TXT or read online on Scribd
14 views

Projet

Uploaded by

Assimi Dembélé
Copyright
© © All Rights Reserved
Available Formats
Download as PDF, TXT or read online on Scribd
You are on page 1/ 5

Samy Labsir

Estimation and identification


Au-512, 5th year SET, IPSA

Project: Autonomous car tracking on an bend


with Kalman filter-based algorithm (∼ 4H)
Theoretical questions and Matlab code are due on Moodle to 24/11/24, 11h59
PM.

Tracking an autonomous car is a crucial task and presents significant challenges across
various applications, including urban environments and aeronautics, such as in aircraft in-
spection. Typically, autonomous vehicles are equipped with a range of sensors (radar, lidar,
ultrasound), which are used to continuously track the car’s position within its environment
as it moves. To address the tracking problem, one effective approach is to adopt a statistical
framework that accounts for measurement uncertainties. This is achieved by using Bayesian
filters, specifically the Kalman filter and its variants, which provide an analytical solution to
the problem.
The goal of this tutorial is to solve this tracking problem by implementing variants of the
Kalman filter taking into account the non-linearities of the problem. The implementation
will be carried out in MATLAB. Theoretical questions are marked with Th., while numerical
aspects are indicated with Num.

Part I: Starting up
We assume the existence of a robot moving in a indoor 2D environment and embedded
with an ultra-sound sensor. At each discrete instant k, its dynamic is characterized by its
position pk and its orientation θk .

It performs a task in such a way that it is forced to make a turn regularly. Consequently,
the discrete evolution model on its position pk is driven by the following discrete equation:
pk = pk−1 + R(θk−1 ) v δt + np np ∼ N (0, σp2 I2×2 ) (1)

page 1
where R(θk ) is the rotation matrix associated to the angle θk , v is the robot velocity
assumed constant. As far as concerns θk , it is controlled by the following linear model:

θk = θk−1 + w δt + nθ nθ ∼ N (0, σθ2 ) (2)

where w is the robot angular velocity assumed constant.

1) (Th.) Give the expression of R(θk ) and explain why the model (1) allows to take into
account a turn in the trajectory.

2) (Num.) Implement this equation for different instant k ∈ {1, . . . , K}. We can set:

• K = 100 • w = 0.01 rad s−1


• δt = 1 • v = [1, 1] m s−1
• p0 = [0, 0] m • σp = 0.01 m
• θ0 = 0.1 rad • σθ = 10−4 rad

Comment an interpret.

Additionally, we assume that the embedded ultrasound sensor can measure the distance
di,k to various points of interest at each time step k. Let’s consider there are N points
of interest in the environment, so we have:

di,k = ∥zi,k ∥ (3)

where zi,k is the coordinate of ith point of interest expressed in the robot local frame. It
is linked to the robot position in the global frame pk by:

zi,k = R(θk )pi + pk ∀i ∈ {1, . . . , N } (4)

and we measure:

di,k = ∥R(θk )pi + pk ∥ + ni,k ni,k ∼ N (0, σd2 ) (5)

We can refer to the figure 1 to illustrate the problem.

3) (Th.) To your opinion, how many ultra sound sensor measurements we need to invert
the model (5) ?

4) (Num.) Implement the equation (5), at each instant k, by taking advantage of the
evolution model coded in the question (2). We can set N = 3, p1 = [40, 10], p2 =
[80, 25], p3 = [120, 30], and σd = 0.1 m

5) (Num.) Draw the trajectory previously obtained superimposed to the set {pi }3i=1 .

page 2
Figure 1: Illustration of the ultrasound measurements process

Part II: Implementation of the tracking algorithm


In this part, we propose to estimate the dynamic parameters of the car by implementation
of two tracking algorithms based on the Kalman filter, the Iterated Extended Kalman
filter (IEKF) and the Extended Kalman Filter (EKF). Contrary to the EKF, the IEKF
preserves the non-linearities of the observation model for the resolution of the estimator.As
the prediction step is similar to the EKF, the challenge is to derive the correction step.The
latter consists in finding the estimated state vector x
bk|k verifying:

bk|k = argmax p(xk |z1:k )


x (6)
xk

The problem is addressed by examining two cases based on whether or not θk is known.
For both cases, we keep the same simulation parameters as previously.

Case I: θk known

In this case, the considered state vector is xk = pk .

1) (Th.) Write the models under the form:

xk = f (xk−1 ) + vk vk ∼ N (0, Qk ) (7)


zk = h(xk ) + nk nk ∼ N (0, Rk ) (8)

page 3
where f and h are to identify as well as Qk and Rk .

2) (Th.) Compute the Jacobian matrix of f and h according to pk . What do you observe
?

The correction step of the IEKF consists in finding the maximum of the equation (6).

8) (Th.) Show that this problem is equivalent to resolve the following optimization
problem:
bk|k = argmin ∥ϕ(xk )∥2Σϕ
x (9)
xk

where:

• ϕ is a smooth function depending on xk , x


bk|k−1 and z1:k
• Σϕ to identify.

The covariance matrix of p(xk |z1:k ) is determined by fitting a Gaussian distribution on


p(xk |z1:k ) with mean x
bk|k such as:

ϕ(xk ) ≃ ϕ(b
xk|k ) + Jϕ (xk − x
bk|k ) (10)

with Jϕ : Jacobian matrix of ϕ computed at x


bk|k .

9) (Th.) By identifying the relation between p(xk |z1:k ) and ∥ϕ(xk )∥2Σϕ , show that this
approximation allows to find:
 
p(xk |z1:k ) ≃∝ exp −0.5 ∥xk − x bk|k ∥Σk|k (11)

where Σk|k is to identify in function of Jϕ

10) (Th.) Write the equations of the correction step of the IEKF.

11) (Num.) Implement the IEKF correction in a function IEKFcorrectionstep.m, then


test the algorithm.

12) (Num.) Draw the instantaneous error estimation and the MSE at each instant k.
Interpret and comment by comparison with the results obtained from the EKF that
that you will have to implement in this question.

Case II: θk unknown (Bonus)


In this section, we assume that θk is integrated in the unknown parameters so that
⊤
xk = p⊤

k , θk . Then, the challenge here is to deal with the rotation matrix R(θk ) and
its Jacobian matrix.

page 4
13) (Th.) Write the new functions f (a) and h(a) such that:
(a)
xk = f (a) (xk−1 ) + v(a) v(a) ∼ N (0, Qk ) (12)
(a)
zk = h(a) (xk ) + n(a) n(a) ∼ N (0, Rk ) (13)

14) (Th.) Determine the new Jacobian matrices of f (a) and h(a) and the corresponding
(a) (a)
covariance matrices Qk and Rk .

15) (Num.) Use the results of the questions (12)-(13) to implement the new IEKF.

16) (Num.) Implement the new EKF and compare the precision performance.

page 5

You might also like