26
Seite 65 2. Optimale Schätzer motors in der Form d dt ϕ ω = 0 1 0 1 ϕ ω + 0 1 u y = ϕ = 1 0 ϕ ω . Es ist nun gewünscht, dass Resonanzfrequenzen mit einer Kreisfrequenz ω 0 , welche durch die mechanische Last resultieren, vom Kalman-Filter unterdrückt werden. Das zugehörige Störmodell kann in der Form d dt z 1 z 2 = 0 1 ω 2 0 2ξω 0 z 1 z 2 + 0 ω 2 0 n v = 1 0 z 1 z 2 mit weißem Rauschen n als Eingangssignal modelliert werden. Entwerfen Sie für diese Spezifikationen ein Kalman-Filter für eine Abtastzeit T a =0.05 s und untersuchen Sie den Einfluss verschiedener Werte für ξ im Bereich 0.01 <ξ< 0.7. Wählen Sie dazu ω 0 = 3, G = E, Q = E sowie R = 1. Zeichnen Sie das Bode-Diagramm des Kalman-Filters und testen Sie ihr Kalman-Filter durch Simulation. 2.4. Das Extended Kalman-Filter Bevor das Extended Kalman-Filter als Beobachter für nichtlineare Systeme besprochen wird, soll im Folgenden das Kalman-Filter von Abschnitt 2.3 für lineare zeitvariante Abtastsysteme der Form x k+1 = Φ k x k + Γ k u k + G k w k x(0) = x 0 (2.111a) y k = C k x k + D k u k + H k w k + v k (2.111b) mit dem n-dimensionalen Zustand x R n , dem p-dimensionalen deterministischen Ein- gang u R p , dem q-dimensionalen Ausgang y R q , der r-dimensionalen Störung w R r , dem Messrauschen v sowie den zeitvarianten Matrizen Φ k R n×n , Γ k R n×p , G k R n×r , C k R q×n , D k R q×p und H k R q×r angeschrieben werden. Analog zu Abschnitt 2.3 werden folgende Annahmen getroffen: (1) Für die Störung w und das Messrauschen v wird vorausgesetzt, dass gilt E(v k )= 0 E v k v T j = R k δ kj (2.112a) E(w k )= 0 E w k w T j = Q k δ kj (2.112b) E w k v T j = 0 (2.112c) Vorlesung Regelungssysteme 1 (WS 2015/2016) © W. Kemmetmüller, A. Kugi, Institut für Automatisierungs- und Regelungstechnik, TU Wien

2.4. Das Extended Kalman-Filter - TU Wien€¦ · Kalman-Filters und testen Sie ihr Kalman-Filter durch Simulation. 2.4. Das Extended Kalman-Filter Bevor das Extended Kalman-Filter

  • Upload
    others

  • View
    17

  • Download
    0

Embed Size (px)

Citation preview

  • Seite 65 2. Optimale Schätzer

    motors in der Form

    ddt

    [

    ϕ

    ω

    ]

    =

    [

    0 1

    0 −1

    ][

    ϕ

    ω

    ]

    +

    [

    0

    1

    ]

    u

    y = ϕ =[

    1 0][

    ϕ

    ω

    ]

    .

    Es ist nun gewünscht, dass Resonanzfrequenzen mit einer Kreisfrequenz ω0, welchedurch die mechanische Last resultieren, vom Kalman-Filter unterdrückt werden. Daszugehörige Störmodell kann in der Form

    ddt

    [

    z1

    z2

    ]

    =

    [

    0 1

    −ω20 −2ξω0

    ][

    z1

    z2

    ]

    +

    [

    0

    ω20

    ]

    n

    v =[

    1 0][

    z1

    z2

    ]

    mit weißem Rauschen n als Eingangssignal modelliert werden. Entwerfen Sie für dieseSpezifikationen ein Kalman-Filter für eine Abtastzeit Ta = 0.05 s und untersuchenSie den Einfluss verschiedener Werte für ξ im Bereich 0.01 < ξ < 0.7. Wählen Siedazu ω0 = 3, G = E, Q = E sowie R = 1. Zeichnen Sie das Bode-Diagramm desKalman-Filters und testen Sie ihr Kalman-Filter durch Simulation.

    2.4. Das Extended Kalman-Filter

    Bevor das Extended Kalman-Filter als Beobachter für nichtlineare Systeme besprochenwird, soll im Folgenden das Kalman-Filter von Abschnitt 2.3 für lineare zeitvarianteAbtastsysteme der Form

    xk+1 = Φkxk + Γkuk + Gkwk x(0) = x0 (2.111a)

    yk = Ckxk + Dkuk + Hkwk + vk (2.111b)

    mit dem n-dimensionalen Zustand x ∈ Rn, dem p-dimensionalen deterministischen Ein-gang u ∈ Rp, dem q-dimensionalen Ausgang y ∈ Rq, der r-dimensionalen Störungw ∈ Rr, dem Messrauschen v sowie den zeitvarianten Matrizen Φk ∈ Rn×n, Γk ∈ Rn×p,Gk ∈ Rn×r, Ck ∈ Rq×n, Dk ∈ Rq×p und Hk ∈ Rq×r angeschrieben werden. Analog zuAbschnitt 2.3 werden folgende Annahmen getroffen:

    (1) Für die Störung w und das Messrauschen v wird vorausgesetzt, dass gilt

    E(vk) = 0 E(

    vkvTj

    )

    = Rkδkj (2.112a)

    E(wk) = 0 E(

    wkwTj

    )

    = Qkδkj (2.112b)

    E(

    wkvTj

    )

    = 0 (2.112c)

    Vorlesung Regelungssysteme 1 (WS 2015/2016)© W. Kemmetmüller, A. Kugi, Institut für Automatisierungs- und Regelungstechnik, TU Wien

  • 2. Optimale Schätzer Seite 66

    mit Qk ≥ 0, Rk ≥ 0 sowie HkQkHTk + Rk > 0 und dem Kroneckersymbol δkj = 1für k = j und δkj = 0 sonst.

    (2) Der Erwartungswert des Anfangswertes und die Kovarianzmatrix des Anfangsfehlerssind durch

    E(x0) = m0 E(

    [x0 − x̂0][x0 − x̂0]T)

    = P0 ≥ 0 (2.113)

    mit dem Schätzwert x̂0 des Anfangswertes x0 gegeben.

    (3) Die Störung wk, k ≥ 0, und das Messrauschen vl, l ≥ 0, sind mit dem Anfangswertx0 nicht korreliert, d. h. es gilt

    E(

    wkxT0

    )

    = 0 (2.114a)

    E(

    vlxT0

    )

    = 0 . (2.114b)

    Die Herleitung des Kalman-Filters für das System (2.111) erfolgt auf vollkommen analo-ge Art und Weise wie im Abschnitt 2.3 und lautet für k ≥ 0, vergleiche dazu (2.89)–(2.91),

    K̂k = ΦkPkCTk

    (

    CkPkCTk + HkQkH

    Tk + Rk

    )−1(2.115a)

    x̂k+1 = Φkx̂k + Γkuk + K̂k(yk − Ckx̂k − Dkuk) (2.115b)

    Pk+1 = ΦkPkΦTk + GkQkG

    Tk − ΦkPkCTk

    (

    CkPkCTk + HkQkH

    Tk + Rk

    )−1CkPkΦ

    Tk .

    (2.115c)

    Wenn der Anfangswert x0 bekannt ist, dann setzt man x̂0 = x0 und P0 = 0 und für denFall, dass über den Anfangswert keine Information vorliegt, wird typischerweise x̂0 = 0und P0 = αE mit α ≫ 1 gewählt.

    In der Literatur ist es oft üblich, das Kalman-Filter in einer etwas anderen Form dar-zustellen. Dabei wird die optimale Schätzung des Zustandes xk und der Fehlerkovarianz-matrix Pk unter Berücksichtigung von 0, . . . , k − 1 Messungen, vergleiche Definition 2.3,

    x̂−k = x̂(k|k − 1) (2.116a)

    P−k = P(k|k − 1) = E([

    xk − x̂−k][

    xk − x̂−k]T)

    (2.116b)

    als a priori Schätzung und die optimale Schätzung von xk und Pk unter Berücksichtigungvon 0, . . . , k Messungen

    x̂+k = x̂(k|k) (2.117a)

    P+k = P(k|k) = E([

    xk − x̂+k][

    xk − x̂+k]T)

    (2.117b)

    als a posteriori Schätzung bezeichnet. (2.115) kann damit in der äquivalenten Form

    Vorlesung Regelungssysteme 1 (WS 2015/2016)© W. Kemmetmüller, A. Kugi, Institut für Automatisierungs- und Regelungstechnik, TU Wien

  • Seite 67 2. Optimale Schätzer

    Kalman Verstärkungsmatrix: L̂k = P−k C

    Tk

    (

    CkP−k C

    Tk + HkQkH

    Tk + Rk

    )−1

    (2.118a)

    Zustandsschätzung Update: x̂+k = x̂−k + L̂k

    (

    yk − Ckx̂−k − Dkuk)

    (2.118b)

    Fehlerkovarianz Update: P+k =(

    E − L̂kCk)

    P−k (2.118c)

    Zustandsextrapolation (2.87): x̂−k+1 = Φkx̂+k + Γkuk (2.118d)

    Fehlerkovarianzextrapolation (2.88): P−k+1 = ΦkP+k Φ

    Tk + GkQkG

    Tk (2.118e)

    für k ≥ 0 und die Anfangswerte x̂−0 = x̂0 und P−0 = P0, angeschrieben werden.

    Aufgabe 2.14. Zeigen Sie die Äquivalenz der Beziehungen (2.115) und (2.118). FührenSie dazu in (2.118) folgende Substitutionen durch x̂−k+1 = x̂k+1, x̂

    −k = x̂k, P

    −k+1 =

    Pk+1, P−k = Pk und ΦkL̂k = K̂k.

    Dem Extended Kalman-Filter (EKF) Entwurf liegt im Allgemeinen ein nichtlineares,zeitvariantes, zeitkontinuierliches Mehrgrößensystem der Form

    ddt

    x = f(x, u, w, t) x(0) = x0 (2.119a)

    y = h(x, u, w, v, t) (2.119b)

    mit dem n-dimensionalen Zustand x ∈ Rn, dem p-dimensionalen deterministischen Ein-gang u ∈ Rp, dem q-dimensionalen Ausgang y ∈ Rq, der r-dimensionalen Störung w ∈ Rrund dem Messrauschen v zugrunde. Da das Kalman-Filter im Normalfall in einem Digi-talrechner implementiert wird, die Stellgrößen über ein Halteglied nullter Ordnung (D/A-Wandler) mit der Abtastzeit Ta auf den Prozess aufgeschaltet und die Messgrößen mit derAbtastzeit Ta über einen A/D-Wandler abgetastet werden, muss das zu (2.119) zugehörigeAbtastsystem

    xk+1 = Fk(xk, uk, wk) x(0) = x0 (2.120a)

    yk = hk(xk, uk, wk, vk) (2.120b)

    berechnet werden. Aus der Vorlesung Automatisierung (Abschnitt 6.2.1) ist bekannt, dasszur Bestimmung des Abtastsystems die exakte Lösung von (2.119) notwendig ist.

    Hinweis: Die Lösung eines nichtlinearen Differentialgleichungssystems der Form(2.119) ist bekanntermaßen nur in Spezialfällen möglich ist. Daher wird im Folgendeneine Näherungslösung mit Hilfe eines Integrationsverfahrens gesucht. Dazu nimmtman an, dass die Stellgröße u(t) und die Störung w(t) für das Abtastintervall kTa ≤t < (k + 1)Ta konstant sind, d. h. u(t) = u(kTa) = uk und w(t) = w(kTa) = wk,und integriert die Differenzialgleichung (2.119a) im Abtastintervall

    xk+1 = xk +∫ (k+1)Ta

    kTaf(x(t), uk, wk, t)dt (2.121)

    Vorlesung Regelungssysteme 1 (WS 2015/2016)© W. Kemmetmüller, A. Kugi, Institut für Automatisierungs- und Regelungstechnik, TU Wien

  • 2. Optimale Schätzer Seite 68

    mit xk+1 = x((k + 1)Ta) und xk = x(kTa). Die Approximation des Integrals in(2.121) kann auf unterschiedliche Arten erfolgen. Im Weiteren sollen lediglich zweimögliche Lösungen angegeben werden:

    (1) Euler-Verfahren

    ∫ (k+1)Ta

    kTaf(x(t), uk, wk, t)dt = f(xk, uk, wk, kTa)Ta (2.122)

    (2) Runge-Kutta Verfahren vierter Ordnung

    ∫ (k+1)Ta

    kTaf(x(t), uk, wk, t)dt =

    ∆x1 + 2∆x2 + 2∆x3 + ∆x46

    (2.123)

    mit

    ∆x1 = f(xk, uk, wk, kTa)Ta

    ∆x2 = f(

    xk +∆x1

    2, uk, wk, kTa +

    Ta2

    )

    Ta

    ∆x3 = f(

    xk +∆x2

    2, uk, wk, kTa +

    Ta2

    )

    Ta

    ∆x4 = f(xk + ∆x3, uk, wk, (k + 1)Ta)Ta .

    (2.124)

    Dabei wurde für ∆x4 in (2.124) der linksseitige Grenzwert limt→(k+1)Ta u(t) =uk und limt→(k+1)Ta w(t) = wk eingesetzt.

    Die Ausgangsgleichung (2.120b) des Abtastsystems erhält man für beide Fälle sehreinfach, indem man die Ausgangsgleichung (2.119b) des zeitkontinuierlichen Mehr-größensystems für t = kTa auswertet

    yk = h(xk, uk, wk, vk, kTa) = hk(xk, uk, wk, vk) . (2.125)

    Nimmt man nun an, dass durch die Beziehungen (2.121)–(2.125) ein Abtastsystem derForm (2.120) gegeben ist, dann beruht die Idee des Extended Kalman-Filters darauf, dassfür die rechte Seite von (2.120a) eine Taylor-Reihenentwicklung um den Punkt xk = x̂

    +k ,

    uk = uk und wk = 0 durchgeführt und nach dem linearen Term abgebrochen wird, d. h.

    xk+1 = Fk(

    x̂+k , uk, 0)

    +∂

    ∂xkFk(

    x̂+k , uk, 0)(

    xk − x̂+k)

    +∂

    ∂wkFk(

    x̂+k , uk, 0)

    wk . (2.126)

    Analog dazu wird die rechte Seite der Ausgangsgleichung (2.120b) in eine Taylor-Reiheum den Punkt xk = x̂

    −k , uk = uk, wk = 0 und vk = 0 entwickelt und nach dem linearen

    Term abgebrochen

    yk = hk(

    x̂−k , uk, 0, 0)

    +∂

    ∂xkhk(

    x̂−k , uk, 0, 0)(

    xk − x̂−k)

    +∂

    ∂wkhk(

    x̂−k , uk, 0, 0)

    wk +∂

    ∂vkhk(

    x̂−k , uk, 0, 0)

    vk .(2.127)

    Vorlesung Regelungssysteme 1 (WS 2015/2016)© W. Kemmetmüller, A. Kugi, Institut für Automatisierungs- und Regelungstechnik, TU Wien

  • Seite 69 2. Optimale Schätzer

    Man beachte, dass hierbei durchgehend folgende vereinfachte Schreibweise

    ∂xkFk(

    x̂−k , uk, 0)

    =∂

    ∂xkFk(xk, uk, wk)

    ∣∣∣∣xk=x̂

    k,uk=uk,wk=0

    (2.128)

    verwendet wurde. Die Beziehungen (2.126) und (2.127) können für die weiteren Betrach-tungen kompakter in der Form

    xk+1 = Φkxk + ūk + Gkwk (2.129a)

    yk = Ckxk + ŭk + Hkwk + v̆k (2.129b)

    mit

    Φk =∂

    ∂xkFk(

    x̂+k , uk, 0)

    ūk = Fk(

    x̂+k , uk, 0)

    − Φkx̂+k

    Gk =∂

    ∂wkFk(

    x̂+k , uk, 0)

    Ck =∂

    ∂xkhk(

    x̂−k , uk, 0, 0)

    ŭk = hk(

    x̂−k , uk, 0, 0)

    − Ckx̂−k Hk =∂

    ∂wkhk(

    x̂−k , uk, 0, 0)

    v̆k =∂

    ∂vkhk(

    x̂−k , uk, 0, 0)

    vk

    (2.130)

    geschrieben werden. Es ist nun offensichtlich, dass die Struktur des Systems (2.129) un-mittelbar die Anwendung des Kalman-Filters gemäß (2.118) ermöglicht. Die für die Im-plementierung erforderlichen Rechenschritte werden im Folgenden nochmals zusammen-gefasst.

    (1) Für das nichtlineare, zeitvariante, zeitkontinuierliche Mehrgrößensystem (2.119) be-rechne man ein Abtastsystem der Form (2.120).

    (2) Der geschätzte Zustand und die Kovarianzmatrix des Schätzfehlers müssen für denAnfangszeitpunkt mit x̂−0 und P

    −0 initialisiert werden.

    (3) Von der Störung wk und vom Messrauschen v̆k in (2.129) wird wiederum voraus-gesetzt, dass gilt

    E(v̆k) = 0 E(

    v̆kv̆Tj

    )

    = Rkδkj (2.131a)

    E(wk) = 0 E(

    wkwTj

    )

    = Qkδkj (2.131b)

    E(

    wkv̆Tj

    )

    = 0 (2.131c)

    mit Qk ≥ 0, Rk ≥ 0 sowie HkQkHTk + Rk > 0 und dem Kroneckersymbol δkj = 1für k = j und δkj = 0 sonst.

    (4) Die Iterationsgleichungen des Extended Kalman-Filters lauten dann für k ≥ 0

    Vorlesung Regelungssysteme 1 (WS 2015/2016)© W. Kemmetmüller, A. Kugi, Institut für Automatisierungs- und Regelungstechnik, TU Wien

  • 2. Optimale Schätzer Seite 70

    Ck =∂

    ∂xkhk(

    x̂−k , uk, 0, 0)

    (2.132a)

    Hk =∂

    ∂wkhk(

    x̂−k , uk, 0, 0)

    (2.132b)

    L̂k = P−k C

    Tk

    (

    CkP−k C

    Tk + HkQkH

    Tk + Rk

    )−1(2.132c)

    x̂+k = x̂−k + L̂k

    (

    yk − Ckx̂−k − ŭk)

    = x̂−k + L̂k(

    yk − hk(

    x̂−k , uk, 0, 0))

    (2.132d)

    P+k =(

    E − L̂kCk)

    P−k (2.132e)

    Φk =∂

    ∂xkFk(

    x̂+k , uk, 0)

    (2.132f)

    Gk =∂

    ∂wkFk(

    x̂+k , uk, 0)

    (2.132g)

    x̂−k+1 = Φkx̂+k + Fk

    (

    x̂+k , uk, 0)

    − Φkx̂+k︸ ︷︷ ︸

    ūk

    = Fk(

    x̂+k , uk, 0)

    (2.132h)

    P−k+1 = ΦkP+k Φ

    Tk + GkQkG

    Tk . (2.132i)

    Hinweis: Beim Extended Kalman-Filter Entwurf wird angenommen, dass die li-nearisierte Transformation von Mittelwert und Kovarianz in guter Genauigkeit demMittelwert und der Kovarianz der nichtlinearen Transformation entspricht. DieseAnnahme trifft im Allgemeinen nicht zu, weshalb zur Verbesserung des Beobach-terentwurfes für nichtlineare Systeme häufig das im nächsten Abschnitt betrachteteUnscented Kalman-Filter verwendet wird.

    Aufgabe 2.15. Das mathematische Modell

    d

    dtx1 = x2 + w1

    d

    dtx2 =

    1

    2ρ0 exp

    (

    −x1k

    )

    CwA

    mx22 − g + w2

    beschreibt den freien Fall eines Körpers der Masse m und der QuerschnittsflächeA in der Erdatmosphäre mit der Höhe x1 und der Geschwindigkeit x2. Der Termρ0 exp(−x1/k) entspricht dabei der höhenabhängigen Dichte in der Atmosphäre (ρ0Dichte auf Meereshöhe), womit der Term mit x22 die Verzögerung durch den Luft-widerstand mit dem Luftwiderstandsbeiwert Cw beschreibt. Weiterhin stellt g dieErdbeschleunigung dar.

    Das Prozessrauschen ist durch die stochastischen Größen w1 und w2 gegeben. DieHöhe x1 kann über die Ausgangsgleichung

    y = x1 + v

    mit dem Messrauschen v ermittelt werden.

    Vorlesung Regelungssysteme 1 (WS 2015/2016)© W. Kemmetmüller, A. Kugi, Institut für Automatisierungs- und Regelungstechnik, TU Wien

  • Seite 71 2. Optimale Schätzer

    Entwerfen Sie für die Parameter ρ0 = 1.2 kg/m3, g = 9.81 m/s2, k = 9100 m,A = 0.5 m2, m = 100 kg, und Cw = 0.5 ein Extended Kalman-Filter, das neben derHöhe x1 und der Geschwindigkeit x2 auch den konstanten LuftwiderstandsbeiwertCw schätzt. Erweitern Sie dazu das Differentialgleichungssystem um den Zustandx3 = Cw mit

    d

    dtx3 = 0 + w3

    und der Prozessstörungskomponente w3. Verwenden Sie zur Bestimmung des Ab-tastsystems das Euler-Verfahren. Nehmen Sie an, dass die nominellen Werte bzw.Anfangsbedingungen der Größen Cw, x1 und x2 normalverteilt sind. Die entspre-chenden Werte der Mittelwerte und Varianzen können der nachfolgenden Tabelle 2.1entnommen werden. Für die Simulation nehmen Sie folgende Werte an: Cw = 0.6,x1(0) = 39 500 m und x2(0) = −10 m/s.

    Variable Mittelwert Varianz

    Cw 0.5 1

    x1(0) 39 · 103 m 1 · 104 m2x2(0) 0 m/s 1 m

    2/s2

    Tabelle 2.1.: Mittelwerte und Varianzen der Parameter bzw. der Anfangsbedingungen.

    Aufgabe 2.16. Zur Positionsbestimmung eines Fahrzeuges in einem zweidimensio-nalen Raum (o-Achse: Ost-Koordinate, n-Achse: Nord-Koordinate) soll ein Exten-ded Kalman-Filter eingesetzt werden. Mehrere Messstationen mit den Koordinaten(Oi, Ni), i = 1, . . . , M messen den Abstand zum Fahrzeug. Die Beschleunigung desFahrzeugs in Nord- und Ost-Richtung wird durch weißes Rauschen modelliert. DasDifferenzengleichungssystem

    ok+1

    nk+1

    ov,k+1

    nv,k+1

    ︸ ︷︷ ︸

    xk+1

    =

    1 0 Ta 0

    0 1 0 Ta

    0 0 1 0

    0 0 0 1

    ︸ ︷︷ ︸

    Φ

    ok

    nk

    ov,k

    nv,k

    ︸ ︷︷ ︸

    xk

    +

    w1,k

    w2,k

    w3,k

    w4,k

    ︸ ︷︷ ︸

    wk

    beschreibt das Fahrzeugverhalten, wobei ok und nk bzw. ov,k und nv,k die Koordi-naten bzw. Geschwindigkeiten des Fahrzeuges bezogen auf den Ursprung eines orts-festen Koordinatensystems in Ost- und Nordrichtung zum Zeitpunkt kTa mit derAbtastzeit Ta bezeichnen und wj,k, j = 1, . . . , 4 die Komponenten des Prozessrau-schens sind. Im Weiteren sind durch

    yi,k =√

    (nk − Ni)2 + (ok − Oi)2 + vi,k, i = 1, . . . , M

    Vorlesung Regelungssysteme 1 (WS 2015/2016)© W. Kemmetmüller, A. Kugi, Institut für Automatisierungs- und Regelungstechnik, TU Wien

  • 2. Optimale Schätzer Seite 72

    mit dem Messrauschen vi,k, i = 1, . . . , M die Entfernungsmessungen des Fahrzeugesvon den Stationen gegeben. Nehmen Sie an, dass alle stochastischen Größen (wj,k)und (vi,k) normalverteilt, unkorreliert und mittelwertfrei sind. Die Abtastzeit sei mitTa = 0.1 s gegeben. Für die Kovarianzmatrix des Prozessrauschens gelte

    E(

    wkwTj

    )

    = Qδkj mit Q = diag(0, 0, 4, 4)

    und die Kovarianz des Messrauschens sei

    E(vi,kvi,j) = Riδkj mit Ri = 1, i = 1, . . . , M .

    Der Anfangszustand xT0 =[

    0 0 50 50]

    ist exakt bekannt. Simulieren Sie das Sys-tem für 60 Sekunden und entwerfen Sie ein Extended Kalman-Filter zur Schätzungder Zustände. Variieren Sie die Anzahl und Position der Messstationen.

    2.5. Das Unscented Kalman-Filter

    Das im letzten Kapitel behandelte Extended Kalman-Filter ist der (industrielle) Stan-dard für die Zustandsschätzung von nichtlinearen dynamischen Systemen. Das ExtendedKalman-Filter besitzt jedoch den Nachteil, dass es häufig schwierig zu parametrieren istund dass es unzuverlässige Schätzungen (des Erwartungswerts und der Fehlerkovarianz)liefert, wenn das System ausgeprägte Nichtlinearitäten aufweist. Diese Unzuverlässig-keit resultiert aus der Linearisierung der nichtlinearen Systemdynamik, welche für dieBerechnung des Erwartungswerts und der Kovarianz des Zustands verwendet wird. Umdiese Problematik genauer darzustellen, wird im nächsten Abschnitt gezeigt, wie sich derErwartungswert und die Kovarianz einer Zufallsvariable durch eine nichtlineare Transfor-mation verändert.

    2.5.1. Erwartungswert und Kovarianz von nichtlinearen Transformationen

    Um den Fehler durch die Linearisierung einer nichtlinearen Transformation bei der Be-rechnung des Erwartungswertes und der Kovarianz zu analysieren, wird das Beispiel einerTransformation von Zylinderkoordinaten in kartesische Koordinaten betrachtet

    y =

    [

    y1

    y2

    ]

    = h(x) =

    [

    x1 cos(x2)

    x1 sin(x2)

    ]

    . (2.133)

    Dabei wird angenommen, dass der Zufallsvektor x = [x1, x2]T durch die nicht korrelierten,

    gleichverteilten Zufallsvariabeln x1 (Intervall: a1 = mx1 −δx1 = 1−0.01, b1 = mx1 +δx1 =1+0.01) und x2 (Intervall: a2 = mx2 −δx2 = π2 −0.35, b2 = mx2 +δx2 = π2 +0.35) definiertist. In Abbildung 2.7 sind 10000 Zufallsvektoren, die nach dieser Verteilung generiertwurden, dargestellt. Der Erwartungswert (Mittelwert) mx ergibt sich zu mx = [1, π/2]

    T

    und die Kovarianzmatrix errechnet sich zu

    Qx =

    [1310

    −4 0

    0 491210−2

    ]

    . (2.134)

    Vorlesung Regelungssysteme 1 (WS 2015/2016)© W. Kemmetmüller, A. Kugi, Institut für Automatisierungs- und Regelungstechnik, TU Wien

  • Seite 73 2. Optimale Schätzer

    In Abbildung 2.7 ist die Kovarianzmatrix durch die Ellipse (x − mx)TQ−1x (x − mx) = 1dargestellt.

    0.990 0.995 1.000 1.005 1.0101.2

    1.4

    1.6

    1.8

    x1

    x2

    Abbildung 2.7.: Verteilung der Zufallsvektoren x mit zugehörigem Erwartungswert mxund Kovarianzmatrix Qx.

    Der Erwartungswert E(y) = my der mittels der nichtlinearen Transformation berech-neten Größe y = h(x) ergibt sich aus

    [

    my1

    my2

    ]

    =

    [

    E(h1(x))

    E(h2(x))

    ]

    =

    [

    E(x1 cos(x2))

    E(x1 sin(x2))

    ]

    . (2.135)

    Berücksichtigt man die Unabhängigkeit der Zufallsvariablen x1 und x2 und führt man dieAufspaltung x = x̃ + mx ein, so erhält man für die erste Zeile von (2.135) unmittelbar

    E(x1 cos(x2)) = E(x1) E(cos(x̃2 + mx2)) . (2.136)

    Natürlich gilt E(x1) = mx1 = 1. Spaltet man den cos-Term in (2.136) auf und setzt denErwartungswert mx2 = π/2 ein, so erhält man

    E(cos(x̃2 + mx2)) = E(cos(x̃2) cos(mx2) − sin(x̃2) sin(mx2)) = − E(sin(x̃2)) . (2.137)

    Da der Erwartungswert einer schiefsymmetrischen Funktion einer Zufallsvariable mit sym-metrischer Verteilungsdichtefunktion verschwindet, gilt E(sin(x̃2)) = 0 und somit auchmy1 = 0. Um diesen letzten Schritt zu zeigen, betrachte man

    E(sin(x̃2)) =

    ∫ δx2

    −δx2sin(x̃2)

    1

    2δx2dx̃2 =

    1

    2δx2(− cos(δx2) + cos(−δx2)) = 0 . (2.138)

    Vorlesung Regelungssysteme 1 (WS 2015/2016)© W. Kemmetmüller, A. Kugi, Institut für Automatisierungs- und Regelungstechnik, TU Wien

  • 2. Optimale Schätzer Seite 74

    Auf analoge Art kann man die zweite Zeile von (2.135) zu E(x1 sin(x2)) = sin(δx2)/δx2berechnen. Zusammengefasst erhält man den exakten Erwartungswert my der Zufallsva-riablen y in der Form

    my =

    [

    0sin(δx2)

    δx2

    ]

    . (2.139)

    Die exakte Berechnung des Erwartungswerts my ist nur für einige wenige Sonderfällemöglich. Man beachte, dass in diesem Beispiel eine einfache Gleichverteilung und einenichtlineare Transformation 2. Ordnung angenommen wurde um eine analytische Lösungberechnen zu können. Im Allgemeinen können die resultierenden Integrale jedoch nurnumerisch gelöst werden. Eine naheliegende Möglichkeit den Erwartungswert näherungs-weise zu berechnen besteht in der Approximation der nichtlinearen Transformation h(x)durch eine Taylorreihe, die um den Erwartungswert mx des Zufallsvektors x ermitteltwurde. Diese Taylorreihe kann in der Form

    h(x) = h(mx) + Dx̃h +1

    2!D2x̃h +

    1

    3!D3x̃h + . . . , (2.140)

    angegeben werden, wobei die vereinfachte Schreibweise

    Dkx̃h =

    (n∑

    i=1

    x̃i∂

    ∂xi

    )k

    h(x)

    ∣∣∣∣∣∣x=mx

    (2.141)

    verwendet wird. Darin bezeichnet n die Dimension der Zufallsvariable x.Verwendet man zur Berechnung des Erwartungswerts eine Taylorreihenapproximation

    von h(x) der Ordnung 1, so erhält man

    h(x) ≈ hl(x) = h(mx) + x̃1∂h(x)∂x1

    ∣∣∣∣x=mx

    + x̃2∂h(x)∂x2

    ∣∣∣∣x=mx

    . (2.142)

    Der zugehörige Erwartungswert berechnet sich damit zu

    E(hl(x)) = h(mx) +∂h(x)∂x1

    ∣∣∣∣x=mx

    E(x̃1) +∂h(x)∂x2

    ∣∣∣∣x=mx

    E(x̃2) = h(mx) (2.143)

    und damit

    E(hl(x)) = myl =

    [

    0

    1

    ]

    (2.144)

    Ein Vergleich mit dem exakten Ergebnis (2.139) zeigt, dass diese Berechnung zu Feh-lern führt, welche mit steigendem δx2 anwachsen. In Abbildung 2.8 ist ein Vergleich desexakten Erwartungswerts my mit dem Erwartungswert myl, welcher auf Basis der Linea-risierung ermittelt wurde, dargestellt. Man erkennt eine maßgebliche Abweichung dieserbeiden Werte. Da diese Linearisierung auch inhärent in der Berechnung eines EKF in-kludiert ist, muss auch für ein EKF eine Abweichung vom exakten Ergebnis erwartet

    Vorlesung Regelungssysteme 1 (WS 2015/2016)© W. Kemmetmüller, A. Kugi, Institut für Automatisierungs- und Regelungstechnik, TU Wien

  • Seite 75 2. Optimale Schätzer

    −0.4 −0.3 −0.2 −0.1 0.0 0.1 0.2 0.3 0.4

    0.94

    0.96

    0.98

    1

    1.02

    y1

    y2

    mymylmyq

    Abbildung 2.8.: Verteilung der Zufallsvektoren y = h(x) mit dem ErwartungswertE(y) = my, der Approximation myl auf Basis einer Taylorreihe 1. Ord-nung und der Approximation myq auf Basis einer Taylorreihe 2. Ordnung.

    werden. Diese Abweichung wächst mit steigender Nichtlinearität der Strecke und mitgrößerer Kovarianz der Zufallsvariablen an.

    Eine Möglichkeit den Fehler zufolge der Linearisierung zu verringern, ist die Verwen-dung einer Approximation hq(x) von h(x) zweiter Ordnung. Für diesen Fall ergibt sichder approximierte Erwartungswert

    E(hq(x)) = myq =

    0

    1 − σ2x22

    , (2.145)

    mit der Varianz σx2 von x2. In Abbildung 2.8 erkennt man die wesentliche Verbesserungder Approximation von E(h(x)) durch E(hq(x)) im Vergleich zur linearen ApproximationE(hl(x)).

    Aufgabe 2.17. Zeigen Sie das Ergebnis E(hq(x)) von (2.145).

    Damit könnte durch hinreichend hohe Approximationsordnung von h(x) eine beliebiggenaue Approximation von E(h(x)) ermittelt werden. Diese Vorgehensweise hat jedochzwei wesentliche Nachteile:

    1. Zur Approximation einer allgemeinen nichtlinearen Funktion kann eine sehr großeApproximationsordnung notwendig sein. Dies führt zu einer wesentlichen Erhöhungdes Rechenaufwands.

    2. In der Berechnung des Erwartungswerts bei einer Approximation der Ordnung ksind die zentralen Momente bis zur Ordnung k des Zufallsvektors x notwendig.

    Vorlesung Regelungssysteme 1 (WS 2015/2016)© W. Kemmetmüller, A. Kugi, Institut für Automatisierungs- und Regelungstechnik, TU Wien

  • 2. Optimale Schätzer Seite 76

    Eine sinnvolle Vorgabe dieser Momente kann in der praktischen Anwendung sehrschwierig werden.

    Deswegen ist diese Vorgehensweise für eine praktische Implementierung nur bedingt sinn-voll.

    Wie bereits mehrfach diskutiert, ist zur Charakterisierung einer (normalverteilten) Zu-fallfsvariable x zusätzlich zum Erwartungswert E(x) = mx auch die KovarianzmatrixE(

    (x − mx)(x − mx)T)

    = Qx notwendig. Daher ist es interessant zu untersuchen, wiesich die Kovarianzmatrix zufolge der nichtlinearen Transformation ändert. Für das be-trachtete Beispiel gilt

    Qy = E

    ([

    x1 cos(x2)

    x1 sin(x2) − sin(δx2)δx2

    ][

    x1 cos(x2) x1 sin(x2) − sin(δx2)δx2])

    = E

    x21 cos(x2)

    2 x21 cos(x2) sin(x2) − x1 cos(x2) sin(δx2)δx2x21 cos(x2) sin(x2) − x1 cos(x2) sin(δx2)δx2

    (

    x1 sin(x2) − sin(δx2)δx2)2

    ,

    (2.146)

    was nach kurzer Rechnung auf

    Qy =

    12

    (1 + σ21

    )(

    1 − sin(2δx2)2δx2)

    0

    0 12(1 − σ21

    )(

    1 + sin(2δx2)2δx2

    )

    −(

    sin(δx2)δx2

    )2

    (2.147)

    führt.

    Aufgabe 2.18. Rechnen Sie die Lösung (2.147) nach.

    Zur Schätzung der transformierten Kovarianzmatrix Qy mit Hilfe der Tayorreihenap-proximation 1. Ordnung verwendet man den Ausdruck

    yl − E(yl) = Dx̃h = x̃1∂h∂x1

    ∣∣∣∣x=mx

    + x̃2∂h∂x2

    ∣∣∣∣x=mx

    . (2.148)

    Damit ergibt sich die Approximation Qyl der Kovarianzmatrix zu

    Qyl = E

    [

    x̃1∂h∂x1

    ∣∣∣∣x=mx

    + x̃2∂h∂x2

    ∣∣∣∣x=mx

    ][

    x̃1∂h∂x1

    ∣∣∣∣x=mx

    + x̃2∂h∂x2

    ∣∣∣∣x=mx

    ]T

    =[

    ∂h∂x1

    ∣∣∣x=mx

    ∂h∂x2

    ∣∣∣x=mx

    ]

    E

    ([

    x̃21 x̃1x̃2

    x̃1x̃2 x̃22

    ])

    ∂h∂x1

    ∣∣∣x=mx

    ∂h∂x2

    ∣∣∣x=mx

    = HQxHT

    (2.149)

    bzw. für das betrachtete Beispiel

    Qyl =

    [

    σ2x2 0

    0 σ2x1

    ]

    . (2.150)

    In Abbildung 2.9 ist ein Vergleich der Kovarianzmatrix Qy und der Approximation Qyl inForm der durch sie definierten Ellipsen für C = 1 dargestellt. Wie schon bei der Berech-nung der Erwartungswerte sieht man eine große Abweichung zwischen der Approximationund der exakten Lösung. Dies ist wiederum auf die ausgeprägte Nichtlinearität sowie dieApproximation 1. Ordnung zurückzuführen.

    Vorlesung Regelungssysteme 1 (WS 2015/2016)© W. Kemmetmüller, A. Kugi, Institut für Automatisierungs- und Regelungstechnik, TU Wien

  • Seite 77 2. Optimale Schätzer

    −0.4 −0.3 −0.2 −0.1 0.0 0.1 0.2 0.3 0.4

    0.94

    0.96

    0.98

    1

    1.02

    y1

    y2

    mymylQyQyl

    Abbildung 2.9.: Verteilung der Zufallsvektoren y = h(x) mit Erwartungswert E(y) = mybzw. Kovarianzmatrix Qy und die Approximationen myl bzw. Qyl aufBasis einer Taylorreihe 1. Ordnung.

    Satz 2.9 (Approximationsordnung). Der Erwartungswert my und die Kovarianz-matrix Qy einer Zufallsvariable y, welche sich durch ein nichtlineare Transformationder Form y = h(x) errechnet, ergeben sich zu

    my = h(mx) + E(

    Dx̃h +1

    2!D2x̃h +

    1

    3!D3x̃h +

    1

    4!D4x̃h + . . .

    )

    (2.151)

    und

    Qy = HQxHT + E

    (1

    3!Dx̃h

    (

    D3x̃h)T

    +1

    4D2x̃h

    (

    D2x̃h)T

    +1

    3!D3x̃h(Dx̃h)

    T)

    − E(

    D2x̃h)

    E(

    D2x̃h)T

    + . . . ,

    (2.152)

    mit H = ∂h/∂x|x=mx.Um den Erwartungswert my mit einer Approximationsgenauigkeit der Ordnung m

    zu berechnen, sind die partiellen Ableitungen von h und die zentralen Momente von xbis zur m.ten Ordnung. Weiterhin kann der Term der m.ten Ordnung der Reihe derKovarianzmatrix nur bestimmt werden, wenn die Ableitungen von h und die zentralenMomente von x bis zur 2m.ten Ordnung bekannt sind.

    Vorlesung Regelungssysteme 1 (WS 2015/2016)© W. Kemmetmüller, A. Kugi, Institut für Automatisierungs- und Regelungstechnik, TU Wien

  • 2. Optimale Schätzer Seite 78

    Die Ergebnisse dieses Abschnitts können wie folgt zusammengefasst werden:

    1. Die Berechnung des Erwartungswertes und der Kovarianzmatrix auf Basis einerLinearisierung von h(x) kann zu sehr ungenauen Ergebnissen führen.

    2. Eine Verbesserung der Genauigkeit der Approximation kann durch Erhöhung derApproximationsordnung von h(x) erreicht werden. Dies führt jedoch direkt zu einerwesentlichen Erhöhung der Komplexität der resultierenden Ausdrücke.

    3. Zur Berechnung der Approximationen sind sowohl die partiellen Ableitungen vonh(x) als auch die zentralen Momente von x notwendig. Damit ist eine hinreichendestetige Differenzierbarkeit der nichtlinearen Transformation notwendig.

    Daher ist für Systeme mit ausgeprägter Nichtlinearität eine bessere Form der Appro-ximation des Erwartungswertes und der Kovarianzmatrix notwendig.

    2.5.2. Die Unscented-Transformation

    Die Unscented-Transformation beruht darauf, dass es häufig einfacher ist die Verteilungeiner Zufallsvariable zu approximieren als eine allgemeine nichtlineare Funktion oderTransformation. Bei der Unscented-Transformation wird eine Menge S von Sigmapunktenξi so gewählt, dass deren Statistik (d.h. im Wesentlichen der Erwartungswert mxu und dieKovarianzmatrix Qxu) der Statistik des ursprünglichen Zufallsvektors x entspricht. DieAnwendung einer nichtlinearen Transformation y = h(x) auf jeden dieser Sigmapunkteξi ergibt die transformierten Sigmapunkte ηi = h(ξi). Die statistischen Eigenschaftender transformierten Punkte ηi sind dann eine Approximation der exakten statistischenEigenschaften der nichtlinearen Transformation.

    Die Menge der Sigmapunkte S ist durch die Vektoren ξi und die zugehörigen Gewich-tungen Wi definiert, d. h. S = {ξi, Wi|i = 1, . . . , p}. Die Wahl der Sigmapunkte ist nichteindeutig, d. h. für einen Zufallsvektor x mit Erwartungswert mx und KovarianzmatrixQx gibt es mehrere mögliche Realisierungen von Sigmapunkten. Um eine erwartungstreueSchätzung zu erhalten, müssen die Gewichtungen Wi die Zwangsbedingung

    p∑

    i=1

    Wi = 1 (2.153)

    erfüllen.In der Literatur wird für einen Zufallsvektor x ∈ Rn häufig eine Menge S von 2n

    Punkten, die auf dem Ellipsoid (x − mx)Q−1x (x − mx)T = n liegen, verwendet. Die Sig-mapunkte für einen Zufallsvektor x sind demnach durch

    ξi =

    {

    mx +(√

    nQx)T

    i für i = 1, . . . , n

    mx −(√

    nQx)T

    i−n für i = n + 1, . . . , 2n(2.154)

    definiert, mit der Dimension n, dem Erwartungswert mx und der Kovarianzmatrix Qxdes Zufallsvektors x. Weiterhin beschreibt

    (√nQx

    )

    i die i.te Zeile der Wurzel der MatrixnQx. Die zugehörigen Gewichtungen ergeben sich zu

    Vorlesung Regelungssysteme 1 (WS 2015/2016)© W. Kemmetmüller, A. Kugi, Institut für Automatisierungs- und Regelungstechnik, TU Wien

  • Seite 79 2. Optimale Schätzer

    Wi =1

    2n. (2.155)

    Hinweis: Die Wurzel R einer Matrix Q kann sehr effizient mit Hilfe der Cholesky-Zerlegung (Matlab Befehl chol) ermittelt werden. Es gilt dann RTR = Q.

    Die Vorgehensweise zur Bestimmung des approximierten Erwartungswertes myu undder Kovarianzmatrix Qyu mit Hilfe der Unscented-Transformation besteht aus folgendenSchritten:

    1. Die nichtlineare Transformation h(x) wird auf jeden Sigmapunkt ξi angewandt undergibt die transformierten Sigmapunkte ηi

    ηi = h(ξi) . (2.156)

    2. Der Erwartungswert myu ergibt sich aus der gewichteten Summe der transformier-ten Sigmapunkte

    myu =2n∑

    i=1

    Wiηi . (2.157)

    3. Die Kovarianzmatrix Qyu der transformierten Sigmapunkte errechnet sich in derForm

    Qyu =2n∑

    i=1

    Wi(ηi − myu)(ηi − myu)T . (2.158)

    Aufgabe 2.19. Zeigen Sie, dass der Mittelwert und die Kovarianzmatrix der durch(2.154) und (2.155) definierten Sigmapunkte dem Mittelwert mx und der Kovarianz-matrix Qx des Zufallsvektors x entsprechen.

    Wie in der Literatur dokumentiert, würden bereits n Sigmapunkte genügen um denErwartungswert und die Kovarianzmatrix korrekt zu approximieren. Durch die symmetri-sche Wahl der 2n Sigmapunkte nach (2.154) wird jedoch auch das dritte zentrale Momentexakt erfüllt.

    Die Approximationsordnung der Unscented-Transformation ist 2, d. h. der Mittelwertund die Kovarianzmatrix werden bis zum zweiten Term korrekt wiedergegeben. Man be-achte, dass zur Berechnung des Mittelwerts und der Kovarianz keine höheren Momentevon x oder partielle Ableitungen der nichtlinearen Transformation notwendig sind. Letz-tere Eigenschaft erlaubt es damit, die Unscented-Transformation auch auf nicht stetig-differenzierbare (sogar nicht stetige) nichtlineare Transformationen anzuwenden.

    Im Folgenden wird die Anwendung der Unscented-Transformation auf die nichtlineareTransformation y = h(x) aus (2.133) in Abschnitt 2.5.1 betrachtet. Die 2n = 4 Sigma-punkte ξi nach (2.154) sind in der Abbildung 2.10 zusammen mit dem exakten Mittelwertmx und der durch die Ellipse für C =

    √2 charakterisierten Kovarianzmatrix Qx darge-

    stellt. Wie erwartet sind die Sigmapunkte ξi symmetrisch auf dieser Ellipse verteilt.

    Vorlesung Regelungssysteme 1 (WS 2015/2016)© W. Kemmetmüller, A. Kugi, Institut für Automatisierungs- und Regelungstechnik, TU Wien

  • 2. Optimale Schätzer Seite 80

    0.990 0.995 1.000 1.005 1.0101.2

    1.4

    1.6

    1.8

    x1

    x2

    ξimxQx

    Abbildung 2.10.: Gleichverteilte Zufallsvariable x nach (2.133) mit dem Mittelwert mx,der Kovarianzmatrix Qx und den Sigmapunkten ξi.

    In Abbildung 2.11 sind die transformierten Sigmapunkte ηi = h(ξi) dargestellt. Wei-terhin sind der nach (2.157) approximierte Mittelwert myu und die approximierte Kovari-anzmatrix Qyu nach (2.158) eingezeichnet. Man erkennt eine sehr gute Übereinstimmungzwischen dem exakten Mittelwert my und dem mit Hilfe der Sigmapunkte approximier-ten Mittelwert myu. Weiterhin sieht man, dass die approximierte Kovarianzmatrix Qyueine relativ gute Approximation der exakten Kovarianzmatrix Qy darstellt. Insbesondereim Vergleich zur mit Hilfe der Linearisierung berechneten Kovarianzmatrix Qyl aus demletzten Abschnitt 2.5.1 ergibt sich eine drastische Verbesserung der Approximation, vgl.Abbildung 2.9.

    Bemerkung 2.3. Wie bereits angemerkt ist die Wahl der Sigmapunkte nicht ein-deutig. Neben der in diesem Skriptum dargestellten Sigmapunkten auf Basis derUnscented-Transformation werden in der Literatur auch häufig sogenannte simplexSigmapunkte oder spherische Sigmapunkte verwendete, siehe z. B. [2.2]. Diese zeich-nen sich durch eine etwas einfachere Berechnung aus, weisen jedoch Nachteile bei derApproximationsgenauigkeit auf.

    Wenn der Zufallsvektor x normalverteilt ist, dann erweist sich häufig die erweiterteWahl der Sigmapunkte der Form

    ξ0 = mx (2.159a)

    ξi =

    mx +(√

    n1−λQx

    )T

    ifür i = 1, . . . , n

    mx −(√

    n1−λQx

    )T

    i−nfür i = n + 1, . . . , 2n

    (2.159b)

    Vorlesung Regelungssysteme 1 (WS 2015/2016)© W. Kemmetmüller, A. Kugi, Institut für Automatisierungs- und Regelungstechnik, TU Wien

  • Seite 81 2. Optimale Schätzer

    −0.4 −0.3 −0.2 −0.1 0.0 0.1 0.2 0.3 0.4

    0.94

    0.96

    0.98

    1

    1.02

    y1

    y2

    myQyηimyuQyu

    Abbildung 2.11.: Transformierte Zufallsvariable y = h(x) nach (2.133) mit dem exaktenMittelwert my, der exakten Kovarianzmatrix Qy, den transformiertenSigmapunkten ηi = h(ξi), dem approximierten Mittelwert myu und derapproximierten Kovarianzmatrix Qyu.

    mit dem skalaren Parameter λ als sinnvoll. Die zugehörigen Gewichtungen sind inder Form

    W0 = λ (2.160a)

    Wi =1 − λ

    2n. (2.160b)

    gegeben. Es kann gezeigt werden, dass die Wahl λ = 1 − n/3 für normalverteilteZufallsvektoren x optimal ist [2.3]. Um die Eigenschaften der approximierten Kova-rianzmatrix Qyu gezielt zu beeinflussen, kann zusätzlich die erweiterte Berechnungdes Mittelwerts myu und der Kovarianzmatrix Qyu durch

    myu =2n∑

    i=0

    Wiηi (2.161a)

    Qyu =2n∑

    i=0

    Wi(ηi − myu)(ηi − myu)T +(

    1 − α2)

    (η0 − myu)(η0 − myu)T (2.161b)

    verwendet werden. Wie noch anhand des Beispiels der nichtlinearen Transformation(2.133) aus Abschnitt 2.5.1 gezeigt wird, kann mit dem skalaren Parameter 0 < α ≤ 1die Form der approximierten Kovarianzmatrix beeinflusst werden. Für α = 1 erfolgtoffensichtlich keine Änderung der Gestalt der approximierten Kovarianzmatrix Qyu.

    Vorlesung Regelungssysteme 1 (WS 2015/2016)© W. Kemmetmüller, A. Kugi, Institut für Automatisierungs- und Regelungstechnik, TU Wien

  • 2. Optimale Schätzer Seite 82

    Die Begründung für die Wahl der erweiterten Sigmapunkte basiert auf einer Ana-lyse des Einflusses von Momenten höherer Ordnung auf die Approximation der Kova-rianzmatrix. Für eine detaillierte Analyse und die Ermittlung des optimalen Wertsvon λ für normalverteilte Zufallsvektoren sei auf die Literatur, insbesondere [2.3],[2.2], [2.4], verwiesen.

    Um den Einfluss der erweiterten Sigmapunkte (2.159), der Gewichtungen (2.160) unddes skalaren Parameters α auf den mittels der Unscented-Transformation geschätztenMittelwert und die zugehörige Kovarianzmatrix zu analysieren, betrachte man nochmalsdie nichtlineare Transformation nach (2.133). Im Vergleich zu Abschnitt 2.5.1 und denAbbildungen 2.10 und 2.11 wird nun angenommen, dass der Zufallsvektor x = [x1, x2]

    T

    durch normalverteilte Zufallszahlen x1 (Erwartungswert mx1 = 1, Varianz σx1 = 0.01)und x2 (Erwartungswert mx2 = π/2, Varianz σx2 = 0.35) definiert ist. In Abbildung2.12 ist die Verteilung von 10000 Zufallsvektoren x, die in Matlab mit dem Befehlrandn erzeugt wurden, mit deren Mittelwert mx und Kovarianzmatrix Qx dargestellt.Weiterhin sind die 4 Sigmapunkte ξi nach (2.154) und die 5 erweiterten Sigmapunkte ξeinach (2.159) eingezeichnet. Dabei wurde die für normalverteilte Zufallsvektoren optimaleWahl λ = 1−2/3 = 1/3 getroffen. Man erkennt, dass die erweiterten Sigmapunkte ξei aufeiner Ellipse mit vergrößertem Wert von C liegen. Äquivalent würde die Wahl λ < 0 dazuführen, dass die Sigmapunkte auf einer Ellipse mit verringertem Wert von C zu liegenkommen. Weiterhin entspricht der Sigmapunkt ξe0 dem Mittelwert mx.

    0.96 0.98 1.00 1.02 1.040

    0.5

    1

    1.5

    2

    2.5

    3

    x1

    x2

    mxQxξiξei

    Abbildung 2.12.: Normalverteilter Zufallsvektor x mit Mittelwert mx und Kovarianzma-trix Qx, und die Sigmapunkte ξi nach (2.154) sowie die erweitertenSigmapunkte ξei nach (2.159) für λ = 1/3.

    Wendet man nun wiederum die nichtlineare Transformation y = h(x) aus (2.133) aufdie Sigmapunkte an, so erhält man die in Abbildung 2.13 dargestellten transformierten

    Vorlesung Regelungssysteme 1 (WS 2015/2016)© W. Kemmetmüller, A. Kugi, Institut für Automatisierungs- und Regelungstechnik, TU Wien

  • Seite 83 2. Optimale Schätzer

    Sigmapunkte ηi und die transformierten erweiterten Sigmapunkte ηei. Ein Vergleich derapproximierten Mittelwerte myu und myue nach (2.157) bzw. (2.161a) zeigt eine sehr guteÜbereinstimmung mit dem exakten Mittelwert my, wobei beide Approximation myu undmyue beinahe gleiche Ergebnisse liefern. Den Vorteil der erweiterten Sigmapunkte siehtman in der Approximation der Kovarianzmatrix Qy. Hier liefern die erweiterten Sigma-punkte ξei aus (2.159) mit den zugehörigen Gewichtungen Wi nach (2.160) eine wesent-liche Verbesserung der Approximationsgenauigkeit im Vergleich zu den ursprünglichenSigmapunkten nach (2.154). Man beachte, dass bei der Berechnung der KovarianzmatrixQyue nach (2.161b) der Parameter α = 1 gesetzt wurde.

    −1.00 −0.75 −0.50 −0.25 0.00 0.25 0.50 0.75 1.000.3

    0.4

    0.5

    0.6

    0.7

    0.8

    0.9

    1

    1.1

    y1

    y2

    myQyηimyuQyuηeimyueQyue

    Abbildung 2.13.: Transformierte normalverteilte Zufallsvariable y = h(x) mit dem exak-ten Mittelwert my und der exakten Kovarianzmatrix Qy. Weiterhin istdie Approximation des Mittelwerts myu und der Kovarianzmatrix Qyuauf Basis der Sigmapunkte aus (2.154) mit den Gewichtungen (2.155)und die Approximation des Mittelwerts myue und der Kovarianzma-trix Qyue auf Basis der erweiterten Sigmapunkte aus (2.159) mit denGewichtungen (2.160) dargestellt.

    Um nun den Einfluss des skalaren Parameters α in der Berechnung der approximiertenKovarianzmatrix nach (2.161b) zu analysieren, ist in Abbildung 2.14 ein Vergleich der Ap-proximation für α = 1 mit der Approximation für α = 0.5 dargestellt. Wie man erkennt,führt die Wahl α < 1 im Wesentlichen zu einer Vergrößerung der geschätzten Kovarianz-matrix. Damit kann man also mit geeigneter Wahl von α die Größe der Kovarianzmatrixbeeinflussen.

    Vorlesung Regelungssysteme 1 (WS 2015/2016)© W. Kemmetmüller, A. Kugi, Institut für Automatisierungs- und Regelungstechnik, TU Wien

  • 2. Optimale Schätzer Seite 84

    −1.00 −0.75 −0.50 −0.25 0.00 0.25 0.50 0.75 1.000.3

    0.4

    0.5

    0.6

    0.7

    0.8

    0.9

    1

    1.1

    y1

    y2

    myQyQyuQyue,α = 1Qyue,α = 0.5

    Abbildung 2.14.: Untersuchung des Einflusses des Parameters α auf die Approximationder Kovarianzmatrix mit Hilfe von (2.161b).

    Bemerkung 2.4. Auf den ersten Blick scheint die Wahl von α < 1 nicht zielführendzu sein, da das obige Beispiel gezeigt hat, dass α = 1 eine wesentlich bessere Appro-ximation von Qy liefert als α = 0.5. Analysiert man den Fall α = 1 jedoch genauer,so erkennt man, dass der Wert der exakten Kovarianzmatrix Qy durch die appro-ximierte Kovarianzmatrix Qyu leicht unterschätzt wird. In manchen Anwendungenmöchte man jedoch auf jeden Fall verhindern, dass die approximierte Kovarianzma-trix kleiner als die reale Kovarianzmatrix ist. Eine Approximation der Kovarianz,die größer als die exakte Kovarianzmatrix ist, wird in der Literatur häufig auch alskonsistent bezeichneta. Um eine konsistente Approximation der Kovarianzmatrix zuerzwingen, kann also der Parameter α < 1 verwendet werden.

    aMan beachte den Unterschied zur Definition der Konsistenz einer Schätzung nach Definition 1.5.

    2.5.3. Zustandsschätzung dynamischer Systeme mit Hilfe derUnscented-Transformation

    In diesem Abschnitt wird die Unscented-Transformation zur Zustandsschätzung von nicht-linearen dynamischen Systemen verwendet. In Analogie zum Extended Kalmanfilter ausAbschnitte 2.4 wird ein nichtlineares, zeitdiskretes dynamisches System der Form

    xk+1 = Fk(xk, uk, wk) (2.162a)

    yk = hk(xk, uk, wk, vk), (2.162b)

    mit dem Zustand xk, dem deterministischen Eingang uk, der Störung wk und dem Mess-rauschen vk, betrachtet. Es wird vorausgesetzt, dass die statistischen Eigenschaften von

    Vorlesung Regelungssysteme 1 (WS 2015/2016)© W. Kemmetmüller, A. Kugi, Institut für Automatisierungs- und Regelungstechnik, TU Wien

  • Seite 85 2. Optimale Schätzer

    wk und vk bekannt sind und durch

    E(vk) = 0 E(

    vkvTj

    )

    = Rδkj (2.163a)

    E(wk) = 0 E(

    wkwTj

    )

    = Qδkj (2.163b)

    E(

    wkvTj

    )

    = 0, (2.163c)

    mit Q > 0, R > 0, gegeben sind.Weiterhin ist eine Schätzung x̂0 des Anfangswertes x0 in Form des Erwartungswertes

    m0 gegeben, d. h. x̂0 = E(x0) = m0 und die Kovarianzmatrix P0 = E(

    [x0 − x̂0][x0 − x̂0]T)

    ≥0 des Schätzfehlers wird als bekannt vorausgesetzt. Wie schon beim Kalmanfilter und auchbeim Extended Kalmanfilter wird angenommen, dass E

    (

    x0wTk)

    = 0 und E(

    x0vTk)

    = 0gilt.

    Für dieses System soll nun auf Basis der Unscented-Transformation ein Kalmanfilter,das sogenannte Unscented Kalmanfilter (in der Literatur auch als Sigmapunkt Kalman-filter bezeichnet) dargestellt werden. Dazu beachte man, dass die Störung wk und dasMessrauschen vk eine nichtlineare Transformation erfahren, weswegen auch deren statis-tische Eigenschaften anhand der Unscented-Transformation erfasst werden müssen. Dazudefiniert man den erweiterten Zustand xak ∈ Rn

    a

    xak =

    xkwkvk

    (2.164)

    und berechnet für diesen erweiterten Zustand die Unscented-Transformation.Damit kann folgende Iteration des Unscented Kalmanfilters formuliert werden, siehe

    [2.3], [2.4], [2.2] für eine detaillierte Herleitung:

    1. Initialisierung:

    x̂0 = E(x0) = m0 (2.165a)

    P0 = E(

    [x0 − x̂0][x0 − x̂0]T)

    (2.165b)

    x̂a+0 = E(xa0) =

    x̂00

    0

    (2.165c)

    Pa0+ = E

    (

    [xa0 − x̂a0][xa0 − x̂a0]T)

    =

    P0 0 0

    0 Q 0

    0 0 R

    (2.165d)

    Vorlesung Regelungssysteme 1 (WS 2015/2016)© W. Kemmetmüller, A. Kugi, Institut für Automatisierungs- und Regelungstechnik, TU Wien

  • 2. Optimale Schätzer Seite 86

    2. Berechnung der Sigmapunkte:

    ξa+0,k−1 = x̂a+k−1 (2.166a)

    ξa+i,k−1 =

    x̂a+k−1 +√

    na

    1−λ

    (√

    Pa+k−1)T

    ifür i = 1, . . . , na

    x̂a+k−1 −√

    na

    1−λ

    (√

    Pa+k−1)T

    i−nafür i = na + 1, . . . , 2na

    (2.166b)

    mit

    ξa+i,k−1 =

    ξx+i,k−1

    ξw+i,k−1

    ξv+i,k−1

    . (2.167)

    3. Zustands- und Kovarianzmatrixextrapolation:

    Prädizierte Sigmapunkte:

    ξx−i,k = Fk−1(

    ξx+i,k−1, uk−1, ξw+i,k−1

    )

    (2.168)

    Prädizierter Mittelwert und prädizierte Fehlerkovarianzmatrix:

    x̂−k =2na∑

    i=0

    Wiξx−i,k (2.169a)

    P−k =2na∑

    i=0

    Wi(

    ξx−i,k − x̂−k)(

    ξx−i,k − x̂−k)T

    +(

    1 − α2)(

    ξx−0,k − x̂−k)(

    ξx−0,k − x̂−k)T

    (2.169b)

    Prädizierter Ausgang (Messung):

    η−i,k = hk(

    ξx,−i,k , uk, ξ

    w,−i,k , ξ

    v,−i,k

    )

    (2.170a)

    ŷ−k =2na∑

    i=0

    Wiη−i,k (2.170b)

    Vorlesung Regelungssysteme 1 (WS 2015/2016)© W. Kemmetmüller, A. Kugi, Institut für Automatisierungs- und Regelungstechnik, TU Wien

  • Seite 87 2. Optimale Schätzer

    4. Messupdate:

    Kovarianzmatrix Pykyk zwischen der prädizierten Messungen und KovarianzmatrixPxkyk zwischen prädizierter Messung und Zustand:

    Pykyk =2na∑

    i=0

    Wi(

    η−i,k − ŷ−k)(

    η−i,k − ŷ−k)T

    +(

    1 − α2)(

    η−0,k − ŷ−k)(

    η−0,k − ŷ−k)T

    (2.171a)

    Pxkyk =2na∑

    i=0

    Wi(

    ξx−i,k − x̂−k)(

    η−i,k − ŷ−k)T

    +(

    1 − α2)(

    ξx−0,k − x̂−k)(

    η−0,k − ŷ−k)T

    (2.171b)

    Messupdate des geschätzten Zustands und der Fehlerkovarianz:

    Kk = Pxkyk(Pykyk)−1 (2.172a)

    x̂+k = x̂−k + Kk

    (

    yk − ŷ−k)

    (2.172b)

    P+k = P−k − KkPykykKTk (2.172c)

    5. Rücksetzen / Initialisieren für nächste Iteration:

    x̂a+k =

    x̂+k0

    0

    (2.173a)

    Pak+ =

    P+k 0 0

    0 Q 0

    0 0 R

    (2.173b)

    Für die Berechnung des Unscented-Kalmanfilter sind nach (2.166) 2(n+dim(w)+dim(w))Sigmapunkte notwendig. Dies kann bei einer hohen Systemordnung zu einem sehr großenRechenaufwand führen. In vielen regelungstechnischen Problemen kann jedoch angenom-men werden, dass sowohl die Prozessstörung wk als auch das Messrauschen vk additivauf das System einwirken. Damit kann das System (2.162) zu

    xk+1 = Fk(xk, uk) + wk (2.174a)

    yk = hk(xk, uk) + vk, (2.174b)

    vereinfacht werden. Für dieses vereinfachte System kann nun, ausgehend von den Iterati-onsgleichungen für den allgemeinen Fall (2.165)-(2.173) die folgende vereinfachte Formu-lierung des Uscented-Kalmanfilters gefunden werden:

    1. Initialisierung:

    x̂+0 = E(x0) = m0 (2.175a)

    P+0 = E(

    [x0 − x̂0][x0 − x̂0]T)

    (2.175b)

    Vorlesung Regelungssysteme 1 (WS 2015/2016)© W. Kemmetmüller, A. Kugi, Institut für Automatisierungs- und Regelungstechnik, TU Wien

  • 2. Optimale Schätzer Seite 88

    2. Berechnung der Sigmapunkte:

    ξ+0,k−1 = x̂+k−1 (2.176a)

    ξ+i,k−1 =

    x̂+k−1 +√

    n1−λ

    (√

    P+k−1)T

    ifür i = 1, . . . , n

    x̂+k−1 −√

    n1−λ

    (√

    P+k−1)T

    i−nfür i = n + 1, . . . , 2n

    (2.176b)

    3. Zustands- und Kovarianzmatrixextrapolation:

    Prädizierte Sigmapunkte:

    ξ−i,k = Fk−1(

    ξ+i,k−1, uk−1)

    (2.177)

    Prädizierter Mittelwert und prädizierte Fehlerkovarianzmatrix:

    x̂−k =2n∑

    i=0

    Wiξ−i,k (2.178a)

    P−k =2n∑

    i=0

    Wi(

    ξ−i,k − x̂−k)(

    ξ−i,k − x̂−k)T

    +(

    1 − α2)(

    ξ−0,k − x̂−k)(

    ξ−0,k − x̂−k)T

    + Q

    (2.178b)

    Korrektur der Sigmapunkte:

    ξ−0,k = x̂−k (2.179a)

    ξ−i,k =

    x̂−k +√

    n1−λ

    (√

    P−k

    )

    ifür i = 1, . . . , n

    x̂−k −√

    n1−λ

    (√

    P−k

    )

    i−nfür i = n + 1, . . . , 2n

    (2.179b)

    Prädizierter Ausgang (Messung):

    η−i,k = hk(

    ξ−i,k, uk)

    (2.180a)

    ŷ−k =2n∑

    i=0

    Wiη−i,k (2.180b)

    Vorlesung Regelungssysteme 1 (WS 2015/2016)© W. Kemmetmüller, A. Kugi, Institut für Automatisierungs- und Regelungstechnik, TU Wien

  • Seite 89 2. Optimale Schätzer

    4. Messupdate:

    Kovarianzmatrix Pykyk der prädizierten Messungen und Kovarianzmatrix Pxkykzwischen prädizierter Messung und Zustand:

    Pykyk =2n∑

    i=0

    Wi(

    η−i,k − ŷ−k)(

    η−i,k − ŷ−k)T

    +(

    1 − α2)(

    η−0,k − ŷ−k)(

    η−0,k − ŷ−k)T

    + R

    (2.181a)

    Pxkyk =2n∑

    i=0

    Wi(

    ξ−i,k − x̂−k)(

    η−i,k − ŷ−k)T

    +(

    1 − α2)(

    ξ−0,k − x̂−k)(

    η−0,k − ŷ−k)T

    (2.181b)

    Messupdate des geschätzten Zustands und der Fehlerkovarianz:

    Kk = Pxkyk(Pykyk)−1 (2.182a)

    x̂+k = x̂−k + Kk

    (

    yk − ŷ−k)

    (2.182b)

    P+k = P−k − KkPykykKTk (2.182c)

    Bemerkung 2.5. Die Korrektur der Sigmapunkte in (2.179) ist notwendig, da der Ein-fluss der Prozessstörung wk erst anhand der Kovarianzmatrix Q in der prädiziertenFehlerkovarianzmatrix P−k nach (2.178b) berücksichtigt wird. Durch diese Korrekturmuss pro Iteration zweimal die Cholesky-Zerlegung einer n × n Matrix berechnetwerden. Diese numerisch aufwändige Operation wird in der praktischen Implemen-tierung häufig umgangen, indem in (2.180), (2.181) die ursprünglichen Sigmapunkteξ−i,k aus (2.177) verwendet werden. Dies resultiert in einem reduzierten numerischenAufwand, es muss jedoch in der praktischen Anwendung überprüft werden, ob diedamit resultierenden Fehler akzeptabel sind.

    Eine weiter Vorgehensweise um die Sigmapunkte zu korrigieren, besteht in derDefinition einer erweiterten Menge von Sigmapunkten in der Form

    ξa−i,k = ξ−i,k für i = 0, . . . , 2n (2.183)

    und

    ξa−i+2n,k =

    ξ−0,k +√

    2n1−λ

    (√Q)T

    i für i = 1, . . . , n

    ξ−0,k −√

    2n1−λ

    (√Q)T

    i−n für i = n + 1, . . . , 2n. (2.184)

    Diese erweiterten Sigmapunkte ξa−i,k und die an die neue Dimension 4n + 1 von ξa−i,k

    angepassten Gewichtungen W ai werden anschließend in der Berechnung von (2.180),(2.181) verwendet. Da Q eine konstante Matrix ist, entfällt die Berechnung derCholesky-Zerlegung in (2.184), was den numerischen Aufwand verringert. Anderer-seits müssen in (2.180) und (2.181) nun 4n + 1 Sigmapunkte berücksichtigt werden,was im Vergleich zu (2.179) wiederum zu einer Erhöhung des numerischen Aufwandsführt.

    Vorlesung Regelungssysteme 1 (WS 2015/2016)© W. Kemmetmüller, A. Kugi, Institut für Automatisierungs- und Regelungstechnik, TU Wien

  • 2. Optimale Schätzer Seite 90

    2.6. Literatur

    [2.1] H. van Trees, Detection, Estimation and Modulation Theory: Part 1. New York,USA: John Wiley & Sons, 2001.

    [2.2] D. Simon, Optimal State Estimation. New Jersey, USA: John Wiley & Sons, 2006.

    [2.3] S. Julier und J. Uhlmann, „Unscented filtering and nonlinear estimation“, Pro-ceedings of the IEEE, Bd. 92, Nr. 3, S. 401–422, 2004.

    [2.4] E. Wan und R. van der Merwe, „Kalman filtering and neural networks“, in, S.Haykin, Hrsg. New York, USA: John Wiley & Sons, 2001, Kap. The UnscentedKalman Filter, S. 221–280.

    [2.5] G. Franklin, J. Powell und M. Workman, Digital Control of Dynamic Systems,3. Aufl. Menlo Park, USA: Addison–Weseley, 1998.

    [2.6] L. Ljung, System Identification. New Jersey, USA: Prentice Hall, 1999.

    [2.7] D. Luenberger, Optimization by Vector Space Methods. New York, USA: JohnWiley & Sons, 1969.

    [2.8] O. Nelles, Nonlinear System Identification. Berlin, Deutschland: Springer, 2001.

    [2.9] R. Isermann, Identifikation dynamischer Systeme 1 und 2, 2. Aufl. Berlin, Deutsch-land: Springer, 1992.

    [2.10] K. Åström und B. Wittenmark, Computer Controlled Systems: Theory and De-sign. New York, USA: Prentice Hall, 1997.

    [2.11] A. Bryson und Y. Ho, Applied Optimal Control. Washington, USA: He, 1975.

    [2.12] P. Dorato, C. Abdallah und V. Cerone, Linear Quadratic Control: An Introduc-tion. Florida, USA: Krieger Publishing Company, 2000.

    [2.13] A. Gelb, Applied Optimal Estimation. Cambridge, USA: MIT Pre, 74.

    Vorlesung Regelungssysteme 1 (WS 2015/2016)© W. Kemmetmüller, A. Kugi, Institut für Automatisierungs- und Regelungstechnik, TU Wien