Autonomous Robots: Kalman Filter

Assignments for my Udemy course on Kalman Filters.

Course link: www.udemy.com/course/autonomous-robots-kalman-filter/

Required Packages
python=3.7.4
numpy=1.16.4
matplotlib=3.1.0

Kalman Filter

Detailed infomation here !

- represent the estimated state as a probability distribution

- Represent the measurements or obervations of the current state (or function of the state) as a probability distribution

- Fuse the two distributions to get a better estimate (Bayes Theorem)

- Prediction process increase the uncertainity with time

- Update/Measurement process decrease the uncertainity

- Kalman Filter allows use to do this numerically and mathematically simply, by making a few assumptions about the probabilty distributions and a few other properties of the dynamic system

First we design 5 matrix below and then we used them in 2 step:

1. Predict new state and state uncertainity

2. Measure and update state and state uncertainity

x = State Vector

-> Contains all the states we are interested in or that are need to solve given problem

-> State can be position, speed, acceleration etc.

-> Size of the matrix is n x 1, where n is number of states

-> At the beginning we populate this matrix with some initial conditions

P = Uncertainty Matrix

-> Tells us how much uncertain we are about individual states

-> Uncertainty corresponding to states are being filled on to diagonal

-> Uncertainty 0 means that we are 100% sure about given state

-> Size of the matrix is n x n, where n is number of states

-> At the begging we populate this matric with information about how much uncertainty we are about given states

F = State Transition Matrix

-> Tells us how we can get from current state to next state i.e how to get from x_t to x_t+1

-> Size of the matrix is n x n, where n is number of states

H = Measurement Matrix

-> Tells us which measurement correspond to which state

-> Usually consist only 0 or 1, meaning measurement correspond to state = 1 or do not = 0

-> Size of the matrix is m x n, where m is number of measurements and n is number of states

R = Measurement Uncertainty

-> Tells us how much uncertain we are about individual measurement

-> Uncertainty corresponding to measurement are being filled on to diagonal

-> Uncertainty 0 means that we are 100% sure about given measurement

-> Size of the matrix is m x m, where m is number of measurements

-> At the begging we populate this matric with information about how much uncertainty we are about measurements

Predict

x = Fx

P = FPF^T

Measure and Update

Z = Measurements

y = Z^T - Hx

S = HPH^T + R

K = PH^TS^-1

x = x + Ky

P = (I - KH)P (I = Identity matric)

Assignment 1: Toy Implementation

-> Intro to localization and principle of Kalman filter using simple model of car in 1D spacer
-> Used to explain prediction of speed based on collected data and new measurement. i.e. The speed cannot change abruptly

Assignment 2: 1D Kalman Filter

-> Same problem as in assignment 1, but this time Linear Kalman filter is used to localize car
-> Explains how to setup Kalman filter and what individual matrices are used for

Assignment 2: 2D Kalman Filter

-> Goal is to localize car as precise as possible while driving on a street with turns and traffic lights
-> Program will not only recieve measurement but also vector U (user input)

Assignment 3: Traffic light prediction

-> In this assignment car is approaching intersection and at some point traffic light will change to red
-> Car need to decide if it will make to other side. And based on this prediction needs to decide to stop or continue
-> In first scenario car makes prediction based on current state
-> In second scenario car makes prediction based on current state but also on fact that it is allowed to raise speed for 1 sec