FAKULTA STAVEBNÍ GEODÉZIE, KARTOGRAFIE A GEOINFORMATIKA

Transkript

1 ČESKÉ VYSOKÉ UČENÍ TECHNICKÉ FAKULTA STAVEBNÍ OBOR GEODÉZIE, KARTOGRAFIE A GEOINFORMATIKA BAKALÁŘSKÁ PRÁCE MATICOVÉ ROZKLADY PRO KALMANŮV FILTR Vedoucí práce: doc. RNDr. Milada Kočandrlová, CSc. Katedra matematiky Květen 2014 Pavel Kulmon

2 Abstrakt V práci jsou uvedeny maticové rozklady s vyšetřením tvaru matic a podrobné realizace jejich výpočtu. Dále jsou uvedeny základní algoritmy Kalmanova filtru společně s rozšířenou a unscentovanou modifikací. Kalmanův filtr je ilustrován na dvou konkrétních případech. Klíčová slova Maticové rozklady, QR, LU, Choleského rozklad, SVD, Kalmanův filtr, EKF, UKF Abstract This work presents a matrix decomposition with the examination of the design of a detailed implementation and their calculation. The following are the basic Kalman filter algorithms together with extended and unscented modifications. The Kalman filter is illustrated in two specific cases. Keywords Matrix decomposition, QR, LU, Cholesky decomposition, SVD, Kalman filter, EKF, UKF

3 Poděkování Rád bych na tomto místě poděkoval své vedoucí bakalářské práce doc. RNDr. Miladě Kočandrlové, CSc. za její odborné vedení, věcné rady a užitečné připomínky. Dále bych rád poděkoval své rodině za podporu při studijích.

4 Čestné prohlášení Prohlašuji, že tuto bakalářskou práci Maticové rozklady pro Kalmanův filtr jsem vypracoval a sepsal zcela samostatně. Veškerá použitá literatura a materiály jsou uvedeny v seznamu zdrojů. V Praze dne (podpis autora)

5 Obsah Úvod 2 1 Lineární algebra Základní pojmy Matice a maticové operace LU rozklad Definice LU rozkladu Užití LU rozkladu Choleského rozklad Definice Choleského rozkladu Prvky matice R Využití Choleskeho rozkladu QR rozklad Gram-Schmidtova ortogonalizace Givensovy rotace QR rozklad Gram-Schmidtův QR rozklad Givensův QR rozklad Rozklad pomocí singulárních hodnot - SVD Lineární transfromace Diagonalizace matice Rozklad pomocí singulárních hodnot Kalmanův filtr Úvod Úvod Statistické pojmy Kalmanův filtr Proces a šumy Predikce a update Rozšířený Kalmanův filtr Příklad k ilustraci EKF Unscentovaný Kalmanův filtr Zpětné vyhlazení Aplikace rozkladů Vlastní výpočty Úvod Norma matice Zkoumání numerické stability maticového rozkladu Druhý příklad aplikace EKF Závěr 31 Literatura 32 Přílohy 33 1

6 Úvod Předložená práce, jak její název a zadání napovídá, je věnována dekompozici matic pro využití v algoritmech Kalmanovy filtrace náhodné veličiny. Práce má tři kapitoly a přílohy. Na začátku první kapitoly jsou shrnuty některé základní pojmy maticového počtu, které jsou používány v následujících algoritmech.těžiště této kapitoly je v podrobném přehledu pěti algoritmů pro rozklad matice. Od nejjednoduššího LU rozkladu a jeho speciálního případu Choleského rozkladu, přes dva QR rozklady (Gram-Schmidtův a Givesův rozklad) po rozklad pomocí singulárních hodnot. V úvodu druhé kapitoly jsou uvedeny definice normálního rozdělení náhodné veličiny. Hlavní pozornost je zde věnována odvození algoritmu klasického, rozšířeného a uncentovaného Kalmanova filtru náhodné veličiny včetně zmínky o zpětném vyhlazení. Třetí kapitola je věnovaná aplikaci rozkladů. V úvodu kapitoly jsou shrnuty normy matic pro posouzení stability zvolené čtveřice matic. Kapitolu uzavírá aplikace filtru na měření dráhy projektilu. V příloze jsou popsány implementace dekompozic matic v jazyce C++ pro čtvercové matice a jsou zde přiloženy výsledné grafické výstupy ze zpracování příkladu ze třetí kapitoly. 2

7 Kapitola 1 Lineární algebra 1.1 Základní pojmy V následujícím odstavci uvedeme některé definice a operace maticového počtu, které budeme dále používat Matice a maticové operace Matice Maticí A = (a ij ) typu m n nazýváme schéma mn reálných nebo komplexních čísel sestavených v m řádcích a n sloupcích. V této práci se omezíme pouze na reálné matice. Matice A typu m m se nazývá čtvercová matice řádu m. Matice jednotková Matice jednotková je čtvercová matice E = (e ij ) řádu m, pro kterou platí e ij = δ ij, kde δ ij je Kroneckerovo delta. Hodnost matice Hodnost matice A typu m n je číslo h(a) rovné maximálnímu počtu lineárně nezávislých řádků či sloupců matice (vždy menší, nebo rovné min(m, n)). Regulární matice Matice A řádu m je regulární, platí-li h(a) = m. Neplatí-li uvedená rovnost, říkáme že matice A je singulární. Matice transponovaná Matice A T = (a ji ) typu n m je matice transponovaná k matici A = (a ij ) typu m n a vzníká výměnou řádků a sloupců. Součin matic Necht A = (a ij ) je matice typu m n a B = (b ij ) matice typu n p. Potom matice C = (c ij ) je typu m p, kde c ij = n a ik b kj, i = 1, 2,, m a j = 1, 2,, p je součinem k=1 matic A, B a značíme C = AB. Součin matic není obecně komutativní, avšak platí pro něj asociativní zákon, tedy platí ABC = (AB)C = (AB)C. Součin konečného počtu matic má smysl, je-li pro každé dvě sousední matice definován součin. 3

8 Matice inverzní Matice A 1 je matice inverzní k regulární matici A, pro kterou platí rovnost A 1 A = AA 1 = E. Transpozice součinu matic Matice transponovaná k matici C = AB je matice C T = B T A T. Inverze součinu matic Matice inverzní k C = AB je matice C 1 = B 1 A 1. Ortogonální matice Matice A = řádu m je ortogonální maticí, platí-li AA T = E, tj. A T = A 1. Dále pro ni platí det(a) = 1 nebo det(a) = 1. Matice symetrická a antisymetrická Matice A = (a ij ) řádu m je symetrická, resp. antisymetrická, je-li A = A T, resp. A = A T. Pozitivně definitní matice - Sylvestrovo kritérium Symetrická matice A řádu m je pozitivně definitní, jsou-li její všechny hlavní minory kladné. Matice je negativně definitní, jestliže v posloupnosti hlavních minorů se střídá znaménko od prvního minoru záporného. Poznámka: Budiž A regulární matice, potom B = AA T je pozitivně definitní. Taktéž opačně B = A T A je pozitivně definitní. Z těchto definic vyplývá existence Choleského rozkladu. Vlastní čísla matice Vlastní čísla λ 1, λ 2,, λ m čtvercové matice A jsou kořeny charakteristické rovnice det(a λe) = 0. Singulární čísla matice Singulární čísla σ 1, σ 2,, σ n matice A typu m n jsou odmocniny vlastních čísel matice B = AA T, která jsou stejná jako vlastní čísla matice C = A T A. 1.2 LU rozklad Definice LU rozkladu Budiž regulární matice A řádu m, splňující Sylvestrovo kritérium. Pak existuje jediná matice L = (l ij ) řádu m pro kterou platí l ii = 1 a zároveň l ij = 0 kdykoli j > i. K této matici existuje jediná matice U = (u ij ), pro kterou platí u ij = 0 kdykoli i > j. Říkáme, že matice L je dolní trojúhelníková s jedničkami na diagonále a U je horní trojúhelníková matice. Pro uvedené matice platí A = LU. Této dekompozici říkáme LU rozklad. Z uvedené definice můžeme pro matici A libovolného řádu vytvořit její podobu LU 4

9 rozkladu. Například pro matici řádu 3 3 platí a 11 a 12 a 13 a 21 a 22 a 23 = a 31 a 32 a 33 u 11 u 12 u 13 l 21 u 11 l 21 u 12 + u 22 l 21 u 13 + u 23 l 31 u 11 l 31 u 12 + l 32 u 22 l 31 u 13 + l 32 u 23 + u 33 Analogicky lze sestavit prvky matice libovolného řádu n. Je tedy patrné, že pro prvky matice L, resp. U platí následující vztahy l ij = 1 j 1 a ij l ik u kj ; i = j + 1, j + 2,..., n (1.1) u jj u ij = a ij i 1 k=1 k=1. l ik u kj ; j = 1, 2,..., n; i = 1, 2,..., j. (1.2) Užití LU rozkladu Máme-li matice L a U, lze jich využít například pro řešení soustavy rovnic. Je-li Ax = b soustava rovnic, v níž A splňuje podmínky LU rozkladu, můžeme její řešení převést na řešení dvou soustav Ly = b a následně Ux = y, jejichž řešení je usnadněno tím, že L a U jsou v trojúhelníkovém tvaru. Pro výpočet matic potřebujeme n 3 operací. Podle odstavce pro LU rozklad je nutné, aby matice A splňovala Sylvestrovo kritérium. Toto ovšem nesplňuje obecně každá regulární matice. Je-li A regulární matice řádu m, nesplňující Sylvestrovo kritérium, provedeme transformaci matice PA, kde P je premutační matice taková, že PA = B. Potom matice B je již regulární řádu m splňujíci Sylvestrovo kritérium. Tuto rovnost využívá většina implementací LU rozkladu (např. software Matlab nebo knihovna LAPACK). LU rozklad není obecně unikátní. V odstavci je uveden unikátní rozklad za předpokladu, že L má na diagonále hodnoty 1. Tvar matice L lze volit, a proto mohou mít rozklady různý tvar (viz. následující Choleského rozklad). 1.3 Choleského rozklad Definice Choleského rozkladu Budiž matice A symetrická a pozitivně definitní. Potom existuje unikátní regulární horní trojúhelníková matice R = (r ij ), pro kterou platí A = R T R. Matici R říkáme Choleského faktor. Choleského rozklad je speciálním případem LU rozkladu, kde volíme R = U = L T. 5

10 1.3.2 Prvky matice R Jak již bylo řečeno, Choleského rozklad je speciálním případem LU rozkladu a rovnice (1.1) tedy přejde na tvar a 11 a 12 a 13 r 2 11 r 11 r 12 r 11 r 13 a 21 a 22 a 23 = r 11 r 12 r r 2 22 r 12 r 13 + r 22 r 23. a 31 a 32 a 33 r 11 r 13 r 12 r 13 + r 22 r 23 r r r33 2 I zde jsou matice řádu n sestavitelné analogicky. Pro prvky matice R můžeme napsat následující vztahy r 11 = a 11 (1.3) r ii = i 1 a ii rir; 2 i = 2, 3,..., n (1.4) r=1 r=1 r ij = 1 j 1 a ij r ir r jr ; i = 2, 3,..., n; j = 1, 2,..., i 1. (1.5) r jj Vzhledem k numerické stabilitě výpočetních procesů s použitím PC a odmocninám v algoritmu může nastat situace, kdy prvky na hlavní diagonále vyjdou obecně komplexní [9] Využití Choleského rozkladu Choleského rozklad lze využít obdobně jako LU rozklad (viz. odstavec 1.2.2). Výhodou Choleského algoritmu je menší výpočetní náročnost, nebot počítá pouze jednu trojúhelníkovou matici. Vzhledem k tomu, že její diagonála nemusí obsahovat pouze jedničky, je výhodnější rozklad s diagonální maticí D a v matici R s jedničkami na diagonále, tedy A = R T DR. 1.4 QR rozklad Gram-Schmidtova ortogonalizace Budiž báze vektorového prostoru V posloupnost vektorů b 1, b 2,..., b n. Množinu všech lineárních kombinací daných vektorů označme b 1, b 2,..., b n. Takové množině říkáme lineární obal bázových vektorů (tedy právě vektorový prostor). Ke každé takové bázi lze sestrojit bázi jinou, kterou budiž posloupnost vektorů e 1, e 2,..., e n. Pro tuto bázi platí, že vektory jsou ortogonální a b 1, b 2,..., b n = e 1, e 2,..., e n. Procesu vytvoření takové báze říkáme Gram-Schmidtova ortogonalizační metoda [1]. Jsou-li všechny vektory e i jednotkové, nazýváme tuto bázi ortonormální bází. 6

11 1.4.2 Givensovy rotace Matici G(i, j, θ) = 1... c s s c... 1 (1.6) kde i < j, nazýváme Givensova matice rotace. Tato matice se od jednotkové matice liší pouze o nediagonální a diagonální prvky s = sin (θ) c = cos (θ). Givensova matice rotace popisuje rotaci prostoru v rovině dané osami i, j o úhel θ. Matice je ortogonální. Násobení AG(i, j, θ) mění i, j-tý řádek matice A a násobení G(i, j, θ)a mění i, j-tý sloupec matice A. U Givensových matic rotace můžeme s výhodou využít možnosti volby úhlu θ. Ten zvolíme tak, abychom dosáhli vynulování všech prvků matice A nacházejících se pod hlavní diagonálou. Vyjdeme-li z definice Givensových rotací, můžeme uvažovat vektor x = (x i ) R n, a pak podle [2] platí a z podmínky y j = 0 vyplývá y = G(i, j, θ) T x (1.7) cx i sx j, y k = sx i + cx j, x k, k = i k = j k i, j (1.8) sx i + cx j = 0 (1.9) x i c = (1.10) x 2 i + x 2 j s = x j. (1.11) x 2 i + x 2 j Dělení v rovnostech (1.10) a (1.11) je normování, které je důlěžité pro to, aby hodnoty odpovídaly uvažovaným hodnotám goniometrických funkcí. 7

12 1.4.3 QR rozklad Budiž A matice typu m n, mající lineárně nezávislé sloupce, pro kterou platí m n. Potom existují matice Q řádu m a horní trojúhelníková matice R typu m n, pro které platí A = QR. Matici Q volíme ortogonální nebo alespoň s ortogonálními sloupci. Existuje mnoho způsobů jak realizovat QR rozklad. Uvedeme zde dva. Prvním je algoritmus využívající Gram-Schmidtovy ortogonalizace, druhý využívá Givensovy rotace Gram-Schmidtův QR rozklad V případě, že použijeme algoritmus využívající Gram-Schmidtovy ortogonalizace, matice Q bude mít jen ortonormální sloupce. Matice bude vznikat postupným vytvářením ortonormální báze k bázi sloupcových vektorů matice A. Matice R je následně dána jednoznačně maticovým násobením v požadované rovnosti A = QR. Vzhledem k tomu, že matice R je horní trojúhelníková, platí pro její první prvek a pro první sloupce matice Q rovnice r 11 = m a 2 k1 (1.12) Zbylé prvky matic se vypočtou k=1 q i1 = a i1 r 11 ; i = 1, 2,, m. (1.13) m r ik = r ij a jk (1.14) j=1 z j = a j,k k 1 kde i = 1, 2,, k 1, j = 1, 2,, m, k = 2, 3,, n Givensův QR rozklad i=1 q ji r ik (1.15) r kk = z (1.16) q jk = 1 r k k z (1.17) Označíme-li Givensovy matice rotace nezávisle na argumentu jako G j, kde index má význam pořadí rotace, můžeme na základě jejich ortogonality psát vztah kde Q T A = R, Q T = G T ng T n 1...G T 2 G T 1. (1.18) 8

13 Je zjevné, že Givensovy rotace samy o sobě zaručují existenci QR rozkladu. V aplikaci Givensových rotací pro QR rozklad není potřeba počítat úhel θ, postačí zvolit dva prvky, z nichž druhý chceme vynulovat. Pro výpočet nediagonálních prvků matic rotace použijeme podle [2] následující vzorce pro parametr τ y k = b > a, τ = a b, s = 1 1+τ 2, b a, τ = b a, c = 1 1+τ 2, c = sτ s = cτ (1.19) kde a, b jsou vybrané prvky matice A a prvek b je určen k vynulování. Pakliže je prvek b již nulový, platí c = 1 a s = 0. Budiž matice D = (d ij ) typu m n, pro kterou chceme získat QR rozklad. Výpočet provádíme podle následujících rovnic a = d i 1 j b = d ij r i 1 k = cd i 1 k sd ik r ik = sd i 1 k + cd ik d i 1 k = r i 1 k d ik = r ik, (1.20) kde j = 1, 2,, n, i = m, m 1,, j + i a k = j, j + 1,, n. V některých aplikacích je vhodné vyčíslit i matici Q. Budiž G = (g ij ) typu m m nějaká matice rotace. Platí a transpozicí vztahu (1.18) získáme g i 1 i 1 = g ii = c g i 1 i = g i i 1 = s Q = G 1 G 2...G n 1 G n. (1.21) 1.5 Rozklad pomocí singulárních hodnot - SVD Lineární transformace Budiž V = v 1, v 2,..., v n báze nějakého vektorového prostoru V a budiž w 1, w 2,..., w n báze vektorového prostoru W. Pak existuje matice A taková, že w 1 v 1 w 2 = v 2 AT. (1.22). w n. v n 9

14 Matice A T se nazývá matice přechodu od báze V k bázi W. Tato matice tedy obsahuje po sloupcích souřadnice vektorů báze W vzhledem k bázi V. Budiž řádkový vektor x V V a x W W, kde x W jsou souřadnice vektoru x V vzhledem k bázi W. Pak platí x T V = Ax T W. Matice A tedy provádí transfomaci vektoru z báze W do báze V. Pro obrácený postup by samozřejmě platilo x T W = A 1 x T V. Mějme například obyčejnou kanonickou bázi V = (1, 0, 0), (0, 1, 0), (0, 0, 1) a dále obecnou bázi B = (2, 3, 2), (1, 4, 2), (2, 4, 2). Matice přechodu tedy bude A = Dále mějme vektor a B = (3, 2, 1), jež má souřadnice vzhledem k bázi B. Chceme-li jeho souřadnice vzhledem k bázi V, vypočteme = 21 = a V Diagonalizace matice Budiž A = (a ij ) čtvercová matice řádu n. Budiž matice S = (s ij ) řádu n mající ve sloupcích n lineárně nezávislých vlastních vektorů matice A. Pak platí kde je diagonální matice s vlastními čísly matice A Rozklad pomocí singulárních hodnot S 1 AS = Λ (1.23) λ 1 0 Λ =..... (1.24) 0 λ n Budiž matice A typu m n, kde m n. Použijme tuto matici ve smyslu AV = UM, tedy jako projekci z vektorového protoru V na prostor U, kde matice M je diagonální matice měřítkových faktorů. Nyní uvažujme prostor V, resp. U jako řádkový, resp. sloupcový prostor matice A. Stanovíme-li jako podmínku, že matice V a U jsou ortogonální, můžeme rovnici upravit na tvar A = UMV 1 = UMV T. 10

15 Nahradíme-li matici M maticí Σ, získáme A = UΣV T. (1.25) Tomuto rozkladu říkáme singulární rozklad matice. Matici V získáme z rovnosti A T A = VΣU T UΣV T. Vzhledem k ortogonálnosti matic V a U je σ A T A = V..... VT. (1.26) 0 σn 2 Matice A T A je čtvercová, symetrická a pozitivně definitní. Odtud vyplývá, že rovnice (1.26) je diagonalizací matice A T A. Sloupcové vektory matice V jsou tedy vlastní vektory matice A T A a matice V je ortogonální. Matice Σ 2 je diagonální a obsahuje vlastní čísla matice A T A, což jsou druhé mocniny singulárních čísel matice A. Matici U bychom mohli dopočítat z definice nebo lze provést podobnou operaci jako pro získání matice V, a to a po úpravě AA T = UΣV T VΣU T (1.27) σ AA T = U..... UT. (1.28) 0 σn 2 Protože platí, že vlastní čísla matice AB jsou stejná jako vlastní čísla matice BA, je další výpočet prostý. Singulární rozklad matice je tímto jednoznačně dán. 11

16 Kapitola 2 Kalmanův filtr 2.1 Úvod Úvod Kalmanův filtr je tzv. estimátor (odhad, angl. estimate), řešící problém odhadu okamžitého stavu lineárního dynamického systému narušeného tzv. bílím šumem s využitím měření na nichž stav lineárně závisí, a která jsou také zatížena bilým šumem. V této práci se budeme zabývat pouze jeho diskrétní verzí. Algoritmus je nazván po mad arském elektroinženýrovi Rudolfu E. Kálmánovi (*1930). Vzhledem k tomu, že se na vývoji algoritmu (především teoretické části) podílel také Richard S. Bucy, je někdy nazýván také Kalman-Bucy filtr. První článek, ve kterém R.E. Kálmán představil svůj filtr, byl vydán v roce 1960 a je uveden v seznamu literatury viz [8] Statistické pojmy Normální rozdělení Náhodná veličina X má normální rozdělení s parametry µ R a σ > 0, je-li její hustota pravděpodobnosti dána přepisem [4] f(x) = 1 σ x µ 2π e ( ) 2 2σ, (2.1) kde x R. Při µ = 0 a σ = σ 2 = 1 mluvíme o tzv. Normovanén normálním rozdělení. Normální rozdělení značíme N(µ, σ 2 ) a normované speciálně N(0, 1). Hustota pravděpodobnosti normovaného normálního rozdělení je tedy dána předpisem f(u) = 1 2π e u2 2, (2.2) kde u R. Každou veličinu X mající normální rozdělení N(µ, σ 2 ) lze převést na veličinu U mající normované normální rozdělení. Převod je dán vztahem U = X µ. (2.3) σ 12

17 Distribuční funkce normovaného normálního rozdělení je dána vztahem Φ(u) = 1 2π Vícerozměrné normální rozdělení u e t2 2 dt. (2.4) Budiž náhodný sloupcový vektor x = (x 1, x 2,, x n ) R n. Tento vektor má n- rozměrné normální rozdělení s parametry µ R n a Σ R n2 +, je-li jeho hustota pravděpodobnosti dána podle [4] vztahem f(x) = 1 (x µ) (2π) T Σ 1 (x µ) n Σ e 2, (2.5) kde µ je vektor středních hodnot jednotlivých složek vektoru x a cov(x 1, x 1 ) cov(x 1, x 2 ) cov(x 1, x n )..... Σ =..... cov(x n, x 1 ) cov(x n, x 2 ) cov(x n, x n ) (2.6) je symetrická, pozitivně definitní matice. Této matici říkáme kovarianční matice vektoru x. Značíme n-rozměrné normální rozdělení N n (µ, Σ), kde pro n = 1 index nepíšeme. 2.2 Kalmanův filtr Proces a šumy Základní předpoklad je existence stavového vektoru x R n. Jeho stav v epoše k chceme odhadnout. Základní vztah má podobu lineární diferenční rovnice x k = F k 1 x k 1 + C k 1 u k 1 + w k 1, (2.7) kde velmi důležitý je vektor w k 1 R r značící tzv. bílý šum systému (procesu), u kterého uvažujeme, že má rozdělení N r (o, Q). Dolní indexy matic a vektorů značí náležitost k časové epoše. Aby mohlo docházet k odhadu, musíme v každé epoše do výpočtu přispět novými hodnotami. Mějme tedy vektor měření z R l a druhou diferenční rovnici tvaru z k = H k x k + v k, (2.8) kde v k R l značí tzv. bílý šum měření, u nějž uvažujeme, že má rozdělení N l (o, R). Tento šum uvažujeme i při klasických metodách odhadu parametrů náhodného vektoru (např. MNČ), kde uvažujeme, že měření je zatíženo chybami mající normální rozdělení. Jak bylo řečeno, pro šumy platí následující Ew i = Ev i = 0 Ew i w T j = δ ij Q i 13

18 Ev i v T j = δ ij R i, kde Q i R r2 resp. R i R l2, jsou kovarianční matice šumu systému, resp. šumu měření a δ ij je Kroneckerovo delta. Kovarianční matice šumu systému a šumu měření mohou být obecně časově proměnné (změna může nastat s každou novou časovou epochou), ve většině aplikací je však uvažujeme jako konstantní. Existuje mnoho forem rovnic KF, především z důvodu jejich snadné algebraické manipulace, motivované především konkrétními aplikacemi. Postihnout je všechny je téměř nemožné, proto se v této práci budeme zabývat určitým systémem značení. V rovnici (2.7) matice F R n2 vztahuje stavový vektor z epochy k 1 k stavovému vektoru v epoše k stejně jako matice C R r2 provádí totéž s vektorem řídícího vstupu u R r. V rovnici (2.8) matice H R ln vztahuje stavový vektor v epoše k na vektor měření v téže epoše. Jednou z nejzajímavějších konstrukcí v KF je Kalmanova matice zisku (angl. Kalman gain matrix). Pro její zavedení uvažujme následující rovnost ˆx + k = K1 kˆx k + K2 kz k, (2.9) která vyjadřuje myšlenku, že a posteriorní odhad stavového vektoru v epoše k (odhad značen stříškou, a posteriorní odhad značen pravým horním indexem - ) je lineárně závislý na a priorním odhadu stavového vektoru v epoše k a na vektoru měření z téže epochy. Tato konstrukce dává smysl, vyjma řídícího vstupu žádná jiná data k dispozici nemáme, a zmiňovaný vstup je již obsažen v a priorním odhadu stavového vektoru. Otázkou zůstává, jaký tvar mají matice K 1 k, resp. K 2 k, jejichž horní indexy slouží pouze pro odlišení (nejedná se o mocniny). Tyto matice jsou K 1 k = E K 2 kh k (2.10) K 2 k = P k HT k (H k P k HT k + R k ) 1. (2.11) Z rovnice (2.12) vyplývá zavedení následujících chyb e x k = ˆx k x k e + x k = ˆx + k x k e z k = ẑ k z k, kde uvažujeme a priorní odhad vektoru měření z k jako z k = Hˆx k. (2.12) Poznámka: Nejprve zaved me kovarianční matice chyb jednotlivých odhadů stavového vektoru P k = Ee x k e T x k (2.13) P + k = Ee+ x k e +T x k. (2.14) 14

19 Zkoumáním nekorelovaných veličin vystupujících v rovnicích (2.7-8) získáme Ew k z T i = 0 Ev k z T i = 0 Ew k z T k = 0 Ew kˆx T k = 0. Tedy šum systému v současné epoše k, i < k, není korelovaný s kteroukoli složkou vektoru měření z epoch předcházejících a pro šum měření platí stejný předpoklad rozšířený i na současnou epochu i k. Z podmínky ortogonality a vzhledem k tomu, že rovnice (2.9) je pouze lineární kombinací v maticové formě, můžeme psát následující podmínku E(x k ˆx + k )zt i = 0 (2.15) pro i k. Vektor x k značí skutečnou hodnotu stavového vektoru, tedy vektor který nejsme schopni znát. Dosazením rovnic (2.7),(2.8),(2.9) do (2.15) a uvážením uvedených předpokladů (včetně nepřítomnosti řídícího vstupu) získáme E(F k 1 x k 1 K 1 kˆx k K2 kh k F k 1 x k 1 K 2 kv k )z T i = 0, kde matici F uvažujeme v nejobecnější formě, tedy jako časově proměnnou a vztaženou ke stejné epoše jako vektor měření jež převádí a podmínka platí pro i < k. Matice lineárních kombinací z diferenčních rovnic neuvažujeme jako náhodné proměnné, proto podle pravidel o počítání se střední hodnotou můžeme psát E((x k K 1 kx k K 2 kh k x k ) K 1 k(ˆx k x k))z T i = 0. Protože naše požadavky na očekávané hodnoty musí platit pro kteroukoli i-tou epochu předcházející momentální, můžeme psát (E K 1 k K 2 kh k )E(x k z T i ) = 0. (2.16) Řešením rovnice (2.16) je matice z rovnice (2.10). Z podmínky pro skutečný vektor měření je odvozena podmínka stejného charakteru pro jeho a priorní odhad E(x k ˆx + k )ẑ T i = 0. (2.17) Odečtením rovnice (2.15) od rovnice (2.17) získáme (po úpravách) E(x k ˆx + k )(ẑ k z k) T = 0. To je velmi důležitá rovnice, protože z ní je vidět, že hledáme střední hodnotu pro součin dvou vektorů chyb, a priorních chyb odhadu vektoru měření a a posteriorních chyb odhadu stavového vektoru systému. Následnými substitucemi a uvážením předešlých korelací přejdeme na tvar E(x k K 1 kˆx k K2 k z k )(H kˆx k z k) T = 0. 15

20 Tato rovnice s uvážením zavedených chyb a provedením transpozice získá tvar E( e x k + K 2 kh k e x k K 2 kv k )(e T x k H T k v T k ) = 0 E((K 2 kh k E)e x k e T x k H T k + K 2 kv k v T k ) = 0 Řešením rovnice (2.18) je matice z rovnice (2.11). [E K 2 kh k ]P k HT k K 2 kr k = 0. (2.18) Predikce a update Jak již bylo řečeno, KF je nejen speciálním případem sekvenčního vyrovnání, ale je také filtrem, který se zakládá na přínosu ke změně stavu systému, zjištěném z nesrovnalosti mezi očekávaným stavem a stavem vyplývajícím z měření (viz. zisková matice K 2 k). Jinak řečeno, zásadní ideou, která se v KF uplatňuje je tzv. update (čes. aktualizace). S každou novou sadou měření z i získáváme nové informace a náš odhad (ve smyslu nejmenších čtverců) se zlepšuje. Z rovnic (2.12),(2.13) a (2.14) získáme pro update, tedy výpočet a posteriorního odhadu stavového vektoru ˆx + k, rovnici ˆx + k = (E K2 kh k )ˆx k + K2 kz k ˆx + k = ˆx k + K2 k(z k H kˆx k ), (2.19) která vyjadřuje smysl Kalmanovy ziskové matice. Matici z k H kˆx k říkáme inovace a vyjadřuje rozdíl mezi naším předpokladem o chování systému a zjištěním z měření. Násobením tohoto rozdílu matici K 2 k získáváme korekci našeho a posteriorního odhadu stavového vektoru. Výpočet a priorního odhadu vyplývá z rovnice (2.7) s uvážením absence řídícího vstupu ˆx k = F k 1ˆx + k 1, (2.20) kde logicky pro výpočet využíváme nejlepší a posteriorní odhad stavového vektoru z předchozí epochy. Úloha odhadu stavového vektoru přirozeně obsahuje i úkol získat k tomuto odhadu příslušnou kovarianční matici. Zakládá-li se výpočet a posteriorního odhadu na novém měření z epochy k a na a priorním odhadu stavového vektoru, musíme do jeho výpočtu zahrnout také kovarianční matice obou těchto vektorů. Kovarianční matice vektoru ˆx k je dána P k = F k 1P + k 1 FT k 1 + Q k 1, (2.21) kde index kovarianční matice šumu procesu odpovídá indexu z rovnice (2.7). Kovarianční matice vektoru ˆx + k je dána Josephovou rovnicí [6] ve tvaru P + k = (E K2 kh k )P k (E K2 kh k ) + K 2 kr k K 2 k. (2.22) Poznámka: Substitucí z rovnic (2.7) a (2.20) do rovnice (2.13) získáme (2.21). Uvážením rovnice (2.19) ve tvaru ˆx + k x k = ˆx k + K2 kh k x k + K 2 kv k K 2 kh kˆx k x k 16

21 a jejím dosazením do (2.14) získáme P + k = E((E K2 kh k )e x k + K 2 kv k )((E K 2 kh k )e x k + K 2 kv k ) T. Z této rovnice vyplývá Josephova forma kovarianční matice a posteriorního odhadu. Násobíme-li rovnici (2.14) zprava výrazem (H k P k HT k + R k ) obdržíme P k HT k = K 2 k(h k P k HT k + R k ). Roznásobením Josephovy rovnice (2.22) a dosazením této rovnosti získáme jiný tvar a posteriorní kovarianční matice P + k = (E K2 kh k )P k. (2.23) Tento tvar kovarianční matice je v aplikacích více používaný vzhledem k jeho kompaktnější formě. Předešlé odstavce dávají dohromady podobu výpočetního procesu základní formy algoritmu KF, která je zdokumentována v následující tabulce. Tabulka 1. - rovnice klasického KF fáze výpočty Predikce ˆx k = F k 1ˆx + k 1 P k = F k 1P + k 1 F k 1 + Q k 1 Korekce K 2 k = P k HT k (H k P k HT k + R k ) 1 ˆx + k = ˆx k + K2 k(z k H kˆx k ) P + k = (E K2 kh k )P k Rozšířený Kalmanův filtr Princip rozšířeného KF (EKF, angl. extended KF ) je podobný filozofii Taylorovy řady. Uvažujme, že vztahy pro výpočet x k a z k z rovnic (2.7-8) nejsou lineární, tedy nedají se zapsat pomocí matic. Pro potřeby výpočtu EKF (stejně jako u zmíněné Taylorovy řady) linearizujeme tyto nelineární vztahy a tuto linearizaci použijeme pouze v nějakém blízkém okolí námi zvoleného výchozího bodu. Vycházejme ze vztahů x k = f(x k 1, u k 1, w k 1 ) z k = h(x k, v k ). Stejně jako v předchozím, šumy procesu ani měření neznáme, máme pouze povědomí o jejich rozdělení. Zavedením matic parciálních derivací F, H, W, V můžeme rovnice psat ve tvaru x k = ˆx k + F(x k 1 ˆx + k 1 ) + Ww k 1 (2.24) 17

22 z k = ẑ k + H(x k ˆx k ) + Vv k. (2.25) Prvky matice parciálních derivací jsou definovány následujícím způsobem f ij = f i(x k 1, u k 1, 0) x j w ij = f i(x k 1, u k 1, 0) w j h ij = h i(ˆx k, 0) x j v ij = h i(ˆx k, 0) v j, (2.26) kde w ij, resp. v ij jsou prvky matic W resp. V a w j, resp. v j jsou složky vektorů šumu. Analogicky pro jednoduchý KF definujeme a priorní odhady stavového vektoru a vektoru měření ˆx k = f(ˆx+ k 1, u k 1, 0) ẑ k = h(ˆx k, 0). Nahradíme lineární kombinace Ww k 1, resp. Vv k novou proměnnou ε k 1, resp. η k. Jsou to opět náhodné veličiny mající přibližně rozdělení ε k 1 N(o, WQW T ) η k N(o, VRV T ). Vyjdeme-li z definice a priorní chyby odhadu stavového vektoru, dostaneme s ohledem k EKF e x k = F(x k 1 ˆx + k 1 ) + ε k 1 e z k = H(x k ˆx k ) + η k. Stejně jako u klasického KF probíhají dvě fáze odhadu, predikce a korekce. Rovnice EKF jsou shrnuty v tabulce 2. Tabulka 2. - rovnice EKF fáze výpočty Predikce ˆx k = f(ˆx+ k 1, 0) P k = F k 1P + k 1 F k 1 + W k 1 Q k 1 W T k 1 Korekce K 2 k = P k HT k (H k P k HT k + V k R k V k T ) 1 ˆx + k = ˆx k + K2 k(z k h(ˆx k, 0)) P + k = (E K2 kh k )P k 18

23 Je nutné podotknout, že v rovnicích předpokládáme absenci řídícího vstupu. Funkce f, h je nutné znát pro výpočet matic parciálních derivací. Proto uvažujeme, že ve výpočtech můžeme a priorní odhad stavového vektoru a měření vypočíst z nich, nicméně lze je nahradit již vypočtenými maticemi parciálních derivací (např. z důvodu rychlejšího výpočtu). Samozřejmě za takového předpokladu klesá numerická stabilita Příklad k ilustraci EKF Inspirováno příkladem ze cvičení předmětu Teorie chyb a vyrovnávací počet 2(152TCV2). Mějme měřící zařízení umístěné v bodě [0, 0, 0] a dále objekt pohybující se po ose y. Souřadnice polohy objektu budou tedy vždy [0, y, 0]. Předpokládejme, že objekt se pohybuje pohybem rovnoměrně zrychleným. Stavový vektor tedy bude x k = x k v k a k kde jednotlivé složky vektoru jsou definovány a k = t v k = x k t kde poloha, rychlost a zrychlení jsou dány, ( ) xk = v k t t, x k = x k 1 + v k 1 t + a k 1 t 2 x k = v k 1 + a k 1 t a k = a l 1. Funkce jednotlivých fyzikálních veličin by měly být obecně vektorové. Vzhledem k tomu, že náš případ je jednorozměrný (pohyb pouze v ose y), postačí definice jako skalárních veličin. Pomocí definic z odstavce o EKF můžeme ihned psát matici F ve tvaru F = 1 t t t Jak již bylo řečeno, KF je diskrétně časový, a tomu jsou přizpůsobeny i rovnice veličin ve stavovém vektoru. Stanovíme-li pro každou epochu i příslušný čas t i, ve kterém započala, pak definujeme časovou změnu mezi epochami t = t i t i 1 a počáteční podmínky pro t i 1 jako x i 1, v i 1, a i 1. Uvažujeme-li absenci řídícího vstupu, pro rovnici procesu platí předpis 2. x k = F k 1 x k 1 + W k 1 w k 1. 19

24 Pro sestavení matice W, tedy matice převádějící vektor šumu procesu, je třeba vrátit se k zadání úlohy. Zde bylo uvedeno, že šum procesu máme uvažovat jako náhodilou změnu zrychlení definovanou rovnicí w = ȧ = a t, kde se opět jedná o počítání se skalárními veličinami. Z definice šumu procesu vyplývají přímo fyzikální rovnice, ze kterých lze získat matici W. Stanovením výchozí veličiny, integrací a zavedením integračních konstant (ve fyzikálním významu počátečních podmínek) získáme a = a 0 + ȧt v = v 0 + a 0 t + ȧt2 2 r = r 0 + v 0 t + a 0t ȧt3 6. Přejdeme-li od fyzikálního značení ke značení zavedenému pro KF, získáme obdobné rovnice, a to a k = a k 1 + w k 1 t v k = v k 1 + a k 1 t + w k 1 t 2 x k = x k 1 + v k 1 t + a k 1 t 2 Odtud z rovnice (2.26) získáme matici W W = t 3 6 t 2 2 t w k 1 t 3. 6 Tato matice byla v úloze pro účely zmíněného předmětu již zadána vyučujícím. Podle rovnice (2.26) získáme matici H ve tvaru 1 H = 0. 0 Kroky výpočtu jsou tedy následující, nejprve se vypočtou a priorní odhady stavového vektoru a jeho kovarianční matice z rovnic ˆx k = F k 1ˆx + k 1 P k = F k 1P + k 1 F k 1 + W k 1 Q k 1 W k 1, 20

25 kde indexy matic F, W, Q dávají najevo přechod od epochy k 1 k epoše k. Kovarianční matice šumu procesu byla volena empiricky, nebot tu nemáme fakticky možnost odhadnout. Byla tedy zvolena skalární hodnota, protože šum je zde také skalár, taková aby nezaváděla zbytečnou nejistotu do výpočtu, ale aby také dávala dostatečnou volnost vývoje procesu. Dále se již podle rovnic (2.11) vypočte zisková matice a pomocí ní a posteriorní odhady stavového vektoru a jeho kovarianční matice. Kovarianční matice měření (zde v každé epoše měřena jedna vzdálenost) byla zvolena jako variance této vzdálenosti, získaná z parametrů měřícího přístroje. Příslušná matice V byla (vzhledem k tomu, že nebylo specifikováno jinak) volena jako 1, nebot byl uvažován standartní chybový model. Inicializační hodnota stavového vektoru x init = a inicializační hodnota jeho kovarianční matice byla nulová matice řádu Unscentovaný Kalmanův filtr Unscentovaná podoba KF (UKF, angl. unscented KF ) řeší obdobný problém jako EKF. Jakým způsobem provádět výpočet pomocí KF v případě, že vztahy pro převod vstupujících vektorů z minulé epochy do současné a vztahy pro převod a priorního odhadu stavového vektoru nejsou lineární? V EKF je použita obdoba Taylorovy řady, tedy využití parciálních derivací pro získání aproximace v blízkém okolí. Tento princip je obecně vhodný, ale přirozeně se vyskytují i závislosti, které nejsou ve svém blízkém okolí lineární a pro tyto je EKF silně nestabilní. Další problémovou stránkou EKF je implementace výpočtu parciálních derivací (Jakobiánů funkcí) do výpočetních algoritmů. V rámci UKF by zmíněné nedostatky EKF měla řešit tzv. unscentovaná transfromace [7]. Máme-li náhodný vektor x se střední hodnotou x a kovarianční maticí Σ xx, který chceme převést na jiný náhodný vektor y tak, že platí y = f(x), je naším základním požadavkem vypočítat střední hodnotu ȳ a kovarianční maticí Σ yy nově vzniklého vektoru. Pro potřeby výpočtu těchto statistik náhodného vektoru provádíme výběr sigma bodů z vektoru x. Takto vybraných bodů bude 2n + 1, nebot výběr bodů je symetrický podle střední hodnoty náhodného vektoru a pro výběr platí X 0 = x z ) X i = x + ( (n + κ)σ xx ) X i+n = x ( (n + κ)σ xx s vektorem vah W χ daným svými složkami W X0 = κ n + κ 21 i i

26 W Xi = W Xi+n = 1 2(n + κ) 1 2(n + κ), kde κ R, i = 1, 2,, n. U hodnot bodů odmocnina z kovarianční matice je přirozeně matice, dolní index závorky značí řádek či sloupec této matice. Statistiky vektoru y se po získání všech sigma bodů vypočtou následovně Σ yy = 2n i=0 ȳ = Y = f(x ) 2n i=0 W Xi Y i W Xi (Y i ȳ)(y i ȳ) T. Algoritmus UKF je pak velmi podobný EKF, nicméně do predikčních kroků je zahrnuta unscentovaná transformace, tedy výběr sigma bodů a výpočet odhadu statistik transformovaného vektoru. Podrobný výpočet je uveden v [7], zde uvedeme jen nejvýznamnější kroky. V EKF počítáme s a posteriorním odhadem z minulé epochy k 1. Nicméně pro účel výběru sigma bodů je třeba tento vektor upravit na vektor ) x a k = ( xk w k 1 resp. pro jeho a posteriorní odhad v předchozí epoše ( ) x + x a+ k 1 = k 1, o kde nulový blok je sloupcový vektor délky r. K tomuto odhadu přísluší kovarianční matice ( P + P a+ k 1 = k 1 P + ) xv k 1. P + xv k 1 Q k 1 Výběr sigma bodů Xk 1 a+ proběhne právě z vektoru x a+ k. Výpočet a priorních odhadů a unscentované transformace po výběru proběhne podle rovnic X k ˆx k = f(x a+ k 1, u k 1 ) 2n a = i=0 W i X k a dále kovarianční matice a priorního odhadu stavového vektoru je P k 2n a = i=0 W i (X k ˆx k )(X k ˆx k )T. 22

27 Obdobným způsobem se získá i a priorní odhad vektoru měření. Je tedy nutné opět transformovat sigma body transformované funkcí f funkcí observačního modelu h Z k a dále již přímo odhad vektoru měření ẑ k = h(x k, u k 1) 2n a = i=0 W i Z k. V posledních dvou krocích, které jsou zahrnuty do predikční části, je zapotřebí vypočítat kovarianční matici inovace P νν a společnou kovarianční matici inovace a a priorního odhadu stavového vektoru P xz. Tyto matice jsou dány vztahy 2n a P νν = R k + W i (Zk ẑ k )(Z k ẑ k )T 2n a i=0 P xν = W i (Xk ˆx k )(Z k ẑ k )T. i=0 UKF řeší především problémy vyskytující se při výpočtu Jakobiánů funkcí, nebot zde probíhá pouze transformace vybraných sigma bodů. Odhad statistik je přesnější než u EKF. Jedná-li se o systém, kde nelinearita není tak silná aby linearizace v blízkém okolí nebyla dostačující a pro kterou není výpočet Jakobiánů náročný, je praktičtější využít EKF. Výpočetní algoritmus podle [7] je shrnut v tabulce 3. Tabulka 3. - rovnice UKF fáze výpočty Predikce ˆx k = 2na W i Xk i=0 P k = 2na W i (Xk ˆx k )(X k ˆx k )T i=0 Korekce W k = P xνp 1 νν ν k = z k ẑ k ˆx + k = ˆx k + W kν k P + k = P k W kp ννw T k Modifikované rovnice korekční části vyplývají především ze znalosti (nízké výpočetní náročnosti) kovariančních matic Zpětné vyhlazení Smyslem Kalmanovy filtrace bylo získat nejlepší odhad stavového vektoru ˆx + k v epoše k s využitím informací z předchozích epoch společně s měřeními, která přicházela v každé 23

28 nové epoše, nezávisle na již vypočtených stavech. Zpětné vyhlazení (BS angl. backward smoothing) dává lepší odhady stavového vektoru ˆx s k využitím dat před a po epoše k. Tak jako forem dopředného procesu KF existuje tolik druhů jako aplikací, i u BS je tomu stejně. Uvedeme zde pouze jednu formu BS z čistě informativních důvodů, nebot na tyto algoritmy není tato práce zaměřena. Uvedeným druhem algoritmu zpětného vyhlazení je fixed-interval smoothing (FI), konkrétně algoritmus RTS navržený v roce Principem FI je získávání odhadů ˆx s k pro k = 0, 1,, N s využitím měření z k pro 0 k N. Některé aplikace obsahují části tři: dopředný filtr, zpětný filtr a vyhlazovač. Algoritmus RTS kombinuje poslední dvě složky dohromady, obsahuje tedy pouze dopředný filtr a zpětný vyhlazovač. Dopředným filtrem tedy získáme a posteriorní odhad stavového vektoru ˆx + k a jeho kovarianční matici P+ k. Tyto hodnoty přísluší epoše k. Výhoda algoritmu vyhlazení RTS je, že závisí pouze na datech produkovaných dopředným filtrem, je tedy výrazně menší objem dat, který je nutno držet v paměti. Další nespornou výhodou je možnost počítat pouze vyhlazený odhad stavového vektoru bez nutnosti k němu počítat kovarianční matici, nebot tyto veličiny se počítají zcela nezávisle. Algoritmus RTS vyhlazení je shrnut v tabulce 4. Tabulka 4. - rovnice RTS vyhlazovacího algoritmu Predikce ˆx k = F k 1ˆx + k 1 P k = F k 1P + k 1 FT k 1 + W k 1 Q k 1 W T k 1 Korekce K 2 k = P k HT k (H k P k HT k + V k R k V T k ) 1 ˆx + k = ˆx k + K2 k(z k H kˆx k ) Smoothing ˆx s N = ˆx + k P s N = P + k K 2s k = P + k FT k (P k+1 ) 1 P s k = P + k K2s k (P k+1 Ps k+1)k 2s T k ˆx s k = ˆx + k + K2s k (ˆx s k+1 ˆx k+1 ) 24

29 Kapitola 3 Aplikace rozkladů 3.1 Vlastní výpočty Úvod V první části se podíváme na podmíněnost matic. Dále na konkrétním příkladě aplikujeme rozšířený Kalmanův filtr Norma matice Analogicky k vektorovým normám definujeme i normy maticové a značíme A normu matice A. Maticových norem je vícero a jsou různě definované. V této práci uvedeme pouze některé z nich, nicméně pro všechny normy platí podle [9] nerovnosti a rovnosti { = 0 A = 0 A > 0 jinak A + B A + B AB A B αa = α A. V uvedených nerovnostech a rovnostech uvažujeme, že na levé i pravé straně užíváme stejný typ normy. Řádková norma n A R = max a ij sloupcová norma p-norma i=1,2,,m j=1 m A S = max a ij j=1,2,,n i=1 m n A p = p a ij p, i=0 j=0 25

30 kde pro p = 1 mluvíme o normě oktaedrické a pro p = 2 mluvíme o Frobeniově normě. Definice a manipulace s normami jsou důležitou součástí výpočtu tzv. podmíněnosti (maticové) úlohy či matice samotné. Je-li dána soustava lineárních rovnic v maticovém tvaru Ax = b, pak její řešení, v případě regulární matice A, hledáme ve tvaru x = A 1 b. Ve výpočtech realizovaných s pomocí osobních PC získáváme pouze do jisté míry numericky nepřesné řešení této soustavy. Po zpětném dosazení vypočteného řešení získáváme reziduum řešení. O velikosti rezidua rozhoduje rozdíl mezi přesným řešením a přibližným řešením a dále podmíněnost matice soustavy A. Podmíněnost matice posuzujeme podle čísla podmíněnosti cond(a), které je dáno jako pro které dále platí cond(a) = A A 1, cond(a) = cond(a 1 ). Jak již bylo popsáno, norem matic je velké množství a všechny nedávají stejné výsledky. Z toho plyne, že i číslo podmíněnosti matice je závislé na zvolené normě. V Kalmanově filtru nás především zajímá vliv inverze na danou matici. Jinak řečeno, v rovnici pro ziskovou matici K 2 k = P k HT k (H k P k HT k + R k ) 1, je zdrojem numerické nestability inverze matice složené z kovarianční matice a priorního odhadu stavového vektoru a kovarianční matice šumu měření. Matice je tedy symetrická, pozitivně definitní. A právě u kovariančních matic často dochází k numerickým nepřesnostem způsobeným špatnou podmíněností matice. Tomuto problému se věnujeme v závěru této práce. Chceme-li provádět inverzi matice A u níž je proveden rozklad takový, že platí pak inverzní matice k matici A je matice A = BCD, A 1 = D 1 C 1 B 1. Jsou-li matice B, C, D lépe podmíněné než matice A, hovoříme o zvýšení numerické stability našeho výpočtu (inverze matice). Uvažujeme-li, že známe například SVD rozklad matice A ve tvaru A = U A Σ A V T A, 26

31 pak její inverzi můžeme psát ve tvaru A 1 = V A Σ 1 A U T A. Vzhledem k tomu, že matice Σ je diagonální, můžeme psát její inverzní matici jako σ σ2 1 0 Σ 1. = σn 1 což je diagonální matice s převrácenými hodnotami k diagonálním prvkům matice Σ. Jak je vidět, žádnou inverzi provádět nemusíme. Ostatní rozklady uvedené v této práci vyžadují inverzi alespoň jedné z matic rozkladu. Pro účely zkoumání numerické stability jsme vybrali čtyři symetrické pozitivně definitní matice, které mají simulovat kovarianční matice vyskytující se často v geodetických aplikacích. V minulém odstavci byla řeč o normách a jejich použití k výpočtu čísla podmíněnosti matice. Vzhledem k tomu, že výpočty v této kapitole byly prováděny v software Matlab R, je vhodné uvést dvě funkce pro výpočet čísla podmíněnosti, které jsou v něm implementované. První z nich je funkce cond, jejíž syntax v Matlabu je následující c = cond(x,p), kde první parametr X je matice pro níž chceme získat číslo podmíněnosti a druhý parametr p dává možnost volby typu normy (1 pro 1-normu, 2 pro 2-normu, fro pro Frobeniovu normu, inf pro nekonečnou normu). Zajímavé je, že při 2-normě využívá algoritmus funkce SVD rozkladu a číslo podmíněnosti určí jako podíl největšího a nejmenšího singulárního čísla matice X. U této funkce platí, že čím větší číslo podmíněnosti, tím blíže je matice singulární. Další funkcí pro určení podmíněnosti matice je funkce rcond, její syntax je c = rcond(x), kde vstupním parametrem je matice pro niž chceme zjistit číslo podmíněnosti. Z matematického hlediska rcond dává převrácenou hodnotu čísla podmíněnosti. Čím blíže je číslo c nule, tím blíže je matice k singulární matici. Pro účely našich výpočtů byly dále v prostředí Matlab vytvořeny funkce pro určení norem uvedených v předchozím odstavci Zkoumání numerické stability maticového rozkladu Jak již bylo řečeno, pro zkoumání numerické stability jsme vybrali čtyři matice. Jsou to matice Σ 1 =

32 Σ 3 = Σ 4 = Σ 2 = Jak bylo řečeno, jsou to matice symetrické, pozitivně definitní, připadá tedy v úvahu i Choleského a LU rozklady. Čísla podmíněnosti těchto matic, získaná využitím dříve zmíněné funkce cond (bez druhého parametru, implicitně tedy počítá 2-normu, viz. poznámky u interních funkcí Matlabu) jsou cond(σ 1 ) = cond(σ 2 ) = cond(σ 3 ) = cond(σ 4 ) = e Matice byly voleny s různým číslem podmíněnosti, od relativně dobře podmíněné matice Σ 1 po velmi špatně podmíněnou matici Σ 4. Následně byly pro všechny zmíněné matice provedeny LU, QR, Choleského a SVD rozklady pomocí interních funkcí Matlabu. Tyto matice byly následně invertovány, nejlepším možným způsobem, tedy pakliže je např. v QR rozkladu matice Q ortogonální, nebyla invetována, ale pouze transponována. Následně byly původní matice násobeny s maticemi k nim inverzními. Výsledkem této operace by měla být matice jednotková (viz. kapitola 1). Nicméně vzhledem k omezenému numerickému rozsahu Matlabu a špatné podmíněnosti matic byly matice pouze blízké jednotkovým maticím. Jako kritérium pro vhodnější rozklad pro snížení nepřesností inverzí matice jsme zvolili řádkovou normu. Výsledky pokusu byly očekávané. Pro matice Σ 1, Σ 2, Σ 3 se jako nejvhodnější rozklad ukázal LU. Pro čtvrtou matici Σ 4 vyšel nejlépe Choleského rozklad, který se využívá v mnoha algoritmech, které pracují se symetrickými pozitivně definitními maticemi (v geodézii především kovariančními) pro zvýšení stability výpočtů. Výsledky také naznačují, že i pro první tři matice by Choleského rozklad byl vhodný, nebot řádkové normy vyšly vždy řádově stejné jako pro LU rozklad. Jednotlivé normy jsou shrnuty v následující tabulce pro každý z rozkladu a každou z matic (tabulka 5). 28

33 Tabulka 5. - vypočtené normy LU QR Choleského SVD Σ Σ Σ Σ V následujících výpočtech algoritmem EKF budeme tedy aplikovat na matici v inverzi v Kalmanově ziskové matici Choleského rozklad Druhý příklad aplikace EKF Druhý příklad na němž budeme ilustrovat práci a výpočty spojené s EKF byl vymyšlen a navržen pro aplikaci rozkladu na inverzi matice (popsáno dříve). Nebyla využita data z odstavce 2.2.4, nebot vektor měření zde obsahuje pouze jeden měřený prvek a nedochází tedy k inverzi matice ale pouze k výpočtu reciproké hodnoty skaláru. Mějme projektil, vystřelený z odpalovacího zařízení s počáteční rychlostí v = 200m/s pod elevačním úhlem α = 20 gon. Jedná se tedy o projektil s plochou dráhou letu. Bez ohledu na technickou realizaci (GPS, em. impulsy, laserové měření) měříme dvě vzdálenosti k danému projektilu. Uvážíme-li, že projektil byl vystřelen z počátku souřadné soustavy X 1 = [0, 0], pak z tohoto místa je měřena první vzdálenost s 1. Druhá vzdálenost s 2 je měřena z místa o souřadnicích X 2 = [50, 0], kde jednotka souřadného systému odpovídá hodnotě 1m. Z fyziky je známo, že při zanedbání všech okolních vlivů, se projektil vystřelený pod elevačním úhlem α 0 s počáteční rychlostí v 0 pohybuje po tzv. balistické křivce, tedy části paraboly. Celý pohyb (poloha projektilu) je pak popsán vektorovou rovnicí r = r 0 + v gt2 2, kde r = (x, y, z) je polohový vektor. Sledovaný pohyb se děje v rovině xy, tedy z = 0. Vektor r 0 = (x 0, y 0, z 0 ) je vektor počátečních hodnot. Vektor v = (v x0, v y0, 0) = (v 0 cos (α), v 0 sin (α), 0) a konečně vektor g = (0, g, 0), kde g. = 9.81, je tíhové zrychlení. Abychom sestavili matice potřebné pro algoritmus EKF, musíme fyzikální rovnice napsat ve tvaru diferenčních rovnic. Platí v xk = v xk 1 v yk = v yk 1 g t x k = x k 1 + v xk 1 t y k = y k 1 + v yk 1 t g t

34 Matici F můžeme tedy rovnou psát ve tvaru 1 t F = t a uvažovanou korekci (uživatelský vstup) ve tvaru T = 0 0 g t2 2 g t Šum procesu můžeme uvažovat například jako náhodnou změnu rychlosti projektilu jak ve směru osy x, tak ve směru osy y. Bez odvození (podobná úvaha jako v odstavci 2.2.4) tedy uvážíme matici W W = t t 0 0 t t Kovarianční matice R vektoru měření z k = (s 1k, s 2k ) je diagonální s variancemi měřených vzdáleností na diagonále ( ) σ 2 R = s1 0 0 σs a dále kovarianční matici Q šumu procesu bylo nutno odhadnout. K dispozici byla měření od 1 sekundy od vystřelení projektilu. Jako inicializační hodnoty byly zvoleny hodnoty spočtené z rovnic pohybu projektilu, tedy bez uvážení narušení pohybu. K dispozici bylo dohromady 532 epoch měření. Grafy odchylek hodnot získaných filtrací od hodnot skutečných jsou součástí příloh stejně jako algoritmus výpočtu v prostředí Matlab. 30

35 Závěr Ve své práci jsem za použití zdrojů uvedených v seznamu literatury prostudoval a sestavil pět teoretických algoritmů pro maticové rozklady: LU rozklad a jeho speciální případ rozklad Choleského, dva QR rozklady, a to Gram-Schmidtův a Givensův rozklad, a dále singulární rozklad, viz kapitola 1. Z těchto pěti algortmů jsem LU, Choleského a QR (Gramm-Schmidtovou ortogonalizací) implementoval v jazyce C++ využitím datové struktury pole. Ve druhé kapitole jsem si odvodil vzorce pro algoritmus klasického a rozšířeného Kalmanova filtru. Rozšířený filtr jsem ilustroval na příkladu rovnoměrného pohybu, řešeného v předmětu Teorie chyb a vyrovnávací počet 2. Dále jsem zde uvedl unscentovanou podobu a zpětné vyhlazení Kalmanova filtru. Ve třetí kapitole jsem zkoumal numerickou stabilitu maticových rozkladů. K tomu účelu jsem zvolil čtyři matice používané v geodetických výpočtech, od relativně dobře podmíněné matice po matici velmi špatně podmíněnou. Pro první tři matice vyšel nejlépe LU rozklad, pro čtvrtou matici rozklad Choleského. Dále jsem v této kapitole navrhl další příklad využití Kalmanova filtru opět pro filtraci dat měření ze sledování objektu, tentokrát pohybujícího se v rovině. Tento příklad jsem vyřešil a použil pro aplikaci maticového rozkladu v algoritmu rozšířeného Kalmanova filtru. 31

36 Literatura [1] KOČANDRLOVÁ, Milada a Jaroslav ČERNÝ. Geo-matematika I. Vyd. 1. Praha: Česká technika - nakladatelství ČVUT, ISBN [2] GOLUB, Gene H a Charles F VAN LOAN. Matrix computations. 3rd ed. Baltimore: Johns Hopkins University Press, c1996, xxvii, 694 s. Johns Hopkins studies in the mathematical sciences. ISBN [3] Greg Welch, Gary Bishop, An Introduction to the Kalman Filter, University of North Carolina at Chapel Hill Department of Computer Science, 2001 [4] LIKEŠ, Jiří a Josef MACHEK. Počet pravděpodobnosti. Praha: Státní nakladatelství technické literatury, 1981, 159 s. [5] MERVART, Leoš a Zdeněk LUKEŠ. Adjustment calculus. Vyd. 1. Praha: Nakladatelství ČVUT, 165 s. ISBN [6] STRANG, Gilbert. Linear algebra, geodesy, and GPS. Wellesley: Wellesley-Cambridge Press, ISBN [7] JULIER, Simon J. a Jeffery K. UHLMANN. A New Extension of the Kalman Filter to Nolinear Systems. s. 12. Dostupné z: welch/kalman/media/pdf/julier1997 SPIE KF.pdf [8] KALMAN, R.E. A New Approach to Linear Filtering and Prediction Problems. Transactions of the ASME - The Journal of Basic Engineering. 1960, s. 12. Dostupné z: [9] MÍKA, Stanislav. Numerické metody algebry: matematika pro vysoké školy technické. 2 vyd. Praha: SNTL - Nakladatelství technické literatury, [10] NEKVINDA, Milislav, Jiří ŠRUBAŘ a Jaroslav VILD. Úvod do numerické matematiky. 1 vyd. Praha: SNTL, [11] REKTORYS, Karel. Přehled užité matematiky. 7. vyd. Praha: Prometheus, 2000, xxxii, 874 s. ISBN [12] HORÁK, Zdeněk a František KRUPKA. Fyzika: Příručka pro vysoké školy technického směru. 2. přep. vyd. Praha: SNTL - Nakladatelství technické literatury, n.p.,

37 Přílohy Algoritmy v C++ Algoritmy LU, QR a Choleského rozkladu v jazyce C++. Vzhledem k pouze ilustrativnimu charakteru je i QR rozklad psán pouze pro čtvercové matice. Funkce mattim, resp. trans jsou algoritmy maticového násobení, resp. transpozice pro čtvercové matice (navrženy pouze pro účely odladění algoritmů rozkladů). Vstupem do funkcí jsou jednorozměrná pole v nichž jsou matice uloženy po řádcích. Funkce jsou obsahem souboru rozklady.h, který je na přiloženém CD. Obsah souboru: #ifndef ROZKLADY_H #define ROZKLADY_H #include "iostream" #include "stdio.h" #include "math.h" using namespace std; #endif // ROZKLADY_H // vsechny funkce jsou psany pro ctvercove matice // matice je jednorozmerne pole v nemz jsou matice ulozeny po radcich void LU(float *A, int ha, float *L, float *U){ // A - matice pro niz chceme provest rozklad // ha - rad matice A // L - matice do niz se ulozi dolni trojuhlenikova // U - matice do niz se ulozi horni trojuhlenikova int rada = ha; for(int i = 0; i < rada; i++){ L[i*radA + i] = 1; for(int j = 0; j < rada ; j++){ for(int i = 0; i <= j; i++){ cout << i << endl; float souc = 0; for(int k = 0; k <= i-1; k++){ souc += L[i*radA + k]*u[k*rada + j]; U[i*radA + j] = A[i*radA + j] - souc; for(int m = j+1; m <= rada; m++){ float souc = 0; for(int k = 0; k <= j-1; k++){ 33

38 souc += L[m*radA + k]*u[k*rada + j]; L[m*radA + j] = (1.0/U[j*radA + j])*(a[m*rada + j] - souc); void QR(float *A, int ha, float *Q, float *R, float *z){ // A - matice pro niz chceme provest rozklad // ha - rad matice A // Q - matice do niz se ulozi ortogonalni // R - matice do niz se ulozi horni trojuhlenikova // z - pomocny vektor int rada = ha; float souc = 0; for(int i = 0; i < rada; i++){ souc += A[i*radA]*A[i*radA]; R[0] = sqrt(souc); for(int i = 0; i < rada; i++){ Q[i*radA] = A[i*radA]/R[0]; for(int k = 1; k < rada; k++){ for(int i = 0; i <= k-1; i++){ float souc = 0; for(int j = 0; j < rada; j++){ souc += Q[j*radA + i]*a[j*rada + k]; R[i*radA + k] = souc; for(int j = 0; j < rada; j++){ float souc = 0; for(int i = 0; i <= k-1; i++){ souc += Q[j*radA + i]*r[i*rada + k]; z[j] = A[j*radA + k] - souc; float souc = 0; for(int i = 0; i < rada; i++){ souc += z[i]*z[i]; R[k*radA + k] = sqrt(souc); for(int i = 0; i < rada; i++){ Q[i*radA + k] = z[i]/r[k*rada + k]; 34

39 void RTR(float *A, int ha, float *RR){ // A - matice pro niž chceme provest rozklad // ha - rad matice A // RR - matice do niz se ulozi horni trojuhlenikova int rada = ha; RR[0] = sqrt(a[0]); for(int i = 1; i < rada; i++){ for(int j = 0; j <= i-1; j++){ float souc = 0; for(int k = 0; k <= j-1; k++){ souc += RR[i*radA + k]*rr[j*rada + k]; RR[i*radA + j] = 1/RR[j*radA + j]*(a[i*rada + j] - souc); float souc = 0; for(int j = 0; j <= i-1; j++){ souc += RR[i*radA + j]*rr[i*rada + j]; RR[i*radA + i] = sqrt(a[i*rada + i] - souc); void mattim(float *A, int ha, float *B, float *C){ // A - prvni matice v soucinu // ha - rad matice A // B - druha matice v soucinu // C - vysledna matice soucinu int rada = ha; for(int i = 0; i < rada; i++){ for(int j = 0; j < rada; j++){ float souc = 0; for(int k = 0; k < rada; k++){ souc += A[i*radA + k]*b[k*rada + j]; C[i*radA + j] = souc; void trans(float *A, int ha, float *At){ // A - matice jiz chceme transponovat // ha - rad matice A // At - transponovana matice int rada = ha; 35

40 for(int i = 0; i< rada; i++){ for(int j = i; j < rada; j++){ At[j*radA + i] = A[i*radA + j]; At[i*radA + j] = A[j*radA + i]; Výsledné grafy Následující grafy vygenerované v software Matlab ilustrují časový vývoj odchylek skutečné a vyfiltrované polohy projektilu v jednotlivých osách. 36

41 37