Kalmanfilter på svingende pendel

Kalmanfilter på svingende pendel Rolf Henriksen og Torbjørn Houge Institutt for teknisk kybernetikk NTNU 2005 Vi skal se på hvordan Kalmanfilteret fungerer på et velkjent eksempel, den svingende pendel (figur 1.1a). Vi regner med masseløs snor og all masse konsentrert i ett punkt. Det skal vurderes både et kontinuerlig og et diskret Kalmanfilter, og implementeres i MatLab og Simulink. Først ser vi på modellen til systemet, og oppbygningen av filtrene. Deretter ser vi på simuleringsresultater for filtrene med forskjellige forutsetninger. Alle simuleringer benytter seg av det samme inngangssignalet for å få et best mulig sammenligningsgrunnlag. I tillegg til de to dynamiske filtrene vil vi se på hvordan et filter med konstant filterforsterkning virker på resultatet. Innhold 1 Modellering 1 1.1 Kontinuerlig......................................... 2 1.2 Diskretisering........................................ 3 2 Implementering 4 2.1 Utvidet kontinuerlig Kalman-filter............................ 4 2.2 Utvidet diskret Kalman-filter............................... 5 3 Simuleringsresultater 7 3.1 Kontinuerlig Kalman-filter................................. 8 3.2 Diskret Kalman-filter.................................... 13 4 Konstant filterforsterkning 18 Referanser 21

1 1 Modellering P l ϕ e n e t ϕ F l F v ϕ mg (a) Svingende pendel (b) Kreftene som virker på kula Figur 1.1: Modellen vi skal se på l : pendelsnoras lengde ϕ : vinkelutslag fra stabil tilstand m : masse k : luftdempings-konstant g : tyngdeakselerasjon P : snordraget F l : demping pga luftmotstand F v : støykrefter (hovedsakelig vind) e n : enhetsvektor - normalt på kulebanen e t : enhetsvektor - tangent til kulebanen Tabell 1.1: Forklaring på parametre Ved å summere opp kraften på kula og bruke ΣF = ma fra Newtons lov som en funksjon av vinkelutslaget og enhetsvektorene fra figur 1.1b, får vi: ΣF = ( mg sin ϕ F v cos ϕ F l ) e t + ( mg cos ϕ F v sin ϕ + P ) e n ΣF = m ( l ϕe t + l ϕ 2 e n ) Vi ser at hvis vi antar at luftmotstanden kan skrives som F l = kl ϕ vet vi alle leddene for tangentialkomponenten av kraften. Siden vi ikke vet snordraget P kan vi ikke bruke normalkomponenten. Vi setter derfor opp ligningene for tangentialkomponenten på tilstandsromform ved å definere vari-

2 1 MODELLERING ablene våre x 1 = ϕ, x 2 = ϕ, w = F v : 1.1 Kontinuerlig ẋ 1 = x 2 (1.1) ẋ 2 = g l sin x 1 k m x 2 + 1 ml cos x 1w (1.2) ] x1 x = (1.3) x 2 Den kontinuerlige, ulineære modellen blir: ] ẋ1 ẋ = = ẋ 2 = f(x) + E(x)w x 2 g l sin x 1 k m x 2 + 1 ml cos x 1w ] (1.4) y = dx 1 + v = d 0 ] x + v (1.5) = Hx + v Vi definerer paremetrene til å være følgende: g = 9.8 l = 2 k = 0.5 m = 5 d = 1 Prosesstøyen (w) har standardavvik 25, og målestøyen (v) har standardavvik 0.5.

1.2 Diskretisering 3 1.2 Diskretisering Senere skal vi se på dette systemet med et diskret filter. Vi kan fortsatt bruke en kontinuerlig systemmodell, men må lage en diskret tilnærming til modellen som vi kan bruke i filteret. Til diskretiseringen vleger vi å bruke den forbedrede Euler-metoden (en 2.ordens eksplisitt RK-metode), som beskrevet i Egeland and Gravdahl (2002, kapittel 14.3.2). Skrevet om til vår notasjon blir det seende slik ut (velger å beholde k 1, k 2 i funksjonene, da det er opplagt at disse er forskjellige fra modellparameteren k). x k+1 = x k + t s 2 (k 1 + k 2 ) = f k (x k ) k 1 = f(x k ) k 2 = f(x k + t s k 1 ) Så setter vi inn ligningene for modellen vår, regner ut og får (alle tilstandene regnes som diskrete, altså menes x 1,k selv om det står x 1 ): x k+1 = ( 1 tsk m + 1 2 ( tsk m ( x 1 + t s t2 s k ) 2 ) x 2 + tsg 2l t 2 s ) 2m x 2 t2 s 2l sin x 1 ( tsk m 1) sin x 1 tsg 2l sin (x 1 + t s x 2 ) (1.6) ] + 2ml cos x 1 t s (( 2lm 1 t sk ) m cos x1 + cos (x 1 + t s x 2 ) ) w = f k (x k ) + E k (x k )w (1.7) y k = Hx k + v (1.8)

4 2 IMPLEMENTERING 2 Implementering 2.1 Utvidet kontinuerlig Kalman-filter Siden dette er et ulineært system må vi bruke det utvidede Kalman-filteret. Ligningene for utvidet kontinuerlig Kalman-filter er beskrevet i Henriksen (1998, ligning 2.169-2.171) og Fossen (2002, tabell 6.1). Her brukes sistnevnte notasjon. Med informasjonen vi har om systemet får vi da ligningene: ˆx = f(ˆx) + K (y Hˆx) (2.1) ] ( ˆx = 2 g l sin ˆx 1 k m ˆx + K y d 0 ] ]) ˆx 1 (2.2) 2 ˆx 2 Ṗ = = T f x T P + P f x T 0 1 g l cos ˆx 1 0 + 1 ml cos ˆx 1 k m + EQE T KRK T (2.3) ] 0 g P + P l cos ˆx ] 1 1 k m (2.4) ] Q 0 1 ml cos ˆx 1 ] KRK T K = PH T R 1 (2.5) Disse ligningene blir lagt inn i MatLab som funksjoner, og deretter inn i Simulink-diagrammer som i figur 2.1: (a) Oversikt over systemet (b) Implementering av filteret Figur 2.1: Implementering av Kalman-filter for et kontinuerlig system I systemet legger vi inn at standardavvikene for støyen er 25 for prosesstøyen og 0.5 for målestøyen. Vi bruker " Band limited white noise" -blokker i Simulink for å generere hvit støy, og så ganger vi med standardavviket for å få det riktig. Disse blokkene må få en samplingstid, da de ikke kan være kontinuerlige, så vi setter begge til 100Hz oppdateringsfrekvens. Ved å stille inn Simulink til å beregne med variabel steglengde blir dette det nærmeste vi kommer et kontinuerlig system. I filterligningene må vi sette inn et estimat av kovariansene, Q og R. Estimatene kommer vi tilbake til senere, men de ideelle kovariansene setter vi til å være kovariansen til støyen i systemet, altså: R 0 = 0.5 2 Q 0 = 25 2

2.2 Utvidet diskret Kalman-filter 5 2.2 Utvidet diskret Kalman-filter La oss så se på tilfellet hvis målingene er diskret, og vi skal bruke en datamaskin for å filtrere signalet (noe vi vanligvis gjør). Kalman-filteret var opprinnelig laget diskret, så vi er interessert i å se på hvordan det fungerer for systemet vårt. Vi må bruke det utvidere filteret, siden systemet er ulineært. For å sette det opp bruker vi det samme systemet som for det kontinuerlige tilfeller, men legger på et 0.-ordens holdeelement på utgangen av målingen. Målingen samples med 10Hz. Fra Henriksen (1998, ligning 2.160-2.165) og Fossen (2002, tabell 6.5) får vi ligningene for filteret og prediktoren. Ved å putte inn informasjonen vi har om systemet (linearisert og diskretisert med samplingstid t s ), får vi: Filtrering ˆx k = x k + K k (y k H k x k ) (2.6) = x k + K k ( yk d 0 ] x k ) ˆP k = (I K k H k ) P k (I K k H k ) T + K k RK k (2.7) = ( ]) ( ]) T I K k d 0 Pk I Kk d 0 + Kk RK k K k = P k H T ( k Hk P k H T k + R) 1 ] ( ] ) d ] 1 d = P k d 0 Pk + R 0 0 (2.8) Prediksjon x k+1 = Φ kˆx k P k+1 = Φ k ˆP k Φ T k + 0 1 ml Φ k = f k = x T k 1 t 2 s g ( t 2 s kg 2l cos x ) 1 2ml tsg 2l ] Q 0 1 ml cos x 1 tsg 2l cos (x 1 + t s x 2 ) ] ( 1 tsk k + 1 2 t s t2 s k 2m ( tsk ) 2 ) m t2 s g 2l cos (x 1 + t s x 2 ) ] (2.9) (2.10) (2.11) Disse ligningene blir lagt inn i MatLab som funksjoner, og deretter inn i Simulink-diagrammer som i figur 2.2:

6 2 IMPLEMENTERING (a) Oversikt over systemet (b) Implementering av filteret (c) Implementering av prediktoren Figur 2.2: Implementering av Kalman-filter for et diskret system

7 3 Simuleringsresultater Alle simuleringene ble gjennomført med de samme kovariansene og initialtilstandene. Først ble det sett på filterets egenskaper med de teoretisk ideelle kovariansmatrisene. Deretter ble det sett på endringer i kovariansen til både støyen og estimatet. Disse verdiene ble simulert: 1 R= R 0 Q= Q 0 P= P 0 2 R= R 0 0.01 Q= Q 0 P= P 0 3 R= R 0 100 Q= Q 0 P= P 0 4 R= R 0 40 Q= Q 0 40 P= P 0 5 R= R 0 Q= Q 0 P= P 0 0.01 Tabell 3.1: Kovarianser brukt for simuleringer Figur 3.1: Målesignal For å forenkle beregningen, og siden systemet ikke har noen regulering, kan vi gjøre en gjennomkjøring og logge systemets tidsforløp, og bruke det samme forløpet på alle filtrene. Vi velger å gjøre det, og får tidsforløpet som i figur 3.1. Dette er målesignalet som alle filtrene kommer til å bruke, og vi ser at det har en del støy.

8 3 SIMULERINGSRESULTATER 3.1 Kontinuerlig Kalman-filter Ved å simulere systemet beskrevet i kapittel 2.1 får vi resultatene som følger i dette kapittelet. (c) Filterforsterkning (d) Kovarians element (1,1) og (2,2) Figur 3.2: Simuleringsresultater fra 1 i tabell 3.1

3.1 Kontinuerlig Kalman-filter 9 (c) Filterforsterkning (d) Kovarians element (1,1) og (2,2) Figur 3.3: Simuleringsresultater fra 2 i tabell 3.1 Vi ser at hvis estimatet av målestøyens kovarians er for lite, vil estimatet følge målingen i alt for stor grad. Estimatoren regner med at målestøyen er neglisjerbar i frohold til prosesstøyen.

10 3 SIMULERINGSRESULTATER (c) Filterforsterkning (d) Kovarians element (1,1) og (2,2) Figur 3.4: Simuleringsresultater fra 3 i tabell 3.1 Når kovariansen til målingen er estimert for stor, vil estimatoren nesten ikke legge vekt på målingen, og bruke sin egen prediksjon i mye større grad enn før. Filterforsterkningen blir konstant ganske raskt, og holder seg lav.

3.1 Kontinuerlig Kalman-filter 11 (c) Filterforsterkning (d) Kovarians element (1,1) og (2,2) Figur 3.5: Simuleringsresultater fra 4 i tabell 3.1 Når kovariansmatrisene til begge støykildene endres med samme forhold (multiplisert med 40), ser vi at det ikke innvirker på estimatet i stor grad. Det vi derimot ser er at kovariansen til estimatet får vesentlig høyere verdier.

12 3 SIMULERINGSRESULTATER (c) Filterforsterkning (d) Kovarians element (1,1) og (2,2) Figur 3.6: Simuleringsresultater fra 5 i tabell 3.1 Når vi antar mindre usikkerhet i begynnelsestilstandene ser vi at estimatoren bruker noe tid på å svinge seg inn, da den trenger litt tid på å " oppdage" at den estimerer feil.

3.2 Diskret Kalman-filter 13 3.2 Diskret Kalman-filter Ved å simulere systemet beskrevet i kapittel 2.2 får vi resultatene som følger i dette kapittelet. (c) Filterforsterkning (d) Kovarians element (1,1) og (2,2) Figur 3.7: Simuleringsresultater fra 1 i tabell 3.1

14 3 SIMULERINGSRESULTATER (c) Filterforsterkning (d) Kovarians element (1,1) og (2,2) Figur 3.8: Simuleringsresultater fra 2 i tabell 3.1

3.2 Diskret Kalman-filter 15 (c) Filterforsterkning (d) Kovarians element (1,1) og (2,2) Figur 3.9: Simuleringsresultater fra 3 i tabell 3.1

16 3 SIMULERINGSRESULTATER (c) Filterforsterkning (d) Kovarians element (1,1) og (2,2) Figur 3.10: Simuleringsresultater fra 4 i tabell 3.1

3.2 Diskret Kalman-filter 17 (c) Filterforsterkning (d) Kovarians element (1,1) og (2,2) Figur 3.11: Simuleringsresultater fra 5 i tabell 3.1 Vi observerer at for alle tilfellene virker det diskrete filteret på samme måte som det kontinuerlige, hvis vi ser bort fra at det naturlig nok får en begrenset oppdateringsfrekvens. I dette eksempelet er det diskretisert med en 2.ordens metode, men det er skjelden nødvendig å bruke en så nøyaktig diskretisering. I de aller fleste tilfeller vil Eulers metode være god nok. En annen måte å bedre nøyaktigheten på er å øke samplingsfrekvensen.

18 4 KONSTANT FILTERFORSTERKNING 4 Konstant filterforsterkning For et filter som skal gå som en del av et reguleringssystem, eller et sted der vi ikke har mye regnekraft tilstede, velger vi som regel å bruke et enklere filter, der prediktoren er erstattet med å bruke en konstant filterforsterkning. Denne kan vi regne ut ved å regne ut hva forsterkningen blir etter at systemet har gått en stund og nådd en stasjonær tilstand. Ut i fra ligningene brukt i 2.1 kan vi regne ut forsterkningen når Ṗ = 0. Da får vi verdier for P og K. Vi ender da opp med filterligningene (disse er også beskrevet i Fossen (2002, ligning 6.50-6.52)): ˆx = f(ˆx) + K (y Hˆx) (4.1) K = P H T R 1 (4.2) Kovariansmatrisen P kan finnes ved å regne ut løsningen på Riccati-ligningen f(x) x T f(x) P + P x=0 x T T x=0 + E(x = 0)QE(x = 0) T P H T R 1 HP = 0 (4.3) Til å regne ut filterforsterkning bruker vi funksjonen lqe i MatLab. Vi får da samtidig regnet ut den stasjonære kovariansmatrisen. Ved å regne ut disse verdiene for tilfellene som er satt opp i tabell 3.1 kan en utvidet tabell settes opp (P 0 er utelatt, siden den ikke benyttes): ] 1.95 1 R= R 0 Q= Q 0 K = 1.90 ] 9.42 2 R= R 0 0.01 Q= Q 0 K = 44.4 ] 0.14 3 R= R 0 100 Q= Q 0 K = 0.01 ] 1.95 4 R= R 0 40 Q= Q 0 40 K = 1.90 ] 0.488 0.476 P = 0.476 3.370 ] 0.024 0.111 P = 0.111 1.172 ] 3.666 0.269 P = 0.269 18.05 ] 19.51 19.04 P = 19.04 135.8 Tabell 4.1: Filterforsterkninger og stasjonære kovarianser Det observeres at selv om forholdet mellom støy-kovariansene er det samme i tilfellene 1 og 4, og derfor filterforsterkningen blir den samme, blir kovariansmatrisen P vesentlig høyere. Ved å gange begge støy-kovariansene med 40, vil kovariansen til signalet øke med det samme forholdet.

19 Figur 4.1: Simuleringsresultater fra 1 i tabell 4.1 Figur 4.2: Simuleringsresultater fra 2 i tabell 4.1

20 4 KONSTANT FILTERFORSTERKNING Figur 4.3: Simuleringsresultater fra 3 i tabell 4.1

REFERANSER 21 Figur 4.4: Simuleringsresultater fra 4 i tabell 4.1 I alle 4 tilfellene viser det seg at filteret klarer seg nesten like bra som i det dynamiske tilfellet. Det er kun i starten av simuleringen at det har litt problemer med å følge signalet, og det tar litt lengre tid før det følger skikkelig. Dette er forståelig, siden det for det dynamiske filteret tar litt tid før forsterkningen blir tilnærmet stasjonær. Det vises også klart at de forskjellige gjettingene på støyens kovarians får det samme effekt her som med det dynamiske filteret. Referanser Olav Egeland and Jan Tommy Gravdahl. Modeling and Simulation for Automatic Control. Marine Cybernetics, 2002. Thor I. Fossen. Marine Control Systems. Marine Cybernetics, 1st edition, 2002. Rolf Henriksen. Stokastiske systemer, august 1998.