Το Extended Kalman Filter (EKF) είναι ένας ευρέως χρησιμοποιούμενος αλγόριθμος στην τοπικοποίηση ρομπότ. Εκτιμά τη θέση και τον προσανατολισμό του ρομπότ συνδυάζοντας δεδομένα αισθητήρων με ένα μαθηματικό μοντέλο της κίνησης του ρομπότ. Η κατανόηση των μαθηματικών θεμελίων του ΕΚΦ είναι απαραίτητη για αποτελεσματική εφαρμογή και συντονισμό.

Κρατική εκπροσώπηση και προκατάληψη

Ο πυρήνας του EKF περιλαμβάνει την αναπαράσταση της κατάστασης του ρομπότ ως φορέα, τυπικά συμπεριλαμβανομένης της θέσης και του προσανατολισμού. Το βήμα πρόβλεψης χρησιμοποιεί ένα μη γραμμικό μοντέλο κίνησης για να προβάλει την τρέχουσα κατάσταση προς τα εμπρός στο χρόνο. Αυτό περιλαμβάνει τον υπολογισμό της Jacobian μήτρας του μοντέλου κίνησης για να γραμμογραφήσει τις μη γραμμικές εξισώσεις γύρω από την τρέχουσα εκτίμηση.

Ενημέρωση και Γραμμοποίηση Μετρήσεων

Οι μετρήσεις των αισθητήρων ενσωματώνονται μέσω του βήματος ενημέρωσης. Δεδομένου ότι οι μετρήσεις είναι συχνά μη γραμμικές λειτουργίες της κατάστασης, το ΕΚΤ γραμμικά τις λειτουργίες αυτές χρησιμοποιώντας τους Ιακωβιανούς τους. Αυτή η διαδικασία προσαρμόζει την προβλεπόμενη κατάσταση με βάση τη διαφορά μεταξύ αναμενόμενων και πραγματικών αναγνώσεων αισθητήρων.

Μαθηματικές εξισώσεις

Οι εξισώσεις πρόβλεψης είναι:

Πρόβλεψη κατάστασης:

x ⁇ k+1 ⁇ = f(x ⁇ k, uk)

Πρόβλεψη ωοθηκών:

Pk+1 ⁇ = Fk Pk Fkt + Qk

όπου f είναι το μη γραμμικό μοντέλο κίνησης, Fk είναι ο Jacobian, P είναι ο πίνακας συμμετρίας, και Q είναι η συνδιαφορά θορύβου διεργασίας.

Οι εξισώσεις ενημέρωσης είναι:

Κέρδος Κάλμαν:

Kk = Pk ⁇ Hkt (Hk Pk ⁇ Hkt + Rk) ⁇ 1

Ενημέρωση κατάστασης:

x ⁇ k = x ⁇ k ⁇ + Kk (zk - h(x ⁇ k ⁇ ))

Ενημέρωση ωοθηκών:

Pk = (I - Kk Hk) Pk ⁇

Εδώ, h είναι το μη γραμμικό μοντέλο μέτρησης, Hk είναι Jacobian του, Rk είναι η συνδιαφορά θορύβου μέτρησης, και Zk είναι η πραγματική μέτρηση αισθητήρων.