Download - ME 597: AUTONOMOUS MOBILE ROBOTICS SECTION STIMATION …wavelab.uwaterloo.ca/sharedata/ME597/ME597_Lecture... · 2020. 1. 24. · ME 597: AUTONOMOUS MOBILE ROBOTICS SECTION 7 –

ME 597: AUTONOMOUS MOBILE ROBOTICSSECTION 7 – ESTIMATION I

Prof. Steven Waslander

2

COMPONENTS

Actuators Vehicle Sensors

Control Estimation

Hardware

Vehicle Autonomy

Environmental Autonomy

Path Planning Mapping

Mission Autonomy

Mission Planning

MissionMapping

Bayes Filter Framework Kalman Filter Extended Kalman Filter Particle Filter

3

OUTLINE

The Bayes Filter forms the foundation for all other filters in this class As described in background slides, Bayes rule is the

right way to incorporate new probabilistic information into an existing, prior estimate

The resulting filter definition can be implemented directly for discrete state systems

For continuous states, need additional assumptions, additional structure to solve the update equations analytically

4

BAYES FILTER

State xt All aspects of the vehicle and its environment that can impact the

future Assume the state is complete

Control inputs ut All elements of the vehicle and its environment that can be controlled

Measurements yt All elements of the vehicle and its environment that can be sensed

Note: sticking with Thrun, Burgard, Fox notation Discrete time index t Initial state is x0 First, apply control action u1 Move to state x1 Then, take measurement y1

5

BAYES FILTER

Motion Modeling Complete state:

At each time t, xt-1 is a sufficient summary of all previous inputs and measurements

Application of Conditional Independence No additional information is to be had by considering

previous inputs or measurements

Referred to as the Markov Assumption Motion model is a Markov Chain

6

BAYES FILTER

0: 1 1: 1 1: 1( | , , ) ( | , )t t t t t t tp x x y u p x x u

Measurement Modeling Complete state:

Current state is sufficient to model all previous states, measurements and inputs

Again, conditional independence

Recall, in standard LTI state space model, measurement model may also depend on the current input

7

BAYES FILTER

0: 1: 1 1:( | , , ) ( | )t t t t t tp y x y u p y x

Combined Model Referred to as Hidden Markov Model (HMM) or

Dynamic Bayes Network (DBN)

8

BAYES FILTER

xt-1

ut-1

yt-1

xt

ut

yt

… …x0 x1

u1

y1

Motion Model

Measurement Model

Example Discrete State Motion Model: States: {No Rain, Drizzle, Steady, Downpour} Inputs: None

9

BAYES FILTER

No Rain

Drizzle

Steady

Down-pour

0.8

0.1

0.1

0.4

0.3

0.5

0.9

0.3

0.05

0.5

0.05

For discrete states, the motion model can be written in matrix form For each input ut, the nXn motion model matrix is

Each row defines the probabilities of transitioning to state xt from all possible states xt-1

Each column defines the probabilities of transitioning to any state xt from a specific state xt-1

Again, the columns must sum to 1 10

BAYES FILTER

1

1 1 1 1 1 2

2 1 1 2 1 2

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

t t t

t t t t

t t t t

p x u u xp x x x x p x x x xp x x x x p x x x x

Example: Motion Model in Matrix Form

No inputs, one matrix

11

BAYES FILTER

1

0.8 0.3 0.05 00.1 0.4 0 0

( | , )0.1 0.3 0.9 0.50 0 0.05 0.5

t t tp x u x

1tx

tx

Example Measurement Model: States: {No Rain, Drizzle, Steady, Downpour} Measurements: {Dry, Light, Medium, Heavy}

Again, the columns sum to 1

12

BAYES FILTER

0.95 0.1 0 00.05 0.8 0.15 0

( | )0 0.1 0.7 0.10 0 0.15 0.9

t tp y x

tx

ty

Example System Evolution

13

BAYES FILTER

No Rain Steady

None

Light

Steady

None

Medium

0t 1t 2t

Aim of Bayes Filter To estimate the current state of the system based on

all known inputs and measurements.

That is, to define a belief about the current state using all available information:

Known as belief, state of knowledge, information state Depends on every bit of information that exists up to time t

Can also define a belief prior to measurement yt

Known as prediction, predicted state 14

BAYES FILTER

1: 1:( ) ( | , )t t t tbel x p x y u

1: 1 1:( ) ( | , )t t t tbel x p x y u

Problem Statement Given a prior for the system state

Given motion and measurement models

Given a sequence of inputs and measurements

Estimate the current state distribution (form a belief about the current state) 15

BAYES FILTER

1: 1 1: 1,..., , ,...,t t t tu u u y y y

( | )t tp y x

1( | , )t t tp x x u

0( )p x

1: 1:( ) ( | , )t t t tbel x p x y u

Bayes Filter Algorithm At each time step, t, for all possible values of the

state x1. Prediction update (Total probability)

2. Measurement update (Bayes Theorem)

η is a normalizing constant that does not depend on the state (will become apparent in derivation)

Recursive estimation technique 16

BAYES FILTER

1 1 1( ) ( | , ) ( )t t t t t tbel x p x u x bel x dx

( ) ( | ) ( ) t t t tbel x p y x bel x

Recall Bayes Theorem

Terminology

17

BAYES FILTER

( | ) ( )( | )( )

p b a p ap a bp b

likelihood priorposteriorevidence

Derivation Proof by induction

Demonstrate that belief at time t can be found using belief at time t-1, input at t and measurement at t

Initially

At time t, Bayes Theorem relates xt, yt

18

BAYES FILTER

0 0( ) ( )bel x p x

1: 1: 1: 1 1:( ) ( | , ) ( | , , )t t t t t t t tbel x p x y u p x y y u

1 2

3

1: 1 1: 1: 1 1:

1: 1 1:

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

t t t t t t t

t t t

p y x y u p x y up y y u

Derivation1. Measurement model simplifies first numerator term

2. Second numerator term is definition of belief prediction

3. Denominator is independent of state, and so is constant for each time step. Define the normalizer,

19

BAYES FILTER

1: 1 1:( | , , ) ( | )t t t t t tp y x y u p y x

11: 1 1:( | , )t t tp y y u

1: 1 1:( ) ( | , )t t t tbel x p x y u

Derivation Summarizing the three substitutions

This is exactly the measurement update step Requires the measurement yt to be known

However, we now need to find the belief prediction Done using total probability, over previous state

20

BAYES FILTER

( ) ( | ) ( )t t t tbel x p y x bel x

1: 1 1:

1 1: 1 1: 1 1: 1 1: 1

( ) ( | , )

( | , , ) ( | , )t t t t

t t t t t t t t

bel x p x y u

p x x y u p x y u dx

1 2

Derivation1. This time, the motion model can be incorporated

2. And we note that the control input at time t does not affect the state at time t-1

21

BAYES FILTER

1 1: 1 1: 1( | , , ) ( | , )t t t t t t tp x x y u p x x u

1 1: 1 1: 1 1: 1 1: 1

1

( | , ) ( | , ) ( )

t t t t t t

t

p x y u p x y ubel x

Derivation And so the prediction update is defined

Which completes the proof by induction For this step, we need the control input to define the correct

motion model distribution

If state, measurements, inputs are discrete, can directly implement Bayes Filter Prediction update is summation over discrete states Measurement update is multiplication of two vectors

22

BAYES FILTER

1 1 1( ) ( | , ) ( )t t t t t tbel x p x x u bel x dx

If state, measurement, inputs are continuous, must define model or approximation to enable computation Kalman Filter:

Linear motion models Linear measurement models Additive Gaussian disturbance and noise distributions

Extended Kalman Filter/Unscented Kalman Filter: Nonlinear motion models Nonlinear measurement models Additive Gaussian disturbance and noise distributions

Particle Filter: (Dis)continuous motion models (Dis)continuous measurement models General disturbance and noise distributions

23

BAYES FILTER

Discrete Bayes Filter Example Problem: Detect if a door is open/closed with a robot

that can sense the door position and pull the door open

State: door={open, closed} State Prior (uniform):

24

BAYES FILTER

0

( ) 0.5( )

( ) 0.5p open

p xp closed

Example Inputs: arm_command={none, pull} Motion Model

If input = none, do nothing:

If input = pull, pull the door open

25

BAYES FILTER

1

11

1

1

( | , ) 1( | , ) 0

( | , )( | , ) 0.8( | , ) 0.2

t t

t tt t t

t t

t t

p open pull openp closed pull open

p x u pull xp open pull closedp closed pull closed

1

11

1

1

( | , ) 1( | , ) 0

( | , )( | , ) 0( | , ) 1

t t

t tt t t

t t

t t

p open none openp closed none open

p x u none xp open none closedp closed none closed

Example Measurements: meas={sense_open, sense_closed} Measurement model (noisy door sensor):

26

BAYES FILTER

( _ | ) 0.6( _ | ) 0.2

( | )( _ | ) 0.4( _ | ) 0.8

p sense open openp sense open closed

p y xp sense closed openp sense closed closed

Example At time step 1, input = none Perform state prediction update

Calculate belief prediction for each possible value of state

27

BAYES FILTER

1 1 0 0 0 0

1 0 0 0

( ) ( | , ) ( )

( | , ) ( )

bel x p x u x bel x dx

p x u x p x

1 1 1 0 0

1 1 0 0

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

1*0.5 0*0.5 0.5

bel open p open none open bel openp open none closed bel closed

1 1 1 0 0

1 1 0 0

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

0*0.5 1*0.5 0.5

bel closed p closed none open bel openp closed none closed bel closed

Example At time step 1,measurement y1 = sense_open Measurement update

Calculate for each possible value of state

Calculate normalizer and solve for posterior

28

BAYES FILTER

1 1 1 1( ) ( | ) ( )bel x p y x bel x

1 1 1 1( ) ( _ | ) ( )0.6 0.5 0.3

bel open p sense open open bel open

1 1 1 1( ) ( _ | ) ( )0.2 0.5 0.1

bel closed p sense open closed bel closed

1 2.50.3 0.1

1

1

( ) 0.75( ) 0.25

bel openbel closed

Example At time step 2, a pull and a sense_open Then state propagation

And measurement update

In summary: Uniform prior, do nothing, measure open: bel(open1) = 0.75 Pull open, measure open: bel(open2) = 0.983 29

BAYES FILTER

2

2

( ) 1 0.75 0.8 0.25 0.95( ) 0 0.75 0.2 0.25 0.05

bel openbel closed

2

2

( ) 0.6 0.95 0.983( ) 0.2 0.05 0.017

bel openbel closed

Example 2: Histogram Filter Motion of robot in a nXn grid

State: Position = {x11, x12, …, x1n, , …, xnn}

Input: Move = {Up, Right, Down, Left} 40% chance the move does not happen Cannot pass through outer walls

Measurement: Accurate to within 3X3 grid

30

BAYES FILTER

.11 .11 .11

( 1: 1, 1: 1) | ( , ) .11 .12 .11.11 .11 .11

p y i i j j x i j

. . .11x 1nx

Example 2: Prior over states

Assume no information, uniform Vector of length n2

Motion model Given a particular input and

previous state, probability of moving to any other state nXn state, one for each grid point 4 input choices

31

BAYES FILTER

2 2 41( | , ) [0,1]n n

t t tp x x u

0 21( )p xn

1

. . .11x 1nx

Example 2 Measurement Model

Given any current state, probability of a measurement

Same number of measurements as states

Same 3X3 matrix governs all interior points

Boundaries cut off invalid measurements and require normalization

Very simplistic and bloated model Could replace with 2 separate

states and measurements to perpendicular walls

32

BAYES FILTER

2 2

( | ) [0,1]n nt tp y x

. . .11x 1nx

Example 2 – Motion Model code

33

BAYES FILTER . . .11x 1nx

mot_mod = zeros(N,N,4); for i=1:n

for j=1:ncur = i+(j-1)*n;% Move upif (j > 1)

mot_mod(cur-n,cur,1) = 0.6; mot_mod(cur,cur,1) = 0.4;

elsemot_mod(cur,cur,1) = 1;

end% Move rightif (i < n)

mot_mod(cur+1,cur,2) = 0.6; mot_mod(cur,cur,2) = 0.4;

elsemot_mod(cur,cur,2) = 1;

end…

Example 2 – Measurement Model

34


%% Create the measurement modelmeas_mod_rel = [0.11 0.11 0.11;

0.11 0.12 0.11;0.11 0.11 0.11];

% Convert to full measurement model% p(y_t | x_t)meas_mod = zeros(N,N);% Fill in non-boundary measurementsfor i=2:n-1

for j=2:n-1cur = i+(j-1)*n;meas_mod(cur-n+[-1:1:1],cur) = meas_mod_rel(1,:); meas_mod(cur+[-1:1:1],cur) = meas_mod_rel(2,:); meas_mod(cur+n+[-1:1:1],cur) = meas_mod_rel(3,:);

endend…

Example 2 – Makin’ movies!

35


videoobj=VideoWriter('bayesgrid.mp4','MPEG-4');truefps = 1;videoobj.FrameRate = 10; %Anything less than 10 fps fails.open(videoobj);

figure(1);clf; hold on;beliefs = reshape(bel,n,n);imagesc(beliefs);plot(pos(2),pos(1),'ro','MarkerSize',6,'LineWidth',2)colormap(bone);title('True state and beliefs')F = getframe;% Dumb hack to get desired frameratefor dumb=1:floor(10/truefps)

writeVideo(videoobj, F);end…

Example 2 – Simulation code

36


%Main Loopfor t=1:T

%% Simulation% Select motion inputu(t) = ceil(4*rand(1));% Select a motionthresh = rand(1);new_x = find(cumsum(squeeze(mot_mod(:,:,u(t)))*x(:,t))>thresh,1);% Move vehiclex(new_x,t+1) = 1;% Take measurementthresh = rand(1);new_y = find(cumsum(meas_mod(:,:)*x(:,t+1))>thresh,1);y(new_y,t) = 1;% Store for plotting…

Example 2 – Bayes Filter

37


…%% Bayesian Estimation% Prediction updatebelp = squeeze(mot_mod(:,:,u(t)))*bel;

% Measurement updatebel = meas_mod(new_y,:)'.*belp;bel = bel/norm(bel);

[pmax y_bel(t)] = max(bel);

%% Plot beliefs…

Example 2: Results

38

BAYES FILTER


39

OUTLINE

Rudolf Kalman 1960 (BS, MS: MIT, PhD: Columbia)

Discrete version (Kalman filter) Continuous version (Kalman-Bucy filter) Many other versions, improvements,

modifications 40

KALMAN FILTER

The filter is named after Rudolf E. Kalman, though Thorvald NicolaiThiele[1] and Peter Swerling developed a similar algorithm earlier.Stanley F. Schmidt is generally credited with developing the firstimplementation of a Kalman filter. It was during a visit of Kalman tothe NASA Ames Research Center that he saw the applicability of hisideas to the problem of trajectory estimation for the Apollo program,leading to its incorporation in the Apollo navigation computer. The filterwas developed in papers by Swerling (1958), Kalman (1960), andKalman and Bucy (1961).

Wikipedia

Kalman Filter Modeling Assumption Continuous state, inputs, measurements Prior over the state is Gaussian

Motion model, linear with additive Gaussian disturbances

Often, robotics systems are more easily described in continuous domain Convert to discrete time using matrix exponential Matlab contains tools to perform this conversion (c2d, d2c) 41

KALMAN FILTER

0 0 0( ) ~ ( , )p x N

1t t t t t tx A x B u ~ (0, )t tN R

Kalman Filter Modeling Assumption Measurement model also linear with additive

Gaussian noise

Can add in input dependence to match up with controls literature

42

KALMAN FILTER

t t t ty C x ~ (0, )t tN Q

t t t t t ty C x D u

Full Model State prior

Motion model

Measurement model

43

KALMAN FILTERING

0 0 0( ) ~ ( , )p x N

1t t t t t tx A x B u ~ (0, )t tN R

t t t ty C x ~ (0, )t tN Q

Assume belief is Gaussian at time t

μt is the best estimate of the current state at time t ∑t is the covariance, indicating the certainty in the

current estimate

Will be able to demonstrate the predicted belief at the next time step is Gaussian

And that the belief at next time step is also Gaussian

44

KALMAN FILTER

( ) ~ ( , )t t tbel x N

11 1( ) ~ ( , )tt tbel x N

1 1 1( ) ~ ( , )t t tbel x N

Goal: To find belief over state as accurately as possible

given all available information Minimize the mean square error of the estimate (MMSE

estimator)

Same as least square problem

Using an unbiased estimator

On average, your estimate is correct!45

KALMAN FILTER

2min [( ) ]t tE x

[ ] 0t tE x

Goal The MMSE estimate can be written as

And is equivalent to minimizing the trace of the error covariance matrix

Proof:

46

KALMAN FILTER

min [( ) ( )]Tt t t tE x x

min tr( )t

2, ,[( ) ( )] ( )

[tr ( )( ) ]

tr [( )( ) ]

tr( )

Tt t t t t i t i

i

Tt t t t

Tt t t t

t

E x x E x

E x x

E x x

Kalman Filter Algorithm At each time step, t, update both sets of beliefs

1. Prediction update

2. Measurement update

Kalman Gain, Kt Blending factor between prediction and measurement

47

KALMAN FILTER

1

1

t t t t tT

t t t t t

A B u

A A R

1( )( )

( )

T Tt tt t t t t

t t t t t t

tt t t

K C C C QK y C

I K C

Example Temperature control

State is current temperature difference with outside One dimensional example Prior: fairly certain of current temperature difference

Motion Model: Decaying temperature + furnace input + disturbances (opening doors, outside effects)

48

KALMAN FILTER

0

0

101

1

0.10.8 30.8, 3

~ (0,2)

t t t t

t

dtx x u rA Br N

Example Measurement Model

Directly measure the current temperature difference

Controller design Bang bang control, based on current estimate of

temperature difference

49

KALMAN FILTER

( ) ( )~ (0,4)

t

t

y t x tN

1 2( ) 10 0

( 1) otherwise

t

tu tu t

Example Simulation

50

KALMAN FILTER

for t=1:length(T)% Select control action

if (t>1) u(t)=u(t-1); endif (mu > 10)

u(t) = 0;elseif (mu < 2);

u(t) = 1;end

% Update statee = sqrt(R)*randn(1);x(t+1) = A*x(t)+ B*u(t) + e;

% Determine measurementd = sqrt(Q)*randn(1);y(t) = C*x(t+1) + d;

Example Estimation

Matrix inverse 0(n2.4), matrix multiplication O(n2) When implementing in Matlab, inv( ) performs

matrix inverse for you For embeddded code, many libraries exist

Try Gnu Scientific Library, easy starting point51

KALMAN FILTER

% Prediction updatemup = A*mu + B*u(t);Sp = A*S*A' + R;

% Measurement updateK = Sp*C'*inv(C*Sp*C'+Q);mu = mup + K*(y(t)-C*mup);S = (1-K*C)*Sp;

Example Beliefs during the first time step

Prior

52

KALMAN FILTER

Example Beliefs during the first time step

Prediction Update: increased variance, shifted mean

53

KALMAN FILTER

10.8 30.8, 3

~ (0,2)

t t t t

t

x x u rA Br N

1

1

t t t t tT

t t t t t

A B u

A A R

Example Beliefs after the first time step

Measurement update: decreased variance

54

KALMAN FILTER

Example Trajectories over time

55

KALMAN FILTER

Example Measurements over time

56

KALMAN FILTER

Example Estimates over time

57

KALMAN FILTER

Recall Goal: To find belief over state as accurately as possible

given all available information Minimize the mean square error of the estimate (MMSE

estimator)

Same as least square problem

Using an unbiased estimator

On average, your estimate is right!58

KALMAN FILTER

2min [( ) ]t tE x

[ ] 0t tE x

Derivation

Define the innovation The difference between the measurement and the expected

measurement given the predicted state and the measurement model

Assume the form of the estimator is a linear combination of the predicted belief and the innovation The following form turns out to be unbiased

59

KALMAN FILTER

Innovation t t t ty C

( )t t t t t tK y C

Steps

Prediction update Find update rule for mean, covariance of predicted belief,

given input and motion model

Measurement update Solve MMSE optimization problem to find update rule for

mean, covariance of belief given measurement model and measurement

60

KALMAN FILTER

Prediction Update Only new information is input ut

Prediction update is a linear transformation of belief at previous time step Motion model is

Motion noise, previous belief are Gaussian so this is an addition of Gaussian distributions

Therefore the predicted mean and covariance are

61

KALMAN FILTER

1t t t t t tx A x B u

1

1

0t t t t tT

t t t t t

A B u

A A R

1 1 1( ) ~ ( , )t t tbel x N ~ (0, )t tN R

Measurement update First, lets define the form of the error covariance,

substituting in the form of the mean update and the measurement model

But, by the assumption of the form of the estimator, and the measurement model

62

KALMAN FILTER

[( )( ) ]Tt t t t tE x x

( )( )

( )( )

t t t t t t t t

t t t t t t t t

t t t t t t

x x K y Cx K C x CI K C x K

Measurement update Next, reorganize terms of the covariance

But the middle term is zero in expectation

63

KALMAN FILTER

[ ( )( ) ( )( ) ]

[ ( )( ) ( )( ) ]2 [ ( )( ) ]

[ ]

Tt t t t t t t t t t t t t

Tt t t t t t t t

t t t t t t

Tt t t t

E I K C x K I K C x K

E I K C x I K C xE I K C x K

E K K

[ ( )( ) ( )( ) ]

[ ]

Tt t t t t t t t t

Tt t t t

E I K C x I K C x

E K K

Measurement Update Recall multiplication by a constant yields

In the above, numerous constants

64

KALMAN FILTER

cov( ) [( )( ) ]cov( )

T

T

Ax E Ax A Ax AA X A

( ) ( )

( )

[ ( ) ( ) ]

[ ]cov ( ) cov

t t t tT

t t t t t

Tt t

t

t t

ttt t t

E x x

E

I K C I K C

I KK K

x KC

Measurement Update The resulting covariance is

The expectations that remain are known quantities

Which leaves us with a quadratic equation in Kt65

KALMAN FILTER

( ) ( )T T

tt t t t t t t t

T T T Tt t t tt t t t t t t t t

I K C I K C K Q K

K C C K K C C Q K

( ) ( )( ) ( )T Tt t t t t t t t t

T Tt t t t

I K C E x x I K C

K E K

Measurement update We now have the covariance in a form that can be

optimized

We need two identities to find this minimum Differentiation of linear matrix expression

Differentiation of quadratic matrix expression

66

KALMAN FILTER

min tr( )t

tr(AXB) tr(B X A )T T TT TA B

X X

tr(XAX )TTXA XA

X

Measurement update The optimization is done by setting the derivative of

the trace w.r.t the Kalman gain to 0

Taking the matrix derivative w.r.t Kt Two linear terms and one quadratic

67

KALMAN FILTER

min tr T T T Tt t t tt t t t t t t t tK C C K K C C Q K

tr( )= T T Tt tt t t

tTT T

t tt t t t t t t t

C CK

K C C Q K C C Q

Measurement update Set the derivative to 0, noting that covariance is

symmetric, and AXAT preserves symmetry

Simplifying

And finally, we arrive at the Kalman gain equation

68

KALMAN FILTER

tr( )= 2 2 0T Tt tt t t t t t

t

C K C C QK

1( )T Tt tt t t t tK C C C Q

T Tt tt t t t tK C C Q C

Measurement Update So far, we have found the optimal gain Kt which

minimizes mean square error in the measurement update for the mean

Next, we need to simplify the covariance update using this result for the Kalman gain

69

KALMAN FILTER

( )t t t t t tK y C


Measurement Update Recall the Covariance update was

Substituting in the Kalman gain gives

70

KALMAN FILTER

T T T Tt t t tt t t t t t t t t tK C C K K C C Q K

1

1

1

1

( )

)

( )

( )(

TT T T

T Tt t t

t t tt t t t t

TT T Tt t tt t t t t

tt t t t t

T Tt tt t t

t

t t t

C

C C Q C C Q

C C C Q

C C C

C C C

C

C Q

Q

Measurement Update Fortunately, almost everything cancels and we are

left with

71

KALMAN FILTER

1( )( )

T Tt t t tt t t t t t

tt t

C C C Q CI K C

Kalman Filter Algorithm At each time step, t, update both sets of beliefs



Kalman Gain, Kt Blending factor between prediction and measurement

72

KALMAN FILTER

1

1

t t t t tT

t t t t t

A B u

A A R

1( )( )

( )

T Tt tt t t t t

t t t t t t

tt t t

K C C C QK y C

I K C

Summary Follows same framework as Bayes filter Requires linear motion and Gaussian disturbance Requires linear measurement and Gaussian noise It is sufficient to update mean and covariance of

beliefs, because they remain Gaussian Prediction step involves addition of Gaussians Measurement step seeks to minimize mean square

error of the estimate Expand out covariance from definition and measurement

model Assume form of estimator, linear combination of prediction

and measurement Solve MMSE problem to find optimal linear combination Simplify covariance update once gain is found

73

KALMAN FILTER

Relation to Bayes Filter Problem Formulation State prior

Motion model

Measurement model

Beliefs

74

KALMAN FILTERING

0 0 0 0( ) ( ) ( , )bel x p x N

1 1 1( | , ) ( , )Tt t t t t t t t t t tp x x u N A x B u A A R

( | ) ( , )t t t t tp y x N C x Q

( ) ( , ), ( ) ( , )t t t t t tbel x N bel x N

Relation to Bayes Filter Algorithm 1. Prediction update (Total probability)

Insert normal distributions

Separate out terms that depend on current state Manipulate remaining integral into a Gaussian pdf

form of previous state Integrate over full range to get 1 Manipulate remaining terms and solve for Kalman

prediction equations.

Refer to Thrun, Burgard & Fox Chap. 3 for details 75

KALMAN FILTER

1 11 1 1 1 1 1 1

1 1 1

1/2( ) ( ) 1/2( ) ( )1

( ) ( | , ) ( )T T

t t t t t t t t t t t t t t t t

t t t t t t

x A x B u R x A x B u x xt

bel x p x u x bel x dx

e e dx

1 1~ ( , )Tt t t t t t t tN A B u A A R

Relation to Bayes Filter Algorithm 2. Measurement update (Bayes Theorem)

Reorganize exponents and note it remains a Gaussian For any Gaussian:

Second derivative of exponent is inverse of covariance Mean minimizes exponent,

• Set first derivative of exponent to 0 and solve Use this to solve for mean and covariance of belief

where Kt is the Kalman gain as before 76

KALMAN FILTER

111/2( ) ( ) 1/2( ) ( )

( ) ( | ) ( )T T

tt t t t t t t t t t t

t t t t

y C x Q y C x x x

bel x p y x bel x

e e

( ( ),( ) )tt t t t t t tN K y C I K C

Example 3D Linear motion model for three thruster AUV

(heading constant) State Input

Continuous dynamics for

77

KALMAN FILTER

n

n

e

e

d

d

pvp

xvpv

n

e

d

Tu T

T

( ) ( )( ) ( ) ( )

n n

n n n

x t v tmv t bv t T t

Example – Omni-directional AUV Discrete Dynamics from zero order hold, dt = 0.1s

Disturbances

78

KALMAN FILTER

1

1 .0975 0 0 .0025 00 .9512 0 0 .0488 00 0 1 .0975 0 .00250 0 0 .9512 0 .0048

t t t tx x u

0.01 0 0 00 0.01 0 0

~ 0,0 0 0.01 00 0 0 0.01

t N R

Example – Omni-directional AUV Measurement Model

Can only measure position (relative to known objects)

With correlated measurement noise

79

KALMAN FILTER

1 0 0 00 0 1 0t t ty x

0.4 0.1~ 0,

0.1 0.1t N Q

Example Control inputs

This time different frequencies of sinusoidal input

Simulation run for 10 seconds, or 101 time steps

Prior distribution Fairly course initialization

80

KALMAN FILTER

sin(2 )10

cos( )t

tu

t

0( ) ~ (0, )bel x N I

Example Ideal results: very low noise and disturbance levels

81

KALMAN FILTER

Example Results with larger noise and disturbances

82

KALMAN FILTER

Example Effect of decreased motion disturbances

83

KALMAN FILTER

Example Effect of decreased measurement noise

84

KALMAN FILTER

Belief mean is tradeoff between prediction and measurement

Kalman gain determines how to blend estimates

If Qt is large, inverse is small, so Kalman gain remains small When measurements are high in covariance, don’t

trust them!

If Rt is large, then so is predicted belief covariance, so Kalman gain becomes large When model is affected by large unknown

disturbances, don’t trust the predicted motion!85

KALMAN FILTER


( )t t t t t tK y C

Example Evolution of Kalman Gain

86

KALMAN FILTER

Steady state Kalman Filter For constant noise/disturbance models, it is possible

to use steady state values for the Kalman gain

Set ∑ = ∑t = ∑t-1 in the Kalman filter update equations and solve for ∑

Referred to as the Discrete Algebraic Ricatti Equation (DARE), Matlab will solve it for you

Can also run Kalman filter until convergence and then eliminate gain update step (matrix inversion)

87

KALMAN FILTER

Example Incorrect measurement distribution (covariance

actually much larger)

Estimate tracks measurements too closely88

KALMAN FILTER

Multi-Rate Kalman Filter At each time step, it is possible to use different

measurement models Time varying Ct and Qt

Identify a base update rate Find greatest common divisor of sample rates

e.g. GPS 5Hz, Sodar 12 Hz, Base rate 60 Hz Create discretized motion model at base rate At each timestep

1. Perform prediction update2. If new measurements exist, perform measurement update

for those measurements only Select appropriate Ct and Qt

89

KALMAN FILTER

Example Multi-rate Kalman Filter

Add in velocity measurements at 100 Hz Base update rate 0.01 s Create two separate types of measurement updates

Velocity only measurement for 9 time steps

Full state measurement on the 10th time step

90

KALMAN FILTER

1:9

0 1 0 00 0 0 1

C

10

1 0 0 00 1 0 00 0 1 00 0 0 1

C

1:9

0.1 0.010.01 0.05

Q

10

0.004 0 0.001 00 0.1 0 0.01

0.001 0 0.001 00 0.01 0 0.05

Q

Example Multi-rate estimation

91

KALMAN FILTER

Alternate formulation: Information Filter Provides possibility for computational savings when

taking many redundant measurements Based on information theory concepts (Fisher

Information)

Define the Information Matrix as the inverse of the covariance

Define the Information vector as

92

KALMAN FILTER

1t t

1t t t

Information Filter Substitution into the Kalman filter equation yields



93

KALMAN FILTER

11 1

1 11

( )

( )t t t t t t t

Tt t t t t

A B u

A A R

1

1t t t t t

Tt t t t t

C Q y

C Q C

Information Filter The matrix inversion is now embedded in the

prediction update Belief and predicted belief inverse depend on the number of

states For Kalman filter, gain inverse depends on the number of

measurements This can be a significant savings in some cases

To compute state estimate

Covariance already calculated

94

KALMAN FILTER

1t t t


95

OUTLINE

Kalman Filter requires linear motion and measurement models Results in compact, recursive estimation technique Not very realistic for most applications

Nonlinear models eliminate the guarantee that the belief distributions remain Gaussian No longer able to simply track mean and covariance No closed form solution to Bayes filter algorithm can

be found for general nonlinear model

96

EXTENDED KALMAN FILTER

Effect of nonlinearity on Gaussian distribution Linear transformation

97


~ (0,1)x N

2 2y x

~ ( 2,4)y N

Arbitrary distribution generation Take 5,000,000 samples of original Gaussian

Apply nonlinear transformation to each sample

Create histogram with 100 bins and normalize counts

Best Gaussian fit generation Calculate mean and covariance of 5,000,000

transformed samples

98


1

1 ni

BGi

yn

~ (0,1), 1, ,ix N i n

1tani iy x

1

11

n Ti iBG BG BG

i

y yn

Effect of nonlinearity on Gaussian distribution Nonlinear transformation

99


~ (0,1)x N

1tan ( )y x

~ ???y


100


~ (0,1)x N

1tan ( )y x

~ ( , )??y N


101


~ (0,0.1)x N

1tan ( )y x

~ ( , )y N

Extended Kalman Filter A direct generalization of the Kalman filter to

nonlinear motion and measurement models

Relies on linearization about current estimate

Works well when the problem maintains locally linear and Gaussian characteristics

Computationally similar to Kalman Filter

Covariance can diverge when approximation is poor!

102


Extended Kalman Filter Modeling Assumption Prior over the state is Gaussian

Motion model, nonlinear but still with additive Gaussian disturbances

Measurement model also nonlinear with additive Gaussian noise

Nonlinearity destroys certainty that beliefs remain Gaussian 103


0 0 0( ) ~ ( , )p x N

1( , )t t t tx g x u ~ (0, )t tN R

( )t t ty h x ~ (0, )t tN Q

Recall Kalman Filter Algorithm 1. Prediction update


Bt only enters predicted mean calculation At,Ct also affect covariance

104


1

1

t t t t tT

t t t t t

A B u

A A R

1( )( )

( )

T Tt tt t t t t

t t t t t t

tt t t

K C C C QK y C

I K C

How to update beliefs while maintaining Gaussian form of distribution?

Key idea of EKF

The mean can be propagated through the nonlinear model

The covariance can be updated with a locally linear approximation to the model

105


First Order Taylor Series Expansion

Motion model Linearize about most likely state (the previous mean)

106


1 1

1 1 1 1 11

1 1 1

( , ) ( , ) ( , ) ( )

( , ) ( )t t

t t t t t t t tt x

t t t t t

g x u g u g x u xx

g u G x

First Order Taylor Series Expansion

Measurement Model Linearize about most likely state (the predicted mean)

Both models are now linear Only valid near point of linearization

107


( ) ( ) ( ) ( )

( ) ( )t t

t t t t tt x

t t t t

h x h h x xx

h H x

Prediction Update Only new information is input ut

Prediction update is a linear transformation of belief at previous time step Motion model is

Motion disturbance, previous belief are Gaussian so this is remains addition of Gaussian distributions

Therefore the predicted mean and covariance are

108


1

1

( , ) 0 0

0t t t

Tt t t t t

g u

G G R

1 1 1( ) ~ ( , )t t tbel x N ~ (0, )t tN R

1 1 1( , ) ( )t t t t t t tx g u G x

Measurement Update Follows same arguments as Kalman Filter derivation

MMSE estimator Assume form of measurement update (linear, Kalman Gain) Substitute in approximate measurement and motion models Mean update relies on nonlinear model Gain, covariance update rely on linearization

109


1( )( ( ))

( )

T Tt tt t t t t

t t t t t

tt t t

K H H H QK y h

I K H

Extended Kalman Filter Algorithm1. Prediction Update

2. Measurement Update

110


1 1

11

1

1

( , )

( , )t t

t t tt x

t t tT

t t t t t

G g x ux

g u

G G R

1

( )

( )( ( ))

( )

t t

t tt x

T Tt tt t t t t

t t t t t

tt t t

H h xx

K H H H QK y h

I K H

Example Radar measurement of an airplane position while

flying at constant altitude and velocity

111


h

d

r

v

Example State Initial

Motion Model Linear, no input (very simple)

112


1, 1, 1 2, 1

2, 2, 1

3, 3, 1

t t t

t t

t t

x x x dtx xx x

1

2

3

x dx x v

x h

1( )t t tx g x

0

202

3x

Example Measurement Model

Using state variables

Linearization of measurement model

113


2 2tr d h

2 21, 3,t t t ty x x 2 2

1, 3,( )t t th x x x

1/22 2 11 3 1 2 2

1 1 3

1 22

h xx x xx x x

1/22 2 31 3 3 2 2

3 1 3

1 22

h xx x xx x x

Sample Code

114


%% Extended Kalman Filter Estimation% Prediction updatemup = Ad*mu;Sp = Ad*S*Ad' + R;

% Measurement updateHt = [(mup(1))/(sqrt(mup(1)^2 + mup(3)^2));

0;(mup(3))/(sqrt(mup(1)^2 + mup(3)^2))]’;

K = Sp*Ht'*inv(Ht*Sp*Ht'+Q);mu = mup + K*(y(:,t)-sqrt(mup(1)^2 + mup(3)^2));S = (eye(n)-K*Ht)*Sp;

Results Low noise, fairly accurate prior

115


0 [22 1.8 3.5]T

o State x Estimate

Dish @Origin

Results Low noise, incorrect prior

116


0 [22 1.8 6]T

o State x Estimate

Dish @Origin

Results Noisy noise, big disturbances, incorrect prior

117


0 [22 1.8 6]T

o State x Estimate

Dish @Origin

Results Symmetrically incorrect prior

118


0 [ 20 2 3]T

o State x Estimate

Dish @Origin

Summary Direct extension of KF to nonlinear models Use Taylor series expansion to find locally linear

approximations No longer optimal Most effective when covariance is low

Local linear approximation more likely to be accurate over range of distribution

Covariance update may diverge

119


120

EXTRA SLIDES

Reminder on generating multivariate random noise samples Define two distributions, the one of interest and the

standard normal distribution

If the covariance is full rank, it can be diagonalized Symmetry implies positive semidefiniteness

Can now relate the two distributions (linear identity)121

KALMAN FILTER

~ ( , )N

1/2 1/2

T

T

T

E EE I EHIH

~ (0, )N I

~ ( , )TN HIHHw

To implement this in Matlab for simulation purposes Define μ,∑

Find eigenvalues , λ, and eigenvectors, E of ∑

The noise can then be created with

122

KALMAN FILTER

1/2randn(n,1)E

4 44 8

Note on confidence ellipses Lines of constant probability

Found by setting pdf exponent to a constant Principal axes are eigenvectors of covariance Magnitudes depend on eigenvalues of

covariance

123

KALMAN FILTER

1 4 4, =

2 4 8

50%, 99% error ellipsesNot easily computed,

code provided

The EKF used linearization about the predicted/previous state estimate to update the mean and covariance of the current estimate Approximation of a nonlinear transformation of a

Gaussian distribution by linear transformation of the mean and covariance

There are other ways to approximate this transformation Unscented transform leads to better estimates of

resulting mean and covariance in some cases Relies on a set of samples known as sigma points or

particles, that get transformed directly UKF first published in 1997, still being discussed,

extended, solidified. 124

UNSCENTED KALMAN FILTER

Key idea: Unscented transform It is more accurate to approximate a distribution

using samples than it is to approximate an arbitrary nonlinear function through linearization.

Let’s first go back to the nonlinear function of a Gaussian and see what the EKF is doing.

125



126

UNSCENTED TRANSFORM

~ (0,1)x N

1tan ( 1 / 2)y x

~ ( , )??y N

Nonlinear distribution generation Take 5,000,000 samples of original Gaussian

Apply nonlinear transformation to each sample

Create histogram with 100 bins and normalize counts

Best Gaussian fit generation Calculate mean and covariance of 5,000,000

transformed samples

127

UNSCENTED TRANSFORM

1

1 ni

BGi

yn

~ (0,1), 1, ,ix N i n

1tan 1/ 2i iy x

1

11

n Ti iBG BG BG

i

y yn

128

UNSCENTED TRANSFORM

Extended Kalman Filter approximation generation Linearize nonlinear function about mean

Propagate mean through nonlinear function and covariance through linearized function

129

UNSCENTED TRANSFORM

1

2

tan 1/ 2

11/ 2 1

x

G xx

1tan 1/ 2EKF T

EKF G G

130


Linearization over-predicts mean shift Assumes symmetry of

atan

Covariance over-predicted as well atan has effect of piling

up tails at +/- 1.57

131

LINEARIZATION

The unscented transform can also be used Linearization is a first order approximation The unscented transform is second order accurate,

and can be tuned to reduce fourth order errors

The transform relies on a set of specially chosen samples known as sigma points 2n+1 points chosen to capture the transformation of

the distribution

132

UNSCENTED TRANSFORM

In 1D case, the unscented transform select 3 points

And we select weights so that we can recover the original mean and variance

133

UNSCENTED TRANSFORM

[0]

[1]

[2]

2[ ] [ ]

0

i im

i

w

2 22 [ ] [ ]

0

i ic

i

w

We then pass the sigma points through the nonlinear function

And construct the new mean and variance using the same weights

134

UNSCENTED TRANSFORM

[ ] [ ]( )i if

2[ ] [ ]

0

i im

i

w

2 22 [ ] [ ]

0

i ic

i

w

135

UNSCENTED TRANSFORM

In general, the sigma points are chosen as follows

Generalized Std. Dev. is square root of covariance Here the square root of the covariance matrix is

ambiguous, but must satisfy Can use sqrtm, which returns the unique solution with non-

negative eigenvalues, Or use chol, the cholesky decomposition, which returns an

upper triangular square root and is very efficient Assumes symmetry

136

UNSCENTED TRANSFORM

[0]

[ ]

[ ]

, 1, ,

, 1, ,

i

i

n i

i

n i n

n i n

TA B A A B

The parameter λ defines the weights to use for generating the mean and covariance, can be tuned

α governs the spread of the sigma points about the mean the larger the α the larger the spread of sigma points Usually,

κ ensures positive semi-definiteness if Can be left at 0 safely (ignored) Also affects the spread of sigma points

137

UNSCENTED TRANSFORM

2 n n

0

0 1

The sigma points are then propagated through the nonlinear function

And a mean and covariance is extracted using special weights for the sigma points

138

UNSCENTED TRANSFORM

[ ] [ ]( )i if

2[ ] [ ]

0

ni i

mi

w

2

[ ] [ ] [ ]

0

n Ti i ic

i

w

The weights are defined as

With another tunable parameter β, Can be ignored Or set to 2

reduces errors in some of the fourth order terms for a Gaussian prior

139

UNSCENTED TRANSFORM

[0]

[ ] 1 , 1, ,22( )

m

im

wn

w i nn

[0] 2

[ ]

1

1 , 1, ,22( )

c

ic

wn

w i nn

Select

140

UNSCENTED TRANSFORM

0.01, 0, 0

Select

141

UNSCENTED TRANSFORM

1.45, 0, 0

Select

142

UNSCENTED TRANSFORM

1.63, 0, 2

Incorporating this method of distribution transformation into the Bayesian framework is possible

There are two nonlinear functions to deal with Two unscented transforms are needed per timestep

The measurement model depends on the state we are trying to estimate The state is augmented by the measurement noise states

and a joint probability density function is updated

143


Example repeat Radar measurement of an airplane position while

flying at constant altitude and velocity

144


h

d

r

v

Example State Initial

Motion Model

Measurement Model 145


1, 1, 1 2, 1

2, 2, 1

3, 3, 1

t t t

t t

t t

x x x dtx xx x

1

2

3

x dx x v

x h

0

202

3x

2 21, 3,t t t ty x x

Simulation results- low disturbances, noise

146


Error plot for position error

147


Simulation results, higher disturbances

148


Error plot for position error

149


Unscented Kalman Filter Modeling Assumptions Prior over the state is Gaussian

Motion model, nonlinear but still with additive Gaussian disturbances

Measurement model also nonlinear with additive Gaussian noise

Nonlinearity destroys certainty that beliefs remain Gaussian 150


0 0 0( ) ~ ( , )p x N

1( , )t t t tx g x u ~ (0, )t tN R

( )t t ty h x ~ (0, )t tN Q

Prediction step Propagation of belief at t-1 through motion model

Pick sigma points

Propagate through motion model

151


1

[0]1 1

[ ]1 1 1

[ ]1 1

, 1, ,

, 1, ,t

t t

it t t

i

n it t

i

n i n

n i n

[ ] [ ]1,

i it t tg u

Prediction step Unscented prediction step

Calculate mean and covariance, adding motion covariance to result

152


2[ ] [ ]

0

ni i

t m ti

w

2

[ ] [ ] [ ]

0

n Ti i it c t t t t t

i

w R

Measurement step Recall from Bayes filter, we are trying to define

We have the mean and covariance of predicted belief

We need to propagate this belief through another unscented transform To do this, we need to look at the joint unscented transform

153


( ) ( | ) ( )t t t tbel x p y x bel x

Joint transform with additive noise in model

Mean and covariance is found as before Generate sigma points Propagate through model Find mean as before and covariance, cross-covariance as

154


~ ,( )

x x xy

y xy y

xN

y h x

2

[ ] [ ] [ ]

0

n Ti i ic

i

w

2

[ ] [ ] [ ]

0

n Ti i ic

i

w Q

~ 0,N Q

Magic trick (Schur’s complement) If

Then

Can in fact derive KF updates using this as well

155


1 1| , Tx y xy y x y xy yp x y N y

~ ,x x xy

y xy y

xN

y

Measurement Step Form the joint distribution of given all inputs

and all but the latest measurement

Solve for all components and apply Schur’scomplement

But some of these we know already

156


,t tx y

1: 1 1:, | , ,t t t t

t t t t

x x x yt t t t

y x y y

p x y y u N

1: 1 1:| ,t t t tp x y u bel t

t

x t

x t

Measurement step The rest we can approximate with the unscented

transform Generate new sigma points from the predicted belief

Propagate through measurement model

157


[0]

[ ]

[ ]

, 1, ,

, 1, ,t

t t

it t t

i

n it t

i

n i n

n i n

[ ] [ ]i it th

Measurement step Then the measurement terms and cross terms can be

approximated as

158


2

[ ] [ ] [ ]

0t t t t

n Ti i ix y c t x t y

i

w

2

[ ] [ ] [ ]

0t t t

n Ti i iy c y y t

i

w Q

2[ ] [ ]

0t

ni i

y m ti

w

Measurement Step Finally, applying Schur’s complement

To the above joint distribution

And therefore,

159


1 1| , Tx y xy y x y xy yp x y N y

1: 1:| , ( ) ,t t t t t tp x y u bel x N

1

1

t t t t

t t t t

t t y x y t y

Tt t y x y y

y

Summary Prediction Step

160


1

[0]1 1

[ ]1 1 1

[ ]1 1

, 1, ,

, 1, ,t

t t

it t t

i

n it t

i

n i n

n i n

[ ] [ ]1,

i it t tg u

2[ ] [ ]

0

ni i

t m ti

w

2

[ ] [ ] [ ]

0

n Ti i it c t t t t t

i

w R

Select sigma points

Apply motion model

Extract mean and covariance

Summary Measurement step

161


[0]

[ ]

[ ]

, 1, ,

, 1, ,t

t t

it t t

i

n it t

i

n i n

n i n

[ ] [ ]i it th

2

[ ] [ ] [ ]

0t t t

n Ti i iy c y y t

i

w Q

2[ ] [ ]

0t

ni i

y m ti

w

Select sigma points

Apply measurement model

Extract mean and covariance

Summary Measurement Step

162


2

[ ] [ ] [ ]

0t t t t

n Ti i ix y c t x t y

i

w

1

1

t t t t

t t t t

t t y x y t y

Tt t y x y y

y

Extract cross-covariance

Update belief using Schur’scomplement

Summary Similar computation time to EKF

longer due to square root and inverse

Potentially capable of reducing errors in propagation of beliefs through nonlinear functions

Tuning effects unclear, can lead to strange results

Benefit minimal when nonlinearities are modest, or uncertainty is low

163


Download - ME 597: AUTONOMOUS MOBILE ROBOTICS SECTION STIMATION …wavelab.uwaterloo.ca/sharedata/ME597/ME597_Lecture... · 2020. 1. 24. · ME 597: AUTONOMOUS MOBILE ROBOTICS SECTION 7 –

Top Related