Filtr Kalmana. Struktury i Algorytmy Sterowania Wykład 1-2. prof. dr hab. inż. Mieczysław A. Brdyś mgr inż. Tomasz Zubowicz

Transkrypt

1 Filtr Kalmana Struktury i Algorytmy Sterowania Wykład 1-2 prof. dr hab. inż. Mieczysław A. Brdyś mgr inż. Tomasz Zubowicz Politechnika Gdańska, Wydział Elektortechniki i Automatyki , Gdańsk

2 Założenia odnośnie filtru Kalmana Przystępując do syntezy filtru Kalmana przyjmuje się następujące założenia: minimum kowariancji liniowość filtru estymaty stanu są nieobciążone: E {x(k)} = E {ˆx(k)}, gdzie ˆx(k) jest estymatą stanu x(k)

3 Dynamika modelu systemu Równania stanu Niech dany będzie liniowy niestacjonarny model dynamiki systemu: gdzie: x (k + 1) = A (k) x (k) + B (k) u (k) + G (k) z (k) (1) x (k) wektor stanu systemu u (k) wektor wejść sterowanych z (k) wektor wejść zakłócających A (k), B (k), G (k) są odpowiednio niestacjonranymi macierzami stanu systemu, wejść i zakłóceń

4 Dynamika modelu systemu Równania wyjścia Równanie wyjścia dane jest zależnością: y (k) = C (k) x (k) + v (k) (2) gdzie: y (k) wektor wyjść systemu v (k) wektor szumów pomiarowych C (k) macierz wyjść

5 Dynamika modelu systemu Zakłócenia i szumy pomiarowe Zakłada się, że zakłócenia z (k) oraz szumy pomiarowe v (k) są sygnałami stochastycznymi takmi, że: E {z (k)} = 0 cov (z (k)) = E {(z } (k) E {z (k)}) (z (k) E {z (k)}) T { } = E z (k) z T (k) = Z (k) (3) E {v (k)} = 0 cov (v (k)) = E {(v } (k) E {v (k)}) (v (k) E {v (k)}) T { } = E v (k) v T (k) = V (k) (4)

6 Dynamika modelu systemu Zakłócenia i szumy pomiarowe c.d. Autokoralecja zakłóceń i szumów jest równa zeru { } E z (k) z T (j) = 0 ; j k E { } v (k) v T (j) = 0 ; j k (5) Ponadto zakłada się, że nie istnieje korelacja pomiędzy zakłóceniami i szumami pomiarowymi: { } E z (k) v T (j) = 0 (6) dla k j (dla uproszczenia)

7 Dynamika modelu systemu Warunki początkowe Zakłada się, że warunki początkowe x 0 są znane i w opraciu o ich znajomość mozna wyznaczyć: E E {x 0 } { } x 0 x T 0 = P 0 (7) gdzie: P 0 macierz kowariancji Zanjomość P 0 jest niezbędna do zainicjowania algorytmu filtru Kalmana

8 Problem Sformułowanie Poszukiwany jest: rekursywny estymator ˆx(k) stanu systemu x (k) który jest liniowy w odniesieniu do pomiarów y (1), y (2),..., y (k) nieobciążony: E {ˆx} = E {x} optymalny: posiada minimalną kowariancję błędu P (k), gdzie { } P (k) = E e (k) e T (k), e (k) = x (k) ˆx (k) to jest dla każdego wektora p i macierzy T, takiego, że p 0 zachodzi: p T P (k) p p T Tp (8)

9 Problem Spostrzeżenie Niech a będzie wektorem. Rozważ liniową funkcję: a T x (k) (9) Błąd średniokwadratowy dla tej funckji wyraża się: [ ( ( ) ] T E a T x (k) a T ˆx (k)) a T x (k) a T ˆx (k) = = E = E [ ( ( ) ] T a T (x (k) ˆx (k))) a T (x (k) ˆx (k)) [ ] [ ] a T e (k) a T e (k) = E a T e (k) e T (k) a (10) = a T E [ ] e (k) e T (k) a = a T P (k) a minimum

10 Problem Spostrzeżenie c.d. 1 Minimalizowany jest błąd średnio kwadratowy w kotekście każdej zmiennej stanu z osobna 2 Minimalizowana jest norma błędu średnio kwadratowego, np.: E [ ] e1 2 + e en 2 minimum (11)

11 Wyprowadzenie Liniowość i rekursywność Wychodząc z założenia o liniowości i rekursywności poszukiwanego estymatora: ˆx (k) = Jˆx (k 1) + Ky (k) + Lu (k 1) (12) gdzie: J,K,L są poszukiwanymi macierzami takimi, że ˆx (k) jest: nieobciążona estymatą stanu gwarantującą minimum kowariancji błędu estymacji

12 Wyprowadzenie Nieobciążoność Wykorzystując założenie o nieobciążoności estymat: E {ˆx (k)} = E {Jˆx (k 1) + Ky (k) + Lu (k 1)} = = JE {ˆx (k 1)} + KE {y (k)} + Lu (k 1) (13) = E {x (k)}

13 Wyprowadzenie Nieobciążoność c.d. Ponieważ ˆx (k 1) jest również nieobciążona: E {ˆx (k)} = JE {ˆx (k 1)} + KE {C(k)x (k) + v (k)} + Lu (k 1) = = JE {x (k 1)} + KC(k)E {x (k)} + Lu (k 1) = = JE {x (k 1)} + KC(k)E +Lu (k 1) = A (k 1) x (k 1) + B (k 1) u (k 1) + G (k 1) z (k 1) + = JE {x (k 1)} + KC(k)A (k 1) E {x (k 1)} + +KC(k)B (k 1) u (k 1) + Lu (k 1) (14)

14 Wyprowadzenie Nieobciążoność c.d. Wykorzystując fakt, że: E {ˆx(k)} = E {x(k)} oraz: E {x(k)} = A(k 1)E {x(k 1)} + B(k 1)u (k 1) (15) E {ˆx (k)} = JE {x (k 1)} + KC(k)A (k 1) E {x (k 1)} + +KC(k)B (k 1) u (k 1) + Lu (k 1) (16) można wyznaczyć wartości macierzy J oraz L

15 Wyprowadzenie Wartości macierzy J i L Z porównania równań (15) oraz (16) wynika: J + KC(k)A (k 1) = A (k 1) L + KC(k)B (k 1) = B (k 1) (17) skąd w rezultacie otrzymano: J(k) = (I KC(k)) A (k 1) L(k) = (I KC(k)) B (k 1) (18) UWAGA: macierze J(k) i L(k) są niestacjonarne!

16 Struktura estymatora predyktor - korektor Wykorzystując J(k) i L(k), ˆx (k) przyjmuje postać: ˆx (k) = [I KC(k)] A (k 1) ˆx (k 1) + + [I KC(k)] B (k 1) u (k 1) + Ky(k) (19) co po uporządkowaniu: ˆx (k) = A (k 1) ˆx (k 1) + B (k 1) u (k 1) + +K [y(k) C(k) (A(k 1)ˆx(k 1)) + B(k 1)u(k 1)] (20) gdzie: A (k 1) ˆx (k 1) + B (k 1) u (k 1) stanowi predykcję stanu systemu x (k) liczoną w opraciu o dane z chwili k 1, co oznaczamy: ˆx (k k 1) Cˆx(k k 1) stanowi predykcję pomiaru y(k) wykonaną w chwili k 1, co oznaczamy: ŷ (k k 1).

17 Struktura estymatora predyktor - korektor c.d. Struktura estymatora ma ogólną postać: ˆx (k) }{{} = ˆx (k k 1) }{{} + K [y(k) ŷ (k k 1)] }{{} aktualna estymata predykcja stanu }{{} } predykcja błedu pomiarowego {{} człon predykcyjny człon korekcyjny (21) gdzie: ŷ(k k 1) jest to wektor predykcji pomiarow y(k): ŷ(k k 1) = Cˆx(k k 1) ˆx(k k 1) jest to predykcja estymaty stanu ˆx(k): ˆx(k k 1) = A(k 1)ˆx(k 1) + B(k 1)u(k 1) wyznaczana w chwili k jako transfer ˆx(k 1) zgodny z równaniami stanu systemu, gdzie z(k 1) zastąpione jest przez E {z(k 1)} = 0

18 Błąd predykcji Definiując błąd predykcji jako: e(k k 1) = x(k) ˆx(k k 1) (22) wykorzystując równania stanu x(k) rozważanego systemu oraz wyznaczoną predykcję estymaty stanu ˆx(k k 1): e(k k 1) = A(k 1)x(k 1) + B(k 1)u(k 1)+ +G(k 1)z(k 1)+ (23) A(k 1)ˆx(k 1) B(k 1)u(k 1)

19 Błąd predykcji c.d. Wykorzystując definicję błedu predykcji oraz redukując B(k 1)u(k 1) otrzymano: e(k k 1) = A(k 1)e(k 1) + G(k 1)z(k 1) (24) A(k 1)e(k 1) dynamika wewnętrzna błędu predykcji zależna od dynamiki stanu systemu G(k 1)z(k 1) wpływ zakłóceń stanu systemu na błąd predykcji

20 Kowaraincja błędu predykcji Wyznaczanie kowaraincji błędu predykcji P(k k 1): P(k k 1) = E { [e(k k 1) E {e(k k 1)}] [ ] T } = = E e(k k 1) A(k 1) E {e(k 1)} [ ] T }{{} = =0 = E { } e(k k 1)e T (k k 1)

21 Kowaraincja błędu predykcji c.d. Kontynuując: P(k k 1) = E = E { } e(k k 1)e T (k k 1) { [A(k 1)e(k 1) + G(k 1)z(k 1)] [ ] T } = = E A(k 1)e(k 1) + G(k 1)z(k 1) }{{}}{{} a b A T (k 1)e T (k 1) + G T (k 1)z T (k 1) }{{}}{{} c d

22 Kowaraincja błędu predykcji c.d. Kontynuując: P(k k 1) = E {a c} + E {a d} + E {b c} + E {b d} = = A(k 1)E { } e(k 1)e T (k 1) A T (k 1)+ +E {a d} + E {b c} + +G(k 1)E = A(k 1)E +G(k 1)E { } z(k 1)z T (k 1) G T (k 1) = { } e(k 1)e T (k 1) A T (k 1)+ +E {a d} + E {b c} { } z(k 1)z T (k 1) G T (k 1)+

23 Kowaraincja błędu predykcji c.d. Oszacowanie E {a d}: a e(k 1) = e(k 1) [x 0, ˆx 0, z(0),..., z(k 2), v(0),..., v(k 1)] d = z(k 1) (25) z(k{ 1) nie jest skorelowane } z x 0,, ˆx 0, z(0),..., z(k 2) oraz E e(k 1)z T (k 1) = 0, stąd: E {a d} = A(k 1)E Analogicznie E {b c} = 0. A zatem: { } e(k 1)z T (k 1) G T (k 1) = 0 (26) P(k k 1) = A(k 1)P(k 1)A T (k 1)+G(k 1)Z(k 1)G T (k 1) (27)

24 Wyznaczanie wartości macierzy P(k) Wychodząc z definicji macierzy P(k): P(k) = E { } { } e(k)e T (k) = E (x(k) ˆx(k)) (x(k) ˆx(k)) T (28) oraz wykorzystując zależność: ˆx(k) = A(k 1)ˆx(k 1) + K [y(k) C(k)ˆx(k k 1)] +B(k 1)u(k 1) = = ˆx(k k 1) KC(k)ˆx(k k 1) + Ky(k) (29)

25 Wyznaczanie wartości macierzy P(k) c.d. Otrzymano: P(k) = E = E {[ x(k) [I KC(k)] ˆx(k k 1) KC(k)x(k) Kv(k) ] [ ] T } = { [(I KC(k)) e(k k 1) Kv(k)] [ ] T } = { } = E (I KC(k)) e(k k 1)e T (k k 1) (I KC(k)) T + { } +E Kv(k)v T (k)k T = (30) poniważ korelacja pomiedzy z, a v jest równa zero

26 Wyznaczanie wartości macierzy P(k) c.d. Kontynuując: { } P(k) = (I KC(k)) E e(k k 1)e T (k k 1) (I KC(k)) T + { } +KE v(k)v T (k) K T (31) Podsumowując, wartość macierzy P(k) można wyznaczyć z zalezności: P(k) = [I KC(k)] P(k k 1) [I KC(k)] T + KV(k)K T (32) Zauważmy, że P(k) jest funkcją macierzy K stanowiącej nadal wolny stopień swobody!

27 Wyznaczanie wartości macierzy P(k) c.d. Para równań: P(k k 1) = A(k 1)P(k 1)A T (k 1)+G(k 1)Z(k 1)G T (k 1) (33) P(k) = [I KC(k)] P(k k 1) [I KC(k)] T + KV(k)K T (34) stanowi kompletny zestaw pozwalający na wyznaczenie wartości macierzy kowariancji błędu P(k), przy ząłożeniu, że znana jest wartość K

28 Wyznaczanie wartości macierzy P(k) c.d. Do inicjalizacji algorytmu niezbędna jest znajomość: wartości początkowej kowariancji: P(0) = P 0 (35) wartości oczekiwanej stanu początkowego: ˆx(0) = E {x 0 } (36)

29 Optymalizacja P(k) względem K Niech K K + K, wtedy: P(k) = [I (K + K)C(k)] P(k k 1) [I (K + K)C(k)] T + +(K + K)V(k)(K + K) T + [I KC(k)] P(k k 1) [I KC(k)] T + KV(k)K T (37) pomijając wyrażenia drugiego rzędu w odniesieniu do K: P(k) = K [ C(k)P(k k ] 1) (I KC(k)) T + V(k)K T + [ ] + (I KC(k)) P(k k 1)C T (k) + KV(k) K T (38)

30 Optymalizacja P(k) względem K c.d. Przyrost wartości macierzy P(k): odpowiada wartości macierzy K: P(k) = 0 (39) [ 1 K(k) = P(k k 1)C T (k) C(k)P(k k 1)C T (k) + V(k)] (40)

31 Równania rekursywnego filtru Kalmana Kompletny zestaw rownań pozwalający na rekursywną estymację stanu składa się z: P(k k 1) = A(k 1)P(k 1)A T (k 1)+ +G(k 1)Z(k 1)G T (k 1) (41) P(k) = [I KC(k)] P(k k 1) [I KC(k)] T + KV(k)K T (42) [ 1 K(k) = P(k k 1)C T (k) C(k)P(k k 1)C T (k) + V(k)] (43) UWAGA: K(k) nie jest funkcją zależną od obserwacji (pomiarów), a zatem może być wyznaczona odpowiednio wcześniej

32 Podsumowanie k-ty krok estymacji Uaktualnienie równań rekursywango estymatora z chwili k 1 do k: w chwili czasu k 1 znane są: ˆx(k 1) oraz ˆx(k k 1), gdzie: ˆx(k k 1) = A(k 1)ˆx(k 1) + B(k 1)u(k 1) (44) a także P(k 1) oraz P(k k 1) wyznaczone zgodnie z (41) i (42) w oparciu o aktualny pomiar (chwila czasu k) wyznaczana jest bieżąca estymata stanu ˆx(k): ˆx(k) = ˆx(k k 1) + K(k) [y(k) C(k)ˆx(k k 1)] (45) gdzie K(k) wyznaczana jest z (43)

33 Podsumowanie Reprezentacja graficzna

34 Pytania?