Generell informasjon om faget er tilgjengelig fra fagets nettside, og for øvinger brukes It s learning.

Transkript

1 Stavanger, 7. november 2016 Det teknisknaturvitenskapelige fakultet ELE620 Systemidentifikasjon, Generell informasjon om faget er tilgjengelig fra fagets nettside, og for øvinger brukes It s learning. Innhold 1 Tilstand- og parameterestimering med Kalman-filter Observerbarhet Ord og uttrykk Modell for prosessen Forutsetninger Utledning av Kalman-filter Forventningsrett estimator Kovariansmatriser Minimum varians estimator Oppsummering av Kalman-filter Kommentarer til Kalman-filteret Eksempel med rekursivt estimat av en konstant Tilstandsestimering med Matlab Augmentert Kalman-filter Eksempel med augmentert Kalman-filter Karl Skretting, Institutt for data- og elektroteknikk (IDE), Universitetet i Stavanger (UiS), 4036 Stavanger. Sentralbord Direkte E-post: karl.skretting@uis.no.

2 5 Ulineære system og utvidet Kalman-filter 19 6 Kombinert tilstands- og parameterestimering 22 7 Matrix Inversion Lemma 23 1 Tilstand- og parameterestimering med Kalman-filter Dette notatet inneholder en ganske grundig utledningen av Kalman-filteret og noen enkle eksempler på bruk. Notasjon (symboler) i dette notatet er med matriser Φ og Γ for diskret TRM (A og B for kontinuerlig TRM), merk at Matlab bruker latinske bokstaver også for matrisene i diskret TRM. Det finnes et utall varianter og implementeringer av Kalman-filteret. Det diskrete Kalman-filteret har størst praktisk anvendelse, også dette finnes i flere varianter. Vi skal kun se på det diskrete Kalman-filteret. Utledningen kan være ganske komplisert og omfattende hvis alle detaljer skal tas med i full dydbe. Derfor er det her forenklet noe. Utledningen er likevel lengre enn de fleste utledninger dere har vært borte i hittil, så det er viktig å dele den opp i flere deler og å holde oversikten. Hovedpunkta fram til oppsummeringen av resultatet er 1. En formell definisjon av observerbarhet, del 1.1 her. 2. Definisjon av noen uttrykk og oppsummering av notasjon kan være nyttig å ha på plass fra starten av. Det er gjort i del Komme fram til hensiktsmessig forenklet modell, del Betingelser for støy og initialtilstand klargjøres, del Ligningene for Kalman-filteret utledes, del Resultatet oppsummeres i ligningene som en må bruke for å finne optimalt estimat for hvert tidssteg, del Observerbarhet Observerbarhet er en egenskap ved et system, og som sådan burde denne delen kanskje heller vært med som en del 4.4 i det allerede ganske omfattende notat4. Men av praktiske (og historiske) grunner tas det heller med her. 2

3 Forenklet sagt betyr observerbarhet at det en måler må henge sammen med, eller må være avhengig av, de tilstandene en ønsker opplysninger om. Her er en mer formell forklaring av observerbarhet. Vi har et diskret lineært system på tilstandsromform x(k + 1) = Φx(k) + Γu(k) (1) y(k) = Dx(k) (2) En har her tatt bort direktekoblingsleddet Eu(k) fra måleligningen (2), dette leddet er ofte 0, altså E = 0 eller hvis ikke kan en la pådraget u(k) være null slik vi uansett gjør nedenfor. Matrisene er her ikke tidsvarierende og støyledd er ikke tatt med. Definisjon av observerbarhet: Kjennskap til u(0), u(1),... u(k 1) og y(0), y(1),... y(k 1) er nok til å bestemme initialtilstanden, x(0). Betingelse for observerbarhet kan finnes som y(0) = Dx(0) (3) y(1) = Dx(1) = DΦx(0) + DΓu(0) y(1) = DΦx(0) antar nå u(n) = 0 n = 0, 1,... (k 1) y(2) = DΦx(2) = DΦx(1) = DΦ 2 x(0) y(3) = DΦ 3 x(0) y(k 1) = DΦ k 1 x(0) (4) Dette kan kompakt skrives som D DΦ. DΦ k 1 x(0) = y(0) y(1). y(k 1) (5) (6) Q 0 x(0) = Y (7) x(0) er her initialtilstanden, som er en vektor med de n tilstandene. Antall tilstander er det samme som systemets orden. Ut fra ligningen over så ser vi at vi kan finne initialtilstanden hvis observerbarhetsmatrisen Q 0 har rang n, den kan ikke ha større rang siden den har n kolonner. Antall linjer i matrisa må da 3

4 være minst n, men kan være større. Hvis matrisa har n linjer, og n kolonner, og har rang n så trenger en ikke større matrise Q 0. Hvis rang ikke er n kan det være en trenger flere linjer kun hvis de første linjene er spesielle, normalt er det ikke slik. For kontinuerlige system, på tilstandsromform, er test for observerbarhet helt tilsvarende, bare at en har matrisa A i stedet for Φ i observerbarhetsmatrisen Q Ord og uttrykk En viktig setning som beskriver Kalman-filteret er: Kalman-filter er optimalt i den forstand at aposterioriestimeringsavvikets varians minimeres. Flere ord og uttrykk her trenger presisering og forklaring Aposteriori : etter måling, tilstand x(k) beregnet når (etter at) u(k) og y(k) er kjent, og tidligere verdier av disse der informasjonen gjerne er samlet i estimat av tilstanden x(k 1), er også kjent. Apriori : før måling. Det er alternativet til aposteriori. Tilstand x(k) beregnet (estimert) ut fra kun tidligere verdier u(k 1) og y(k 1) og estimat av forrige tilstand x(k 1). Estimeringsavvik er differanse mellom sann tilstand x(k), som forøvrig ikke er kjent, og aposteriori-estimatet som her skrives med hatt ˆx(k). Estimeringsavviket skrive med tilde: x(k) = x(k) ˆx(k). Variansen minimeres betyr at varians, E[( x(k) E[ x(k)]) 2 ], får en minimal verdi, den er jo alltid positiv, for hvert tidssteg. Forventningsverdi for feilen er forøvrig lik 0 for hvert tidssteg, altså E[ x(k)] = 0. Merk at selv om vi ikke (aldri) kjenner virkelig tilstand, og dermed heller ikke (aldri) kjenner virkelig estimeringsavvik så kan vi likevel beregne statistiske egenskaper. Liten varians betyr at virkelig feil får liten verdi. Kalman-filter er en algoritme som kan implementere en tilstandsestimator, et adaptivt filter eller begge deler (samtidig). 4

5 Vi vil i utledningen av Kalman-filteret bruke tilstandsvektoren og ulike estimat av denne, en liten oppsummering kan derfor være på sin plass: x(k) er den sanne tilstandsvektoren ved steg k. Denne er i prinsippet aldri kjent, og er den sanne verdien for tilstanden i systemet. Hvis en simulerer systemet så er dette den simulerte verdien. ˆx(k) er aposteriori tilstandsestimatet, det vil si estimatet etter at en har tatt hensyn til målingene i steg k. x(k) er apriori tilstandsestimatet ved steg k, det er estimat før det er tatt hensyn til målingene i steg k. x(k) = x(k) ˆx(k) er estimeringsavvik ved steg k. Merk at det er aposterioriestimatet som brukes her. Vi har ikke noe eget symbol for apriori estimeringsavviket, x(k) x(k), selv om det også brukes, for eksempel i ligningene 26 og 28. La oss også oppsummere dimensjonene for vektorene og matrisene, og vi tar også med noen matriser som defineres senere for kompletthetens skyld. x(k) n 1 Φ n n K(k) n l u(k) s 1 Γ n s K(k) n n y(k) l 1 D l n ˆP (k) n n v(k) n 1 Q n n P (k) n n w(k) l 1 R l l 1.3 Modell for prosessen Utgangspunktet her er prosessligningene for diskret tilstandsrommodell x(k + 1) = Φ(k)x(k) + Γ(k)u(k) + Ω(k)v(k), (8) y(k) = D(k)x(k) + w(k). (9) En har alt her tatt bort direktekoblingsleddet Eu(k) fra måleligningen (9), leddet er av og til med i den lineære tilstandsrommodellen men er ofte utelatt siden modellen alltid kan lages med E = 0 ved å ta med flere tilstander. Nå forenkles notasjonen noe ved at vi utelater (k) for matrisene, det gir en forenkling i notasjonen uten at det påvirker resultatet noe. Men matrisene tillates likevel å være tidsvarierende. I praksis er de ofte konstante eller langsomt varierende. Støyleddet for signalet forenkles også noe, en legger støyen direkte til prosessen i stedet for å la den gå gjennom matrisa Ω, dette har heller ikke 5

6 større betydning. Vi ser også bort fra pådraget i denne utledningen, det gjør det hele en del enklere uten at resultatet endres. En kan argumentere for dette ved å splitte tilstandsvektoren i to deler, en deterministisk del og en stokastisk del, x(k) = x d (k) + x s (k). Prosessen her blir da: x(k + 1) = Φx(k) + v(k), (10) y(k) = Dx(k) + w(k). (11) 1.4 Forutsetninger Kalman-filter bygger på en del forutsetninger for å kunne si at det er optimalt Prosessen er påvirket av tilfeldig (stokastisk) prosesstøy. Stokastisk målestøy. Hvit initialtilstanden. Vi antar at støyen, v(k) og w(k), har forventningsverdi (middelverdi) null E[v(k)] = 0, E[w(k)] = 0, (12) og at sekvensene v(k) og w(k) er ukorrelerte med hverandre (R vw (τ, k) er en n l matrise) E[v(k + τ)w T (k)] = R vw (τ, k) = 0, (13) og støyen er hvit, det vil si uavhengig av tidligere verdier, E[v(k + τ)v T (k)] = R v (τ, k) = δ(τ)r v (0, k) = Q(k) = Q, (14) E[w(k + τ)w T (k)] = R w (τ, k) = δ(τ)r w (0, k) = R(k) = R. (15) Q og R er da autokovariansmatriser for prosesstøy v(k) og målestøy w(k) henholdsvis. Vi skal også ha at initialtilstanden x(0) skal være en hvit stokastisk variabel med forventningsverdi E[x(0)] = m 0. 2 Utledning av Kalman-filter Målet med utledningen er å finne en forventningsrett minimumvarians estimator for tilstanden, og vi ønsker at denne estimatoren skal være lineær og basere seg på (estimat av) forrige tilstand og nye målinger. ˆx(k) = K(k) ˆx(k 1) + K(k) y(k) (16) 6

7 Merk at K(k) og K(k) føreløpig bare er to matriser og at de inneholder de tidsvarierende koeffisientene vi ønsker å bruke i estimatoren. Det er disse matrisene vi ønsker å finne i utledningen nedenfor. 2.1 Forventningsrett estimator Siden vi ønsker at estimatoren i ligning (16) skal være forventningsrett så skal vi ha forventning for estimeringsavvik, x(k) = x(k) ˆx(k), lik null: E[x(k) ˆx(k)] = E[ x(k)] = 0 (17) Setter inn for ˆx(k) og videre for x(k) og y(k), vi har også et sted at vi legger til ett uttrykk av type +a a. x(k) = x(k) ˆx(k) (18) = x(k) K(k)ˆx(k 1) K(k)y(k) = Φx(k 1) + v(k 1) K(k)ˆx(k 1) K(k) [Dx(k) + w(k)] = Φx(k 1) + v(k 1) K(k)ˆx(k 1) K(k) [D(Φx(k 1) + v(k 1)) + w(k)] + K(k)x(k 1) K(k)x(k 1) = K(k) [x(k 1) ˆx(k 1)] + [Φ K(k)DΦ K(k)] x(k 1) + [I K(k)D] v(k 1) K(k)w(k) (19) Når vi nå tar forventningen av likningen over og har at denne skal være null så får vi E[ x(k)] = K(k)E[ x(k 1)] 0 = 0 + [Φ K(k)DΦ K(k)] E[x(k 1)] + [I K(k)D] E[v(k 1)] K(k)E[w(k)] = 0 + [Φ K(k)DΦ K(k)] E[x(k 1)] = 0 (20) som fører til og dette betyr at Φ K(k)DΦ K(k) = 0 (21) K(k) = (I K(k)D) Φ. (22) 7

8 Innsatt i estimatoren (16) gir dette ˆx(k) = [I K(k)D]Φˆx(k 1) + K(k)y(k) = Φˆx(k 1) + K(k) [y(k) DΦˆx(k 1)] = x(k) + K(k) [y(k) Dx(k)] (23) der vi har definert apriori tilstandsestimatet som x(k) = Φˆx(k 1) (24) Og en ser at estimatoren nå er en ekstrapolert tilstand pluss en korrigering basert på differansen mellom virkelig måling og estimert måling. Vi har igjen å finne verdien for K(k), denne finner vi ved å minimalisere forventningsverdien av summen av kvadratene av estimeringsavvikene E[ x 2 1(k) + x 2 2(k) + + x 2 n(k)] = E[ x T (k) x(k)] = tr ( ˆP (k) ) (25) der x-ene med subskript på venstre side er de ulike tilstandene, mens x-en uten subskript på høyre side er tilstandsvektoren. Merk at x T (k) x(k) = tr( x(k) x T (k)). Vi starter med å definere noen flere matriser, og så finner vi et uttrykk som minimeres ved å sette den deriverte til null. 2.2 Kovariansmatriser Kovariansmatrisa ˆP (k) ble alt brukt (25). Vi skal her definere kovariansmatrisene for estimeringsavvikene og skrive disse uttrykkene på en hensiktsmessig måte. Vi er interessert i kovariansmatrisa til apriori estimeringsavviket x(k) x(k) som vi kaller P (k) og kovariansmatrisa til aposteriori estimeringsavviket x(k) ˆx(k) som vi kaller ˆP (k). Hvis en snakker om bare kovariansmatrisa P (k), som i forbindelse med Kalman-filter strengt tatt er litt unøyaktig, er det helst ˆP (k) en mener. Definisjonen for kovariansmatrisene er P (k) = E[(x(k) x(k))(x(k) x(k)) T ] (26) ˆP (k) = E[(x(k) ˆx(k))(x(k) ˆx(k)) T ] (27) Med x(k) = x(k) ˆx(k) har en at ˆP (k) i (27) er som i (25). Vi skal nå finne noen alternative uttrykk for kovariansmatrisene og starter med P (k), som jo er kovariansmatrisa til apriori estimeringsavviket x(k) x(k). Ut fra (24) og (10) og kan vi skrive. x(k) x(k) = Φx(k 1) + v(k 1) Φˆx(k 1) = Φ [x(k 1) ˆx(k 1)] + v(k 1) (28) 8

9 Vi bruker dette videre P (k) = E[(x(k) x(k))(x(k) x(k)) T ] (29) = E[(Φ[x(k 1) ˆx(k 1)] + v(k 1))(Φ[x(k 1) ˆx(k 1)] + v(k 1)) T ] = ΦE[(x(k 1) ˆx(k 1))(x(k 1) ˆx(k 1)) T ]Φ T + E[v(k 1)v T (k 1)] = Φ ˆP (k 1)Φ T + Q (30) Der vi har brukt at x(k) x(k) har kovariansematrise P (k) x(k 1) ˆx(k 1) har kovariansematrise ˆP (k 1) og v(k 1) har kovariansematrise Q(k 1) eller bare Q. Videre ser vi på aposteriori estimeringsavviket x(k) ˆx(k), dette ble i (18) skrevet x(k). Fra estimatoren (23) har vi at ˆx(k) = x(k)+k(k)[y(k) Dx(k)]. Dette gir for aposteriori estimeringsavviket x(k) = x(k) x(k) K(k)[y(k) Dx(k)] For kovariansmatrisa har vi = x(k) [I K(k)D]x(k) K(k)y(k) = x(k) [I K(k)D]x(k) K(k)[Dx(k) + w(k)] = x(k) x(k) + K(k)Dx(k) K(k)Dx(k) K(k)w(k) = [x(k) x(k)] K(k)D[x(k) x(k)] K(k)w(k) = (I K(k)D)[x(k) x(k)] K(k)w(k) (31) ˆP (k) = E[(x(k) ˆx(k))(x(k) ˆx(k)) T ] (32) = E[((I K(k)D)[x(k) x(k)] K(k)w(k)) ((I K(k)D)[x(k) x(k)] K(k)w(k)) T ] = (I K(k)D) E[(x(k) x(k))(x(k) x(k)) T ] (I K(k)D) T (I K(k)D) E[(x(k) x(k))w(k) T ] K(k) T K(k) E[w(k)(x(k) x(k)) T ] (I K(k)D) T +K(k) E[w(k)w(k) T ] K(k) T = (I K(k)D) P (k) (I K(k)D) T + K(k) R K(k) T (33) 2.3 Minimum varians estimator Neste steg blir da å finne K(k) slik forventningsverdien av summen av kvadratene av estimeringsavvikene minimeres. Dette er sum av diagonalelementene i ˆP (k), altså tr( ˆP (k)), se ligning (25). Vi skal minimere varians for estimeringsavviket, altså er oppgaven å finne et uttrykk for K(k) som minimerer tr( ˆP (k)). 9

10 ˆP (k) = (I K(k)D) P (k) (I K(k)D) T + K(k) R K(k) T (34) = (I K(k)D) P (k) (I D T K(k) T ) + K(k) R K(k) T = P (k) K(k)D P (k) P (k) D T K(k) T +K(k)D P (k) D T K(k) T + K(k) R K(k) T tr( ˆP (k)) = tr(p (k)) 2 tr(k(k)d P (k)) (35) + tr(k(k)d P (k) D T K(k) T ) + tr(k(k) R K(k) T ) Husk at tr( ) er sum av diagonalelementene og dermed har en generelt tr(a + B + C) = tr(a) + tr(b) + tr(c). Vi har også brukt identiteten (AB T C T ) T = CBA T og dermed tr(ab T C T ) = tr(cba T ), her med A = P (k), B = D, og C = K(k). Husk også at kovariansmatrisene er symmetriske, ˆP (k) = ˆP T (k) og P (k) = P T (k). For derivasjon av matriser har vi (med B symmetrisk) tr(aba T ) A tr(ac) A Vi deriverer nå tr( ˆP (k)) med hensyn på K(k). Dette gir = 2AB, (36) = C T (37) tr( ˆP (k)) K(k) = 2P (k)d T + 2K(k)DP (k)d T + 2K(k)R. (38) Dette skal bli null når en har minimum for tr( ˆP (k)) 2P (k)d T + 2K(k)DP (k)d T + 2K(k)R = 0 (39) K(k) (DP (k)d T + R) = P (k)d T (40) K(k) = P (k)d T (DP (k)d T + R) 1. (41) Fra (33) har vi ˆP (k) som vi gjentar i (42). Når vi, midlertidig, forenkler notasjonen skrives dette som i (43). Overgangen fra (43) til (44) er forklart på slutten av dette dokumentet, ligningene (100) - (113). ˆP (k) = (I K(k)D) P (k) (I K(k)D) T + K(k) R K T (k) (42) = (I KD)P (I KD) T + KRK T (43). regne, regne, med mye Matrise Algebra = (I KD) P (44) = (I K(k)D) P (k) (45) En kan gjerne merke seg at (42) er mer numerisk robust enn (45) og derfor ofte blir brukt i stedet for den enklere formen. 10

11 3 Oppsummering av Kalman-filter Når vi nå summerer opp det vi har kommet fram til får vi den rekursive algoritmen som gitt i ligningene (46)-(50). Her har en før tidsteg k alle verdier for tidsteg (k 1) kjent, det vil spesielt si ˆx(k 1) og ˆP (k 1). Ved tidsteg k får en så målingen y(k) og nytt pådrag u(k) og kan gjennomføre beregningene i ligningene (46)-(50). x(k) = Φˆx(k 1) + Γu(k 1) (46) P (k) = Φ ˆP (k 1)Φ T + Q (47) K(k) = P (k)d T (DP (k)d T + R) 1 (48) ˆx(k) = x(k) + K(k)[y(k) Dx(k)] (49) ˆP (k) = (I K(k)D) P (k) (50) 3.1 Kommentarer til Kalman-filteret Rekkefølgen ovenfor kan gjerne være annerledes, for eksempel (47), (48), (50), (46) og (49). Hvis en ønsker å finne ny tilstand så rask som råd etter ny måling kan en regne ut ligningene (46)-(48) og (50), ved tidssteg (k 1). Her kunne en gjerne tatt med en del flere kommetarer, men de finnes andre steder. 3.2 Eksempel med rekursivt estimat av en konstant En har nå ingen pådrag, prosessen er konstant og inneholder en tilstand (det er konstanten) som måles i hvert tidsssteg. Det er målestøy men ikke prosesstøy. Modellen er da x(k + 1) = x(k) (51) y(k) = x(k) + w(k) (52) En har D = 1, Φ = 1, E[w(k)] = 0, og R = R(0), der R(l) = E[w(k + l)w T (k)] = σ 2 δ l. En starter ved tidssteg k = 0 og måler da verdien y(0). Nå settes initialverdiene for Kalman-filteret, naturlig nok velges ˆx(0) = y(0), men ˆP (0) er kanskje litt vanskeligere å sette, det skal være kovariansmatrisa for (aposteriori) avviket, altså differansen mellom virkelig verdi og estimatet, dermed er det variansen for målefeilen som skal brukes ˆP (0) = σ 2. 11

12 v w x s u Reg. Pros. y + Sa. Sa. u(k) Γ Kalman-filter y(k) + x(k + 1) z 1 x(k) ȳ(k) D - + Φ + K ˆx(k) e(k) Figur 1: Prosess med Kalman-filter. [Reg.] er en regulator der input er et settpunkt x s, altså ønska verdier for en eller flere tilstander, og de estimerte tilstandene ˆx(k). Prosessen har pådrag u og måling y som begge samples og går inn til Kalman-filter. Her er figuren tgnet som om forsterkningsfaktoren K er konstant (stasjonært KF), men generelt er K en faktor som beregnes på ny for hvert steg k. Når K beregnes brukes (og finnes) også kovariansmatrisene P (k) og ˆP (k), der spesielt aposteriorimatrisa er interessant i seg selv siden den gir et estimat over usikkerheten i tilstandsestimatene. For en ulineær systemmodell (EKF) vil også Φ og eventuelt også D være tidsvarierende og beregnes på ny i hvert tidssteg, som en linearisering omkring arbeidspunktet. Denne figuren er basert på figurer 9.12, 9.14 og 9.27 i Finn Haugens blå bok. 12

13 For neste tidssteg startes Kalman-filteret, for k = 1 får vi fra ligningene (46) til (50) x(1) = Φˆx(0) = ˆx(0) = y(0) (53) P (1) = ˆP (0) = σ 2 (54) K(1) = σ 2 (σ 2 + σ 2 ) 1 = 1/2 (55) ˆx(1) = x(1) + K(1)[y(1) Dx(1)] (56) = y(0) + 1 y(0) + y(1) [y(1) y(0)] = 2 2 (57) ˆP (1) = (1 1/2) P (1) = 1 2 σ2 (58) og så for neste k, (k = 2), får vi x(2) = ˆx(1) = 1 (y(0) + y(1)) (59) 2 P (2) = ˆP (1) = 1 2 σ2 (60) K(2) = 1 2 σ2 ( 1 2 σ2 + σ 2 ) 1 = 1/2 3/2 = 1 3 (61) ˆx(2) = x(2) + K(2)[y(2) Dx(2)] (62) = 1 2 (y(0) + y(1)) + 1 ( y(0) + y(1) ) y(2) 3 2 (63) = y(0) 2 + y(1) 2 + y(2) 3 y(0) 6 y(1) 6 (64) y(0) + y(1) + y(2) = 3 (65) ˆP (2) = (1 1/3) 1 2 σ2 = 1 3 σ2 (66) og så videre for en generell k ( 2) har vi x(k) = ˆx(k 1) (67) P (k) = ˆP (k 1) (68) K(k) = P (k)/(p (k) + σ 2 ) = P (k) P (k) + σ 2 (69) ˆx(k) = x(k) + K(k)[y(k) Dx(k)] (70) = (1 K(k))x(k) + K(k)y(k) (71) = σ 2 ˆx(k 1) + P (k) y(k) (72) P (k) + σ2 P (k) + σ2 ˆP (k) = (1 K(k))P (k) = σ2 P (k) P (k) + σ 2 (73) 13

14 Med induksjon og utgangspunktet at ˆP (k) = σ2 k+1 vises at dette gjelder generelt. for k = 1 (og for k = 2) så ˆP (k) = = σ2 P (k) P (k) + σ 2 = σ2 ˆP (k 1) ˆP (k 1) + σ 2 (74) σ2 σ2 k σ 2 k + σ2 = = σ2 k + 1 Altså gjelder det for alle k. Vi får da for K(k) og for ˆx(k) K(k) = 1 k σ4 k+1 k σ2 (75) P (k) 1 P (k) + σ = k σ2 2 1 k σ2 + σ = 1 2 k + 1 (76) (77) ˆx(k) = ˆx(k 1) + 1 [y(k) ˆx(k 1)] = k 1 ˆx(k 1) + k + 1 k + 1 k + 1 y(k) = 1 k y(n), (78) k + 1 n=0 der også siste overgang kan vises med induksjon. Resultatet av Kalman-filter skulle ikke være overraskende ut fra det en ellers vet om estimering av en fast verdi ut fra flere likeverdige målinger. Det fine med Kalman-filter, både her og generelt, er at en kan beregne et estimat etter hver måling, og at estimatet er optimalt etter hver måling. 3.3 Tilstandsestimering med Matlab Matlab-funksjonen kalman gir et kontinuerlig Kalman-filter for et kontinuerlig system og et diskret Kalman-filter for et diskret system. En har også funksjonen kalmd som gir et diskret Kalman-filter for et kontinuerlig system. Merk at både kalman og kalmd beregner den stasjonære løsningen, det vil si at matrisene Φ, Ω, D, Q og R antas å være konstante. Resultatet blir altså ikke et adaptivt filter, men et stasjonært filter. Se dokumentasjonen for kalman, dlqe og kalmd for hvordan funksjonen brukes. En må, som tidligere nevnt, merke seg at notasjon i Matlab og notasjon her ikke stemmer helt overens. Matlab bruker andre navn på matrisene, og betegnelsene for støyen er byttet om, w og v. I nyere versjoner av Matlab (fra R2011a eller litt før) er det kommet med adativt Kalmanfilter i DSP-toolboxen (filterdesign), se adaptkalman og initkalman. Jeg har også laget en funksjon, kalmanfilter.m, som er nært knyttet opp til ligningene slik de står i dette notatet. 14

15 Et eksempel viser gjerne best hvordan Kalman-filter kan brukes i Matlab. Et tilsvarende eksempel er i ADC, ex side , her er også med hvordan det kan gjøres i Matlab. En har gitt følgende diskrete system: x(k + 1) = Φx(k) + Γu(k) + Ωv(k) (79) y(k) = Dx(k) + w(k) (80) der Φ = [ 1 0, , 8187 ] [ 0, 0187, Γ = 0, 1813 ] [ 1 0, Ω = 0 0, 2 En har videre D = [1 0] og prosesstøyen v er hvit med forventningsverdi 0 og kovariansmatrise [ ] 0, Q =. 0 0, 04 Også målestøyen, w, er hvit med forventningsverdi 0 og den har kovariansmatrise R = [0.0025]. Tidssteget T skal være 0.2 sekund. Apriori-estimatet settes lik x(0) = [0 0] T. Initialtilstanden i den virkelige (simulerte) prosessen er imidlertid x(0) = [0 1] T. Pådrag settes til et sprang (ved tid k = 0) med høyde 2, altså u(k) = 2. ]. v 1 v 2 w u x 1 x 2 x 1 y + Figur 2: Blokkdiagram for systemet slik en gjerne tegner det i utgangspunktet. I Matlab kan systemet defineres med nx=2; nu=1; ny=1; nv=nx; % nw=ny; er underforstått i modellen, bruker bare ny nedenfor Phi=[1, ; 0, ]; % Phi er nx x nx Gamma=[0.0187; ]; % Gamma er nx x nu Omega=[1,0; 0,0.2]; % Omega er nx x nv D=[1,0]; % D er ny x nx % kovarianser for støy 15

16 Q=[0.0001, 0; 0, 0.04]; % Q er nv x nv R=0.0025; % R er ny x ny T = 0.2; % tidssteget sys = ss(phi,[gamma, Omega], D, [0,0,0], T,... StateName,{ x1, x2 },... InputName,{ u, v_1, v_2 },... OutputName,{ y },... % måling uten målestøy!! Notes, Eksempel 9.9 fra Haugens lærebok. ); [kest, L, P_strek, Ks, P_hatt] = kalman(sys,q,r); [Kh,P_apr,P_apo,E] = dlqe(phi,omega,d,q,r); Det er flere måter å gjøre dette på, ovenfor har jeg brukt både kalman og dlqe. Resultatene av de to funksjonene som brukt over blir det samme, men kalman returnerer også estimatoren som et ss-objekt. w Q u v 1 v 2 x 1 x 2 x 1 y + Figur 3: Blokkdiagram systemet slik en har det for beregning av stasjonært Kalman-filter, sys brukt i funksjonen kalman i Matlab. Merk at både pådrag og prosesstøy settes som pådrag til systemet og størrelsen for prosesstøymatrisa Q i funksjonen kalman avgjør hvor mange av disse som betraktes som støy, resten blir kjente pådrag. For kalman ovenfor har vi brukt også støyen som pådrag, etter det jeg har testet meg fram til er det slik det må være. Dokumentasjonen for kalman funksjonen sier at systemet må være en state-space model with matrices: A, [B,G], C, [D,H]. Det betyr at for systemet så skiller en ikke på støy og pådrag, begge deler betraktes som pådrag. Variansmatrisa, Q eller Qn, angir da varians (for støy som legges til) de siste pådragene, med tre pådrag som i systemet her så kan altså andre argument i kalman funksjonen være skalar, 1 1, og da angi varians for siste pådrag, 2 2, og da angi varians for de to siste pådrag (som her er støyen), eller 3 3 med varians for alle pådrag. Når en skal simulere systemet, inkludert Kalman-estimatoren, med CST så er det noe mer komplisert. Et eksempel er i vedlagte m-fil eks9p9.m. Det vil være nyttig å tegne opp systemene som lages med CST i denne m-fila, spesielt med vekt på innganger og utganger og hvordan systot dannes, dette kan gjøres på tavla i en time. Figurene 4, 5 og 6 viser system for simulering, estimator, og totalsystem slik det kan gjøres i CST. 16

17 u v 1 v 2 w x 1 x 2 y (= x 1 ) y noise Figur 4: Blokkdiagram for simuleringssytemet syssim. Dette er et deterministisk system uten støy. Output y noise er måling y = x 1 pluss målestøyen w som her betraktes som inngang. u y x 1e x 2e x 1e x 2e y e Figur 5: Blokkdiagram for Kalman-estimatoren kest. Dette er et deterministisk system uten støy. v 1 v 2 w u x 1 x 2 x 1e x 2e y ye x 1e x 2e Figur 6: Blokkdiagram for totalsytemet systot. Her har en først parallellkoblet systemene syssim og kest, med systot = parallel(syssim, kest, 1, 1, [], []);. Inngangene u ble samtidig koblet sammen, gitt av tredje og fjerde argument. Alt det andre fra begge systemene tas med i nytt system, for eksempel får det nye systemet fire tilstander, fem innganger og fem utganger. Videre brukes feedback-funksjonene for å koble utgang til inngang, med systot = feedback(systot, 1, 5, 2, 1);. Tilbakekoblingssystemet er her bare 1 (andre argument), så er det gitt at inngang nummer 5 får tilbakekobling (y), og utgang nummer 2 (ynoise) brukes, siste argument 1 angir positiv tilbakekobling. Tilslutt fjernes utgang 2 og inngang 5, men tilbakekoblingen beholdes, med systot = systot([ ], [ ]);. 17

18 4 Augmentert Kalman-filter Utgangspunktet er system gitt av følgende ligninger x(k + 1) = Φ(k)x(k) + Γ(k)u(k) + Ω(k)v(k), y(k) = D(k)x(k) + w(k). (81) Kalman-filter ble utledet under flere forutsetninger, blant annet: E[v(k)] = 0 og E[w(k)] = 0. Ofte kan en ha E[v(k)] = m v (k) = m v 0. Hvis m v er kjent er dette greitt nok, alt som endres er beregning av x(k) eller x(k + 1), som x(k + 1) = Φ(k)ˆx(k) + Γ(k)u(k) + Ω(k)m v (k). (82) Tilsvarende hvis en kjenner E[w(k)] = m w (k) = m w 0, da endrer en måleestimatet: y(k) = D(k)x(k) + m w (k). (83) Hvis en ikke kjenner m v (k) men vet at verdien er ulik null, hva så. Løsningen er da et augmentert Kalman-filter. En utvider systemet med flere tilstander, for eksempel m v (k). Denne tilstanden estimeres nå samtidig med de opprinnelige tilstandene. 4.1 Eksempel med augmentert Kalman-filter Eksempelet gjøres ganske grundig, og vi har også med Matlab m-fila som lager figurene eks9p10.m. Systemet er som eksempelet i del 3.3 men en har her at middelverdien for prosesstøy for tilstand to, m v2, er ukjent og ulik null men antas å være konstant. Vi har fortsatt at E[v 1 ] = m v1 = 0 slik som Kalman-filter forutsetter. Men for å få med m v2 må vi augmentere Kalman-filteret fra eksempelet i del 3.3 med en ekstra tilstand, x 3 = m v2. For at denne skal ha anledning til å gå mot sin riktige verdi må vi ha at det er en tidsvarierende tilstand som påvirkes av litt støy, ny prosesstøy v 3. Ligningen for denne tilstanden er da x 3 (k + 1) = x 3 (k) + v 3 (k). (84) Noe vilkårlig antar vi at variansen til v 3 (k) er q 3 = v 2 erstattes nå med en ny støy, som vi også (kanskje noe forvirrende) kaller v 2, og som nå har null i middelverdi og samme varians som før q 2 = For v 1 har vi varians som før, q 1 = Nå får vi den augmenterte modellen x 1 (k + 1) = x 1 (k) x 2 (k) u(k) + v 1 (k) (85) x 2 (k + 1) = x 2 (k) + 0.2x 3 (k) u(k) + 0.2v 2 (k) (86) x 3 (k + 1) = x 3 (k) + v 3 (k) (87) y(k) = x 1 (k) + w(k) (88) 18

19 Vi kan nå bruke Kalman-filter på det augmenterte systemet. Jeg har laget Matlab-fila eks9p10.m som starter med å definere systemet tilsvarende som i eksempelet i del 3.3. Simulering kjøres på dette systemet, med middelverdi for prosesstøyen v 2 på 1, altså m v2 = 1, og pådrag lik u(k) = 2. Systemet som estimeres er som gitt % systemet som estimeres nx=3; nu=1; ny=1; nv=nx; % nw=ny; er underforstått i modellen, bruker bare ny nedenfor Phi=[1, , 0; 0, , 0.2; 0,0,1]; % Phi er nx x nx Gamma=[0.0187; ; 0]; % Gamma er nx x nu Omega=[1,0,0; 0,0.2,0; 0,0,1]; % Omega er nx x nv D=[1,0,0]; % D er ny x nx % kovarianser for støy Q=diag([0.0001, 0.04, ]); % Q er nv x nv R=0.0025; % R er ny x ny Estimering kjøres med et adaptivt Kalman-filter. Siden φ, Ω, D, Q, og R ikke er tidsvarierende så vil Kalman-forsterkningene gå mot stasjonære verdier, K s = [0, , , 1567]. Noen resultater viser i figurene 7 og 8. 5 Ulineære system og utvidet Kalman-filter Vi skal her se litt på hvordan en kan gjøre (lineær) tilstandsestimering når en har ulineære system. Den vanligste måte å gjøre tilstandsestimering på i et ulineært system er å linearisere systemet omkring et fast eller et flytende arbeidspunkt. Φ og D matrisene finnes ved linearisering. Et fast arbeidspunkt er enklest, men det kan være noe upresist spesielt hvis virkelig arbeidspunkt avviker en del fra det valgte. Et løpende arbeidspunkt er mer presist, men beregningsmessig mer krevende. Estimatorforsterkningen K, og kovariansmatrisene P og ˆP, beregnes så ut fra den lineære modellen. Tilstandsestimatene og forventet målinger, x og y, beregnes imidlertid ut fra den ulineære modellen. Matematisk modellering, ut fra fysisk system, er viktig og ofte både komplisert og krevende, og gir som regel ulineære system. Parametrisk modellering (som en gjerne bruker i systemidentifikasjon) finner ofte en lineær modell direkte. Kalman-filteret som oppsummert i del 3 kan utvides til ulineære systemer. Det kalles da gjerne også Extended Kalman-Filter (EKF) eller Kalman-Schmidt- 19

20 4 x 2 virkelig verdi (hel) og estimert (stiplet) Tid (sek), med T=0.2 (sek) 2 x 3 virkelig verdi (hel) og estimert (stiplet) Tid (sek), med T=0.2 (sek) Figur 7: Estimatene for x 2 og x 3 i eksempel med augmentert Kalman-filter. 6 k 1 (blaa, hel), k 2 (rod, stiplet) og k 3 (gronn, hel) Tid (sek), med T=0.2 (sek) Figur 8: Kalman-forsterkningene for den adaptive estimatoren i eksempel med augmentert Kalman-filter. 20

21 filter. Det ulineære systemet er nå gitt av følgende modell x(k + 1) = f[x(k), u(k),...] + v(k), (89) y(k) = g[x(k),...] + w(k). (90) Legg merke til at Ω(k) som en kunne hatt med i ligning 89 ikke er med. Her har en altså Ω(k) = I n. Som over har en at før tidsteg k er alle verdier for tidsteg (k 1) kjent, det vil spesielt si ˆx(k 1) og ˆP (k 1). Oppsummeringa for utvidet Kalman-filter blir x(k) = f[ˆx(k 1), u(k 1),...] (91) Φ(k) = f( ) x ˆx(k 1),u(k 1) (92) P (k) = Φ(k) ˆP (k 1)Φ T (k) + Q (93) D(k) = g( ) x x(k) (94) K(k) = P (k)d T (k) (D(k)P (k)d T (k) + R) 1 (95) y(k) = g[x(k),...] (96) ˆx(k) = x(k) + K(k)[y(k) y(k)] (97) ˆP (k) = (I K(k)D(k)) P (k) (98) Ved implementering er det vanlig å ikke lagre verdiene for hvert tidssteg, altså for tidssteg k skriver en over gammel verdi, fra tidssteg (k 1). Dette kan gjøres for alle symboler til venstre for likhetstegnet, kanskje med unntak for ˆx(k). Merk at EKF ikke er en optimal estimator for det ulineære systemet. 21

22 6 Kombinert tilstands- og parameterestimering Det er ofte nødvendig å estimere både ukjente eller tidsvarierende tilstander i systemet ukjente eller tidsvarierende parametre i systemet. Det er parametre som er vanskelige å finne eller å måle direkte (på forhånd). De ukjente parametrene kan betraktes som tilstander! Da får en typisk en augmentert ulineær modell. Det enkleste og vanligste er at parameteren betraktes som en ukjent konstant, det blir da (nesten) som middelverdien for støy i eksempel i del 4.1. For en langsomt varierende (konstant) parameter modelleres denne med i systemet som x p (k + 1) = x p (k) + v p (k) (99) der E[v p (k)] = m vp (k) = 0 og varians er E[v p (k) v p (l)] = δ kl q p. Det kan være hensiktsmessig å ha q p ikke helt liten, spesielt i begynnelsen. Størrelsen for q p angir hvor mye en tillater parameteren å endres på et tidssteg. Diskretiseringen av det ulineære systemet kan ikke lenger gjøres nøyaktig, Eulers forovermetode er enklest og mest brukt. Støy legges ofte til den diskrete modellen, selv om parameteren tillegges det kontinuerlige systemet. Kalman-filter kan estimere det meste på en optimal måte. Men det er noen viktige forutsetninger. Systemet må være observerbart. Hvis ikke har vi ingen garanti for resultatet. Prosesstøy må være med for å få endringer. Modellen må være (tilnærmet) riktig! Kombinert tilstands- og parameterestimering brukes vanligvis ved farget støy. Modell for støy (med ukjente parametre) inkluderes da i en augmentert modell. Farget støy beskrives da som en tilstandsrommodell. Hvis den er gitt ved transferligningen så må den overføres til en tilstandsrommodell. 22

23 7 Matrix Inversion Lemma Hvordan en går fra (43) til (44) er ikke direkte innlysende. En bruker matrise algebra. La oss først sette opp en opplagt ligning og utlede en formel fra denne D T R 1 DP D T + D T = D T + D T R 1 DP D T (100) D T R 1 DP D T + D T R 1 R = P 1 P D T + D T R 1 DP D T D T R 1 [DP D T + R] = [P 1 + D T R 1 D] P D T (101) D T R 1 = [P 1 + D T R 1 D] P D T [DP D T + R] 1 (102) [P 1 + D T R 1 D] 1 D T R 1 = P D T [DP D T + R] 1 (103) Noe mer matriseregning viser også [P 1 + D T R 1 D] 1 = [P 1 + D T R 1 D] 1 [I + D T R 1 DP D T R 1 DP ] = [P 1 + D T R 1 D] 1 [P 1 P + D T R 1 DP D T R 1 DP ] = [P 1 + D T R 1 D] 1 [ (P 1 + D T R 1 D)P D T R 1 DP ] = P (P 1 + D T R 1 D) 1 D T R 1 DP (104) Når en setter (103) inn i (104)=(105) så får en Matrix Inversion Lemma, (106). [P 1 + D T R 1 D] 1 = P [ (P 1 + D T R 1 D) 1 D T R 1 ] DP (105) [P 1 + D T R 1 D] 1 = P P D T [DP D T + R] 1 DP (106) Ligning (41) skrevet uten (k)-ene blir (107), og brukes (103) får vi (108). Videre (107) innsatt i (109) gir (110), og videre med (106) får vi (111). Fra (108) får vi (112) K = P D T (DP D T + R) 1 (107) K = [P 1 + D T R 1 D] 1 D T R 1 (108) (I KD)P = P KDP (109) (I KD)P = P P D T (DP D T + R) 1 DP (110) (I KD)P = [P 1 + D T R 1 D] 1 (111) KR = [P 1 + D T R 1 D] 1 D T (112) Nå kan vi gå (42) til (45) ved hjelp av (111) og (112). ˆP (k) = (I KD)P (I KD) T + KRK T = [P 1 + D T R 1 D] 1 (I KD) T + [P 1 + D T R 1 D] 1 D T K T = [P 1 + D T R 1 D] 1 [(I KD) T + D T K T ] = [P 1 + D T R 1 D] 1 [I D T K T + D T K T ] = [P 1 + D T R 1 D] 1 = (I KD)P (113) 23