Lecture 4: Extended Kalman filter and Statistically Linearized Filter

Transcription

1 Lecture 4: Extended Kalman filter and Statistically Linearized Filter Department of Biomedical Engineering and Computational Science Aalto University February 17, 2011

2 Contents 1 Overview of EKF 2 Linear Approximations of Non-Linear Transforms 3 Extended Kalman Filter 4 Properties of Extended Kalman Filter 5 Statistically Linearized Filter 6 Properties of Statistically Linearized Filter 7 Summary

3 EKF Filtering Model Basic EKF filtering model is of the form: x k = f(x k 1 )+q k 1 y k = h(x k )+r k, x k R n is the state y k R m is the measurement q k 1 N(0, Q k 1 ) is the Gaussian process noise r k N(0, R k ) is the Gaussian measurement noise f( ) is the dynamic model function h( ) is the measurement model function.

4 Bayesian Optimal Filtering Equations The EKF model is clearly a special case of probabilistic state space models with p(x k x k 1 ) = N(x k f(x k 1 ), Q k 1 ) p(y k x k ) = N(y k h(x k ), R k ) Recall the formal optimal filtering solution: p(x k y 1:k 1 ) = p(x k x k 1 ) p(x k 1 y 1:k 1 ) dx k 1 p(x k y 1:k ) = 1 Z k p(y k x k ) p(x k y 1:k 1 ) No closed form solution for non-linear f and h.

5 The Idea of Extended Kalman Filter In EKF, the non-linear functions are linearized as follows: f(x) f(m)+f x (m)(x m) h(x) h(m)+h x (m)(x m) where x N(m, P), and F x, H x are the Jacobian matrices of f, h, respectively. Only the first terms in linearization contribute to the approximate means of the functions f and h. The second term has zero mean and defines the approximate covariances of the functions. Let s take a closer look at transformations of this kind.

6 Linear Approximations of Non-Linear Transforms [1/4] Consider the transformation of x into y: x N(m, P) y = g(x). The probability density of y is now non-gaussian: p(y) = J(y) N(g 1 (y) m, P), Taylor series expansion of g on mean m: g(x) = g(m+δx) = g(m)+g x (m)δx where δx = x m. + i 1 2 δxt G (i) xx(m)δx e i +...

7 Linear Approximations of Non-Linear Transforms [2/4] First order, that is, linear approximation: g(x) g(m)+g x (m)δx Taking expectations on both sides gives approximation of the mean: E[g(x)] g(m) For covariance we get the approximation: Cov[g(x)] = E [(g(x) E[g(x)]) (g(x) E[g(x)]) T] E [(g(x) g(m)) (g(x) g(m)) T] G x (m) P G T x(m)

8 Linear Approximations of Non-Linear Transforms [3/4] In EKF we will need the joint covariance of x and g(x)+q, where q N(0, Q). Consider the pair of transformations x N(m, P) q N(0, Q) y 1 = x y 2 = g(x)+q. Keeping q fixed and applying the linear approximation gives [( ) ] ( ) x m E q g(x)+q g(m)+q [( ) ] ( x P P G Cov q T x (m) ) g(x)+q G x (m) P G x (m) P G T x(m)

9 Linear Approximations of Non-Linear Transforms [4/4] Taking expectation w.r.t. random variable q just adds Q to the lower right block of covariance, thus we get: Linear Approximation of Non-Linear Transform The linear Gaussian approximation to the joint distribution of x and y = g(x)+q, where x N(m, P) and q N(0, Q) is ( ) (( ) ( )) x m P CL N,, y where µ L = g(m) µ L C T L S L S L = G x (m) P G T x(m)+q C L = P G T x(m).

10 Derivation of EKF [1/4] Assume that the filtering distribution of previous step is Gaussian p(x k 1 y 1:k 1 ) N(x k 1 m k 1, P k 1 ) The joint distribution of x k 1 and x k = f(x k 1 )+q k 1 is non-gaussian, but can be approximated linearly as ([ ] ) xk 1 p(x k 1, x k, y 1:k 1 ) N m, P, where ( ) m mk 1 = f(m k 1 ) ( P P = k 1 F x (m k 1 ) P k 1 x k P k 1 F T x (m ) k 1) F x (m k 1 ) P k 1 F T x (m. k 1)+Q k 1

11 Derivation of EKF [2/4] Recall that if x and y have the joint Gaussian probability density ( ) (( ) ( )) x a A C N, y b C T, B then y N(b, B) Thus, the approximate predicted distribution of x k given y 1:k 1 is Gaussian with moments m k = f(m k 1) P k = F x(m k 1 ) P k 1 F T x (m k 1)+Q k 1

12 Derivation of EKF [3/4] The joint distribution of x k and y k = h(x k )+r k is also non-gaussian, but by linear approximation we get ([ ] ) xk p(x k, y k y 1:k 1 ) N m, P, where ( m m ) = k h(m k ( ) P P = k P k HT x(m k ) ) H x (m k ) P k H x (m k ) P k HT x(m k )+R k y k

13 Derivation of EKF [4/4] Recall that if then Thus we get where ( ) (( ) ( )) x a A C N, y b C T, B x y N(a+C B 1 (y b), A C B 1 C T ). p(x k y k, y 1:k 1 ) N(x k m k, P k ), m k = m k + P k HT x (H x P k HT x + R k) 1 [y k h(m k )] P k = P k P k HT x (H x P k HT x + R k) 1 H x P k

14 EKF Equations Extended Kalman filter Prediction: Update: m k = f(m k 1) P k = F x(m k 1 ) P k 1 F T x (m k 1)+Q k 1. v k = y k h(m k ) S k = H x (m k ) P k HT x (m k )+R k K k = P k HT x(m k ) S 1 k m k = m k + K k v k P k = P k K k S k K T k.

15 EKF Example [1/2] Pendulum with mass m = 1, pole length L = 1 and random force w(t): d 2 θ = g sin(θ)+w(t). dt2 In state space form: ( ) ( d θ dθ/dt = dt dθ/dt g sin(θ) ) ( ) 0 + w(t) Assume that we measure the x-position: y k = sin(θ(t k ))+r k,

16 EKF Example [2/2] If we define state as x = (θ, dθ/dt), by Euler integration with time step t we get ( ) ( x 1 k x 1 xk 2 = k 1 + x2 k 1 t ) ( ) 0 xk 1 2 g sin(x1 k 1 }{{ ) t + q k 1 } f(x k 1 ) y k = sin(xk 1 }{{} ) +r k, h(x k ) The required Jacobian matrices are: ( ) 1 t F x (x) = g cos(x 1, H ) t 1 x (x) = ( cos(x 1 ) 0 )

17 Advantages of EKF Almost same as basic Kalman filter, easy to use. Intuitive, engineering way of constructing the approximations. Works very well in practical estimation problems. Computationally efficient. Theoretical stability results well available.

18 Limitations of EKF Does not work in considerable non-linearities. Only Gaussian noise processes are allowed. Measurement model and dynamic model functions need to be differentiable. Computation and programming of Jacobian matrices can be quite error prone.

19 The Idea of Statistically Linearized Filter In SLF, the non-linear functions are statistically linearized as follows: where x N(m, P). f(x) b f + A f (x m) h(x) b h + A h (x m) The parameters b f, A f and b h, A h are chosen to minimize the mean squared errors of the form MSE f (b f, A f ) = E[ f(x) b f A f δx) 2 ] MSE h (b h, A h ) = E[ h(x) b h A h δx 2 ] where δx = x m. Describing functions of the non-linearities with Gaussian input.

20 Statistical Linearization of Non-Linear Transforms [1/4] Again, consider the transformations x N(m, P) y = g(x). Form linear approximation to the transformation: where δx = x m. g(x) b+aδx, Instead of using the Taylor series approximation, we minimize the mean squared error: MSE(b, A) = E[(g(x) b Aδx) T (g(x) b Aδx)]

21 Statistical Linearization of Non-Linear Transforms [2/4] Expanding the MSE expression gives: MSE(b, A) = E[g T (x) g(x) 2 g T (x) b 2 g T (x) Aδx Derivatives are: + b T b 2 } b T {{ Aδx } +δx } T A{{ T Aδx} ] =0 tr{a P A T } MSE(b, A) b MSE(b, A) A = 2 E[g(x)]+2 b = 2 E[g(x)δx T ]+2 A P

22 Statistical Linearization of Non-Linear Transforms [3/4] Setting derivatives with respect to b and A zero gives b = E[g(x)] A = E[g(x)δx T ] P 1. Thus we get the approximations E[g(x)] E[g(x)] Cov[g(x)] E[g(x)δx T ] P 1 E[g(x)δx T ] T. The mean is exact, but the covariance is approximation. The expectations have to be calculated in closed form!

23 Statistical Linearization of Non-Linear Transforms [4/4] Statistical linearization The statistically linearized Gaussian approximation to the joint distribution of x and y = g(x)+q where x N(m, P) and q N(0, Q) is given as where ( ) x N y µ S = E[g(x)] (( m µ S ), ( P CS C T S S S )), S S = E[g(x)δx T ] P 1 E[g(x)δx T ] T + Q C S = E[g(x)δx T ] T.

24 Statistically Linearized Filter [1/3] The statistically linearized filter (SLF) can be derived in the same manner as EKF. Statistical linearization is used instead of Taylor series based linearization. Requires closed form computation of the following expectations for arbitrary x N(m, P): where δx = x m. E[f(x)] E[f(x)δx T ] E[h(x)] E[h(x)δx T ],

25 Statistically Linearized Filter [2/3] Statistically linearized filter Prediction (expectations w.r.t. x k 1 N(m k 1, P k 1 )): m k = E[f(x k 1)] P k = E[f(x k 1)δx T k 1 ] P 1 k 1 E[f(x k 1)δx T k 1 ]T + Q k 1, Update (expectations w.r.t. x k N(m k, P k )): v k = y k E[h(x k )] S k = E[h(x k )δx T k ](P k ) 1 E[h(x k )δx T k ]T + R k K k = E[h(x k )δx T k ]T S 1 k m k = m k + K k v k P k = P k K k S k K T k.

26 Statistically Linearized Filter [3/3] If the function g(x) is differentiable, we have E[g(x)(x m) T ] = E[G x (x)] P, where G x (x) is the Jacobian of g(x), and x N( m, P). In practice, we can use the following property for computation of the expectation of the Jacobian: µ(m) = E[g(x)] µ(m) m = E[G x(x)]. The resulting filter resembles EKF very closely. Related to replacing Taylor series with Fourier-Hermite series in the approximation.

27 Statistically Linearized Filter: Example [1/2] Recall the discretized pendulum model ( ) ( x 1 k x 1 xk 2 = k 1 + x2 k 1 t ) ( ) 0 xk 1 2 g sin(x1 k 1 }{{ ) t + q k 1 } f(x k 1 ) y k = sin(xk 1 }{{} ) +r k, h(x k ) If x N(m, P), by brute-force calculation we get ( ) m E[f(x)] = 1 + m 2 t m 2 g sin(m 1 ) exp( P 11 /2) t E[h(x)] = sin(m 1 ) exp( P 11 /2)

28 Statistically Linearized Filter: Example [2/2] The required cross-correlation for prediction step is ( ) E[f(x)(x m) T c11 c ] = 12, c 21 c 22 where c 11 = P 11 + t P 12 c 12 = P 12 + t P 22 c 21 = P 12 g t cos(m 1 ) P 11 exp( P 11 /2) c 22 = P 22 g t cos(m 1 ) P 12 exp( P 11 /2) The required term for update step is ( ) E[h(x)(x m) T cos(m1 ) P ] = 11 exp( P 11 /2) cos(m 1 ) P 12 exp( P 11 /2)

29 Advantages of SLF Global approximation, linearization is based on a range of function values. Often more accurate and more robust than EKF. No differentiability or continuity requirements for measurement and dynamic models. Jacobian matrices do not need to be computed. Often computationally efficient.

30 Limitations of SLF Works only with Gaussian noise terms. Expected values of the non-linear functions have to be computed in closed form. Computation of expected values is hard and error prone. If the expected values cannot be computed in closed form, there is not much we can do.

31 Summary EKF and SLF can be applied to filtering models of the form x k = f(x k 1 )+q k 1 y k = h(x k )+r k, EKF is based on Taylor series expansions of the non-linear functions f and h. Advantages: Simple, intuitive, computationally efficient Disadvantages: Local approximation, differentiability requirements, only for Gaussian noises. SLF is based on statistical linearization of the non-linearities: Advantages: Global approximation, no differentiability requirements, computationally efficient Disadvantages: Closed form computation of expectations, only for Gaussian noises.