24677 Project 4 Solved

1 Introduction

In this project, you will complete the following goals:
1. Design a EKF SLAM to estimate the position and heading of the vehicle.

[Remember to submit the write-up, plots, and codes on Gradescope.]

2

2 P4: Problems

Exercise 1. 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 the tunnel, or closed to tall infrastructures. In this case, we do not have direct access to the global position X, Y and heading ψ and have to estimates them from x ̇, y ̇, ψ ̇ on the vehicle frame and range and bearing measurements of map features.

Consider the discrete-time dynamics of the system:

Xt+1 =Xt +δtX ̇t +ωtx Yt+1 = Yt + δtY ̇t + ωty ψt+1 = ψt + δtψ ̇t + ωtψ

SubstituteX ̇t =x ̇tcosψt −y ̇tsinψt andY ̇t =x ̇tsinψt +y ̇tcosψt into(1), Xt+1 =Xt+δt(x ̇tcosψt−y ̇tsinψt)+ωtx

Yt+1 =Yt+δt(x ̇tsinψt+y ̇tcosψt)+ωty ψt+1 = ψt + δtψ ̇t + ωtψ

δt is the discrete time step. The input ut is [x ̇t y ̇t ψ ̇t]T . Let pt = [Xt

(1)

(2)

Yt]T . Suppose

you have n map features at global position mj = [mjx mjy]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 yj = ∥mj − pt∥+vj for

j = 1, …, n. 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 yj =

atan2(mj −Y ,mj −X )−ψ +vj
y t x t t t,bearing

the measurement noises. Let the state vector be

for j = 1,…,n, where the vj t,distance

 Xt   Yt 

 ψt 

 m1  x  m1  y

t,bearing

and vj are t,bearing

(3)

xt=m2  x  m2   .y 

 .   m nx  mny

3

t,distance t,distance

The measurement system be



 

yt= 



∥m1−p∥

 v1  t,distance

t

.
. . 

nn
∥m − pt∥   vt,distance 

.

1 1 +1  (4) atan2(my −Yt,mx −Xt)−ψt   vt,bearing 

..
.   . 

atan2(mn −Y ,mn −X )−ψ vn ytxtt t,bearing

Derive Ft and Ht for EKF SLAM to estimate the vehicles state X, Y , ψ and feature positions mj = [mjx mjy ]T simultaneously.

Hints:

• write out the complete dynamical system with the state in (3) and then follow the standard procedure of deriving EKF. Think what is the dynamic for static landmarks?

4

Exercise 2. For this exercise, you will implement EKF SLAM in the ekf slam.py file by completing four functions (marked with TODO in the script). You can use the same con- troller from your previous parts or write a new one. Before integrating this module with Webots, you can run the script by $ python ekf slam.py to test your implementation. Feel free to write your own unit-testing scripts. You should not use any existing Python package that implements EKF. [Hints: remember to wrap heading angles to [−π, π]]

Check the performance of your controller by running the Webots simulation. You can press the play button in the top menu to start the simulation in real-time, the fast-forward button to run the simulation as quickly as possible, and the triple fast-forward to run the simulation without rendering (any of these options is acceptable, and the faster options may be better for quick tests). If you complete the track, the scripts will generate a performance plot via matplotlib. This plot contains a visualization of the car’s trajectory, and also shows the variation of states with respect to time.

Submit your controller ekf slam.py, ekf slam.py, and the final completion plot as de- scribed on the title page. You do not need to submit the plot generated by running the test script (by running $ python ekf slam.py). Your controller is required to achieve the following performance criteria to receive full points:

1. Time to complete the loop = 250 s
2. Maximum deviation from the reference trajectory = 10.0 m 3. Average deviation from the reference trajectory = 5 m

Debugging tips:

• Do not hardcode the number of map features. Instead, use n in your code.
• Check all the signs carefully.
• Check all the numpy array indexing.
• Use wrap to pi function smartly. Only wrap the angle terms but not the distance terms.
• When you run $ python ekf slam.py, it is normal if the estimation diverge gradually, but you should have some reasonable tracking performance.

5

3 Appendix (Already covered in P1)

where ay = dt2

inertial

Figure 1: Bicycle model[2]

Figure 2: Tire slip-angle[2]

We will make use of a bicycle model for the vehicle, which is a popular model in the study of vehicle dynamics. Shown in Figure 1, the car is modeled as a two-wheel vehicle with two degrees of freedom, described separately in longitudinal and lateral dynamics. The model parameters are defined in Table 2.

3.1 Lateral dynamics

Ignoring road bank angle and applying Newton’s second law of motion along the y-axis: may =Fyf cosδf +Fyr

􏰀d2y􏰁

is the inertial acceleration of the vehicle at the center of geometry in the direction of the y axis, Fyf and Fyr are the lateral tire forces of the front and rear

6

wheels, respectively, and δf is the front wheel angle, which will be denoted as δ later. Two terms contribute to ay: the acceleration y ̈, which is due to motion along the y-axis, and the centripetal acceleration. Hence:

a y = y ̈ + ψ ̇ x ̇
Combining the two equations, the equation for the lateral translational motion of the vehicle

is obtained as:

y ̈=−ψ ̇x ̇+m1(Fyf cosδ+Fyr)
Moment balance about the axis yields the equation for the yaw dynamics as

ψ ̈Iz =lfFyf −lrFyr

The next step is to model the lateral tire forces Fyf and Fyr. Experimental results show that the lateral tire force of a tire is proportional to the “slip-angle” for small slip-angles when vehicle’s speed is large enough – i.e. when x ̇ ≥ 0.5 m/s. The slip angle of a tire is defined as the angle between the orientation of the tire and the orientation of the velocity vector of the vehicle. The slip angle of the front and rear wheel is

αf =δ−θVf αr =−θVr

where θV p is the angle between the velocity vector and the longitudinal axis of the vehicle, for p ∈ {f, r}. A linear approximation of the tire forces are given by

􏰄 y ̇ + l f ψ ̇ 􏰅 Fyf=2Cα δ− x ̇

􏰄 y ̇−lrψ ̇􏰅 Fyr=2Cα − x ̇

where Cα is called the cornering stiffness of the tires. If x ̇ < 0.5 m/s, we just set Fyf and Fyr both to zeros.

3.2 Longitudinal dynamics

Similarly, a force balance along the vehicle longitudinal axis yields:

x ̈ = ψ ̇ y ̇ + a x max = F − Ff

Ff =fmg
where F is the total tire force along the x-axis, and Ff is the force due to rolling resistance

at the tires, and f is the friction coefficient.

7

3.3 Global coordinates

In the global frame we have:

3.4 System equation

X ̇ =x ̇cosψ−y ̇sinψ Y ̇ =x ̇sinψ+y ̇cosψ

Gathering all of the equations, if x ̇ ≥ 0.5 m/s, we have:

2 C 􏰄 y ̇ + l ψ ̇ 􏰅 y ̈=−ψ ̇x ̇+α(cosδδ− f − r)

y ̇ − l ψ ̇ m x ̇ x ̇

x ̈ = ψ ̇y ̇ + m1 (F − fmg)
2lC􏰄 y ̇+lψ ̇􏰅2lC􏰄y ̇−lψ ̇􏰅

ψ ̈= f α δ− f − r α − r Iz x ̇ Iz x ̇

X ̇ =x ̇cosψ−y ̇sinψ Y ̇ =x ̇sinψ+y ̇cosψ

otherwise, since the lateral tire forces are zeros, we only consider the longitudinal model.

3.5 Measurements

The observable states are:

3.6 Physical constraints

The system satisfies the constraints that:

 x ̇ 

 y ̇ 

  ψ ̇   y = X  Y 

ψ

|δ|􏰆π6 rad
F 􏰇0andF 􏰆15736N x ̇ 􏰇 10−5 m/s

8

Name

δ or δf F
m

Cα Iz

Fpq
f delT

Table 1: Model parameters. Unit

Description

Front wheel angle Total input force Vehicle mass

Cornering stiffness of each tire Yaw intertia
Tire force, p ∈ {x, y},q ∈ {f, r} Rolling resistance coefficient Simulation timestep

rad N kg

N
kg mˆ2 N
N/A sec

Value

State Input 1888.6

20000
25854
Depends on input force 0.019
0.032

3.7 Simulation

Figure 3: Simulation code flow

Several files are provided to you within the controllers/main folder. The main.py script initializes and instantiates necessary objects, and also contains the controller loop. This loop runs once each simulation timestep. main.py calls your controller.py’s update method

9

on each loop to get new control commands (the desired steering angle, δ, and longitudinal force, F). The longitudinal force is converted to a throttle input, and then both control commands are set by Webots internal functions. The additional script util.py contains functions to help you design and execute the controller. The full codeflow is pictured in Figure 3.

Please design your controller in the your controller.py file provided for the project part you’re working on. Specifically, you should be writing code in the update method. Please do not attempt to change code in other functions or files, as we will only grade the relevant your controller.py for the programming portion. However, you are free to add to the CustomController class’s init method (which is executed once when the CustomController object is instantiated).

3.8 BaseController Background

The CustomController class within each your controller.py file derives from the Base- Controller class in the base controller.py file. The vehicle itself is equipped with a Webots-generated GPS, gyroscope, and compass that have no noise or error. These sensors are started in the BaseController class, and are used to derive the various states of the vehicle. An explanation on the derivation of each can be found in the table below.

Name (X, Y ) (x ̇,y ̇) ψ

ψ ̇
3.9 Trajectory Data

Table 2: State Derivation.

Explanation
From GPS readings
From the derivative of GPS readings From the compass readings
From the gyroscope readings

The trajectory is given in buggyTrace.csv. It contains the coordinates of the trajectory as (x, y). The satellite map of the track is shown in Figure 4.

10

4 Reference

Figure 4: Buggy track[3]

1. Rajamani Rajesh. Vehicle Dynamics and Control. Springer Science & Business Media, 2011.
2. Kong Jason, et al. “Kinematic and dynamic vehicle models for autonomous driving control design.” Intelligent Vehicles Symposium, 2015.
3. cmubuggy.org, https://cmubuggy.org/reference/File:Course_hill1.png
4. “PID Controller – Manual Tuning.” Wikipedia, Wikimedia Foundation, August 30th,

   2020. https://en.wikipedia.org/wiki/PID_controller#Manual_tuning

11