3. Optimální odhad stavů systému
Hana Fitzová
Brno, 2007
Obsah
Optimální estimátor
Odhad stavů dynamického systému
Kalmanův filtr
Smoothing
Rozšířený Kalmanův filtr
Odhad nelineárních systémů metodou Monte Carlo
Shrnutí
3. Optimální odhad stavů systému ­ p.1/39
Úvod
Řada systémů je zadána přímo svým vnitřním
popisem.
Pozorovateli je skryta přímá informace o hodnotách
stavových proměnných.
Měřící přístroje podávají informaci o stavu systému.
Úkol: jak z pozorovaných výstupů y určit co
nejpřesněji stavy x .
Problém je vyřešen pro lineární systém
s gaussovskými šumy (Kalmanův filtr).
3. Optimální odhad stavů systému ­ p.2/39
Informace o stavech I
Mějme neznámý náhodný vektor X  Lnx
2 a vektor
naměřených dat z  Rnz
, který je realizací
náhodného vektoru Z  LnZ
2 .
Dále známe simultánní hustotu pravděpodobnosti
p(x, z).
Před měřením z je informace o X obsažena pouze
v hustotě p(x, z) a rozložení X udává tzv. apriorní
hustota
p0(x) = p(x, z)dz
3. Optimální odhad stavů systému ­ p.3/39
Informace o stavech II
Po provedení měření z je informace obohacena a
rozložení X je dáno podmíněnou hustotou, tzv.
aposteriorní hustotou
p1(x) = p(x|z)
Tyto úvahy jsou jádrem tzv. bayessovského přístupu
k optimálnímu odhadu.
Jediným skutečným rozložením je rozložení apriorní,
avšak aposteriorní rozložení má některé výhodné
vlastnosti s ohledem na co nejpřesnější odhad
realizace vektoru X, ne hustoty vektoru X.
3. Optimální odhad stavů systému ­ p.4/39
Kriterium nejmenších čtverců
Přesnost odhadu (estimace) se nejčastěji měří
kriteriem nejmenších čtverců (least mean squares).
Střední kvadratická chyba
Bud' E  Rnx
množina přípustných estimátorů
náhodného vektoru X. Pro každý přípustný estimátor
y  E zavedeme střední kvadratickou chybu vztahem
J(y) = E((X - y)T
(X - y))
3. Optimální odhad stavů systému ­ p.5/39
Optimální estimátor I
Optimální estimátor
Estimátor ^X  E nazveme optimálním podle kriteria
nejmenších čtverců (optimální MS estimátor), pokud
platí
J( ^X) = min
yE
J(y)
Optimální apriorní estimátor
Střední hodnota EX je optimálním estimátorem na
množině všech apriorních estimátorů.
Při označení ~X = X - ^X platí E( ~X) = 0 (odhad je
nestranný) a D( ~X) = D(X).
3. Optimální odhad stavů systému ­ p.6/39
Optimální estimátor II
Optimální aposteriorní estimátor
Podmíněná střední hodnota E(X|z) je optimálním
estimátorem na množině všech aposteriorních
estimátorů.
Opět při označení ~X = X - ^X platí E( ~X) = 0 (odhad
je nestranný) a D( ~X) = D(X|Z).
Jsou-li vektory X a Z nezávislé, pak
p(x, z) = p(x)p(z) a tedy i E(X|z) = E(X). Tedy při
nezávislosti apriorní a aposteriorní estimátor splývají.
3. Optimální odhad stavů systému ­ p.7/39
Podmíněné normální rozdělení
Necht'
X
Z
 N
x
z
,
x xz
zx z
Pak aposteriorní náhodná veličina X|z má normální
rozložení se střední hodnotou E(X|z) a varianční maticí
D(X|z), kde
E(X|z) = x + xz-1
z (z - z)
D(X|z) = x - xz-1
z zx
3. Optimální odhad stavů systému ­ p.8/39
Lineární MS estimátor I
Optimální MS estimátor na přípustné množině lineárních
estimátorů
E = {y = Az + b : A  Rnx×nz
, b  Rnx
}
je estimátor
^XLMS = x + xz-1
z (z - z)
kde x = EX, z = EZ, xz = C(X, Z), z = DZ.
3. Optimální odhad stavů systému ­ p.9/39
Lineární MS estimátor II
Lineární MS estimátor ^XLMS je nestranný.
Je-li vektor (XT , ZT )T rozložen normálně, pak
estimátory ^XMS a ^XLMS splývají.
3. Optimální odhad stavů systému ­ p.10/39
Příklad
Náhodná veličina X má rozložení X  Rs(0, 1). Tuto
veličinu měříme pomocí náhodné veličiny Z. Vztah mezi
veličinami je následující:
Z = ln
1
X
+ V
kde V  Ex(1), p(v) = pV (x) je šum měření.
Vypočtěte optimální lineární MS estimátor veličiny X.
3. Optimální odhad stavů systému ­ p.11/39
Odhad stavů dynamického systému
Mějme diskrétní dynamický systém
xt+1 = f(xt, ut, vt)(1)
yt = g(xt, ut, wt)(2)
s počátečním rozložením p(x0).
Budeme konstruovat optimální odhady stavů
x0, x1, . . . , xn rekurentním způsobem.
Pro odhad stavů máme k dispozici jen časové řady
vstupů a výstupů.
Aktuální stav xt se vypočte na základě minulého
stavu xt-1 a nových dat ut a yt.
3. Optimální odhad stavů systému ­ p.12/39
Odhad stavů DS (II)
Rozložení šumu vt pokládáme za známé, je tedy
rovnicí (1) jednoznačně určena podmíněná hustota
pravděpodobnosti p(xt+1|xt, ut).
Rovnicí (2) je určena podmíněná hustota
pravděpodobnosti p(yt|xt, ut).
Označíme
Dt = {u0, y0, . . . , ut, yt}(3)
data dostupná do času t.
3. Optimální odhad stavů systému ­ p.13/39
Odhad stavů DS (III)
Obě rovnice (1) a (2) jsou na minulých datech
nezávislé, lze tedy psát
p(xt+1|xt, ut, Dt-1) = p(xt+1|xt, ut)(4)
p(yt|xt, ut, Dt-1) = p(yt|xt, ut)(5)
Tj. stav xt obsahuje veškerou informaci obsaženou
v datech Dt-1 potřebnou pro predikci výstupu yt.
Dále řízení ut neovlivňuje přímo stav xt (přirozené
podmínky řízení), což lze zapsat jako:
p(xt|Dt-1, ut) = p(xt|Dt-1)(6)
3. Optimální odhad stavů systému ­ p.14/39
Odhad stavů DS (IV)
Jsme v čase t, máme k dispozici data Dt-1, chceme
odhadnout hodnotu xt.
Nejlépe ji aproximuje podmíněná hustota p(xt|Dt-1),
tzv. apriorní hustota.
Předpokládáme, že apriorní hustota je známá
funkce. Po doplnění dat hodnotami ut, yt máme
k dispozici data Dt, díky nimž můžeme aktualizovat
odhad stavu xt.
Hledáme tedy novou podmíněnou hustotu p(xt|Dt),
což je tzv. aposteriorní hustota.
3. Optimální odhad stavů systému ­ p.15/39
Věta (aposteriorní hustota)
Aposteriorní hustota p(xt|Dt) je dána vztahem
p(xt|Dt) =
p(yt|xt, ut)
p(yt|ut, Dt-1)
p(xt|Dt-1)
Důkaz plyne z Bayessova řetězového pravidla o
podmíněných hustotách.
Přechod od apriorní hustoty k hustotě aposteriorní se
nazývá datový krok.
Pro rekurentní výpočty je třeba ještě odvodit přechod
obrácený ­ tzv. predikční krok.
3. Optimální odhad stavů systému ­ p.16/39
Věta (predikční krok)
Predikční krok odhadu stavu je dán vztahem
p(xt+1|Dt) = p(xt+1|xt, ut)p(xt|Dt)dxt
Důkaz plyne z řetězového pravidla, z rovnice (4) a
dokončí se marginalizací rozdělení xt+1 podle xt.
Posledním krokem je zahájení rekurze. Je třeba znát
počáteční hustotu
p(x0|D-1) = p(x0)
která je předem známa nebo se určuje zkusmo.
3. Optimální odhad stavů systému ­ p.17/39
Spojení kroků
Datový a predikční krok lze spojit do jednoho, ale
ztratíme tím aposteriorní hustoty, které jsou obecně
lepší než hustoty apriorní.
p(xt+1|Dt) =
p(xt+1|xt, ut)p(yt|xt, ut)
p(yt|ut, Dt-1)
p(xt|Dt-1)dxt =
=
p(xt+1, yt|ut, Dt-1)
p(yt|ut, Dt-1)
3. Optimální odhad stavů systému ­ p.18/39
Kalmanův filtr
Kalmanův filtr je analogií výše uvedené nelineární
rekurze pro speciální případ, kdy jsou stavy x i
výstupy y rozloženy normálně.
Pak je apriorní i aposteriorní hustota opět normální.
Normálně rozloženou náhodnou veličinu plně určuje
její střední hodnota a rozptyl.
V průběhu výpočtu tedy stačí sledovat pouze tyto
dvě číselné charakteristiky, čímž se celý výpočet
zjednodušuje.
Mají-li být stavy rozloženy normálně, musí být
příslušný dynamický systém lineární s gaussovskými
šumy, což je největší slabina tohoto algoritmu.
3. Optimální odhad stavů systému ­ p.19/39
Věta (Kalmanův filtr I)
Mějme diskrétní stochastický systém tvaru:
xt+1 = Axt + But + vt(7)
yt = Cxt + Dut + wt(8)
splňující: x0  N(0, 0), vt  N(0, v), wt  N(0, w),
EvtwT
t = 0, Ex0vT
t = 0 t, kde vektor 0 a matice
0, v, w jsou známé.
Necht' jsou známa data Dt. Označme xt|k = xt|Dk.
3. Optimální odhad stavů systému ­ p.20/39
Věta (Kalmanův filtr II)
Pak apriorní estimátor xt|t-1 a aposteriorní estimátor xt|t
jsou normální pro všechna t:
xt|t-1  N(t|t-1, t|t-1)
xt|t  N(t|t, t|t)
kde střední hodnoty t|t-1, t|t a varianční matice
t|t-1, t|t se vypočtou dle následujících rekurentních
vztahů:
3. Optimální odhad stavů systému ­ p.21/39
Věta (Kalmanův filtr III)
t|t = t|t-1 + Kt(yt - Ct|t-1 - Dut)(9)
t|t = t|t-1 - KtCt|t-1(10)
t+1|t = At|t + But(11)
t+1|t = At|tAT
+ v(12)
Kt = t|t-1CT
(Ct|t-1CT
+ w)-1
(13)
První dva vztahy tvoří tzv. datový krok algoritmu, druhé
dva vztahy krok predikční, poslední vztah je tzv.
Kalmanovo zesílení (Kalmanův zisk).
3. Optimální odhad stavů systému ­ p.22/39
Smoothing v KF
Zpětný běh filtru na základě informací z času t = 1, . . . , N
t|N = t|t + Ft(t+1|N - t+1|t)(14)
t|N = t|t - Ft(t+1|t - t+1|N )Ft
T
(15)
Ft = t|tAT
-1
t+1|t
(16)
3. Optimální odhad stavů systému ­ p.23/39
Příklad (část I)
Lod' se pohybuje po rovníku východním směrem
rychlostí 10 námořních mil za hodinu. Okamžitou
rychlost lodi ovlivňují náhodné poryvy větru a nárazy
vln.
Navigátor lodi odhaduje každou hodinu zeměpisnou
délku lodi l a rychlost lodi s = dl/dt v mph. V čase
t = 0 odhadl navigátor polohu l0 = 0 a rychlost s = 10.
Dále pak zaznamenal údaje:
čas 1 2 3 4 5 6
poloha 9" 19.5" 29" 38.4" 50" 59.5"
3. Optimální odhad stavů systému ­ p.24/39
Příklad (část II)
Označíme-li lt, st polohu a rychlost lodi v čase t, pak
je úlohou navigátora optimální odhad veličin lt, st.
Počáteční odhady můžeme modelovat jako nezávislé
náhodné veličiny s normálním rozložením. Rozptyly
odhadů jsou sledovány a jejich odhady jsou Dl0 = 2,
Ds0 = 3.
Nejprve popíšeme chování systému. Během hodiny t
se lod' pohybuje rychlostí st, takže její poloha se
změní na:
lt+1 = lt + st
3. Optimální odhad stavů systému ­ p.25/39
Příklad (část III)
Rychlost kolísá díky náhodným vlivům, což
popíšeme pomocí náhodné veličiny et  N(0, 1):
st+1 = st + et
Rozptyl měrení sextantu udávaný výrobcem je
w = 2.
Stavový vektor definujeme jako xt =
lt
st
S využitím vztahů (9)-(13) odhadněte optimlání stavy
tohoto systému pomocí Kalmanova filtru.
3. Optimální odhad stavů systému ­ p.26/39
Návod
Načtěte matice systému a data (A, B, C, D, v, w,
x0, x0 , U, y, . . . ) 0|-1 = x0, 0|-1 = x0
V cyklu spočtěte t|t, t|t, t+1|t, t+1|t, t|N , t|N
Výsledky vykreslete do obrázků: predikce + filtrace +
smoothing pro polohu, totéž pro rychlost; dále vývoj
rozptylu (f+p+s) obou veličin.
Vytvořte funkci, která bude provádět jednotlivé kroky
Kalmanova filtru včetně smoothingu a příklad upravte
tak, aby tuto funkci využíval. Vstupními parametry
funkce bude: y, U, A, B, C, D, v, w, x0 a x0 .
Výstupy budou: t|t, t|t, t+1|t, t+1|t, t|N , t|N .
3. Optimální odhad stavů systému ­ p.27/39
Další příklady
Odhad mezery výstupu
Transmisní kanály
3. Optimální odhad stavů systému ­ p.28/39
Rozšířený Kalmanův filtr I
Princip rozšířeného Kalmanova filtru spočívá v tom,
že nelineární dynamický systém v každém kroku
približně nahradíme lineárním dynamickým
systémem a na tento pak aplikujeme obyčejný
Kalmanův filtr.
Vztahy odvozené výše nelze užít přímo, poněvadž
linearizace vede na jiný typ systému (viz dále).
3. Optimální odhad stavů systému ­ p.29/39
Rozšířený Kalmanův filtr II
Mějme nelineární diskrétní stochastický systém:
xt+1 = f(xt, ut) + vt(17)
yt = g(xt, ut) + wt(18)
s počáteční podmínkou x0  N(0, 0).
Rozšířený Kalmanův filtr definujeme jako algoritmus
výpočtů následujících estimátorů:
xt|t-1  N(t|t-1, t|t-1)(19)
xt|t  N(t|t, t|t)(20)
xt|N  N(t|N , t|N )(21)
3. Optimální odhad stavů systému ­ p.30/39
Rozšířený Kalmanův filtr III
t|t = t|t-1 + Kt(yt - g(t|t-1, ut))(22)
t|t = t|t-1 - KtCtt|t-1(23)
t+1|t = f(t|t, ut)(24)
t+1|t = Att|tAt
T
+ v(25)
Kt = t|t-1Ct
T
(Ctt|t-1Ct
T
+ w)-1
(26)
t|N = t|t + Ft(t+1|N - t+1|t)(27)
t|N = t|t - Ft(t+1|t - t+1|N )Ft
T
(28)
Ft = t|tAt
T
-1
t+1|t
(29)
3. Optimální odhad stavů systému ­ p.31/39
Rozšířený Kalmanův filtr IV
kde matice At, Ct jsou následující Jakobiány:
At =
f
x
(t|t) Ct =
g
x
(t|t-1)
Vztahy 19-20 představují filtrační (datový) krok.
Vztahy 21-22 představují predikční krok.
Vztahy 24-25 představují zpětný běh filtru
(smoothing).
3. Optimální odhad stavů systému ­ p.32/39
Princip metody Monte Carlo I
Metoda Monte Carlo je založena na tom, že
informaci o rozložení náhodné veličiny nese náhodný
výběr z tohoto rozložení.
Čím větší je rozsah výběru, tím je informace
přesnější.
Pro náhodný vektor x  Lnx
máme náhodný výběr
(x1, . . . , xn) rozsahu n, který obsahuje vzorky
xi  Rnx
.
Empirická hustota pravděpodobnosti získaná
z náhodného výběru je aproximací skutečné hustoty
pravděpodobnosti náhodného vektoru x.
3. Optimální odhad stavů systému ­ p.33/39
Princip metody Monte Carlo II
Empirickou hustotu pravděpodobnosti lze psát jako:
pn(x) =
1
n
n
i=1
(x - xi)
kde (x) : Rnx
 R je tzv. Diracova funkce.
Diracova funkce
(x) = lim
h0
h(x)
h(x) =
1
h
pro 0 < x < h, h(x) = 0 jinak
3. Optimální odhad stavů systému ­ p.34/39
Princip metody Monte Carlo III
Dále přiřadíme jednotlivým vzorkům xi váhy wi  0
tak, aby suma všech vah byla rovna jedné.
Empirická hustota pravděpodobnosti je pak tvaru:
pn(x) =
n
i=1
wi (x - xi)
3. Optimální odhad stavů systému ­ p.35/39
Vážený Bootstrap algoritmus I
Mějme dynamický systém tvaru:
xt+1 = f(xt, ut, vt)
yt = g(xt, ut, wt)
s počátečním stavem x0 a s empirickou hustotou
pn(x) =
n
i=1
wi(0| - 1)(x - xi(0| - 1))
Mějme data Dt. Pak odhady xt|t-1, xt|t, xt|N s empirickými
hustotami
3. Optimální odhad stavů systému ­ p.36/39
Vážený Bootstrap algoritmus II
pn(xt|t-1) =
n
i=1
wi(t|t - 1)(x - xi(t|t - 1))
pn(xt|t) =
n
i=1
wi(t|t)(x - xi(t|t))
pn(xt|N ) =
n
i=1
wi(t|N)(x - xi(t|N))
jsou spočteny váženým bootstrap algoritmem právě když
platí:
3. Optimální odhad stavů systému ­ p.37/39
Vážený Bootstrap algoritmus III
Filtrační (datový) krok: hustoty a váhy se
přepočítávají; vzorky zůstávají stejné.
xi(t|t) = xi(t|t - 1)
wi(t|t) = p(y(t)|xi(t|t - 1), u(t)) wi(t|t - 1)
wi(t|t) =
wi(t|t)
n
j=1 wj(t|t)
Predikční krok: hustoty a vzorky se přepočítávají
(vzhledem k jejich významnosti); váhy zůstávají
stejné.
xi(t + 1|t) = f(xi(t|t), u(t)) + vi(t)
wi(t + 1|t) = wi(t|t)
3. Optimální odhad stavů systému ­ p.38/39
Vážený Bootstrap algoritmus IV
Zpětný běh (smoothing): hustoty a váhy se
přepočítávají; vzorky zůstávají stejné.
xi(t|N) = xi(t|t)
wi(t|N) = wi(t|t)
n
j=1
wj(t + 1|N) p(xj(t + 1|N)|xi(t|N), u(t))
wi(t|N) =
wi(t|t)
n
j=1 wj(t|t)
3. Optimální odhad stavů systému ­ p.39/39