Upload
others
View
5
Download
0
Embed Size (px)
Citation preview
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
BAYES FILTER . . .11x 1nx
%% 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
BAYES FILTER . . .11x 1nx
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
BAYES FILTER . . .11x 1nx
%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
BAYES FILTER . . .11x 1nx
…%% 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
Bayes Filter Framework Kalman Filter Extended Kalman Filter Particle 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
1( )T Tt tt t t t tK C C C Q
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
1. Prediction update
2. Measurement update
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
1( )T Tt tt t t t tK C C C Q
( )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
1. Prediction update
2. Measurement update
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
Bayes Filter Framework Kalman Filter Extended Kalman Filter Particle Filter
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
EXTENDED KALMAN FILTER
~ (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
EXTENDED KALMAN FILTER
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
EXTENDED KALMAN FILTER
~ (0,1)x N
1tan ( )y x
~ ???y
Effect of nonlinearity on Gaussian distribution Nonlinear transformation
100
EXTENDED KALMAN FILTER
~ (0,1)x N
1tan ( )y x
~ ( , )??y N
Effect of nonlinearity on Gaussian distribution Nonlinear transformation
101
EXTENDED KALMAN FILTER
~ (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
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
EXTENDED KALMAN FILTER
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
2. Measurement update
Bt only enters predicted mean calculation At,Ct also affect covariance
104
EXTENDED 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
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
EXTENDED KALMAN FILTER
First Order Taylor Series Expansion
Motion model Linearize about most likely state (the previous mean)
106
EXTENDED KALMAN FILTER
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
EXTENDED KALMAN FILTER
( ) ( ) ( ) ( )
( ) ( )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
EXTENDED KALMAN FILTER
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
EXTENDED KALMAN FILTER
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
EXTENDED KALMAN FILTER
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
EXTENDED KALMAN FILTER
h
d
r
v
Example State Initial
Motion Model Linear, no input (very simple)
112
EXTENDED KALMAN FILTER
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
EXTENDED KALMAN FILTER
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
%% 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
EXTENDED KALMAN FILTER
0 [22 1.8 3.5]T
o State x Estimate
Dish @Origin
Results Low noise, incorrect prior
116
EXTENDED KALMAN FILTER
0 [22 1.8 6]T
o State x Estimate
Dish @Origin
Results Noisy noise, big disturbances, incorrect prior
117
EXTENDED KALMAN FILTER
0 [22 1.8 6]T
o State x Estimate
Dish @Origin
Results Symmetrically incorrect prior
118
EXTENDED KALMAN FILTER
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
EXTENDED KALMAN FILTER
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
UNSCENTED KALMAN FILTER
Effect of nonlinearity on Gaussian distribution Nonlinear transformation
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
UNSCENTED KALMAN FILTER
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
UNSCENTED KALMAN FILTER
Example repeat Radar measurement of an airplane position while
flying at constant altitude and velocity
144
UNSCENTED KALMAN FILTER
h
d
r
v
Example State Initial
Motion Model
Measurement Model 145
UNSCENTED KALMAN FILTER
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
UNSCENTED KALMAN FILTER
Error plot for position error
147
UNSCENTED KALMAN FILTER
Simulation results, higher disturbances
148
UNSCENTED KALMAN FILTER
Error plot for position error
149
UNSCENTED KALMAN FILTER
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
UNSCENTED KALMAN FILTER
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
UNSCENTED KALMAN FILTER
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
UNSCENTED KALMAN FILTER
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
UNSCENTED KALMAN FILTER
( ) ( | ) ( )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
UNSCENTED KALMAN FILTER
~ ,( )
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
UNSCENTED KALMAN FILTER
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
UNSCENTED KALMAN FILTER
,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
UNSCENTED KALMAN FILTER
[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
UNSCENTED KALMAN FILTER
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
UNSCENTED KALMAN FILTER
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
UNSCENTED KALMAN FILTER
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
UNSCENTED KALMAN FILTER
[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
UNSCENTED KALMAN FILTER
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
UNSCENTED KALMAN FILTER