Toegepaste Wiskunde 2: Het Kalman-filter

Transcriptie

1 Toegepaste Wiskunde 2: Het Kalman-filter 25 februari, 2008 Hans Maassen 1. Inleiding Het Kalman filter schat de toestand van een systeem op basis van een reeks, door ruis verstoorde waarnemingen. Een meer correcte benaming zou zijn: het lineaire Stratonovich- Kalman-Bucy filter. Het is zeer geschikt om electronica te programmeren die vliegende objecten in real time localiseert. Tegenwoordig wordt het filter ook ingezet om beurskoeren te voorspellen, en diverse andere, in de tijd veranderende grootheden. Het filter wordt gevoed met stochastische a priori aannamen over bijvoorbeeld plaats en snelheid van het te volgen object, en vertaalt deze na elke nieuwe waarneming in bijgestelde, a posteriori schattingen. De schatting op zeker tijdstip is de voorwaardelijke verwachting van de betreffende grootheid, gegeven alle observaties die tot dan toe zijn gedaan. De geschiedenis van het Kalmanfilter is een van de succesverhalen van de Toegepaste Wiskunde. Meteen na de ontwikkeling ervan door Kalman, op basis van ideeën van Stratonovich, is het door de NASA ingezet in het Apolloproject, dat ten doel had een mens op de maan te brengen. Without me no moon, placht Kalman te zeggen. 2. Voorwaardelijke Verwachting De voorwaardelijke kans op een gebeurtenis A, gegeven een gebeurtenis B is gedefinieerd als P(A B) P(A B) :=, P(B) mits P(B) 0. Dit leidt onmiddellijk tot de definitie van een voorwaardelijke verwachting van een discrete stochast X, gegeven een gebeurtenis B: zij W de waardenverzameling van X, dan is E(X B) := x W x P([X = x] B). 1 Modellenpracticum 2008: Kalmanfilter

2 En deze leidt weer tot de definitie van een voorwaardelijke verwachting van een discrete stochast X, gegeven een tweede discrete stochast Z: E(X Z) : Ω R : ω f(z(ω)), waarbij f(z) := E(X Z = z). (1) Merk op dat deze voorwaardelijke verwachting zelf een stochast is, helemaal vastgelegd door Z. We beweren nu dat dit de beste schatting is van X op basis van kennis over Z. Lemma 1. Laten X en Z discrete stochasten zijn, zeg met waardeverzamelingen W X en W Z. Dan is E(X Z) = f(z), waarbij f díe functie is van W Z naar R, waarvoor minimaal is. E ( (f(z) X) 2) Dit is een uitbreiding van het eenvoudige stellinkje dat zegt dat voor een willekeurige stochast X de waarde van λ R waarvoor E ( (λ X) 2) minimaal is gegeven wordt door de verwachting E(X). Immers: voor alle λ R geldt: E ( (λ X) 2) = λ 2 2λE(X) + E(X 2 ) = ( (λ E(X)) 2) + Var(X). Bewijs van het Lemma. Zij f : W Z R zó dat E(f(Z) 2 ) <. Dan geldt: E ( (f(z) X) 2) = ( P[Z = z] E (f(z) X) 2 Z = z ) z W Z = ( P[Z = z] f(z) 2 2f(z)E(X Z = z) + E(X 2 Z = z) ) z W Z = ( P[Z = z] (f(z) E(X Z = z)) 2 + E(X 2 Z = z) E(X Z = z) 2) z W Z = E ( (f(z) E(X Z)) 2) + E ( E(X 2 Z) E(X Z) 2). De tweede term hangt niet van f af, en de eerste is minimaal (namelijk 0) voor f(z) = E(X Z = z), dus f(z) = E(X Z). Voor continu verdeeld stochasten, waar we in het volgende voornamelijk in geïnteresseerd zullen zijn, gaat definitie (1) van E(X Z) niet meer op, omdat een gebeurtenis als [Z = z] kans 0 heeft, zodat je er niet op kunt conditioneren. De karakterisering als gegeven in het Lemma blijft echter zinvol, en we zullen deze daarom gaan gebruiken als definitie van E(X Z). Het zal handig blijken te zijn, meer dan één stochast te gebruiken als voorwaarde. Definitie. Laten X en Z 1, Z 2,..., Z n stochasten zijn met eindige variantie. Onder de voorwaardelijke verwachting E(X Z 1,..., Z n ) van X, gegeven Z 1, Z 2,, Z n verstaan we f(z 1, Z 2,, Z n ), waarbij (de meetbare functie) f : R n R zó gekozen is dat ( (X E f(z1, Z 2,, Z n ) ) 2) minimaal is. 2 Modellenpracticum 2008: Kalmanfilter

3 3. Gaussische stochastische variabelen Het Kalman-filter berekent voorwaardelijke verwachtingen in het heel speciale, maar belangrijke geval van normaal verdeelde stochasten. Het zal blijken dat bovenstaande definitie een mooie meetkundige interpretatie krijgt Een concreet model Zij N N. We maken een kansruimte (Ω N, Σ N, P N ) als volgt. Voor Ω N nemen we de N- dimensionale ruimte R N, en voor Σ N de standaard-sigma-algebra die daarbij hoort. (We maken ons nu niet druk over de maattheorie van R N, en doen alsof we precies weten wat meetbare verzamelingen zijn. Voor de kenners: we nemen de Borel-verzamelingen. Voor anderen: we beschouwen alleen verzamelingen waar we over kunnen integreren.) Voor P = P N kiezen we die kansmaat op R N die de coordinaten tot onafhankelijke normaalverdeelde stochasten maakt: voor A R N : P(A) := 1 (2π) n/2 A e 1 2 (x2 1 +x x2 N ) dx 1 dx 2 dx N. De stochastische variabelen die we zullen beschouwen zijn de lineaire functies Ω R, oftewel vectoren v in de duale ruimte Ω van Ω: X v : Ω R : x v, x. Deze keuze van stochasten heeft de volgende consequenties: 1. De stochast X v is normaal verdeeld, met verwachting 0 en variantie v Als v w, dan zijn X v en X w onafhankelijk: X v X w. Deze twee eigenschappen vatten we samen met: De stochasten X v, v Ω zijn gezamenlijk Gaussisch. Verdere gevolgen van onze constructie zijn: 3. De voorwaardelijke verwachting van X v, gegeven X z is X Pv, waarbij P de orthogonale projectie is op de lijn door z Voorbeeld Zij X een stochastische variabele met gemiddelde 0 en variantie a 2. Stel dat we X niet direct kunnen observeren, maar alleen met een normaal verdeelde waarnemingsfout W, die onafhankelijk is van X en die gemiddelde 0 en variantie m 2 heeft. We nemen dus de volgende stochast waar: Z := X + W. Wat is de beste schatting X van X, gebaseerd op onze waarneming van Z? Bewering. De beste schatting van X, gegeven Z is (zie figuur) X = E(XZ) E(Z 2 ) Z = a 2 a 2 + m 2 Z. 3 Modellenpracticum 2008: Kalmanfilter

4 Bewijs. We modelleren deze situatie met de kansruimte (Ω N, Σ N, P N ). Zij N = 2, X = X (a,0), W = X (o,m), zodat Z = X (a,m). De projectie van (a, 0) op de lijn door 0 en (a, m) laat zich in het plaatje gemakkelijk aflezen: p = a 2 (a, m). a 2 + m2 En dus is X := E(X Z) = X p = a 2 a 2 + m 2 Z. W = (0, m) Z = (a, m) X^ X = (a, 0) Je zou de factor a 2 /(a 2 + m 2 ) kunnen interpreteren als de geloofwaardigheid van de waarneming Z. Deze brengt tot uitdrukking brengt in hoeverre wij de afwijking van Z van de a priori schatting 0 toeschrijven aan het voorwerp zelf, en in hoeverre we ze afdoen als ruis. 4. Een Kalman-filter Neem aan dat de positie van een vliegend voorwerp ten tijde t gegeven wordt door X + tv, waarbij X en V onafhankelijke, normaal verdeelde stochasten zijn, zeg met varianties respectievelijk α 2 en β 2, en met verwachting 0. Het idee is, dat α en β heel groot zijn, zodat wij van plaats en snelheid van het voorwerp op tijdstip 0 niet veel weten. De aanname E(X) = E(V ) = 0 is hier enkel voor de eenvoud gemaakt. Nu gaan we waarnemingen doen op tijdstippen t 1, t 2,, t n ; op tijdstip t j meten we Z j := X + t j V + R j. De ruis R j is een rij onafhankelijke normaal verdeelde stochasten met verwachting 0 en variantie ε 2, onafhankelijk van X en V. We modelleren dit probleem met behulp van Paragraaf Modellenpracticum 2008: Kalmanfilter

5 Laat N = n + 2 en identificeer de volgende vectoren in Ω N probleem. X = (α, 0, 0, 0, 0,..., 0) V = (0, β, 0, 0, 0,..., 0) R 1 = (0, 0, ε, 0, 0,..., 0) R 2 = (0, 0, 0, ε, 0,..., 0). Z 1 = (α, t 1 β, ε, 0, 0,..., 0) Z 2 = (α, t 1 β, 0, ε, 0,..., 0). met de stochasten uit het Zij nu Z j de lineaire ruimte, opgespannen door Z 1, Z 2,..., Z j. Zij P j de orthogonale projectie van Ω op Z j. Dan is de beste schatting X j := E(X Z 1, Z 2,, Z j ) de projectie van X = X (α,0,0,...,0) op Z j : Xj = P j (X), en evenzo is V j = P j (V ). Op tijdstip 0 hebben we nog niets gezien, en schatten we X en V met hun verwachtingswaarden: X 0 = 0, V0 = 0. Op tijdstip t 1 zien we Z 1, en we updaten onze kennis over X en V : X 1 = c 1 Z 1, met c 1 zó dat X X 1 Z 1. Dus moet X c 1 Z 1, Z 1 = 0, zodat X, Z 1 = c 1 Z 1, Z 1, dat wil zeggen: α 2 = c 1 (α 2 + β 2 t ε 2 ). We vinden: α X 2 1 = α 2 + β 2 t Z ε2 1, en analoog: V 1 = β 2 t 2 1 α 2 + β 2 t ε2 Z 1. De volgende stap, na de waarneming van Z 2, is ingewikkelder dat de eerste, omdat Z 1 en Z 2 niet onafhankelijk (orthogonaal) zijn. Het is dan voordelig, in stap j 2 eerst het innovatieve of verrassende deel N j uit de nieuwe waarneming Z j te isoleren: N j := Z j P j 1 (Z j ). Deze innovaties N 1, N 2,, N n zijn onderling onafhankelijk, en, gezien als vecoren in Ω, orthogonaal. Hieruit volgt dat X j = X j 1 + E(XN j) E(N 2 j ) N j en Vj = V j 1 + E(V N j) E(N 2 j ) N j. (2) Op basis hiervan komen we tot het volgende resultaat 5 Modellenpracticum 2008: Kalmanfilter

6 Stelling 2. De beste schattingen van X en V, gegeven de eerste j waarnemingen worden gegeven door de volgende recursieve vergelijkingen. X j = X α 2 j 1 j t ( jγ j 1 α 2 j 1 + t2 j β2 j 1 + 2t jγ j 1 + ε 2 Z j ( ) ) Xj 1 + t j Vj 1 ; V j = V t j βj 1 2 j γ ( j 1 α 2 j 1 + t2 j β2 j 1 + 2t jγ j 1 + ε 2 Z j ( ) ) (3) Xj 1 + t j Vj 1, waarin de parameters α j, β j en γ j worden berekend met de recursie α 0 = α ; β 0 = β ; γ 0 = 0 ; α 2 j = α 2 j 1 β 2 j = β 2 j 1 (α 2 j 1 + t jγ j 1 ) 2 α 2 j 1 + t2 j β2 j 1 + 2t jγ j 1 + ε 2 ; (t j β 2 j 1 + γ j 1) 2 α 2 j 1 + t2 j β2 j 1 + 2t jγ j 1 + ε 2 ; γ j = γ j 1 (α2 j 1 + t jγ j 1 )(t j β 2 j 1 + γ j 1) α 2 j 1 + t2 j β2 j 1 + 2t jγ j 1 + ε 2. Bewijs. Definieer α 2 j := X X j 2 ; β 2 j := V V j 2 ; γ j := X X j, V V j. Dan geldt: E(XN j ) = X, (1 P j )Z j = X X j 1, (X X j 1 ) + t j (V V j 1 ) + R j = X X j t j X X j 1, V V j 1 = α 2 j 1 + t jγ j 1. E(Nj 2 ) = (X X j 1 ) + t j (V V j 1 ) + R j 2 = α 2 j 1 + t2 j β2 j 1 + 2t jγ j + ε 2. Met (2) volgt nu (3). De recursies voor α j, β j en γ j worden nu gevonden met α 2 j 1 α 2 j = X, (P j P j 1 )X = E(XN j) 2 E(N 2 j ) ; β 2 j 1 β 2 j = V, (P j P j 1 )V = E(V N j) 2 E(N 2 j ) ; γ j 1 γ j = X, (P j P j 1 )V = E(XN j)e(v N j ) E(N 2 j ). 6 Modellenpracticum 2008: Kalmanfilter