Filtr KalmanaStruktury 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

2013-10-09, Gdańsk

Założenia odnośnie filtru Kalmana

Przystępując do syntezy filtru Kalmana przyjmuje się następującezałożenia:

minimum kowariancji

liniowość filtru

estymaty stanu są nieobciążone: E {x(k)} = E {x(k)}, gdziex(k) jest estymatą stanu x(k)

Dynamika modelu systemuRównania stanu

Niech dany będzie liniowy niestacjonarny model dynamiki systemu:

x (k + 1) = A (k) x (k) + B (k)u (k) + G (k) z (k) (1)

gdzie:

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 niestacjonranymimacierzami stanu systemu, wejść i zakłóceń

Dynamika modelu systemuRó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ść

Dynamika modelu systemuZakłó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) zT (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) vT (k)

}= V (k)

(4)

Dynamika modelu systemuZakłócenia i szumy pomiarowe c.d.

Autokoralecja zakłóceń i szumów jest równa zeru

E{z (k) zT (j)

}= 0 ; j 6= k

E{v (k) vT (j)

}= 0 ; j 6= k

(5)

Ponadto zakłada się, że nie istnieje korelacja pomiędzyzakłóceniami i szumami pomiarowymi:

E{z (k) vT (j)

}= 0 (6)

dla k 6= j (dla uproszczenia)

Dynamika modelu systemuWarunki początkowe

Zakłada się, że warunki początkowe x0 są znane i w opraciu o ichznajomość mozna wyznaczyć:

E {x0}

E{x0xT0

}= P0

(7)

gdzie: P0 – macierz kowariancjiZanjomość P0 jest niezbędna do zainicjowania algorytmu filtruKalmana

ProblemSformuł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) eT (k)

}, e (k)

∆= x (k)− x (k) to jest dla

każdego wektora p i macierzy T, takiego, że p 6= 0 zachodzi:

pTP (k)p ¬ pTTp (8)

ProblemSpostrzeżenie

Niech a będzie wektorem. Rozważ liniową funkcję:

aTx (k) (9)

Błąd średniokwadratowy dla tej funckji wyraża się:

E[(aTx (k)− aT x (k)

) (aTx (k)− aT x (k)

)T]=

= E[(aT (x (k)− x (k))

) (aT (x (k)− x (k))

)T]

= E[aTe (k) · aTe (k)

]= E

[aTe (k) · eT (k) a

]= aTE

[e (k) eT (k)

]a = aTP (k) a – minimum

(10)

ProblemSpostrzeżenie c.d.

1 Minimalizowany jest błąd średnio kwadratowy w kotekściekażdej zmiennej stanu z osobna

2 Minimalizowana jest norma błędu średnio kwadratowego, np.:

E[e21 + e22 + . . .+ e2n

]– minimum (11)

WyprowadzenieLiniowość i rekursywność

Wychodząc z założenia o liniowości i rekursywnościposzukiwanego estymatora:

x (k) = Jx (k − 1) +Ky (k) + Lu (k − 1) (12)

gdzie:J,K,L są poszukiwanymi macierzami takimi, że x (k) jest:

nieobciążona estymatą stanugwarantującą minimum kowariancji błędu estymacji

WyprowadzenieNieobciążoność

Wykorzystując założenie o nieobciążoności estymat:

E {x (k)} = E {Jx (k − 1) +Ky (k) + Lu (k − 1)} =

= JE {x (k − 1)}+KE {y (k)}+ Lu (k − 1)

= E {x (k)}

(13)

WyprowadzenieNieobciąż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

A (k − 1) x (k − 1) +B (k − 1)u (k − 1) +G (k − 1) z (k − 1)

+

+Lu (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)

WyprowadzenieNieobciąż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

WyprowadzenieWartoś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!

Struktura estymatorapredyktor - 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)

Cx(k |k − 1) stanowi predykcję pomiaru y(k) wykonanąw chwili k − 1, co oznaczamy: y (k|k − 1).

Struktura estymatorapredyktor - korektor c.d.

Struktura estymatora ma ogólną postać:

x (k)︸ ︷︷ ︸aktualna estymata

= x (k|k − 1)︸ ︷︷ ︸predykcja stanu︸ ︷︷ ︸człon predykcyjny

+K [y(k)− y (k|k − 1)]︸ ︷︷ ︸predykcja błedu pomiarowego︸ ︷︷ ︸człon korekcyjny

(21)

gdzie:

y(k |k − 1) jest to wektor predykcji pomiarow y(k):y(k |k − 1) = Cx(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 zrównaniami stanu systemu, gdzie z(k − 1) zastąpione jestprzez E {z(k − 1)} = 0

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 orazwyznaczoną 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)+

−A(k − 1)x(k − 1)− B(k − 1)u(k − 1)

(23)

Błąd predykcji c.d.

Wykorzystując definicję błedu predykcji oraz redukującB(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 predykcjizależna od dynamiki stanu systemu

G(k − 1)z(k − 1) wpływ zakłóceń stanu systemu na błądpredykcji

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)}︸ ︷︷ ︸

=0

[·]T =

= E{e(k|k − 1)eT (k|k − 1)

}

Kowaraincja błędu predykcji c.d.

Kontynuując:

P(k|k − 1) = E{e(k|k − 1)eT (k|k − 1)

}= E

{[A(k − 1)e(k − 1) + G(k − 1)z(k − 1)] [·]T

}=

= E

A(k − 1)e(k − 1)︸ ︷︷ ︸a

+G(k − 1)z(k − 1)︸ ︷︷ ︸b

××

AT (k − 1)eT (k − 1)︸ ︷︷ ︸c

+GT (k − 1)zT (k − 1)︸ ︷︷ ︸d

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)eT (k − 1)

}AT (k − 1)+

+E {a · d}+ E {b · c}+

+G(k − 1)E{z(k − 1)zT (k − 1)

}GT (k − 1) =

= A(k − 1)E{e(k − 1)eT (k − 1)

}AT (k − 1)+

+G(k − 1)E{z(k − 1)zT (k − 1)

}GT (k − 1)+

+E {a · d}+ E {b · c}

Kowaraincja błędu predykcji c.d.

Oszacowanie E {a · d}:

a ≈ e(k − 1) = e(k − 1) [x0, x0, z(0), . . . , z(k − 2), v(0), . . . , v(k − 1)]d = z(k − 1)

(25)z(k − 1) nie jest skorelowane z x0,, x0, z(0), ..., z(k − 2) orazE{e(k − 1)zT (k − 1)

}= 0, stąd:

E {a · d} = A(k − 1)E{e(k − 1)zT (k − 1)

}GT (k − 1) = 0 (26)

Analogicznie E {b · c} = 0.A zatem:

P(k |k−1) = A(k−1)P(k−1)AT (k−1)+G(k−1)Z(k−1)GT (k−1)(27)

Wyznaczanie wartości macierzy P(k)

Wychodząc z definicji macierzy P(k):

P(k) = E{e(k)eT (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)

Wyznaczanie wartości macierzy P(k) c.d.

Otrzymano:

P(k) = E

{[x(k)− [I−KC(k)] x(k |k − 1)−KC(k)x(k)−Kv(k)

][·]T

}=

= E{

[(I−KC(k)) e(k|k − 1)−Kv(k)] [·]T}

=

= E{

(I−KC(k)) e(k |k − 1)eT (k |k − 1) (I−KC(k))T}

+

+E{Kv(k)vT (k)KT

}=

(30)poniważ korelacja pomiedzy z, a v jest równa zero

Wyznaczanie wartości macierzy P(k) c.d.

Kontynuując:

P(k) = (I−KC(k)) E{e(k |k − 1)eT (k |k − 1)

}(I−KC(k))T +

+KE{v(k)vT (k)

}KT

(31)Podsumowując, wartość macierzy P(k) można wyznaczyć zzalezności:

P(k) = [I−KC(k)]P(k|k − 1) [I−KC(k)]T +KV(k)KT (32)

Zauważmy, że P(k) jest funkcją macierzy K stanowiącej nadalwolny stopień swobody!

Wyznaczanie wartości macierzy P(k) c.d.

Para równań:

P(k|k−1) = A(k−1)P(k−1)AT (k−1)+G(k−1)Z(k−1)GT (k−1)(33)

P(k) = [I−KC(k)]P(k|k − 1) [I−KC(k)]T +KV(k)KT (34)

stanowi kompletny zestaw pozwalający na wyznaczenie wartościmacierzy kowariancji błędu P(k), przy ząłożeniu, że znana jestwartość K

Wyznaczanie wartości macierzy P(k) c.d.

Do inicjalizacji algorytmu niezbędna jest znajomość:

wartości początkowej kowariancji:

P(0) = P0 (35)

wartości oczekiwanej stanu początkowego:

x(0) = E {x0} (36)

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)KT

(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)KT

]+

+[− (I−KC(k))P(k |k − 1)CT (k) +KV(k)

]∆KT

(38)

Optymalizacja P(k) względem K c.d.

Przyrost wartości macierzy P(k):

∆P(k) = 0 (39)

odpowiada wartości macierzy K:

K(k) = P(k |k−1)CT (k)[C(k)P(k|k − 1)CT (k) + V(k)

]−1(40)

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)AT (k − 1)++G(k − 1)Z(k − 1)GT (k − 1)

(41)

P(k) = [I−KC(k)]P(k|k − 1) [I−KC(k)]T +KV(k)KT (42)

K(k) = P(k |k−1)CT (k)[C(k)P(k |k − 1)CT (k) + V(k)

]−1(43)

UWAGA:K(k) nie jest funkcją zależną od obserwacji (pomiarów), a zatemmoże być wyznaczona odpowiednio wcześniej

Podsumowaniek-ty krok estymacji

Uaktualnienie równań rekursywango estymatora z chwili k − 1 dok :

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 jestbieżą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)

PodsumowanieReprezentacja graficzna

Pytania?