Kalman Predictor/Filter - Semantic Scholar

Kalman Predictor/Filter

ME 450 – Data-driven Modeling and Robust Control

1

Stochastic Processes Optimal Estimation and Prediction Let us assume now that our plant is:

with initial condition x0 given, and where wk and vk are independent zero-mean, white stochastic processes (independent of x0).

⎛ ⎡w k ⎤ E ( w k ) = 0, E (v k ) = 0, E ⎜ ⎢ ⎥[ w Tl ⎝ ⎣ v k ⎦

⎞ ⎡Qk v ]⎟ = ⎢ ⎠ ⎣ 0 T l

0 ⎤ ⎥δ k,l , Rk ⎦

Qk ,Rk ≥ 0 ∀k

Prediction Problem: How can we estimate xk given measurements of {ul, yl: l≤k-d}? We denote the d-step-ahead prediction as

€ Filtering Problem: How do we compute the filtered estimate ME 450 – Data-driven Modeling and Robust Control

? 2

Stochastic Processes How do we take two independent, unbiased measurements, x1 and x2, with variances σ12 and σ22, of the same quantity x and combine them to get a better, unbiased estimate?

xˆ = ax1 + (1 − a) x 2 Covariance

€

( ) ∂E (( xˆ − E ( xˆ )) ) = 2aσ 2

unbiased 2

E ( xˆ − E ( xˆ )) = a σ + (1 − a) σ22 2

2 1

2

€

€

2 σ 2 2 2 − 2 1 − a σ = 0 ⇒ a = ( ) 1 2 σ12 + σ22

∂a σ22 σ12 xˆ = 2 2 x1 + 2 2 x2 σ1 + σ2 σ1 + σ2


3

Stochastic Processes σ22 σ12 xˆ = 2 2 x1 + 2 2 x2 σ1 + σ2 σ1 + σ2 Linear, variance-weighted sum. Nothing mysterious.

€ 2 2σ12σ22 2 −1 2 −1 E ( xˆ ) = 2 2 = (σ1 ) + (σ 2 ) σ1 + σ2

[

€

]

−1

≤ min{σ12 ,σ22 }

The quality of the estimation is improved. Can we do similar things when x satisfies a state equation? We just need to propagate means and variances. •  taking into account the effect of state equation (easy, deterministic) •  taking into account the new measurements (a bit harder, conditional probability)


4

Stochastic Processes Minimum Variance Estimates We want to find the estimate

(

E x k − xˆ k

which minimizes 2

)

y k −d , y k −d −1,…

The minimum variance estimate is given by the conditional mean

€

(

Most important property in estimation

)

xˆ k = E x k y k −d , y k −d −1,…

2 ⎞ ⎛⎜ T T T ⎟ ˆ ˆ ˆ ˆ E x − x Y = E x x Y − 2E x x Y + E x Proof: k) k k k k k xk Y ⎝ ( k ⎠ 2 € = xˆ k − E x k Y + E x Tk x k Y − E x k Y

(

)

(

is minimized when

(

)) = E(x Y)

(

xˆ k

) ( ) ( ) ( ( ))

2

k

The conditional mean is the least square estimate.

€


5

Stochastic Processes Jointly Gaussian Random variables x is a gaussian random n-vector if its probability density function is of the form

1 1 − 12 ( x −m ) T ∑ −1 (x −m ) p( x ) = n /2 1/ 2 e (2π ) ∑ where m is the mean value of x, E(x), and non-singular matrix Σ is the covariance matrix, E[(x-m)(x-m)T]. If x€is gaussian N(m, Σ) then y=Ax+b is gaussian N(Am+b, AΣAT). If x and y are commensurate gaussian random n-vectors, then x+y is a gaussian random n-vector N(mx+my, Σx+y).


6

Stochastic Processes If x and y are jointly gaussian, i.e, [xT yT]T is a gaussian process N([mxT myT], Σ) with Then, −1 E [ x y ] = mx − ∑ xy ∑ −1 m + ∑ ∑ yy y xy yy y

(1)

also a gaussian random n-vector. 2 ⎤ 2 ⎤ ⎡ ⎡ −1 E€ x − E x y y = E x − E x y = ∑ −∑ ∑ [ ] [ ] xx xy yy ∑ yx ⎢⎣ ⎥⎦ ⎢⎣ ⎥⎦

(

)

(

)

(2)

The gaussian conditional-mean estimate has the conditional error variance equal to the unconditional error variance (not true if not gaussian).

[

]

E x − E [ x y] y = E [ x y] − E [ x y] = 0 The conditional-mean estimate is unbiased. ME 450 – Data-driven Modeling and Robust Control

7

Stochastic Processes Kalman Filter State equations:

Assumptions: •  x0 is gaussian with mean

and covariance

.

•  wk is gaussian, zero-mean, white process ( E ( w k w Tl ) = Qkδ k,l ), independent of x0 and vk. •  vk is gaussian, zero-mean, white process ( € independent of x0 and wk.

E (v k v Tl ) = Rkδ k,l ),

Everything is gaussian --- linear operations € preserve gaussian properties ME 450 – Data-driven Modeling and Robust Control

8

Stochastic Processes ⎛ ⎡ x 0 ⎤ ⎡ P ⎡ x 0 ⎤ 0 ⎜ N , is gaussian ⎢ ⎥ ⎢ ⎥ ⎢ ⎜ ⎣ y 0 ⎦ ⎝ ⎣C0 x 0 ⎦ ⎣C0 P0 From (1):

−1

⎤⎞ P0C0T ⎥⎟⎟ T C0 P0C0 + R0 ⎦⎠ −1

xˆ 0|0 = x 0 − P0C (C0 P0C + R0 ) C0 x 0 + P0C (C0 P0C + R0 ) y 0 T 0

€

T 0

€P0C0T (C0 P0C0T + R0 ) = x0 +

From (2): ∑ 0|0

€

Time update:

€

−1

T 0

( y 0 − C0 x 0 )

−1

= P0 − P0C (C0 P0C + R0 ) C0 P0 T 0

T 0

xˆ1|0 = A0 xˆ 0|0 + B0 u0 ∑1|0 = A0 ∑ 0|0 A0T + Q0

€ €

is gaussian

T 0

Filter Filter covariance

One-step predictor Predictor covariance

N (C1 xˆ1|0 ,C1 ∑1|0 C1T + R1 )


9

Stochastic Processes ⎡ x1 ⎤ ⎢ ⎥ y 0 ⎣ y1 ⎦

⎛ ⎡ xˆ1|0 ⎤ ⎡ ∑ 1|0 N ⎜⎜ ⎢ , ⎥ ⎢ ˆ ⎝ ⎣C1 x1|0 ⎦ ⎣C1 ∑1|0

is gaussian

⎤⎞ ∑1|0 C1T ⎥⎟⎟ T C1 ∑1|0 C1 + R1 ⎦⎠

−1

From (1):

T 1

T 1

−1

= xˆ1|0€ + ∑1|0 C1T (C1 ∑1|0 C1T + R1) ( y1 − C1 xˆ1|0 )

€ From (2):

−1

xˆ1|1 = xˆ1|0 − ∑1|0 C (C1 ∑1|0 C + R1) C1 xˆ1|0 + ∑1|0 C (C1 ∑1|0 C + R1 ) y1 T 1

−1

∑1|1 = ∑1|0 −∑1|0 C (C1 ∑1|0 C + R1) C1 ∑1|0 T 1

€

Time update:

€ € €

T 1

T 1

Filter Filter covariance

xˆ 2|1 = A1 xˆ1|1 + B1u1

One-step predictor

∑ 2|1 = A1 ∑1|1 A1T + Q1

Predictor covariance

is gaussian

N (C2 xˆ 2|1,C2 ∑ 2|1 C2T + R2 )


10

Stochastic Processes Generalizing, for the plant

−1

xˆ k|k = xˆ k|k −1 + ∑ k|k −1 C (Ck ∑ k|k −1 C + Rk ) ( y k − Ck xˆ k|k −1) T k

T k

xˆ k +1|k = Ak xˆ k|k + Bk uk €

Filter Predictor

−1

∑ k|k = ∑ k|k −1 −∑ k|k −1 C (Ck ∑ k|k −1 C + Rk ) Ck ∑ k|k −1 T k

T k

∑ k +1|k = Ak ∑ k|k AkT + Qk

€ €

These are the discrete-time Kalman filtering equations They consist of time update and measurement update parts One-step-ahead predictions and filtered estimates are given The prediction error is used to update the state estimate ME 450 – Data-driven Modeling and Robust Control

11

Stochastic Processes Kalman Filter properties

x k +1 = Ak x k + Bk uk + w k y k = Ck x k + v k

Kalman Predictor:

xˆ k +1|k = ( Ak − M k Ck ) xˆ k|k −1 + Bk uk + M k y k M k = Ak ∑ k|k −1 C (Ck ∑ k|k −1 C + Rk ) T k

€

T k

−1

−1

∑ k +1|k = Ak ∑ k|k −1 A − Ak ∑ k|k −1 C (Ck ∑ k|k −1 C + Rk ) Ck ∑ k|k −1 AkT + Qk T k

T k

T k

Kalman Filter:

xˆ k|k = ( I − Lk Ck ) Ak xˆ k −1|k −1 + ( I − Lk Ck ) Bk −1uk −1 + Lk y k €

Lk = ∑ k|k −1 C (Ck ∑ k|k −1 C + Rk ) T k

T k

−1

∑ k|k = ( I − Lk Ck ) ∑ k|k −1 •  The Kalman filter and the Kalman predictor are state observers! •  satisfies a Riccati Difference Equation (RDE) ME € 450 – Data-driven Modeling and Robust Control

12

Stochastic Processes Kalman Filter properties Kalman Filter RDE: −1


T k

T k

LQR RDE:

€ −1

Sk = A Sk +1 Ak − A Sk +1Bk ( B Sk +1Bk + Rk ) BkT Sk +1 Ak + Qk T k

€

Covariances:

T k

[ = E [( x

T k

∑ k|k −1 = E ( x k − xˆ k|k −1 )( x k − xˆ k|k −1 ) ∑ k|k

k

− xˆ k|k )( x k − xˆ k|k )


T

T

]

] 13

€

Stochastic Processes Kalman Filter properties Observer error equations: Kalman Predictor:

x k +1 − xˆ k +1|k = Ak x k + Bk uk + w k − ( Ak − M k Ck ) xˆ k|k −1 − Bk uk − M k (Ck x k + v k ) x˜ k +1|k = ( Ak − M k Ck ) x˜ k|k −1 + w k − M k v k Kalman Filter:

x k +1 − xˆ k +1|k +1 = Ak x k + Bk uk + w k − ( I − Lk +1Ck +1 )( Ak xˆ k|k + Bk uk ) + Lk +1 (Ck +1 x k +1 + v k +1 ) x˜ k +1|k +1 = ( I − Lk +1Ck +1 ) Ak x˜ k|k − Lk +1v k +1 + ( I − Lk +1Ck +1 ) w k


14

Stochastic Processes Kalman Filter properties

x˜ k +1|k = ( Ak − M k Ck ) x˜ k|k −1 + w k − M k v k Use this equation to compute the covariance propagation

E ( x˜ k +1|k x˜ €

T k +1|k

)=X

T

T = A − M C X A − M C + M R M ( ) ( ) k +1 k k k k k k k k k k + Qk

The Kalman filter is an observer in which we balance •  the stabilizing properties of big gain Mk •  the role of this stability in reducing the Qk effect effect in Xk •  the amplification of the measurement noise variance Rk effect in Xk The Kalman filter is an optimal observer which balances the smoothing of measurements noise vk against the tracking of the state knocked around by wk. ME 450 – Data-driven Modeling and Robust Control

15

Stochastic Processes Kalman Filter properties •  The Kalman filter is a linear, discrete-time, finite-dimensional system. •  The input of the filter is yk and the output is

.

•  The gains Mk, Lk and covariance matrix do not depend on the data and can be pre-computed before the filter is run. •  The matrix

is the error covariance matrix of the state estimator

[

∑ k|k −1 = E ( x k − xˆ k|k −1 )( x k − xˆ k|k −1 )

T

]

• Even if the underlying signal model [A, B, C, Q, R] is time-invariant, the resulting Kalman filter will be time-varying unless

€ ME 450 – Data-driven Modeling and Robust Control

16

Stochastic Processes Kalman Filter properties •  The Kalman filter equations are dual of the LQR equations.

• The LQR RDE is replaced by the KF RDE evolving forwards in time.


17

Stochastic Processes Extended Kalman Filter For the nonlinear signal model

x k +1 = f ( x k ,uk ) + w k y k = h( x k ) + v k

with the usual assumptions holding and all functions sufficiently continuously differentiable. The extended Kalman filter is given by

xˆ k +1|k = f ( xˆ k|k ,uk ) € xˆ k|k = xˆ k|k −1 + Lk y k − h ( xˆ k|k −1 )

(

)

Lk = ∑ k|k −1 C (Ck ∑ k|k −1 C + Rk ) T k

T k

−1

−1


T k

T k

∂f ∂h ,Ck = ∂x xˆk|k ∂x xˆk|k The Extended Kalman Filter is a non-linear filter! The linearization is only in the gain calculation. Ak =


€

18