1 of 21

Aerial Robotics

State Estimation: Bayesian Estimation – Kalman Filter

C. Papachristos

Robotic Workers (RoboWork) Lab

University of Nevada, Reno

CS-491/691

2 of 21

Probabilistic Robotics

Uncertainty defines the State Estimation Process

Uncertain Sensor observations
Uncertain robot Models
Uncertain Prior knowledge

Notation(s) :

Probabilistic sensor models:

Probabilistic motion models:

Fusion of multiple sensors:

Sensor & Model fusion over time:

State

Measurement

Input

CS491/691 C. Papachristos

3 of 21

Probabilistic Robotics

C. Papachristos

4 of 21

Probabilistic Robotics

A Function /Table of Random Variables - Integrates to 1

Specific Probability Values

Pick a 10 of Diamonds (card is 10 AND card is Diamonds)

UNCONDITIONED Probability: Pick a 10 (or Pick a Diamond …)

Probability “DISTRIBUTION” of card Numbers and Shapes

C. Papachristos

Conditional Probability (or Likelihood):

GIVEN that we picked a Diamond, Probability of being a 10

Probability of being 10 of Diamonds = Probability of being a 10 GIVEN that it is a Diamond * Probability of being a Diamond




…	…	…	…

D H S C

1

2

3

…

1/52

5 of 21

Probabilistic Robotics

Probability�of State given Observation

Marginal Probability of State

Marginal Probability of Observation




…	…	…	…

D H S C

1

2

3

…

1/52

6 of 21

Probabilistic Robotics

C. Papachristos

3-Level Thin Obstacle Detection Sensor

Probability�of State given Observation

Marginal Probability of State

Marginal Probability of Observation

7 of 21

Bayes Filter

Markov Chain

Markov Property:

Conditional Probability Distribution of future states of a process (conditional on both past and present states) depends only upon the present state, i.e. :

Observations depend only on current state

[If action is available, future state depends�only on current state and current action]

Markov Chain:

A discrete-time stochastic process satisfying the Markov property

CS491/691 C. Papachristos

8 of 21

Bayes Filter

HMM: State variable

isn't observed, only�a noisy measurement of it is observed)

(General Description – given Conditional Independences from Markov Process)

CS491/691 C. Papachristos

9 of 21

Bayes Filter

CS491/691 C. Papachristos

10 of 21

Kalman Filter

Bayes Filter for Multivariate Normal PDFs

Univariate Normal (Gaussian) Distribution:

Multivariate Normal (Gaussian) Distribution:

Probability Density Function

CS491/691 C. Papachristos

11 of 21

Kalman Filter

Bayes Filter for Multivariate Normal Distributions

Linear Transformation of Gaussian Distribution:

Product of two Gaussian Probability Density Functions

(Note 1: Not the Distribution of the product�of the 2 Random Variables themselves (!),�but the product of the PDFs of the two RVs)

CS491/691 C. Papachristos

12 of 21

Kalman Filter

Bayes Filter for Multivariate Normal Distributions

Assuming a Discrete Time Stochastic Process that follows the Markov Property

Assuming the state Probability Density Function is Gaussian:

Assuming that it evolves according to a Linear Process Model:

Note: These are the Gauss-Markov Assumptions

such that Ordinary Least Squares provide the

Best, Linear, Unbiased Estimation methodology�(BLUE)

CS491/691 C. Papachristos

13 of 21

Kalman Filter

Bayes Filter for Multivariate Normal Distributions

Assuming a Discrete Time Stochastic Process that follows the Markov Property

Assuming the state Probability Density Function is Gaussian:

Assuming that it evolves according to a Linear Process Model:

Assuming that the Measurement Model is also Linear:

CS491/691 C. Papachristos

14 of 21

Kalman Filter

Bayes Filter for Multivariate Normal Distributions

Recursive Bayes Estimator

State Prediction:

State Update:

Kalman Assumptions:

CS491/691 C. Papachristos

15 of 21

Kalman Filter

Bayes Filter for Multivariate Normal Distributions

Applied, gives the Kalman Filter Predict & Update (/Correct) steps:

Kalman Prediction:

Kalman Update:

Note: Prediction & Correction steps can take place in various orders� depending on the Markov Chain

CS491/691 C. Papachristos

16 of 21

Kalman Filter

where

(1)

(2)

(1)

CS491/691 C. Papachristos

17 of 21

Kalman Filter

(1)

(2)

Solve to yield “Kalman Gain”

CS491/691 C. Papachristos

18 of 21

Kalman Filter

Kalman Filter – Recursive Estimation

Final Notes:

Statistics – Maximum Likelihood Estimation (MLE): Estimate unknown parameters of a statistical model by constructing a�(log-)likelihood function of the Joint Distribution of the data, then maximizing this function over all possible parameter values
Statistics – Ordinary Least Squares (OLS): Linear least squares method for estimating the unknown parameters in a linear regression model. Under Markov Assumptions, it is the Optimal (Best) Linear Unbiased Estimator (BLUE)

OLS under additional assumption for Normally-distributed errors, is identical to the MLE !�Assuming a Multivariate Normal Distribution, the construction of the Log-Likelihood function of the Joint Distribution of data in order to perform MLE turns out to yield an equivalent form as the OLS method

Prediction

Correction

Project State Ahead:

Project Error Covariance Ahead:

Update Error Covariance:

Update Estimate with Measurement:

Compute Kalman Gain:

CS491/691 C. Papachristos

19 of 21

Kalman Filter

Kalman Filter – Recursive Estimation

Final Notes:

Kalman Filter is Recursive, adheres by Markov Assumptions, assumes Normally-Distributed errors in state, process, measurement, resulting Joint Distributions are Multivariate Normal Distributions

Recursive OLS and Kalman-Filter MLE coincide if a conditional (log-)likelihood function is used

But, when the model is time varying, MLE estimates are obtained with mis-specified errors�These are not asymptotically equivalent to those of the correct model

Thus the Kalman-Filter estimates are not Best Linear MSE (Mean Squared Error) ones

Prediction

Correction

Project State Ahead:

Project Error Covariance Ahead:

Update Error Covariance:

Update Estimate with Measurement:

Compute Kalman Gain:

CS491/691 C. Papachristos

20 of 21

Kalman Filter

Kalman Filter – Recursive Estimation

Final Notes:

When the models are nonlinear, the Extended Kalman filter (EKF) works by linearizing them.

Then A and H represent the Jacobian matrices of partial derivatives
Effectively, propagations are calculated based on “first-order” linearizations of the nonlinear system

So: Distributions of the Random Variables are no longer normal after the respective nonlinear transformations

EKF does not work well when the model is highly non-linear, but another variant, the�Unscented Kalman filter (UKF – family of Sigma-Point Kalman Filters), which uses a Monte Carlo-based�approach to calculate updates, works better

Prediction

Correction

Project State Ahead:

Project Error Covariance Ahead:

Update Error Covariance:

Update Estimate with Measurement:

Compute Kalman Gain:

21 of 21

Time for Questions !

CS-491/691

CS491/691 C. Papachristos