9990200023

2. Podstawy teoretyczne 16

2.8 Filtracja Kalmana

Jedną z najczęściej stosowanych metod filtracji w systemach nawigacji jest filtracja Kalmana. Jest to rodzaj filtracji dyskretnej, która estymuje stan procesu tak, aby zminimalizować błąd średnio-kwadratowy. Filtr ten korzysta z wszystkich dostępnych pomiarów, bez względu na to z jaką dokładnością i precyzją zostały one wykonane. Dokonuje fuzji sygnałów, a następnie na ich podstawie dokonuje najlepszej estymacji stanu.

Filtr Kalmana składa się z dwóch faz: predykcji, i korekcji (rysunek 2.11). Pierwsza faza to tak zwana aktualizacja czasowa, gdzie na podstawie informacji ze stanu poprzedniego wyznacza się estymowaną wartość stanu x oraz jego kowariancję. W fazie drugiej nazywanej inaczej aktualizacją pomiarową dokonuje się korekcji wartości stanu i jego kowariancji. Jest to pewien rodzaj sprzężenia zwrotnego.

Do opisu procesu oraz systemu pomiarowego stosuje się modele matematyczne

x_k = Ax_k-1 + Bu_k-1 + w_k-1

rr .    (A¹⁸,)

z_k = Hx_k + v_k

Pierwsze równanie to równanie procesu, gdzie x_k to stan procesu, x_k^i to stan procesu z chwili poprzedniej u to sterowanie, natomiast w to szum procesu. Drugie równanie to model pomiaru, gdzie H to tzw. Wyjście filtru łączące stan procesu z filtrem, v to szum pomiaru.

Błąd pomiaru a priori e_k oraz a posteriori e_k definiuje się jako

(2.19)

^ek =^xk ~ x_k G_k —X_k X_k

gdzie x_k to estymowany stan a priori uzyskany z procesu, x_k to estymowany stan a posteriori uwzględniający pomiar z_k, natomiast x_k to stan rzeczywisty, który nie jest znany.

Macierze kowariancji a priori P_k i a posteriori P_k mają postać:

(2.20)

P_k —E[e_k, e_k ] P_k =E[e_k,e_k\.

Rysunek 2.11 Algorytm filtracji Kalmana [19]