Το πρόβλημα του φιλτραρίσματος είναι να υπολογιστεί η βέλτιστη εκτίμηση. μέχρι και τη χρονική στιγμή k. Η εκτίμηση είναι:

Transcript

1 1 2. ΦΙΛΤΡΟ KALMAN 2.1.ΧΡΟΝΙΚΑ ΜΕΤΑΒΑΛΛΟΜΕΝΟ ΦΙΛΤΡΟ KALMAN Το πρόβλημα του φιλτραρίσματος είναι να υπολογιστεί η βέλτιστη εκτίμηση (φιλτράρισμα) x( k / k ) της κατάστασης τη χρονική στιγμή δεδομένου του συνόλου των μετρήσεων Z { z(1), z(2),..., z( k)} μέχρι και τη χρονική στιγμή k. Η εκτίμηση είναι: k x( k / k) E[ x( k)/ Z k ] (2.1) και η αντίστοιχη διασπορά λάθους εκτίμησης είναι: P( k / k) E[[ x( k) x( k / k)][ x( k) x( k / k)] / Z k ] (2.2) Επίσης, η πρόβλεψη (κατά ένα βήμα) διαστάσεων nx1είναι: x( k 1/ k) E[ x( k 1)/ Z k ] (2.3) και η αντίστοιχη διασπορά λάθους πρόβλεψης διαστάσεων nnείναι: x P( k 1/ k) E[[ x( k 1) x( k 1/ k)][ x( k 1) x( k 1/ k)] / Z ] k (2.4) Για το χρονικά μεταβαλλόμενο μοντέλο, όπου οι μήτρες F( k 1, k), Hk ( 1), Qk ( ) και Rk ( 1) είναι χρονικά μεταβαλλόμενες, προκύπτει το χρονικά μεταβαλλόμενο φίλτρο Kalman (ime Varying Kalman Filter): Χρονικά μεταβαλλόμενο φίλτρο Kalman ime Varying Kalman Filter (V) x( k 1/ k) F( k 1, k) x( k / k) (2.5) P( k 1/ k) F( k 1, k) P( k / k) F ( k 1, k) Q( k) (2.6)

2 2 K( k 1) = P( k 1 /k) H ( k 1) [ H( k 1) P( k 1 /k) H ( k 1) + R( k 1)] x( k 1/ k 1) [ I K( k 1) H( k 1)] x( k 1/ k) K( k 1) z( k 1) 1 (2.7) (2.8) P( k 1/ k 1) [ I K( k 1) H( k 1)] P( k 1/ k) (2.9) για k 0,1,... με αρχικές συνθήκες x(0/0) x 0 P(0/0) P 0 Οι μήτρες Qk ( ), Rk ( 1), P( k 1/ k) και P( k / k ) είναι συμμετρικές (symmetric) και θετικά ημιορισμένες (nonnegative definite), ως μήτρες διασπορών. Η ύπαρξη των αντίστροφων μητρών που εμφανίζονται στην εξίσωση (2.7) εξασφαλίζεται στην περίπτωση που οι μήτρες Rk ( 1) είναι θετικά ορισμένες (ositive definite), γεγονός που συμβαίνει στην περίπτωση που καμμία μέτρηση δεν είναι ακριβής. Σε διαφορετική περίπτωση μπορεί να γίνει χρήση της ψευδοαντίστροφης μήτρας (seudo-inverse). Από τη θεωρία μητρών είναι γνωστοί οι παρακάτω ορισμοί: - Μία μήτρα A διαστάσεων nnορίζεται x ως συμμετρική (symmetric), αν ισχύει η σχέση A A. Μία συμμετρική μήτρα έχει πραγματικές ιδιοτιμές. - Μία συμμετρική μήτρα A διαστάσεων nnορίζεται x ως θετικά ορισμένη (ositive definite), αν ισχύει η σχέση x Ax 0 για κάθε διάνυσμα x 0. Μία θετικά ορισμένη μήτρα έχει όλες τις ιδιοτιμές της θετικές. - Μία συμμετρική μήτρα A διαστάσεων nnορίζεται x ως θετικά ημιορισμένη (nonnegative definite), αν ισχύει η σχέση x Ax 0 για κάθε διάνυσμα x 0. Μία θετικά ημιορισμένη μήτρα έχει μη αρνητικές ιδιοτιμές.

3 3 Η μήτρα Kk ( ) διαστάσεων nm x καλείται κέρδος (gain) του φίλτρου Kalman. Η διασπορά λάθους εκτίμησης, η διασπορά λάθους πρόβλεψης και το κέρδος δεν εξαρτώνται από τις μετρήσεις. Επομένως, μπορούν είτε να υπολογιστούν σε πραγματικό χρόνο (real time) είτε να υπολογιστούν εκ των προτέρων (off-line) χρησιμοποιώντας τις εξισώσεις (2.6), (2.7) και (2.9). Από τις εξισώσεις (2.7) και (2.8) προκύπτει ότι: - Όσο ο θόρυβος των μετρήσεων τείνει στο μηδέν, τόσο η εκτίμηση βασίζεται περισσότερο στη μέτρηση και λιγότερο στην πρόβλεψη. Όταν ο θόρυβος των μετρήσεων είναι μηδέν: Rk ( 1) 0, τότε είναι: I K( k 1) H( k 1) 0, οπότε: x( k 1/ k 1) K( k 1) z( k 1) - Όσο η διασπορά λάθους πρόβλεψης τείνει στο μηδέν, τόσο η εκτίμηση βασίζεται λιγότερο στη μέτρηση και περισσότερο στην πρόβλεψη. Όταν η διασπορά λάθους πρόβλεψης είναι μηδέν P( k 1/ k ) 0, τότε είναι : Kk ( 1) 0, οπότε: x( k 1/ k 1) x( k 1/ k )

4 ΧΡΟΝΙΚΑ ΑΜΕΤΑΒΛΗΤΟ ΦΙΛΤΡΟ KALMAN Για το χρονικά αμετάβλητο μοντέλο, όπου οι μήτρες F( k 1, k) F, H( k 1) H, Q( k) Q και R( k 1) R είναι σταθερές (χρονικά αμετάβλητες), προκύπτει το χρονικά αμετάβλητο φίλτρο Kalman (ime Invariant Kalman Filter): Χρονικά αμετάβλητο φίλτρο Kalman ime Invariant Kalman Filter (I) x( k 1/ k) Fx( k / k) (2.10) P( k 1/ k) FP( k / k) F Q (2.11) K( k 1) = P( k 1 /k) H [ HP( k 1 /k) H + R] 1 (2.12) x( k 1/ k 1) [ I K( k 1) H] x( k 1/ k) K( k 1) z( k 1) (2.13) P( k 1/ k 1) [ I K( k 1) H] P( k 1/ k) (2.14) για k 0,1,... με αρχικές συνθήκες x(0/0) x 0 P(0/0) P 0 Το φίλτρο Kalman είναι ένα αναδρομικό φίλτρο ακόμη και στην περίπτωση του χρονικά αμετάβλητου μοντέλου, γιατί το κέρδος είναι χρονικά μεταβαλλόμενο. Παρατήρηση. Η περίπτωση του άπειρου θορύβου μετρήσεων. Εξετάζοντας την περίπτωση του άπειρου θορύβου μετρήσεων, μπορούμε να θεωρήσουμε ότι η διασπορά θορύβου μετρήσεων είναι άπειρη, δηλαδή R. Τότε προκύπτουν τα ακόλουθα: - από την εξίσωση (2.12) το κέρδος είναι μηδέν: Kk ( 1) 0 - από την εξίσωση (2.13) η εκτίμηση είναι ίση με την πρόβλεψη:

5 5 x( k 1/ k 1) x( k 1/ k) - από την εξίσωση (2.14) η διασπορά λάθους εκτίμησης είναι ίση με τη διασπορά λάθους πρόβλεψης: P( k 1/ k 1) P( k 1/ k) Οπότε η εκτίμηση είναι: x( k 1/ k 1) Fx( k / k) (2.15) και η αντίστοιχη διασπορά λάθους εκτίμησης είναι: P( k 1/ k 1) FP( k / k) F Q (2.16) 2.3. ΦΙΛΤΡΟ KALMAN ΣΤΗ ΜΟΝΙΜΗ ΚΑΤΑΣΤΑΣΗ Για το χρονικά αμετάβλητο μοντέλο, όπου οι μήτρες F( k 1, k) F, H( k 1) H, Q( k) Q και R( k 1) R είναι σταθερές (χρονικά αμετάβλητες), είναι γνωστό [Anderson and Moore, 1979] ότι: αν για κάθε G με GG Q το ζεύγος [ FG, ] είναι πλήρως σταθεροποιήσιμο (comletely stabilizable) και αν το ζεύγος [ FH, ] είναι πλήρως ανιχνεύσιμο (comletely detectable), τότε το φίλτρο τείνει σε μόνιμη κατάσταση (steady state), δηλαδή η διασπορά λάθους πρόβλεψης τείνει σε μία σταθερή τιμή P, η οποία είναι μοναδική (unique) και καλείται διασπορά λάθους πρόβλεψης στη μόνιμη κατάσταση. Σημειώνεται ότι: - το ζεύγος [ FG, ] είναι πλήρως σταθεροποιήσιμο (comletely stabilizable), αν ισχύει η πρόταση: αν wg 0 και w F λ w για κάποια σταθερά λ, τότε λ 1 ή w 0

6 6 - το ζεύγος [ FH, ] είναι πλήρως ανιχνεύσιμο (comletely detectable), αν το ζεύγος [ F, H ] είναι πλήρως σταθεροποιήσιμο (comletely stabilizable). Επίσης, αν το μοντέλο είναι ασυμπτωτικά ευσταθές (asymtotically stable), που σημαίνει ότι οι ιδιοτιμές (eigenvalues) της μήτρας F βρίσκονται εντός του μοναδιαίου κύκλου, τότε οπωσδήποτε το μοντέλο είναι πλήρως ανιχνεύσιμο και πλήρως σταθεροποιήσιμο. Επομένως, αν το μοντέλο είναι ασυμπτωτικά ευσταθές, τότε το φίλτρο τείνει σε μόνιμη κατάσταση. Το φίλτρο μπορεί να τείνει σε μόνιμη κατάσταση ακόμη και όταν το μοντέλο δεν είναι ασυμπτωτικά ευσταθές. Από τις εξισώσεις (2.11), (2.12) και (2.14) προκύπτει η εξίσωση Riccati για το φίλτρο Kalman: P( k 1/ k) FP( k / k 1) F Q FP k k H HP k k H R HP k k F 1 ( / 1) [ ( / 1) ] ( / 1) (2.17) Η εξίσωση Riccati είναι μία μη γραμμική αναδρομική εξίσωση. Η διασπορά λάθους πρόβλεψης τείνει στη διασπορά λάθους πρόβλεψης στη μόνιμη κατάσταση όταν P( k 1/ k ) P( k / k 1) (2.18) όπου είναι ένας μικρός θετικός αριθμός και κατάστασης (steady state time). k είναι ο χρόνος μόνιμης Με A συμβολίζεται η φασματική νόρμα (norm) της μήτρας A, δηλαδή η τετραγωνική ρίζα της μέγιστης ιδιοτιμής της μήτρας AA.

7 7 Έτσι, προκύπτει η εξίσωση Riccati μόνιμης κατάστασης: P FP F Q FP H [ HP H R] HP F (2.19) 1 η μοναδική λύση (unique solution) της οποίας είναι η διασπορά λάθους πρόβλεψης στη μόνιμη κατάσταση. Από τις εξισώσεις (2.11), (2.12) και (2.14) είναι φανερό ότι το κέρδος τείνει σε μία σταθερή τιμή K που καλείται κέρδος στη μόνιμη κατάσταση και υπολογίζεται ως συνάρτηση της διασποράς λάθους πρόβλεψης στη μόνιμη κατάσταση: K P H HP H R 1 [ ] (2.20) Επίσης, είναι φανερό ότι η διασπορά λάθους εκτίμησης τείνει σε μία σταθερή τιμή P e, η οποία καλείται διασπορά λάθους πρόβλεψης στη μόνιμη κατάσταση και υπολογίζεται ως συνάρτηση της διασποράς λάθους πρόβλεψης στη μόνιμη κατάσταση: P I P H HP H R H P (2.21) 1 e [ [ ] ] Στη μόνιμη κατάσταση, προκύπτει το φίλτρο Kalman μόνιμης κατάστασης (Steady State Kalman Filter): Φίλτρο Kalman μόνιμης κατάστασης Steady State Kalman Filter (SS) x( k 1/ k 1) A x( k / k) B z( k 1) (2.22) A [ I KH ] F (2.23) B K (2.24) για k k, k 1,...

8 8 Η υλοποίηση του φίλτρου Kalman μόνιμης κατάστασης απαιτεί τη γνώση: - της εκτίμησης x( k / k ) για την εφαρμογή της αναδρομής και - της διασποράς λάθους εκτίμησης P( k / k ) για τον υπολογισμό των μητρών A και B. Επομένως, η υλοποίηση του φίλτρου Kalman μόνιμης κατάστασης προϋποθέτει την υλοποίηση του χρονικά αμετάβλητου φίλτρου Kalman για k 0,1,..., k με αρχικές συνθήκες x(0/0) x0 και P(0/0) P0. Οι μήτρες A και B μπορούν να υπολογιστούν εκ των προτέρων (off-line) χρησιμοποιώντας τις εξισώσεις (2.19), (2.20), (2.23) και (2.24). Παρατήρηση. Η περίπτωση του άπειρου θορύβου μετρήσεων. Εξετάζοντας την περίπτωση του άπειρου θορύβου μετρήσεων, μπορούμε να θεωρήσουμε ότι η διασπορά θορύβου μετρήσεων είναι άπειρη, δηλαδή R. Τότε για το χρονικά αμετάβλητο μοντέλο είναι γνωστό [Anderson and Moore, 1979] ότι: αν για κάθε G με GG Q το ζεύγος [ FG, ] είναι πλήρως προσβάσιμο (comletely reachable) και αν οι ιδιοτιμές (eigenvalues) της μήτρας F βρίσκονται εντός του μοναδιαίου κύκλου, τότε το φίλτρο τείνει σε μόνιμη κατάσταση (steady state), δηλαδή η διασπορά λάθους πρόβλεψης τείνει σε μία σταθερή τιμή P, η οποία είναι μοναδική (unique) και καλείται διασπορά λάθους πρόβλεψης στη μόνιμη κατάσταση. Σημειώνεται ότι: - το ζεύγος [ FG, ] είναι πλήρως προσβάσιμο (comletely reachable), αν ισχύει η πρόταση: αν wg 0 και w F λ w για κάποια σταθερά λ, τότε w 0.

9 9 Επίσης, το κέρδος στη μόνιμη κατάσταση είναι μηδέν: K 0. Οπότε η διασπορά λάθους εκτίμησης στη μόνιμη κατάσταση είναι ίση με τη διασποράς λάθους πρόβλεψης στη μόνιμη κατάσταση: Pe P. Στην περίπτωση αυτή από τις εξισώσεις του φίλτρου Kalman προκύπτει η εξίσωση Lyaunov (Lyaunov equation) ως ειδική μορφή της εξίσωσης Riccati (για R ): P( k 1/ k) FP( k / k 1) F Q (2.25) Έτσι, προκύπτει η εξίσωση Lyaunov μόνιμης κατάστασης: P FPF Q (2.26) η μοναδική λύση (unique solution) της οποίας είναι η διασπορά λάθους πρόβλεψης στη μόνιμη κατάσταση.