$30.00

Category: 24-677

Description

5/5 - (6 votes)

24-677 Special Topics: Linear Control Systems

1 Introduction

In this project, you will design an EKF SLAM to estimate the position and heading of the

vehicle.

In this final part of the project, you will design and implement an Extended Kalman

Filter Simultaneous Localization and Mapping (EKF SLAM). In previous parts,

we assume that all state measurements are available. However, it is not always true in the

real world. Localization information from GPS could be missing or inaccurate in a tunnel,

or closed to tall structures in an urban environment. In this case, we do not have direct

access to the global position X, Y and heading ψ and have to estimate them from ˙x, ˙y, ψ˙

on the vehicle frame and range and bearing measurements of map features. In fact, given

that we will always need CV / LiDAR for obstacle avoidance, we can always use local

measurements to improve the accuracy of our position estimate by augmenting GPS with

landmark information.

Consider the discrete-time dynamics of the system:

Xt+1 = Xt + δtX˙

t + ω

x

t

Yt+1 = Yt + δtY˙

t + ω

y

t

ψt+1 = ψt + δtψ˙

t + ω

ψ

t

(1)

Substitute X˙

t = ˙xt cos ψt − y˙t sin ψt and Y˙

t = ˙xt sin ψt + ˙yt cos ψt

in to (1),

Xt+1 = Xt + δt( ˙xt cos ψt − y˙t sin ψt) + ω

x

t

Yt+1 = Yt + δt( ˙xt sin ψt + ˙yt cos ψt) + ω

y

t

ψt+1 = ψt + δtψ˙

t + ω

ψ

t

,

(2)

with δt the discrete time step. The input ut

is [ ˙xt y˙t ψ˙

t

]

T

. Let pt = [Xt Yt

]

T

. Suppose

you have n map features at global position mj = [mj

x mj

y

]

T

for j = 1, …, n. The ground

truth of these map feature positions are static but unknown, meaning they will not move

but we do not know where they are exactly. However, the vehicle has both range and

bearing measurements relative to these features. The range measurement is defined as the

distance to each feature with the measurement equations y

j

t,distance = kmj − ptk + v

j

t,distance

for j = 1, …, n, with v

j

t,distance representing the measurement noise. The bearing measure is

defined as the angle between the vehicle’s heading (yaw angle) and ray from the vehicle to the

feature with the measurement equations y

j

t,bearing = atan2(mj

y − Yt

, mj

x − Xt) − ψt + v

j

t,bearing

for j = 1, …, n, where v

j

t,bearing is the measurement noise.

2

Let the state vector be

xt =

Xt

Yt

ψt

m1

x

m1

y

m2

x

m2

y

.

.

.

mn

x

mn

y

(3)

The measurement system be

yt =

km1 − ptk

.

.

.

kmn − ptk

atan2(m1

y − Yt

, m1

x − Xt) − ψt

.

.

.

atan2(mn

y − Yt

, mn

x − Xt) − ψt

+

v

1

t,distance

.

.

.

v

n

t,distance

v

1

t,bearing

.

.

.

v

n

t,bearing

(4)

In class we derived the full equations for the EKF SLAM problem – in this project you

will implement them.

3

2 P4: Problems [Due May 6, 2021]

Exercise 1. Before implementing the EKF in the simulator we should practice on some

simple Kalman filter problems. Complete the following using Matlab for all computations.

1. Consider the SISO system

xk+1 =

1 1

0 1

xk +

0

1

wk

yk =

1 0

xk + vk,

with wk a zero mean Gaussian noise with variance 0.1 and vk a zero mean Gaussian

noise with variance 0.01.

Starting from a random initial condition, solve the Kalman filter problem over the first

100 steps. Plot the mean-square current state prediction error vs. k in one graph, and

the estimated states and predicted states on a second graph.

2. A kinematic Kalman filter (KKF) is often used to fuse accelerometer measurements

with encoder readings. The benefit is that the measurements are related purely kinematically, i.e. the plant dynamics do not affect the estimator. Using a zero order hold

approximation to the double integrator kinematics we have

xk+1 =

1 T

0 1

xk +

T

2

2

T

(ak + wk),

where T is the sampling period, ak is the true acceleration at time k, and wk is the

accelerometer noise. We measure the position directly from the encoder which has its

own associated noise.

yk =

1 0

xk + vk,

where vk is the encoder noise which can reasonably approximated by vk =

q

2

12 and q is

the encoder quantization level.

Using a sampling period of 0.002, an accelerometer noise variance of 2.5, and an encoder

noise variance of 2×10−4

, solve the Kalman filter problem over 1 second. Simulate the

system and plot the true position state, position state estimated with the KKF, and

direct encoder position measurement on the same graph.

4

WhatsApp us