Localisation

Overview:

Localisation:

Localisation is the process of determining the position or the state of a robot with respect to the enviroment.

Provided Dataset:

Data provided in the data.pickle:

Variable	Description
t	timestamps for the measurements provided
x_init	initial x position of the bot
y_init	initial y position of the bot
th_init	initial θ of the bot
v	Odometry reading of the velocity
v_var	Variance in the odometry reading of the velocity
om	Odometry Reading of the angular velocity
om_var	Variance in the odometry reading of the angular velocity
l	Location of the landmarks [a b]. Where a is the x coordinate of the landmark and b is the y coordinate of the landmark
d	Distance between the center of the bot and the LiDAR sensor
b	Bearing measurement provided by the LiDAR sensor
b_var	Variance in the Bearing measurement of the LiDAR sensor
r	Range measurement provided by the LiDAR sensor
r_var	Variance in the Range measurement provided by the LiDAR sensor

Importing the data into the code:

import pickle
import numpy

with open('./data.pickle', 'rb') as f:
    data = pickle.load(f)

Extended Kalman Filter Algorithm:

Refer to Bayes Filter and Kalman Filter Algorithm if you are unfimiliar with Bayes Filter and Kalman Filter

Extended Kalman Filter is a Recursive Bayes Filter used for estimating the state of a robot by obtaining linear appoximation of the non-linear system.

Steps in State Estimation using Extended Kalman Filter:

Step 1: State Prediction

State Prediction involves predicting the state of the robot using only odmetry commands given to the motors of the robot.

The above equation is the motion model. This is used to predict the position of robot given the position (x, y, θ)^T at time t-1 and control commands (u,w)^T given to the robot at time t.

(x^', y^', θ^')^T is the predicted pose of the robot at time t.