Autonomous Robots Lab
  • Home
  • News
  • Research
    • Autonomous Navigation and Exploration
    • Fixed-Wing UAVs
    • Agile and Physical Interaction Control
    • Localization and 3D Reconstruction
    • Subterranean Robotics
    • Collision-tolerant Aerial Robots
    • Marine Robotics
    • Intelligent Mobility >
      • Student Projects
      • Electric Bus Datasets
    • Robotics for Nuclear Sites
    • Degerminator
    • Autonomous Robots Arena
    • Code
    • Media
    • Research Presentations
    • Projects
  • Publications
  • Group
    • People
    • Research Collaborators
    • Positions
  • Education
    • Introduction to Aerial Robotics >
      • Online Textbook >
        • Modeling >
          • Frame Rotations and Representations
          • Multirotor Dynamics
        • State Estimation >
          • Inertial Sensors
          • Batch Discrete-Time Estimation
          • The Kalman Filter
        • Flight Control >
          • PID Control
          • LQR Control
          • Linear Model Predictive Control
        • Motion Planning >
          • Holonomic Vehicle BVS
          • Dubins Airplane
          • Collision-free Navigation
          • Structural Inspection Path Planning
        • Simulation Tools >
          • Simulations with SimPy
          • MATLAB & Simulink
          • RotorS Simulator >
            • RotorS Simulator Video Examples
      • Lecture Slides
      • Literature and Links
      • RotorS Simulator
      • Student Projects
      • Homework Assignments
      • Independent Study
      • Video Explanations
      • Syllabus
      • Grade Statistics
    • Autonomous Mobile Robot Design >
      • Lecture Slides
      • Semester Projects
      • Code Repository
      • Literature and Links
      • RotorS Simulator
      • Video Explanations
      • Resources for Semester Projects
      • Syllabus
    • Robotics for DDD Applications
    • CS302 - Data Structures
    • Student Projects >
      • Robot Competitions
      • Undergraduate Researchers Needed
      • ConstructionBots - Student Projects
    • EiT TTK4854 - Robotic Ocean Waste Removal
    • Aerial Robotic Autonomy >
      • Breadth Topics
      • Deep-dive Topics
      • Literature
    • Robotics Seminars
    • Robotics Days
    • Outreach >
      • Drones Demystified! >
        • Lecture Slides
        • Code Repository
        • Video Explanations
        • RotorS Simulator
        • Online Textbook
      • Autonomous Robots Camp >
        • RotorS Simulator
      • Outreach Student Projects
    • BadgerWorks >
      • General Study Links
      • Learn ROS
      • SubT-Edu
  • Resources
    • Autonomous Robots Arena
    • Robot Development Space
  • Contact

The Kalman Filter

Basic Introduction to Kalman Filtering. The basic Kalman Filter structure is explained and accompanied with a simple python implementation.

Kalman Filter Basic Intro

Introduction

The Kalman Filter (KF) is a set of mathematical equations that when operating together implement a predictor-corrector type of estimator that is optimal in the sense that it minimizes the estimated error covariance when some presumed conditions are met.

Mathematical Formulation

The KF addresses the general problem of trying to estimate the state x∈Rn of a discrete-time controlled process that is governed by the linear stochastic difference equation:

xk=Axk−1+Buk+wk−1

with a measurement y∈Rm that is:

yk=Hxk+vk

The random variables wk and vk represent the process and measurement noise respectively. They are assumed to be independent of each other, white, and with normal probability distributions:

p(w)≈N(0,Q)
p(v)≈N(0,R)

The n×n matrix A relates the state at the previous time step to the state at the current step, in the absence of either a driving input or process noise. The n×l matrix B relates the control input u∈Rl to the state x. The m×n matrix H in the measurement equation relates the state to the measurement yk.

How the KF works

The KF process has two steps, namely:
* Prediction step: the next step state of the system is predicted given the previous measurements
* Update step: the current state of the system is estimated given the measurement at that time step
These steps are expressed in equation-form as follows:

Prediction
Xk−=Ak−1Xk−1+BkUk
Pk−=Ak−1Pk−1ATk−1+Qk−1

Update
Vk=Yk−Hk−X−k
Sk=HkPk−HTk+Rk
Kk=Pk−HTkSk−1
Xk=Xk−+KkVk
Pk=Pk−−KkSkKTk

where:
* Xk− and Pk− are the predicted mean and covariance of the state, respectively, on the time step k before seeing the measurement.
* Xk and Pk are the estimated mean and covariance of the state, respectively, on time step k after seeing the measurement.
* Yk is the mean of the measurement on time step k.
* Vk is the innovation or the measurement residual on time step k.
* Sk is the measurement prediction covariance on the time step k.
* Kk is the filter gain, which tells how much the predictions should be corrected on time step k.

Python Implementation

File: KalmanFilter_Basic.py
from numpy import *
import numpy as np
from numpy.linalg import inv
from KalmanFilterFunctions import *

# time step of mobile movement
dt = 0.1

# Initialization of state matrices
X = array([[0.0], [0.0], [0.1], [0.1]])
P = diag((0.01, 0.01, 0.01, 0.01))
A = array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]])
Q = eye(X.shape[0])
B = eye(X.shape[0])
U = zeros((X.shape[0],1))

# Measurement matrices
Y = array([[X[0,0] + abs(random.randn(1)[0])], [X[1,0] + abs(random.randn(1)[0])]])
H = array([[1, 0, 0 , 0], [0, 1, 0, 0]])
R = eye(Y.shape[0])

# Number of iterations in Kalman Filter
N_iter = 50

# Applying the Kalman Filter
for i in range(0, N_iter):
    (X, P) = kf_predict(X, P, A, Q, B, U)
    (X, P, K, IM, IS, LH)  = kf_update(X, P, Y, H, R)
    Y = array([[X[0,0] + abs(0.1 * random.randn(1)[0])], [X[1, 0] + abs(0.1 * random.randn(1)[0])]])
File: KalmanFilterFunctions.py
from numpy import dot, sum, tile, linalg, log, pi, exp
from numpy.linalg import inv, det

def kf_predict(X, P, A, Q, B, U):
    X = dot(A, X) + dot(B, U)
    P = dot(A, dot(P, A.T)) + Q
    return(X, P)

def gauss_pdf(X, M, S):
    if M.shape[1] == 1:
        DX = X - tile(M, X.shape[1])
        E = 0.5 * sum(DX * (dot(inv(S), DX)), axis=0)
        E - E + 0.5 * M.shape[0] * log(2 * pi) + 0.5 * log(det(S))
        P = exp(-E)
    elif X.shape[1] == 1:
        DX = tile(X, M.shape[1] - M)
        E = 0.5 * sum(DX * (dot(inv(S), DX)), axis =0)
        E = E + 0.5 * M.shape[0] * log(2 * pi) + 0.5 * log(det(S))
        P = exp(-E)
    else:
        DX = X - M
        E = 0.5 * dot(DX.T, dot(inv(S), DX))
        E = E + 0.5 * M.shape[0] * log(2 * pi) + 0.5 * log(det(S))
        P = exp(-E)
    return (P[0],E[0])

def kf_update(X, P, Y, H, R):
    IM = dot(H, X)
    IS = R + dot(H, dot(P, H.T))
    K = dot(P, dot(H.T, inv(IS)))
    X = X + dot(K, (Y-IM))
    P = P - dot(K, dot(IS, K.T))
    LH = gauss_pdf(Y, IM, IS)
    return (X,P,K,IM,IS,LH)

Proudly powered by Weebly