DESIGN AV KALMANFILTER Oddvar Hallingstad UniK Hva er et Kalmanfilter? Kalmanfilteret er en rekursiv algoritme som ved å prosessere målinger av inngangen og utgangen av et system og ved å utnytte en matematisk modell av systemet beregner et estimat av systemets tilstand. Det kan vises at under gitte betingelser er Kalmanfilteret en optimal estimator. Hva er Kalmanfilterets styrke? Kalmanfilterets styrke er at det kan veie sammen målinger fra sensorer med forskjellig nøyaktighet og tatt ved ulike tidspunkter. Algoritmen er rekursiv og egner seg godt for implementering på datamaskin. Hva er Kalmanfilterets svakhet? Kalmanfilterets svakhet er at det forutsetter at det fysiske systemet lar seg modellere eksakt med endeligdimensjonale ordinære differensiallikninger, at støyen er hvit og at vi kjenner støyens 1. og 2. ordens moment. Page 1
Foredragets innhold Notasjon og nomenklatur Kalmanfilter for lineære systemer Kalmanfilter brukt ved glatting Kalmanfilter for ulineære systemer Suboptimale Kalmanfilter Numeriske metoder Design av Kalmanfilter for kritiske systemer Spiller notasjonen noen rolle? Eksempel hentet fra Schweppes Uncertain dynamic systems x!!n " 1 N " 1" #!!N"x!!N N" " K!N " 1" $ #z!n " 1"! H!N " 1"!!N"x!!N N"$ K!N " 1" #%!N " 1 N"H &!N " 1"%H!N " 1"%!N " 1 N"H &!N " 1" " R!N " 1"&!1 Ekvivalente likninger skrevet med en annen notasjon: x' k"1 #! k x! k x! k # x' k " K k! z k! H k x' k " K k # P' kh T k!h kp' kh T k " R k"!1 Page 2
Nomenklatur Filtermodell (S * ) Filtermodellens anslag av system- og støymatrisene isystemet w u Prosess (o.d.l.) x Sensor (a.l) z Kalmanfilter x! x' (Fysisk) System (S) x u w Tilstand Pådrag Prosesstøy z Måling x! Estimert tilstand x' Prediktert tilstand Estimeringstyper x!!t 1 t 2" x!!t 2" x'!t 3" t 1 t 2 t 3 tiden Måleintervallet x!!t 1 t 2" Glatta tilstand x!!t 2" Filtrert tilstand x'!t 3" Prediktert tilstand Page 3
Filtermodellen for et kontinuerlig-diskret KF x( # F x " L u " Gw z k # H x k " v k Prosesslikning : alle matriser og pådraget antas kjente Målelikning : målematrisa antas kjent " N x! 0, P! 0 w!t" " N! 0,Q!!t! """ v k " N! 0,R d ", w!t" og v k er ukorrelerte }Middelverdier, spektraltetthesmatrisen og målekovariansmatrisa antas kjente Disse matrisene er vesensforskjellige Hvitstøy Kontinuerlig hvitstøy er ikke fysisk fordi den har uendelig effekt. Den kan likevel brukes til å modellere fysisk støy fordi en kan lage et filter som har hvitstøy på inngangen og som gir det riktige effektspekteret ut Q w!t" Filter y!t" I alle utledninger hvor kontinuerlig hvitstøy er involvert inngår blir den integrert opp. Page 4
Optimalitet av Kalmanfilteret Kalmanfilteret kan utledes under flere forskjellige antagelser, bl a: Dersom vi bare kjenner middelverdier og kovarianser til støyprosessene kan det vises at Kalmanfilterlikningene er et minimum variansestimat. Dvs det minimalisere. trace%p! & Dersom støyprosessene er normalfordelte kan det vises at Kalmanfilteret minimaliserer den midlere kvadratiske feilen, dvs den minimaliserer E%! x! x! " T! x! x! " z & Dersom systemet er lineært, men støyprosessene ikke er normalfordelt og vi kjenner sannsynlighetsfordelingene er Kalmanfilteret vanligvis ikke optimalt. Kalmanfilteret for et kontinuerlig-diskret system For den kontinuerlig-diskrete filtermodellen vi viste foran får vi Kalmanfilteret:. x' # F x' " L u, x! k er gitt. P' # FP' " P' F T " GQG T, P! k er gitt Tidsoppdatering x! k # x' k " K k! z k! H k x' k " K k # P' k H T k!h k P' k H kt " R k"!1 Måleoppdatering P! k #!I! K k H k"p' k Page 5
Farget og/eller korrelert måleog prosesstøy Forutsetningene til Kalmanfilteret er da ikke oppfyllt, men en kan få den over på standardformen igjen : 1. Farget prosesstøy: utvid tilstandvektoren 2. Korrelasjon mellom prosess- og målestøy: Kan dekorreleres. I det kontinuerlige tilfellet blir forsterkningen: K # %PH T " GC&R!1 hvor E%w!t" v T!""& # C!t"!!t! "" 3. Dersom en har farget målestøy kan en i visse tilfeller utvide tilstandsvektoren, men generelt kan en lage seg en ny måling hvor en må bruke 2 for å få likningene over på standardform. Antagelsene til Kalmanfilteret er derfor ikke så restriktive som de kan se ut når det gjelder støyantagelsene Stokastisk ekvivalens Gitt de diskrete og kontinuerlige stokastiske prosessene: x k"1 #! k x k ") k u k "* k w k " N! x' 0,P' 0"; w k " N! 0,Q d! kl"; og w k er ukorrelerte x( # F x " L u " Gw " N! x' 0,P' 0"; w!t" " N! 0,Q!!t! """; og w er ukorrelerte Dersom vi krever at middelverdiene og kovariansene utvikler seg likt for de to systemene, dvs at x' k # x'!t k" og P' k # P'!t k " får vi sammenhengene:! #!!t k"1, t k " t ) u k # # tk!!t, ""L u!""d" Får ofte forskjellig dimensjon *Q d* T t # # k"1 tk!!tk"1, ""GQG T! T!t k"1, ""d" Page 6
Simulering av kontinuerlig stokastisk system 1. Beregn!!T", )!T" 2. Cholesky-faktoriser P 0 : P 0 # C T 0 C 0 Trekk n standardnormalfordelte tall #,finnså fra : # x! 0 " C T 0 # 3. Cholesky-faktoriser S #*Q d * T : S # C T C; sett *#C T og Q d # I 4. Trekk n standardnormalfordelte tall $ og sett w k # $, beregn så x k"1 #! x k ")u k "*w k Gjenta dette trinnet for k # 0, 1,2, + Det er ikke enkelt å bruke en integrasjonsrutine på et kontinuerlig stokastisk system! Algebraisk ekvivalens Kalmanfilteret har mange algebraisk ekvivalente former for Kalmanfilter likningene 1 x! # x' " K! z! H x' " K # P' H T!HP' H T " R "!1 P! #!I! KH"P' 3 x! # x' " K! z! H x' " P! #!P'!1 " H T R!1 H"!1 K # P! H T R!1 2 x! # x' " K! z! H x' " K # P' H T!HP' H T " R "!1 4 x! # P! P'!1 x' " P! H T R!1 z P! #!P'!1 " H T R!1 H"!1 P! #!I! KH"P'!I! KH" T " KRK T I tillegg har vi formler for informasjonsfilteret, og desentraliserte Kalmanfilter Ŷ # P!!1,!1 Y' # P' Informasjonsmatrisa ŷ # Ŷx!, y' # Y' x' Informasjonstilstanden Page 7
Blokkskjema for lineært Kalmanfilter Blokkskjemaet for Kalmanfilteret sammen med systemet det er koblet til: v k w u Prosess x!t" x k H z k - K k System KF prosess x'!t" x' k H x! k Blokkskjema for linearisert Kalmanfilter x( # f! x, u " " Gw Filtermodell! x( # F!t"! x " Gw! z k # H k! x " v k v k w x!t" x z u Prosess k k h!' " -! x! k Kalmanfilter! x'!t" KF prosess x,!t" x, k h!' " x, 0 x! k x'!t" ' x, # f! x,, u " Page 8
Blokkskjema for utvidet Kalmanfilter Blokkskjemaet for Kalmanfilteret med systemet det er koblet til: Filtermodell! x( # F!t"! x " Gw x( # f! x, u " " Gw! z k # H k! x " v k v k w u Prosess x!t" x k h!' " z k - K k! x! k KF prosess x'!t" x' k h!' " x! k ' x' # f! x', u " Suboptimale Kalmanfilter Vi klarer skjelden å modellere prosessen, sensoren og støyene eksakt i et Kalmanfilter. Ofte velger vi bevisst en lavereordens modell for Kalmanfilteret fordi dette kan gi et mer robust filter overfor parametervariasjoner. Det vil for et subotimalt Kalmanfilter være 3 interessante kovariansmatriser: P o P $ P s : Kovariansen til det optimale systemet, dvs dersom S*=S : Kovariansen Kalmanfilteret beregner : Den sanne kovariansen til estimeringsfeilen For disse kovariansene har vi sammenhengene: mens den kovariansen Kalmanfilteret beregner ikke trenger å ha noen sammenheng med den sanne kovariansen : P o % P s P $ & P s Page 9
For å undersøke hvordan det suboptimale KF oppfører seg undersøkes statistikken til ê k Dette kan gjøres vha kovarians- eller Monte Carlosimulering i det lineære og vha Monte Carlo-simulering i det ulineære tilfellet v k x! 0 $ w x z k Prosess Sensor Kalmanfilter x! k $ - ê k N Kovariansanalyse Dersom systemet foran er lineært kan vi finne sanne middelverdier og sann kovarians for estimeringsfeilen ê k vha kovarianslikningene. Disse likningene har dimensjon n+n* i det generelle tilfellet. De har samme struktur som de vanlige Kalmanfilterlikningene Monte Carlo simulering Dersom systemet er ulineært må vi gjøre en Monte Carlo simulering for å beregne et anslag av: E# ê k $ og Kov# ê k $ Vi implementer system og Kalmanfilterlikningene på datamaskin, trekker, w k og v k fra de gitte fordelingene og beregner ê k for k # 0, 1, +,M Ved å foreta nye trekninger kan vi generere et sett trajektorer: ê k i, k # 0,1, +,M, i # 1, 2,+, N Page 10
Dette gir oss en rekke trajektorer: ê ê 2 ê 1 ê N t k Dersom vi lagrer unna trajektorene kan vi beregne estimatene m! N k # 1 N N ' i#1 ê k i P! k N 1 # ' N N!1 i#1 ê k i! m k N ê k i! m k N T Numeriske metoder Algebraisk ekvivalens sikrer ikke numerisk ekvivalens fordi flyttall har en endelig oppløsning. a "!! a #! a"! " a Tilsvarende skjer ved måleoppdateringen i Kalmanfilteret. Dersom vi får inn en svært god måling vil kovariansen bli kraftig redusert ved at de to ledda på høyre siden er nesten like store: P! k # P' k! P' kh T!HP' kh T " R "!1 HP' k Ved å bruke UD-faktorisering eller en annen kvadratrotalgoritme forbedres de numeriske egenskapene radikalt Page 11
Andre numeriske problem forbundet med Kalmanfilteret Beregning av matrise eksponentialfunksjonen Diskretisering av målingen med approksimasjonen Gir ofte dårligere resultat enn tilsvarende Forsøk på bruk av en Runge Kutta metode på den stokastiske differensiallikningen ' x # F x " L u " Gw e F'-t R d # Q d # 1 R -t 1 Q -t Design av Kalmanfilter for kritiske system 1. Sett opp spesifikasjonene for systemet (krav til nøyaktighet) 2. Velg prosessmodell og modeller aktuelle sensorer 3. Bruk et simuleringsprogram med høy numerisk nøyaktighet både mht diskretisering og ordlengde Lag et optimalt KF, dvs bruk S*=S Kjør kovariansanalyse og lag et feilbudsjett for å finne de sensorene som er kritisk mht ytelsen Når spesifikasjonene er oppfyllt må vi ofte foreta en modellreduksjon for å finne et KF som kan tilfredstille kravene til regnehastighet og lagerplass Kjør kovariansanalyse/mont Carlo simuleringer for å se om Kalman-filteret tilfredstiller spesifikasjonene. Page 12
4. I et simuleringsprogram hvor KF er implementert med de diskretiseringsalgoritmene og den ordlengden som vil bli brukt: Kjør kovariansanalyse/mont Carlo simuleringer for å se om Kalman-filteret tilfredstiller spesifikasjonene. 5. Kjør om mulig HW-in-the-loop simuleringer for å test KF sammen med den HW som vil bli brukt. Page 13