Invariant udvidet Kalman filter - Invariant extended Kalman filter

Den invariant udvidet Kalman-filter (IEKF) (ikke at forveksle med den gentog udvidet Kalman filter) blev først introduceret som en version af den udvidede Kalman-filteret (EKF) til ikke-lineære systemer besidder symmetrier (eller invariances ), derefter generaliseret og omarbejdning som en tilpasning til Lie-grupper i den lineære Kalman-filtreringsteori . I stedet for at bruge en lineær korrektionsperiode baseret på en lineær outputfejl bruger IEKF en geometrisk tilpasset korrektionsperiode baseret på en uændret outputfejl; på samme måde opdateres forstærkningsmatricen ikke fra en lineær tilstandsfejl, men fra en uforanderlig tilstandsfejl. Den største fordel er, at gevinst- og kovariansligningerne har reduceret afhængighed af statens estimerede værdi. I nogle tilfælde konvergerer de til konstante værdier på et meget større sæt baner, end det er tilfældet for EKF, hvilket resulterer i en bedre konvergens af estimeringen.

Filterafledning

Diskret tidsramme

Overvej et system, hvis tilstand er kodet til tidstrin af et element i en Lie-gruppe, og dynamikken har følgende form: ${\ displaystyle n}$${\ displaystyle x_ {n}}$ ${\ displaystyle G}$

${\ displaystyle x_ {n} = \ phi _ {n} (x_ {n-1}) \ cdot u_ {n}}$

hvor er en gruppeautomisme af , er gruppens operation og et element af . Systemet formodes at blive observeret gennem en måling med følgende form: ${\ displaystyle \ phi _ {n}}$${\ displaystyle G}$${\ displaystyle \ cdot}$${\ displaystyle u_ {n}}$${\ displaystyle G}$${\ displaystyle y_ {n}}$

${\ displaystyle y_ {n} = x_ {n} \ cdot b_ {n}}$

hvor hører til et vektorrum udstyret med en venstre handling af elementerne betegnet igen med (som ikke kan skabe forvirring med gruppearbejdet, da det andet medlem af operationen er et element af , ikke ). Alternativt gælder den samme teori for en måling defineret af en rigtig handling : ${\ displaystyle b_ {n}}$${\ displaystyle {\ mathcal {Y}}}$${\ displaystyle G}$${\ displaystyle \ cdot}$${\ displaystyle {\ mathcal {Y}}}$${\ displaystyle G}$

${\ displaystyle y_ {n} = b_ {n} \ cdot x_ {n}}$

Filtrer ligninger

Det uændrede udvidede Kalman-filter er en observatør defineret af følgende ligninger, hvis målefunktionen er en venstre handling: ${\ displaystyle {\ hat {x}} _ {n}}$

${\ displaystyle {\ hat {x}} _ {n | n-1} = \ phi _ {n} ({\ hat {x}} _ {n-1 | n-1}) \ cdot u_ {n} }$

${\ displaystyle {\ hat {x}} _ {n | n} = {\ hat {x}} _ {n | n-1} \ cdot \ exp (K_ {n} ({\ hat {x}} _ {n | n-1} ^ {- 1} \ cdot y_ {n} -b_ {n}))}$

hvor er det eksponentielle kort over og er en forstærkningsmatrix, der skal indstilles gennem en Riccati-ligning. ${\ displaystyle \ exp ()}$${\ displaystyle G}$${\ displaystyle K_ {n}}$

Hvis målefunktionen er en rigtig handling, defineres opdateringstilstanden som:

${\ displaystyle {\ hat {x}} _ {n | n} = \ exp (K_ {n} (y_ {n} \ cdot {\ hat {x}} _ {n | n-1} ^ {- 1 } -b_ {n})) \ cdot {\ hat {x}} _ {n | n-1}}$

Løbende ramme

Ovenstående diskrete tidsramme blev først introduceret til kontinuerlig dynamik i formen:

${\ displaystyle {\ frac {d} {dt}} x_ {t} = f_ {t} (x_ {t}),}$

hvor vektorfeltet til enhver tid verificerer forholdet: ${\ displaystyle f_ {t}}$${\ displaystyle t}$

${\ displaystyle \ forall a, b \ i G, f_ {t} (a \ cdot b) = f_ {t} (a) b + af_ {t} (b) -af (Id) b}$

hvor gruppens identitetselement er betegnet med og bruges korthåndsnotationen (hhv. ) til venstre oversættelse (hhv. den højre oversættelse ), hvor det angiver tangentrummet til at . Det fører til mere involverede beregninger end den diskrete tidsramme, men egenskaber er ens. ${\ displaystyle Id}$${\ displaystyle gu = L_ {g} (u)}$${\ displaystyle ug = R_ {g} (u)}$${\ displaystyle L_ {g}: TG_ {x} \ rightarrow TG_ {gx}}$${\ displaystyle R_ {g}: TG_ {x} \ rightarrow TG_ {xg}}$${\ displaystyle TG_ {x}}$${\ displaystyle G}$${\ displaystyle x}$

Vigtigste egenskaber

Den største fordel ved invariant-udvidet Kalman-filtrering er opførelsen af ​​den variabelfejlvariabel, hvis definition afhænger af målingstypen. For venstre handlinger definerer vi en venstrevariabel fejlvariabel som:

${\ displaystyle e_ {n | n-1} ^ {L} = {\ hat {x}} _ {n | n-1} ^ {- 1} x_ {n}}$ ,

${\ displaystyle e_ {n | n} ^ {L} = {\ hat {x}} _ {n | n} ^ {- 1} x_ {n}}$ ,

mens vi for rigtige handlinger definerer en variabel med højre invariant som:

${\ displaystyle e_ {n | n-1} ^ {R} = x_ {n} {\ hat {x}} _ {n | n-1} ^ {- 1}}$ ,

${\ displaystyle e_ {n | n} ^ {R} = x_ {n} {\ hat {x}} _ {n | n} ^ {- 1}}$ ,

Faktisk erstatter , , deres værdier, vi får til venstre handlinger, efter nogen algebra: ${\ displaystyle x_ {n}}$${\ displaystyle {\ hat {x}} _ {n | n-1}}$${\ displaystyle {\ hat {x}} _ {n | n}}$

${\ displaystyle e_ {n | n-1} ^ {L} = u_ {n} ^ {- 1} \ cdot \ phi _ {n} (e_ {n | n-1} ^ {L}) \ cdot u_ {n}}$ ,

${\ displaystyle e_ {n | n} ^ {L} = \ exp (-K_ {n} (e_ {n | n-1} ^ {L} \ cdot b_ {n} -b_ {n})) \ cdot e_ {n | n-1} ^ {L}}$ ,

og for rigtige handlinger:

${\ displaystyle e_ {n | n-1} ^ {R} = \ phi _ {n} (e_ {n | n-1} ^ {R})}$ ,

${\ displaystyle e_ {n | n} ^ {R} = e_ {n | n-1} ^ {R} \ exp (-K_ {n} (b_ {n} \ cdot e_ {n | n-1} ^ {R} -b_ {n}))}$ ,

Vi ser, at den estimerede værdi af staten ikke er involveret i ligningen efterfulgt af fejlvariablen, en egenskab ved lineær Kalman-filtrering af det klassiske udvidede Kalman-filter deler ikke, men ligheden med det lineære tilfælde går faktisk meget længere. Lad være en lineær version af fejlvariablen defineret af identiteten: ${\ displaystyle \ xi _ {n | n-1}, \ xi _ {n | n}}$

${\ displaystyle e_ {n | n-1} = \ exp (\ xi _ {n | n-1}),}$

${\ displaystyle e_ {n | n} = \ exp (\ xi _ {n | n}).}$

Derefter, med defineret af Taylor-udvidelsen, har vi faktisk: ${\ displaystyle F_ {n}}$${\ displaystyle \ xi _ {n | n} = F_ {n} \ xi _ {n | n-1} + \ circ (|| \ xi _ {n | n-1} ||)}$

${\ displaystyle \ xi _ {n | n} = F_ {n} \ xi _ {n | n-1}.}$

Med andre ord er der ingen begreber med højere ordre: dynamikken er lineær for fejlvariablen . Dette uafhængighed af resultat og fejldynamik er kernen i IEKF's teoretiske egenskaber og praktiske ydeevne. ${\ displaystyle \ xi _ {n | n}, \ xi _ {n | n-1}}$

Forhold til observatører, der bevarer symmetri

De fleste fysiske systemer har naturlige symmetrier (eller invarians), dvs. der findes transformationer (f.eks. Rotationer, oversættelser, skaleringer), der efterlader systemet uændret. Fra matematisk og teknisk synspunkt er det fornuftigt, at et filter, der er godt designet til det betragtede system, skal bevare de samme uforanderlige egenskaber. Ideen til IEKF er en modifikation af EKF-ligningerne for at drage fordel af systemets symmetrier.

Definition

Overvej systemet

${\ displaystyle {\ dot {x}} {=} f (x, u) + M (x) w}$

${\ displaystyle y {=} h (x, u) + N (x) v}$

hvor er uafhængige hvide gaussiske lyde . Overvej en Lie-gruppe med identitet og (lokale) transformationsgrupper ( ) sådan, at . Det tidligere system med støj siges at være uforanderligt, hvis det efterlades uændret af handlingen, som transformationerne grupperer ; det vil sige hvis ${\ displaystyle w, v}$${\ displaystyle G}$${\ displaystyle e}$ ${\ displaystyle \ varphi _ {g}, \ psi _ {g}, \ rho _ {g}}$${\ displaystyle g \ in G}$${\ displaystyle (X, U, Y) = (\ varphi _ {g} (x), \ psi _ {g} (u), \ rho _ {g} (y))}$${\ displaystyle \ varphi _ {g}, \ psi _ {g}, \ rho _ {g}}$

${\ displaystyle {\ dot {X}} {=} f (X, U) + M (X) w}$ .

${\ displaystyle Y {=} h (X, U) + N (X) v}$

Filtrer ligninger og hovedresultat

Da det er et symmetri-bevarende filter , læser den generelle form for en IEKF

${\ displaystyle {\ dot {\ hat {x}}} = f ({\ hat {x}}, u) + W ({\ hat {x}}) L {\ Bigl (} I ({\ hat { x}}, u), E ({\ hat {x}}, u, y) {\ Bigr)} E ({\ hat {x}}, u, y)}$

hvor

• ${\ displaystyle E ({\ hat {x}}, u, y)}$ er en uændret outputfejl, som er forskellig fra den sædvanlige outputfejl ${\ displaystyle {\ hat {y}} - y}$
• ${\ displaystyle W ({\ hat {x}}) = {\ bigl (} w_ {1} ({\ hat {x}}), .., w_ {n} ({\ hat {x}}) { \ bigr)}}$ er en invariant ramme
• ${\ displaystyle I ({\ hat {x}}, u)}$ er en invariant vektor
• ${\ displaystyle L (I, E)}$ er en frit valgt forstærkningsmatrix.

For at analysere fejlkonvergensen defineres en uforanderlig tilstandsfejl , som er forskellig fra standardoutputfejlen , da standardoutputfejlen normalt ikke bevarer systemets symmetrier. ${\ displaystyle \ eta ({\ hat {x}}, x)}$${\ displaystyle {\ hat {x}} - x}$

I betragtning af det betragtede system og den tilknyttede transformationsgruppe findes der en konstruktiv metode til at bestemme , baseret på metoden med bevægelig ramme. ${\ displaystyle E ({\ hat {x}}, u, y), W ({\ hat {x}}), I ({\ hat {x}}, u), \ eta ({\ hat {x }},x)}$

På samme måde som EKF bestemmes forstærkningsmatricen ud fra ligningerne ${\ displaystyle L (I, E)}$

${\ displaystyle L {=} PC ^ {T} {\ bigl (} N (e) N ^ {T} (e) {\ bigr)} ^ {- 1}}$ ,

${\ displaystyle {\ dot {P}} {=} AP + PA ^ {T} + M (e) M ^ {T} (e) -PC ^ {T} {\ bigl (} N (e) N ^ {T} (e) {\ bigr)} ^ {- 1} CP}$ ,

hvor matricerne kun afhænger her af den kendte invariante vektor snarere end af som i standard EKF. Denne meget enklere afhængighed og dens konsekvenser er IEKFs hovedinteresser. Faktisk er matricerne konstant på et meget større sæt af baner (såkaldte permanente baner ) end ligevægtspunkter, som det er tilfældet for EKF. I nærheden af ​​sådanne baner er vi tilbage til det "sande", dvs. lineære Kalman-filter, hvor konvergens er garanteret. Uformelt betyder det, at IEKF generelt konvergerer i det mindste omkring enhver langsomt varierende permanent bane snarere end bare omkring ethvert langsomt varierende ligevægtspunkt for EKF. ${\ displaystyle A, C}$${\ displaystyle I ({\ hat {x}}, u)}$${\ displaystyle ({\ hat {x}}, u)}$${\ displaystyle A, C}$

Eksempler på anvendelse

Attitude- og kursreferencesystemer

Invariant udvidede Kalman-filtre bruges for eksempel i attitude- og kursreferencesystemer . I sådanne systemer estimeres orientering, hastighed og / eller position for et bevægeligt stift legeme, f.eks. Et fly, ud fra forskellige indlejrede sensorer, såsom inerti-sensorer, magnetometre, GPS eller ekkolod. Brugen af en IEKF naturligt fører til at overveje quaternionen fejl , der ofte bruges som en ad hoc- trick for at bevare de begrænsninger af quaternionen gruppe. Fordelene ved IEKF sammenlignet med EKF vises eksperimentelt for et stort sæt baner. ${\ displaystyle {\ hat {q}} q ^ {- 1}}$

Træningsnavigation

En vigtig anvendelse af Invariant-udvidet Kalman-filter er inertial navigation , som passer til rammen efter indlejring af tilstanden (bestående af holdningsmatrix , hastighedsvektor og positionsvektor ) i Lie-gruppen defineret af gruppedriften: ${\ displaystyle R}$${\ displaystyle v}$${\ displaystyle x}$${\ displaystyle SE_ {2} (3)}$

${\ displaystyle (R_ {1}, v_ {1}, x_ {1}) \ cdot (R_ {2}, v_ {2}, x_ {2}) = (R_ {1} R_ {2}, x_ { 1} + R_ {1} x_ {2}, v_ {1} + R_ {1} v_ {2})}$

Samtidig lokalisering og kortlægning

Problemet med samtidig lokalisering og kortlægning passer også til rammerne for invariant udvidet Kalman-filtrering efter indlejring af staten (bestående af holdningsmatrix , positionsvektor og en sekvens af statiske modningspunkter ) i Lie-gruppen (eller for plane systemer) defineret af gruppedrift: ${\ displaystyle R}$${\ displaystyle x}$${\ displaystyle p ^ {1}, \ prikker, p ^ {K}}$${\ displaystyle SE_ {K + 1} (3)}$${\ displaystyle SE_ {K + 1} (2)}$

${\ displaystyle (R_ {1}, x_ {1}, p_ {1} ^ {1}, \ prikker, p_ {1} ^ {K}) \ cdot (R_ {2}, x_ {2}, p_ { 2} ^ {2}, \ dots, p_ {2} ^ {K}) = (R_ {1} R_ {2}, x_ {1} + R_ {1} x_ {2}, p_ {1} ^ { 1} + R_ {1} p_ {2} ^ {1}, \ prikker, p_ {1} ^ {K} + R_ {1} p_ {2} ^ {K})}$

Den største fordel ved Invariant-udvidede Kalman-filter i dette tilfælde er at løse problemet med falsk observerbarhed.

Referencer