Kálmán-szűrés Korszerű matematikai módszerek a geodéziában 2014.03.10.
A Kálmán-szűrés definíciója „Olyan algoritmus, amely valamely lineáris dinamikus rendszerben egzakt következetést tesz lehetővé, amely a rejtett Markov-modellhez hasonló Bayesféle modell, azonban a rejtett változók állapottere folytonos, és minden rejtett és megfigyelhető változó (gyakran többváltozós) normális eloszlású.” n UGYE MINDEN VILÁGOS!? n
Az axiomatikus módszer „E módszer lényege abban áll, hogy a tételeket definíciókká tesszük. A tétel tartalmi része ilyenkor a definíció motivációjává válik, s az algebristák tudományuk tekintélyét növelendő, többnyire ezt elhagyják … egy nem motivált definíciót megérteni nem lehet…” „az axiomatikus módszer[nek] számtalan előnye van, hasonlóan ahhoz, hogy a lopásnak számtalan előnye van a tisztességes munkával szemben” (V. I. Arnold, orosz matematikus)
A Kálmán-szűrő n n n n n n
Rudolf Emil Kalman publikálja az elméletet (1960) Első nevezetes alkalmazása: Apolló űrhajó fedélzeti számítógépe (Armstrong) Navigációs és mérnöki alkalmazások Szenzor adatok fúziója (radar, lézerszkenner, kamera, INS,…) GNSS vevők, okostelefonok, számítógépek, játékok, stb. elmaradhatatlan része Tőzsde előrejelzés, idősor elemzés, függvény approximáció Rudolf Emil Kalman (Kálmán Rudolf) (1930-) magyar származású villamosmérnök
A publikáció n
R.E. Kalman: A New Approach to Linear Filtering and Prediction Problems, Transactions of the ASME – Journal of Basic Engineering, 82 (Series D): 35-45
Meg lehet érteni? n
n n n
a homályosnak tűnő matematikai jelölés ellenére, a Kálmán-szűrés elgondolása egészen egyszerű és közvetlen: időben változó rendszer, amelynek állapotát leíró változókat (x) nem (teljesen) ismerjük ezt az állapotot szeretnénk minden lépésben a lehető legjobban megismerni rendelkezünk folyamatos mérésekkel (y), melyek kapcsolatba hozhatók a rendszer állapotával (x)
A Kálmán szűrő jellemzői lépésenkénti számítás, csak az előző és a jelenlegi lépés függvényében = rekurzív n a kapcsolatok lineárisak (állapot változása, mérések-állapot kapcsolata) n sztochasztikus (mérési hibák, állapot és a hibája valószínűségi változók) n legkisebb hibájú optimális becslés (csak lineáris esetben bizonyítható) n
Rekurzív becslés – példa egyváltozós rendszer állapotát mérjük egymás utáni időpontokban n mérések: x1, x2, … xn, n a legjobb állapot becslés az átlag: n
1 n m n = å xi n i =1 n
új mérésünk van: xn+1, a következő becslés:
1 n +1 n æ1 n 1 ö m n +1 = xi = ç å xi + xn +1 ÷ å n + 1 i =1 n + 1 è n i =1 n ø
Erősítés, új információ n
a frissített becslés kis korrekcióval számítható:
n 1 m n +1 = mn + xn +1 = m n + K ( xn +1 - m n ) n +1 n +1 n
K az erősítési tényező („nyereség”) ¨
n
ez dönti el, mekkora lesz a korrekció
xn+1 – µn az új információ („innováció”) ha nincs új információ, nem változik az előző becslés
¨
Rekurzív átlag becslés
Különböző információk optimális felhasználása egy távolságot két különböző eszközzel mértünk (mérőszalag, lézer távmérő) n mérések: x1, x2, szórások: σ1, σ2 n az optimális becslést keressük x-re n
¨ez
a súlyozott átlag:
xˆ = wx1 + (1 - w) x2 n
Mi az optimális w súly?
Optimális súly meghatározása n
mivel x1, x2 normális eloszlású, a lineáris kombinációjuk is normális eloszlású, melynek szórása 2 2 2 2 2
sˆ = w s 1 + (1 - w) s 2
n
az optimális w súly ennek a kifejezésnek a minimuma, ahol a w szerinti derivált zérus lesz 2
¶sˆ 2 2 = 2 wopts 1 - 2(1 - wopt )s 2 = 0 ¶w
n
az optimális súly tehát
wopt
s = 2 s 1 + s 22 2 2
Optimális becslés és szórása az optimális becslés x-re: 2 2 s2 s1 xˆ = 2 x + 2 x 2 1 2 2 s1 + s 2 s1 + s 2 n az optimális becslés szórása: 2 2 s 2 1s2 sˆ = 2 s 1 + s 22 n az optimális becslés minimalizálja az x1, x2 -től vett súlyozott távolságösszeget: n
éæ x - x ö 2 æ x - x ö 2 ù ÷÷ + çç 2 ÷÷ ú xˆ = arg min êçç 1 x êëè s 1 ø è s 2 ø úû
Optimális becslés n
az optimális becslés x-re szélsőérték:
2 2 é ¶ æ x1 - x ö æ x2 - x ö ù 2( x1 - xˆ )s 22 + 2( x1 - xˆ )s 12 ÷÷ + çç ÷÷ ú = êçç =0 2 2 ¶x êè s 1 ø è s 2 ø ú s1 s 2 ë û
n
ezt megoldva valóban s 22 s 12 xˆ = 2 x + 2 x 2 1 2 2 s1 + s 2 s1 + s 2
Rekurzív optimális becslés n
Két lépésben mérünk, először x1-et, azután x2 -t. Az első lépésben sˆ12 = s 12 xˆ1 = x1
n
A második lépésben frissítjük a becslést 2 ˆ s1 új információ ˆ ( ) xˆ2 = xˆ1 + 2 x x 2 1 2 (innováció) sˆ1 + s 2 2 æ ˆ s1 ö 2 2 ÷ sˆ sˆ 2 = çç1 - 2 2 ÷ 1 è sˆ1 + s 2 ø
K erősítés (nyereség)
Rekurzív optimális becslés n
a becslési egyenletek xˆ2 = xˆ1 + K ( x2 - xˆ1 ) 2 2 ˆ ˆ s 2 = (1 - K )s 1 2 ˆ s1 K= 2 sˆ1 + s 22
n
becslés hibája (szórás)
K: erősítés
ismerős az egyenlet alakja? m n +1 = m n + K ( xn +1 - m n )
Közvetett mérések n n
n
Mi van, ha x-et nem tudjuk közvetlenül mérni (rejtett változó, rendszerállapot)? Geodéziában ez jól ismert: közvetett mérések kiegyenlítése (II. kiegyenlítési csoport, paraméteres kiegyenlítés), Gauss-Markov modell Lineáris (közvetítő) egyenlet az y mérés és x rendszerállapot között (z a mérési hiba, „zaj”):
y = Cx + z
n
a mérési zajt zérus átlag ( z = 0 ) és az Sz kovariancia mátrix jellemzi (E[ ] a várható érték):
[
S z = E ( z - z )( z - z )T
]
Közvetett állapotbecslés Tegyük fel, hogy egyszerre N db. y méréssel rendelkezünk n Mindegyik mérés külön Ck és Szk kapcsolatban lehet a rendszer állapotával: n
yk = Ck x + z k n
Ekkor az optimális becslésre a zk javítások súlyozott négyzetösszege minimális lesz: N
xˆ = arg min å ( yk - Ck x)T S zk-1 ( yk - Ck x) x
k =1
Közvetett állapotbecslés n
A korábbi (távolságmérés) példában a mérések közvetlenül az állaportra vonatkoztak, tehát yk= xk volt és Ck = 1. Így N
xˆ = arg min å ( xk - x)T S zk-1 ( xk - x) x
k =1
ami megegyezik a korábbival, ha N = 2: éæ x - x ö 2 æ x - x ö 2 ù ÷÷ + çç 2 ÷÷ ú xˆ = arg min êçç 1 x êëè s 1 ø è s 2 ø úû
Közvetett állapotbecslés n
Ha van információnk a kezdeti xˆ0 rendszerállapotról és annak kovariancia mátrixáról ( Pˆ0 ), ezt a 0-adik időponti mérésként lehet kezelni: N é ù T ˆ -1 T -1 xˆ = arg min ê( xˆ0 - x) P0 ( xˆ0 - x) + å ( xk - x) S zk ( xk - x)ú x k =1 ë û
n
Ha a mérések nem egyszerre, hanem egymás után érkeznek, a k-adik időpontban:
[
xˆ = arg min ( xˆk -1 - x)T Pˆk--11 ( xˆk -1 - x) + ( yk - Ck x)T S zk-1 ( yk - Ck x) x
n
Ennek a megoldása a statikus Kálmán-szűrő
]
Statikus Kálmán-szűrő példa GPS mérések feldolgozása n rejtett állapot (x paraméter): vevő koordinátája és az órájának igazítatlansága n mérések (y): műhold-vevő (ál)távolságok n összes mérés rendelkezésre áll: hagyományos LKN kiegyenlítés n epochánként érkező mérések: rekurzív kiegyenlítés, statikus Kálmán-szűrő n
Helymeghatározás egyenletei n
mért (ál)távolságok: PRi = ( X i - x) 2 + (Y i - y ) 2 + ( Z i - z ) 2 + Cr
n
i = 1..n
jelölések: Xi, Yi, Zi
az i-edik műhold ismert térbeli derékszögű koordinátái
PRi x, y, z Cr n
az i-edik műholdra végzett távolságmérés eredménye a vevő ismeretlen térbeli derékszögű koordinátái a vevő órahibájának hatása az észlelt műholdak száma
Linearizált mérési modell n
mért (ál)távolságok:
PRi = r 0i -
n
X i - x0
r 0i
DX -
Y i - y0
r 0i
DY -
Z i - z0
r 0i
DZ + Cr
jelölések: x0, y0, z0
a vevő előzetes térbeli derékszögű koordinátái
ΔX, ΔY, ΔZ az előzetes koordináták javítása r 0i = ( X i - x0 ) 2 + (Y i - y0 ) 2 + ( Z i - z0 ) 2
a geometriai távolság közelítő értéke
i = 1..n
Megoldandó egyenletrendszer é X 1 - x0 ê1 1 r é PR1 - r 0 ù ê 0 2 ê 2 ú ê X - x0 ê PR2 - r 0 ú = ê 2 r ê ê ú 0 M ê ú ê M n êë PRn - r 0 úû ê X n - x 0 êêë r 0n
-
Y 1 - y0
-
Y - y0
-
M Y n - y0
r 01 2
r 02
r 0n
-
-
Z 1 - z0
r 01 Z 2 - z0 r 02 M Z n - z0 r 0n
ù + 1ú ú éDX ù úê ú + 1ú ê DY ú ú ê DZ ú M úê ú ú ë Cr û + 1ú úû
jelöljük az A alakmátrix i-edik sorát a hi vektorral, az állapotvektort az x vektorral
Rekurzív kiegyenlítés Takács Bence, 2001
A számítási összefüggések a Kálmán-szűrő egyenletei (lásd később):
K i = Pi × h i × (h i ,T × Pi × h i + R ) -1 x i +1 = x i + K i × (l i - h
i ,T
× xi )
új információ (innováció)
Pi +1 = Pi - K i × h i ,T × Pi K erősítés (nyereség) Az iteráció utolsó lépése után a P mátrix a normálmátrix inverzévé válik
Dinamikus rendszer n
Sokszor van az, hogy a rendszer állapota változik, vagyis x függ az időtől ¨ ¨ ¨ ¨
n
pl. GNSS mozgásvizsgálati hálózat pontjai mozognak mesterséges hold helyzete, sebessége változik jármű helyzete, sebessége változik járműforgalom folyamatosan változik
Tegyük fel, hogy lineáris a változást leíró egyenlet (állapotegyenlet):
xk = Axk -1 + Buk -1 + wk
n
A folyamat zajt, wk-t a Q (vagy ha változik, Qk-1) kovariancia mátrixszal jellemezzük, uk-1 pedig egy ismert input vektor
Dinamikus rendszer n
A rendszer állapota változik a k-1 és k időpontok között. Ezért a korábbi (statikus) egyenlet első tagját meg kell változtatni az előrejelzett állapotváltozásnak megfelelően:
[
xˆ = arg min ( xˆk -1 - x)T Pˆk--11 ( xˆk -1 - x) + ( yk - Ck x)T S zk-1 ( yk - Ck x) x
n
Az előrejelzett számítható:
~ xk rendszerállapot az állapot egyenletből ~ x = Axˆ + Bu k
n
]
k -1
k -1
Ha az uk inputot tökéletesen ismerjük, akkor az előrejelzés hibája
~ xk - xk = A( xˆk -1 - xk -1 ) - wk
Optimális becslés n n
~ xk kovariancia mátrixa ennek megfelelően: ~ Pk = APˆk -1 AT + Qk -1 ~ ~ Az egyenletbe beírva xk és Pk értékeit xˆk -1 és Pˆk -1 helyére,
[
T ~ -1 ~ ~ xˆ = arg min ( xk - x) Pk ( xk - x) + ( yk - Ck x)T S zk-1 ( yk - Ck x) x
n
]
A minimumot ismét az x szerinti parciális deriváltat zérussal egyenlővé téve kapjuk. Az eredmény: xˆk = ~ xk + K k ( y k - C k ~ xk ) ~ ˆ Pk = ( I - K k Ck ) Pk
Optimális becslés n
a szükséges mátrixok: ~ T -1 K k = Pk Ck S k ~ T S k = S zk + Ck Pk Ck
n
A bizonyításhoz alkalmazni kell a következő mátrix inverziós lemmát: ( A + BCB T ) -1 = A-1 - A-1 B ( B T A-1 B + C -1 ) -1 B T A-1
Rendszerállapot kovariancia mátrixa n
ˆ kovariancia mátrixa az xˆ A rendszerállapot P k k állapotbecslés bizonytalanságnak mértéke. Ezt az T ˆ -1 ˆ ( xk -1 - x) Pk ( xˆk -1 - x) = 1
ellipszoid mutatja:
Pˆk = US V T SVD felbontás
Rendszerállapot konvergencia n
Ahogy telik az idő, a rendszer állapotának becslése egyre bizonytalanabbá válik ¨
n
ezt az egyre növekvő hibaellipszoid jelzi
Ezzel szemben, a mérések csökkentik az állapot becslés bizonytalanságát ¨
ezt a zsugorodó hibaellipszoid jelzi
A hibaellipszoid tengelyhosszai jelzik a rendszer állapot bizonytalanságát n A becslés konvergenciája azt jelenti, hogy az ellipszoid minden irányban zsugorodik n
Kálmán-szűrő egyenletei Állapotegyenlet: xk+1 = Axk + Buk + wk Átviteli egyenlet: yk = Cxk + zk A, B, C mátrixok; k az időváltozó (index) x a rendszer állapota (közvetlenül nem mérhető) u a rendszer ismert bemenete y a mért kimenete w és z a zaj w állapot (folyamat) zaj z-t a mérési zaj
Kálmán-szűrő egyenletei Folyamat zaj kovariancia: S w = E ( wk wkT ) Mérési zaj kovariancia: S z = E ( z k z kT )
Kálmán-szűrő algoritmusa: -1
K k = APk C (CPk C + S z ) xˆ k +1 = ( Axˆ k + Bu k ) + K k ( yk +1 - Cxˆ k ) T
T
Pk +1 = APk A + S w - APk C T
T
-1 T S z CPk A
Állapot optimális becslése K mátrix: Kálmán-féle nyereségmátrix P mátrix: becslési hiba kovariancia mátrix
állapot terjedése
korrekció
xˆ k +1 = ( Axˆ k + Bu k ) + K k ( y k +1 - Cxˆ k )
K nyereségmátrix Ha nagy a mérési zaj, Sz is nagy lesz, tehát K kicsi, és így nem adunk nagy hitelt az y méréseinknek a következő állapot becslésénél. Viszont ha kicsi a mérési zaj, Sz is kicsi lesz, tehát K nagy, és így több hitelt adunk a méréseinknek a következő állapot becslésénél.
K k = APk C (CPk C + S z ) T
T
-1
P kovariancia terjedés
A kovariancia terjedés összefüggése: -1 z
Pk +1 = APk A + S w - APk C S CPk A T
T
T
Kálmán-szűrés
Kálmán-szűrő alkalmazásai • • • • • • • • • • • •
objektumkövetés, mesterséges látás dinamikus helymeghatározás gazdaságtan, makroökonómia inerciális navigáció pályameghatározás radar követés GNSS szeizmológia beszédjavítás időjárás előrejelzés navigációs rendszerek 3D modellezés
Példa: járműnavigáció n n n n n n
egyenes úton haladó jármű állapot vektor: x = [p , v] helyzet, sebesség input vektor u: vezérlő gyorsulások ~ sebesség változás: vk+1 = vk + Tuk + vk helyzet változás: pk+1 = pk + Tvk + ½T2uk + állapot átviteli és mérési egyenlet
éT 2 / 2ù é1 T ù x k +1 = ê xk + ê ú u k + wk ú êë T úû ë0 1 û y k = [1 0] xk + z k
~ pk
Példa: járműnavigáció n n
helyzet bizonytalanság: 10 méter véletlenszerű gyorsulások szórása 0,5 m/s2
Euler szimuláció function kalman(duration, dt) ## function kalman(duration, dt) - Kalman filter simulation ## duration = length of simulation (seconds) ## dt = step size (seconds) measnoise = 10; ## position measurement noise (meter) accelnoise = 0.5; ## acceleration noise (meter/sec^2) a = [1 dt; 0 1]; ## transition matrix c = [1 0]; ## measurement matrix x = [0; 0]; ## initial state vector xhat = x; ## initial state estimate Q = accelnoise^2 * [dt^4/4 dt^3/2; dt^3/2 dt^2]; ## process noise covariance P = Q; ## initial estimation covariance R = measnoise^2; ## measurement error covariance ## set up the size of the innovations vector Inn = zeros(size(R)); pos = []; ## true position array poshat = []; ## estimated position array posmeas = []; ## measured position array
Euler szimuláció Counter = 0; for t = 0 to duration step dt; Counter = Counter + 1; ## Simulate the process ProcessNoise = accelnoise * [(dt^2/2)*normal(1,1); dt*normal(1,1)]; x = a . x + ProcessNoise; ## Simulate the measurement MeasNoise = measnoise * normal(1,1); z = c . x + MeasNoise; ## Innovation Inn = z - c . xhat; ## Covariance of Innovation s = c . P . c' + R; ## Gain matrix K = a . P . c' . inv(s); ## State estimate xhat = a . xhat + K . Inn; ## Covariance of prediction error P = a . P . a' + Q - a . P . c' . inv(s) . c . P . a'; ## Save some parameters in vectors for plotting later pos = pos_x(1); posmeas = posmeas_z; poshat = poshat_xhat(1); end; ## Plot the results
Eredmények
piros : valódi helyzet kék : mért helyzet zöld : a Kálmán-szűrő által becsült helyzet
