A New Approach to Linear Filtering and Prediction Problems

a.k.a. Kalman Filters

Transcript

1. 10/8/2016 pwlnyc_kalman http://localhost:8888/nbconvert/html/pwlnyc_kalman.ipynb?download=false 1/12 A New Approach to Linear Filtering and Prediction Problems a.k.a. Kalman Filters Papers We Love NYC Elizabeth Ramirez I n [ 1 ] : % m a t p l o t l i b i n l i n e i m p o r t n u m p y f r o m n u m p y . r a n d o m i m p o r t r a n d n i m p o r t m a t p l o t l i b . p y p l o t a s p l t Intro Original paper: [http://www.cs.unc.edu/~welch/kalman/media/pdf/Kalman1960.pdf (http://www.cs.unc.edu/~welch/kalman/media/pdf/Kalman1960.pdf)]
2. 10/8/2016 pwlnyc_kalman http://localhost:8888/nbconvert/html/pwlnyc_kalman.ipynb?download=false 2/12 What is a Kalman Filter Prediction-correction technique Calculate recursively system state at time given previous state and new information Kalman Filters for Rocket Science Used for Apollo Space Program of NASA in early 1960's Transcription of the original code available at [http://www.ibiblio.org/apollo/ (http://www.ibiblio.org/apollo/)] Implemented in AGC4 assembly language. # C o p y r i g h t : P u b l i c d o m a i n . # F i l e n a m e : K A L M A N _ F I L T E R . a g c # P u r p o s e : A s e c t i o n o f L u m i n a r y 1 C , r e v i s i o n 1 3 1 . # I t i s p a r t o f t h e s o u r c e c o d e f o r t h e L u n a r M o d u l e ' s ( L M ) # A p o l l o G u i d a n c e C o m p u t e r ( A G C ) f o r A p o l l o 1 3 a n d A p o l l o 1 4 . # T h i s f i l e i s i n t e n d e d t o b e a f a i t h f u l t r a n s c r i p t i o n , e x c e p t # t h a t t h e c o d e f o r m a t h a s b e e n c h a n g e d t o c o n f o r m t o t h e # r e q u i r e m e n t s o f t h e y a Y U L a s s e m b l e r r a t h e r t h a n t h e # o r i g i n a l Y U L a s s e m b l e r . # R e f e r e n c e : p p . 1 4 6 7 - 1 4 6 8 o f 1 7 2 9 . p d f . # C o n t a c t : R o n B u r k e y < i n f o @ s a n d r o i d . o r g > . # W e b s i t e : w w w . i b i b l i o . o r g / a p o l l o / i n d e x . h t m l # M o d h i s t o r y : 0 6 / 0 8 / 0 3 R S B . B e g a n t r a n s c r i b i n g . # 0 5 / 1 4 / 0 5 R S B C o r r e c t e d w e b s i t e r e f e r e n c e a b o v e Background Random processes (Appendix) Dynamic systems Vector spaces Least Squares and Normal Equation Vector Spaces a.k.a. Linear Spaces. A space of vectors with a metric: norm, notion of distance, often represented by and the following properties: u k t k u k −1 b k  || ⋅ || f , g ∈  → f + g = h ∈  λ ∈ ℝ, f ∈  → λf ∈ 
3. 10/8/2016 pwlnyc_kalman http://localhost:8888/nbconvert/html/pwlnyc_kalman.ipynb?download=false 3/12 Least Squares Motivation Wiener Filter Problems: Impulse response Numerical determination Least Squares Linear System Au = b A = m × n u, b are n-vectors If A is square: m = n → u = b A −1 But if A is not square: m > n System is overdetermined Example: 100 points that fit Cx + D Solution: Find best estimate for state u that minimizes: E = b − A ∣ ∣ u ̂ ∣ ∣2 Solve for (an estimate) to minimize E u ̂ Normal Equation: A = b A T u ̂ A T = A T n×m A m×n A A T n×n = ( A b u ̂ A T ) −1 A T
4. 10/8/2016 pwlnyc_kalman http://localhost:8888/nbconvert/html/pwlnyc_kalman.ipynb?download=false 4/12 Situation Theorem 1 Given a signal: y(t) = (t) + (t) x 1 x 2 (t) : true signal, (t) : noise x 1 x 2 y( )? t 1 If < t : Smoothing t 1 If = t : Filtering t 1 If > t : Prediction t 1 Statistical Estimate ( |t) X 1 t 1 Estimation Error ϵ = ( ) − ( |t) x 1 t 1 X 1 t 1 Loss Function L( ϵ) : L(0) = 0 L( ) ≥ L( ) ≥ 0 when ≥ ≥ 0 ϵ 2 ϵ 1 ϵ 2 ϵ 1 L( ϵ) = L( −ϵ) Minimize E{L[ ( ) − ( )]|y( ) …y(t)} x 1 t 1 X 1 t 1 t 0
5. 10/8/2016 pwlnyc_kalman http://localhost:8888/nbconvert/html/pwlnyc_kalman.ipynb?download=false 5/12 Theorem 1-a Problem Summary Orthogonal Projections If L( ϵ) = , Theorem 1 is true without assumptions about symmetry and convexity around the mean. ϵ2 Given a vector-valued random process x(t) and observed random variables y( ) …y(t) t 0 Find an estimate X( ) that minimizes the expected loss E{L||x( ) − X( )||} t 1 t 1 t 1 Vector Space (t) = y(i) ∑ i=t 0 t a i Given u, v ∈ (t) orthogonal if E[u ⋅ v] = 0 Orthonormal basis in (t) = . . . , such that e t 0 e t = x ¯ ∑ i=t 0 t a i e i x = + x ¯ x ̃
6. 10/8/2016 pwlnyc_kalman http://localhost:8888/nbconvert/html/pwlnyc_kalman.ipynb?download=false 6/12 From functional analysis, we know that our estimate X1(t1) might or might not live in this vector space Y(t). We would really like our solution to live here. If the vector space is too small, we want to minimize the distance between my estimate and the vector space. Given this, and given we can determine the coefficients of the linear combination, any random variable x, not necessarily in Y(t) can be uniquely decomposed into two parts: a part in Y(t) and a part orthogonal to Y(t) (orthogonal to every vector in Y(t)). See equation 10. The orthogonal projection of x in Y(t): x bar is that vector in Y(t) that minimizes the quadratic loss. This is the key part. Theorem 2 Recursive Least Squares Let {x(t)} and {y(t)} be random processes with zero mean E[x(t)] = E[y(t)] = 0 Observe y( ). . . y(t) t 0 If either the random processes are gaussian or the loss function is restricted to be L( ϵ) = ϵ2 then the optimal estimate of x(t1|t) is given by the orthogonal projection of x( ) on (t) x ¯ t 1 : Average of , , …, u 99 ^ b 1 b 2 b 99 → b 100 u 100 ^ = + u 100 ^ 99 100 u 99 ^ 1 100 b 100 = + ( − ) u 99 ^ 1 100 b 100 u 99 ^ − : innovation : gain factor b 100 u 99 ^ 1 100
7. 10/8/2016 pwlnyc_kalman http://localhost:8888/nbconvert/html/pwlnyc_kalman.ipynb?download=false 7/12 Recursive Least Squares Generalizing the Normal Equation: Right-hand side Recursive Least Squares [ ] [ ] = [ ] → A old A new u b old b new u new ^ = [ ] A T AT old AT new A = [ ] [ ] A T AT old AT new AT old AT new = + = (known) + (new) AT old A old AT new A new b = [ ] [ ] A T AT old AT new bT old bT new = + AT old b old AT new b new = + AT old A old u old ^ AT new b new = + ( A ( − ) u new ^ u old ^ A T ) −1 AT new b new A new b old ( A : gain matrix, K (as in Kalman, duh) A T ) −1 AT new [ ] = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ 1 1 ⋮ 1 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ u ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ b 1 b 2 ⋮ b 99 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ u = = → = [ ] b100 bnew Anew 1 = + ( − ) u 100 ^ u 99 ^ 1 100 b 100 u 99 ^
8. 10/8/2016 pwlnyc_kalman http://localhost:8888/nbconvert/html/pwlnyc_kalman.ipynb?download=false 8/12 Covariance Matrix When errors are independent: Weighted Normal Equation The Kalman Filter = = E[ ] = 0 σ ij σ ji e i e j = E(e ) = Σ e e T ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ σ2 1 σ2 2 ⋱ σ2 k ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ C = : Weighting Matrix Σ −1 e CA = Cb A T u ̂ A T = ( CA Cb = Lb u ̂ A T ) −1 A T Time varying least squares problem: Estimate at each time u k ^ t k = L + K u new ^ u old ^ b new = + K( − ) u new ^ u old ^ b new A new u old ^ innovation − b new A new u old ^ covariance matrix P = ( CA A T ) −1
9. 10/8/2016 pwlnyc_kalman http://localhost:8888/nbconvert/html/pwlnyc_kalman.ipynb?download=false 9/12 Algorithm Prediction Correction F is called the state transition matrix. It establishes how the state vector changes from one time step to another. Q is the covariance matrix of system error Implementation Predicted state and covariance (before the measurement) Corrected state and covariance (after the measurement) Innovation Filter gain Prediction / Correction Previous state vector Predicted state vector Matrix of observation equations Vector of observations Predicted covariance matrix Process noise matrix Observations noise matrix = F + u k|k −1 ^ u k −1|k −1 ^ ϵ i = A + Q P k|k −1 P k −1|k −1 A T F : state transition matrix Q : covariance matrix of random excitation = (A + R K k P k|k −1 A T P k|k −1 A T ) −1 = + ( − A ) u k|k ^ u k|k −1 ^ K k b k u k|k −1 ^ = (I − A) P k|k K k P k|k −1 u u ̂ A b P Q R
10. 10/8/2016 pwlnyc_kalman http://localhost:8888/nbconvert/html/pwlnyc_kalman.ipynb?download=false 10/12 I n [ 2 ] : d e f p r e d i c t ( u , P , F , Q ) : u = n u m p y . d o t ( F , u ) P = n u m p y . d o t ( F , n u m p y . d o t ( P , F . T ) ) + Q r e t u r n u , P I n [ 3 ] : d e f c o r r e c t ( u , A , b , P , Q , R ) : C = n u m p y . d o t ( A , n u m p y . d o t ( P , A . T ) ) + R K = n u m p y . d o t ( P , n u m p y . d o t ( A . T , n u m p y . l i n a l g . i n v ( C ) ) ) u = u + n u m p y . d o t ( K , ( b - n u m p y . d o t ( A , u ) ) ) P = P - n u m p y . d o t ( K , n u m p y . d o t ( C , K . T ) ) r e t u r n u , P I n [ 4 ] : # T e s t d a t a d t = 0 . 1 A = n u m p y . a r r a y ( [ [ 1 , 0 ] , [ 0 , 1 ] ] ) u = n u m p y . z e r o s ( ( 2 , 1 ) ) # R a n d o m i n i t i a l m e a s u r e m e n t c e n t e r e d a t s t a t e v a l u e b = n u m p y . a r r a y ( [ [ u [ 0 , 0 ] + r a n d n ( 1 ) [ 0 ] ] , [ u [ 1 , 0 ] + r a n d n ( 1 ) [ 0 ] ] ] ) P = n u m p y . d i a g ( ( 0 . 0 1 , 0 . 0 1 ) ) F = n u m p y . a r r a y ( [ [ 1 . 0 , d t ] , [ 0 . 0 , 1 . 0 ] ] ) # U n i t v a r i a n c e f o r t h e s a k e o f s i m p l i c i t y Q = n u m p y . e y e ( u . s h a p e [ 0 ] ) R = n u m p y . e y e ( b . s h a p e [ 0 ] ) I n [ 5 ] : N = 5 0 0 p r e d i c t i o n s , c o r r e c t i o n s , m e a s u r e m e n t s = [ ] , [ ] , [ ] f o r k i n n u m p y . a r a n g e ( 0 , N ) : u , P = p r e d i c t ( u , P , F , Q ) p r e d i c t i o n s . a p p e n d ( u ) u , P = c o r r e c t ( u , A , b , P , Q , R ) c o r r e c t i o n s . a p p e n d ( u ) m e a s u r e m e n t s . a p p e n d ( b ) b = n u m p y . a r r a y ( [ [ u [ 0 , 0 ] + r a n d n ( 1 ) [ 0 ] ] , [ u [ 1 , 0 ] + r a n d n ( 1 ) [ 0 ] ] ] ) p r i n t ' p r e d i c t e d f i n a l e s t i m a t e : % f ' % p r e d i c t i o n s [ - 1 ] [ 0 ] p r i n t ' c o r r e c t e d f i n a l e s t i m a t e : % f ' % c o r r e c t i o n s [ - 1 ] [ 0 ] p r i n t ' m e a s u r e d s t a t e : % f ' % m e a s u r e m e n t s [ - 1 ] [ 0 ] p r e d i c t e d f i n a l e s t i m a t e : 1 7 7 . 5 2 9 1 1 3 c o r r e c t e d f i n a l e s t i m a t e : 1 7 6 . 6 0 2 6 2 7 m e a s u r e d s t a t e : 1 7 6 . 0 0 7 3 6 1
11. 10/8/2016 pwlnyc_kalman http://localhost:8888/nbconvert/html/pwlnyc_kalman.ipynb?download=false 11/12 I n [ 6 ] : t = n u m p y . a r a n g e ( 5 0 , 1 0 0 ) f i g = p l t . f i g u r e ( f i g s i z e = ( 1 5 , 1 5 ) ) a x e s = f i g . a d d _ s u b p l o t ( 2 , 2 , 1 ) a x e s . s e t _ t i t l e ( " K a l m a n F i l t e r " ) a x e s . p l o t ( t , n u m p y . a r r a y ( p r e d i c t i o n s ) [ 5 0 : 1 0 0 , 0 ] , ' o ' , l a b e l = ' p r e d i c t i o n ' ) a x e s . p l o t ( t , n u m p y . a r r a y ( c o r r e c t i o n s ) [ 5 0 : 1 0 0 , 0 ] , ' x ' , l a b e l = ' c o r r e c t i o n ' ) a x e s . p l o t ( t , n u m p y . a r r a y ( m e a s u r e m e n t s ) [ 5 0 : 1 0 0 , 0 ] , ' ^ ' , l a b e l = ' m e a s u r e m e n t ' ) p l t . l e g e n d ( ) p l t . s h o w ( ) Conclusions Kalman Filter is a generalized least squares problem for dynamical systems. Computationally more efficient Strang, Gilbert. Computational Science and Engineering
12. 10/8/2016 pwlnyc_kalman http://localhost:8888/nbconvert/html/pwlnyc_kalman.ipynb?download=false 12/12 Thank you! eramirem@gmail.com @eramirem