ˇ ´ VYSOKE ´ UCEN ˇ ´I TECHNICKE ´ CESK E FAKULTA STAVEBN´I ´ OBOR GEODEZIE, KARTOGRAFIE A GEOINFORMATIKA
´ RSK ˇ ´ PRACE ´ BAKALA A ´ ROZKLADY PRO KALMAN˚ MATICOVE UV FILTR
Vedouc´ı pr´ ace: doc. RNDr. Milada Koˇ candrlov´ a, CSc.
Katedra matematiky
Kvˇ eten 2014
Pavel Kulmon
cesxÉ VYsoxÉ uČENÍrecHNlcxÉ v PRAzE Fakulta stavební
Thákur ova'l , 166 29 Praha 6
ZADANI BAKALARSKE PRACE studijní program:
Geod ér!" .* kprtg grafi
studijní obor:
G+K
akademic$ rok:
2013-14
9.
Jméno a prumení sfudenta:
P*rpl Kulmgg
Zadávající katedra:
katedra matematiky
Vedoucí bakalá ské práce:
Doc. RNDr. Milada Kočandrlová, CSc.
Název baka|áské práce Název bakaláské práce v anglickém jaryce
Maticové rozklady pro Kalman v filtr
"?'_N-p"pt,
*lgp-til*'y-Q&
:
Matrix decompositions in Kalman Filter
!U
3 Qholegkéhg1"o-7k|pdu'''1t"p_ppgr?_m--o.l_*-v*jaryce C{-*-,
Datum zadání bakalá ské práce
:
24.2.2014
Termín odevzdání:
16.5.2014 (vypl te poslední den v uky p íslušnéhosemestru)
Pokud student neodevzdal bakalá skou práci v určenémtermínu' tuto skuteěnost p edem písemně zdrivodnil a omluva byla děkanem uznána, stanoví děkan studentovi náhradní termín odevzdáni bakalá ské práce. Pokud se však student ádně neomluvil nebo omluva nebyla děkanem unÉna, mttže si student zapsat bakalrá skou práci podruhé. Studentovi, ktery p i opakovaném zápisu bakalrí skou práci neodevzda| v urěeném termínu a tuto skutečnost ádně neomluvil nebo omluva nebyla děkanem uznána, se ukončuje studium podle $ 56 zákona o vŠe. 111/1998. 1szŘ Čvur ě1.2t, odst. 4) Student bere na vědomí, že je povinen vypracovat s vyjimkou poskytnuQch konzultací. Seznam použité
t eba uvést v bakalá
ské práci.
vedoucí bakalá
sk
é práce
Zadání bakalá ské práce pŤevzal dne:
,{ ť, r{ . ,tŮ tťť
hotovit ve 3 iscích _ lx katedra, lx student' lx studii ní odd. (zašle katedra Nejpozději do konce 2. tydne vyuky v semestru odešle katedra l kopii zadáni BP na studijní oddělení
Formulá nutno
a provede zápis dajťr f kajícíchse BP do databráze KoS. BP zadávákatedra nejpozději 1. dden semestru, v němž má student BP zapsanou. (Směrnice děkana pro realizaci studijních programri a SZZ na FSv
Čvur et. 5, odst.
7)
Abstrakt V pr´aci jsou uvedeny maticov´e rozklady s vyˇsetˇren´ım tvaru matic a podrobn´e realizace jejich v´ ypoˇctu. D´ale jsou uvedeny z´akladn´ı algoritmy Kalmanova filtru spoleˇcnˇe s rozˇs´ıˇrenou a unscentovanou modifikac´ı. Kalman˚ uv filtr je ilustrov´an na dvou konkr´etn´ıch pˇr´ıpadech.
Kl´ıˇ cov´ a slova Maticov´e rozklady, QR, LU, Cholesk´eho rozklad, SVD, Kalman˚ uv 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
Podˇ ekov´ an´ı
R´ad bych na tomto m´ıstˇe podˇekoval sv´e vedouc´ı bakal´aˇrsk´e pr´ace doc. RNDr. Miladˇe Koˇcandrlov´e, CSc. za jej´ı odborn´e veden´ı, vˇecn´e rady a uˇziteˇcn´e pˇripom´ınky. D´ale bych r´ad podˇekoval sv´e rodinˇe za podporu pˇri studij´ıch.
ˇ Cestn´ e prohl´ aˇ sen´ı
Prohlaˇsuji, ˇze tuto bakal´aˇrskou pr´aci Maticov´e rozklady pro Kalman˚ uv filtr jsem vypracoval a sepsal zcela samostatnˇe. Veˇsker´a pouˇzit´a literatura a materi´aly jsou uvedeny v seznamu zdroj˚ u.
V Praze dne (podpis autora)
Obsah ´ Uvod
2
1 Line´ arn´ı algebra 1.1 Z´ akladn´ı pojmy . . . . . . . . . . . . . . . . 1.1.1 Matice a maticov´e operace . . . . . 1.2 LU rozklad . . . . . . . . . . . . . . . . . . 1.2.1 Definice LU rozkladu . . . . . . . . . 1.2.2 Uˇzit´ı LU rozkladu . . . . . . . . . . 1.3 Cholesk´eho rozklad . . . . . . . . . . . . . . 1.3.1 Definice Cholesk´eho rozkladu . . . . 1.3.2 Prvky matice R . . . . . . . . . . . . 1.3.3 Vyuˇzit´ı Choleskeho rozkladu . . . . 1.4 QR rozklad . . . . . . . . . . . . . . . . . . 1.4.1 Gram-Schmidtova ortogonalizace . . 1.4.2 Givensovy rotace . . . . . . . . . . . 1.4.3 QR rozklad . . . . . . . . . . . . . . 1.4.4 Gram-Schmidt˚ uv QR rozklad . . . . 1.4.5 Givens˚ uv QR rozklad . . . . . . . . 1.5 Rozklad pomoc´ı singul´ arn´ıch hodnot - SVD 1.5.1 Line´ arn´ı transfromace . . . . . . . . 1.5.2 Diagonalizace matice . . . . . . . . . 1.5.3 Rozklad pomoc´ı singul´ arn´ıch hodnot
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
3 3 3 4 4 5 5 5 6 6 6 6 7 8 8 8 9 9 10 10
2 Kalman˚ uv filtr ´ 2.1 Uvod . . . . . . . . . . . . . . . . . . ´ 2.1.1 Uvod . . . . . . . . . . . . . . 2.1.2 Statistick´e pojmy . . . . . . . 2.2 Kalman˚ uv filtr . . . . . . . . . . . . 2.2.1 Proces a ˇsumy . . . . . . . . 2.2.2 Predikce a update . . . . . . 2.2.3 Rozˇs´ıˇren´ y Kalman˚ uv filtr . . 2.2.4 Pˇr´ıklad k ilustraci EKF . . . 2.2.5 Unscentovan´ y Kalman˚ uv filtr 2.2.6 Zpˇetn´e vyhlazen´ı . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
12 12 12 12 13 13 16 17 19 21 23
3 Aplikace rozklad˚ u 3.1 Vlastn´ı v´ ypoˇcty . . . . . . . . . . . . ´ 3.1.1 Uvod . . . . . . . . . . . . . . 3.1.2 Norma matice . . . . . . . . . 3.1.3 Zkoum´ an´ı numerick´e stability 3.1.4 Druh´ y pˇr´ıklad aplikace EKF
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . maticov´eho rozkladu . . . . . . . . . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
25 25 25 25 27 29
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
Z´ avˇ er
31
Literatura
32
Pˇ r´ılohy
33
1
´ Uvod Pˇredloˇzen´a pr´ace, jak jej´ı n´azev a zad´an´ı napov´ıd´a, je vˇenov´ana dekompozici matic pro vyuˇzit´ı v algoritmech Kalmanovy filtrace n´ahodn´e veliˇciny. Pr´ace m´a tˇri kapitoly a pˇr´ılohy. Na zaˇc´atku prvn´ı kapitoly jsou shrnuty nˇekter´e z´akladn´ı pojmy maticov´eho poˇctu, kter´e jsou pouˇz´ıv´any v n´asleduj´ıc´ıch algoritmech.Tˇeˇziˇstˇe t´eto kapitoly je v podrobn´em pˇrehledu pˇeti algoritm˚ u pro rozklad matice. Od nejjednoduˇsˇs´ıho LU rozkladu a jeho speci´aln´ıho pˇr´ıpadu Cholesk´eho rozkladu, pˇres dva QR rozklady (Gram-Schmidt˚ uv a Gives˚ uv rozklad) po rozklad pomoc´ı singul´arn´ıch hodnot. Vu ´vodu druh´e kapitoly jsou uvedeny definice norm´aln´ıho rozdˇelen´ı n´ahodn´e veliˇciny. Hlavn´ı pozornost je zde vˇenov´ana odvozen´ı algoritmu klasick´eho, rozˇs´ıˇren´eho a uncentovan´eho Kalmanova filtru n´ahodn´e veliˇciny vˇcetnˇe zm´ınky o zpˇetn´em vyhlazen´ı. Tˇret´ı kapitola je vˇenovan´a aplikaci rozklad˚ u. V u ´vodu kapitoly jsou shrnuty normy matic pro posouzen´ı stability zvolen´e ˇctveˇrice matic. Kapitolu uzav´ır´a aplikace filtru na mˇeˇren´ı dr´ahy projektilu. V pˇr´ıloze jsou pops´any implementace dekompozic matic v jazyce C++ pro ˇctvercov´e matice a jsou zde pˇriloˇzeny v´ ysledn´e grafick´e v´ ystupy ze zpracov´an´ı pˇr´ıkladu ze tˇret´ı kapitoly.
2
Kapitola 1
Line´ arn´ı algebra 1.1
Z´ akladn´ı pojmy
V n´asleduj´ıc´ım odstavci uvedeme nˇekter´e definice a operace maticov´eho poˇctu, kter´e budeme d´ale pouˇz´ıvat. 1.1.1
Matice a maticov´ e operace
Matice Matic´ı A = (aij ) typu m × n naz´ yv´ame sch´ema mn re´aln´ ych nebo komplexn´ıch ˇc´ısel sestaven´ ych v m ˇra´dc´ıch a n sloupc´ıch. V t´eto pr´aci se omez´ıme pouze na re´aln´e matice. Matice A typu m × m se naz´ yv´a ˇctvercov´a matice ˇra´du m. Matice jednotkov´ a Matice jednotkov´a je ˇctvercov´a matice E = (eij ) ˇra´du m, pro kterou plat´ı eij = δij , kde δij je Kroneckerovo delta. Hodnost matice Hodnost matice A typu m×n je ˇc´ıslo h(A) rovn´e maxim´aln´ımu poˇctu line´arnˇe nez´avisl´ ych ˇra´dk˚ u ˇci sloupc˚ u matice (vˇzdy menˇs´ı, nebo rovn´e min(m, n)). Regul´ arn´ı matice Matice A ˇra´du m je regul´arn´ı, plat´ı-li h(A) = m. Neplat´ı-li uveden´a rovnost, ˇr´ık´ame ˇze matice A je singul´arn´ı. Matice transponovan´ a T Matice A = (aji ) typu n × m je matice transponovan´a k matici A = (aij ) typu m × n a vzn´ık´a v´ ymˇenou ˇra´dk˚ u a sloupc˚ u. Souˇ cin matic Necht’ A = (aij ) je matice typu m × n a B = (bij ) matice typu n × p. Potom matice C = (cij ) je typu m×p, kde cij =
n P
aik bkj , i = 1, 2, · · · , m a j = 1, 2, · · · , p je souˇcinem
k=1
matic A, B a znaˇc´ıme C = AB. Souˇcin matic nen´ı obecnˇe komutativn´ı, avˇsak plat´ı pro nˇej asociativn´ı z´akon, tedy plat´ı ABC = (AB)C = (AB)C. Souˇcin koneˇcn´eho poˇctu matic m´a smysl, je-li pro kaˇzd´e dvˇe sousedn´ı matice definov´an souˇcin.
3
Matice inverzn´ı Matice A−1 je matice inverzn´ı k regul´arn´ı matici A, pro kterou plat´ı rovnost A−1 A = AA−1 = E. Transpozice souˇ cinu matic Matice transponovan´a k matici C = AB je matice CT = BT AT . Inverze souˇ cinu matic Matice inverzn´ı k C = AB je matice C−1 = B−1 A−1 . Ortogon´ aln´ı matice Matice A = ˇra´du m je ortogon´aln´ı matic´ı, plat´ı-li AAT = E, tj. AT = A−1 . D´ale pro ni plat´ı det(A) = 1 nebo det(A) = −1. Matice symetrick´ a a antisymetrick´ a Matice A = (aij ) ˇra´du m je symetrick´a, resp. antisymetrick´a, je-li A = AT , resp. A = −AT . Pozitivnˇ e definitn´ı matice - Sylvestrovo krit´ erium Symetrick´a matice A ˇra´du m je pozitivnˇe definitn´ı, jsou-li jej´ı vˇsechny hlavn´ı minory kladn´e. Matice je negativnˇe definitn´ı, jestliˇze v posloupnosti hlavn´ıch minor˚ u se stˇr´ıd´a znam´enko od prvn´ıho minoru z´aporn´eho. Pozn´ amka: Budiˇz A regul´arn´ı matice, potom B = AAT je pozitivnˇe definitn´ı. Takt´eˇz opaˇcnˇe B = AT A je pozitivnˇe definitn´ı. Z tˇechto definic vypl´ yv´a existence Cholesk´eho rozkladu. Vlastn´ı ˇ c´ısla matice Vlastn´ı ˇc´ısla λ1 , λ2 , · · · , λm ˇctvercov´e matice A jsou koˇreny charakteristick´e rovnice det(A − λE) = 0. Singul´ arn´ı ˇ c´ısla matice Singul´arn´ı ˇc´ısla σ1 , σ2 , · · · , σn matice A typu m × n jsou odmocniny vlastn´ıch ˇc´ısel matice B = AAT , kter´a jsou stejn´a jako vlastn´ı ˇc´ısla matice C = AT A.
1.2 1.2.1
LU rozklad Definice LU rozkladu
Budiˇz regul´arn´ı matice A ˇra´du m, splˇ nuj´ıc´ı Sylvestrovo krit´erium. Pak existuje jedin´a matice L = (lij ) ˇra´du m pro kterou plat´ı lii = 1 a z´aroveˇ n lij = 0 kdykoli j > i. K t´eto matici existuje jedin´a matice U = (uij ), pro kterou plat´ı uij = 0 kdykoli i > j. ˇ ık´ame, ˇze matice L je doln´ı troj´ R´ uheln´ıkov´a s jedniˇckami na diagon´ale a U je horn´ı troj´ uheln´ıkov´a matice. Pro uveden´e matice plat´ı A = LU. T´eto dekompozici ˇr´ık´ame LU rozklad.
Z uveden´e definice m˚ uˇzeme pro matici A libovoln´eho ˇr´adu vytvoˇrit jej´ı podobu LU
4
rozkladu. Napˇr´ıklad pro matici ˇr´adu 3 × 3 plat´ı
u11 u12 u13 a11 a12 a13 l21 u12 + u22 l21 u13 + u23 . a21 a22 a23 = l21 u11 l31 u11 l31 u12 + l32 u22 l31 u13 + l32 u23 + u33 a31 a32 a33 Analogicky lze sestavit prvky matice libovoln´eho ˇra´du n. Je tedy patrn´e, ˇze pro prvky matice L, resp. U plat´ı n´asleduj´ıc´ı vztahy
j−1 X 1 lij = aij − lik ukj ; i = j + 1, j + 2, . . . , n ujj k=1
uij = aij −
i−1 X
lik ukj ; j = 1, 2, . . . , n; i = 1, 2, . . . , j.
(1.1)
(1.2)
k=1
1.2.2
Uˇ zit´ı LU rozkladu
M´ame-li matice L a U, lze jich vyuˇz´ıt napˇr´ıklad pro ˇreˇsen´ı soustavy rovnic. Je-li Ax = b soustava rovnic, v n´ıˇz A splˇ nuje podm´ınky LU rozkladu, m˚ uˇzeme jej´ı ˇreˇsen´ı pˇrev´est na ˇreˇsen´ı dvou soustav Ly = b a n´aslednˇe Ux = y, jejichˇz ˇreˇsen´ı je usnadnˇeno t´ım, ˇze L a U jsou v troj´ uheln´ıkov´em tvaru. Pro v´ ypoˇcet matic potˇrebujeme n3 operac´ı.
Podle odstavce 1.2.1. pro LU rozklad je nutn´e, aby matice A splˇ novala Sylvestrovo krit´erium. Toto ovˇsem nesplˇ nuje obecnˇe kaˇzd´a regul´arn´ı matice. Je-li A regul´arn´ı matice ˇra´du m, nesplˇ nuj´ıc´ı Sylvestrovo krit´erium, provedeme transformaci matice PA, kde P je premutaˇcn´ı matice takov´a, ˇze PA = B. Potom matice B je jiˇz regul´arn´ı ˇra´du m splˇ nuj´ıci Sylvestrovo krit´erium. Tuto rovnost vyuˇz´ıv´a vˇetˇsina implementac´ı LU rozkladu (napˇr. software Matlab nebo knihovna LAPACK).
LU rozklad nen´ı obecnˇe unik´atn´ı. V odstavci 1.2.1 je uveden unik´atn´ı rozklad za pˇredpokladu, ˇze L m´a na diagon´ale hodnoty 1. Tvar matice L lze volit, a proto mohou m´ıt rozklady r˚ uzn´ y tvar (viz. n´asleduj´ıc´ı Cholesk´eho rozklad).
1.3 1.3.1
Cholesk´ eho rozklad Definice Cholesk´ eho rozkladu
Budiˇz matice A symetrick´a a pozitivnˇe definitn´ı. Potom existuje unik´atn´ı regul´arn´ı horn´ı troj´ uheln´ıkov´a matice R = (rij ), pro kterou plat´ı A = RT R. Matici R ˇr´ık´ame Cholesk´eho faktor. Cholesk´eho rozklad je speci´aln´ım pˇr´ıpadem LU rozkladu, kde vol´ıme R = U = LT .
5
1.3.2
Prvky matice R
Jak jiˇz bylo ˇreˇceno, Cholesk´eho rozklad je speci´aln´ım pˇr´ıpadem LU rozkladu a rovnice (1.1) tedy pˇrejde na tvar 2 r11 r12 r11 r13 a11 a12 a13 r11 2 2 r12 r13 + r22 r23 + r22 r12 . a21 a22 a23 = r11 r12 2 2 2 r11 r13 r12 r13 + r22 r23 r13 + r23 + r33 a31 a32 a33
I zde jsou matice ˇr´adu n sestaviteln´e analogicky. Pro prvky matice R m˚ uˇzeme napsat n´asleduj´ıc´ı vztahy √ r11 = a11 (1.3)
rii =
v u u ta
i−1 X
ii −
2 rir ; i = 2, 3, . . . , n
(1.4)
r=1
j−1 X 1 aij − rir rjr ; i = 2, 3, . . . , n; j = 1, 2, . . . , i − 1. rij = rjj r=1
(1.5)
Vzhledem k numerick´e stabilitˇe v´ ypoˇcetn´ıch proces˚ u s pouˇzit´ım PC a odmocnin´am v algoritmu m˚ uˇze nastat situace, kdy prvky na hlavn´ı diagon´ale vyjdou obecnˇe komplexn´ı [9]. 1.3.3
Vyuˇ zit´ı Cholesk´ eho rozkladu
Cholesk´eho rozklad lze vyuˇz´ıt obdobnˇe jako LU rozklad (viz. odstavec 1.2.2). V´ yhodou Cholesk´eho algoritmu je menˇs´ı v´ ypoˇcetn´ı n´aroˇcnost, nebot’ poˇc´ıt´a pouze jednu troj´ uheln´ıkovou matici. Vzhledem k tomu, ˇze jej´ı diagon´ala nemus´ı obsahovat pouze jedniˇcky, je v´ yhodnˇejˇs´ı rozklad s diagon´aln´ı matic´ı D a v matici R s jedniˇckami na diagon´ale, tedy A = RT DR.
1.4 1.4.1
QR rozklad Gram-Schmidtova ortogonalizace
Budiˇz b´aze vektorov´eho prostoru V posloupnost vektor˚ u b1 , b2 , . . . , bn . Mnoˇzinu vˇsech line´arn´ıch kombinac´ı dan´ ych vektor˚ u oznaˇcme hb1 , b2 , . . . , bn i. Takov´e mnoˇzinˇe ˇr´ık´ame line´arn´ı obal b´azov´ ych vektor˚ u (tedy pr´avˇe vektorov´ y prostor). Ke kaˇzd´e takov´e b´azi lze sestrojit b´azi jinou, kterou budiˇz posloupnost vektor˚ u e1 , e2 , . . . , en . Pro tuto b´azi plat´ı, ˇze vektory jsou ortogon´aln´ı a hb1 , b2 , . . . , bn i = he1 , e2 , . . . , en i. Procesu vytvoˇren´ı takov´e b´aze ˇr´ık´ame Gram-Schmidtova ortogonalizaˇcn´ı metoda [1]. Jsou-li vˇsechny vektory ei jednotkov´e, naz´ yv´ame tuto b´azi ortonorm´aln´ı b´az´ı.
6
1.4.2
Givensovy rotace
Matici
G(i, j, θ) =
1 ...
c ··· ··· .. . 1 .. . 1 −s · · · · · ·
s .. . .. . c ..
. 1
(1.6)
kde i < j, naz´ yv´ame Givensova matice rotace. Tato matice se od jednotkov´e matice liˇs´ı pouze o nediagon´aln´ı a diagon´aln´ı prvky s = sin (θ) c = cos (θ). Givensova matice rotace popisuje rotaci prostoru v rovinˇe dan´e osami i, j o u ´hel θ. Matice je ortogon´aln´ı. N´asoben´ı AG(i, j, θ) mˇen´ı i, j-t´ y ˇra´dek matice A a n´asoben´ı G(i, j, θ)A mˇen´ı i, j-t´ y sloupec matice A.
U Givensov´ ych matic rotace m˚ uˇzeme s v´ yhodou vyuˇz´ıt moˇznosti volby u ´hlu θ. Ten zvol´ıme tak, abychom dos´ahli vynulov´an´ı vˇsech prvk˚ u matice A nach´azej´ıc´ıch se pod hlavn´ı diagon´alou. Vyjdeme-li z definice Givensov´ ych rotac´ı, m˚ uˇzeme uvaˇzovat vektor x = (xi ) ∈ Rn , a pak podle [2] plat´ı y = G(i, j, θ)T x
cxi − sxj , k = i yk = sxi + cxj , k = j x , k 6= i, j k
(1.7)
(1.8)
a z podm´ınky yj = 0 vypl´ yv´a sxi + cxj = 0 xi c= q 2 xi + x2j −xj s= q . x2i + x2j
(1.9) (1.10) (1.11)
Dˇelen´ı v rovnostech (1.10) a (1.11) je normov´an´ı, kter´e je d˚ ulˇeˇzit´e pro to, aby hodnoty odpov´ıdaly uvaˇzovan´ ym hodnot´am goniometrick´ ych funkc´ı.
7
1.4.3
QR rozklad
Budiˇz A matice typu m × n, maj´ıc´ı line´arnˇe nez´avisl´e sloupce, pro kterou plat´ı m ≥ n. Potom existuj´ı matice Q ˇra´du m a horn´ı troj´ uheln´ıkov´a matice R typu m × n, pro kter´e plat´ı A = QR. Matici Q vol´ıme ortogon´aln´ı nebo alespoˇ n s ortogon´aln´ımi sloupci.
Existuje mnoho zp˚ usob˚ u jak realizovat QR rozklad. Uvedeme zde dva. Prvn´ım je algoritmus vyuˇz´ıvaj´ıc´ı Gram-Schmidtovy ortogonalizace, druh´ y vyuˇz´ıv´a Givensovy rotace. 1.4.4
Gram-Schmidt˚ uv QR rozklad
V pˇr´ıpadˇe, ˇze pouˇzijeme algoritmus vyuˇz´ıvaj´ıc´ı Gram-Schmidtovy ortogonalizace, matice Q bude m´ıt jen ortonorm´aln´ı sloupce. Matice bude vznikat postupn´ ym vytv´aˇren´ım ortonorm´aln´ı b´aze k b´azi sloupcov´ ych vektor˚ u matice A. Matice R je n´aslednˇe d´ana jednoznaˇcnˇe maticov´ ym n´asoben´ım v poˇzadovan´e rovnosti A = QR. Vzhledem k tomu, ˇze matice R je horn´ı troj´ uheln´ıkov´a, plat´ı pro jej´ı prvn´ı prvek a pro prvn´ı sloupce matice Q rovnice r11 =
v um uX t a2
k1
(1.12)
k=1
qi1 =
ai1 ; i = 1, 2, · · · , m. r11
(1.13)
Zbyl´e prvky matic se vypoˇctou rik =
m X
rij ajk
(1.14)
j=1
zj = aj,k −
k−1 X
qji rik
(1.15)
i=1
rkk =k z k
(1.16)
1 z rk k
(1.17)
qjk =
kde i = 1, 2, · · · , k − 1, j = 1, 2, · · · , m, k = 2, 3, · · · , n . 1.4.5
Givens˚ uv QR rozklad
Oznaˇc´ıme-li Givensovy matice rotace nez´avisle na argumentu jako Gj , kde index m´a v´ yznam poˇrad´ı rotace, m˚ uˇzeme na z´akladˇe jejich ortogonality ps´at vztah QT A = R, kde QT = GTn GTn−1 ...GT2 GT1 . 8
(1.18)
Je zjevn´e, ˇze Givensovy rotace samy o sobˇe zaruˇcuj´ı existenci QR rozkladu. V aplikaci Givensov´ ych rotac´ı pro QR rozklad nen´ı potˇreba poˇc´ıtat u ´hel θ, postaˇc´ı zvolit dva prvky, z nichˇz druh´ y chceme vynulovat. Pro v´ ypoˇcet nediagon´aln´ıch prvk˚ u matic rotace pouˇzijeme podle [2] n´asleduj´ıc´ı vzorce pro parametr τ yk =
|b| > |a|, τ = − ab , s =
√ 1 , 1+τ 2
c = sτ
|b| ≤ |a|, τ = − ab , c =
√ 1 , 1+τ 2
s = cτ
(1.19)
kde a, b jsou vybran´e prvky matice A a prvek b je urˇcen k vynulov´an´ı. Pakliˇze je prvek b jiˇz nulov´ y, plat´ı c = 1 a s = 0. Budiˇz matice D = (dij ) typu m × n, pro kterou chceme z´ıskat QR rozklad. V´ ypoˇcet prov´ad´ıme podle n´asleduj´ıc´ıch rovnic a = di−1 j b = dij ri−1 k = cdi−1 k − sdik rik = sdi−1 k + cdik di−1 k = ri−1 k dik = rik ,
(1.20)
kde j = 1, 2, · · · , n, i = m, m − 1, · · · , j + i a k = j, j + 1, · · · , n. V nˇekter´ ych aplikac´ıch je vhodn´e vyˇc´ıslit i matici Q. Budiˇz G = (gij ) typu m × m nˇejak´a matice rotace. Plat´ı gi−1 i−1 = gii = c gi−1 i = −gi i−1 = s a transpozic´ı vztahu (1.18) z´ısk´ame Q = G1 G2 ...Gn−1 Gn .
1.5 1.5.1
(1.21)
Rozklad pomoc´ı singul´ arn´ıch hodnot - SVD Line´ arn´ı transformace
Budiˇz V = hv1 , v2 , ..., vn i b´aze nˇejak´eho vektorov´eho prostoru V a budiˇz hw1 , w2 , ..., wn i b´aze vektorov´eho prostoru W. Pak existuje matice A takov´a, ˇze
w1 w2 .. . wn
=A
T
9
v1 v2 .. . vn
.
(1.22)
Matice AT se naz´ yv´a matice pˇrechodu od b´aze V k b´azi W. Tato matice tedy obsahuje po sloupc´ıch souˇradnice vektor˚ u b´aze W vzhledem k b´azi V. Budiˇz ˇra´dkov´ y vektor xhVi ∈ V a xhWi ∈ W, kde xhWi jsou souˇradnice vektoru xhVi vzhledem k b´azi W. Pak plat´ı T xT hVi = AxhWi .
Matice A tedy prov´ad´ı transfomaci vektoru z b´aze W do b´aze V. Pro obr´acen´ y postup by samozˇrejmˇe platilo −1 T xT hWi = A xhVi .
Mˇejme napˇr´ıklad obyˇcejnou kanonickou b´azi V = h(1, 0, 0), (0, 1, 0), (0, 0, 1)i a d´ale obecnou b´azi B = h(2, 3, 2), (1, 4, 2), (2, 4, 2)i. Matice pˇrechodu tedy bude
2 1 2 A = 3 4 4 . 2 2 2 D´ale mˇejme vektor ahBi = (3, 2, 1), jeˇz m´a souˇradnice vzhledem k b´azi B . Chceme-li jeho souˇradnice vzhledem k b´azi V, vypoˇcteme
10 3 2 1 2 2 3 4 4 = 21 = ahVi . 12 1 2 2 2
1.5.2
Diagonalizace matice
Budiˇz A = (aij ) ˇctvercov´a matice ˇra´du n. Budiˇz matice S = (sij ) ˇr´adu n maj´ıc´ı ve sloupc´ıch n line´arnˇe nez´avisl´ ych vlastn´ıch vektor˚ u matice A. Pak plat´ı S−1 AS = Λ
(1.23)
kde
λ1 · · · 0 . . . . ... Λ = .. 0 · · · λn
(1.24)
je diagon´aln´ı matice s vlastn´ımi ˇc´ısly matice A. 1.5.3
Rozklad pomoc´ı singul´ arn´ıch hodnot
Budiˇz matice A typu m × n, kde m ≤ n. Pouˇzijme tuto matici ve smyslu AV = UM, tedy jako projekci z vektorov´eho protoru V na prostor U, kde matice M je diagon´aln´ı matice mˇeˇr´ıtkov´ ych faktor˚ u. Nyn´ı uvaˇzujme prostor V, resp. U jako ˇr´adkov´ y, resp. sloupcov´ y prostor matice A. Stanov´ıme-li jako podm´ınku, ˇze matice V a U jsou ortogon´aln´ı, m˚ uˇzeme rovnici upravit na tvar A = UMV−1 = UMVT . 10
Nahrad´ıme-li matici M matic´ı Σ, z´ısk´ame A = UΣVT .
(1.25)
Tomuto rozkladu ˇr´ık´ame singul´arn´ı rozklad matice. Matici V z´ısk´ame z rovnosti AT A = VΣUT UΣVT . Vzhledem k ortogon´alnosti matic V a U je
σ12 · · · 0 . .. . VT . AT A = V . .. .. 0 · · · σn2
(1.26)
Matice AT A je ˇctvercov´a, symetrick´a a pozitivnˇe definitn´ı. Odtud vypl´ yv´a, ˇze rovnice T (1.26) je diagonalizac´ı matice A A. Sloupcov´e vektory matice V jsou tedy vlastn´ı vektory matice AT A a matice V je ortogon´aln´ı. Matice Σ2 je diagon´aln´ı a obsahuje vlastn´ı ˇc´ısla matice AT A, coˇz jsou druh´e mocniny singul´arn´ıch ˇc´ısel matice A. Matici U bychom mohli dopoˇc´ıtat z definice nebo lze prov´est podobnou operaci jako pro z´ısk´an´ı matice V, a to AAT = UΣVT VΣUT
(1.27)
a po u ´pravˇe
σ12 · · · 0 . .. . UT . AAT = U . .. .. 2 0 · · · σn
(1.28)
Protoˇze plat´ı, ˇze vlastn´ı ˇc´ısla matice AB jsou stejn´a jako vlastn´ı ˇc´ısla matice BA, je dalˇs´ı v´ ypoˇcet prost´ y. Singul´arn´ı rozklad matice je t´ımto jednoznaˇcnˇe d´an.
11
Kapitola 2
Kalman˚ uv filtr 2.1
´ Uvod
2.1.1
´ Uvod
Kalman˚ uv filtr je tzv. estim´ator (odhad, angl. estimate), ˇreˇs´ıc´ı probl´em odhadu okamˇzit´eho stavu line´arn´ıho dynamick´eho syst´emu naruˇsen´eho tzv. b´ıl´ım ˇsumem s vyuˇzit´ım mˇeˇren´ı na nichˇz stav line´arnˇe z´avis´ı, a kter´a jsou tak´e zat´ıˇzena bil´ ym ˇsumem. V t´eto pr´aci se budeme zab´ yvat pouze jeho diskr´etn´ı verz´ı. Algoritmus je nazv´an po mad’arsk´em elektroinˇzen´ yrovi Rudolfu E. K´alm´anovi (*1930). Vzhledem k tomu, ˇze se na v´ yvoji algoritmu (pˇredevˇs´ım teoretick´e ˇc´asti) pod´ılel tak´e Richard S. Bucy, je nˇekdy naz´ yv´an tak´e Kalman-Bucy filtr. Prvn´ı ˇcl´anek, ve kter´em R.E. K´alm´an pˇredstavil sv˚ uj filtr, byl vyd´an v roce 1960 a je uveden v seznamu literatury viz [8]. 2.1.2
Statistick´ e pojmy
Norm´ aln´ı rozdˇ elen´ı N´ahodn´a veliˇcina X m´a norm´aln´ı rozdˇelen´ı s parametry µ ∈ R a σ > 0, je-li jej´ı hustota pravdˇepodobnosti d´ana pˇrepisem [4] 1 √ −( x−µ )2 f (x) = √ e 2σ , σ 2π
(2.1)
kde x ∈ R. Pˇri µ = 0 a σ = σ 2 = 1 mluv´ıme o tzv. Normovan´en norm´aln´ım rozdˇelen´ı. Norm´aln´ı rozdˇelen´ı znaˇc´ıme N (µ, σ 2 ) a normovan´e speci´alnˇe N (0, 1). Hustota pravdˇepodobnosti normovan´eho norm´aln´ıho rozdˇelen´ı je tedy d´ana pˇredpisem u2 1 f (u) = √ e− 2 , 2π
(2.2)
kde u ∈ R. Kaˇzdou veliˇcinu X maj´ıc´ı norm´aln´ı rozdˇelen´ı N (µ, σ 2 ) lze pˇrev´est na veliˇcinu U maj´ıc´ı normovan´e norm´aln´ı rozdˇelen´ı. Pˇrevod je d´an vztahem U=
X −µ . σ
12
(2.3)
Distribuˇcn´ı funkce normovan´eho norm´aln´ıho rozdˇelen´ı je d´ana vztahem u
1 Z − t2 Φ(u) = √ e 2 dt. 2π −∞
(2.4)
V´ıcerozmˇ ern´ e norm´ aln´ı rozdˇ elen´ı Budiˇz n´ahodn´ y sloupcov´ y vektor x = (x1 , x2 , · · · , xn ) ∈ Rn . Tento vektor m´a n2 rozmˇern´e norm´aln´ı rozdˇelen´ı s parametry µ ∈ Rn a Σ ∈ Rn+ , je-li jeho hustota pravdˇepodobnosti d´ana podle [4] vztahem (x−µ)T Σ−1 (x−µ) 1 2 e− , f (x) = q (2π)n |Σ|
(2.5)
kde µ je vektor stˇredn´ıch hodnot jednotliv´ ych sloˇzek vektoru x a
Σ=
cov(x1 , x1 ) cov(x1 , x2 ) · · · cov(x1 , xn ) .. .. .. . . . .. .. ... . . cov(xn , x1 ) cov(xn , x2 ) · · · cov(xn , xn )
(2.6)
je symetrick´a, pozitivnˇe definitn´ı matice. T´eto matici ˇr´ık´ame kovarianˇcn´ı matice vektoru x. Znaˇc´ıme n-rozmˇern´e norm´aln´ı rozdˇelen´ı Nn (µ, Σ), kde pro n = 1 index nep´ıˇseme.
2.2 2.2.1
Kalman˚ uv filtr Proces a ˇ sumy
Z´akladn´ı pˇredpoklad je existence stavov´eho vektoru x ∈ Rn . Jeho stav v epoˇse k chceme odhadnout. Z´akladn´ı vztah m´a podobu line´arn´ı diferenˇcn´ı rovnice xk = Fk−1 xk−1 + Ck−1 uk−1 + wk−1 ,
(2.7)
kde velmi d˚ uleˇzit´ y je vektor wk−1 ∈ Rr znaˇc´ıc´ı tzv. b´ıl´ y ˇsum syst´emu (procesu), u kter´eho uvaˇzujeme, ˇze m´a rozdˇelen´ı Nr (o, Q). Doln´ı indexy matic a vektor˚ u znaˇc´ı n´aleˇzitost k ˇcasov´e epoˇse. Aby mohlo doch´azet k odhadu, mus´ıme v kaˇzd´e epoˇse do v´ ypoˇctu pˇrispˇet nov´ ymi hodnotami. Mˇejme tedy vektor mˇeˇren´ı z ∈ Rl a druhou diferenˇcn´ı rovnici tvaru zk = Hk xk + vk ,
(2.8)
kde vk ∈ Rl znaˇc´ı tzv. b´ıl´ y ˇsum mˇeˇren´ı, u nˇejˇz uvaˇzujeme, ˇze m´a rozdˇelen´ı Nl (o, R). Tento ˇsum uvaˇzujeme i pˇri klasick´ ych metod´ach odhadu parametr˚ u n´ahodn´eho vektoru ˇ kde uvaˇzujeme, ˇze mˇeˇren´ı je zat´ıˇzeno chybami maj´ıc´ı norm´aln´ı rozdˇelen´ı. (napˇr. MNC), Jak bylo ˇreˇceno, pro ˇsumy plat´ı n´asleduj´ıc´ı Ewi = Evi = 0 Ewi wjT = δij Qi 13
Evi vjT = δij Ri , 2
2
kde Qi ∈ Rr resp. Ri ∈ Rl , jsou kovarianˇcn´ı matice ˇsumu syst´emu, resp. ˇsumu mˇeˇren´ı a δij je Kroneckerovo delta. Kovarianˇcn´ı matice ˇsumu syst´emu a ˇsumu mˇeˇren´ı mohou b´ yt obecnˇe ˇcasovˇe promˇenn´e (zmˇena m˚ uˇze nastat s kaˇzdou novou ˇcasovou epochou), ve vˇetˇsinˇe aplikac´ı je vˇsak uvaˇzujeme jako konstantn´ı. Existuje mnoho forem rovnic KF, pˇredevˇs´ım z d˚ uvodu jejich snadn´e algebraick´e manipulace, motivovan´e pˇredevˇs´ım konkr´etn´ımi aplikacemi. Postihnout je vˇsechny je t´emˇeˇr nemoˇzn´e, proto se v t´eto pr´aci budeme zab´ yvat urˇcit´ ym syst´emem znaˇcen´ı. n2 V rovnici (2.7) matice F ∈ R vztahuje stavov´ y vektor z epochy k − 1 k stavov´emu r2 vektoru v epoˇse k stejnˇe jako matice C ∈ R prov´ad´ı tot´eˇz s vektorem ˇr´ıd´ıc´ıho vstupu u ∈ Rr . V rovnici (2.8) matice H ∈ Rln vztahuje stavov´ y vektor v epoˇse k na vektor mˇeˇren´ı v t´eˇze epoˇse. Jednou z nejzaj´ımavˇejˇs´ıch konstrukc´ı v KF je Kalmanova matice zisku (angl. Kalman gain matrix ). Pro jej´ı zaveden´ı uvaˇzujme n´asleduj´ıc´ı rovnost 1 − x ˆ+ ˆk + K2k zk , k = Kk x
(2.9)
kter´a vyjadˇruje myˇslenku, ˇze a posteriorn´ı odhad stavov´eho vektoru v epoˇse k (odhad znaˇcen stˇr´ıˇskou, a posteriorn´ı odhad znaˇcen prav´ ym horn´ım indexem ”-”) je line´arnˇe z´avisl´ y na a priorn´ım odhadu stavov´eho vektoru v epoˇse k a na vektoru mˇeˇren´ı z t´eˇze epochy. Tato konstrukce d´av´a smysl, vyjma ˇr´ıd´ıc´ıho vstupu ˇza´dn´a jin´a data k dispozici nem´ame, a zmiˇ novan´ y vstup je jiˇz obsaˇzen v a priorn´ım odhadu stavov´eho vektoru. Ot´azkou z˚ ust´av´a, jak´ y tvar maj´ı matice K1k , resp. K2k , jejichˇz horn´ı indexy slouˇz´ı pouze pro odliˇsen´ı (nejedn´a se o mocniny). Tyto matice jsou K1k = E − K2k Hk
(2.10)
− T T −1 K2k = P− k Hk (Hk Pk Hk + Rk ) .
(2.11)
Z rovnice (2.12) vypl´ yv´a zaveden´ı n´asleduj´ıc´ıch chyb e− ˆ− xk = x k − xk e+ ˆ+ xk = x k − xk e− z− zk = ˆ k − zk , kde uvaˇzujeme a priorn´ı odhad vektoru mˇeˇren´ı z− k jako z− x− k = Hˆ k.
(2.12)
Pozn´ amka: Nejprve zaved’me kovarianˇcn´ı matice chyb jednotliv´ ych odhad˚ u stavov´eho vektoru T
− − P− k = Eexk exk
T
+ + P+ k = Eexk exk .
14
(2.13) (2.14)
Zkoum´an´ım nekorelovan´ ych veliˇcin vystupuj´ıc´ıch v rovnic´ıch (2.7-8) z´ısk´ame Ewk zTi = 0 Evk zTi = 0 Ewk zTk = 0 T
Ewk x ˆ− k = 0. Tedy ˇsum syst´emu v souˇcasn´e epoˇse k, i < k, nen´ı korelovan´ y s kteroukoli sloˇzkou vektoru mˇeˇren´ı z epoch pˇredch´azej´ıc´ıch a pro ˇsum mˇeˇren´ı plat´ı stejn´ y pˇredpoklad rozˇs´ıˇren´ y i na souˇcasnou epochu i ≤ k. Z podm´ınky ortogonality a vzhledem k tomu, ˇze rovnice (2.9) je pouze line´arn´ı kombinac´ı v maticov´e formˇe, m˚ uˇzeme ps´at n´asleduj´ıc´ı podm´ınku T E(xk − x ˆ+ k )zi = 0
(2.15)
pro i ≤ k. Vektor xk znaˇc´ı skuteˇcnou hodnotu stavov´eho vektoru, tedy vektor kter´ y nejsme schopni zn´at. Dosazen´ım rovnic (2.7),(2.8),(2.9) do (2.15) a uv´aˇzen´ım uveden´ ych pˇredpoklad˚ u (vˇcetnˇe nepˇr´ıtomnosti ˇr´ıd´ıc´ıho vstupu) z´ısk´ame 2 2 T E(Fk−1 xk−1 − K1k x ˆ− k − Kk Hk Fk−1 xk−1 − Kk vk )zi = 0,
kde matici F uvaˇzujeme v nejobecnˇejˇs´ı formˇe, tedy jako ˇcasovˇe promˇennou a vztaˇzenou ke stejn´e epoˇse jako vektor mˇeˇren´ı jeˇz pˇrev´ad´ı a podm´ınka plat´ı pro i < k. Matice line´arn´ıch kombinac´ı z diferenˇcn´ıch rovnic neuvaˇzujeme jako n´ahodn´e promˇenn´e, proto podle pravidel o poˇc´ıt´an´ı se stˇredn´ı hodnotou m˚ uˇzeme ps´at T E((xk − K1k xk − K2k Hk xk ) − K1k (ˆ x− k − xk ))zi = 0.
Protoˇze naˇse poˇzadavky na oˇcek´avan´e hodnoty mus´ı platit pro kteroukoli i-tou epochu pˇredch´azej´ıc´ı moment´aln´ı, m˚ uˇzeme ps´at (E − K1k − K2k Hk )E(xk zTi ) = 0.
(2.16)
ˇ sen´ım rovnice (2.16) je matice z rovnice (2.10). Reˇ Z podm´ınky pro skuteˇcn´ y vektor mˇeˇren´ı je odvozena podm´ınka stejn´eho charakteru pro jeho a priorn´ı odhad T
E(xk − x ˆ+ z− = 0. i k )ˆ
(2.17)
Odeˇcten´ım rovnice (2.15) od rovnice (2.17) z´ısk´ame (po u ´prav´ach) T E(xk − x ˆ+ z− k )(ˆ k − zk ) = 0.
To je velmi d˚ uleˇzit´a rovnice, protoˇze z n´ı je vidˇet, ˇze hled´ame stˇredn´ı hodnotu pro souˇcin dvou vektor˚ u chyb, a priorn´ıch chyb odhadu vektoru mˇeˇren´ı a a posteriorn´ıch chyb odhadu stavov´eho vektoru syst´emu. N´asledn´ ymi substitucemi a uv´aˇzen´ım pˇredeˇsl´ ych korelac´ı pˇrejdeme na tvar 2 T E(xk − K1k x ˆ− ˆ− k − Kk · zk )(Hk x k − zk ) = 0.
15
Tato rovnice s uv´aˇzen´ım zaveden´ ych chyb a proveden´ım transpozice z´ısk´a tvar T
2 − 2 − T T E(−e− xk + Kk Hk exk − Kk vk )(exk Hk − vk ) = 0 T
− T 2 T E((K2k Hk − E)e− xk exk Hk + Kk vk vk ) = 0 T 2 [E − K2k Hk ]P− k Hk − Kk Rk = 0.
(2.18)
ˇ sen´ım rovnice (2.18) je matice z rovnice (2.11). Reˇ
2.2.2
Predikce a update
Jak jiˇz bylo ˇreˇceno, KF je nejen speci´aln´ım pˇr´ıpadem sekvenˇcn´ıho vyrovn´an´ı, ale je tak´e filtrem, kter´ y se zakl´ad´a na pˇr´ınosu ke zmˇenˇe stavu syst´emu, zjiˇstˇen´em z nesrovnalosti mezi oˇcek´avan´ ym stavem a stavem vypl´ yvaj´ıc´ım z mˇeˇren´ı (viz. ziskov´a matice K2k ). Jinak ˇreˇceno, z´asadn´ı ideou, kter´a se v KF uplatˇ nuje je tzv. update (ˇces. aktualizace). S kaˇzdou novou sadou mˇeˇren´ı zi z´ısk´av´ame nov´e informace a n´aˇs odhad (ve smyslu nejmenˇs´ıch ˇctverc˚ u) se zlepˇsuje. Z rovnic (2.12) ,(2.13) a (2.14) z´ısk´ame pro update, tedy v´ ypoˇcet a posteriorn´ıho odhadu stavov´eho vektoru x ˆ+ k , rovnici 2 2 x ˆ+ x− k = (E − Kk Hk )ˆ k + Kk zk 2 x ˆ+ ˆ− ˆ− k = x k + Kk (zk − Hk x k ),
(2.19)
kter´a vyjadˇruje smysl Kalmanovy ziskov´e matice. Matici zk − Hk x ˆ− r´ık´ame inovace k ˇ a vyjadˇruje rozd´ıl mezi naˇs´ım pˇredpokladem o chov´an´ı syst´emu a zjiˇstˇen´ım z mˇeˇren´ı. N´asoben´ım tohoto rozd´ılu matici K2k z´ısk´av´ame korekci naˇseho a posteriorn´ıho odhadu stavov´eho vektoru. V´ ypoˇcet a priorn´ıho odhadu vypl´ yv´a z rovnice (2.7) s uv´aˇzen´ım absence ˇr´ıd´ıc´ıho vstupu x ˆ− ˆ+ k = Fk−1 x k−1 ,
(2.20)
kde logicky pro v´ ypoˇcet vyuˇz´ıv´ame nejlepˇs´ı a posteriorn´ı odhad stavov´eho vektoru z pˇredchoz´ı epochy. ´ Uloha odhadu stavov´eho vektoru pˇrirozenˇe obsahuje i u ´kol z´ıskat k tomuto odhadu pˇr´ısluˇsnou kovarianˇcn´ı matici. Zakl´ad´a-li se v´ ypoˇcet a posteriorn´ıho odhadu na nov´em mˇeˇren´ı z epochy k a na a priorn´ım odhadu stavov´eho vektoru, mus´ıme do jeho v´ ypoˇctu zahrnout tak´e kovarianˇcn´ı matice obou tˇechto vektor˚ u. Kovarianˇcn´ı matice vektoru x ˆ− k je d´ana + T P− k = Fk−1 Pk−1 Fk−1 + Qk−1 ,
(2.21)
kde index kovarianˇcn´ı matice ˇsumu procesu odpov´ıd´a indexu z rovnice (2.7). Kovarianˇcn´ı matice vektoru x ˆ+ ana Josephovou rovnic´ı [6] ve tvaru k je d´ − 2 2 2 2 P+ k = (E − Kk Hk )Pk (E − Kk Hk ) + Kk Rk Kk .
Pozn´ amka: Substituc´ı z rovnic (2.7) a (2.20) do rovnice (2.13) z´ısk´ame (2.21). Uv´aˇzen´ım rovnice (2.19) ve tvaru 2 2 2 x ˆ+ ˆ− ˆ− k − xk = x k + Kk Hk xk + Kk vk − Kk Hk x k − xk
16
(2.22)
a jej´ım dosazen´ım do (2.14) z´ısk´ame 2 − 2 2 − 2 T P+ k = E((E − Kk Hk )exk + Kk vk )((E − Kk Hk )exk + Kk vk ) .
Z t´eto rovnice vypl´ yv´a Josephova forma kovarianˇcn´ı matice a posteriorn´ıho odhadu. T N´asob´ıme-li rovnici (2.14) zprava v´ yrazem (Hk P− z´ıme k Hk + Rk ) obdrˇ − T 2 T P− k Hk = Kk (Hk Pk Hk + Rk ).
Rozn´asoben´ım Josephovy rovnice (2.22) a dosazen´ım t´eto rovnosti z´ısk´ame jin´ y tvar a posteriorn´ı kovarianˇcn´ı matice − 2 P+ k = (E − Kk Hk )Pk .
(2.23)
Tento tvar kovarianˇcn´ı matice je v aplikac´ıch v´ıce pouˇz´ıvan´ y vzhledem k jeho kompaktnˇejˇs´ı formˇe.
Pˇredeˇsl´e odstavce d´avaj´ı dohromady podobu v´ ypoˇcetn´ıho procesu z´akladn´ı formy algoritmu KF, kter´a je zdokumentov´ana v n´asleduj´ıc´ı tabulce. Tabulka 1. - rovnice klasick´eho KF f´ aze Predikce
v´ ypoˇ cty x ˆ− = F ˆ+ k−1 x k k−1 + P− k = Fk−1 Pk−1 Fk−1 + Qk−1
Korekce
− T T −1 K2k = P− k Hk (Hk Pk Hk + Rk ) 2 ˆ− x ˆ+ ˆ− k) k = x k + Kk (zk − Hk x − 2 P+ k = (E − Kk Hk )Pk
2.2.3
Rozˇ s´ıˇ ren´ y Kalman˚ uv filtr
Princip rozˇs´ıˇren´eho KF (EKF, angl. extended KF ) je podobn´ y filozofii Taylorovy ˇrady. Uvaˇzujme, ˇze vztahy pro v´ ypoˇcet xk a zk z rovnic (2.7-8) nejsou line´arn´ı, tedy nedaj´ı se zapsat pomoc´ı matic. Pro potˇreby v´ ypoˇctu EKF (stejnˇe jako u zm´ınˇen´e Taylorovy ˇrady) linearizujeme tyto neline´arn´ı vztahy a tuto linearizaci pouˇzijeme pouze v nˇejak´em bl´ızk´em okol´ı n´ami zvolen´eho v´ ychoz´ıho bodu. Vych´azejme ze vztah˚ u xk = f (xk−1 , uk−1 , wk−1 ) zk = h(xk , vk ). Stejnˇe jako v pˇredchoz´ım, ˇsumy procesu ani mˇeˇren´ı nezn´ame, m´ame pouze povˇedom´ı o jejich rozdˇelen´ı. Zaveden´ım matic parci´aln´ıch derivac´ı F, H, W, V m˚ uˇzeme rovnice psat ve tvaru xk = x ˆ− ˆ+ k + F(xk−1 − x k−1 ) + Wwk−1 17
(2.24)
zk = ˆ z− ˆ− k + H(xk − x k ) + Vvk .
(2.25)
Prvky matice parci´aln´ıch derivac´ı jsou definov´any n´asleduj´ıc´ım zp˚ usobem fij =
∂fi (xk−1 , uk−1 , 0) ∂xj
wij =
∂fi (xk−1 , uk−1 , 0) ∂wj
hij =
∂hi (ˆ xk , 0) ∂xj
vij =
∂hi (ˆ xk , 0) , ∂vj
(2.26)
kde wij , resp. vij jsou prvky matic W resp. V a wj , resp. vj jsou sloˇzky vektor˚ u ˇsumu. Analogicky pro jednoduch´ y KF definujeme a priorn´ı odhady stavov´eho vektoru a vektoru mˇeˇren´ı x ˆ− x+ k = f (ˆ k−1 , uk−1 , 0) ˆ z− x− k = h(ˆ k , 0). Nahrad´ıme line´arn´ı kombinace Wwk−1 , resp. Vvk novou promˇennou εk−1 , resp. ηk . Jsou to opˇet n´ahodn´e veliˇciny maj´ıc´ı pˇribliˇznˇe rozdˇelen´ı εk−1 ∼ N (o, WQWT ) ηk ∼ N (o, VRVT ). Vyjdeme-li z definice a priorn´ı chyby odhadu stavov´eho vektoru, dostaneme s ohledem k EKF e− ˆ+ xk = F(xk−1 − x k−1 ) + εk−1 e− ˆ− zk = H(xk − x k ) + ηk . Stejnˇe jako u klasick´eho KF prob´ıhaj´ı dvˇe f´aze odhadu, predikce a korekce. Rovnice EKF jsou shrnuty v tabulce 2. Tabulka 2. - rovnice EKF f´ aze Predikce
x ˆ− k
v´ ypoˇ cty = f (ˆ x+ k−1 , 0)
+ T P− k = Fk−1 Pk−1 Fk−1 + Wk−1 Qk−1 Wk−1
Korekce
T −1 − T T K2k = P− k Hk (Hk Pk Hk + Vk Rk Vk ) 2 x ˆ+ ˆ− x− k = x k + Kk (zk − h(ˆ k , 0)) − 2 P+ k = (E − Kk Hk )Pk
18
Je nutn´e podotknout, ˇze v rovnic´ıch pˇredpokl´ad´ame absenci ˇr´ıd´ıc´ıho vstupu. Funkce f, h je nutn´e zn´at pro v´ ypoˇcet matic parci´aln´ıch derivac´ı. Proto uvaˇzujeme, ˇze ve v´ ypoˇctech m˚ uˇzeme a priorn´ı odhad stavov´eho vektoru a mˇeˇren´ı vypoˇc´ıst z nich, nicm´enˇe lze je nahradit jiˇz vypoˇcten´ ymi maticemi parci´aln´ıch derivac´ı (napˇr. z d˚ uvodu rychlejˇs´ıho v´ ypoˇctu). Samozˇrejmˇe za takov´eho pˇredpokladu kles´a numerick´a stabilita. 2.2.4
Pˇ r´ıklad k ilustraci EKF
Inspirov´ano pˇr´ıkladem ze cviˇcen´ı pˇredmˇetu Teorie chyb a vyrovn´avac´ı poˇcet 2(152TCV2). Mˇejme mˇeˇr´ıc´ı zaˇr´ızen´ı um´ıstˇen´e v bodˇe [0, 0, 0] a d´ale objekt pohybuj´ıc´ı se po ose y. Souˇradnice polohy objektu budou tedy vˇzdy [0, y, 0]. Pˇredpokl´adejme, ˇze objekt se pohybuje pohybem rovnomˇernˇe zrychlen´ ym. Stavov´ y vektor tedy bude
xk xk = vk , ak kde jednotliv´e sloˇzky vektoru jsou definov´any vk = ∂ ak = ∂t
∂xk ∂t
∂xk ∂t
!
=
∂vk , ∂t
kde poloha, rychlost a zrychlen´ı jsou d´any xk = xk−1 + vk−1 ∆t +
ak−1 ∆t2 2
xk = vk−1 + ak−1 ∆t ak = al−1 . Funkce jednotliv´ ych fyzik´aln´ıch veliˇcin by mˇely b´ yt obecnˇe vektorov´e. Vzhledem k tomu, ˇze n´aˇs pˇr´ıpad je jednorozmˇern´ y (pohyb pouze v ose y), postaˇc´ı definice jako skal´arn´ıch veliˇcin. Pomoc´ı definic z odstavce o EKF m˚ uˇzeme ihned ps´at matici F ve tvaru 2
1 ∆t ∆t2 F = 0 1 ∆t . 0 0 1 Jak jiˇz bylo ˇreˇceno, KF je diskr´etnˇe ˇcasov´ y, a tomu jsou pˇrizp˚ usobeny i rovnice veliˇcin ve stavov´em vektoru. Stanov´ıme-li pro kaˇzdou epochu i pˇr´ısluˇsn´ y ˇcas ti , ve kter´em zapoˇcala, pak definujeme ˇcasovou zmˇenu mezi epochami ∆t = ti − ti−1 a poˇc´ateˇcn´ı podm´ınky pro ti−1 jako xi−1 , vi−1 , ai−1 . Uvaˇzujeme-li absenci ˇr´ıd´ıc´ıho vstupu, pro rovnici procesu plat´ı pˇredpis xk = Fk−1 xk−1 + Wk−1 wk−1 .
19
Pro sestaven´ı matice W, tedy matice pˇrev´adˇej´ıc´ı vektor ˇsumu procesu, je tˇreba vr´atit se k zad´an´ı u ´lohy. Zde bylo uvedeno, ˇze ˇsum procesu m´ame uvaˇzovat jako n´ahodilou zmˇenu zrychlen´ı definovanou rovnic´ı ∂a , ∂t kde se opˇet jedn´a o poˇc´ıt´an´ı se skal´arn´ımi veliˇcinami. Z definice ˇsumu procesu vypl´ yvaj´ı pˇr´ımo fyzik´aln´ı rovnice, ze kter´ ych lze z´ıskat matici W. Stanoven´ım v´ ychoz´ı veliˇciny, integrac´ı a zaveden´ım integraˇcn´ıch konstant (ve fyzik´aln´ım v´ yznamu poˇca´teˇcn´ıch podm´ınek) z´ısk´ame w = a˙ =
a = a0 + at ˙ v = v0 + a0 t +
at ˙ 2 2
˙ 3 a0 t2 at + . 2 6 Pˇrejdeme-li od fyzik´aln´ıho znaˇcen´ı ke znaˇcen´ı zaveden´emu pro KF, z´ısk´ame obdobn´e rovnice, a to r = r0 + v0 t +
ak = ak−1 + wk−1 ∆t vk = vk−1 + ak−1 ∆t +
xk = xk−1 + vk−1 ∆t +
wk−1 ∆t2 2
ak−1 ∆t2 wk−1 ∆t3 + . 2 6
Odtud z rovnice (2.26) z´ısk´ame matici W W=
∆t3 62 ∆t . 2
∆t Tato matice byla v u ´loze pro u ´ˇcely zm´ınˇen´eho pˇredmˇetu jiˇz zad´ana vyuˇcuj´ıc´ım. Podle rovnice (2.26) z´ısk´ame matici H ve tvaru
1 H= 0 . 0 Kroky v´ ypoˇctu jsou tedy n´asleduj´ıc´ı, nejprve se vypoˇctou a priorn´ı odhady stavov´eho vektoru a jeho kovarianˇcn´ı matice z rovnic x ˆ− ˆ+ k = Fk−1 x k−1 + P− k = Fk−1 Pk−1 Fk−1 + Wk−1 Qk−1 Wk−1 ,
20
kde indexy matic F, W, Q d´avaj´ı najevo pˇrechod od epochy k−1 k epoˇse k. Kovarianˇcn´ı matice ˇsumu procesu byla volena empiricky, nebot’ tu nem´ame fakticky moˇznost odhadnout. Byla tedy zvolena skal´arn´ı hodnota, protoˇze ˇsum je zde tak´e skal´ar, takov´a aby nezav´adˇela zbyteˇcnou nejistotu do v´ ypoˇctu, ale aby tak´e d´avala dostateˇcnou volnost v´ yvoje procesu. D´ale se jiˇz podle rovnic (2.11) vypoˇcte ziskov´a matice a pomoc´ı n´ı a posteriorn´ı odhady stavov´eho vektoru a jeho kovarianˇcn´ı matice. Kovarianˇcn´ı matice mˇeˇren´ı (zde v kaˇzd´e epoˇse mˇeˇrena jedna vzd´alenost) byla zvolena jako variance t´eto vzd´alenosti, z´ıskan´a z parametr˚ u mˇeˇr´ıc´ıho pˇr´ıstroje. Pˇr´ısluˇsn´a matice V byla (vzhledem k tomu, ˇze nebylo specifikov´ano jinak) volena jako 1, nebot’ byl uvaˇzov´an standartn´ı chybov´ y model. Inicializaˇcn´ı hodnota stavov´eho vektoru
xinit
z1 = 0 0
a inicializaˇcn´ı hodnota jeho kovarianˇcn´ı matice byla nulov´a matice ˇr´adu 3. 2.2.5
Unscentovan´ y Kalman˚ uv filtr
Unscentovan´a podoba KF (UKF, angl. unscented KF ) ˇreˇs´ı obdobn´ y probl´em jako EKF. Jak´ ym zp˚ usobem prov´adˇet v´ ypoˇcet pomoc´ı KF v pˇr´ıpadˇe, ˇze vztahy pro pˇrevod vstupuj´ıc´ıch vektor˚ u z minul´e epochy do souˇcasn´e a vztahy pro pˇrevod a priorn´ıho odhadu stavov´eho vektoru nejsou line´arn´ı? V EKF je pouˇzita obdoba Taylorovy ˇrady, tedy vyuˇzit´ı parci´aln´ıch derivac´ı pro z´ısk´an´ı aproximace v bl´ızk´em okol´ı. Tento princip je obecnˇe vhodn´ y, ale pˇrirozenˇe se vyskytuj´ı i z´avislosti, kter´e nejsou ve sv´em bl´ızk´em okol´ı line´arn´ı a pro tyto je EKF silnˇe nestabiln´ı. Dalˇs´ı probl´emovou str´ankou EKF je implementace v´ ypoˇctu parci´aln´ıch derivac´ı (Jakobi´an˚ u funkc´ı) do v´ ypoˇcetn´ıch algoritm˚ u. V r´amci UKF by zm´ınˇen´e nedostatky EKF mˇela ˇreˇsit tzv. unscentovan´a transfromace [7]. M´ame-li n´ahodn´ y vektor x se stˇredn´ı hodnotou x ¯ a kovarianˇcn´ı matic´ı Σxx , kter´ y chceme pˇrev´est na jin´ y n´ahodn´ y vektor y tak, ˇze plat´ı y = f (x), je naˇs´ım z´akladn´ım poˇzadavkem vypoˇc´ıtat stˇredn´ı hodnotu y ¯ a kovarianˇcn´ı matic´ı Σyy novˇe vznikl´eho vektoru. Pro potˇreby v´ ypoˇctu tˇechto statistik n´ahodn´eho vektoru prov´ad´ıme v´ ybˇer sigma bod˚ u ’ z vektoru x. Takto vybran´ ych bod˚ u bude 2n + 1, nebot v´ ybˇer bod˚ u je symetrick´ y podle stˇredn´ı hodnoty n´ahodn´eho vektoru a pro v´ ybˇer plat´ı X0 = x ¯ Xi = x ¯+
q
(n + κ)Σxx i
Xi+n = x ¯−
q
(n + κ)Σxx i
s vektorem vah Wχ dan´ ym sv´ ymi sloˇzkami WX0 =
κ n+κ 21
WXi =
1 2(n + κ) 1 , 2(n + κ)
WXi+n =
kde κ ∈ R, i = 1, 2, · · · , n. U hodnot bod˚ u odmocnina z kovarianˇcn´ı matice je pˇrirozenˇe matice, doln´ı index z´avorky znaˇc´ı ˇr´adek ˇci sloupec t´eto matice. Statistiky vektoru y se po z´ısk´an´ı vˇsech sigma bod˚ u vypoˇctou n´asledovnˇe Y = f (X ) y ¯=
2n X
WXi Yi
i=0
Σyy =
2n X
WXi (Yi − y ¯)(Yi − y ¯ )T .
i=0
Algoritmus UKF je pak velmi podobn´ y EKF, nicm´enˇe do predikˇcn´ıch krok˚ u je zahrnuta unscentovan´a transformace, tedy v´ ybˇer sigma bod˚ u a v´ ypoˇcet odhadu statistik transformovan´eho vektoru. Podrobn´ y v´ ypoˇcet je uveden v [7], zde uvedeme jen nejv´ yznamnˇejˇs´ı kroky. V EKF poˇc´ıt´ame s a posteriorn´ım odhadem z minul´e epochy k − 1. Nicm´enˇe pro u ´ˇcel v´ ybˇeru sigma bod˚ u je tˇreba tento vektor upravit na vektor xak
=
xk wk−1
!
resp. pro jeho a posteriorn´ı odhad v pˇredchoz´ı epoˇse + xak−1
x+ k−1 o
=
!
,
kde nulov´ y blok je sloupcov´ y vektor d´elky r. K tomuto odhadu pˇr´ısluˇs´ı kovarianˇcn´ı matice + Pak−1
=
P+ P+ xvk−1 k−1 P+ Q k−1 xvk−1
+
!
. +
a V´ ybˇer sigma bod˚ u Xk−1 probˇehne pr´avˇe z vektoru xak . V´ ypoˇcet a priorn´ıch odhad˚ ua unscentovan´e transformace po v´ ybˇeru probˇehne podle rovnic +
a Xk− = f (Xk−1 , uk−1 ) a
x ˆ− k
=
2n X
Wi Xk−
i=0
a d´ale kovarianˇcn´ı matice a priorn´ıho odhadu stavov´eho vektoru je a
P− k
=
2n X
− T Wi (Xk− − x ˆ− ˆ− k )(Xk − x k) .
i=0
22
Obdobn´ ym zp˚ usobem se z´ısk´a i a priorn´ı odhad vektoru mˇeˇren´ı. Je tedy nutn´e opˇet transformovat sigma body transformovan´e funkc´ı f funkc´ı observaˇcn´ıho modelu h Zk− = h(Xk− , uk−1 ) a d´ale jiˇz pˇr´ımo odhad vektoru mˇeˇren´ı a
ˆ z− k
=
2n X
Wi Zk− .
i=0
V posledn´ıch dvou kroc´ıch, kter´e jsou zahrnuty do predikˇcn´ı ˇca´sti, je zapotˇreb´ı vypoˇc´ıtat kovarianˇcn´ı matici inovace P− cnou kovarianˇcn´ı matici inovace a a priorn´ıho νν a spoleˇ − odhadu stavov´eho vektoru Pxz . Tyto matice jsou d´any vztahy a
P− νν
= Rk +
2n X
− T Wi (Zk− − ˆ z− z− k )(Zk − ˆ k)
i=0 a
P− xν
=
2n X
− T Wi (Xk− − x ˆ− z− k )(Zk − ˆ k) .
i=0
UKF ˇreˇs´ı pˇredevˇs´ım probl´emy vyskytuj´ıc´ı se pˇri v´ ypoˇctu Jakobi´an˚ u funkc´ı, nebot’ zde prob´ıh´a pouze transformace vybran´ ych sigma bod˚ u. Odhad statistik je pˇresnˇejˇs´ı neˇz u EKF. Jedn´a-li se o syst´em, kde nelinearita nen´ı tak ”siln´a”aby linearizace v bl´ızk´em okol´ı nebyla dostaˇcuj´ıc´ı a pro kterou nen´ı v´ ypoˇcet Jakobi´an˚ u n´aroˇcn´ y, je praktiˇctˇejˇs´ı vyuˇz´ıt EKF. V´ ypoˇcetn´ı algoritmus podle [7] je shrnut v tabulce 3. Tabulka 3. - rovnice UKF f´ aze
v´ ypoˇ cty x ˆ− k
Predikce P− k Korekce
=
2n Pa i=0
=
2n Pa i=0
Wi Xk−
− T Wi (Xk− − x ˆ− ˆ− k )(Xk − x k) −1
− Wk = P − xν Pνν
νk = z k − ˆ z− k x ˆ+ ˆ− k = x k + W k νk − − T P+ k = Pk − Wk Pνν Wk
Modifikovan´e rovnice korekˇcn´ı ˇca´sti vypl´ yvaj´ı pˇredevˇs´ım ze znalosti (n´ızk´e v´ ypoˇcetn´ı n´aroˇcnosti) kovarianˇcn´ıch matic. 2.2.6
Zpˇ etn´ e vyhlazen´ı
Smyslem Kalmanovy filtrace bylo z´ıskat nejlepˇs´ı odhad stavov´eho vektoru x ˆ+ se k k v epoˇ s vyuˇzit´ım informac´ı z pˇredchoz´ıch epoch spoleˇcnˇe s mˇeˇren´ımi, kter´a pˇrich´azela v kaˇzd´e 23
nov´e epoˇse, nez´avisle na jiˇz vypoˇcten´ ych stavech. Zpˇetn´e vyhlazen´ı (BS angl. backward smoothing) d´av´a lepˇs´ı odhady stavov´eho vektoru x ˆsk vyuˇzit´ım dat pˇred a po epoˇse k. Tak jako forem dopˇredn´eho procesu KF existuje tolik druh˚ u jako aplikac´ı, i u BS je tomu stejnˇe. Uvedeme zde pouze jednu formu BS z ˇcistˇe informativn´ıch d˚ uvod˚ u, nebot’ na tyto algoritmy nen´ı tato pr´ace zamˇeˇrena. Uveden´ ym druhem algoritmu zpˇetn´eho vyhlazen´ı je fixed-interval smoothing (FI), konkr´etnˇe algoritmus RTS navrˇzen´ y v roce 1965. Principem FI je z´ısk´av´an´ı odhad˚ u x ˆsk pro k = 0, 1, · · · , N s vyuˇzit´ım mˇeˇren´ı zk pro 0 ≤ k ≤ N . Nˇekter´e aplikace obsahuj´ı ˇca´sti tˇri: dopˇredn´ y filtr, zpˇetn´ y filtr a vyhlazovaˇc. Algoritmus RTS kombinuje posledn´ı dvˇe sloˇzky dohromady, obsahuje tedy pouze dopˇredn´ y filtr a zpˇetn´ y vyhlazovaˇc. Dopˇredn´ ym filtrem tedy z´ısk´ame a posteri+ orn´ı odhad stavov´eho vektoru x ˆk a jeho kovarianˇcn´ı matici P+ r´ısluˇs´ı k . Tyto hodnoty pˇ epoˇse k. V´ yhoda algoritmu vyhlazen´ı RTS je, ˇze z´avis´ı pouze na datech produkovan´ ych dopˇredn´ ym filtrem, je tedy v´ yraznˇe menˇs´ı objem dat, kter´ y je nutno drˇzet v pamˇeti. Dalˇs´ı nespornou v´ yhodou je moˇznost poˇc´ıtat pouze vyhlazen´ y odhad stavov´eho vektoru ’ bez nutnosti k nˇemu poˇc´ıtat kovarianˇcn´ı matici, nebot tyto veliˇciny se poˇc´ıtaj´ı zcela nez´avisle. Algoritmus RTS vyhlazen´ı je shrnut v tabulce 4. Tabulka 4. - rovnice RTS vyhlazovac´ıho algoritmu Predikce x ˆ− ˆ+ k = Fk−1 x k−1 + T T P− k = Fk−1 Pk−1 Fk−1 + Wk−1 Qk−1 Wk−1
Korekce
− T T T −1 K2k = P− k Hk (Hk Pk Hk + Vk Rk Vk ) 2 ˆ− x ˆ+ ˆ− k) k = x k + Kk (zk − Hk x
x ˆsN = x ˆ+ k
Smoothing
PsN = P+ k s
− −1 T K2k = P+ k Fk (Pk+1 ) sT
s
− 2 s 2 Psk = P+ k − Kk (Pk+1 − Pk+1 )Kk s
2 x ˆsk = x ˆ+ xsk+1 − x ˆ− k + Kk (ˆ k+1 )
24
Kapitola 3
Aplikace rozklad˚ u 3.1 3.1.1
Vlastn´ı v´ ypoˇ cty ´ Uvod
V prvn´ı ˇc´asti se pod´ıv´ame na podm´ınˇenost matic. D´ale na konkr´etn´ım pˇr´ıkladˇe aplikujeme rozˇs´ıˇren´ y Kalman˚ uv filtr. 3.1.2
Norma matice
Analogicky k vektorov´ ym norm´am definujeme i normy maticov´e a znaˇc´ıme ||A|| normu matice A. Maticov´ ych norem je v´ıcero a jsou r˚ uznˇe definovan´e. V t´eto pr´aci uvedeme pouze nˇekter´e z nich, nicm´enˇe pro vˇsechny normy plat´ı podle [9] nerovnosti a rovnosti (
||A||
=0 A=0 > 0 jinak
||A + B|| ≤ ||A|| + ||B|| ||AB|| ≤ ||A||||B|| ||αA|| = |α|||A||. V uveden´ ych nerovnostech a rovnostech uvaˇzujeme, ˇze na lev´e i prav´e stranˇe uˇz´ıv´ame stejn´ y typ normy. ˇ adkov´a norma R´ ||A||R =
max
i=1,2,···,m
n X
|aij |
j=1
sloupcov´a norma ||A||S = max
j=1,2,···,n
m X
|aij |
i=1
p-norma ||A||p =
v uX n um X p t |aij |p , i=0 j=0
25
kde pro p = 1 mluv´ıme o normˇe oktaedrick´e a pro p = 2 mluv´ıme o Frobeniovˇe normˇe. Definice a manipulace s normami jsou d˚ uleˇzitou souˇca´st´ı v´ ypoˇctu tzv. podm´ınˇenosti (maticov´e) u ´lohy ˇci matice samotn´e.
Je-li d´ana soustava line´arn´ıch rovnic v maticov´em tvaru Ax = b, pak jej´ı ˇreˇsen´ı, v pˇr´ıpadˇe regul´arn´ı matice A, hled´ame ve tvaru x = A−1 b. Ve v´ ypoˇctech realizovan´ ych s pomoc´ı osobn´ıch PC z´ısk´av´ame pouze do jist´e m´ıry numericky nepˇresn´e ˇreˇsen´ı t´eto soustavy. Po zpˇetn´em dosazen´ı vypoˇcten´eho ˇreˇsen´ı z´ısk´av´ame reziduum ˇreˇsen´ı. O velikosti rezidua rozhoduje rozd´ıl mezi pˇresn´ ym ˇreˇsen´ım a pˇribliˇzn´ ym ˇreˇsen´ım a d´ale podm´ınˇenost matice soustavy A. Podm´ınˇenost matice posuzujeme podle ˇc´ısla podm´ınˇenosti cond(A), kter´e je d´ano jako cond(A) = ||A||||A−1 ||, pro kter´e d´ale plat´ı cond(A) = cond(A−1 ). Jak jiˇz bylo pops´ano, norem matic je velk´e mnoˇzstv´ı a vˇsechny ned´avaj´ı stejn´e v´ ysledky. Z toho plyne, ˇze i ˇc´ıslo podm´ınˇenosti matice je z´avisl´e na zvolen´e normˇe. V Kalmanovˇe filtru n´as pˇredevˇs´ım zaj´ım´a vliv inverze na danou matici. Jinak ˇreˇceno, v rovnici pro ziskovou matici − T T −1 K2k = P− k Hk (Hk Pk Hk + Rk ) ,
je zdrojem numerick´e nestability inverze matice sloˇzen´e z kovarianˇcn´ı matice a priorn´ıho odhadu stavov´eho vektoru a kovarianˇcn´ı matice ˇsumu mˇeˇren´ı. Matice je tedy symetrick´a, pozitivnˇe definitn´ı. A pr´avˇe u kovarianˇcn´ıch matic ˇcasto doch´az´ı k numerick´ ym nepˇresnostem zp˚ usoben´ ym ˇspatnou podm´ınˇenost´ı matice. Tomuto probl´emu se vˇenujeme v z´avˇeru t´eto pr´ace.
Chceme-li prov´adˇet inverzi matice A u n´ıˇz je proveden rozklad takov´ y, ˇze plat´ı A = BCD, pak inverzn´ı matice k matici A je matice A−1 = D−1 C−1 B−1 . Jsou-li matice B, C, D l´epe podm´ınˇen´e neˇz matice A, hovoˇr´ıme o zv´ yˇsen´ı numerick´e stability naˇseho v´ ypoˇctu (inverze matice). Uvaˇzujeme-li, ˇze zn´ame napˇr´ıklad SVD rozklad matice A ve tvaru T A = UA ΣA VA ,
26
pak jej´ı inverzi m˚ uˇzeme ps´at ve tvaru T A−1 = VA Σ−1 A UA .
Vzhledem k tomu, ˇze matice Σ je diagon´aln´ı, m˚ uˇzeme ps´at jej´ı inverzn´ı matici jako
Σ−1 =
σ1−1 0 · · · · · · 0 0 σ2−1 0 .. .. .. . . . .. .. ... . . −1 0 0 · · · · · · σn
coˇz je diagon´aln´ı matice s pˇrevr´acen´ ymi hodnotami k diagon´aln´ım prvk˚ um matice Σ. Jak je vidˇet, ˇz´adnou inverzi prov´adˇet nemus´ıme. Ostatn´ı rozklady uveden´e v t´eto pr´aci vyˇzaduj´ı inverzi alespoˇ n jedn´e z matic rozkladu. Pro u ´ˇcely zkoum´an´ı numerick´e stability jsme vybrali ˇctyˇri symetrick´e pozitivnˇe definitn´ı matice, kter´e maj´ı simulovat kovarianˇcn´ı matice vyskytuj´ıc´ı se ˇcasto v geodetick´ ych aplikac´ıch. V minul´em odstavci byla ˇreˇc o norm´ach a jejich pouˇzit´ı k v´ ypoˇctu ˇc´ısla podm´ınˇenosti matice. Vzhledem k tomu, ˇze v´ ypoˇcty v t´eto kapitole byly prov´adˇeny v R je vhodn´ software Matlab , e uv´est dvˇe funkce pro v´ ypoˇcet ˇc´ısla podm´ınˇenosti, kter´e jsou v nˇem implementovan´e. Prvn´ı z nich je funkce cond, jej´ıˇz syntax v Matlabu je n´asleduj´ıc´ı c = cond(X,p), kde prvn´ı parametr X je matice pro n´ıˇz chceme z´ıskat ˇc´ıslo podm´ınˇenosti a druh´ y parametr p d´av´a moˇznost volby typu normy (1 pro 1-normu, 2 pro 2-normu, ’fro’ pro Frobeniovu normu, inf pro nekoneˇcnou normu). Zaj´ımav´e je, ˇze pˇri 2-normˇe vyuˇz´ıv´a algoritmus funkce SVD rozkladu a ˇc´ıslo podm´ınˇenosti urˇc´ı jako pod´ıl nejvˇetˇs´ıho a nejmenˇs´ıho singul´arn´ıho ˇc´ısla matice X. U t´eto funkce plat´ı, ˇze ˇc´ım vˇetˇs´ı ˇc´ıslo podm´ınˇenosti, t´ım bl´ıˇze je matice singul´arn´ı. Dalˇs´ı funkc´ı pro urˇcen´ı podm´ınˇenosti matice je funkce rcond, jej´ı syntax je c = rcond(X), kde vstupn´ım parametrem je matice pro niˇz chceme zjistit ˇc´ıslo podm´ınˇenosti. Z mateˇ ım bl´ıˇze je matick´eho hlediska rcond d´av´a pˇrevr´acenou hodnotu ˇc´ısla podm´ınˇenosti. C´ ˇc´ıslo c nule, t´ım bl´ıˇze je matice k singul´arn´ı matici. Pro u ´ˇcely naˇsich v´ ypoˇct˚ u byly d´ale v prostˇred´ı Matlab vytvoˇreny funkce pro urˇcen´ı norem uveden´ ych v pˇredchoz´ım odstavci.
3.1.3
Zkoum´ an´ı numerick´ e stability maticov´ eho rozkladu
Jak jiˇz bylo ˇreˇceno, pro zkoum´an´ı numerick´e stability jsme vybrali ˇctyˇri matice. Jsou to matice
0.991780832576018 −0.117862095198783 −0.596260561640973 0.167114265955008 Σ1 = −0.117862095198783 0.904576424161489 −0.596260561640973 0.167114265955008 0.42206289705396
27
0.02500 0.00750 0.00175 Σ2 = 0.00750 0.00700 0.00135 0.00175 0.00135 0.00043
0.535042133582262 0.114231632552805 −0.308134531201913 Σ3 = 0.114231632552805 1.00092814352175 0.195158772317945 −0.308134531201913 0.195158772317945 0.247185291110973
1.66666666666667 5.23598775598299 4.53046971409841 Σ4 = 5.23598775598299 16.4493406684823 14.2328903711226 . 4.5304697140984 14.2328903711226 12.3150934982178 Jak bylo ˇreˇceno, jsou to matice symetrick´e, pozitivnˇe definitn´ı, pˇripad´a tedy v u ´vahu i Cholesk´eho a LU rozklady. ˇ ısla podm´ınˇenosti tˇechto matic, z´ıskan´a vyuˇzit´ım dˇr´ıve zm´ınˇen´e funkce cond (bez C´ druh´eho parametru, implicitnˇe tedy poˇc´ıt´a 2-normu, viz. pozn´amky u intern´ıch funkc´ı Matlabu) jsou cond(Σ1 ) cond(Σ2 ) cond(Σ3 ) cond(Σ4 )
= = = =
37.028 919 350 676 175.106 452 408 183 25 390 514 254.153 5 8.502 273 834 296 1e + 015.
Matice byly voleny s r˚ uzn´ ym ˇc´ıslem podm´ınˇenosti, od relativnˇe dobˇre podm´ınˇen´e matice Σ1 po velmi ˇspatnˇe podm´ınˇenou matici Σ4 . N´aslednˇe byly pro vˇsechny zm´ınˇen´e matice provedeny LU, QR, Cholesk´eho a SVD rozklady pomoc´ı intern´ıch funkc´ı Matlabu. Tyto matice byly n´aslednˇe invertov´any, nejlepˇs´ım moˇzn´ ym zp˚ usobem, tedy pakliˇze je napˇr. v QR rozkladu matice Q ortogon´aln´ı, nebyla invetov´ana, ale pouze transponov´ana. N´aslednˇe byly p˚ uvodn´ı matice n´asobeny s maticemi k nim inverzn´ımi. V´ ysledkem t´eto operace by mˇela b´ yt matice jednotkov´a (viz. kapitola 1). Nicm´enˇe vzhledem k omezen´emu numerick´emu rozsahu Matlabu a ˇspatn´e podm´ınˇenosti matic byly matice pouze bl´ızk´e jednotkov´ ym matic´ım. Jako krit´erium pro vhodnˇejˇs´ı rozklad pro sn´ıˇzen´ı nepˇresnost´ı inverz´ı matice jsme zvolili ˇr´adkovou normu. V´ ysledky pokusu byly oˇcek´avan´e. Pro matice Σ1 , Σ2 , Σ3 se jako nejvhodnˇejˇs´ı rozklad uk´azal LU. Pro ˇctvrtou matici Σ4 vyˇsel nejl´epe Cholesk´eho rozklad, kter´ y se vyuˇz´ıv´a v mnoha algoritmech, kter´e pracuj´ı se symetrick´ ymi pozitivnˇe definitn´ımi maticemi (v geod´ezii pˇredevˇs´ım kovarianˇcn´ımi) pro zv´ yˇsen´ı stability v´ ypoˇct˚ u. V´ ysledky tak´e naznaˇcuj´ı, ˇze i pro prvn´ı tˇri matice by Cholesk´eho rozklad byl vhodn´ y, nebot’ ˇra´dkov´e normy vyˇsly vˇzdy ˇr´adovˇe stejn´e jako pro LU rozklad. Jednotliv´e normy jsou shrnuty v n´asleduj´ıc´ı tabulce pro kaˇzd´ y z rozkladu a kaˇzdou z matic (tabulka 5).
28
Σ1 Σ2 Σ3 Σ4
Tabulka 5. - vypoˇcten´e normy LU QR Cholesk´eho 1.00000000000000 1.00000000000000 1.00000000000000 1.00000000000000 1.00000000000000 1.00000000000000 0.999999868916348 1.00000164838275 1.00000092992559 1.05891036987305 1.1456127166748 0.973299026489258
SVD 1.00000000000001 1.00000000000000 1.00000046461355 1.13163423538208
V n´asleduj´ıc´ıch v´ ypoˇctech algoritmem EKF budeme tedy aplikovat na matici v inverzi v Kalmanovˇe ziskov´e matici Cholesk´eho rozklad. 3.1.4
Druh´ y pˇ r´ıklad aplikace EKF
Druh´ y pˇr´ıklad na nˇemˇz budeme ilustrovat pr´aci a v´ ypoˇcty spojen´e s EKF byl vymyˇslen a navrˇzen pro aplikaci rozkladu na inverzi matice (pops´ano dˇr´ıve). Nebyla vyuˇzita data z odstavce 2.2.4, nebot’ vektor mˇeˇren´ı zde obsahuje pouze jeden mˇeˇren´ y prvek a nedoch´az´ı tedy k inverzi matice ale pouze k v´ ypoˇctu reciprok´e hodnoty skal´aru. Mˇejme projektil, vystˇrelen´ y z odpalovac´ıho zaˇr´ızen´ı s poˇca´teˇcn´ı rychlost´ı v = 200m/s pod elevaˇcn´ım u ´hlem α = 20gon . Jedn´a se tedy o projektil s plochou dr´ahou letu. Bez ohledu na technickou realizaci (GPS, em. impulsy, laserov´e mˇeˇren´ı) mˇeˇr´ıme dvˇe vzd´alenosti k dan´emu projektilu. Uv´aˇz´ıme-li, ˇze projektil byl vystˇrelen z poˇc´atku souˇradn´e soustavy X1 = [0, 0], pak z tohoto m´ısta je mˇeˇrena prvn´ı vzd´alenost s1 . Druh´a vzd´alenost s2 je mˇeˇrena z m´ısta o souˇradnic´ıch X2 = [50, 0], kde jednotka souˇradn´eho syst´emu odpov´ıd´a hodnotˇe 1m. Z fyziky je zn´amo, ˇze pˇri zanedb´an´ı vˇsech okoln´ıch vliv˚ u, se projektil vystˇrelen´ y pod elevaˇcn´ım u ´hlem α0 s poˇc´ateˇcn´ı rychlost´ı v0 pohybuje po tzv. balistick´e kˇrivce, tedy ˇca´sti paraboly. Cel´ y pohyb (poloha projektilu) je pak pops´an vektorovou rovnic´ı r = r0 + v −
gt2 , 2
kde r = (x, y, z) je polohov´ y vektor. Sledovan´ y pohyb se dˇeje v rovinˇe xy, tedy z = 0. Vektor r0 = (x0 , y0 , z0 ) je vektor poˇc´ateˇcn´ıch hodnot. Vektor v = (vx0 , vy0 , 0) = . (v0 cos (α), v0 sin (α), 0) a koneˇcnˇe vektor g = (0, −g, 0), kde g = 9.81, je t´ıhov´e zrychlen´ı. Abychom sestavili matice potˇrebn´e pro algoritmus EKF, mus´ıme fyzik´aln´ı rovnice napsat ve tvaru diferenˇcn´ıch rovnic. Plat´ı vxk = vxk−1 vyk = vyk−1 − g∆t xk = xk−1 + vxk−1 ∆t yk = yk−1 + vyk−1 ∆t −
29
g∆t2 . 2
Matici F m˚ uˇzeme tedy rovnou ps´at ve tvaru
F=
1 ∆t 0 0 0 1 0 0 0 0 1 ∆t 0 0 0 1
a uvaˇzovanou korekci (uˇzivatelsk´ y vstup) ve tvaru 0 0
T=
2 − g∆t 2
.
−g∆t
ˇ Sum procesu m˚ uˇzeme uvaˇzovat napˇr´ıklad jako n´ahodnou zmˇenu rychlosti projektilu jak ve smˇeru osy x, tak ve smˇeru osy y. Bez odvozen´ı (podobn´a u ´vaha jako v odstavci 2.2.4) tedy uv´aˇz´ıme matici W
∆t2 2
∆t 0 0
W=
0 0
∆t2 2
∆t
.
Kovarianˇcn´ı matice R vektoru mˇeˇren´ı zk = (s1k , s2k ) je diagon´aln´ı s variancemi mˇeˇren´ ych vzd´alenost´ı na diagon´ale R=
σs21 0 0 σs22
!
a d´ale kovarianˇcn´ı matici Q ˇsumu procesu bylo nutno odhadnout. K dispozici byla mˇeˇren´ı od 1 sekundy od vystˇrelen´ı projektilu. Jako inicializaˇcn´ı hodnoty byly zvoleny hodnoty spoˇcten´e z rovnic pohybu projektilu, tedy bez uv´aˇzen´ı naruˇsen´ı pohybu. K dispozici bylo dohromady 532 epoch mˇeˇren´ı. Grafy odchylek hodnot z´ıskan´ ych filtrac´ı od hodnot skuteˇcn´ ych jsou souˇca´st´ı pˇr´ıloh stejnˇe jako algoritmus v´ ypoˇctu v prostˇred´ı Matlab.
30
Z´ avˇ er Ve sv´e pr´aci jsem za pouˇzit´ı zdroj˚ u uveden´ ych v seznamu literatury prostudoval a sestavil pˇet teoretick´ ych algoritm˚ u pro maticov´e rozklady: LU rozklad a jeho speci´aln´ı pˇr´ıpad rozklad Cholesk´eho, dva QR rozklady, a to Gram-Schmidt˚ uv a Givens˚ uv rozklad, a d´ale singul´arn´ı rozklad, viz kapitola 1. Z tˇechto pˇeti algortm˚ u jsem LU, Cholesk´eho a QR (Gramm-Schmidtovou ortogonalizac´ı) implementoval v jazyce C++ vyuˇzit´ım datov´e struktury pole. Ve druh´e kapitole jsem si odvodil vzorce pro algoritmus klasick´eho a rozˇs´ıˇren´eho Kalmanova filtru. Rozˇs´ıˇren´ y filtr jsem ilustroval na pˇr´ıkladu rovnomˇern´eho pohybu, ˇreˇsen´eho v pˇredmˇetu Teorie chyb a vyrovn´avac´ı poˇcet 2. D´ale jsem zde uvedl unscentovanou podobu a zpˇetn´e vyhlazen´ı Kalmanova filtru. Ve tˇret´ı kapitole jsem zkoumal numerickou stabilitu maticov´ ych rozklad˚ u. K tomu u ´ˇcelu jsem zvolil ˇctyˇri matice pouˇz´ıvan´e v geodetick´ ych v´ ypoˇctech, od relativnˇe dobˇre podm´ınˇen´e matice po matici velmi ˇspatnˇe podm´ınˇenou. Pro prvn´ı tˇri matice vyˇsel nejl´epe LU rozklad, pro ˇctvrtou matici rozklad Cholesk´eho. D´ale jsem v t´eto kapitole navrhl dalˇs´ı pˇr´ıklad vyuˇzit´ı Kalmanova filtru opˇet pro filtraci dat mˇeˇren´ı ze sledov´an´ı objektu, tentokr´at pohybuj´ıc´ıho se v rovinˇe. Tento pˇr´ıklad jsem vyˇreˇsil a pouˇzil pro aplikaci maticov´eho rozkladu v algoritmu rozˇs´ıˇren´eho Kalmanova filtru.
31
Literatura ˇ ´ Milada a Jaroslav CERN ˇ ´ Geo-matematika I. Vyd. 1. Praha: Cesk´ ˇ a technika [1] KOCANDRLOV A, Y. ˇ - nakladatelstv´ı CVUT, 2008. ISBN 978-80-01-03936-6 [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 08-018-5414-8 [3] Greg Welch, Gary Bishop, ”An Introduction to the Kalman Filter”, University of North Carolina at Chapel Hill Department of Computer Science, 2001 ˇ Jiˇr´ı a Josef MACHEK. Poˇcet pravdˇepodobnosti. Praha: St´atn´ı nakladatelstv´ı technick´e [4] LIKES, literatury, 1981, 159 s. ˇ Adjustment calculus. Vyd. 1. Praha: Nakladatelstv´ı CVUT, ˇ [5] MERVART, Leoˇs a Zdenˇek LUKES. 165 s. ISBN 978-80-01-03593-1. [6] STRANG, Gilbert. Linear algebra, geodesy, and GPS. Wellesley: Wellesley-Cambridge Press, 1997. ISBN 09-614-0886-3. [7] JULIER, Simon J. a Jeffery K. UHLMANN. A New Extension of the Kalman Filter to Nolinear Systems. s. 12. Dostupn´e z: http://www.cs.unc.edu/ 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´e z: http://webee.technion.ac.il/people/shimkin/Estimation09/Kalman1960.pdf [9] M´IKA, Stanislav. Numerick´e metody algebry: matematika pro vysok´e ˇskoly technick´e. 2 vyd. Praha: SNTL - Nakladatelstv´ı technick´e literatury, 1985. ˇ ˇ a Jaroslav VILD. Uvod ´ [10] NEKVINDA, Milislav, Jiˇr´ı SRUBA R do numerick´e matematiky. 1 vyd. Praha: SNTL, 1976. [11] REKTORYS, Karel. Pˇrehled uˇzit´e matematiky. 7. vyd. Praha: Prometheus, 2000, xxxii, 874 s. ISBN 80-719-6179-5. ´ Zdenˇek a Frantiˇsek KRUPKA. Fyzika: Pˇr´ıruˇcka pro vysok´e ˇskoly technick´eho smˇeru. 2. [12] HORAK, pˇrep. vyd. Praha: SNTL - Nakladatelstv´ı technick´e literatury, n.p., 1976
32
Pˇ r´ılohy Algoritmy v C++ Algoritmy LU, QR a Cholesk´eho rozkladu v jazyce C++. Vzhledem k pouze ilustrativnimu charakteru je i QR rozklad ps´an pouze pro ˇctvercov´e matice. Funkce mattim, resp. trans jsou algoritmy maticov´eho n´asoben´ı, resp. transpozice pro ˇctvercov´e matice (navrˇzeny pouze pro u ´ˇcely odladˇen´ı algoritm˚ u rozklad˚ u). Vstupem do funkc´ı jsou jednorozmˇern´a pole v nichˇz jsou matice uloˇzeny po ˇr´adc´ıch. Funkce jsou obsahem souboru rozklady.h, kter´ y je na pˇriloˇzen´em 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
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
} void RTR(float *A, int hA, float *RR){ // A - matice pro niˇ z 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
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´ ysledn´ e grafy N´asleduj´ıc´ı grafy vygenerovan´e v software Matlab ilustruj´ı ˇcasov´ y v´ yvoj odchylek skuteˇcn´e a vyfiltrovan´e polohy projektilu v jednotliv´ ych os´ach.
36
37
