I dette kapitlet vil vi gi en rask innfring i Kalman-ltrering. Malet er a sette leseren

Transkript

1 Kapittel 2 Tilstandsestimering I dette kapitlet vil vi gi en rask innfring i Kalman-ltrering. Malet er a sette leseren i stand til a bruke og implementere et Kalman-lter (KF) uten a matte ga igjennom en detaljert utledningen av KF. For fullstendig utledning av KF se f.eks. Gelb, Kasper, Jr., Nash, Jr., Price & Sutherland, Jr. (1988) eller Minkler & Minkler (1993). 2.1 Innledning I navigasjonssammenheng benyttes frst og fremst KF som tilstandsestimator. Fordelene med a benytte en tilstandsestimator er mange, f.eks.: En tilstandsestimator kan beregne ere av et systems tilstandsvariable enn de som er fysisk malbare. En tilstandsestimator kan beregne ogsa malbare tilstander fremfor a kjpe inn kostbart maleutstyr. En tilstandsestimator kan gi et bedre estimat av malte tilstander da disse ofte er belagt med sty. En tilstandsestimator vil fortsette a gi fornuftige tilstandsestimater selv en tid etter at en eller ere malinger faller ut (prediksjon). En tilstandsestimator er svrt hensiktsmessig i integrerte navigsjonssystemer dvs. nar du har ere malesystemer. En tilstandsestimator kan benyttes for deteksjon av malefeil. En tilstandsestimator kan f.eks. beregne et skips hastighet selv om en kun maler skipets posisjon. Under kritiske operasjoner f.eks. i Nordsjen kan det vre helt avgjrende at ikke navigasjonssystemet bryter fullstendig sammen dersom malinger faller ut. I integrerte navigasjonssystemer der en rekke malinger fra forskjellige instrumenter trekkes inn for a skae data om et fartys bevegelse, er det nettopp teorien for optimal tilstandsestimering (KF) som ligger til grunn for beregning av et beste estimat av fartyets bevegelsestilstand. KF er utledet fra stokastisk teori og egner seg derfor til bruk i forbindelse med stokastiske systemer (ikke-deterministiske). I et deterministisk system er alle eksitasjoner og initialbetingelser kjente i bade fortid, natid og fremtid, mens de i et stokastisk

2 6 Tilstandsestimering system bare vil kunne vre statistisk beskrevet. Eksempler pa stokastiske prosesser i denne sammenheng er vind, havstrm og blger, men ogsa feildynamikken til navigasjonsinstrumenter (f.eks. malefeil i GPS) kan ofte beskrives som stokastiske prosesser. Stokastiske prosesser som eksiterer dynamiske systemer gir som resultat nye stokastiske prosesser. Skip eller y utsatt for miljkrefter er derfor ogsa stokastiske systemer. Figur 2.1 viser prinsippet for tilstandsestimering ved bruk av KF. Vi ser at Kalman- lteret (som inneholder en modell av den virkelige prosessen) patrykkes det kjente padragssignalet u, og blir oppdatert av de tilgjengelige malingene z. Ut av Kalman- lteret kommer sa tilstandsestimatet ^x. v (malesty) w (stokastisk prosessty) u (padrag) Prosess x (tilstander) z (malinger) Kalman- lter ^x (estimerte tilstander) Figur 2.1: Kalman-lteret som tilstandsestimator. 2.2 Tilstandsrommodeller Utgangspunktet for Kalman-ltrering er en matematisk modell av det (stokastiske) systemet en star ovenfor. Jo bedre modellen er, jo bedre resultat far man. En ma derimot alltid foreta avveininger mellom kompleksitet kontra ytelse. Den matematiske modellen skrives gjerne pa tilstandsrom form som inneholder to deler, nemlig dynamikk og maling. I denne sammenheng vil disse delene besta av: Dynamikk: Maling: dynamisk modell av farty-bevegelse, feildynamikk i maleinstrumenter og stokastiske forstyrrelser. sammenhengen mellom tilstander og malinger. Man skiller gjerne mellom ulinere kontra linere systemer, samt tidsinvariante kontra tidsvarierende systemer Linere modeller En liner dynamisk modell skrives pa tilstandsrom form som:

3 2.2 Tilstandsrommodeller 7 Dynamikk: _x(t) = A(t)x(t)+B(t)u(t)+E(t)w(t) (2.1) Maling: z(t) = C(t)x(t)+v(t) (2.2) Egenskap: y(t) = F (t)x(t) (2.3) hvor en har flgende benevnelser: x(t) = [x 1 x 2 ::: x n ] T - tilstandsvektor A(t) - systemmatrise u(t) = [u 1 u 2 ::: u r ] T - padrag B(t) - padragsmatrise w(t) = [w 1 w 2 ::: w p ] T - prosessty E(t) - prosesstymatrise z(t) = [z 1 z 2 ::: z m ] T - malevektor C(t) - malematrise v(t) = [v 1 v 2 ::: v m ] T - malesty F (t) - egenskapmatrise Iet tidsinvariant linert system er matrisene A B C E og F konstante og flgelig uavhengige av tiden (t). Malelikning hvor padrasvektoren inngar Dersom padraget u(t) inngar eksplisitt i uttrykket for malingen z(t) dvs: kan vi transformere malelikningen til: z(t) =C(t)x(t)+D(t)u(t)+v(t) (2.4) z 0 (t) = z(t) D(t)u(t) =C(t)x(t) (2.5) som er av samme form som (2.2). Dette spesialtilfellet vil derfor ikke bli diskutert i beskrivelsen av Kalman-lterlikningene. Diskretisering av linere tidsinvariante systemer (transisjonsmetoden) For a implementere dynamiske modeller i en datamaskin, trenger vi en diskret (rekursiv) modell. Dersom vi har et linert, tidsinvariant system, kan vi benytte transisjonsmetoden, se f.eks. Balchen, Andresen & Jagtyen (1997), til a oppna en \perfekt" diskretisering av tilstandsrommodellen (2.2). Transisjonsmetoden gir: der h er tastetid, k er tidsskrittet (t = kh) og x k+1 = x k + u k + w k z k = Cx k + v k (2.6) = e Ah = Me h M 1 I + Ah (Ah)2 + ::: + 1 N! (Ah)N (2.7) = A 1 ( I) B (2.8) = A 1 ( I) E (2.9) M = egenvektor- og egenverdimatrisen til A

4 8 Tilstandsestimering A-matrisen behver ikke a vre inverterbar for a beregne og a kombinere (2.7){(2.9) slik at:. Dette fremgar ved = (Ih Ah2 + ::: + 1 N! AN 1 h N ) B (2.10) = (Ih Ah2 + ::: + 1 N! AN 1 h N ) E (2.11) hvor N 1 er et heltall. Tabell 2.1: Notasjon brukt for kontinuerlig og diskret liner tilstandsrommodell. _x(t) = A(t)x(t) +B(t)u(t) +E(t)w(t) z(t) = C(t)x(t) +D(t)u(t) +v(t) y(t) = F (t)x(t) x k+1 = k x k + k u k + k w k z k = C k x k + D k u k + v k y k = F k x k Ulinere modeller En mere generell form for en tilstandsrommodell er den ulinere, som kan skrives: Dynamikk: _x(t) = f(x(t) u(t) w(t) t) Maling: z(t) = h(x(t) t)+v(t) (2.12) Egenskap: y(t) = (x(t) t) Her har vi i tillegg flgende benevnelser: f(x(t) u(t) t) - uliner tilstandsfunksjon h(x(t) t) - uliner malefunksjon (x(t) t) - uliner egenskapfunksjon En tidsinvariant, uliner modell er ikke eksplisitt avhengig av tiden (t), dvs: f() = f(x(t) u(t) w(t)), h() =h(x(t)) og () =(x(t)). Diskretisering av ulinre tidsvarierende systemer Dersom en har et tidsvarierende ulinert system, ma en benytte tilnrmede diskretiseringsmetoder som f.eks. Runge{Kutta, trapesmetoden, forover Euler, m.. Disse metodene er beskrevet i detalj i Henriksen & Iversen (1990). Vi vil her presentere forover Euler som er svrt enkel bade a forsta og implementere. Ulempen med forover Euler er at den trenger kortere tastetid og har darligere stabilitetsegenskaper enn de mer avanserte metodene.

5 2.3 Kalman-lterligningene 9 Forover Euler Forover Euler gar ganske enkelt ut paa sette t = kh (h = tastetid) og foreta tilnrmingen: _x(t) x k+1 x k (2.13) h Vi skriver dessuten x(kh) somx k, A(kh) soma k, osv. Ved a sette dette inn i den linere modellen (2.2) far vi da: x k+1 = x k + h[a k x k + B k u k + E k w k ] (2.14) z k = C k x k + v k (2.15) som lett kan omskrives til diskret tilstandsrom form: x k+1 = k x k + k u k + k w k z k = C k x k + v k (2.16) Dette svarer til a velge N = 1 i (2.10) og (2.11). Denne diskretiseringsmetoden fungerer pa samme mate ogsa for ulinere systemer. Etter diskretisering av (2.12) far vi da: x k+1 = x k + hf k (x k u k w k ) = f 0 k (x k u k w k ) (2.17) z k = h k (x k )+v k (2.18) hvor f 0 k (x k u k w k )=x k + hf k (x k u k w k ) (2.19) Legg forvrig merke til at selv om maleligningen skulle vre uliner og/eller tidsvarierende, kan den perfekte diskretisering i avsnitt fremdeles benyttes sa lenge dynamikken er liner og tidsinvariant. 2.3 Kalman-lterligningene Flgende danner grunnlaget for a sette opp et Kalman-lter: Kunnskap om prosess og maleinstrumenters dynamikk Statistisk beskrivelse av prosessty, malefeil og modellusikkerhet Informasjon om begynnelsesverdier til aktuelle tilstandsvariable Det er kun observerbare tilstander som kan estimeres i et Kalman-ter. For at det skal vre mulig a estimere hele tilstandsvektoren x ma systemet vre observerbart:

6 10 Tilstandsestimering Denisjon 2.1 (Observerbarhet for tidsinvariante systemer) Et linert, tidsinvariant system med system- og malematrise (A C) er observerbart hvis (n n) observerbarhetsmatrisen Balchen et al. (1997): O =[C T j A T C T j ::: j (A T ) n 1 C T ] (2.20) har full rang n. En ndvendig og tilstrekkelig betingelse for O a ha full rang er: deto > 0 (2.21) Denisjon 2.2 (Observerbarhet for tidsvarierende systemer) Et linert, tidsvarierende system med system- og malematrise (A(t) C(t)) er observerbart hvis 9 T >0 og > 0 slik at, Fossen (1994): Z t0 +T I 1 T t0 exp(a T ( ) )C T ( )C( )exp(a( ) ) d I (2.22) 8 t 0 2 R +. Dette betyr at integralet av matrisen exp(a T )C T C exp(a ) er uniformt, positiv denit over ethvert intervall med lengde T. Det er derimot fullt mulig a estimere noen av tilstandene selv om ikke systemet er fullstendig observerbart Kontinuerlig Kalman-lter Det tradisjonelle kontinuerlige Kalman-lteret kan benyttes for kontinuerlige, linere, tidsvarierende og tidsinvariante systemer pa formen: _x(t) = A(t) x(t) +B(t) u(t) +E(t) w(t) (2.23) z(t) = C(t) x(t) (2.24) Ligningene som utgjr det kontinuerlige Kalman-lteret er gitt i tabell Strrelsene som inngar er stort sett gjennomgatt i avsnitt 2.2, men vi tar likevel en rask gjennomgang: A B C E Q R K ^x ^X Matrisene i den linere tilstandsrommodellen Kovariansmatrisene til hhv. prosess- og malesty KF-forsterkningen Estimat av tilstandsvektoren Estimat av kovariansmatrisen til tilstandsvektoren

7 2.3 Kalman-lterligningene 11 Tabell 2.2: Kontinuerlig Kalman-lter. Initialverdier ^x(0) = x 0 ^X(0) = E[(x(0) ^x(0))(x(0) ^x(0)) T ]=X 0 KF-forsterkningen K(t) = ^X(t)C T (t) R 1 (t) Tilstandsestimat _^x(t) =A(t) ^x(t) +B(t) u(t)+k(t) [z(t) C(t) ^x(t)] Kovarians _^X(t) =A(t) ^X(t)+ ^X(t) A T (t)+e(t) Q(t) E T (t) ^X(t) C T (t) R 1 (t) C(t) ^X(t) I gur 2.2 er lteret fremstilt grask. Vi ser at Kalman-lteret inneholder en modell som gar i parallell med den virkelige prosessen. KF-modellen skal altsa gjengi den virkelige prosessen. Det er derimot umulig a modellere helt nyaktig, og i tillegg vil prosessen alltid vre utsatt for sty. Av den grunn ser vi at modellen var blir oppdatert ved at dieransen mellom virkelig og estimert maling (" = z ^z) multipliseres med KF-forsterkningen for sa a ga inn som padrag til ltermodellen. Vi oppnar et tilstandsestimat ^x som er tilgjengelig i modellen. u(t) w(t) Virkelig prosess v(t) z(t) K(t) "(t) B(t) _^x(t) ^x(t) C(t) ^z(t) A(t) Figur 2.2: Kontinuerlig Kalman-lter. Stasjonrt kontinuerlig Kalman-lter Dersom tilstandsrommodell matrisene (A B C E) og sty-kovariansmatrisene (Q R) erkonstante (tidsinvariante), og systemet er asymptotisk stabilt, vil lterligningene konvergere mot en stasjonr lsning. Vi far da det stasjonre Kalman- lteret som er gitt ved:

8 12 Tilstandsestimering _^x(t) =A ^x(t)+b u(t)+k1 [z(t) C ^x(t)] (2.25) Den stasjonre KF-forsterkningen er gitt av: K1 = ^X1 C T R 1 (2.26) hvor ^X1 er lsningen pa den stasjonre Ricatti-likningen: A ^X1 + ^X1 A T + EQE T ^X1 CR 1 C ^X1 = 0 (2.27) Vi merker oss at K1 og ^X1 na erkonstante matriser som kan beregnes pa forhand. Ytelsen av det stasjonre lteret vil vre litt darligere enn det tidsvarierende lteret de frste sekundene av simuleringen. Stasjonrt vil ytelsen vre tilnrmet den samme. Dette gjelder bare for linere systemer siden K ikke vil konvergere til en konstant verdi for det ulinere KF. Ricatti-ligningen er riktignok ikke triviell a lse, men den kan lses ganske enkelt ved a benytte Matlab-kommandoen lqe(a,e,c,q,r), se eksempel 2.1. u k w k Virkelig prosess v k z k ^x k+1-1 z ^x k ^x k Figur 2.3: Diskretisert stasjonrt kontinuerlig Kalman-lter. Diskretisering av stasjonrt kontinuerlig Kalman-lter Den stasjonre kontinuerlige KF-ligningen (2.25) kan omskrives til: der _^x(t) =A kf ^x(t)+bu(t)+k1 z(t) (2.28) A kf = A K1C (2.29) Denne formen lar seg lett diskretisere som vist i kapittel (2.2), og er dermed enkel a implementere i en datamaskin. F.eks. vi kan benytte transisjonsmetoden under

9 2.3 Kalman-lterligningene 13 antakelse at malingene av u(t) og z(t) tilfredsstiller _u(t) = 0 og _z(t) mellom hver tasting. Dette gir: der ^x k+1 = kf ^x k + kf u k + kf z k (2.30) kf = A kf e h (2.31) kf = A 1 kf kf I) B (2.32) kf = A 1 kf kf I) K1 (2.33) Merk at denne metoden for diskretisering kun er mulig dersom en har et stasjonrt kontinuerlig KF. Eksempel 2.1 (Diskretisering av stasjonrt kontnuerlig KF i Matlab) [K,X,L] = lqe(a,e,c,q,r) % Stasjonaert KF-forsterkning mm. Akf = A - K*C % Filter systemmatrise h = 1 % Tastetid = 1 sek. [Phi,Delta] = c2d(akf,b,h) % Diskretisering [Phi,Omega] = c2d(akf,k,h) % ---- `` ---- Dette Matlab programmet returnerer altsa: K=K1 X= ^X1 L=egenverdiene til A kf Phi = Delta = Omega = : Diskret Kalman-lter Det diskrete KF er basert pa modellen: x k+1 = k x k + k u k + k w k (2.34) z k = C k x k + D k u k + v k (2.35) Dersom en skal implementere et KF for et linert tidsvarierende system i en datamaskin, br en benytte den diskrete versjonen av KF-ligningene. Disse er gitt i tabell 2.3 samt gur 2.4. C Matrisene i den diskrete tilstandsrommodellen Q R Kovariansmatrisene til hhv. prosess- og malesty K KF-forsterkningen x Apriori-estimat av tilstandsvektoren ^x Aposteriori-estimat av tilstandsvektoren X ^X Apriori/postriori-estimat av kovariansmatrisen til tilstandsvektoren

10 14 Tilstandsestimering Tabell 2.3: Diskret Kalman-lter. Initialverdier x k=0 = x 0 X k=0 = E[(x 0 ^x k=0 )(x 0 ^x k=0 ) T ]=X 0 KF-forsterkning K k = X k C T k [C kx k C T k + R k] 1 Tilstandsestimat oppdatering ^x k = x k + K k [z k C k x k ] Kovarians oppdatering ^X k =[I K k C k ] X k [I K k C k ] T +K k R k K T k Neste tidskritt av tilstandsest. x k+1 = k ^x k + k u k Neste tidskritt av kovarians X k+1 = k ^X k T k + k Q k T k w k v k u k Virkelig prosess z k k x k+1-1 z x k C k z k k ^x k K k " k Figur 2.4: Diskret Kalman-lter. Legg merke til at apriori og aposteriori referer til fr og etter oppdatering av estimatene. Dersom det er mange tilstander, vil denne metoden kreve stor datakraft siden ^X k og K k ma beregnes for hvert tidsskritt. Den diskrete versjonen av detstasjonre kontinuerlige lteret (2.30) br derfor benyttes sa lenge modell- og kovariansmatrisene er tilnrmet tidsinvariante. Dersom en har et system med relativt stor modellusikkerhet, kan man med fordel starte med det diskrete Kalman-lteret over. Hvis systemet er tidsinvariant vil da KF-forsterkningen K konvergere mot K1 og en kan ga over til det stasjonre Kalman-lteret som trenger mindre regnekraft.

11 2.3 Kalman-lterligningene 15 Eksempel 2.2 (Diskret kontinuerlig KF i Matlab) R Q x = x_0 X = X_0 % valgbare matriser % initielle verdier dim_x = max(size(x)) for i=1:n, % tid k les inn maaling z les inn paadrag u K = X*C'*inv(C*X*C'+R) x_hat = x + K*(z-C*x) IKC = eye(dim_x)-k*c X_hat = IKC*X*IKC' + K*R*K' lagre estimat x_hat % tid k+1 x = PHI*x_hat + DELTA*u X = PHI*X_hat*PHI' + GAMMA*Q*GAMMA' end Sammenligning av diskret Kalman-lter og diskretisert stasjonrt kontinuerlig Kalman-lter Det er viktig a legge merke til at ytelsen for det diskrete Kalman-lteret og det diskretiserte kontinuerlige Kalman-lteret vil vre forskjellig spesielt nar tastefrekvensen er liten. Dette er ofte tilfelle for navigasjonsystemer basert pa GPS eller HPR siden disse ofte har en tastetid opp mot et sekund. Forskjellen ligger i oppdateringen av ^x. Fra tabell 2.3 har vi: hvilket kan omskrives til: ^x k = x k + K k [z k C k x k ] (2.36) x k+1 = ^x k+1 K k+1 " k+1 (2.37) hvor " k+1 = z k+1 C k+1 x k+1. Dersom vi substituterer (2.37) inn i utrykket for x k+1 i tabell 2.3 far vi: ^x k+1 = k ^x k + k u k + K k+1 " k+1 (2.38) Dersom vi diskretiserer den kontinuerlige Kalman-lter likningen:

12 16 Tilstandsestimering _^x(t) =A ^x(t) +B u(t)+k"(t) (2.39) ved a benytte transisjonsmetoden under antakelse at _u(t) = 0 og _z(t) = 0 mellom hver tasing far vi: ^x k+1 = ^x k + u k + K k " k (2.40) Vi ser na en viktig forskjell mellom den diskrete representasjonen og den diskretiserte kontinuerlige representasjon i oppdateringsleddet. Det diskrete lteret bruker malinger ved tiden k + 1 mens den diskretiserte kontinuerlige representasjon oppdaterer ved tiden k jmf. leddene K k+1 " k+1 og K k " k. Dette kan gi store utslag dersom tastefrekvensen er liten. Det er viktig a merke seg at antakelsen _z(t) = C _x(t)+ _v(t) = 0 i det kontinuerlige tilfellet mellom tastingene er hovedarsaken til dette problemet. En annen diskretiseringsmetode hvor dynamikken til z(t) innkluderes br derfor vurderes i de tilfeller hvor dette er viktig. Likevel vil det diskretiserte kontinuerlige Kalman-lteret med antakelsen _z(t) = 0 i mange tilfeller vre en svrt god approksimasjon Ulinert diskret Kalman-lter Ta utgangspunkt i den ulinere tidsinvariante tilstandsrommodellen: _x(t) = f (x(t) u(t)) + Ew(t) z(t) = h(x(t)) + v(t) + (Euler diskretisering) (2.41) x k+1 = f k (x k u k )+ w k z k = h k (x k )+v k hvor f k (x k u k )=x k + hf(x k u k )og = he. Siden dette er et ulinert system br det ulinere (utvidete) Kalman-lteret benyttes. Det ulinere KF (presentert i tabell 2.4) fungerer bade for tidsvarierende og tidsinvariante systemer. Vi ser her at i et ulinert KF foretar man en linearisering av bade den ulinere funksjonen f (x), og maleligningen h(x), omkring gjeldene (apriori) tilstandsestimat. Dersom en har et system der dynamikken er liner og malingen er uliner (eller omvendt), er det ingen ting i veien for a kombinere metoden for diskret KF og ulinert KF. Stasjonrt KF kan uansett ikke benyttes for et system med en eller ere ulineariteter (i safall ma systemet frst lineariseres om et arbeidspunkt). Eksempel 2.3 (Matlab program for ulinert diskret KF) Ta utgangspunkt i det andreordens systemet:

13 2.3 Kalman-lterligningene 17 Tabell 2.4: Ulinert diskret Kalman-lter. Dynamikk x k+1 = f k (x k u k )+ k w k w k N(0 Q k ) Maling z k = h k (x k )+v k v k N(0 R k ) Initialverdier ^x k=0 = x 0 ^X k=0 = X 0 KF-forsterkning h K k = X k H T k H k X k H T + R k k Tilstandsestimat oppdatering ^x k = x k + K k [z k h k (x k )] Kovarians oppdatering ^X k =[I K k H k ] X k [I K k H k ] T + K k R k K T k Neste tidsskritt av x k+1 = f k (^x k u k ) tilstandsestimat Neste tidsskritt av X k+1 = ^X k k T + T k k Q k k kovarians Denisjoner k k k i 1 H k k() x k =x k x k =x k _x 1 = x 2 (2.42) _x 2 = ax 2 jx 2 j + bu+ ew (2.43) hvor x 1 er posisjon, x 2 er hastighet og a = 1, b =1og e =1er konstanter. Et ulinert diskret KF kan benyttes til a estimere hastigheten x 2 dersom posisjonen x 1 kan males dvs.: z = x 1 + v (2.44) Et Matlab-program for dette systemet er gjengitt nedenfor. Simuleringsresultatene er vist i gur 2.5. Figuren viser at KF kan produsere et nesten styfritt estimat av hastigheten selv om posisjonsmalingen og prosessen er utsatt for hvit sty. % Ulineaert diskret Kalman-filter h = 0.1 % tastetid (s) N = 1000 % antall iterasjoner a = -1 % modellparametre b = 1 e = 1 x = [0 0]' % startverdier for x og u u = 0 x_bar = [0 0]' % estimatorparametre X_bar = diag([1 1]) Q = 1

14 18 Tilstandsestimering R = 10 tabell = zeros(n+1,9) for i=1:n+1, t = (i-1)*h % maaling z = x(1)+0.1*randn(1) % minneallokering % tid (sek) % posisjon + hvit stoey % innovasjon (estimeringsfeil) z_bar = x_bar(1) eps = z-z_bar % kontinuerlig prosessmodell u = 0.1*sin(0.1*t) w = 0.1*randn(1) f = [ x(2) a*x(2)*abs(x(2))+b*u] E = [0 e]' x_dot = f + E*w % paadrag % prosesstoey % dx/dt = f + Ew % PHI, GAMMA, H PHI = [1 h 0 1+h*2*a*abs(x_bar(2))] GAMMA = h*e H = [1 0] % KF forsterkning K = X_bar*H'*inv(H*X_bar*H'+R) % korreksjon IKH = eye(2)-k*h X_hat = IKH*X_bar*IKH' + K*R*K' x_hat = x_bar + K*eps % lagring i tabell tabell(i,:) = [t x' x_hat' X_hat(1,1) X_hat(2,2) eps z] % diskret KF-modell f_hat = [x_hat(2) a*x_hat(2)*abs(x_hat(2))+b*u] f_k = x_hat + h*f_hat % prediksjon (k+1) x_bar = f_k X_bar = PHI*X_hat*PHI' + GAMMA*Q*GAMMA' % Euler integrering av prosessmodell (k+1) x = x + h*x_dot end % Plotting av figurer t = tabell(:,1)

15 2.3 Kalman-lterligningene 19 x = tabell(:,2:3) x_hat = tabell(:,4:5) X_hat = tabell(:,6:7) eps = tabell(:,8) z = tabell(:,9) clg subplot(221),plot(t,z),hold on,plot(t,x_hat(:,1)),hold off xlabel('tid (s)'),title('z og x1_hat'),grid subplot(222),plot(t,x(:,2)),hold on,plot(t,x_hat(:,2)),hold off xlabel('tid (s)'),title('x2 og x2_hat'),grid subplot(223),plot(t,eps),xlabel('tid (s)'),title('innovasjon'),grid subplot(224),plot(t,x_hat),xlabel('tid (s)'),title('diagonal av X_hat'),grid 8 z og x1_hat 0.4 x2 og x2_hat tid (s) innovasjon tid (s) 1 diagonal av X_hat tid (s) tid (s) Figur 2.5: Eksempel 2.3: Estimering av hastighet fra stybelagt posisjonsmaling ved hjelp av ulinert diskret Kalman-lter Hvordan bestemme prosess- og malestykovariansmatrisene? Ved bruk av KF ma prosess- og malesty komponentene i tilstandsrommodellene (v i w i )vresakalte normalfordelte (Gaussiske) hvitsty prosesser med null middelverdi (heretter kun betegnet som hvitsty). En slik hvitstyprosess (t), beskrives av kun en parameter nemlig standardavviket, og vi skriver da:

16 20 Tilstandsestimering (t) N(0 2 ) (2.45) der N(0 2 ) betegner normalfordeling med null middelverdi og varians 2.En vektor (t) av hvitstyprosesser, skrives tilsvarende: (t) N(0 F) (2.46) der F er kovariansmatrisen til. Denisjon 2.3 (Kovarians) Kovariansmatrisen til en vilkarlig vektor y er gitt av: covfyg =Ef(y y)(y y) T g (2.47) der Efg er forventningsverdi, og y er middelverdi. Kovariansmatrisen til prosess- og malestyvektorene betegnes med henholdsvis Q og R, og vi skriver da: w(t) N(0 Q) v(t) N(0 R) (2.48) Dersom vektorelementene er uavhengige av hverandre (dvs. at de enkelte stykomponentene ikke pavirker hverandre), vil kovariansmatrisen bli diagonal. Siden hvitsty har null middelverdi far vi da f.eks. for malestyen: R =Efvv T g = diagf ::: 2 p g (2.49) Vi ser at kvadratet av standardavviket (variansen) ligger langs diagonalen. For komersielle maleinstrumenter vil det statiske standardavviket til malestyen som regel vre oppgitt, sa R-matrisen pleier a vre grei a sette opp. Nar det gjelder a sette opp Q-matrisen, som innebrer a nne standardavviket til de stokastiske prosessforstyrrelsene (som vind, strm, modellfeil, feildynamikk i maleinstrumenter, osv.) samt koblinger mellom disse, ma vi foruten mulige matematiske beregninger basere oss pa statistiske data eller sunn fornuft. Om vi ikke skulle nne korrekte data ved frste forsk, er det dessuten mulig a justere Q-matrisen (og dermed Kalman-lteret) on-line. Bestemmelse av Kalman-lterets kovariansmatriser Det er frst og fremst forholdet mellom kovariansmatrisene Q og R som er avgjrende for et godt Kalman-lterresultat. Man kan derfor med fordel justere forholdet mellom disse to matrisene ved a angi Q slik: Q = Q o (2.50) der Q o er den forhandsberegnete kovariansmatrise, mens er en skaleringsparameter. Selv om man na kan justere Q ved hjelp av parameteren, ma en likevel

17 2.3 Kalman-lterligningene 21 prve aoppna innbyrdes riktig forhold mellom elementene i Q o. Dette gjres vanligvis ved a benytte den inverse kvadratiske metoden til Bryson & Ho. (1969). Anta at vektmatrisene er diagonale dvs: Q o = diagfq1 ::: q n g R =diagfr1 ::: r m g (2.51) Algoritme 2.1 (Bryson's inverse kvadratiske metode) 1. Angi maksimale tilatte estimeringsavvik x i =maxjx i ^x i j og beregn: ( q i = 1 (x i ) 2 ) n i=1 2. Velg malevariansene slik at ( r j = 1 (y j ) 2 ) m j=1 hvor y j =maxjy j y j j hvor y j er middelverdien til den statiske malingen og y j er det strste observerte maleavviket fra denne middelverdien. Dersom informasjon om malevariansen (standardavviket) er gitt brukes denne istedet direkte i uttrykket for R, se (2.49). 3. Beregn KF forsterkningen K og simuler systemet. En darlig tidsrepons forbedres vedamodisere vektene q i og r j samt skaleringsparameteren. 4. Punkt 3 repeteres til en tilfredsstillende respons oppnas. Modellering av stokastiske prosesser (hvit og farget sty) Dersom man har stokastiske prosesser som ikke er eksitert hvitsty, men av sakalt farget sty, ma en frst lage en stymodell som er eksitert (drevet) av hvitsty. Fra signalbehandlings teorien, se f.eks. Minkler & Minkler (1993), har vi nemlig at ethvert stokastisk signal kan beskrives som et system (modell) eksitert av hvitsty. Stymodellen kan sa innlemmes i tilstandsrommodellen slik at Kalman- lterligningene kan benyttes. Eksempel 2.4 (Modellering av stokastiske forstyrrelser pa et skip) En forenklet modell av et skip som beveger seg rett forover (jag) er gitt av: mx(t) = d _x(t)+u(t)+dv c (t)+kv w (t) z1(t) = x(t)+v1(t) z2(t) = V w (t)+v2(t) (2.52)

18 22 Tilstandsestimering der x er posisjonen, _x er hastigheten forover, u er skyvkraften fra propellen, z 1 og z 2 er malingene, v 1 og v 2 er malestyen (hvit), V c er strmhastigheten, V w er vindhastigheten, mens m, d og k er konstanter bestemt av hhv. skipets masse, demping i vannet og luftmotstand. Skriver vi dette systemet pa tilstandsrom form far vi: _x = " 0 1 d 0 m # x + " 0 1 m # u + " 0 0 d k m m # w f (2.53) der x = [x _x] T og w f = [V c V w ] T. Forstyrrelsene w f er her naturligvis ikke hvitsty siden det her er snakk om strm- og vindhastigheter, men de er likevel stokastiske prosesser. For bruk i KF trenger vi derfor en hvitstyeksitert modell som beskriver forstyrrelsene. Vi foreslar flgende modell for strmhastighet: og vindhastighet: _V c = w 1 (2.54) _V w1 = w 2 (2.55) _V w2 = 1 T V w2 + w 3 (2.56) V w = V w1 + V w2 (2.57) der T er en tidskonstant og w i i = 1:::3 er hvitsty prosesser. Vi ser vi har modellert langsomtvarierende strmhastighet som integrert hvitsty mens vindhastigheten er modellert som summen av en langsomtvarrierende komponent V w1 og en turbulent komponent V w2 (integrert lavpassltrert hvitsty). Trekker vi sa dette inn i modellen (2.53) far vi: _x = z = " d d k k 0 m m m m T # 3 2 x m u w (2.58) x + v (2.59) Vi har na utvidet tilstandsvektoren slik at x =[x _x V c V w1 V w2 ] T og har innfrt z =[z 1 z 2 ] T, v =[v 1 v 2 ] T og w =[w 1 w 2 w 3 ] T. Vi har dermed oppnadd en tilstandsrommodell der prosess- og malestykomponentene alle er hvitstyprosesser. Vi kan derfor benytte et KF til a estimere ^x fra malingene z og u, setabell Kovariansmatrisene velges som:

19 2.4 Luenberger-estimator Q = 4 q q q R = " r r 22 # (2.60) 2.4 Luenberger-estimator Dersom estimatorforsterkningen K i Kalman-lteret beregnes pa en deterministisk mate istedet for a lse Kalman-lterlikningene sa blir tilstandsestimatoren ofte kalt en Luenberger-estimator. Dette kan f.eks. gjres ved a bruke en polplasseringsalgoritme. Ta utgangspunkt i et linert tidsinvariant system Luenberger-estimatoren velges som: _x(t) = Ax(t)+Bu(t)+Ew(t) (2.61) z(t) = Cx(t)+v(t) (2.62) _^x(t) = A^x(t)+Bu(t)+K[z(t) C ^x(t)] (2.63) ^z(t) = C ^x(t) (2.64) hvor K er en konstant valgbar matrise. Estimeringsfeilen ~z = z ^z er gitt av: _~x(t) = (A KC) ~x(t) +Ew(t) (2.65) ~z(t) = C ~x(t)+v(t) (2.66) Vi antar vidre at middelverdiene til styen tilfredsstiller E(v) = 0 og E(w) = 0 (hvit sty antakelse). Stabilitet av systemet (2.65) er gitt av egenverdiene til A KC.Vikan derfor bruke enegenverdianalyse som et kriterium for a beregne K. Vi krever med andre ord at realdelene til egenverdiene av A KC skal ligge ivenstre halvplan dvs.: Re f i fa KCgg < 0 (2.67) for i = 1...n. Dette vil medfre at estimatoren er global asymptotisk stabil som igjen medfrer at ~x! 0. En enkel mate a gjre denne polplasseringen pa er a benytte Matlab. Eksempel 2.5 (Luenberger-estimator basert pa polplassering i Matlab) LAM = [l_1,l_2...l_n] % egenverdier for A-KC K0 = place(a',c',lam) % MIMO polplasseringsalgoritme K = K0' % Luenberger forsterkningsmatrise

20 24 Tilstandsestimering 2.5 Lyapunov-basert tilstandsestimering Denne seksjonen gir en introduksjon til Lyapunov-basert tilstandsestimering Lyapunov-stabilitet av Kalman-lteret I den linere analysen vil vi ta utgangspunkt i Kalman-lteret (evt. Luenbergerestimatoren) skrevet pa formen: _x(t) = Ax(t)+Bu(t) (2.68) z(t) = Cx(t) (2.69) Estimeringsfeilen ~x = x ^x er gitt av: Ta utgangspunkt i Lyapunov-funksjonen: _~x(t) = (A KC) ~x(t) (2.70) ~z = C ~x(t) (2.71) Derivasjon av V m.h.p. tiden gir: V = ~x T P 1 ~x P = P T > 0 (2.72) _V = ~x d T P 1 ~x + ~x T P 1 T ~x _ + ~x _ P 1 ~x (2.73) dt! d = ~x T P 1 + P 1 A + A T P 1 P 1 KC C T K T P 1 ~x (2.74) dt Den tidsderiverte av P 1 omskrives ved a derivere: hvilket gir: PP 1 = I (2.75) Vi far da: hvor d dt P = _ P. Dette gir: d dt PP 1 + P d dt P 1 = 0 (2.76) d dt P 1 = P 1 _PP 1 (2.77) _V = ~x T P 1 _P + AP + PA T KCP PC T K T P 1 ~x (2.78)

21 2.5 Lyapunov-basert tilstandsestimering 25 Global asymptotisk stabilitet I Kalman-lteret velges: K = PC T R 1 R = R T > 0 (2.79) Substitutering av uttrykket for K inn i _ V gir: _V = ~x T P 1 _P + AP + PA T 2PC T R 1 CP P 1 ~x (2.80) Kovariansmatriselikningen i Kalman-lteret er valgt som: hvilket gir: _P = AP + PA T + Q PC T R 1 CP (2.81) eller _V = ~x T P 1 Q + PC T R 1 CP P 1 ~x < 0 8x 6= 0 (2.82) _V = ~x T P 1 QP 1 ~x ~z T R 1 ~z 8x z 6= 0 (2.83) Vi ser da at V gar mot null siden vektmatrisene P > 0, Q > 0 og R > 0 alle er positive denitte matriser. I henhold til Lypunovs direkte metode er da Kalman-lteret globalt asymptotiskt stabilt. Et deterministisk valg av kovariansmatrisen Q er: hvilket gir: Q = PQ 0 P (2.84) _V = ~x T Q 0 ~x ~z T R 1 ~z 8x z 6= 0 (2.85) hvor Q 0 vanligvis velges som en diagonal matrise med positive elementer pa diagonalen. Kommentar: For linere systemer impliserer global asymptotisk stabilitet eksponesiell stabilitet. Dette kan f.eks. vises ved a benytte egenverdibetrakninger. Vi kan skrive: V = ~x T P 1 ~x m ~x T ~x (2.86) hvor m er den minste egenverdien til P 1. Tilsvarende vil: _V = ~x T P 1 Q + PC T R 1 CP P 1 ~x M ~x T ~x (2.87) hvor M er den strste egenverdien til P 1 (Q+PC T R 1 CP)P 1.Ved abenytte ulikheten (2.86) kan vi skrive om (2.87) til:

22 26 Tilstandsestimering _V M ~x T ~x M V m = V (2.88) hvor = M = m.viserdaatv og dermed ~x avtar eksponensielt siden: V (t) e t V (0) (2.89) Dette bekrefter at global asymptotisk stabiltet og eksponensiell stabilitet er ekvivalente begreper i linere systemer. Dette er imidlertid ikke tilfelle for ulinere systemer.