This action might not be possible to undo. Are you sure you want to continue?

**Understanding and Applying
**

Kalman Filtering

Lindsay Kleeman

Department of Electrical and Computer Systems Engineering

Monash University, Clayton

2

Introduction

Objectives:

1. Provide a basic understanding of Kalman Filtering and assumptions

behind its implementation.

2. Limit (but cannot avoid) mathematical treatment to broaden appeal.

3. Provide some practicalities and examples of implementation.

4. Provide C++ software overview.

3

What is a Kalman Filter and What Can It Do?

A Kalman filter is an optimal estimator - ie infers parameters of interest from

indirect, inaccurate and uncertain observations. It is recursive so that new

measurements can be processed as they arrive. (cf batch processing where all

data must be present).

Optimal in what sense?

If all noise is Gaussian, the Kalman filter minimises the mean square error of

the estimated parameters.

4

What if the noise is NOT Gaussian?

Given only the mean and standard deviation of noise, the Kalman filter is the

best linear estimator. Non-linear estimators may be better.

Why 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 inverted.

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.

However a Kalman filter also doesn’t just clean up the data measurements, but

also projects these measurements onto the state estimate.

6

What is a Covariance Matrix?

The covariance of two random variables x

1

and x

2

is

cov( , ) [( )( )]

( )( ) ( , )

x x E x x x x

x x x x p x x dx dx

x x

1 2 1 1 2 2

1 1 2 2 1 1 1 2

2

1 2

≡ − −

· − −

≡

−∞

∞

−∞

∞

∫ ∫

σ

where p is the joint probability density function of x

1

and x

2

.

The correlation coefficient is the normalised quantity

ρ

σ

σ σ

ρ

12

2

12

1 2

1 2

1 1 ≡ − ≤ ≤ +

x x

x x

,

7

The covariance of a column vector x=[x

1

.. x

n

]’ is defined as

cov( ) [( )( )' ]

... ( )( )' ( ) ..

x x x x x

x x x x x

P

xx

≡ − −

· − −

≡

−∞

∞

−∞

∞

∫ ∫

E

p dx dx

n 1

and is a symmetric n by n matrix and is positive definite unless there is a linear

dependence among the components of x.

The (i,j)

th

element of P

xx

is

σ

x x

i j

2

Interpreting a covariance matrix:

diagonal elements are the variances, off-diagonal encode correlations.

8

Diagonalising a Covariance Matrix

cov(x) is symmetric => can be diagonalised using an orthonormal basis.

By changing coordinates (pure rotation) to these unity orthogonal vectors we

achieve decoupling of error contributions.

The basis vectors are the eigenvectors and form the axes of error ellipses.

The lengths of the axes are the square root of the eigenvalues and correspond to

standard deviations of the independent noise contribution in the direction of the

eigenvector.

Example: Error ellipses for mobile robot odometry derived from covariance

matrices:

9

A

Error Ellipses corresponding to 50 standard deviations

B

D

E

A to B

C to D

B to C

D to E

C

10

10000 Monte-Carlo runs for

k k m

L R

· ·

−

10

3

1

2

, B=0.5 m

Means Covariance Matrix Stand dev/ Corr Matrix

Theor-

etical

results

0

0

0

3.032e-5 -4.763e-5 -2.817e-5

-4.763e-5 8.974e-5 4.700e-5

-2.817e-5 4.700e-5 3.4849e-5

0.005506 -0.913208 -0.8667

-0.9132 0.009473 0.8404

-0.8667 0.8404 0.005903

Run 1

10000

samples

-1.997e-5

5.321e-5

3.184e-5

2.980e-5 -4.651e-5 2.761e-5

-4.651e-5 8.708e-5 4.585e-5

-2.761e-5 4.585e-5 3.437e-5

0.005459 -0.9130 -0.8627

-0.9130 0.009332 0.8380

0.8627 0.8380 0.005862

11

Formulating a Kalman Filter

Problem

We require discrete time linear dynamic system description by vector

difference equation with additive white noise that models unpredictable

disturbances.

STATE DEFINITION - the state of a deterministic dynamic system is the

smallest vector that summarises the past of the system in full.

Knowledge of the state allows theoretically prediction of the future (and prior)

dynamics and outputs of the deterministic system in the absence of noise.

12

STATE SPACE REPRESENTATION

State equation:

x F x G u v ( ) ( ) ( ) ( ) ( ) ( ) , , ... k k k k k k k + · + + · 1 0 1

where x(k) is the n

x

dimensional state vector, u(k) is the n

u

dimensional known

input vector, v(k) is (unknown) zero mean white process noise with covariance

E k k k [ ( ) ( )' ] ( ) v v Q ·

Measurement equation:

z( ) ( ) ( ) ( ) ,.... k k k k k · + · 1

w(k) is unknown zero mean white measurement noise with known covariance

E k k k [ ( ) ( )' ] ( ) w w R ·

13

FALLING BODY EXAMPLE

Consider an object falling under a constant gravitational field. Let y(t) denote

the height of the object, then

y t g

y t y t g t t

y t y t y t t t

g

t t

..

. .

.

( )

( ) ( ) ( )

( ) ( ) ( )( ) ( )

· −

⇒ · − −

⇒ · + − − −

0 0

0 0 0 0

2

2

As a discrete time system with time increment of t-t

0

=1

14

y k y k y k

g

( ) ( ) ( )

.

+ · + − 1

2

the height y(k+1) depends on the previous velocity and height at time k.

We can define the state as

x(k) [y(k) y(k)]'

.

≡

and then the state equation becomes

15

x x

Fx Gu

(k +1) =

1 1

0 1

(k) +

0.5

¸

1

]

1

¸

1

]

1

−

· +

1

( )

( )

g

k

Assuming we observe or measure the height of the ball directly. The

measurement equation is:

z x

Hx

(k) = [1 0] (k) + w(k)

· + ( ) ( ) k w k

The variance of w(k) needs to be known for implementing a Kalman filter.

Given the initial state and covariance, we have sufficient information to find the

optimal state estimate using the Kalman filter equations.

16

Kalman Filter Equations

The Kalman filter maintains the estimates of the state:

$

( | ) ( ) ( ), ( ),...

$

( | ) ( ) ( ), ( ),...

x x

x x

k k k z k z k

k k k z k z k

− −

+ − + −

estimateof given measurements

estimateof given measurements

1

1 1 1

and the error covariance matrix of the state estimate

P x

P x

( | ) ( ) ( ), ( ),...

( | ) ( ) ( ), ( ),...

k k k z k z k

k k k z k z k

− −

+ − + −

covarianceof given

estimateof given

1

1 1 1

We shall partition the Kalman filter recursive processing into several simple

stages with a physical interpretation:

17

State Estimation

0. Known are $ ( | ), ( ), ( | ) x u P k k k k k and the new measurement z(k+1).

1. State Prediction

$( | ) ( ) $( | ) ( ) ( ) x F x G u k k k k k k k + · + 1

2. Measurement Prediction:

$( | ) ( ) $( | ) z H x k k k k k + · + 1 1

3. Measurement Residual: v z z ( ) ( ) $( | ) k k k k + · + − + 1 1 1

4. Updated State Estimate:

$( | ) $ ( | ) ( ) ( ) x x W v k k k k k k + + · + + + + 1 1 1 1 1

where W(k+1) is called the Kalman Gain defined next in the state

covariance estimation.

Time update

measurement

update

18

State Covariance Estimation

1. State prediction covariance: P F P F Q ( | ) ( ) ( | ) ( )' ( ) k k k k k k k + · + 1

2. Measurement prediction covariance:

S H P H R ( ) ( ) ( | ) ( )' ( ) k k k k k k + · + + + + + 1 1 1 1 1

3. Filter Gain W P H S

1

( ) ( | ) ( )' ( ) k k k k k + · + + +

−

1 1 1 1

4. Updated state covariance

P P W S W ( | ) ( | ) ( ) ( ) ( )' k k k k k k k + + · + − + + + 1 1 1 1 1 1

19

Page 219 Bar-Shalom ANATOMY OF KALMAN FILTER

State at t

k

x(k)

20

Matrix Riccati Equation

The covariance calculations are independent of state (not so for EKF later)

=> can be performed offline and are given by:

[ ]

P F

P P H H P H R

H P

F Q ( | ) ( )

( | ) ( | ) ( )' ( ) ( | ) ( )' ( )

. ( ) ( | )

( )' ( k k k

k k k k k k k k k k

k k k

k + ·

− − − − +

−

¹

'

¹

¹

¹

¹

)

¹

¹

¹

+

−

1

1 1 1

1

1

This is the Riccati equation and can be obtained from the Kalman filter

equations above.

The solution of the Riccati equation in a time invariant system converges to

steady state (finite) covariance if the pair {F, H} is completely observable (ie

the state is visible from the measurements alone).

21

{F, H} is completely observable if and only if the observability matrix

Q

F

FH

FH

n

x

0

1

·

¸

1

]

1

1

1

1

−

...

has full rank of n

x

.

The convergent solution to the Riccati equation yields the steady state gain for

the Kalman Filter.

22

FALLING BODY KALMAN

FILTER (continued)

Assume an initial true state of position = 100 and velocity = 0, g=1.

We choose an initial estimate state estimate

$

( ) x 0

and initial state covariance

P( ) 0

based on mainly intuition. The state noise covariance Q is all zeros.

The measurement noise covariance R is estimated from knowledge of predicted

observation errors, chosen as 1 here.

F, G, H are known the Kalman filter equations can be applied:

23

85

87

89

91

93

95

97

99

101

1 2 3 4 5 6

True position

Estimate

measurement

24

True values Estimates Errors in Estimate

Position Velocity Meas. Position velocity Position velocity

t=kT x

1

x

2

z(k) $ ( ) x k

1

$ ( ) x k

2

P

11

(k) P

22

(k)

0 100.0 0 95.0 1.0 10.0 1.0

1 99.5 -1.0 100.0 99.63 0.38 0.92 0.92

2 98.0 -2.0 97.9 98.43 -1.16 0.67 0.58

3 95.5 -3.0 94.4 95.21 -2.91 0.66 0.30

4 92.0 -4.0 92.7 92.35 -3.70 0.61 0.15

5 87.5 -5.0 87.3 87.68 -4.84 0.55 0.08

25

Kalman Filter Extensions

• Validation gates - rejecting outlier measurements

• Serialisation of independent measurement processing

• Numerical rounding issues - avoiding asymmetric covariance

matrices

• Non-linear Problems - linearising for the Kalman filter.

26

Validation Gate

Recall the measurement prediction covariance:

S H P H R ( ) ( ) ( | ) ( )' ( ) k k k k k k + · + + + + + 1 1 1 1 1

and the measurement prediction:

$( | ) ( ) $( | ) z H x k k k k k + · + 1 1

and measurement residual: v z z ( ) ( ) $( | ) k k k k + · + − + 1 1 1

A validation gate can be set up around measurements as follows:

e k k k g

2 2

1 1 1 · + + + ≤

−

v S v'

1

( ) ( ) ( )

where g

2

is chosen to for a confidence level. Normalised error e

2

varies as a

Chi-Squared distribution with number of measurements degrees of freedom.

measurement

update

27

Sequential Measurement Processing

If the measurement noise vector components are uncorrelated then state update

can be carried out one measurement at a time.

Thus matrix inversions are replaced by scalar inversions.

Procedure: state prediction as before

scalar measurements are processed sequentially (in any order)

using scalar measurement equations.

28

Numerical Rounding Problems

The covariance update

P P W S W ( | ) ( | ) ( ) ( ) ( )' k k k k k k k + + · + − + + + 1 1 1 1 1 1

involves subtraction and can results in loss of symmetry and positive

definiteness due to rounding errors.

Joseph’s form covariance update avoids this at expense of computation

burden:

[ ] [ ]

P I W H P W H

W R W

( | ) ( ) ( ) ( | ) ( ) ( ) '

( ) ( ) ( )'

k k k k k k I k k

k k k

+ + · − + + + − + +

+ + + +

1 1 1 1 1 1 1

1 1 1

Only subtraction is “squared” and preserves symmetry.

29

Extended Kalman Filter (EKF)

Many practical systems have non-linear state update or measurement equations.

The Kalman filter can be applied to a linearised version of these equations with

loss of optimality:

30

EKF - p 387 Bar-Shalom

31

Iterated Extended Kalman Filter (IEKF)

The EKF linearised the state and measurement equations about the predicted

state as an operating point. This prediction is often inaccurate in practice.

The estimate can be refined by re-evaluating the filter around the new estimated

state operating point. This refinement procedure can be iterated until little

extra improvement is obtained - called the IEKF.

32

C++ Software Library

Matrix class - operators overloaded including:

+ * / ~(transpose) =, +=, -=, *=

() for accessing elements,

|| && vertical and horizontal composition,

<< >> input output with automatic formatting

inverse, determinant, index range checking, begin at 1 (not 0!), complex

numbers can be used, noise sources, iterative root finding

Constant matrices, Eye, Zeros etc

33

Kalman filtering classes, for defining and implementing KF, EKF and IEKF

-allows numerical checking of Jacobian functions

Software source is available to collaborators for non-commercial use provided

appropriately acknowledged in any publication. Standard disclaimers apply!

contact Lindsay.Kleeman@monash.edu.au

34

Further Reading

Bar-Shalom and Xiao-Rong Li, Estimation and Tracking: Principles,

Techniques and Software, Artech House Boston, 1993.

Jazwinski, A. H. . Stochastic Processes and Filtering Theory. New York,

Academic Press, 1970.

Bozic, S M, Digital and Kalman Filtering, Edward Arnold, London 1979.

Maybeck, P. S. “The Kalman filter: An introduction to concepts.” Autonomous

Robot Vehicles. I. J. Cox and G. T. Wilfong. New York, Springer-Verlag: 194-

204, 1990.

35

Odometry Error Covariance Estimation for Two

Wheel Robot Vehicles (Technical Report MECSE-95-1, 1995)

A closed form error covariance matrix is developed for

(i) straight lines and

(ii) constant curvature arcs

(iii) turning about the centre of axle of the robot.

Other paths can be composed of short segments of constant curvature arcs.

Assumes wheel distance measurement errors are zero mean white noise.

Previous work incrementally updates covariance matrix in small times steps.

36

Our approach integrates noise over the entire path for a closed form error

covariance - more efficient and accurate

37

Scanned Monocular Sonar Sensing

Small ARC project 1995 - aims:

• To investigate a scanned monocular ultrasonic sensor capable of high speed

multiple object range and bearing estimation.

• Deploy the sensor in these robotic applications:

• obstacle avoidance,

• doorway traversal and docking operations,

• localisation and mapping.

Introduction

Objectives:

1. Provide a basic understanding of Kalman Filtering and assumptions behind its implementation. 2. Limit (but cannot avoid) mathematical treatment to broaden appeal. 3. Provide some practicalities and examples of implementation. 4. Provide C++ software overview.

2

**What is a Kalman Filter and What Can It Do?
**

A Kalman filter is an optimal estimator - ie infers parameters of interest from indirect, inaccurate and uncertain observations. It is recursive so that new measurements can be processed as they arrive. (cf batch processing where all data must be present).

**Optimal in what sense?
**

If all noise is Gaussian, the Kalman filter minimises the mean square error of the estimated parameters.

3

Measurement equations need not be inverted. the Kalman filter is the best linear estimator. Non-linear estimators may be better. Why is Kalman Filtering so popular? • • • • Good results in practice due to optimality and structure. Easy to formulate and implement given a basic understanding.What if the noise is NOT Gaussian? Given only the mean and standard deviation of noise. 4 . Convenient form for online real time processing.

Why use the word “Filter”? The process of finding the “best estimate” from noisy data amounts to “filtering out” the noise. but also projects these measurements onto the state estimate. Robot Localisation and Map building from range sensors/ beacons. missiles using RADAR.eg aircraft.Word examples: • • • Determination of planet orbit parameters from limited earth observations. Tracking targets . However a Kalman filter also doesn’t just clean up the data measurements. 5 .

x2 ) ≡ = ≡ ∫ ∫ ∞ ∞ E [( x1 − x1 )( x2 − x2 )] ( x1 − x1 )( x 2 − x 2 ) p( x1 .What is a Covariance Matrix? The covariance of two random variables x1 and x2 is cov( x1 . − 1 ≤ ρ12 ≤ +1 6 . The correlation coefficient is the normalised quantity ρ12 ≡ 2 σ x1x2 σ x1σ x 2 . x1 )dx1dx 2 σ 21 x2 x −∞ −∞ where p is the joint probability density function of x1 and x2.

The (i. xn]’ is defined as cov( x ) ≡ = ≡ ∫ ∞ −∞ . dx n −∞ Pxx ∞ E [( x − x )( x − x )' ] and is a symmetric n by n matrix and is positive definite unless there is a linear dependence among the components of x. ∫ ( x − x )( x − x )' p( x ) dx1 .. off-diagonal encode correlations.The covariance of a column vector x=[x1 . 7 ....j)th element of Pxx is σ 2i x j x Interpreting a covariance matrix: diagonal elements are the variances.

The basis vectors are the eigenvectors and form the axes of error ellipses. Example: Error ellipses for mobile robot odometry derived from covariance matrices: 8 .Diagonalising a Covariance Matrix cov(x) is symmetric => can be diagonalised using an orthonormal basis. By changing coordinates (pure rotation) to these unity orthogonal vectors we achieve decoupling of error contributions. The lengths of the axes are the square root of the eigenvalues and correspond to standard deviations of the independent noise contribution in the direction of the eigenvector.

Error Ellipses corresponding to 50 standard deviations D to E E D C to D A to B C A B B to C 9 .

913208 -0.005903 0.763e-5 8.8380 0.708e-5 4.032e-5 -4.005862 3.009473 0.8627 -0.8667 -0.651e-5 8.8404 -0.009332 0.9130 -0.817e-5 4.585e-5 10000 3.997e-5 2.651e-5 2.005506 -0.8404 0.5 m 1 Means Covariance Matrix Stand dev/ Corr Matrix 0.9132 0.437e-5 samples 10 .817e-5 0 Theor-4.321e-5 -4.700e-5 0 etical -2.8667 0.4849e-5 0 results Run 1 -1.9130 0.3 10000 Monte-Carlo runs for k L = k R = 10 − m 2 . B=0.585e-5 3.184e-5 -2.005459 -0.8627 0.974e-5 4.700e-5 3.761e-5 4.761e-5 5.763e-5 -2.980e-5 -4.8380 0.

Knowledge of the state allows theoretically prediction of the future (and prior) dynamics and outputs of the deterministic system in the absence of noise.Formulating a Kalman Filter Problem We require discrete time linear dynamic system description by vector difference equation with additive white noise that models unpredictable disturbances. 11 . STATE DEFINITION .the state of a deterministic dynamic system is the smallest vector that summarises the past of the system in full.

v(k) is (unknown) zero mean white process noise with covariance E [ v( k ) v( k )' ] = Q ( k ) Measurement equation: z( k ) = (k ) (k ) + (k ) k = 1.....STATE SPACE REPRESENTATION State equation: x ( k + 1) = F ( k )x ( k ) + G ( k ) u ( k ) + v ( k ) k = 0.1. w(k) is unknown zero mean white measurement noise with known covariance E[ w ( k ) w ( k )' ] = R( k ) 12 ... u(k) is the nu dimensional known input vector. where x(k) is the nx dimensional state vector..

13 . . .FALLING BODY EXAMPLE Consider an object falling under a constant gravitational field. y(t ) = − g ⇒ y ( t ) = y ( t0 ) − g ( t − t0 ) g ⇒ y ( t ) = y ( t 0 ) + y ( t0 )( t − t 0 ) − ( t − t0 )2 2 As a discrete time system with time increment of t-t0=1 . Let y(t) denote the height of the object.. then .

g y ( k + 1) = y ( k ) + y ( k ) − 2 the height y(k+1) depends on the previous velocity and height at time k. x(k) ≡ [y(k) y(k)]' and then the state equation becomes . We can define the state as . 14 .

measurement equation is: The z(k) = [1 0] x (k) + w(k) = H x ( k ) + w( k ) The variance of w(k) needs to be known for implementing a Kalman filter. Given the initial state and covariance. 15 .1 1 0.5 x (k + 1) = x (k) + 1 ( − g ) 0 1 = F x( k ) + G u Assuming we observe or measure the height of the ball directly. we have sufficient information to find the optimal state estimate using the Kalman filter equations.

.... and the error covariance matrix of the state estimate P( k| k ) − covarianceof x( k ) given z( k ). z( k − 1)... z( k − 1)..Kalman Filter Equations The Kalman filter maintains the estimates of the state: $ x( k| k ) − estimate of x( k ) given measurements z ( k )... z( k − 1). z( k − 1). We shall partition the Kalman filter recursive processing into several simple stages with a physical interpretation: 16 .. $ x( k + 1| k ) − estimate of x( k + 1) given measurements z ( k ).. P( k + 1| k ) − estimate of x( k + 1) given z ( k )..

State Prediction x ( k + 1| k ) = F( k ) x ( k | k ) + G ( k ) u( k ) $ $ 2. $ $ 1. P( k | k ) and the new measurement z(k+1). Measurement Prediction: z( k + 1| k ) = H ( k ) x ( k + 1| k ) $ 3. Known are x ( k | k ). Updated State Estimate: x ( k + 1| k + 1) = x ( k + 1| k ) + W( k + 1)v( k + 1) where W(k+1) is called the Kalman Gain defined next in the state covariance estimation.State Estimation $ 0. Measurement Residual: v( k + 1) = z( k + 1) − z( k + 1| k ) Time update measurement update $ $ 4. 17 . u( k ).

Filter Gain W( k + 1) = P( k + 1| k )H ( k + 1)' S( k + 1) −1 4. State prediction covariance: P( k + 1| k ) = F( k )P ( k | k )F( k )'+ Q( k ) 2.State Covariance Estimation 1. Updated state covariance P( k + 1| k + 1) = P( k + 1| k ) − W( k + 1)S( k + 1)W( k + 1)' 18 . Measurement prediction covariance: S( k + 1) = H ( k + 1)P( k + 1| k )H ( k + 1)' + R ( k + 1) 3.

Page 219 Bar-Shalom ANATOMY OF KALMAN FILTER State at tk x(k) 19 .

Matrix Riccati Equation The covariance calculations are independent of state (not so for EKF later) => can be performed offline and are given by: P( k | k − 1) − P( k | k − 1)H ( k )' [ H ( k )P( k | k − 1) H ( k )'+ R ( k )]−1 P( k + 1| k ) = F ( k ) F( k )'+ Q( . The solution of the Riccati equation in a time invariant system converges to steady state (finite) covariance if the pair {F. H} is completely observable (ie the state is visible from the measurements alone). H ( k )P( k | k − 1) This is the Riccati equation and can be obtained from the Kalman filter equations above. 20 .

H} is completely observable if and only if the observability matrix F FH Q0 = . The convergent solution to the Riccati equation yields the steady state gain for the Kalman Filter. nx −1 FH has full rank of nx... 21 .{F.

chosen as 1 here. H are known the Kalman filter equations can be applied: 22 . F. We choose an initial estimate state estimate $ x ( 0) and initial state covariance P( 0) based on mainly intuition.FALLING BODY KALMAN FILTER (continued) Assume an initial true state of position = 100 and velocity = 0. The measurement noise covariance R is estimated from knowledge of predicted observation errors. The state noise covariance Q is all zeros. G. g=1.

True position 101 measurement 99 97 95 93 Estimate 91 89 87 85 1 2 3 4 5 6 23 .

91 -3.5 x2 0 -1.92 0.4 92.66 0.5 92.0 95.True values Position Velocity Meas.58 0.92 0.0 -2.35 87.0 97. t=kT 0 1 2 3 4 5 Estimates Errors in Estimate Position velocity Position velocity x1 100.08 100.0 -5.0 $ x2 ( k ) 1.0 -3.21 92.38 -1.61 0.0 -4.84 P11(k) 10.9 94.43 95.16 -2.30 0.0 0.67 0.15 0.63 98.0 87.0 z(k) $ x1 ( k ) 95.68 24 .3 99.0 0.5 98.0 0.55 P22(k) 1.7 87.0 99.70 -4.

25 .Kalman Filter Extensions • • • • Validation gates .linearising for the Kalman filter.avoiding asymmetric covariance matrices Non-linear Problems .rejecting outlier measurements Serialisation of independent measurement processing Numerical rounding issues .

26 .Validation Gate Recall the measurement prediction covariance: S( k + 1) = H ( k + 1)P( k + 1| k )H ( k + 1)' + R ( k + 1) $ $ and the measurement prediction: z( k + 1| k ) = H ( k ) x ( k + 1| k ) $ and measurement residual: v( k + 1) = z( k + 1) − z( k + 1| k ) A validation gate can be set up around measurements as follows: measurement update e 2 = v( k + 1)S −1 ( k + 1)v' ( k + 1) ≤ g 2 where g2 is chosen to for a confidence level. Normalised error e2 varies as a Chi-Squared distribution with number of measurements degrees of freedom.

27 .Sequential Measurement Processing If the measurement noise vector components are uncorrelated then state update can be carried out one measurement at a time. Thus matrix inversions are replaced by scalar inversions. Procedure: state prediction as before scalar measurements are processed sequentially (in any order) using scalar measurement equations.

Joseph’s form covariance update avoids this at expense of computation burden: P(k + 1| k + 1) = [ I − W(k + 1)H(k + 1)]P(k + 1| k )[ I − W( k + 1)H( k + 1)]' + W(k + 1)R(k + 1)W(k + 1)' Only subtraction is “squared” and preserves symmetry.Numerical Rounding Problems The covariance update P( k + 1| k + 1) = P( k + 1| k ) − W( k + 1)S( k + 1)W( k + 1)' involves subtraction and can results in loss of symmetry and positive definiteness due to rounding errors. 28 .

Extended Kalman Filter (EKF) Many practical systems have non-linear state update or measurement equations. The Kalman filter can be applied to a linearised version of these equations with loss of optimality: 29 .

EKF .p 387 Bar-Shalom 30 .

Iterated Extended Kalman Filter (IEKF) The EKF linearised the state and measurement equations about the predicted state as an operating point. The estimate can be refined by re-evaluating the filter around the new estimated state operating point. This refinement procedure can be iterated until little extra improvement is obtained . 31 .called the IEKF. This prediction is often inaccurate in practice.

+=. << >> input output with automatic formatting inverse. complex numbers can be used. noise sources.operators overloaded including: + * / ~(transpose) =. Eye. index range checking. begin at 1 (not 0!). -=. iterative root finding Constant matrices.C++ Software Library Matrix class . *= () for accessing elements. Zeros etc 32 . || && vertical and horizontal composition. determinant.

Standard disclaimers apply! contact Lindsay. EKF and IEKF -allows numerical checking of Jacobian functions Software source is available to collaborators for non-commercial use provided appropriately acknowledged in any publication.Kalman filtering classes.Kleeman@monash.au 33 .edu. for defining and implementing KF.

Digital and Kalman Filtering.Further Reading Bar-Shalom and Xiao-Rong Li. A. 1993. P. Maybeck. 1970. Edward Arnold. Cox and G. New York. Wilfong. Stochastic Processes and Filtering Theory. Estimation and Tracking: Principles. Artech House Boston. T. Techniques and Software. . London 1979. H. Bozic. Springer-Verlag: 194204. New York.” Autonomous Robot Vehicles. 34 . J. I. S M. Academic Press. S. “The Kalman filter: An introduction to concepts. 1990. Jazwinski.

Odometry Error Covariance Estimation for Two Wheel Robot Vehicles (Technical Report MECSE-95-1. Previous work incrementally updates covariance matrix in small times steps. Assumes wheel distance measurement errors are zero mean white noise. 1995) A closed form error covariance matrix is developed for (i) straight lines and (ii) constant curvature arcs (iii) turning about the centre of axle of the robot. Other paths can be composed of short segments of constant curvature arcs. 35 .

more efficient and accurate 36 .Our approach integrates noise over the entire path for a closed form error covariance .

Scanned Monocular Sonar Sensing Small ARC project 1995 . • 37 .aims: • To investigate a scanned monocular ultrasonic sensor capable of high speed multiple object range and bearing estimation. • doorway traversal and docking operations. Deploy the sensor in these robotic applications: • obstacle avoidance. • localisation and mapping.

- Observers for Lipschitz Nonlinear Systems
- 4TH_CO.pdf
- Industrial Instruments
- Catalyst Systems, Inc Contract With Celerity
- moi
- Utkin Sliding Mode Control in Electromechanical Systems Utkin Guldner Shi 1999
- ltxprimer-1.0
- 5671
- Pole Placement
- 7360w1a Overview Linear Algebra
- Data Pre Processing
- Design and Implementation of an Adaptive Controller
- Fundamentals of Electric Motors
- A Reconfigurable Motor for Experimental Emulation Of
- A Model for Induction Motor Aggregation for Power System Studies
- 3-Phase Induction Machines
- Ana of IM for Adjustable Speed Drv
- An Investigation Into the Torque Behavior of A
- A Model for Induction Motor Aggregation for Power System Studies
- Copper Die Cast Problems
- Advances in EEM

- kalman filter
- Kalman Filter
- Introduction to Kalman Filter
- Kalman Filter
- KalmanFilter
- A Kalman Filter Primer - Eubank - 0824723651
- kalman
- Kalman Filter
- Motion Tracking Using Kalman Filter Matlab Code
- Kalman
- understanding and applying Kalman filter
- Generalized Multiple-Model Adaptive Estimation Using an Autocorrelation Approach
- TKJ Electronics a Practical Approach to Kalman Filter and How to Implement It
- discrete kalman filter
- CalmanFilter.pdf
- 9781420009026%2Ech8
- 201304
- Relative Study of Measurement Noise Covariance R and Process Noise Covariance Q of the Kalman Filter in Estimation.
- Aid Ala
- How to use the HMM toolbox
- Kalman Filter
- The Kalman Filter
- Oyeyemi and Ipinyomi
- Ch05_p97-133
- 16Ligterink
- State Estimation
- Kalman Filter - Wikipedia, The Free Encyclopedia
- 12-28 許家豪
- Target Tracking
- Sparse Estimation Co Variance Matrix
- Kalman filter

Are you sure?

This action might not be possible to undo. Are you sure you want to continue?

We've moved you to where you read on your other device.

Get the full title to continue

Get the full title to continue reading from where you left off, or restart the preview.

scribd