6/6/2011

Introduction to the Kalman Filter, Part II

Home Page

Login

Register

RSS

Home Contact About Oct 22 2010 Search:

Introduction to the Kalman Filter, Part II

Posted on 23:40, October 22nd, 2010 by Billy McCafferty As discussed in Part I, the Kalman filter is an optimal, prediction/correction estimation technique which minimizes estimated error and can be used a wide array of problems. While Part I focused on a one-dimensional problem space to more easily convey the underlying concepts of the Kalman filter, Part II will now expand the discussion to bring the technique to higher dimensions (e.g., 2D and 3D) while still being constrained to linear problems. Whats so cool about the Kalman filter you ask? Lets highlight a few areas where the Kalman filter may provide value. (This should help you remain motivated while youre delving into a myriad of Greek symbols and matrix transformations!) The Kalman filter can help: Determine the true pose of a mobile robot given its control input and measurements; Provide sensor fusion capabilities by further correcting an estimate with each subsequent measurement input; Track an object within a video (e.g., face tracking); and Determine the attitude of a satellite using measurement of star locations.

Tags
Algorithms

Architecture
DeepThoughtsByJackHandey

C++

Design Patterns Fundamentals

HardwareInt egration

Methodologies
PepperPicker

Planning

ROS Simulation TDD

Tools
Authors
Billy McCafferty (33)

sharprobotica.com//introduction-to-the

1/9

6/6/2011

Introduction to the Kalman Filter, Part II

locations. These few examples demonstrate just how useful the Kalman filter can be on a wide variety of problems. To limit the scope of discussions, the context of this tutorial will be on determining the true pose of a mobile robot given noisy control inputs and measurement data. In examining the Kalman filter a bit more, well discuss the following topics: Applicable systems for use of the Kalman filter Kalman filter algorithm, inputs and outputs Kalman filter algorithm formalized Limitations of the Kalman filter Direction for further study The current discussion will avoid the derivations of the associated, base equations in favor of pragmatic use of the filter itself. For a comprehensive derivation and discussion of the involved equations and mathematical roots, see Robert Stengels Optimal Control and Estimation. Applicable Systems for Use of the Kalman filter In Part I, it was discussed that a system must adhere to three constraints for Kalman filter applicability: 1) it must be describable by a linear model, 2) the noise within the model must be white, and 3) the noise within the model must be Gaussian. More precisely, the state must be estimable via:

The state estimation equation is broken down as follows: xk is the estimate of the state after taking into consideration the previous state, the control input, and process noise. xk-1 is the state estimate from the previous time iteration (k-1) where k is the current time iteration. The state will likely surely be a vector; e.g., (x y )T for a mobile robot on a 2D plane having a location (x, y) and orientation . uk is the control input for the current time iteration. While being sensor data in the strict sense, odometry measurements may be considered control input; and would be a good fit for our mobile robot example. Accordingly, control input could be represented as a three dimensional vector containing an initial rotation (in radians), a translation distance (distance travelled in a straight line), and a second rotation (rot1 trans rot2)T. wk-1 is the process noise, or the amount of error inherent in the state estimation
when taking into consideration noisy control input. (The process noise is
sharprobotica.com//introduction-to-the 2/9

Latest Posts
Using State Diagrams to Express Robot Behaviors & Transitions Using O-MaSE Methodology for Goal Modeling Planning-Domain Research Guidance STRIPS for Classical Plan

6/6/2011

Introduction to the Kalman Filter, Part II

when taking into consideration noisy control input. (The process noise is Guassian, having mean of 0 and covariance .) The process noise is a vector having the same dimension n as the state vector.

A is a matrix which transforms the previous state into the current state without regard to any control input. For example, if the system being modeled were a satellite orbiting the earth, A would be a matrix which would modify the state to reflect the orbital distance traveled between time iterations k and (k 1). In more earth-bound scenarios, A would be an identity matrix. Regardless, A is a square matrix of size (nxn) where n is the dimension of the state vector xk. B is a matrix which transforms the control input to be compatible for summing to the previous state to reflect the current state. Within our mobile robot context, the control input is (rot1 trans rot2)T which cannot simply be added to the previous state to determine the current state; accordingly, B must transform uk into a vector reflecting the relative state change induced by the control input. For example, if the control input were to move the robot 55 mm on the x axis, 120 mm on the y axis, and 0.8 rad, then the result of Buk would be (55mm 120mm 0.8 rad) which could then be easily summed to the previous state to get the current state. B is a matrix of size (nxl) where n is the dimension of the state vector and l is the dimension of the control vector. In addition to adhering to the linear state estimation just discussed, in order to be Kalman-filter-compatible, the system being modeled must also have a measurement model estimable via:

Representation and Planning InteRRaP Hybrid Architecture for Robotic & MultiAgent Systems Agent-Oriented Methodology Selection: O-MaSE Methodologies for the Design of Robotic and MultiAgent Systems Development of Multi-Agent Bell Pepper Picking Robot in Unstructured Horticulture Simulation as a Source of New Knowledge Serial Communications with SICK S300 Standard

The measurement equation is broken down as follows: zk is the predicted measurement model after taking into account the estimated state and known measurement noise. Dont take that at face value; ask yourself why its important to be able to predict what the measurement model will be for a given state. Spoiler alert If were able to predict what the measurement model should be for a given state, then we can compare the predicted measurement model against the actual measurement data returned by our sensors. The difference between the two will be used for improving the state estimation within the Kalman filter algorithm. For our little robot, the measurement model may be a vector of laser scans (s 1 s 2 s n)T, with each scan having an orientation and range (x )T. xk is the result of the state estimation equation discussed above. vk is the measurement noise, or the amount of error inherent in the measurement estimation when taking into consideration noisy sensor
sharprobotica.com//introduction-to-the

Latest Comments
Tim Kelsey on Including Boost.Test in Your C++ Project jigolo on Robot Operating System (ROS) Command Cheat Sheet Billy McCafferty on Serial Communications with SICK S300 Standard escort on Serial Communications with SICK S300 3/9

6/6/2011

Introduction to the Kalman Filter, Part II

input. (The measurement noise is Guassian, having mean of 0 and covariance .) The measurement noise is a vector having the same dimension n as the resulting measurement vector. H is a matrix which transforms the state xk into the predicated measurement model. In other words, if given the state xk, Hxk will calculate what the measurement model should look like if there were no uncertainty involved. H is of size (mxn) where m is the dimension of the measurement vector zk and n is the dimension of the state vector. And to answer your question, yes, H can be quite onerous to fabricate. In fact, H may actually be implemented as a function, accepting a state and returning a measurement model based on the known map, estimated location and orientation; the Extended Kalman filter or other extension would need to be leveraged if we digressed in this way from our simple linear model. In summary, if the system can be modeled by the the process and measurement equations described above, then the Kalman filter may be used on the system to estimate state, if given control and measurement inputs. Lets now look at the general Kalman filter algorithm, at a very high level, including specific inputs and outputs. Kalman Filter Algorithm, Inputs and Outputs The Kalman filter algorithm follows a surprisingly straight-forward algorithm broken down into two phases. The first phase is called the time estimate (or prediction) in which the previous state and control input is used to estimate the current state and estimate covariance. The second phase is called the measurement update (or correction) in which the Kalman gain is calculated and the state estimate and covariance is improved upon using measurement data and the Kalman gain. Roughly, the algorithm is as follows: 1. Estimate the predicted state based on process (control) input. This estimate is the a priori estimate. 2. Calculate the state estimate covariance (our confidence in the state estimate). 3. Calculate the Kalman gain which will be used for weighting the amount of correction the measurement data will have on the state estimate. 4. Refine the estimated state using measurement input. This refined estimate is the a posteriori estimate. 5. Refine the state estimate covariance (our confidence in the state estimate after taking measurement data into account). During the first time iteration t0, the Kalman filter accepts as input the initial state and estimate covariance (which may be zero if the initial state is known with 100% certainty) along with the control input u and measurement data z.
sharprobotica.com//introduction-to-the

with SICK S300 Standard Markus on Serial Communications with SICK S300 Standard Markus on Serial Communications with SICK S300 Standard Markus on Serial Communications with SICK S300 Standard Billy McCafferty on Development of Multi-Agent Bell Pepper Picking Robot in Unstructured Horticulture ian on Development of Multi-Agent Bell Pepper Picking Robot in Unstructured Horticulture Ray Renteria on Architectural Paradigms of Robotic Control

4/9

6/6/2011

Introduction to the Kalman Filter, Part II

On subsequent time iterations tn , the Kalman filter accepts as input the output from the previous run (with mean and covariance discussed more below) along with the control input u and measurement data z from t n. The output of the Kalman filter is an estimate of the state represented by a normal distribution having mean (the estimated state) and covariance (the confidence, or more accurately, the noise, in that estimate). (As a reminder, the covariance of a normal distribution is the standard deviation squared 2.) Note that need not be limited to a scalar value; in fact, itll almost always be a vector. For example, the pose of a mobile robot may be a three dimensional vector containing the location and orientation (x y )T.

Accordingly, this vector would be the resulting mean value. Furthermore, with a three dimensional state vector as the mean, the covariance would be a (33) diagonal matrix having a covariance for each corresponding value of the vector, as shown at right. Kalman Filter Algorithm Formalized Weve discussed the initial Kalman filter equations for process and measurement estimation; weve also discussed the overall algorithm for implementation, broken down into prediction and correction phases. Whats missing are the actual calculations for concretely carrying out the estimation process itself. The concrete calculations for implementing the Kalman filter algorithm are easily derived from the process and measurement equations by taking the partial derivatives of them and setting them to zero for minimizing errorand jumping around three times and standing on your head for minutes. (My eyes quickly begin to glaze over when I start to follow derivations of this naturebut if you like this kind of stuff, Sebastian Thrun shows the complete derivation within Probabilistic Robotics; Robert Stengel takes it to 11 within Optimal Control and Estimation with more Greek symbols than you can shake a stick at.) But I digress To formalize, the Kalman filter algorithm accepts four inputs: t-1 the mean state vector t-1 the covariance of the mean (the error in the state estimate) ut the process (control) input zt the measurement data With the given inputs, the Kalman filter algorithm is implemented as follows: 1.
sharprobotica.com//introduction-to-the 5/9

6/6/2011

Introduction to the Kalman Filter, Part II

2. 3. 4. 5. Line 1 should be comfortingly familiar; this is the calculation for estimating the current state given the previous state and control input. But whats missing from the original process equation? Have you spotted it yet? Ill give you a noisy hint. (No mean for the punthank you, thank you, Ill be here all week.) Thats right, the noise parameter has been left off of the state estimation equation in line 1. Line 1 simply calculates the a priori state estimate, ignoring process noise. Line 2 calculates the covariance of the current state estimate, taking process noise into consideration. Matrix A has already been discussed; it comes from the Kalman filter state estimate equation described earlier. R is a diagonal matrix representing the process noise covariance. Line 3 calculates the Kalman gain which will be used to weight the effect of the measurement model when correcting the estimate. C is identical to the matrix H described earlier in the base Kalman filter measurement equation. As tricky as this line looks (and some of those matrix calculations can make your head hurt a bit), the only thing new is Q; this diagonal matrix is the measurement noise covariance. The resulting Kalman gain K is a matrix having dimensions (nxm) where n is the dimension of the state vector and m is the dimension of the measurement vector. (As a side, take note that in different reading sources, the meaning of R and Q may be switched; Q would be process noise and R would be measurement noise and would have their locations in the equations swapped, accordingly. Just be cognizant of which is which within the source youre reading from.) Line 4 updates the state estimate taking into account the weighted measurement information. Note that the Kalman gain is multiplied by the difference between the actual measurement model and the predicted measurement model. What happens if they happen to be identical? Jeopardy daily double sounds playing in the background If the actual and predicted measurement models happen to be identical, then the estimated state will not be corrected at all since our sensors have verified that were exactly where we thought we were. I.e., dont fix what aint broken. The result of line 4 is the a posteriori state estimate. Finally, line 5 corrects the covariance, taking into account the Kalman gain
sharprobotica.com//introduction-to-the 6/9

6/6/2011

Introduction to the Kalman Filter, Part II

used to correct the state estimate. As output, the Kalman filter algorithm returns two values: t the current mean state vector. t the current covariance of the mean (the error in the state estimate). The covariance matrix would be a diagonal matrix having dimensions (nxn) where n is the dimension of the state vector. With these outputs, it is now known with some amount of error what the current state of the system is; or where our intrepid little robot is on the map. Limitations of the Kalman Filter The Kalman filter is incredibly powerful and can be used in a surprising number of scenarios. The primary limitation of the Kalman filter is that it assumes use within a linear system. Many systems are non-linear (such as a mobile robot moving with a rotational trajectory) yet may still benefit from the Kalman filter. The applicable approach is to form a linear estimate of the non-linear system for use by the Kalman filter; similar in effect to a Taylor series expansion. Popular extensions to the Kalman filter to support nonlinear systems include the Extended Kalman filter and, even better, the Unscented Kalman filter. Specifically, chapter 7 of Sebastian Thruns Probabilistic Robotics goes into good detail on describing how to apply both of these extensions to the context of mobile robotics. Googling for Kalman filter will quickly show just how much more there is to this topic. But I certainly hope this two part series has helped to clarify the overall algorithm with particular attention to describing the various elements of the calculations themselves. Enjoy! Billy McCafferty Written by: Billy McCafferty on October 22, 2010. Tags: Algorithms, Fundamentals Posted in Algorithms, Fundamentals | Print September 22nd, 2010: Introduction to the Kalman Filter, Part I (1 comment) Related October 14th, 2010: Fundamentals: Vectors, Matrices Posts: and Matrix Operations (0 comments) September 9th, 2010: Fundamentals: Coordinate Frames (1 comment)

Recent Posts:
sharprobotica.com//introduction-to-the 7/9

6/6/2011

Introduction to the Kalman Filter, Part II

Author: Billy McCafferty

Full Name: Billy McCafferty Web: http://www.sharprobotica.com Email: billy@emccafferty.com

April 22nd, 2011: Using State Diagrams to Express Robot Behaviors & Transitions (0 comments) April 21st, 2011: Using OMaSE Methodology for Goal Modeling (0 comments) April 14th, 2011: PlanningDomain Research Guidance (0 comments) April 11th, 2011: STRIPS for Classical Plan Representation and Planning (0 comments) January 22nd, 2011: InteRRaP Hybrid Architecture for Robotic & Multi-Agent Systems (0 comments)

Leave a Reply
Name: Email: Website: Comment:
Submit
sharprobotica.com//introduction-to-the 8/9

(required) (required) (will not be published)

6/6/2011

Introduction to the Kalman Filter, Part II

CAPTCHA Code *

Recent Posts:

Tag Cloud

Meta
Register Log in

Using State Diagrams to Algorithms Express Robot Behaviors & C++ DeepT houghtsByJackHandey Design Transitions Using O-MaSE Methodology Patterns Fundamentals for Goal Modeling HardwareIntegration Methodologies Planning-Domain Research PepperPicker Planning Guidance STRIPS for Classical Plan Simulation TDD Tools Representation and Planning

Architecture

ROS

2008 SharpRobotica.com Sharp ideas for the software side of robotics StarScape Theme (1.5.6: Scorpius) | Design by: GDragoN Valid XHTML Valid CSS 2011 Codai, Inc. All Rights Reserved

sharprobotica.com//introduction-to-the

9/9