2 Utledning av Kalman-filter Forventningsrett estimator Kovariansmatriser Minimum varians estimator... 9

Transkript

1 Stavanger, 3. august 2018 Det teknisknaturvitenskapelige fakultet ELE620 Systemidentifikasjon, Innhold 1 Tilstands- 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 Ulineære system og utvidet Kalman-filter 21 6 Kombinert tilstands- og parameterestimering 24 Karl Skretting, Institutt for data- og elektroteknikk (IDE), Universitetet i Stavanger (UiS), 4036 Stavanger. Sentralbord Direkte E-post: karl.skretting@uis.no.

2 7 Matrix Inversion Lemma 25 1 Tilstands- 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 dybde. 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 notat1. Men av praktiske (og historiske) grunner tas det heller med her. 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 2

3 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 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 3

4 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). Vi vil i utledningen av Kalman-filteret bruke tilstandsvektoren og ulike estimat av denne, en liten oppsummering kan derfor være på sin plass: 4

5 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 aposteriori estimatet 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 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 5

6 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 minimum varians 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) Merk at K(k) og K(k) forelø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. 6

7 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) 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) 7

8 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. Vi bruker dette videre x(k) x(k) = Φx(k 1) + v(k 1) Φˆx(k 1) = Φ [x(k 1) ˆx(k 1)] + v(k 1) (28) 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) 8

9 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 og bruk av (41) = (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 tidssteg k alle verdier for tidssteg (k 1) kjent, det vil spesielt si ˆx(k 1) og ˆP (k 1). Ved tidssteg 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 Dette er en del som jeg fortsatt kan jobbe mye med å skrive bedre, det er altså work in progress. Mye sies under forelesningene, også uten at det er spesielt nevnt her. En kan også finne mye om Kalman-filter andre steder, både i lærebøker og artikler. Følgende stikkordliste tar med noe av det som bør sies om Kalman-filter. Rekkefølge av ligningene i oppsummeringen kan gjerne være annerledes. For eksempel kan (46) og (47) bytte plass, og (49) og (50) kan bytte plass. Ny tilstand ønskes fort, det vil si med minst mulig regning etter at y(k) er tilgjengelig. Da kan en i tidssteg k regne ut ligningene (46)-(48) og (50) for tidssteg (k + 1). Hvis ønskelig kan en også, i tidssteg k, regne ut apriori estimat for neste måling, y(k + 1) = Dx(k + 1). Rekursivt filter. Av og til sier en at Kalman-filter er et rekursivt filter. Med det mener en at verdien i steg k altså ˆx(k) er avhengig av verdien i steg k 1 og at en da kan regne seg bakover til en kommer til en kjent verdi (ˆx(0)). Tilsvarende som en rekursiv løsning for fakultet funksjonen, fakultet(n) = n fakultet(n 1). For Kalman-filter regner en rimeligvis ikke bakover men forover, og har da en begrenset arbeidsmengde i hvert tidssteg. 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 [Pros.] med Kalman-filter og regulator [Reg.]. Input til regulator 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 tegnet 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 Forsterkningsmatrise K(k) og kovariansmatrisene P (k) og ˆP (k) er uavhengige av pådrag og målinger. Ved å se på ligningene (47), (48) og (50) kan en legge merke til at ingen av disse er avhengige av pådrag og målinger. Dermed kan disse om ønskelig beregnes på forhånd så sant systemmatrisene Φ(k) og D(k), kovariansmatriser for støy (tuningsmatriser) Q(k) og R(k) og initialverdi for tilstandsavvikets kovariansmatrise ˆP (0) er kjent. Merk at Γ(k) ikke trengs her. Riccati -ligningene er et navn som ofte brukes for ligningene (47), (48) og (50). Dette kan gjerne skrives som en ligning ved å kombinere de til en rekursiv ligning for ˆP (k), eller P (k), denne ligningen kalles da Riccatiligningen. De enkleste formen er gjerne med P (k) og da får en fra (47) P (k + 1) = ΦP (k)φ T + Q ΦP (k)d T (DP (k)d T + R) 1 DP (k)φ T Hvis Riccati-ligningen over har en stabiliserende løsning betyr det at P (k) konvergerer mot en stasjonær matrise P s, likeså vil ˆP (k) konvergere mot en stasjonær matrise ˆP s = (I K s D)P s. Stasjonær løsning er spesielt interessant. Det stasjonære Kalman-filter kan finnes hvis Φ og D, Q og R er (valgt som) konstante og ikke endrer seg med tida ved å beregne grenseverdien K s = lim k K(k). Hvis denne finnes (og er ulik 0) så kan en velge å forenkle Kalman-filteret svært mye ved kun å bruke ligningene (46) og (49) der en bruker K s i stedet for K(k). For stasjonær løsning kan en også på forhånd finne stasjonær kovariansmatrise for avviket, ˆPs. Altså vet en usikkerheten i tilstandsestimatene på forhånd. Utvidet Kalman-filter (EKF) har at systemmatrisene Φ(k) og D(k) er avhengige av tilstandsestimatene, (92) og (92). Dermed er ikke stasjonært Kalman-filter en mulighet i dette tilfellet. Tuningsparameter Det er vanskelig å sette riktige verdier til Q-matrisa, den skal både ta hensyn til støy som naturlig finnes i prosessen og til modellfeil. R-matrisa er enklere da kovariansmatrisa for målestøyen (oftest) er diagonal og avhengig av egenskaper til måleinstrumentene, gjerne gitt i datablad. I praksis brukes da ofte Q-matrisa som tuningsparametre for Kalman-filteret. Gjerne holdes Q diagonal og hvert element angir da varians for støy som legges til hver av tilstandene i hvert tidssteg. Prediktor-korrektor type Kalman-filter, som presentert i ligningene (46)- (50), er nå den vanligste måte å implementere Kalman-filter på. Men det fins også en enklere, men ikke så presis, variant, prediktor-type Kalmanfilter, som ikke skiller mellom apriori og aposteriori estimater. 13

14 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 tidssteg. 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. 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) 14

15 ˆ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) = Med induksjon og utgangspunktet at ˆP (k) = σ2 vises at dette gjelder generelt. ˆP (k) = = σ2 P (k) P (k) + σ 2 (73) k+1 for k = 1 (og for k = 2) så σ2 P (k) P (k) + σ = σ2 ˆP (k 1) (74) 2 ˆP (k 1) + σ 2 σ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 15

16 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 Control System Toolbox (CST) i Matlab inneholder flere funksjoner som kan brukes i forbindelse med Kalman-filter (KF). Generelt bør en lese dokumentasjonen grundig for hver enkelt funksjon før den brukes, spesielt må en merke seg at notasjon i Matlab og notasjon brukt i dette dokumentet ikke stemmer helt overens. Matlab bruker andre navn på matrisene, og betegnelsene for støyen er byttet om, w og v. Det viktigste i dokumentasjonen er å forstå hva input og output er og å bruke disse riktig. Et eksempel viser gjerne best hvordan KF kan brukes i Matlab, eks9p9.m. Dette er basert på eksempel 9.9 i Finn Haugens blå bok, Regulering av dynamiske system del 2. 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. Et blokkdiagram av systemet viser i figur 2. I Matlab og CST defineres systemet litt annerledes enn i figur 2. Tilstandsrommodellen, sys nedenfor, er basert på blokkdiagram i figur 3. ]. nx=2; nu=1; ny=1; 16

17 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. 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 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 i del 3.3 fra notat3 (KS, 2017). ); [kest, L, P_strek, Ks, P_hatt] = kalman(sys,q,r); Funksjonen kalman ovenfor returnerer estimatoren som et nytt ss-objekt og de stasjonære matrisene. Eksempelet eks9p9.m viser resultatene både for tidsvarierende KF og stasjonært KF. For tidsvarierende KF kan en velge mellom tre ulike implementeringer som alle virker likt. For dokumentasjon til kalman ser en at en har større fleksibilitet i hva en gir inn enn det vi har tatt med her, en kan ha direktekoblingsmatrise (både for pådrag og prosesstøy) og en kan ha kovarians mellom prosesstøy og målestøy. Når de ubrukte matriser settes til null og med egen notasjon kan en se fra dokumentasjonen at matrisa L er L = ΦP D T (DP D T + R) 1 Der jeg fant ut (ved å prøve) at P her må være P. Med å bruke 41 får en L s = Φ P D T (DP D T + R) 1 = ΦK s Siden dette er den stasjonære løsning brukes subskript s. Et stasjonært prediktor variant KF bruker da L s = ΦK s i en ligning som gjøres i ei løkke etter hvert som k øker. I hver iterasjon får en et nytt pådrag 17

18 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 KF, 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. u(k) og ei ny måling y(k). Ligningen i løkka tar utgangspunkt i ligning 46, x(k + 1) = Φˆx(k) + Γu(k), og setter inn for ˆx(k) fra ligning 49 ( ( ) ) x(k + 1) = Φ x(k) + K s y(k) y(k) + Γu(k) med y(k) = Dx(k) og L s = ΦK s blir dette ( ) x(k + 1) = Φx(k) + Γu(k) + L s y(k) Dx(k) Hvis modellen er ulineær er dette ( ) x(k + 1) = f(x, u,...) + L s y(k) g(x, u,...) 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. Kovariansmatrisa, 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 med CST så er det noe mer komplisert. Et eksempel er i vedlagte m-fil eks9p9.m. Systemet for simulering blir syssim og er basert på blokkdiagram i figur 4. syssim = ss(phi,[gamma, Omega, [0; 0]], [D; D],... [0,0,0,0; 0,0,0,1], T,... StateName,{ x1, x2 },... InputName,{ u, v_1, v_2, w },... OutputName,{ y, ynoise },... 18

19 u v 1 v 2 w x 1 x 2 y (= x 1 ) y noise Figur 4: Blokkdiagram for simuleringssystemet 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. Notes, Simuleringmodell i del 3.3 fra notat3 (KS, 2017). ); systot = parallel(syssim, kest, 1, 1, [], []); systot = feedback(systot, 1, 5, 2, 1); systot = systot([ ], [ ]); Funksjonen kalman returnerer en estimator som en tilstandsrommodell, et blokkdiagram for denne vises i figur 5. Videre kan en bygge et totalsystem, ved å koble sammen simulator og estimator. I koden vist ovenfor har en først parallellkoblet systemene syssim og kest. 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. 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. Det ferdige systemet vises som blokkdiagram i figur 6. 19

20 v 1 v 2 w u x 1 x 2 x 2e x 1e y ye x 1e x 2e Figur 6: Blokkdiagram for totalsystemet systot. 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) KF 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 KF. 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 2, 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) 20

21 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) Vi kan nå bruke KF 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 KF. 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 21

22 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 KF. 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 KF. 22

23 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-Schmidtfilter. 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 tidssteg k er alle verdier for tidssteg (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. 23

24 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. 24

25 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) 25