Example: air traffic controller

Kalman Filtering Tutorial - Biorobotics

1 Understanding and ApplyingKalman FilteringLindsay KleemanDepartment of Electrical and Computer Systems EngineeringMonash University, Clayton2 a basic understanding of Kalman Filtering and assumptionsbehind its (but cannot avoid) mathematical treatment to broaden some practicalities and examples of C++ software is a Kalman Filter and What Can It Do?A Kalman filter is an optimal estimator - ie infers parameters of interest fromindirect, inaccurate and uncertain observations. It is recursive so that newmeasurements can be processed as they arrive. (cf batch processing where alldata must be present).Optimal in what sense?If all noise is Gaussian, the Kalman filter minimises the mean square error ofthe estimated if the noise is NOT Gaussian?

5 Word examples: • Determination of planet orbit parameters from limited earth observations. • Tracking targets - eg aircraft, missiles using RADAR. • Robot Localisation and Map building from range sensors/ beacons. Why use the word “Filter”? The process of finding the “best estimate” from noisy data amounts to “filtering out” the noise.

Tags:

  Findings, Kalman

Information

Domain:

Source:

Link to this page:

Please notify us if you found a problem with this document:

Other abuse

Transcription of Kalman Filtering Tutorial - Biorobotics

1 1 Understanding and ApplyingKalman FilteringLindsay KleemanDepartment of Electrical and Computer Systems EngineeringMonash University, Clayton2 a basic understanding of Kalman Filtering and assumptionsbehind its (but cannot avoid) mathematical treatment to broaden some practicalities and examples of C++ software is a Kalman Filter and What Can It Do?A Kalman filter is an optimal estimator - ie infers parameters of interest fromindirect, inaccurate and uncertain observations. It is recursive so that newmeasurements can be processed as they arrive. (cf batch processing where alldata must be present).Optimal in what sense?If all noise is Gaussian, the Kalman filter minimises the mean square error ofthe estimated if the noise is NOT Gaussian?

2 Given only the mean and standard deviation of noise, the Kalman filter is thebest linear estimator. Non-linear estimators may be is Kalman Filtering so popular? Good results in practice due to optimality and structure. Convenient form for online real time processing. Easy to formulate and implement given a basic understanding. Measurement equations need not be examples: Determination of planet orbit parameters from limited earth observations. Tracking targets - eg aircraft, missiles using RADAR. Robot Localisation and Map building from range sensors/ use the word Filter ?The process of finding the best estimate from noisy data amounts to filteringout the a Kalman filter also doesn t just clean up the data measurements, butalso projects these measurements onto the state is a Covariance Matrix?

3 The covariance of two random variables x1 and x2 iscov(,)[()()]()()(,)xxExxxxxxxxpxxdxdxx x12112211221112212 = where p is the joint probability density function of x1 and correlation coefficient is the normalised quantity 12212121211 +xxxx,7 The covariance of a column vector x=[x1 .. xn] is defined ascov()[()()']..()()'()..xxxxxxxxxxPxx = Epdxdxn1and is a symmetric n by n matrix and is positive definite unless there is a lineardependence among the components of (i,j)th element of Pxx is xxij2 Interpreting a covariance matrix:diagonal elements are the variances, off-diagonal encode a Covariance Matrixcov(x) is symmetric => can be diagonalised using an orthonormal changing coordinates (pure rotation) to these unity orthogonal vectors weachieve decoupling of error basis vectors are the eigenvectors and form the axes of error lengths of the axes are the square root of the eigenvalues and correspond tostandard deviations of the independent noise contribution in the direction of : Error ellipses for mobile robot odometry derived from covariancematrices.

4 9 AError Ellipses corresponding to 50 standard deviationsBDEA to BC to DB to CD to EC1010000 Monte-Carlo runs for kkmLR== 10312, B= mMeansCovariance MatrixStand dev/ Corr MatrixTheor-eticalresults000 a Kalman FilterProblemWe require discrete time linear dynamic system description by vectordifference equation with additive white noise that models DEFINITION - the state of a deterministic dynamic system is thesmallest vector that summarises the past of the system in of the state allows theoretically prediction of the future (and prior)dynamics and outputs of the deterministic system in the absence of SPACE REPRESENTATIONS tate equation:xFxGuv()()()()()().

5 Kkkkkkk+=++=101where x(k) is the nx dimensional state vector, u(k) is the nu dimensional knowninput vector, v(k) is (unknown) zero mean white process noise with covarianceEkkk[()()']()vvQ=Measurement equation:z()()()(),..kkkkk=+=1w(k) is unknown zero mean white measurement noise with known covarianceEkkk[()()']()wwR=13 FALLING BODY EXAMPLEC onsider an object falling under a constant gravitational field. Let y(t) denotethe height of the object, ()()()()()()()()()= = =+ 00000022As a discrete time system with time increment of t-t0=114ykykykg()()().+=+ 12the height y(k+1) depends on the previous velocity and height at time can define the state asx(k) [y(k) y(k)]'.

6 And then the state equation becomes15xxFxGu(k+1) =1101 (k)+ =+1()()gkAssuming we observe or measure the height of the ball directly. Themeasurement equation is:zxHx(k) =[1 0] (k)+w(k)=+()()kwkThe variance of w(k) needs to be known for implementing a Kalman the initial state and covariance, we have sufficient information to find theoptimal state estimate using the Kalman filter Filter EquationsThe Kalman filter maintains the estimates of the state:$(|)()(),(),..$(|)()(),(),..xxxxkk kzkzkkkkzkzk + + estimateofgivenmeasurementsestimateofgiv enmeasurements 1111and the error covariance matrix of the state estimatePxPx(|)()(),(),..(|)()(),(),..kk kzkzkkkkzkzk + + covarianceofgivenestimateofgiven1111We shall partition the Kalman filter recursive processing into several simplestages with a physical interpretation:17 State Estimation0.

7 Known are $(|),(),(|)xuPkkkkk and the new measurement z(k+1).1. State Prediction $(|)()$(|)()()xFxGukkkkkkk+=+12. Measurement Prediction: $(|)()$(|)zHxkkkkk+=+113. Measurement Residual: vzz()()$(|)kkkk+=+ +1114. Updated State Estimate: $(|)$(|)()()xxWvkkkkkk++=++++11111where W(k+1) is called the Kalman Gain defined next in the statecovariance updatemeasurementupdate18 State Covariance Estimation1. State prediction covariance: PFPFQ(|)()(|)()'()kkkkkkk+=+12. Measurement prediction covariance:SHPHR()()(|)()'()kkkkkk+=++++ +111113. Filter Gain WPHS1()(|)()'()kkkkk+=+++ 11114. Updated state covariancePPWSW(|)(|)()()()'kkkkkkk++=+ +++11111119 Page 219 Bar-Shalom ANATOMY OF Kalman FILTERS tate at tk x(k)20 Matrix Riccati EquationThe covariance calculations are independent of state (not so for EKF later)=> can be performed offline and are given by:[]PFPPHHPHRHPFQ(|)()(|)(|)()'()(|)()' ().

8 ()(|)()'(kkkkkkkkkkkkkkkkk+= + + 111111 This is the Riccati equation and can be obtained from the Kalman filterequations solution of the Riccati equation in a time invariant system converges tosteady state (finite) covariance if the pair {F, H} is completely observable (iethe state is visible from the measurements alone).21{F, H} is completely observable if and only if the observability matrixQFFHFHnx01= ..has full rank of convergent solution to the Riccati equation yields the steady state gain forthe Kalman BODY KALMANFILTER (continued)Assume an initial true state of position = 100 and velocity = 0, g= choose an initial estimate state estimate $()x0 and initial state covarianceP()0 based on mainly intuition.)

9 The state noise covariance Q is all measurement noise covariance R is estimated from knowledge of predictedobservation errors, chosen as 1 , G, H are known the Kalman filter equations can be applied:238587899193959799101123456 True positionEstimatemeasurement24 True valuesEstimatesErrors in (k)$()xk1$()xk2P11(k)P22(k) Filter Extensions Validation gates - rejecting outlier measurements Serialisation of independent measurement processing Numerical rounding issues - avoiding asymmetric covariancematrices Non-linear Problems - linearising for the Kalman GateRecall the measurement prediction covariance:SHPHR()()(|)()'()kkkkkk+=++++ +11111 and the measurement prediction: $(|)()$(|)zHxkkkkk+=+11and measurement residual: vzz()()$(|)kkkk+=+ +111A validation gate can be set up around measurements as follows:ekkkg22111=+++ vSv'1()()()where g2 is chosen to for a confidence level.

10 Normalised error e2 varies as aChi-Squared distribution with number of measurements degrees of Measurement ProcessingIf the measurement noise vector components are uncorrelated then state updatecan be carried out one measurement at a matrix inversions are replaced by scalar : state prediction as beforescalar measurements are processed sequentially (in any order)using scalar measurement Rounding ProblemsThe covariance updatePPWSW(|)(|)()()()'kkkkkkk++=+ +++111111involves subtraction and can results in loss of symmetry and positivedefiniteness due to rounding s form covariance update avoids this at expense of computationburden.


Related search queries