Integrierte Navigationssysteme: Sensordatenfusion, GPS und Inertiale Navigation 9783486595154

Citation preview

Integrierte Navigationssysteme Sensordatenfusion, GPS und Inertiale Navigation von Jan Wendel

Oldenbourg Verlag München Wien

Nach dem Studium der Elektrotechnik an der Universität Karlsruhe (TH) promovierte Jan Wendel über das Thema „Entwurfs- und Analysemethoden Integrierter Navigationssysteme“. 2006 folgte die Habilitation über „Sensorfusion in Integrierten Navigationssystemen“. Seit 2006 arbeitet Dr. habil. Jan Wendel bei MBDA Systems in München.

Bibliografische Information der Deutschen Nationalbibliothek Die Deutsche Nationalbibliothek verzeichnet diese Publikation in der Deutschen Nationalbibliografie; detaillierte bibliografische Daten sind im Internet über abrufbar.

© 2007 Oldenbourg Wissenschaftsverlag GmbH Rosenheimer Straße 145, D-81671 München Telefon: (089) 45051-0 oldenbourg.de Das Werk einschließlich aller Abbildungen ist urheberrechtlich geschützt. Jede Verwertung außerhalb der Grenzen des Urheberrechtsgesetzes ist ohne Zustimmung des Verlages unzulässig und strafbar. Das gilt insbesondere für Vervielfältigungen, Übersetzungen, Mikroverfilmungen und die Einspeicherung und Bearbeitung in elektronischen Systemen. Lektorat: Kathrin Mönch Herstellung: Anna Grosser Coverentwurf: Kochan & Partner, München Gedruckt auf säure- und chlorfreiem Papier Druck: Grafik + Druck, München Bindung: Thomas Buchbinderei GmbH, Augsburg ISBN 978-3-486-58160-7

Vorwort Thema dieses Buches sind integrierte Navigationssysteme, wobei der Schwerpunkt auf der Kombination von inertialer Navigation mit Satellitennavigationssystemen liegt. Zum einen sind diese Systeme weit verbreitet, zum anderen k¨onnen anhand dieser Thematik die meisten in der Navigation ben¨ otigten Techniken, Verfahren und Methoden behandelt werden. Dies umfasst eine Beschreibung der eingesetzten Subsysteme und deren Sensoren sowie die Verarbeitung von Sensordaten mit Hilfe von stochastischen Filtern. Mit der detaillierten Darstellung der Strapdown-Rechnung und den daf¨ ur ben¨otigten Prinzipien wie Koordinatensystemstransformationen, Quaternionen und Eulerwinkeln wird in die Technik der inertialen Navigation eingef¨ uhrt. Grundlegende Mechanismen wie die Schuler Oszillationen oder die Instabilit¨at des H¨ohenkanals werden ebenfalls herausgearbeitet. Durch die Erl¨ auterung der Signalverarbeitung in einem GPS-Empf¨anger wird ein Verst¨ andnis f¨ ur die von einem GPS-Empf¨ anger zur Verf¨ ugung gestellten Messgr¨oßen vermittelt. Die angesprochenen Zusammenh¨ ange dieser Messgr¨oßen mit der Position und Geschwindigkeit des Empf¨ angers erm¨ oglichen die Entwicklung von Algorithmen zu deren Nutzung in einem integrierten Navigationssystem. Die Diskussion der in der Satellitennavigation relevanten Fehlerquellen vermittelt einen Eindruck von der zu erwartenden Genauigkeit der Navigationsinformationen. Eine Einf¨ uhrung in die stochastische Filterung mit den Schwerpunkten Kalman-Filter und erweiterte sowie linearisierte Kalman-Filter schafft die theoretischen Kenntnisse, die f¨ ur das Verst¨ andnis der Datenfusionsalgorithmen in einem integrierten Navigationssystem ben¨ otigt werden. Diese Buch richtet sich an Studierende der Luft- und Raumfahrt oder verwandter Gebiete, aber auch an den Systemingenieur, der zeiteffizient ein umfassendes Verst¨andnis dieser Thematik entwickeln muss, um entscheidungsf¨ahig zu sein. Großer Wert wurde darauf gelegt, den Bezug der erarbeiteten Theorie zur Praxis anhand von Beispielen darzulegen, die aus den Bereichen GPS/INS-Integration, Transfer Alignment und Micro Aerial Vehicles gew¨ ahlt wurden. Denn nur wenn der praktische Nutzen der vermittelten Theorie erkennbar ist, kann diese vom Leser sp¨ater gewinnbringend eingesetzt werden. M¨ unchen

J. Wendel

Inhaltsverzeichnis 1

Einleitung

1

2

Lineare Systeme

5

2.1

Zustandsraumdarstellung linearer Systeme . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

7

2.2

Eigenwerte und Eigenvektoren . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

9

2.3

Lineare Systeme im Zeitdiskreten . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

2.4

Nichtlinearit¨ aten . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

3

Inertiale Navigation

27

3.1 3.1.1 3.1.2 3.1.3

Koordinatensysteme . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Das Erdmodell WGS-84 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Transformationen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Nomenklatur . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

28 30 31 33

3.2 3.2.1 3.2.2 3.2.3

Lagedarstellungen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Eulerwinkel . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Orientierungsvektor und Quaternion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Richtungskosinusmatrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

36 37 38 43

3.3 3.3.1 3.3.2 3.3.3

Strapdown-Rechnung . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Lage . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Geschwindigkeit . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Position . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

45 45 52 60

3.4 3.4.1 3.4.2 3.4.3 3.4.4 3.4.5

Fehlercharakteristik eines Inertialnavigationssystems . . . . . . . . . . . . . . . . . . . Drehratensensoren . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Beschleunigungsmesser . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Generische Inertialsensorfehlermodelle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Kurzzeitcharakteristik . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Langzeitcharakteristik . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

60 61 65 67 73 75

3.5

Initialisierung . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78

4

Satellitennavigation

4.1

System¨ uberblick Navstar GPS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83

4.2 4.2.1

Funktionsprinzip eines GPS-Empf¨angers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 85 GPS-Signalstruktur . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 86

83

VIII

Inhaltsverzeichnis

4.2.2 4.2.3

Akquisition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92 Tracking . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94

4.3 4.3.1 4.3.2 4.3.3 4.3.4 4.3.5

GPS-Beobachtungsgr¨ oßen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Pseudorange . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Tr¨agerphasenmessung . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Deltarange . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Fehlerquellen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Differential GPS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

4.4

Modernisierung des GPS-Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 110

4.5

Galileo System¨ uberblick . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111

5

Grundlagen der Stochastik

5.1 5.1.1 5.1.2

Die Zufallsvariable. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 117 Wahrscheinlichkeitsdichte . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 117 Gaußverteilung . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 120

5.2 5.2.1 5.2.2

Stochastische Prozesse . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 123 Weißes Rauschen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124 Zeitkorreliertes Rauschen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 125

6

Das Kalman-Filter

129

6.1 6.1.1 6.1.2 6.1.3

Kalman-Filter Gleichungen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Herleitung u ¨ ber normalverteilte Zufallsvektoren . . . . . . . . . . . . . . . . . . . . . . . . Herleitung u ¨ ber Minimierung einer Kostenfunktion . . . . . . . . . . . . . . . . . . . . . Diskussion der Filtergleichungen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

130 130 134 137

6.2

Beobachtbarkeit . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 140

6.3

¨ Ubergang kontinuierlich – diskret . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 141

6.4 6.4.1 6.4.2 6.4.3 6.4.4

Nichtlineare System- und Messmodelle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Linearisiertes Kalman-Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Erweitertes Kalman-Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Sigma-Point-Kalman-Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Kalman-Filter 2. Ordnung . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

6.5 6.5.1 6.5.2

Filterung bei zeitkorreliertem Rauschen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 157 Erweiterung des Zustandsvektors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 158 Messwertdifferenzen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 159

6.6 6.6.1 6.6.2

Covariance Intersection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 160 Bekannte Kreuzkorrelationen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 161 Unbekannte Kreuzkorrelationen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 163

6.7 6.7.1 6.7.2

Adaptive Filterung . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 167 Interacting Multiple Model Filter: Problemformulierung . . . . . . . . . . . . . . . . 168 Herleitung der IMM-Filtergleichungen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 169

99 100 104 106 108 109

117

143 144 146 147 156

Inhaltsverzeichnis

IX

7

Monte-Carlo-Methoden

179

7.1

Chapman-Kolmogorov-Gleichung . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 179

7.2

Ber¨ ucksichtigung von Beobachtungen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 181

7.3 7.3.1 7.3.2 7.3.3 7.3.4 7.3.5

Partikelfilter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Repr¨ asentation der WDF . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Propagationsschritt . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Estimationsschritt . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Resampling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Simulationsergebnisse. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

182 182 183 183 184 186

8

Anwendungsbeispiel GPS/INS-Integration

191

8.1 8.1.1 8.1.2 8.1.3

GPS/INS-Integrationsstrategien . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Loosely Coupled System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Tightly Coupled System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Ultra-Tight und Deep Integration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

192 192 193 193

8.2 8.2.1 8.2.2 8.2.3 8.2.4

Entwurf eines Navigationsfilters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Systemmodell . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Messmodelle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Korrektur der totalen Gr¨ oßen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Vergleich von Loosely Coupled und Tightly Coupled Systemen . . . . . . . . .

194 194 206 214 215

8.3 8.3.1 8.3.2 8.3.3

Nutzung von Tr¨ agerphasenmessungen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Carrier Aided Smoothing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Festlegung der Tr¨ agerphasenmehrdeutigkeitswerte . . . . . . . . . . . . . . . . . . . . . . Zeitlich differenzierte Tr¨ agerphasenmessungen . . . . . . . . . . . . . . . . . . . . . . . . . .

219 220 221 224

8.4

Verz¨ ogerte Verf¨ ugbarkeit von Messwerten . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 231

8.5

Integrity Monitoring . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 238

8.6 8.6.1 8.6.2 8.6.3

Sigma-Point-Kalman-Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Nichtlinearit¨ at eines Sch¨ atzproblems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Simulationsergebnisse. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Theoretischer Vergleich mit Objektverfolgung . . . . . . . . . . . . . . . . . . . . . . . . . .

8.7 8.7.1 8.7.2

Fixed-Interval Smoother . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 254 Gleichungen des RTS Smoothers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 254 Simulationsergebnisse. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 256

9

Anwendungsbeispiel Transfer Alignment

9.1

Konventionelle Transfer-Alignment-Verfahren . . . . . . . . . . . . . . . . . . . . . . . . . . 259

9.2

Rapid Transfer Alignment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 260

9.3 9.3.1 9.3.2

Effiziente Ber¨ ucksichtigung von Zeitkorrelationen . . . . . . . . . . . . . . . . . . . . . . 263 Propagationsschritt . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 265 Messwertverarbeitung . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 266

239 240 243 252

259

X

Inhaltsverzeichnis

9.3.3

Diskussion der Filtergleichungen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 267

9.4 9.4.1 9.4.2

Numerische Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 268 Erzeugung von Inertialsensordaten . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 269 Ergebnisse . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 270

9.5 9.5.1 9.5.2

Adaptive Sch¨ atzung der Rauschprozessmodelle . . . . . . . . . . . . . . . . . . . . . . . . . 274 Identifikation anhand von Messwertdifferenzen . . . . . . . . . . . . . . . . . . . . . . . . . 275 Ergebnisse . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 278

10

Anwendungsbeispiel unbemanntes Flugger¨ at

10.1 10.1.1

Beobachtbarkeit des Yaw-Winkels . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 283 St¨ utzung mit Erdmagnetfeldmessungen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 285

10.2 10.2.1 10.2.2

Stabilisierung bei GPS-Ausfall . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 287 Systemmodell des Lagefilters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 288 St¨ utzung mit Beschleunigungsmessungen. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 288

10.3 10.3.1 10.3.2 10.3.3 10.3.4 10.3.5

Systemsimulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Funktionsprinzip des Flugger¨ ats . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Mathematisches Modell . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Einfluss der Trajektoriendynamik . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Sch¨ atzung von Modellparametern . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Ergebnisse der Gesamtsystemsimulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

10.4 10.4.1 10.4.2

Experimentelle Verifikation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 302 Kalibration der Beschleunigungsmesser . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 304 Ergebnisse . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 305

A

Sherman-Morrison-Woodbury-Formel

309

B

Differentiation von Spuren von Matrizen

313

C

MATLAB-Code zum Beispiel Abschnitt 7.3.5

315

281

289 290 292 296 297 299

Symbolverzeichnis

319

Literaturverzeichnis

325

Index

335

1

Einleitung

Navigationsinformationen werden bei einer Vielzahl von Anwendungen ben¨otigt. Vor allem in der Seefahrt war man schon zu fr¨ uhesten Zeiten darauf angewiesen, Position und Kurs zu kennen; fehlerhafte Navigationsinformationen f¨ uhrten zum Verlust unz¨ahliger Schiffe. Eines der bekanntesten Beispiele hierf¨ ur ist der Untergang von vier englischen Kriegsschiffen am 22. Oktober 1707, die sich nach siegreicher Schlacht bei Gibraltar auf der R¨ uckfahrt nach England befanden und aufgrund eines fehlerhaft berechneten L¨ angengrades mit den Scilly Inseln kollidierten, was zweitausend Menschen das Leben kostete [116]. Zur damaligen Zeit war man noch auf eine fr¨ uhe Form der Koppelnavigation angewiesen, die zur¨ uckgelegte Strecke wurde mit dem Log und der Kurs mit dem Kompass bestimmt. Da sich hierbei die Fehler mit der Zeit aufsummierten, wurden St¨ utzinformationen ben¨ otigt; so konnte bespielsweise der Breitengrad mit einem Sextanten anhand des Sonnenstandes ermittelt werden, die Bestimmung des L¨angengrades, also die Unfallursache in der Nacht vom 22. Oktober 1707, blieb jedoch in Ermangelung genauer Uhren oder einfach durchf¨ uhrbarer astronomischer Beobachtungen noch lange Zeit ein Problem. Heutzutage werden vielfach integrierte Navigationssysteme eingesetzt, die dadurch gekennzeichnet sind, dass die verschiedensten Navigationsverfahren und Sensoren kombiniert werden. Ziel ist hierbei, durch die Vorteile des einen Navigationsverfahrens die Nachteile eines anderen Navigationsverfahrens zu kompensieren und wenn m¨oglich eine gewisse Redundanz zu schaffen. Der Grundgedanke dieser Navigationssysteme ¨ahnelt aber h¨ aufig immer noch in erstaunlicher Weise der Koppelnavigation der Seefahrer fr¨ uherer Jahrhunderte: Extrapolation der Position anhand der zur¨ uckgelegten Strecke und der Bewegungsrichtung sowie Korrektur der Navigationsl¨osung durch Einbeziehung zus¨ atzlicher Messungen. Die Extrapolation der Position kann heute durch ein Inertialnavigationssystem (INS), das auch als Tr¨ agheitsnavigationssystem bezeichnet wird, erfolgen. F¨ ur die Korrektur der Navigationsl¨ osung k¨ onnen Messungen eines GPS-Empf¨angers herangezogen werden. Die Leistungsf¨ ahigkeit eines solchen integrierten GPS/INS-Systems ist durchaus beeindruckend. Allerdings steigen auch st¨andig die Anforderungen, die an ein Navigationssystem bez¨ uglich Genauigkeit, Zuverl¨assigkeit und Verf¨ ugbarkeit der Navigationsinformationen gestellt werden, dies gilt insbesondere in der Luftfahrt. Hierbei dienen Navigationsinformationen h¨ aufig als Eingangsgr¨oßen der Flugregelung, so dass neben Positions- und Geschwindigkeitsinformationen auch Lageinformationen von besonderem Interesse sind. Ziel dieses Buches ist es, eine umfassende Einf¨ uhrung in die Sensorik und Algorithmik integrierter Navigationssysteme zu geben. In einem Inertialnavigationssystem kommen Beschleunigungssensoren und Drehratensensoren zum Einsatz. Anhand der Drehratensensoren kann die Lage in der Zeit pro-

2

1 Einleitung

pagiert werden. Die so gewonnenen Lageinformationen erlauben es, die gemessenen Beschleunigungen so umzurechnen, dass die Bestimmung von Geschwindigkeits- und Positions¨ anderungen gegen¨ uber der Erde m¨ oglich wird. Die dabei eingesetzten Inertialsensoren nutzen unterschiedliche physikalische Effekte, die letztendlich auch die G¨ ute der Sensoren und damit des Inertialnavigationssystems bestimmen. Neben den in der inertialen Navigation ben¨ otigten mathematischen Zusammenh¨angen werden daher auch die eingesetzten Sensoren und die Fehlercharakteristiken eines Inertialnavigationssystems beschrieben. Schließlich wird auf das Navstar Global Positioning System, kurz GPS, eingegangen, das eine St¨ utzung der Inertialnavigation erm¨oglicht. Hierbei werden die prinzipielle Funktionsweise eines GPS-Empf¨ angers und die gelieferten Beobachtungsgr¨oßen beschrieben. Eine Diskussion der Fehlerquellen bei der Positions- und Geschwindigkeitsbestimmung mittels GPS sowie ein kurzer Ausblick auf das europ¨aische Satellitennavigationssystem Galileo schließen dieses Kapitel ab. Ein wesentlicher Teil dieses Buches ist der stochastischen Filterung gewidmet. Prinzipiell k¨ onnen stochastische Filter dazu genutzt werden, den Zustand eines vorzugsweise linearen Systems anhand von vorliegenden Messungen zu sch¨atzen. Neben einer Ein¨ f¨ uhrung in die Theorie linearer Systeme und einer Ubersicht u ¨ ber einige stochastische Grundlagen wird das am h¨ aufigsten eingesetzte stochastische Filter, das Kalman-Filter, ausf¨ uhrlich diskutiert. Hierbei wird auch auf das erweiterte Kalman-Filter und das linearisierte Kalman-Filter eingegangen, die bei schwach nichtlinearen Systemen oder schwach nichtlinearen Zusammenh¨ angen zwischen Messwerten und Systemzustand eingesetzt werden k¨ onnen. Dar¨ uber hinaus wird die Klasse der Sigma Point Kalman-Filter angesprochen, die bei vorliegenden Nichtlinearit¨aten bessere Ergebnisse als z.B. ein erweitertes Kalman-Filter liefern k¨ onnen. Eine Diskussion der Filterung bei zeitkorreliertem System- oder Messrauschen schließt dieses Kapitel ab. Im verbleibenden Teil dieses Buches werden Anwendungsbeipiele integrierter Navigationssysteme detailliert betrachtet. So wird auf unterschiedliche Systemarchitekturen bei der GPS/INS-Integration eingegangen, die unter den Begriffen Loosely Coupled System und Tightly Coupled System bekannt sind. Dazu werden der systematische Entwurf entsprechender Navigationsfilter beschrieben und anhand von numerischen Simulationen die unterschiedlichen Charakteristiken dieser Integrationsans¨atze herausgearbeitet und anschließend experimentell verifiziert. Schließlich wird auf ein Verfahren eingegangen, mit dem die Leistungsf¨ ahigkeit eines Tightly Coupled Systems durch die Verarbeitung zeitlicher Differenzen von Tr¨ agerphasenmessungen deutlich gesteigert werden kann, ohne dass wie normalerweise bei der Verarbeitung von Tr¨agerphasenmessungen eine Differential GPS Base Station ben¨ otigt wird. Schließlich wird aufgezeigt, dass obwohl GPS/INS-Integration formal ein nichtlineares Filterproblem darstellt, der Einsatz von Sigma-Point-Kalman-Filtern hier zu keiner Steigerung von Genauigkeit oder Integrit¨ at der Navigationsl¨ osung f¨ uhren kann. Die Ber¨ ucksichtigung der Zeitdifferenz zwischen G¨ ultigkeit und Verf¨ ugbarkeit von St¨ utzinformationen wird ebenfalls thematisiert. Als weiteres Anwendungsbeispiel wird Transfer Alignment betrachtet. Darunter versteht man die Bestimmung der initialen Navigationsl¨osung eines Inertialnavigationssystems oder eines integrierten GPS/INS-Systems unter Verwendung der Navigationsl¨osung eines zweiten, meist deutlich hochwertigeren Navigationssystems. Die unterschiedlichen

1 Einleitung

3

Ans¨ atze zur Bestimmung einer solchen initialen Navigationsl¨osung werden erl¨autert, bevor auf den Einfluss von Vibrationen eingegangen wird. Vibrationen zeigen sich als zus¨ atzliches, zeitkorreliertes Rauschen der Inertialsensoren. Schließlich wird ein Verfahren vorgestellt, dass die Ber¨ ucksichtigung dieses zeitkorrelierten Rauschens erlaubt, ohne dass der Zustandsvektor des Filters erweitert werden muss. Dies ist im Hinblick auf den Rechenaufwand und die numerische Robustheit des Filters von Vorteil. Abschließend wird noch auf ein unbemanntes, schwebeflugf¨ahiges Flugger¨at eingegangen, bei dem bez¨ uglich des Navigationssystementwurfs einige Besonderheiten zu beachten sind: Zum einen ist im ortsfesten Schwebeflug und im geradlienigen, gleichf¨ormigen Flug der Yaw-Winkel, also der Winkel gegen¨ uber der Nordrichtung, bei einem GPS/INSSystem unbeobachtbar. Daher wurde zus¨ atzlich ein Magnetometer, das Messungen des Erdmagnetfeldvektors liefert, in das Navigationssystem integriert. Zum anderen ist aufgrund der geringen G¨ ute der aus Platz- und Gewichtsgr¨ unden eingesetzten MEMS¨ Inertialsensoren die u uckung von GPS-Ausf¨allen an¨ bliche Vorgehensweise der Uberbr¨ hand der Inertialnavigation nicht m¨ oglich, da die Lagefehler zu schnell mit der Zeit anwachsen und somit eine Stabilisierung des Flugger¨ats unm¨oglich machen w¨ urden. Daher wird w¨ ahrend GPS-Ausf¨ allen ein Lagefilter verwendet, das die Langzeitgenauigkeit der Lageinformationen dadurch erzielt, dass Beschleunigunsmessungen und Magnetometerdaten als St¨ utzinformationen verarbeitet werden. Hierbei wird ausgenutzt, dass die Messungen der Beschleunigungssensoren von der Schwerbeschleunigung dominiert sind. Auch hier wird die Leistungsf¨ ahigkeit der entwickelten Algorithmik anhand von numerischen Simulationen und den Ergebnissen von Flugversuchen aufgezeigt.

2

Lineare Systeme

Um Aussagen treffen zu k¨ onnen, mit welchen Ausgangsgr¨oßen ein physikalisches System auf bestimmte Eingangsgr¨ oßen reagiert, ist eine mathematische Beschreibung des Systemverhaltens notwendig. Hierbei ist meist ein Kompromiss zwischen mathematischer Komplexit¨ at und geforderter Genauigkeit der Systembeschreibung zu treffen. Reale Systeme weisen praktisch immer in irgendeiner Form Nichtlinearit¨aten auf. Diese m¨ ussen jedoch nicht dominant sein, h¨ aufig stellt sich das Verhalten des nichtlinearen Systems in der N¨ ahe eines Arbeitspunktes als n¨ aherungsweise linear dar. Aufgrund der guten mathematischen Handhabbarkeit bietet sich in solchen F¨allen eine lineare Systembeschreibung an. Lineare Systeme sind dadurch gekennzeichnet, dass f¨ ur Ein- und Ausgangsgr¨oßen das ¨ Superpositionsprinzip und das Uberlagerungsprinzip gelten. Ein SISO (Single-InputSingle-Output) System bildet eine Eingangsgr¨oße u(t) u ¨ ber einen Operator f {} auf eine Ausgangsgr¨ oße x(t) ab: x(t) = f {u(t)}

(2.1)

Reagiert dieses System auf eine beliebige Eingangsgr¨oße u1 (t) mit der Ausgangsgr¨oße oße u2 (t) mit der Ausgangsgr¨oße x2 (t), so sind x1 (t) und auf eine beliebige Eingangsgr¨ Superpositionsprinzip und Verst¨ arkungsprinzip erf¨ ullt, wenn f¨ ur beliebige Konstanten c1 und c2 gilt: c1 x1 (t) + c2 x2 (t) = f {c1 u1 (t) + c2 u2 (t)}

(2.2)

Beispiel RC-Glied Im Folgenden soll als Beispiel f¨ ur ein lineares System ein RC-Glied betrachtet werden, siehe Abb. 2.1. Die Eingangsspannung ist mit u(t) bezeichnet, x(t) ist die Ausgangs1 , spannung. Dieses RC-Glied realisiert einen Tiefpass 1. Ordnung. Setzt man ω0 = RC so findet man f¨ ur die Differentialgleichung des RC-Gliedes dx(t) + ω0 x(t) = ω0 u(t) . dt

(2.3)

Die zur L¨ osung der Differentialgleichung notwendige Randbedingung sei durch x(0) = 0 gegeben. Transformiert man Gl. (2.3) in den Laplace-Bereich, erh¨alt man sX(s) + ω0 X(s) = ω0 U (s) .

(2.4)

¨ Die Ubertragungsfunktion G(s) dieses Tiefpasses X(s) = G(s)U (s) =

ω0 U (s) s + ω0

(2.5)

6

2 Lineare Systeme

R u(t)

x(t)

C

Abbildung 2.1: RC-Glied.

¨ gibt Aufschl¨ usse u at: Liegen die Pole der Ubertragungsfunktion ¨ ber die Systemstabilit¨ in der linken Halbebene des s-Bereichs, ist das System BIBO (Bounded-Input-BoundedOutput) stabil, d.h. endliche Eingangsgr¨ oßen haben nur endliche Ausgangsgr¨oßen zur ¨ Folge. Die Polstelle der Ubertragungsfunktion des Tiefpasses befindet sich bei s = −ω0 , damit handelt es sich um ein stabiles System. Die R¨ ucktransformation in den Zeitbereich liefert X(s) = G(s)U (s) s

c x(t) = g(t) ∗ u(t) =


t g(τ )u(t − τ )dτ

(2.6)

0

Hierbei bezeichnet ∗ den linearen Faltungsoperator, g(t) wird als Impulsantwort bezeichnet. Angewendet auf den Tiefpass erh¨ alt man
t x(t) =

ω0 e−ω0 τ u(t − τ )dτ .

(2.7)

0

Es l¨ asst sich nun leicht zeigen, dass f¨ ur Ein- und Ausgangsgr¨oße in Gl. (2.7) das Superpositionsprinzip und das Verst¨ arkungsprinzip Gl. (2.2) erf¨ ullt sind, es handelt sich folglich um ein lineares System. ¨ Allgemein ist jedes System, dass sich im Laplace-Bereich durch eine Ubertragungsfunktion charakterisieren l¨ asst, linear. Des Weiteren erkennt man, dass sich der Pol der ¨ Ubertragungsfunktion in der Exponentialfunktion in Gl. (2.7) wiederfindet. Damit ist anschaulich verst¨ andlich, dass ein Pol in der rechten Halbebene des s-Bereichs zu einem instabilen System f¨ uhrt, da in diesem Fall das Integral f¨ ur t → ∞ auch bei endlichen Anregungen gegen unendlich streben kann.
Umfassende Einf¨ uhrungen in die Theorie linearer SISO-Systeme lassen sich z.B. in [22], ¨ [94], [29], [50] finden. Analoge Uberlegungen lassen sich aber auch f¨ ur MIMO (MultipleInput-Multiple-Output) Systeme anstellen. Um MIMO-Systeme mathematisch zu beschreiben, bietet sich die Zustandsraumdarstellung an.

2.1 Zustandsraumdarstellung linearer Systeme

2.1

7

Zustandsraumdarstellung linearer Systeme

Viele physikalische Systeme lassen sich anhand von Differentialgleichungen beschreiben. Gelingt eine Systembeschreibung mit einer gew¨ohnlichen, linearen Differentialgleichung n-ter Ordnung, dn−1 x(t) dx(t) dn x(t) + a0 x(t) = b0 u(t) + a + ... + a1 n−1 dtn dtn−1 dt

(2.8)

so spricht man von einem linearen System n-ter Ordnung. Um zu einer Zustandsraumdarstellung dieses Systems zu gelangen, werden neue Variablen x1 ...xn wie folgt eingef¨ uhrt: x1 =

x(t) dx(t) = dt

x2 =

(2.9) dx1 dt

(2.10)

.. .

(2.11) n−1

xn =

d

x(t) dxn−1 = dtn−1 dt

(2.12)

Damit ergibt sich aus Gl. (2.8): dn x(t) dxn = −an−1 xn − ... − a1 x2 − a0 x1 + b0 u = n dt dt Die Zustandsraumdarstellung von Gl. (2.8) der Gleichungen (2.9) – (2.13): ⎛ ⎛ ⎞• 0 1 0 x1 0 1 ⎜ 0 ⎜ x2 ⎟ ⎜ . ⎟ = ⎜ .. ⎝ ⎝ . ⎠ . . −a0 −a1 −a2 xn

(2.13)

erh¨alt man schließlich durch Umschreiben ⎞ ⎛ ⎞ x1 0 ⎟ ⎜ x2 ⎟ ⎜ 0 ⎟ ⎟⎜ . ⎟ + ⎜ . ⎟u ⎠⎝ . ⎠ ⎝ . ⎠ . . · · · −an−1 xn b0 ··· ···

0 0 .. .

⎞⎛

(2.14)

Allgemein ist die Zustandsdifferentialgleichung eines linearen Systems, das auch durch mehrere, gekoppelte Differentialgleichungen beschrieben sein kann, gegeben durch x˙ (t) = Ax(t) + Bu(t) ,

(2.15)

das zugeh¨ orige Blockdiagramm ist in Abb. 2.2 dargestellt. F¨ ur die Matrizen und Vektoren in Gl. (2.15) sind folgende Bezeichnungen gel¨aufig: x(t) : Zustandsvektor x˙ (t) : Zeitliche Ableitung des Zustandsvektors u(t) : Eingangsvektor A : Systemmatrix B : Eingangsmatrix

8

2 Lineare Systeme

u



x˙

+

B

x

A

Abbildung 2.2: Zeitkontinuierliches Zustandsraummodell.

Sind die Systemmatrix und die Eingangsmatrix zeitlich nicht ver¨anderlich, spricht man von einem zeitinvarianten System. In diesem Fall l¨asst sich die Zustandsdifferentialgleichung (2.15) l¨ osen, indem man zun¨ achst mit e−At multipliziert: e−At x˙ (t) = e−At Ax(t) + e−At Bu(t)

(2.16)

Die Matrizen e−At und A kommutieren, wie sich anhand der Reihenentwicklung der Matrix-Exponentialfunktion verifizieren l¨ asst: e

−At



1 1 2 2 · A = I + At + A t + . . . · A 1! 2! 1 1 2 = A + A t + A3 t2 + . . . 1! 2! = A · e−At

(2.17)

Damit ergibt sich: e−At x˙ (t) − Ae−At x(t) d  −At e x(t) dt
t  d −Aτ e x(τ ) dτ dτ

=

e−At Bu(t)

(2.18)

=

e−At Bu(t)

(2.19)


t =

t0

e

−At

e−Aτ Bu(τ ) dτ

(2.20)

e−Aτ Bu(τ ) dτ

(2.21)

t0

x(t) − e

−At0


t x(t0 )

= t0

Multiplikation mit eAt und Umstellen liefert die gesuchte L¨osung der Zustandsdifferentialgleichung (2.15):

x(t)

=

e

A(t−t0 )


t x(t0 ) + t0

eA(t−τ ) Bu(τ ) dτ

(2.22)

2.2 Eigenwerte und Eigenvektoren

9

Die Matrix Φ(t, τ ) = eA(t−τ )

(2.23)

wird als Transitionsmatrix bezeichnet. F¨ ur die Transitionsmatrix gelten folgende Zusammenh¨ange: Φ(t, t) Φ(τ, t) Φ(τ, λ)Φ(λ, t) ∂Φ(τ, t) τ ∂Φ(τ, t) t

= I = Φ−1 (t, τ ) = Φ(τ, t)

(2.24) (2.25) (2.26)

= A Φ(τ, t)

(2.27)

= −A Φ(τ, t)

(2.28)

Ist die Zustandsdifferentialgleichung homogen, d.h. x˙ (t) = Ax(t) ,

(2.29)

kann mit Hilfe dieser Transitionsmatrix direkt von einem zu einem Zeitpunkt τ bekannten Systemzustand x(τ ) auf den Systemzustand zu einem beliebigen Zeitpunkt t geschlossen werden: x(t) = Φ(t, τ )x(τ )

(2.30)

Hierbei ist es unerheblich, ob sich der Zeitpunkt t bez¨ uglich τ in der Zukunft oder in der Vergangenheit befindet. H¨ aufig wird das dynamische Verhalten des Systems ausreichend genau erfasst, wenn die Transitionsmatrix durch einige Glieder der Reihenentwicklung der MatrixExponentialfunktion angen¨ ahert wird. Alternativ kann eine Berechnung mittels LaplaceTransformation von Gl. (2.15) erfolgen:    (s) sX(s) − x(0) = AX(s) + BU   (s) (sI − A) X(s) = x(0) + BU −1 −1   (s) X(s) = (sI − A) x(0) + (sI − A) BU

(2.31) (2.32) (2.33)

Durch R¨ ucktransformation in den Zeitbereich ergibt sich die Transitionsmatrix zu Φ(t, 0) c

2.2

s(sI − A)−1 .

(2.34)

Eigenwerte und Eigenvektoren

Auch wenn mit Gl. (2.22) die L¨ osung der Zustansdifferentialgleichung (2.15) bereits bekannt ist, kann die Berechnung von Eigenwerten und Eigenvektoren der Systemmatrix A wichtige Einsichten in das dynamische Verhalten des linearen Systems liefern.

10

2 Lineare Systeme

Die Gleichung Az = λz

(2.35)

wird als algebraisches Eigenwertproblem bezeichnet. Erf¨ ullen ein Vektor zi und eine skalare Gr¨ oße λi die Gl. (2.35), so wird λi als Eigenwert der Matrix A und zi als zugeh¨ origer Eigenvektor bezeichnet. Handelt es sich bei A um eine reelle Matrix, k¨onnen nur reelle Eigenwerte und konjugiert komplexe Paare von Eigenwerten auftreten. Eigenvektoren zu verschiedenen reellen Eigenwerten sind linear unabh¨ angig, der Eigenvektor zu einem Eigenwert eines konjugiert komplexen Paares ist konjugiert komplex zu dem Eigenvektor des konjugiert komplexen Eigenwertes. Ist A reell und symmetrisch, so treten nur reelle Eigenwerte und Eigenvektoren auf. Zur Berechnung der Eigenwerte wird Gl. (2.35) umgestellt: (A − λI) x = 0

(2.36)

Offensichtlich k¨ onnen hierf¨ ur nur dann nicht-triviale L¨osungen x = 0 existieren, wenn die Matrix (A − λI) nicht invertierbar ist. Dies ist der Fall, wenn die Determinante verschwindet: det (A − λI) = 0

(2.37)

Die Gl. (2.37) ist ein Polynom n-ten Grades in λ und wird als charakteristisches Polynom der Matrix A bezeichnet. Die gesuchten Eigenwerte λi sind folglich gerade die Nullstellen des charakteristischen Polynoms, die Vielfachheit einer Nullstelle wird als algebraische Vielfachheit des Eigenwertes bezeichnet. Z¨ahlt man die Eigenwerte entsprechend ihrer Vielfachheit, so besitzt eine n × n Matrix n Eigenwerte. Da f¨ ur jeden Eigenwert λi die Matrix (A − λi I) nicht invertierbar ist, erkennt man durch Vergleich mit Gl. (2.33), ¨ dass es sich bei den Eigenwerten auch gerade um die Polstellen der Ubertragungsfunktion handelt. Liegen nur Eigenwerte mit negativem Realteil vor, so befinden sich alle ¨ Polstellen der Ubertragungsfunktion in der linken Halbebene des Laplace-Bereichs und das System ist stabil. Ist ein Eigenwert λi gefunden, kann der zugeh¨orige Eigenvektor zi anhand von Gl. (2.36) bestimmt werden. Dabei zeigt sich, dass Eigenvektoren lediglich eine Richtung im Raum festlegen, ihre L¨ ange ist nicht festgelegt: Ist zi ein Eigenvektor, so ist c · zi ebenfalls Eigenvektor. Die Dimension des Raumes, der von den zu einem bestimmten Eigenwert geh¨ orenden Eigenvektoren aufgespannt wird, d.h. die Anzahl der zu einem eventuell mehrfachen Eigenwert geh¨ orenden linear unabh¨angigen Eigenvektoren, wird als geometrische Vielfachheit des Eigenwertes bezeichnet. Linearkombinationen von Eigenvektoren zu einem mehrfachen Eigenwert sind ebenfalls wieder Eigenvektoren. Prinzipiell ist die geometrische Vielfachheit gr¨ oßer oder gleich eins, die algebraische Vielfachheit ist gr¨ oßer oder gleich der geometrischen Vielfachheit. Besitzt die n × n Matrix A n linear unabh¨angige Eigenvektoren, so bilden diese eine Basis des zur Matrix geh¨ orenden Vektorraumes. Es liegen dann n Gleichungen in der Form von Gl. (2.35) vor, die wie folgt geschrieben werden k¨onnen: A [z1 , z2 , . . . , zn ] = [z1 , z2 , . . . , zn ] diag[λ1 , λ2 , . . . , λn ]

(2.38)

2.2 Eigenwerte und Eigenvektoren

11

Da die Eigenvektoren linear unab¨ angig sind, ist die Matrix [z1 , z2 , . . . , zn ] invertierbar, man erh¨ alt [z1 , z2 , . . . , zn ]

−1

A [z1 , z2 , . . . , zn ] = diag[λ1 , λ2 , . . . , λn ] .

(2.39)

Die Matrix A kann also mit Hilfe der Matrix der Eigenvektoren diagonalisiert werden, die Hauptdiagonale der resultierenden Diagonalmatrix enth¨alt gerade die Eigenwerte der Matrix A. Dar¨ uber hinaus kann gezeigt werden, dass die L¨osung der homogenen Zustandsdifferentialgleichung (2.29) durch x(t) = c1 z1 eλ1 t + c2 z2 eλ2 t + . . . + cnzn eλn t

(2.40)

gegeben ist[32]. Die Koeffizienten c1 – cn h¨angen dabei von der Anfangsbedingung x(0) ab. Da die Eigenvektoren eine Basis bilden, ist sichergestellt, dass jeder beliebige Anfangsvektor x(0) als Linearkombination der Eigenvektoren dargestellt werden kann: x(0) = c1z1 + c2 z2 + . . . + cn zn

(2.41)

Die Gr¨ oßen zi eλi t werden als Eigenmoden des Systems bezeichnet, ein Eigenvektor ist gerade diejenige Anfangsbedingung, die den zugeh¨origen Eigenmode anregt. Beispiel Feder-Masse-Schwinger Zur Veranschaulichung soll im Folgenden ein dynamisches System betrachtet werden, siehe Abb. 2.3, das aus zwei identischen Massen m, die u ¨ ber Federn gekoppelt sind, besteht. Die Massen sollen reibungsfrei schwingen k¨onnen, die identischen Federn werden als ideal angenommen, ihre Federkonstante ist mit k bezeichnet. Die Auslenkungen der Massen aus den Ruhelagen werden mit x1 und x2 bezeichnet, die Geschwindigkeiten der Massen mit v1 und v2 . Das dynamische Verhalten des Systems wird durch zwei gekoppelte Differentialgleichungen beschrieben, ∂ 2 x1 2k k = − x1 + x2 ∂t2 m m ∂ 2 x2 k 2k x1 − x2 . = ∂t2 m m Setzt man ω 2 =

k m,

(2.43)

so erh¨ alt man als Zustandsraumdarstellung des Systems

⎞• ⎛ x1 0 ⎜ v1 ⎟ ⎜−2ω 2 ⎝x ⎠ = ⎝ 0 2 v2 ω2 ⎛

(2.42)

1 0 0 ω2 0 0 0 −2ω 2

⎞⎛ ⎞ x1 0 0⎟ ⎜ v1 ⎟ . 1⎠ ⎝x2 ⎠ v2 0

Die Systemmatrix besitzt die Eigenwerte √ λ1/2 = ±jω , λ3/4 = ±jω 3 , wobei j die imagin¨ are Einheit bezeichnet.

(2.44)

(2.45)

12

2 Lineare Systeme

x1

10 10

10 10

10 10 10 10

10

x2

10 10 10

10 10

10 10

10

10

10 10 10 10

10 10

Abbildung 2.3: Dynamisches System bestehend aus zwei gekoppelten Massen.

Da bei diesem System weder Eigenwerte mit positivem noch mit negativem Realteil vorliegen, m¨ usste eine formale Stabilit¨ atsaussage – es ist offensichtlich, dass das betrachtete System stabil ist – anhand des Minimalpolynoms getroffen werden: Ein nicht erregtes, zeitinvariantes, lineares System ist stabil, wenn keine mehrfachen imagin¨aren Nullstellen des Minimalpolynoms existieren. F¨ ur weiterf¨ uhrende Betrachtungen hierzu sei jedoch z.B. auf [120] verwiesen. Es liegen vier linear unabh¨ angige Eigenvektoren vor, ⎛ √ ⎞ ⎛ ⎞ ±j 3ω ∓jω ⎜ −1 ⎟ ⎜ 1 ⎟ √ ⎟ z1/2 = ⎝ , z3/4 = ⎜ ⎝∓j 3ω⎠ , ∓jω⎠ 1 1

(2.46)

so dass mit diesen Eigenwerten und Eigenvektoren durch Gl. (2.40) die L¨osung der Zustandsdifferentialgleichung (2.44) gegeben ist. Da Realteil und Imagin¨arteil einer komplexen L¨ osung ebenfalls L¨ osungen sind, kann die physikalische, reelle L¨osung durch Realteilbildung gewonnen werden. Da hier nur komplexe Eigenvektoren vorliegen, kann keine reelle Anregung gefunden werden, die nur einen einzelnen Eigenmode anregt. Mit physikalisch m¨oglichen, reellen Anregungen k¨ onnen nur Linearkombinationen der Eigenmoden angeregt werden. Besonders hervorzuheben sind hier zwei F¨ alle: 1. Fall. F¨ ur die Koeffizienten c1 = j

a a , c2 = −j 2ω 2ω

,

c3 = 0 , c4 = 0

(2.47)

erh¨ alt man die reelle Anregung x(0) = (a, 0, a, 0)T ,

(2.48)

2.2 Eigenwerte und Eigenvektoren

13

was gerade dem gleichsinnigen Auslenken beider Massen in x-Richtung entspricht. Dadurch werden die beiden Eigenmoden zu den Eigenwerten λ1 und λ2 angeregt, es ergibt sich x1 (t) = a cos ωt x2 (t) = a cos ωt ,

(2.49) (2.50)

das gleichphasige Schwingen beider Massen mit der Kreisfrequenz ω, die als Eigenfrequenz bezeichnet wird. 2. Fall. F¨ ur die Koeffizienten c1 = 0 , c2 = 0 ,

c3 = j

a a √ , c4 = −j √ 2ω 3 2ω 3

(2.51)

erh¨ alt man ebenfalls eine reelle Anregung x(0) = (−a, 0, a, 0)T ,

(2.52)

was gerade dem gegensinningen Auslenken beider Massen entspricht. Nun werden die beiden Eigenmoden zu den Eigenwerten λ3 und λ4 angeregt, man erh¨alt mit √ x1 (t) = −a cos ω 3t √ x2 (t) = a cos ω 3t ,

(2.53) (2.54)

√ ein gegenphasiges Schwingen beider Massen, diesmal mit der Kreisfrequenz ω 3, die gr¨ oßer ist als im gleichsinnigen Schwingfall. Eine weitere M¨ oglichkeit, jeweils nur die beiden ersten oder die beiden letzten Eigenmoden anzuregen, ist die Vorgabe entsprechender gleichsinniger bzw. gegensinniger Geschwindigkeiten in den jeweiligen Ruhelagepunkten der Massen. Neben dieser Konstruktion einer L¨ osung der homogenen Zustandsdifferentialgleichung k¨ onnen Eigenvektoren auch zu einer effizienten Berechnung von h¨oheren Potenzen der Systemmatrix A verwendet werden. Anhand von Gl. (2.39) findet man n¨amlich folgenden Zusammenhang: 

−1 Ak = [z1 , z2 , . . . , zn ] diag λk1 , λk2 , . . . , λkn [z1 , z2 , . . . , zn ]

(2.55)

Zur Berechnung der k-ten Potenz gen¨ ugt es also, die Diagonalmatrix der potenzierten Eigenwerte mit der Matrix der Eigenvektoren und ihrer Inversen zu multiplizieren. Offensichtlich sind die Eigenvektoren der Systemmatrix A aber auch Eigenvektoren von Ak , da Ak anhand dieser Eigenvektoren diagonalisiert werden kann: [z1 , z2 , . . . , zn ]

−1

 Ak [z1 , z2 , . . . , zn ] = diag λk1 , λk2 , . . . , λkn

(2.56)

14

2 Lineare Systeme

Der Umkehrschluss ist jedoch nicht zul¨ assig, Eigenvektoren der Matrix Ak m¨ ussen keine Eigenvektoren der Matrix A sein: Die Eigenwerte des Quadrats der in diesem Beispiel betrachteten Systemmatrix sind λ21 = −ω 2

λ23 = −3ω 2 .

(2.57)

Es gibt zwei Eigenwerte mit der Vielfachheit zwei, vier linear unabh¨angige Eigenvektoren der Matrix A2 sind durch ⎛ ⎞ ⎛ ⎞ ⎛ ⎞ ⎛ ⎞ 0 1 0 1 ⎜1⎟ ⎜0⎟ ⎜−1⎟ ⎜0⎟ z1,λ21 = ⎝ ⎠ , z2,λ21 = ⎝ ⎠ , z3,λ22 = ⎝ ⎠ , z4,λ22 = ⎝ ⎠ 0 1 0 −1 1 0 1 0 (2.58) gegeben. Die ersten beiden und die letzten beiden Eigenvektoren spannen jeweils einen Eigenraum der Dimension zwei auf. Bei diesen Eigenvektoren handelt es sich aber nicht um Eigenvektoren der Systemmatrix A. Wie eingangs erw¨ahnt, sind jedoch Linearkombinationen von Eigenvektoren zu einem Eigenwert ebenfalls wieder Eigenvektoren, es wird lediglich eine andere Basis des Eigenraums gew¨ahlt. Bildet man in diesem Beispiel die Linearkombinationen
z1,λ21 − jωz2,λ21 z1,λ 2 =  1


z1,λ21 + jωz2,λ21 z2,λ 2 =  1 √
z3,λ2 = z3,λ21 + jω 3z4,λ21 1 √
z3,λ21 − jω 3z4,λ21 , z4,λ 2 =  1

(2.59) (2.60) (2.61) (2.62)

so erh¨ alt man aus den in Gl. (2.58) angegebenen Eigenvektoren der Matrix A2 gerade diejenigen Eigenvektoren, die auch Eigenvektoren der Matrix A sind.
Die Existenz einer Basis aus n linear unabh¨angigen Eigenvektoren ist nur im Falle n verschiedener Eigenwerte garantiert. Treten mehrfache Eigenwerte auf, kann es sein, dass – anders als im obigen Beispiel – eine solche Basis nicht existiert. In diesem Fall muss die L¨ osung der homogenen Zustandsdifferentialgleichung (2.29) eine andere Gestalt als Gl. (2.40) aufweisen. Dies soll anhand eines weiteren Beispiels verdeutlicht werden. Beispiel geradlinige, gleichf¨ ormige Bewegung Gegeben sei die Zustandsdifferentialgleichung •

x 0 1 x = , v 0 0 v

(2.63)

die gerade eine eindimensionale Bewegung mit konstanter Geschwindigkeit beschreibt. Hier besitzt die Systemmatrix den Eigenwert null mit der Vielfachheit zwei, es existiert lediglich ein Eigenvektor z1 = (1, 0)T . Damit ist klar, dass sich beliebige Anfangsbedingungen nicht wie in Gl. (2.41) als Linearkombination der Eigenvektoren der

2.3 Lineare Systeme im Zeitdiskreten

15

Systemmatrix darstellen lassen, die Systemmatrix ist nicht diagonalisierbar. F¨ ur die Anfangsbedingung

0 (2.64) x(0) = v0 findet man als L¨ osung von Gl. (2.63)



x(t) v0 · t , = v0 v(t)

(2.65)

was sich formal von Gl. (2.40) unterscheidet. F¨ ur die gew¨ahlte Anregung w¨achst x(t) f¨ ur t → ∞ u ¨ ber alle Maßen, so gesehen handelt es sich hier um kein stabiles System.


2.3

Lineare Systeme im Zeitdiskreten

In den vorangegangenen Abschnitten wurden lineare Systeme anhand von Differentialgleichungen beschrieben. H¨ aufig ist es jedoch von Interesse, das Verhalten eines solchen Systems mit Hilfe eines Rechners zu simulieren – sei es als Teil eines Simulationsprogrammes, das damit ein n¨ aherungsweises Abbild der realen Welt zu schaffen versucht, oder als Teil eines Sch¨ atzalgorithmus wie dem Kalman-Filter. Eine Implementierung auf einem Rechner kann jedoch immer nur in diskreter Zeit erfolgen, somit ist die Er¨ mittlung eines zeitdiskreten Aquivalents eines in kontinuierlicher Zeit beschriebenen, linearen Systems von zentraler Bedeutung. Im Folgenden wird daher der Systemzustand nur zu diskreten Zeitpunkten t(k) = tk = k · T

(2.66)

betrachtet, wobei die Zeitschrittweite mit T bezeichnet ist. Um von einem zum Zeitpunkt tk bekannten Systemzustand xk auf den Systemzustand zum n¨achsten Zeitschritt zu schließen, muss lediglich in Gl. (2.22) eingesetzt werden:

xk+1 = eA(tk+1 −tk ) xk +

t
k+1

eA(tk+1 −τ ) Bu(τ ) dτ

(2.67)

tk

Unter der Annahme, dass sich die Eingangsgr¨oßen u(t) im betrachteten Zeitintervall nicht ¨ andern, l¨ asst sich der Integralterm n¨aherungsweise berechnen. Dazu wird die Schreibweise eA(tk+1 −tk ) = Φtk+1 ,tk = Φk

(2.68)

16

2 Lineare Systeme

verwendet, man erh¨ alt t
k+1

eA(tk+1 −τ ) Bu(τ ) dτ =

tk

t
k+1

eA(tk+1 −τ ) dτ Buk

tk t
k+1

= tk

∞  1 i A (tk+1 − τ )i dτ Buk i! i=0

 =

= =

tk+1 ∞  1 i −1 i+1 A (tk+1 − τ ) Buk i! i + 1 i=0 tk ∞  1 i 1 i+1 A (tk+1 − tk ) Buk i! i + 1 i=0

1 2 2 1 I + AT + A T + ... BT uk . 2 6

(2.69)

Der Klammerausdruck wird bis zum linearen Glied durch 12 (I + Φk ) korrekt wiedergegeben, so dass man Bk ≈

1 I + Φk BT 2

(2.70)

schreiben kann. ¨ Man erh¨ alt daher als zeitdiskretes Aquivalent zu Gl. (2.15) die Differenzengleichung xk+1 = Φk xk + Bk uk .

(2.71)

Alternative, ungenauere N¨ aherungen der Eingangsmatrix Bk , die an Stelle von Gl. (2.70) verwendet werden k¨ onnen, sind durch Bk = Φk BT

(2.72)

Bk = BT

(2.73)

und

gegeben. Beispiel geradlinige, beschleunigte Bewegung Eine geradlinige, beschleunigte Bewegung wird im Zeitkontinuierlichen beschrieben durch



x˙ 0 1 x 0 = + a. v˙ 0 0 v 1

(2.74)

2.3 Lineare Systeme im Zeitdiskreten

17

Die Transitionsmatrix wird durch Reihenentwicklung berechnet; bei der Systemmatrix handelt es sich um eine nilpotente Matrix, d.h. h¨ohere Potenzen dieser Matrix verschwinden. Dies erlaubt hier eine einfache, exakte Berechnung der Transitionsmatrix. Mit der N¨ aherung Gl. (2.70) erh¨ alt man im Zeitdiskreten 1 2





1 T x x T = + 2 ak . 0 1 v k+1 v k T

(2.75)


Prinzipiell kann nat¨ urlich bei der Berechnung des Integrals in Gl. (2.69) zun¨achst auf eine Reihenentwicklung verzichtet werden, man erh¨alt in diesem Fall t
k+1

e

A(tk+1 −τ )

tk

t
k+1

eA(tk+1 −τ ) dτ Buk

Bu(τ ) dτ = tk

= = =

 tk+1 −eA(tk+1 −τ ) A−1 Buk tk   eA(tk+1 −tk ) − eA(tk+1 −tk+1 ) A−1 Buk

AT  e − I A−1 Buk .

(2.76)

In einfachen F¨ allen l¨ asst sich die Matrix-Exponentialfunktion und die Inverse der Systemmatrix analytisch berechnen, Gleichung (2.76) stellt dann eine interessante Alternative zu den zuvor angegebenen N¨ aherungen dar. Eine weitere M¨ oglichkeit, um von einer zeitkontinuierlichen Differentialgleichung zu einer zeitdiskreten Differenzengleichung zu gelangen, ist die z-Transformation [64], [75], [31]. Der Grundgedanke besteht hierbei darin, die Differentialgleichung zun¨achst in den Laplace-Bereich zu transformieren, von dort in den z-Bereich u ¨ berzugehen, um anhand des Zusammenhangs xk−n = z −n X(z)

(2.77)

schließlich eine Differenzengleichung zu erhalten. Der exakte Zusammenhang zwischen Laplace-Bereich und z-Bereich, z = esT ,

(2.78)

ist hierzu jedoch ungeeignet und muss approximiert werden. Dies soll anhand des bereits betrachteten Tiefpasses, der durch Gl. (2.3) beschrieben ist, veranschaulicht werden.

18

2 Lineare Systeme

Beispiel RC-Glied Numerische Integration von Gl. (2.3) mit der Rechteckregel r¨ uckw¨arts liefert
tk −ω0 x(t) + ω0 u(t) dt

xk = xk−1 + tk−1

≈ xk−1 − ω0 xk T + ω0 uk T .

(2.79)

Die Transformation des so gewonnenen, n¨aherungsweisen Zusammenhangs xk =

1 ω0 T xk−1 + uk 1 + ω0 T 1 + ω0 T

(2.80)

anhand von Gl. (2.77) in den z-Bereich f¨ uhrt auf X(z) =

1 ω0 T z −1 X(z) + U (z) , 1 + ω0 T 1 + ω0 T

(2.81)

¨ so dass die Ubertragungsfunktion im z-Bereich berechnet werden kann: X(z) =

ω0 +

1 T

ω0 U (z) (1 − z −1 )

(2.82)

¨ Durch Vergleich mit der Ubertragungsfunktion im Laplace-Bereich, siehe Gl. (2.5), findet man die Transformationen: s=

1 (1 − z −1 ) , T

z=

1 1 − sT

(2.83)

Dies ist keineswegs die einzige M¨ oglichkeit, den Zusammenhang zwischen LaplaceBereich und z-Bereich zu approximieren. Die Verwendung der Trapezregel zur numerischen L¨ osung der Differentialgleichung f¨ uhrt zum Beispiel auf s=

2 z−1 , T z+1

z=

2 + sT . 2 − sT

(2.84)

Prinzipiell f¨ uhrt diese Approximation jedoch dazu, dass auch das tats¨achliche Systemverhalten durch die zeitdiskrete Repr¨ asentation nur approximiert wird. Durch Einsetzen des exakten Zusammenhangs Gl. (2.78) in Gl. (2.81) erh¨alt man im Laplace-Bereich die ¨ Ubertragungsfunktion der zeitdiskreten Repr¨asentation des Systems. Mit s = jω

(2.85)

¨ ist der Ubergang in den Frequenzbereich m¨oglich. Die zeitdiskrete Beschreibung des ¨ Tiefpasses Gl. (2.80) besitzt daher im Frequenzbereich die Ubertragungsfunktion X(ω) =

ω0 +

1 T

ω0 U (ω) , (1 − e−jωT )

(2.86)

2.3 Lineare Systeme im Zeitdiskreten

19

¨ die sich von der tats¨ achlichen Ubertragungsfunktion des zeitkontinuierlichen Systems, X(ω) =

ω0 U (ω) , ω0 + jω

(2.87)

unterscheidet. Frequenz- und Phasengang von Gl. (2.86) und (2.87) sind in Abb. 2.4 0 dargestellt. Hierbei wurde als Grenzfrequenz des Tiefpasses f0 = ω 2π = 10Hz und als 1 Abtastfrequenz f = T = 200Hz gew¨ ahlt. Man erkennt, dass f¨ ur Signalfrequenzen deutlich unterhalb der halben Abtastfrequenz eine sehr gute Approximation vorliegt, bei h¨ oheren Signalfrequenzen jedoch nicht. In diesem Zusammenhang ist auch das Shannonsche Abtasttheorem von Bedeutung, das besagt, dass ein zeitkontinuierliches Signal nur dann aus einer zeitdiskreten Abtastung des Signals rekonstruiert werden kann, wenn alle im Signal enthaltenen Frequenzen kleiner als die halbe Abtastfrequenz sind.
¨ Auch im z-Bereich k¨ onnen anhand der Lage der Polstellen der Ubertragungsfunktion Stabilit¨ atsaussagen getroffen werden. Im Laplace-Bereich ist die Stabilit¨atsgrenze durch die imagin¨ are Achse s = jω gegeben. Durch Einsetzten in Gl. (2.78) erkennt man, dass die imagin¨ are Achse des Laplace-Bereichs gerade auf den Einheitskreis im z-Bereich abgebildet wird, die linke Halbebene findet sich im Inneren des Einheitskreises wieder. ¨ Ein System ist daher stabil, wenn sich die Pole der Ubertragungsfunktion im z-Bereich im Inneren des Einheitskreises befinden. Prinzipiell k¨ onnen also Differentialgleichungen durch Ermittlung ihrer zeitdiskreten Zustandsraumdarstellung anhand von Gl. (2.68), (2.70) und (2.71) oder durch Anwendung der z-Transformation gel¨ ost werden. Dar¨ uber hinaus existieren nat¨ urlich noch eine Vielzahl weiterer Verfahren zur numerischen L¨osung von Differentialgleichungen, die bei gegebener Zeitschrittweite genauere Ergebnisse liefern, hier sei z.B. die Simpson-Regel und das Verfahren von Heun erw¨ ahnt. Prinzipiell wird zwischen impliziten und expliziten Verfahren unterschieden. Explizite Verfahren berechnen den L¨ osungswert zum n¨achsten Zeitschritt ausschließlich anhand von zum aktuellen Zeitschritt bekannten Informationen, wohingegen bei impliziten Verfahren Gleichungen gel¨ ost werden m¨ ussen. Explizite Verfahren liefern jedoch nur bei kleinen Zeitschrittweiten eine gute N¨ aherung, implizite Verfahren lassen gr¨oßere Zeitschrittweiten zu. Der Unterschied zwischen einem impliziten und einem expliziten Verfahren soll anhand der Euler-Methode veranschaulicht werden. Die L¨ osung der Differentialgleichung ∂x = f (x, t) , ∂t

x0 = x(0)

(2.88)

anhand des expliziten Euler-Verfahrens ist gegeben durch xk+1 = xk + T · f (xk , tk ) .

(2.89)

Beim impliziten Euler-Verfahren wird xk+1 = xk + T · f (xk+1 , tk+1 )

(2.90)

20

2 Lineare Systeme

1.2 kontinuierlich zeitdiskret

1

Betrag

0.8 0.6 0.4 0.2 0 0.1

1

10 Frequenz (Hz)

80

100

1000

kontinuierlich zeitdiskret

60

Phase (◦ )

40 20 0 -20 -40 -60 -80 0.1

1

10 Frequenz (Hz)

100

Abbildung 2.4: Amplitudengang und Phasengang des RC-Tiefpasses.

1000

2.3 Lineare Systeme im Zeitdiskreten

21

angesetzt, so dass die sich ergebende Gleichung erst noch nach xk+1 aufgel¨ost werden muss. Im bisher betrachteten linearen Fall stellt das keine Schwierigkeit dar, bei nichtlinearen Differentialgleichungen muss die sich ergebende Gleichung eventuell numerisch, z.B. durch Verwendung der Newton-Raphson-Methode, gel¨ost werden. Ein weiteres, weit verbreitetes Verfahren zur numerischen L¨osung von Differentialgleichungen ist das Runge-Kutta-Verfahren vierter Ordnung [100]. Die Grundidee der Runge-Kutta-Verfahren l¨ asst sich wie folgt skizzieren: Es existieren verschiedene Verfahren erster Ordnung zur Integration der rechten Seite von Gl. (2.88), die sich jedoch in ihren Fehlertermen unterscheiden. Somit k¨onnen Verfahren h¨oherer Ordnung konstruiert werden, wenn diese Verfahren erster Ordnung so kombiniert werden, dass sich deren Fehlerterme – bis zu einer bestimmten Ordnung – gegenseitig wegheben. Ein Runge-Kutta-Verfahren vierter Ordnung zu L¨osung von Gl. (2.88) ist gegeben durch k1 = T · f (xk , tk ) 1 T k2 = T · f (xk + k1 , tk + ) 2 2 1 T k3 = T · f (xk + k2 , tk + ) 2 2 k4 = T · f (xk + k3 , tk + T ) 1 1 1 1 xk+1 = xk + k1 + k2 + k3 + k4 . 6 3 3 6

(2.91) (2.92) (2.93) (2.94) (2.95)

Die angesprochenen Verfahren sollen anhand eines einfachen Beispiels verglichen werden. Hierzu wird die Differentialgleichung ∂x = −λx , ∂t

x0 = 1

(2.96)

f¨ ur λ = 12 betrachtet. Der Parameter λ beeinflusst hierbei entscheidend die zul¨assige Zeitschrittweite, je gr¨ oßer λ ist, desto kleiner muss bei den expliziten Verfahren die Zeitschrittweite gew¨ ahlt werden. Die analytische L¨osung von Gl. (2.96) ist durch x(t) = e−λt

(2.97)

gegeben. Das explizite Euler Verfahren f¨ uhrt auf xk+1 = xk − λxk T ,

(2.98)

das implizite Euler-Verfahren liefert xk+1 = xk − λxk+1 T 1 xk . xk+1 = 1 + λT

(2.99)

22

2 Lineare Systeme

Zeitschrittweite T = 0.01s 1

♦ 

0.5

 x(t)

analytisch Euler explizit Euler implizit 4.Ordn. Runge-Kutta

0

♦  

♦   ♦   ♦   ♦   ♦   ♦ 

-0.5

-1 0

0.2

0.4

0.6

0.8

1

Zeit (s) Abbildung 2.5: Numerische L¨ osung der DGL (2.96) f¨ ur verschiedene L¨ osungsverfahren, Zeitschrittweite 0.1 s, λ = 12.

Die L¨ osungen der Differentialgleichung (2.96) anhand dieser beiden Verfahren und unter Verwendung des Runge-Kutta-Verfahrens vierter Ordnung sind – im Vergleich zur korrekten, analytischen L¨ osung und f¨ ur verschiedene Zeitschrittweiten – in Abb. 2.5 – 2.7 dargestellt. Man erkennt, dass f¨ ur eine ausreichend kleine Zeitschrittweite alle drei betrachteten Verfahren vergleichbare Ergebnisse liefern, die gut mit der exakten, analytischen L¨osung u ¨ bereinstimmen. Wird die Zeitschrittweite vergr¨oßert, treten beim expliziten EulerVerfahren Oszillationen auf, wohingegen das implizite Euler-Verfahren und das RungeKutta-Verfahren vierter Ordnung noch gute Ergebnisse liefern. Wird die Zeitschrittweite weiter vergr¨ oßert, divergieren beide explizite Verfahren, lediglich das implizite EulerVerfahren bleibt stabil. Es l¨ asst sich zeigen, dass dies bei dieser Differentialgleichung f¨ ur beliebige Zeitschrittweiten der Fall ist. Obwohl das Runge-Kutta-Verfahren vierter Ordnung fast schon als Standard-Verfahren zur numerischen L¨osung von Differentialgleichungen bezeichnet werden kann, liefert hier das einfachere, implizite Euler-Verfahren bessere Ergebnisse. Differentialgleichungen, die sich numerisch schwierig“ l¨osen lassen, werden als steife ” Differentialgleichungen bezeichnet. Zur L¨osung dieser Differentialgleichungen kommen meist implizite Verfahren zum Einsatz.

2.3 Lineare Systeme im Zeitdiskreten

23

Zeitschrittweite T = 0.15s 2 analytisch Euler explizit Euler implizit 4.Ordn. Runge-Kutta ♦       ♦ ♦

1.5 1 ♦  x(t)

0.5

♦

 

 

0 -0.5 ♦

-1

♦   ♦  

-1.5 -2 0

0.2

0.4

0.6

0.8

1

Zeit (s) Abbildung 2.6: Numerische L¨ osung der DGL (2.96) f¨ ur verschiedene L¨ osungsverfahren, Zeitschrittweite 0.15 s, λ = 12.

Zeitschrittweite T = 0.25s 2 1.5

 analytisch Euler explizit Euler implizit 4.Ordn. Runge-Kutta



1 ♦  x(t)

0.5



0





♦   

-0.5 -1 -1.5 -2 0

0.2

♦

0.4

0.6

0.8

1

Zeit (s) Abbildung 2.7: Numerische L¨ osung der DGL (2.96) f¨ ur verschiedene L¨ osungsverfahren, Zeitschrittweite 0.25 s, λ = 12.

24

2.4

2 Lineare Systeme

Nichtlinearit¨aten

In den vorangegangenen Abschnitten wurden ausschließlich lineare Systeme betrachtet. Wie eingangs erw¨ ahnt, kann ein System h¨aufig nicht exakt als lineares System beschrieben werden. Um die bisher eingef¨ uhrten Methoden wie die Analyse des Systemverhaltens anhand der Systemmatrix oder die Berechnung einer Transitionsmatrix auch bei nichtlinearen Systemen anwenden zu k¨onnen, muss eine lineare Systembeschreibung gefunden werden, die dem tats¨ achlichen, nichtlinearen Systemverhalten m¨oglichst nahe kommt. Dies kann bei nicht allzu nichtlinearen Systemen durch Linearisierung erreicht werden. Betrachtet wird das nichtlineare System ∂x = f(x, u) . ∂t

(2.100)

Mit ¯, ∆x = x − x

∆u = u − u¯

(2.101)

erh¨ alt man als Taylor-Reihenentwicklung der rechten Seite von (2.100) um die Lineari¯, u ¯ sierungspunkte x   (x, u)    ∂ f(x, u)  ∂ f ¯ ¯ f (x, u) ≈ f x, u + · ∆x + · ∆u . (2.102)   ∂x  ¯ ∂u  ¯  x= x

u=  u

Hierbei wurde die Reihenentwicklung nach dem linearen Glied abgebrochen. Durch Einsetzen von Gl. (2.102) und den Zusammenh¨angen ¯ ∂∆x ¯   ∂x ∂ x ∂ x ¯, u¯ = + , = f x ∂t ∂t ∂t ∂t in Gl. (2.100) ergibt sich schließlich   ∂∆x ∂ f(x, u)  ∂ f(x, u)  = · ∆x +   ∂t ∂x  ¯ ∂u  x=  x

(2.103)

· ∆u .

(2.104)

¯ u=  u

Bei Gl. (2.104) handelt es sich um ein lineares System in Zustandsraumdarstellung, das das Systemverhalten des nichtlinearen Systems in der N¨ahe der Linearisierungspunkte beschreibt. Die Matrix ⎛ ∂f1 ∂f1 ⎞  . . . ∂x ∂x 1 n   ∂ f(x, u)  ⎜ .. ⎟ F= = ⎝ ... (2.105)   ⎠ .  ∂x  ¯ ∂fn ∂fn   x= x ¯ ∂x1 . . . ∂xn  x= x ¯ bezeichnet. Analog wird als Jacobi-Matrix der Funktion f im Linearisierungspunkt x stellt die Jacobi-Matrix ⎛ ∂f1 ∂f1 ⎞  ∂u1 . . . ∂um    ∂ f (x, u)  ⎜ .. ⎟ B= = ⎝ ... (2.106)  . ⎠ ∂u  ¯ ∂fn ∂fn  u=  u ¯ ∂x1 . . . ∂xm u=  u

2.4 Nichtlinearit¨ aten

25

10

10

10 10

10

10

ϕ l y˜ g

Abbildung 2.8: Mathematisches Pendel.

die Eingangsmatrix des linearisierten Systems dar. Die beschriebene Vorgehensweise soll im Folgenden am Beispiel des mathematischen Pendels gezeigt werden. Mathematisches Pendel Das mathematische Pendel ist durch die Annahmen gekennzeichnet, dass die gesamte Pendelmasse im Schwerpunkt konzentriert ist, die Aufh¨angung masselos ist und das Pendel reibungsfrei schwingen kann. Anhand der Skizze in Abb. 2.8 erkennt man, dass bei einer Auslenkung des Pendels um den Winkel ϕ aufgrund der angreifenden Gravitation eine r¨ ucktreibende Beschleunigung x¨ = −g sin ϕ

(2.107)

resultiert. Die Geschwindigkeit des Pendels ist gegeben durch x˙ = v = ωl , so dass sich mit ϕ˙ = ω folgende nichtlineare Differentialgleichung ergibt: •

ω ϕ = g − l sin ϕ ω Die Jacobi-Matrix in der N¨ ahe der Ruhelage ϕ = 0 berechnet sich zu 



0 1  0 1 ∂ f(x)  = = , F= g g  − cos ϕ 0  − 0 ∂x  l l ϕ=0 ϕ=0

(2.108)

(2.109)

(2.110)

das Verhalten des Pendels kann f¨ ur kleine Auslenkungen folglich durch das lineare System •



0 1 ∆ϕ ∆ϕ = (2.111) g −l 0 ∆ω ∆ω beschrieben werden.


26

2 Lineare Systeme

Abschließend sei noch darauf hingewiesen, dass sich auch die vorgestellten numerischen Integrationsverfahren zur L¨ osung von Differentialgleichungen im nichtlinearen Fall anwenden lassen. Bei den expliziten Verfahren m¨ ussen dazu lediglich Funktionswerte der nichtlinearen rechten Seite berechnet werden. Bei den impliziten Verfahren muss eine nichtlineare Gleichung – eventuell numerisch – gel¨ost werden. Ebenso k¨ onnen diese Integrationsverfahren auch auf Differentialgleichungssysteme wie Gl. (2.100) angewendet werden. Die prinzipielle Vorgehensweise bleibt dabei unverandert, es ergeben sich beispielsweise beim Runge-Kutta-Verfahren vierter Ordnung ¨ lediglich vektorielle Koeffizienten k1 − k4 .

3

Inertiale Navigation

Die inertiale Navigation gewann ab Mitte des 20. Jahrhunderts kontinuierlich an Bedeutung. Prinzipiell handelt es sich bei inertialer Navigation um ein Koppelnavigationsverfahren. Bei der Koppelnavigation (engl. dead reckoning) wird fortlaufend die Bewegungsrichtung, die Geschwindigkeit und die seit der letzten Positionsbestimmung vergangene Zeit bestimmt. Die aktuelle Position wird ermittelt, indem anhand dieser Informationen die in dem betrachteten Zeitintervall zur¨ uckgelegte Strecke berechnet – in einigen F¨ allen kann diese auch direkt gemessen werden – und unter Ber¨ ucksichtigung der Bewegungsrichtung zur letzten bekannten Position hinzu addiert wird. Die Grundidee eines Inertialnavigationssystems (INS) besteht darin, die f¨ ur eine Koppelnavigation notwendigen Informationen durch Messung von Beschleunigungen und Drehraten zu gewinnen. Prinzipiell werden daher immer jeweils drei orthogonal zueinander angeordnete Drehratensensoren und Beschleunigungssensoren ben¨otigt. Bei den ersten Systemen wurden die Beschleunigungssensoren auf einer kardanisch gelagerten, stabilisierten Plattform montiert. Die sensitiven Achsen der Beschleunigungssensoren waren dadurch von den Bewegungen des Fahrzeugs1 entkoppelt und wiesen in Richtung raumfester Achsen wie Norden, Osten und Unten. Anhand der gemessenen Beschleunigungen konnten daher direkt Geschwindigkeitsinkremente in diesen Raumrichtungen bestimmt werden, durch Integration erhielt man Geschwindigkeit und Position. Die Drehratensensoren hatten lediglich die Aufgabe, Lage¨anderungen der stabilisierten Plattform im Raum z.B. aufgrund von Reibung in den Lagern der kardanischen Aufh¨ angung zu detektieren. Diese unerw¨ unschten Lage¨anderungen wurden mit Hilfe von Elektromotoren korrigiert. Die hierf¨ ur ben¨otigten Drehratensensoren mussten nur Drehungen von Bruchteilen eines Grads pro Stunde messen k¨onnen, andererseits durften nur minimalste Nullpunktsfehler vorliegen. Die Lage des Fahrzeugs konnte an den Stellungen der Kardanrahmen abgelesen werden. Mit dem Aufkommen der Ringlaserkreisel (RLG) ab Mitte der sechziger Jahre wurden Strapdown-Systeme technisch realisierbar. Bei einem Strapdown-System kommt eine Inertialsensoreinheit bestehend aus drei Beschleunigungs- und drei Drehratensensoren zum Einsatz, die fest mit dem Fahrzeug verbunden ist. Eine solche Inertialsensoreinheit wird als Inertial Measurement Unit (IMU) bezeichnet2 . Der Unterschied zu den Plattform-Systemen besteht darin, dass anhand der Drehratensensordaten die Lageanderungen des Fahrzeugs erfasst werden und so durch Integration die Fahrzeuglage ¨ berechnet wird. Diese Lageinformationen werden verwendet, um die in k¨orperfesten Achsen gemessenen Beschleunigungen in ein Koordinatensystem mit raumfesten Koor1 Der

¨ stehen. Begriff Fahrzeug kann im Folgenden f¨ ur ein Schiff, ein Flugger¨ at, ein Automobil o.A. wird noch zwischen Inertial Sensor Assembly (ISA), d.h. den eigentlichen Inertialsensoren, und Inertial Measurement Unit (IMU), d.h. der Sensorik im Verbindung mit der Elektronik zur Bereitstellung einer Schnittstelle, unterschieden. 2 Manchmal

28

3 Inertiale Navigation

dinatenrichtungen, h¨ aufig ein Nord-Ost-Unten-Koordinatensystem, umzurechnen. Anschließend wird ebenfalls durch Integration Geschwindigkeit und Position bestimmt. Da anhand der Drehratensensordaten die Drehbewegungen des Fahrzeugs nachvollzogen werden sollen, m¨ ussen die Drehratensensoren eines Strapdown-Systems auch Drehungen von mehreren hundert Grad pro Sekunde messen k¨onnen. Andererseits ist eine wartungsintensive, komplexe Mechanik wie bei den Plattformsystemen nicht mehr n¨otig, was in der Regel zu Gewichtsvorteilen und einem geringeren Platzbedarf f¨ uhrt. Die weiteren Abschnitte dieses Kapitels befassen sich mit der Strapdown-Rechnung, also dem mathematischen Hintergrund eines Strapdown-Systems. Plattformsysteme werden nicht betrachtet. Da bei jedem Inertialnavigationssystem die Navigationsinformationen durch Integration unvermeidlich fehlerbehafteter Messwerte gewonnen werden, wachsen die Navigationsfehler mit der Zeit an. Daher werden auch die wesentlichen Fehlercharakteristiken eines INS n¨ aher untersucht.

3.1

Koordinatensysteme

In der Strapdown-Rechnung werden eine Reihe von Koordinatensystemen ben¨otigt. Zum Beispiel erfassen Inertialsensoren Beschleunigungen und Drehraten bez¨ uglich eines Inertialkoordinatensystems, die Geschwindigkeit des Fahrzeugs wird h¨aufig in den Koordinatenrichtungen Norden, Osten und Unten ben¨otigt. Im Folgenden werden alle ben¨otigten Koordinatensysteme, wie sie in Abb. 3.1 veranschaulicht sind, definiert. • Die Achsen des k¨ orperfesten Koordinatensystems (b-frame) sind fest in Bezug zum Fahrzeug und weisen in Fahrzeugl¨angsrichtung (xb ), nach rechts (y b ) und nach unten (z b ). Der Ursprung befindet sich im Fahrzeug. Sind die sensitiven Achsen der Inertialsensoren exakt orthogonal ausgerichtet, fallen diese mit den Achsen des k¨ orperfesten Koordinatensystems zusammen. Die Messwerte einer IMU fallen daher in Koordinaten dieses Koordinatensystems an. • Der Ursprung des Inertialkoordinatensystems (i-frame) befindet sich im Mittelpunkt des Rotationsellipsoids, der die Erdgestalt ann¨ahert. Die Koordinatenachsen sind fest in Bezug zu den Fixsternen. Die z i -Achse des Inertialkoordinatensystems f¨ allt mit der Rotationsachse der Erde zusammen, die xi - und y i -Achse ¨ liegen in der Aquatorebene. Eine IMU misst Beschleunigungen und Drehraten des k¨ orperfesten Koordinatensystems bez¨ uglich des Inertialkoordinatensystems. • Das erdfeste Koordinatensystem (e-frame) besitzt denselben Ursprung wie das Inertialkoordinatensystem. Die Koordinatenachsen sind fest im Bezug zur Erde, wobei die z e -Achse mit der z i -Achse zusammenf¨allt. Die xe -Achse ist bestimmt ¨ durch die Schnittgerade von Aquatorebene und der Ebene des Nullmeridians. Das erdfeste Koordinatensystem rotiert bez¨ uglich des Inertialkoordinatensystems um die z e -Achse mit der Winkelgeschwindigkeit Ω. Dieses erdfeste Koordinatensystem wird auch als Earth Centered, Earth Fixed“ (ECEF) Koordinatensystem ” bezeichnet.

3.1 Koordinatensysteme

29

Ω

xn zi = ze

xb yn

zb

yb z

n

ye yi xi xe

Abbildung 3.1: Koordinatensysteme f¨ ur die erdnahe Navigation.

• Der Ursprung des Navigationskoordinatensystems (n-frame) f¨allt mit dem Ursprung des k¨ orperfesten Koordinatensystems zusammen. Die xn - und y n -Achse weisen in Nord- bzw. Ostrichtung und liegen in der Tangentialebene an den Erdellipsoid. Die z n -Achse weist nach unten und ist parallel zur Schwerebeschleunigung, die sich aus der Summe von Gravitation und erdrotationsbedingter Zentripetalbeschleunigung ergibt. Einzelne Komponenten eines Vektors in Koordinaten des Navigationskoordinatensystems werden mit n, e und d (north, east, down) indiziert.

F¨ ur die Fahrzeugposition – also die Position des Ursprungs des n-frames und b-frames – ist lediglich eine Angabe bez¨ uglich des erdfesten Koordinatensystems sinnvoll. Diese kann anhand von Breitengrad ϕ, L¨ angengrad λ und H¨ohe h (h¨aufig LLH f¨ ur latitude, longitude, height) bez¨ uglich des Erdellipsoids oder bez¨ uglich der Achsen des e-frames erfolgen. Um hierbei eine eindeutige Zuordnung zwischen ECEF-Koordinaten und Breitengrad und H¨ ohe zu erhalten, muss ein Erdmodell definiert werden.

30

3 Inertiale Navigation

Ω

ze Lokale Vertikale

b a

λ

ϕ

ye

xe

Abbildung 3.2: Erdellipsoid.

3.1.1

Das Erdmodell WGS-84

Die Form der Erde l¨ asst sich in erster N¨ aherung als ein Rotationsellipsoid modellieren. Die Summe aus Zentripetalkraft aufgrund der Erdrotation und Massenanziehungskraft der Erde steht an jedem Punkt des Rotationsellipsoids senkrecht auf der dortigen Tangentialebene, ist also parallel zur lokalen Vertikalen. Anschaulich l¨asst sich das dadurch erkl¨ aren, dass diese Kr¨ afte im Laufe der Zeit die Gestalt der Erde geformt haben. Dadurch haben Schwerebeschleunigungsmodelle im Navigationskoordinatensystem eine besonders einfache Darstellung, da lediglich die z-Komponente der Schwerebeschleunigung von null verschieden ist. Im englischen Sprachgebrauch bezeichnet gravitation die Beschleunigung aufgrund der Massenanziehung der Erde, die Summe aus Zentripetalbeschleunigung und Beschleunigung aufgrund der Massenanziehung der Erde wird gravity genannt. Im Deutschen entspricht sowohl Gravitation als auch Schwerkraft dem englischen gravitation, die Schwerebeschleunigung entspricht dem englischen Ausdruck gravity. Ein g¨ angiges, wenn auch nicht besonders genaues Schwerebeschleunigungsmodell ist gegeben durch3 3 Die

H¨ ohe geht hier mit negativem Vorzeichen ein, da die H¨ ohe nach unten positiv gez¨ ahlt wird.

3.1 Koordinatensysteme

31

Tabelle 3.1: Parameter des Erdmodells WGS-84

Bezeichnung

Symbol

Wert

Große Halbachse des Ellipsoids:

a

6378137.0m

Kleine Halbachse des Ellipsoids:

b

a(1 − f ) = 6356752.3142m

Abflachung des Ellipsoids:

f

Exzentrizit¨ at des Ellipsoids:

e

Durchschnittlicher Kr¨ ummungsradius:

R0

a−b 1 = 298.257223563 a f (2 − f ) = 0.0818191908426 √ Re Rn

Nord-S¨ ud Kr¨ ummungsradius

Rn

Ost-West Kr¨ ummungsradius

Re

a (1−e21−e sin2 ϕ)3/2 √ a 2

Erddrehrate:

Ω

7.292115 × 10−5 rad s

⎛ ⎜ g ln = ⎝

0 0

2

1−e2 sin ϕ

⎞

⎟  2 ⎠   R0 g0 1 + g1 sin2 ϕ + g2 sin2 2ϕ R0−h

(3.1)

ur hochgenaue mit g0 = 9.780318 m/s2 , g1 = 5.3024 × 10−3 , g2 = 5.9 × 10−6 . F¨ Anwendungen m¨ ussen die in der Realit¨ at auftretenden Abweichungen der Schwerebeschleunigung von der lokalen Vertikalen jedoch ber¨ ucksichtigt werden. Die Parameter des Erdellipsoids nach dem World Geodetic System of 1984“ (WGS-84) ” sind in Tabelle 3.1 angegeben4 . Die geographische Breite ϕ eines Ortes ist der Winkel, unter dem die lokale Vertikale die ¨ Aquatorebene schneidet. Aufgrund der Elliptizit¨at der Erde weist die lokale Vertikale und somit auch die Schwerebeschleunigung nicht zum Erdmittelpunkt. Diese Zusammenh¨ ange sind in Abb. 3.2 verdeutlicht. Die H¨ ohe u ¨ ber dem Erdellipsoid wird im Folgenden stets negativ angegeben. Diese Konvention wird h¨ aufig gew¨ ahlt, da eine Bewegung nach oben weg vom Erdellipsoid in Richtung Weltall in negativer Richtung der vertikalen Achse des Navigationskoordinatensystems erfolgt. Eine weitere, gebr¨ auchliche H¨ohenangabe anstelle der H¨ohe u ¨ ber dem Erdellipsoid ist die H¨ ohe u ¨ ber dem Geoid, was vereinfacht gesagt der H¨ohe u ¨ ber dem Meeresspiegel entspricht. Der Geoid soll im Weiteren keine Rolle spielen.

3.1.2

Transformationen

Um LLH-Positionsangaben und ECEF-Positionsangaben ineinander umrechnen zu k¨onnen, sind entsprechende Transformationsformeln n¨otig. Diese werden im Folgenden angegeben. 4 Ger¨ uchteweise

ist die Jahreszahl 1984 der einzige Parameter, der seitdem nicht ge¨ andert wurde.

32

3 Inertiale Navigation

LLH → ECEF Die Umrechnung von Breitengrad, L¨ angengrad und H¨ohe in ECEF-Koordinaten ist nach [26] gegeben durch a N (ϕ) =  1 − e2 · sin(ϕ)2 x = (N − h) · cos(ϕ) · cos(λ) y = (N − h) · cos(ϕ) · sin(λ)   z = N · (1 − e2 ) − h · sin(ϕ) .

(3.2)

(3.3) (3.4) (3.5)

ECEF → LLH Um eine Transformationsvorschrift zu erhalten, die eine Umrechnung von ECEFKoordinaten in Breitengrad, L¨ angengrad und H¨ohe erlaubt, m¨ ussen prinzipiell nur die Gl. (3.2) – (3.5) nach den gesuchten Gr¨ oßen aufgel¨ost werden. Aufgrund der Nichtlinearit¨ at dieser Gleichungen ist das jedoch durchaus anspruchsvoll. In [52], [65] und [26] findet man folgende L¨ osung:  p = x2 + y 2 E 2 = a2 − b 2 F = 54b2z 2 G = p2 + (1 − e2 )z 2 − e2 E 2 e 4 F p2 c= G3 13   s = 1 + c + c2 + 2c P = Q = r0 = U = V = z0 = e
=

F 3(s + 1s + 1)2 G2  1 + 2e4 P 

1 1 2 P e2 p 1 P (1 − e2 )z 2 + a 1+ − P p2 − − 1+Q 2 Q Q(1 + Q) 2  (p − e2 r0 )2 + z 2  (p − e2 r0 )2 + (1 − e2 )z 2 2 b z aV a e b

(3.6) (3.7) (3.8) (3.9) (3.10) (3.11) (3.12) (3.13) (3.14) (3.15) (3.16) (3.17) (3.18)

3.1 Koordinatensysteme z + (e
)2 z0 p λ = arctan 2(y, x) 2

b h= U −1 aV

ϕ = arctan

33

(3.19) (3.20) (3.21)

In [26] wird zus¨ atzlich noch eine wesentlich einfachere N¨aherungsl¨osung angegeben, die bis zum Low Earth Orbit5 G¨ ultigkeit besitzt: a N (ϕ) =  1 − e2 · sin(ϕ)2  p = x2 + y 2

za θ = arctan pb 2 2 − b a (e
)2 = b2

z + (e
)2 b sin3 θ ϕ = arctan p − e2 a cos3 θ λ = arctan 2 (y, x) p − N (ϕ) h= cos ϕ

(3.22) (3.23) (3.24) (3.25) (3.26) (3.27) (3.28)

Alternativ zu diesen geschlossenen Transformationsformeln k¨onnen Breitengrad und H¨ ohe auch iterativ bestimmt werden, dies soll hier jedoch nicht n¨aher betrachtet werden. W¨ ahrend Positionsangaben wenig Raum f¨ ur Mehrdeutigkeiten und Unklarheiten lassen, kann das bei anderen Gr¨ oßen wie Geschwindigkeiten oder Drehraten nur durch eine exakte Nomenklatur erreicht werden.

3.1.3

Nomenklatur

Zur eindeutigen Bezeichnung von Geschwindigkeiten v , Beschleunigungen a und Drehraten ω  werden drei Indizes ben¨ otigt, die beispielhaft anhand des Geschwindigkeitsvektors vebn eingef¨ uhrt werden sollen: Der obere Index gibt an, in Koordinaten welchen Koordinatensystems die Gr¨ oße gegeben ist. Bei diesem Beispiel steht der obere Index n f¨ ur Navigationskoordinatensystem, die so bezeichnete Geschwindigkeit ist also in den Koordinatenrichtungen Norden, Osten und Unten angegeben. Die beiden unteren Indizes geben an, dass es sich um die Geschwindigkeit des k¨orperfesten Koordinatensystems (Index b f¨ ur body) bez¨ uglich des erdfesten Koordinatensystems (Index e) handelt. 5 Der

Low Earth Orbit reicht von ca. 200km bis 2000km H¨ ohe.

34

3 Inertiale Navigation

Auf Grundlage dieser Nomenklatur k¨ onnen eine Reihe hilfreicher Rechenregeln angegeben werden. Diese werden hier anhand von konkreten Beispielen eingef¨ uhrt, sie gelten jedoch in derselben Art und Weise auch f¨ ur beliebige andere Koordinatensysteme. F¨ ur Geschwindigkeiten v gilt n n v eb = −v be ,

(3.29)

ebenso f¨ ur Drehraten ω  , die auch verkettet werden k¨onnen: n n  eb ω = − ω be n n n ω eb = ω  ei +  ω ib .

Mit Hilfe einer schiefsymmetrischen Matrix (engl. skew symmetric matrix ) kann das Kreuzprodukt zweier Vektoren als Matrix-Vektor-Multiplikation dargestellt werden. Gegeben sind zwei Vektoren ⎛ ⎞ ⎛ ⎞ ax bx a = ⎝ay ⎠ , b = ⎝by ⎠ . (3.30) az bz Dann gilt a × b = Ab mit

(3.31)

⎛

⎞ 0 −az ay 0 −ax ⎠ . A = ⎝ az −ay ax 0

(3.32)

Die kreuzproduktbildende Matrix wird mit dem fettgedruckten Großbuchstaben des zugeh¨ origen Vektors bezeichnet. Alternativ sind auch die Schreibweisen A = [a×] = skew(a)

(3.33)

u ¨ blich. F¨ ur die kreuzproduktbildenden Matrizen von Drehraten sind die bisher vorgestellten Rechenregeln g¨ ultig: n n = [ ω eb ×] Ω eb n n Ω eb = −Ω be n n n Ω eb = Ω ei + Ω ib

(3.34) (3.35) (3.36)

Die Umrechnung eines Vektors in ein anderes Koordinatensystem kann mit Hilfe der Richtungskosinusmatrix erfolgen, siehe hierzu auch Abschnitt 3.2.3.

3.1 Koordinatensysteme

35

F¨ ur die Transformation von Vektoren gilt n b ω  ei = Cbn ·  ω ei ,

(3.37)

wohingegen kreuzproduktbildene Matrizen von Vektoren wie folgt transformiert werden m¨ ussen: n b = C bn · Ω ei · C nb Ω ei

(3.38)

Dies gilt auch f¨ ur die Umrechnung beliebiger Matrizen: In Koordinaten des k¨orperfesten Koordinatensystems sei mit einer beliebigen Matrix Ab der Zusammenhang y b = Ab · x b

(3.39)

gegeben. Durch Einsetzen der Transformationsgleichungen f¨ ur die Vektoren x b , y b findet man Cbn y n = Ab · Cbn x n y n = Cnb Ab Cbn x n .

(3.40) (3.41)

Da der Gl. (3.39) entsprechende Zusammenhang in Koordinaten des Navigationskoordinatensystems y n = An · xn

(3.42)

lauten muss, findet man durch Koeffizientenvergleich mit Gl. (3.41) f¨ ur die Transformation der Matrix A An = Cnb Ab Cbn .

(3.43)

Bei der Richtungskosinusmatrix handelt es sich um eine orthonormale Matrix, ihre Inverse ist gleich der Transponierten. Man kann daher schreiben: C bn = C nb,−1 = C nb,T

(3.44)

Auch eine Verkettung von Richtungskosinusmatrizen, die manchmal auch kurz als Drehoder Rotationsmatrizen bezeichnet werden, ist zul¨assig: C bn n C e C en,T

= C en C be = C en C ne = C nn = I

(3.45) (3.46)

Richtungskosinusmatrizen sind nicht die einzige M¨oglichkeit, die Lage zweier Koordinatensysteme zueinander zu beschreiben.

36

3 Inertiale Navigation ves OS

rSn rSe

ON rT

OE Abbildung 3.3: Transformation von Vektoren.

3.2

Lagedarstellungen

Um einen Vektor von einem Koordinatensystem in ein anderes umrechnen zu k¨onnen, muss zwischen zwei F¨ allen unterschieden werden: 1. Es handelt sich um einen Vektor, der vom Ursprung des Koordinatensystems zu einem festen Punkt im Raum weisen soll. Fallen nun die Urspr¨ unge der beiden beteiligten Koordinatensysteme nicht zusammen, ¨andert sich auch der Fußpunkt des Vektors. Die Transformation in ein anderes Koordinatensystem kann daher nicht durch eine bloße Rotation erfolgen, vielmehr muss die Translation des Vektorfußpunktes ber¨ ucksichtigt werden. Ein Beispiel hierf¨ ur w¨are der Ortsvektor rSe zur Position eines Satelliten, der im ECEF-Koordinatensystem vom Erdmittelpunkt OE zum Satelliten weist, siehe Abb. 3.3. Im Navigationskoordinatensystem weist der entsprechende Vektor rSn nun aber von der Fahrzeugposition, also dem Ursprung des Navigationskoordinatensystems ON zum Satelliten, besitzt also einen anderen Fußpunkt, man erh¨ alt rSn = Cne (rSe − rTe ) .

(3.47)

F¨ ur den Sonderfall, dass zwar ein Ursprungsvektor zu einem festen Punkt im Raum gemeint ist, die Urspr¨ unge der beteiligten Koordinatensysteme jedoch zusammenfallen, gilt rT = 0 und die Umrechnung erfolgt durch eine reine Rotation. Ein Beispiel hierf¨ ur w¨ are der Ortsvektor zu einem Satelliten, der von Koordinaten des Inertialkoordinatensystems in ECEF-Koordinaten umgerechnet werden soll: rSe = CeirSi

(3.48)

2. Der Vektor beschreibt eine Richtung im Raum und eine bestimmte L¨ange. Hier erfolgt die Transformation in ein anderes Koordinatensystem durch eine reine Rotation, auch wenn die Urspr¨ unge der Koordinatensysteme nicht zusammenfallen.

3.2 Lagedarstellungen

37

Ein Beispiel hierf¨ ur w¨ are der Vektor der Relativgeschwindigkeit von Satellit und e , der von ECEF-Koordinaten ins Navigationskoordinatensystem umgeErde ves rechnet werden soll: n e ves = Cneves

(3.49)

Dabei ist es unerheblich, ob das Navigationskoordinatensystem bez¨ uglich des erdfesten Koordinatensystems bewegt ist: Der Geschwindigkeitsvektor wird durch die Rotation nur auf andere Koordinatenachsen bezogen, beschreibt aber auch im Navigationskoordinatensystem noch die Geschwindigkeit des Satelliten bez¨ uglich dem erdfestem Koordinatensystem. Im Folgenden wird die Lage zweier Koordinatensysteme betrachtet. Gemeint ist hierbei, wie das eine Koordinatensystem rotiert werden m¨ usste, um seine Achsen parallel zu den Achsen des anderen Koordinatensystems auszurichten. Ohne Beschr¨ankung der Allgemeing¨ ultigkeit wird f¨ ur diese Betrachtungen das Navigationskoordinatensystem und das k¨ orperfeste Koordinatensystem gew¨ahlt. Die Lage der Koordinatensysteme kann durch Eulerwinkel, eine Richtungskosinunsmatrix, einen Orientierungsvektor oder ein Quaternion beschrieben werden.

3.2.1

Eulerwinkel

Durch die drei Eulerwinkel roll, pitch und yaw werden drei Drehungen beschrieben, die nacheinander ausgef¨ uhrt das Navigationskoordinatensystem in das k¨orperfeste Koordinatensystem u uhren. Hierbei beschreibt der Yaw-Winkel ψ, der auch als Azimuth ¨ berf¨ bezeichnet wird, die erste Drehung um die z-Achse des Navigationskoordinatensystems, also um die lokale Vertikale. Anschließend wird um die neue y-Achse des rotierten Koordinatensystems um den Pitch-Winkel θ gedreht. In der deutschsprachigen Literatur wird der Pitch-Winkel manchmal auch als Nickwinkel bezeichnet. Die letzte Drehung um den Roll-Winkel φ erfolgt schließlich um die neue x-Achse, die sich nach Drehung um yaw und pitch ergeben hat. Diese drei Drehungen sind in Abb. 3.4 veranschaulicht. Die Richtungskosinusmatrix Cnb kann als Funktion der Eulerwinkel angegeben werden ⎛

⎞ cθ cψ −cφ sψ + sφ sθ cψ sφ sψ + cφ sθ cψ Cnb = ⎝cθ sψ cφ cψ + sφ sθ sψ −sφ cψ + cφ sθ sψ ⎠ , −sθ sφ cθ cφ cθ

(3.50)

hierbei bezeichnet cx = cos x, sx = sin x. Eine Herleitung dieses Zusammenhangs ist in [117] zu finden. Umgekehrt k¨ onnen die Eulerwinkel auch als Funktionen der Koeffizienten der Richtungskosinusmatrix dargestellt werden: φ = arctan2(c32 , c33 ) θ = arcsin(−c31 ) ψ = arctan2(c21 , c11 )

(3.51) (3.52) (3.53)

38

3 Inertiale Navigation

xb

2. θ

1.

yn

ψ

1. ψ

xn

φ y zb

3.

b

zn

Abbildung 3.4: Eulerwinkel.

b ¨ Die zeitliche Anderung der Eulerwinkel aufgrund von Drehraten ω  nb wird durch die Differentialgleichungen b b b φ˙ = (ωnb,y sin φ + ωnb,z cos φ) tan θ + ωnb,x b b cos φ − ωnb,z sin φ θ˙ = ωnb,y b b ψ˙ = (ω sin φ + ω cos φ)/ cos θ nb,y

nb,z

(3.54) (3.55) (3.56)

beschrieben, eine Herleitung ist ebenfalls in [117] zu finden. Eulerwinkel sind vor allem aufgrund ihrer Anschaulichkeit attraktiv. Nachteilig ist jedoch, dass bei ±90 Grad pitch Singularit¨ aten auftreten: In Gl. (3.56) erfolgt hier eine Division durch null. Auch die Berechnung von roll und yaw aus Komponenten der Richtungskosinusmatrix anhand von Gl. (3.51) und Gl. (3.53) scheitert. Diese Singularit¨aten sind wie folgt erkl¨ arbar: Der Yaw-Winkel beschreibt die Drehung um die lokale Vertikale. Wird anschließend um einen Pitch-Winkel von ±90 Grad gedreht, ist die durch den Roll-Winkel beschriebene Drehung ebenfalls eine Drehung um die lokale Vertikale, die Eulerwinkel sind in diesem Fall daher nicht eindeutig. Diese Mehrdeutigkeit ist die Ursache f¨ ur die Singularit¨ aten in den Eulerwinkel-Differentialgleichungen. Interessanterweise tritt dieses Problem unvermeidlich auch bei Plattform-Systemen auf und wird als gimbal-lock bezeichnet.

3.2.2

Orientierungsvektor und Quaternion

Die Lage zweier Koordinatensysteme kann anhand des Orientierungsvektors σ beschrieben werden. Der Orientierungsvektor legt dabei diejenige Achse im Raum fest, um die gedreht werden muss, um mit einer einzigen Drehung die beiden Koordinatensysteme ineinander zu u uhren. Die L¨ ange des Orientierungsvektors gibt dabei den Winkel an, ¨ berf¨

3.2 Lagedarstellungen

39

um den gedreht werden muss. Dabei gibt es prinzipiell zwei M¨oglichkeiten: Beschreibt der Orientierungsvektor T  σ1 = σ1,x σ1,y σ1,z

(3.57)

mit der L¨ ange σ1 = |σ | die Lage zweier Koordinatensysteme, so beschreibt der Orientierungsvektor σ2 = −

σ1 (2π − σ1 ) σ1

(3.58)

dieselbe Situation. Hintergrund hierbei ist, dass man sowohl durch eine Rechtsdrehung um den Winkel σ1 als auch durch eine Linksdrehung um den Winkel 2π − σ1 die Koordinatensysteme ineinander u uhren kann. ¨ berf¨ b ¨ Um die Anderung des Orientierungsvektors in Abh¨angigkeit von Drehraten ωnb zu erfassen, muss die Bortzsche Orientierungsvektordifferentialgleichung

1 1 b b ω nb + σ ×  + 2 σ˙ =  ω nb 2 σ

1−

σ sin(σ) 2(1 − cos σ)

b ) σ × (σ × ω nb

(3.59)

gel¨ ost werden. Diese wird in Abschnitt 3.3.1 bei der Entwicklung der StrapdownRechnung eine zentrale Rolle spielen. Die Herleitung dieser Gleichung ist vergleichsweise umfangreich, siehe hierzu [15]. Der Orientierungsvektor wird meist vorteilhaft als auf die L¨ange |q| = 1 normiertes Quaternion gespeichert: ⎛ ⎞ ⎛ ⎞ a cos(σ/2) ⎜ b ⎟ ⎜(σ /σ) sin(σ/2)⎟ qnb = ⎝ ⎠ = ⎝ x (σy /σ) sin(σ/2)⎠ c (σz /σ) sin(σ/2) d

(3.60)

Bildet man die Quaternionen zu den Orientierungsvektoren Gl. (3.57) und (3.58), so findet man q2 = −q1 .

(3.61)

Die Lage zweier Koordinatensysteme wird also sowohl durch q als auch durch −q be¨ schrieben, im einen Fall entspricht dies einer Uberf¨ uhrung des einen Koordinatensystems in das andere u ¨ ber eine Rechtsdrehung, im anderen Fall u ¨ ber eine Linksdrehung. Wie bei Richtungskosinusmatrizen ist auch mit Quaternionen eine Verkettung von Drehungen m¨ oglich: qnb = qne • qeb

(3.62)

40

3 Inertiale Navigation

Die daf¨ ur ben¨ otigte Quaternionenmultiplikation l¨asst sich f¨ ur zwei Quaternionen q1 (a, b, c, d), q2 (e, f, g, h) als Matrix-Vektor-Multiplikation darstellen: ⎛ a ⎜b q 1 • q 2 = ⎝  c d

−b a d −c

−c −d a b

⎞ ⎛ ⎞ e −d c ⎟ ⎜f ⎟ · −b ⎠ ⎝ g ⎠ h a

(3.63)

Die entsprechende inverse Transformation zum Quaternion qnb erh¨alt man durch Einsetzen des negierten Orientierungsvektors in Gl. (3.60): ⎛

⎞ a ⎜ −b ⎟ qbn = ⎝ ⎠ −c −d

(3.64)

Das Quaternion kann auch direkt zur Transformation eines Vektors in ein anderes Koordinatensystem genutzt werden:



0 0 n (3.65) = qb • • qbn x n x b Alternativ hierzu kann eine solche Transformation auch realisiert werden, indem zun¨ achst aus dem Quaternion eine Richtungskosinusmatrix berechnet wird und dann der Vektor mit Hilfe dieser Richtungskosinusmatrix umgerechnet wird. Der daf¨ ur ben¨otigte Zusammenhang zwischen Quaternion und Richtungskosinusmatrix ist gegeben durch  ⎞ ⎛ 2 2(bd + ac) a + b2 − c2 − d2  2(bc − ad)  Cnb = ⎝ 2(bc + ad) a2 − b2 + c2 − d2  2(cd − ab) ⎠ . 2(bd − ac) 2(cd + ab) a2 − b2 − c2 + d2 (3.66) Umgekehrt ist bei der Berechnung eines Quaternions aus einer Richtungskosinusmatrix zwischen vier verschiedenen M¨ oglichkeiten zu w¨ahlen: 1. Fall. ζ = a= b= c= d=

1√ 1 + c11 + c22 + c33 2 ζ 1 (c32 − c23 ) 4ζ 1 (c13 − c31 ) 4ζ 1 (c21 − c12 ) 4ζ

(3.67) (3.68) (3.69) (3.70) (3.71)

3.2 Lagedarstellungen

41

2. Fall. ζ = a= b= c= d=

1√ 1 + c11 − c22 − c33 2 1 (c32 − c23 ) 4ζ ζ 1 (c21 + c12 ) 4ζ 1 (c13 + c31 ) 4ζ

(3.72) (3.73) (3.74) (3.75) (3.76)

3. Fall. ζ = a= b= c= d=

1√ 1 − c11 + c22 − c33 2 1 (c13 − c31 ) 4ζ 1 (c21 + c12 ) 4ζ ζ 1 (c32 + c23 ) 4ζ

(3.77) (3.78) (3.79) (3.80) (3.81)

4. Fall. ζ = a= b= c= d=

1√ 1 − c11 − c22 + c33 2 1 (c21 − c12 ) 4ζ 1 (c13 + c31 ) 4ζ 1 (c32 + c23 ) 4ζ ζ

(3.82) (3.83) (3.84) (3.85) (3.86)

Prinzipiell kann jede dieser vier Varianten gew¨ahlt werden, solange darauf geachtet wird, dass keine Division durch null erfolgt und ζ reell ist. So erh¨alt man beispielsweise f¨ ur die Eulerwinkel φ = 180◦ , θ = 0◦ , ψ = 90◦ die Richtungskosinusmatrix ⎛ ⎞ 0 1 0 Cnb (180◦ , 0◦ , 90◦ ) = ⎝1 0 0 ⎠ . (3.87) 0 0 −1

42

3 Inertiale Navigation

Hierbei w¨ urde die Berechnung des Quaternions nach 1. und nach 4. aufgrund einer Division durch null scheitern. In [111] wird vorgeschlagen, diejenige Variante zu w¨ahlen, bei der der Koeffizient ζ am gr¨ oßten ist. Bei der Berechnung des Quaternions aus Eulerwinkeln sind solche Fallunterscheidungen nicht notwendig, der Zusammenhang ist gegeben durch θ ψ φ θ ψ φ cos cos + sin sin sin 2 2 2 2 2 2 θ ψ φ θ ψ φ b = sin cos cos − cos sin sin 2 2 2 2 2 2 θ ψ φ θ ψ φ c = cos sin cos + sin cos sin 2 2 2 2 2 2 θ ψ φ θ ψ φ d = cos cos sin − sin sin cos . 2 2 2 2 2 2

a = cos

(3.88) (3.89) (3.90) (3.91)

Umgekehrt lassen sich Eulerwinkel aus einem Quaternion am einfachsten berechnen, indem zun¨ achst mit Gl. (3.66) die zum Quaternion geh¨orende Richtungskosinusmatrix gebildet wird und anschließend die Gl. (3.51) – (3.53) ausgewertet werden. b ¨ wird durch eine DifferentialDie Anderung des Quaternions aufgrund von Drehraten ωnb gleichung beschrieben, die im Folgenden hergeleitet werden soll. Zum Zeitpunkt t = 0 sei die Lage der Koordinatensysteme durch das Quaternion qnb (0) gegeben. Vom Zeitpunkt t = 0 bis zum Zeitpunkt t = τ erfolgt eine Rotation, die durch den Orientierungsvektor b σ =  ωnb τ beschrieben wird. Man kann daher schreiben:

 qnb (τ )

=

qnb (0)

•

b | ωnb |τ 2 b b | ωnb |τ ω  nb b | sin 2 | ωnb

cos



Die zeitliche Ableitung von Gl. (3.92) ergibt sich zu   | ωb | | ω b |τ − 2nb sin nb n n 2 q˙ b (τ ) = qb (0) • . b b | ωnb |τ ωnb  2 cos 2

(3.92)

(3.93)

Um zur gesuchten Differentialgleichung zu gelangen, muss der Grenzwert τ → 0 gebildet werden, man erh¨ alt q˙ nb (0) = qnb (0) •

0 b ωnb 

.

(3.94)

2

Diese Differentialgleichung kann in die u ¨ bliche Darstellung der Quaternionendifferentialgleichung umgeschrieben werden: q˙ nb



1 n 0 = qb • b  nb ω 2

(3.95)

3.2 Lagedarstellungen

43

H¨ aufig wird das Quaternion auch mit Hilfe von imagin¨aren Einheiten i, j, k dargestellt, f¨ ur die die folgenden Multiplikationsregeln gelten: i·i i·j j·k k·i

= = = =

j · j = k · k = −1 −j · i = k −k · j = i −i · k = j

(3.96) (3.97) (3.98) (3.99)

Mit der Quaternionendarstellung q = a + ib + jc + kd

(3.100)

und obigen Multiplikationsregeln k¨ onnen dann ebenfalls Quaternionenmultiplikationen und Vektortransformationen durchgef¨ uhrt werden. Dieser Formalismus wird jedoch im Weiteren keine Rolle spielen. Abschließend soll noch eine wichtige Eigenschaft des Orientierungsvektors hergeleitet werden. Anhand von Gl. (3.66) l¨ asst sich zeigen, dass die Richtungskosinusmatrix einen Eigenvektor ⎛ ⎞ b (3.101) zCnb = ⎝ c ⎠ d besitzt. Durch Vergleich mit Gl. (3.60) erkennt man, dass dieser Eigenvektor und der Orientierungsvektor parallel sind. Da der zum Eigenvektor geh¨orende Eigenwert 1 ist, kann man schreiben: Cnb σ = 1 · σ

(3.102)

Es stellt sich also nicht die Frage, in Koordinaten welchen Koordinatensystems der Orientierungsvektor angegeben wird, der Orientierungsvektor besitzt in beiden beteiligten Koordinatensystemen die gleichen Komponenten.

3.2.3

Richtungskosinusmatrix

Die Transformation eines Vektors in ein anderes Koordinatensystem mit Hilfe der Richtungskosinusmatrix wurde in den vorherigen Abschnitten bereits eingef¨ uhrt. Von besonderer Bedeutung ist die N¨ aherung der Richtungskosinusmatrix f¨ ur kleine Drehungen; diese spielt z.B. bei der Sch¨ atzung von Lagefehlern mit einem Kalman-Filter eine zentrale Rolle. Im Folgenden sollen kleine Drehungen betrachtet werden, so dass f¨ ur die Drehwinkel die N¨ aherungen sin δ ≈ δ cos δ ≈ 1 δ·δ ≈ 0 gerechtfertigt sind.

(3.103) (3.104) (3.105)

44

3 Inertiale Navigation

F¨ ur das Quaternion erh¨ alt man unter diesen Voraussetzungen aus Gl. (3.60) ⎞ ⎛ ⎞ cos(σ/2) 1 ⎜(σ /σ) sin(σ/2)⎟ ⎜σx /2⎟ qnb = ⎝ x ≈ . (σy /σ) sin(σ/2)⎠ ⎝σy /2⎠ (σz /σ) sin(σ/2) σz /2 ⎛

(3.106)

Berechnet man hierzu die Richtungskosinusmatrix anhand von Gl. (3.66), erh¨alt man mit der N¨ aherung (3.105) ⎛

⎞ 1 −σz σy 1 −σx ⎠ = I + [σ ×] . Cnb ≈ ⎝ σz −σy σx 1

(3.107)

F¨ ur kleine Eulerwinkel φ, θ, ψ erh¨ alt man anhand von Gl. (3.50) in analoger Weise ⎛

⎞ ⎡⎛ ⎞ ⎤ 1 −ψ θ φ Cnb ≈ ⎝ ψ 1 −φ⎠ = I + ⎣⎝ θ ⎠ ×⎦ . −θ φ 1 ψ

(3.108)

In dieser N¨ aherung f¨ ur kleine Winkel besteht offensichtlich kein Unterschied mehr zwischen den Komponenten des Orientierungsvektors und den Eulerwinkeln. Auch f¨ ur die Richtungskosinusmatrix l¨ asst sich eine Differentialgleichung angeben. Hierzu wird wieder davon ausgegangen, dass vom Zeitpunkt t = 0 bis zum Zeitpunkt t = τ b τ beschreiben eine Rotation stattfindet, die sich durch den Orientierungsvektor σ = ωnb l¨ asst. Damit erh¨ alt man den Zusammenhang

Cnb (τ ) = Cnb (0) (I + [σ ×]) .

(3.109)

Durch zeitliche Ableitung und Bilden des Grenzwertes τ → 0 erh¨alt man die gesuchte Richtungskosinusmatrixdifferentialgleichung b ˙ nb = Cnb [ C ωnb ×] .

(3.110)

Alternative Herleitungen u ¨ ber den Differenzenquotienten der Richtungskosinusmatrix sind in [117] und [57] zu finden.

3.3 Strapdown-Rechnung

45

φ0 , θ0 , ψ0 Gyro

ωibb 



n ωen  , ω  ien

Erddrehrate Transportrate

Cnb Acc

aibb

n aeb

v0 

vebn

Gravitation Fliehkraft

x0 

Position

Geschwindigkeit

CoriolisKorrektur Lage

Abbildung 3.5: Strapdown-Algorithmus.

3.3

Strapdown-Rechnung

Unter einem Strapdown-Algorithmus versteht man eine Rechenvorschrift, die angibt, wie anhand von gemessenen Beschleunigungen und Drehraten aus der Navigationsl¨osung zum vorherigen Zeitschritt die Navigationsl¨osung zum aktuellen Zeitschritt berechnet wird. Die Strapdown-Rechnung l¨ asst sich grob in drei Schritte einteilen: Propagation der Lage durch Integration der Drehraten, Propagation der Geschwindigkeit durch Integration der Beschleunigungen und Propagation der Position durch Integration der Geschwindigkeit. Ein Blockdiagramm eines Strapdown-Algorithmus ist in Abb. 3.5 dargestellt. H¨ aufig liefern IMUs anstelle von Beschleunigungen und Drehraten Geschwindigkeitsund Winkelinkremente. Da in der hier dargestellten Strapdown-Rechnung Beschleunigungen und Drehraten immer als Produkt mit der Zeitschrittweite auftreten, spielt das f¨ ur die folgenden Betrachtungen keine Rolle.

3.3.1

Lage

¨ Die Anderung der Lage kann durch L¨ osung der Bortzschen Orientierungsvektordifferentialgleichung 3.59 ermittelt werden6 . Da eine IMU immer nur zu diskreten Zeitpunkten 6 Die Quaternionen-Differentialgleichung Gl. (3.95), bei der es sich um eine exakte, d.h. n¨ aherungsfreie Gleichung handelt, eignet sich zur Propagation der Lage ebenfalls hervorragend.

46

3 Inertiale Navigation

Messwerte liefern kann und eine Lageberechnung immer zeitdiskret in einem Navigationscomputer implementiert werden muss, ist nur eine numerische, n¨aherungsweise L¨ osung dieser Differentialgleichung m¨ oglich. Um hierbei eine Auswertung trigonometrischer Funktionen zu vermeiden, kann der trigonometrische Funktionen enthaltende Term der Orientierungsvektordifferentialgleichung f¨ ur kleine Drehungen mit Hilfe einer Reihenentwicklung als konstanter Faktor angen¨ahert werden: 1 σ2

1−

σ sin(σ) 2(1 − cos σ)

=

1 σ2

≈

1 σ2

1 σ2 1 = 12 =

1−

1 3 σ + ...) σ(σ − 3! 1 2 1 4 2(1 − (1 − 2! σ + 4! σ − ...)) 1 2 1 4 1 4

2 2( 2! σ − 4! σ ) − (σ − 3! σ ) 1 2 1 4 2( 2! σ − 4! σ ) 1 4 1 4

1 4 1 4

− 12 σ + 6 σ σ + 6σ 1 − 12 ≈ 2 1 4 σ σ2 σ 2 − 12 σ (3.111)

Man erh¨ alt die vereinfachte Differentialgleichung   1 1 b b b σ˙ ≈ ω  nb ω nb . + σ ×  + σ × σ × ω nb 2 12

(3.112)

Es l¨ asst sich leicht u ¨ berlegen, dass der zweite Kreuzprodukt-Term einen kleineren Beitrag liefert als der erste, so dass dieser zweite Term h¨aufig komplett vernachl¨assigt wird: 1 b b ω nb σ˙ ≈ ω  nb + σ ×  2

(3.113)

Voraussetzung f¨ ur diese Vereinfachungen ist, dass die Lageberechnung mit einer ausreichend hohen Update-Rate ausgef¨ uhrt wird. Dadurch wird sichergestellt, dass die in einem Zeitschritt stattfindenden Drehungen klein sind. Das ist sowieso notwendig, da Drehungen nicht kommutativ sind: Werden Drehungen verkettet, so h¨angt die finale Orientierung nicht nur von den einzelnen Drehungen, sondern auch von der Reihenfolge der Drehungen ab. Eine IMU kann aber niemals Informationen u ¨ ber die Reihenfolge der in einem Zeitintervall aufgetretenen Drehungen liefern, die zeitliche Abfolge der in dem Zeitintervall aufgetretenen Drehbewegungen kann daher prinzipiell nicht ber¨ ucksichtigt werden. Je gr¨ oßer die Update-Rate der Lage-Berechnung ist, desto kleiner ist der aus der Nichtkommutativit¨ at von Drehungen resultierende Fehler. Die Lage¨ anderung im Zeitintervall von tk−1 bis tk ist anhand von Gl. 3.113 n¨aherungsweise gegeben durch
t k ∆σ k ≈

b ω nb  dt + t k−1

1 2


t k t k−1

⎛ ⎜ ⎝


t

t k−1

⎞ ⎟ b b ω  nb dτ ⎠ × ω nb dt .

(3.114)

3.3 Strapdown-Rechnung

47

Der Kreuzprodukt-Term wird als Coning-Term bezeichnet. Wenn sich die Richtung des b im betrachteten Zeitintervall nicht ¨andert, verschwindet dieser Term und Vektors  ω nb man erh¨ alt b ∆σ k ≈  ω nb,k ·T .

(3.115)

¨ Ist die Anderung des Orientierungsvektors berechnet, muss das Lagequaternion entsprechend angepasst werden. Hierzu berechnet man ein Quaternion rk , das die im betrachteten Zeitintervall erfolgte Drehbewegung repr¨asentiert: ⎛ ⎞ k cos ∆σ 2 ⎠ , ∆σk = |∆σk | (3.116) rk = ⎝ ∆σ k sin ∆σk 2 ∆σk Bei der Implementierung im Navigationscomputer muss darauf geachtet werden, dass ur ∆σk = 0 keine Division durch null erfolgt. Dies beim Aufbau des Quaternions rk f¨ kann zum Beispiel erreicht werden, indem die Reihenentwicklungen der trigonometrischen Funktionen verwendet werden: rk =

1−

∆σk



1 1 1 2 4 6 2!·22 ∆σk + 4!·24 ∆σk − 6!·26 ∆σk + . . .  1 1 1 1 2 4 6 2 − 3!·23 ∆σk + 5!·25 ∆σk − 7!·27 ∆σk + . . .

(3.117)

Das aktuelle Quaternion erh¨ alt man schließlich durch qnb,k = qnb,k−1 rk .

(3.118)

b In der gew¨ ahlten Formulierung der Lageberechnung wird die Drehrate ωnb ben¨otigt. Diese kann aus der von der IMU gelieferten Drehrate ωibb wie folgt berechnet werden: b n ω  nb = ωibb − Cn,T ( ωien +  ωen ) b

(3.119)

Die daf¨ ur ben¨ otigte Drehrate  ωien ist die Erddrehrate in Koordinaten des Navigationskoordinatensystems. Sie h¨ angt nur vom Breitengrad ϕ ab und ist gegeben durch ⎛

n ω  ie

⎞ Ω cos ϕ ⎠ , 0 =⎝ −Ω sin ϕ

(3.120)

n wird als Transportrate bezeichnet. Diese resultiert siehe Abb. 3.6. Die Drehrate  ωen daraus, dass sich bei einer Bewegung gegen¨ uber der Erde in Nord- oder Ostrichtung aufgrund der Erdkr¨ ummung das Navigationskoordinatensystem drehen muss, damit die Unten-Koordinatenachse immer in Richtung der lokalen Vertikalen weist. Folglich h¨ angt die Transportrate von der Geschwindigkeit des Fahrzeugs ab. Bezeichnet man

48

3 Inertiale Navigation

z

Ω ez

en Re cos ϕ

ee

ϕ ed Re

ϕ

Abbildung 3.6: Geometrische Zusammenh¨ ange zur Bestimmung von Erddrehrate und Transportrate.

mit ωvn und ωve die Drehraten aufgrund von Geschwindigkeitskomponenten in Nordund Ostrichtung, findet man mit Abb. 3.6 die Transportrate zu ⎛ n = ⎝ ω en

⎞

⎛

ve (Re −h) cos ϕ

cos ϕ

⎞

ωve cos ϕ ⎠=⎝ ⎠= ωvn ωvn ve −ωve sin ϕ − (Re −h) sin ϕ cos ϕ

⎛

n v eb,e Re −h ⎜ n ⎜ − v eb,n ⎝ Rn −h v n tan ϕ − eb,e Re −h

⎞ ⎟ ⎟ . ⎠ (3.121)

Alternativ zur hier beschriebenen Formulierung k¨onnen Algorithmen zur Lageberechnung auch mit der Drehrate  ωibb formuliert werden, der Einfluss von Erddrehrate und Transportrate wird dann an anderer Stelle, meist mit einer deutlich geringeren UpdateRate, ber¨ ucksichtigt. Abschließend soll noch der Fall betrachtet werden, dass sich in Gl. (3.114) die Richtung b des Vektors  ω nb im betrachteten Zeitintervall ¨andert und der Coning-Term einen signi¨ fikanten Beitrag zur Anderung des Orientierungsvektors liefert. Hier ist ein besonderes Bewegungsmuster hervorzuheben, Coning Motion, das z.B. aufgrund von Vibrationen vorliegen kann. Als Coning Motion werden Drehbewegungen bezeichnet, bei denen phasenverschobene, oszillatorische Drehbewegungen um zwei orthogonale Achsen vorliegen. Prinzipiell kann einem beschleunigten Anwachsen von Lagefehlern mit der Zeit bei Co-

3.3 Strapdown-Rechnung

49

tk−1

Lage-Update Intervalle

tk T

t

δt

IMU-Daten Intervalle

Abbildung 3.7: IMU-Daten und Lage-Update Intervalle.

ning Motion aufgrund der Nichtkommutativit¨at von Drehungen nur durch eine hohe Datenrate der IMU begegnet werden. In der Praxis kann der Fall auftreten, dass die IMU zwar eine ausreichend hohe Datenrate aufweist, die Rechenlast durch eine vollst¨andige Neuberechnung der Lage mit dieser hohen Datenrate jedoch vermieden werden muss. Dies kann mit Coning-Kompensationsalgorithmen erreicht werden, deren Grundidee im Folgenden skizziert werden soll.

Coning-Kompensation Ziel eines Coning-Kompensationsalgorithmus ist, den Coning-Term mit den zwischen zwei Lage-Updates zur Verf¨ ugung stehenden Messdaten bestm¨oglich zu approximieren. Lage-Updates sollen nach wie vor im Zeitabstand T = tk − tk−1 erfolgen, IMU-Daten stehen im Zeitabstand δt zur Verf¨ ugung, siehe Abb. 3.7. Zur Herleitung eines Coning-Kompensationsalgorithmus wird Coning Motion gem¨aß ⎛

⎞ 0 b (t) = ⎝ 2πf β cos(2πf t) ⎠ ω  nb 2πf β sin(2πf t)

(3.122)

b (t) in diesem Fall analytisch gegeben ist, kann der betrachtet. Da die Drehrate ω  nb Coning-Term

∆σc =

1 2


tk tk−1

⎛ ⎜ ⎝


t

tk−1

⎞ ⎟ b b  nb ω dt⎠ ×  dt ωnb

(3.123)

50

3 Inertiale Navigation

exakt berechnet werden. Man erh¨ alt ⎛ ⎞
tk 0 1 b ⎝ β(sin(2πf t) − sin(2πf tk−1 )) ⎠ × ωnb ∆σc = (t) dt 2 −β(cos(2πf t) − cos(2πf t )) tk−1

k−1

⎛

⎞ 2πf β 2 (1 − cos(2πf (t − tk−1 )) ⎠ dt ⎝ 0 0 tk−1  1 2 sin (2πf T ) ex . ∆σc = πf β T − 2πf 1 = 2


tk

(3.124)

Dieser Coning-Term soll mit Hilfe der IMU-Daten approximiert werden. Innerhalb ei¨ nes Lage-Update-Intervalls sollen N IMU-Daten-Intervalle vorliegen, die Anderung des Orientierungsvektors innerhalb des IMU-Daten Intervalls i ist mit δσi bezeichnet. Eine Approximation des Coning-Terms aus Gl. (3.123) l¨asst sich dann allgemein in der Form ˆc = ∆σ

N −1 

N 

kij δσi × δσj

(3.125)

i=1 j=i+1

darstellen. Diese Approximation kann vereinfacht werden. Nach durchaus umfangreicher Rechnung, siehe hierzu auch [54], findet man δσi × δσj = β 2 [2 sin ((j − i) λ) − sin ((j − i + 1) λ) − sin((j − i − 1)λ)] ex . (3.126) Hierbei wurde λ = 2πf δt gesetzt. Da in diesem Kreuzprodukt nur Terme mit der Differenz j − i auftreten, ist nur die zeitliche Differenz von δσi und δσj relevant, nicht jedoch der absolute Zeitpunkt innerhalb des Lage-Update-Intervalls. Damit vereinfacht sich Gl. (3.125) zu N −1   ˆ ki δσi × δσN . (3.127) ∆σc = i=1

Die Koeffizienten ki werden bestimmt, indem man die Reihenentwicklung des analytisch berechneten Coning-Terms (3.124) mit der Reihenentwicklung der Approximation (3.127) vergleicht. Beispielhaft soll der Fall betrachtet werden, dass innerhalb eines Lage-Update-Intervalls drei IMU-Daten-Intervalle vorliegen: ˆc = [k1 δσ1 + k2 δσ2 ] × δσ3 ∆σ

(3.128)

Mit Gl. (3.126) erh¨ alt man so ˆc = k1 β 2 [2 sin(2λ) − sin(λ) − sin(3λ)] ex + k2 β 2 [2 sin(λ) − sin(2λ)] ex . ∆σ (3.129)

3.3 Strapdown-Rechnung

51

Ersetzt man die Sinus-Funktionen durch ihre Reihenentwicklung, sin x = x −

x3 x5 x7 x2n+1 + − + · · · + (−1)n , 3! 5! 7! (2n + 1)!

(3.130)

so ergibt sich nach Zusammenfassen:

 ˆc = β 2 k1 2λ3 − 3 λ5 + 161 λ7 − · · · ∆σ 2 420

1 63 7 +k2 λ3 − λ5 + λ − ··· ex 4 2520

(3.131)

Ebenfalls durch Einsetzen der Reihententwicklung der Sinus-Funktion in den exakten Coning-Term (3.124) erh¨ alt man mit 2πf T = 3λ  1 (2πf T )3 ∆σc = πf β 2 T − 2πf T − 2πf 3!

(2πf T )5 (2πf T )7 − + . . . ex + 5! 7!  (3λ)5 (3λ)7 β 2 (3λ)3 − + − ... . = 2 3! 5! 7!

(3.132)

Vergleicht man nun in (3.131) und (3.132) die Terme der dritten und f¨ unften Potenzen von λ, so ergeben sich zwei Gleichungen zur Bestimmung der Koeffizienten k1 und k2 , (3λ)3 2 · 3! 3 5 1 5 ! (3λ)5 −k1 λ − k2 λ = , 2 4 2 · 5! !

2k1 λ3 + k2 λ3 =

(3.133) (3.134)

man erh¨ alt schließlich k1 =

9 , 20

k2 =

27 . 20

(3.135)

Der Coning-Term l¨ asst sich also f¨ ur den Fall, dass drei IMU-Daten-Intervalle pro LageUpdate-Intervall vorliegen, anhand von  ˆc = 9 δσ1 + 27 δσ2 × δσ3 ∆σ (3.136) 20 20 approximieren, wobei δσi das Orientierungsvektorinkrement des i-ten IMU-DatenIntervalls bezeichnet. Der Nutzen eines solchen Coning-Kompensationsalgorithmus wird in Abb. 3.8 verdeutlicht. In diesem Beispiel wurde eine IMU-Datenrate von 1000Hz angenommen, es lag

52

3 Inertiale Navigation 0.06 ♦ 

Abs. Lagefehler (rad)

0.05

Ohne Coning-Kompensation Mit Coning-Kompensation

♦ ♦ ♦

0.04 ♦ ♦

0.03 ♦ 0.02

♦ ♦

0.01 ♦ 0 0

♦ 





200





400

 600



 800

 1000

Zeit (s) Abbildung 3.8: Coning-Kompensation.

Coning Motion mit einer Frequenz von f = 50Hz und β = 0.1◦ vor. Abb. 3.8 zeigt nun die Lagefehler, die sich ergeben, wenn f¨ ur alle drei IMU-Daten-Intervalle eine Neuberechnung der Lage erfolgt. Im einen Fall wurde hierbei der Coning-Term vernachl¨assigt, im anderen Fall anhand von Gl. (3.136) approximiert. Als Referenz wurde die Lage verwendet, die sich bei einer Neuberechnung der Lage mit der vollen IMU-Datenrate ergibt, als Lagefehler wurde der rms-Wert der Eulerwinkel-Abweichungen angesetzt. Man erkennt, dass auch bei Verwendung des Coning-Kompensationsalgorithmus ein Anwachsen der Lagefehler mit der Zeit stattfindet, die Lagefehler wachsen jedoch ohne Verwendung eines solchen Algorithmus deutlich schneller an.

3.3.2

Geschwindigkeit

Die Formulierung der Strapdown-Rechnung in Koordinaten des Navigationskoordinatensystems hat Vorteile: Die Koordinatenrichtungen Norden, Osten und Unten sind intuitiv leicht zu fassen, außerdem resultieren vergleichsweise einfache Schwerebeschleunigungsmodelle. Andererseits handelt es sich beim Navigationssystem um kein Inertialsystem, so dass bei der Integration von Beschleunigungen Coriolis-Beschleunigung und Schwerebeschleunigung ber¨ ucksichtigt werden m¨ ussen, um auf Geschwindigkeits¨anderungen des Fahrzeugs schließen zu k¨ onnen. Daher soll zun¨achst die Differentialgleichung hergeleitet werden, die den Zusammenhang zwischen Beschleunigung und Geschwindigkeit im Navigationskoordinatensystem beschreibt.

3.3 Strapdown-Rechnung

53

Im Inertialkoordinatensystem gilt i i v˙ ib = a ib + g i .

(3.137)

i i Hierbei bezeichnen v ib und a ib die Geschwindigkeit und Beschleunigung des Fahrzeugs bez¨ uglich des Inertialkoordinatensystems, g i ist die Beschleunigung aufgrund der Massenanziehung der Erde.

Mit r wird der Vektor vom Ursprung des Inertialkoordinatensystems zum Ursprung des Navigationskoordinatensystems bezeichnet. Diesen kann man vom Inertialkoordinatensystem ins erdfeste Koordinatensystem umrechnen: r e = C ier i

(3.138)

Durch Differentiation nach der Zeit erh¨ alt man ˙ er i + C er˙ i r˙ e = C i i  ˙r e = C e Ω i r i + r˙ i i ei  i i  e i v eb . = C ie Ω ei r + v ib

(3.139)

Hierbei wurde die Richtungskosinusmatrixdifferentialgleichung eingesetzt, siehe Gl. i (3.110), Ω ei ist die kreuzproduktbildende Matrix der negativen Erddrehrate. Multiplikation von links mit C en liefert  i i  n i v eb = C en C ie Ω ei r + v ib  i i  n i . = C in Ω ei r + v ib v eb

(3.140)

i Diesen Zusammenhang kann man nach v ib aufl¨osen, i n i i v ib = C ni v eb − Ω ei r ,

Differentiation nach der Zeit liefert  i n n n i i i ˙i ˙ ei −Ω v˙ ib = C ni Ω in v eb + v˙ eb r − Ω ei r  i n n n ˙ i r i + Ω i r˙ i . +Ω v˙ ib = C ni Ω in v eb + v˙ eb ie ie

(3.141)

(3.142)

Durch Vergleich von Gl. (3.137) mit Gl. (3.142) findet man  n n n ˙ i r i + Ω i r˙ i = a i + g i +Ω C ni Ω in v eb + v˙ eb ie ie ib

(3.143)

und schließlich durch Multiplikation mit C in n n n i i i ˙i n ˙ ie Ω in v eb + v˙ eb + C in Ω r + C in Ω ie + g n . r = a ib

(3.144)

54

3 Inertiale Navigation

Da C ni C in = I gilt, kann man dieses Matrizenprodukt einf¨ ugen, um die kreuzproduktbildenden Matrizen der Drehrate und ihrer Ableitung ins Navigationskoordinatensystem umzurechnen: n n n ˙ i C i C nr i + C n Ω i C i C nv i = a n + g n Ω in v eb + v˙ eb + C in Ω ie n i i ie n i ib ib n n n ˙ n C nr i + Ω n C nv i = a n + g n Ω in v eb + v˙ eb +Ω ie i ie i ib ib

(3.145)

Durch Einsetzen von (3.141) erh¨ alt man   n n n ˙ n r n + Ω n C n C i v n − Ω i r i = a n + g n . v eb + v˙ eb +Ω Ω in ie ie i n eb ei ib

(3.146)

n Aufl¨ osen nach v˙ eb liefert schließlich

  n n n n n n n n i i ˙ ie = a ib − Ω in v eb − Ω r − Ω ie C in C ni v eb − Ω ei r + g n v˙ eb n n n n ˙ n r n − Ω n v n + Ω n C n Ω i r i + g n . v˙ eb = a ib − Ω in v eb − Ω ie ie eb ie i ei

(3.147)

Dieser Ausdruck l¨ asst sich durch erneutes Einf¨ ugen von C ni C in und Zusammenfassen weiter vereinfachen: n n n n ˙ n r n − Ω n v n + Ω n C n Ω i C i C nr i + g n v˙ eb = a ib − Ω in v eb − Ω ie ie eb ie i ei n i n n n n ˙ n r n − Ω n v n + Ω n Ω n C nr i + g n v˙ eb = a ib − Ω in v eb − Ω ie ie eb ie ei i ˙ n r n − Ω n v n − Ω n Ω n r n + g n v˙ n = a n − Ω n v n − Ω eb

ib

in eb

ie

ie eb

ie

ie

n n n n n n n n n n n n ˙ ie = a ib − (Ω ie + Ω en ) v eb −Ω r − Ω ie v eb − Ω ie Ω ie r + g n v˙ eb n n n n n ˙ n r n − Ω n Ω n r n + g n = a ib − (2Ω ie + Ω en ) v eb −Ω (3.148) v˙ eb ie ie ie

˙ n r n handelt es sich um die Euler-Beschleunigung. Da die Erddrehrate als n¨aBei Ω ie herungsweise konstant angenommen werden kann, verschwindet dieser Term. Durch ¨ Ubergang zur Darstellung ohne kreuzproduktbildende Matrizen und Zusammenfassen von Beschleunigung durch Massenanziehung und Zentrifugalbeschleunigung ergibt sich n n n n n n n v˙ eb = a ib − (2 ω ie + ω en ) × v eb + g n − ω  ie × (ω ie × r n ) n n n n n = a ib − (2 ω ie + ω en ) × v eb + g ln . v˙ eb

(3.149)

Eine IMU misst die Beschleunigungen aibb , so dass die gesuchte Differentialgleichung durch n b n n n v˙ eb = Cnba ib − (2 ω ie +ω  en ) × v eb + g ln

(3.150)

gegeben ist. Bei der Kompensation der Schwerebeschleunigung f¨allt auf, dass diese addiert und nicht, wie man zun¨ achst vermuten k¨ onnte, durch Subtraktion aus der gemessenen Beschleunigung herausgerechnet wird. Wie man anhand von Abb. 3.9 erkennt, misst eine IMU

3.3 Strapdown-Rechnung

55

0

gl

0

a

Abbildung 3.9: Links: Beschleunigungs-Messer in Ruhe, im Schwerefeld der Erde. Rechts: Beschleunigungs-Messer nach oben beschleunigt, kein Schwerefeld.

die Schwerebeschleunigung als eine scheinbare Beschleunigung nach oben, also entgegen der Koordinatenrichtung Unten. Die gemessene Schwerebeschleunigung ist demnach (0, 0, −gln)T und wird daher durch Addition von (0, 0, gln )T kompensiert. Um die Geschwindigkeit des Fahrzeugs in der Zeit zu propagieren, wird die rechte Seite von Gl. (3.150) numerisch integriert. W¨ ahrend der Kreuzprodukt-Term und die Schwerebeschleunigung in der Regel als konstant im betrachteten Zeitintervall angenommen b n¨ aher betrachtet werden. Problematisch hierwerden k¨ onnen, muss der Term Cnba ib bei ist, dass zur exakten Integration die Richtungskosinusmatrix als Funktion der Zeit bekannt sein m¨ usste, diese aber von der Lageberechnung nur zu diskreten Zeitpunkten bestimmt werden kann. Die Herleitung einer geeigneten Approximation wird im Folgenden skizziert. Es l¨ asst sich durch Differentiation verifizieren, dass die L¨osung der Richtungskosinusmatrixdifferentialgleichung (3.110) durch Rt

C bn (t)

=

n C b,k−1

·e

tk−1

b Ω nb dτ

(3.151)

gegeben ist. Zun¨ achst werden die Gr¨ oßen σ (t) und δv gem¨aß

δσ (t) =

t tk−1

δv =

t tk−1

b b ω nb  dt ≈  ω nb (t − tk−1 )

(3.152)

b b a ib dt ≈ a ib (t − tk−1 )

(3.153)

56

3 Inertiale Navigation

eingef¨ uhrt, dies f¨ uhrt sofort auf die Zusammenh¨ange δσ (tk ) = ∆σk b δσ˙ (t) = ω  nb δv˙ = a b .

(3.154) (3.155) (3.156)

ib

Mit Gl. (3.151) erh¨ alt man so:
tk

Rt


tk C bn (t)

b · a ib dt

=

n C b,k−1

·

tk−1

e

tk−1

b Ω nb dτ

b · a ib dt

tk−1

≈

n C b,k−1

·


tk 

b b I + Ω nb (t − tk−1 ) · a ib dt

tk−1

=

n C b,k−1


tk  b b a ib dt · + δσ (t) × a ib

(3.157)

tk−1

Um den Integranden Gl. (3.153) geeignet zerlegen zu k¨onnen, sind einige Zwischen¨ uberlegungen notwendig. Es gilt d (δσ × δv ) = δσ˙ × δv + δσ × δv˙ = −δv × δσ˙ + δσ × δv˙ , dt

(3.158)

Umstellen liefert d δσ × δv˙ = (δσ × δv ) + δv × δσ˙ . dt

(3.159)

Des Weiteren kann man schreiben δσ × δv˙ =

1 1 δσ × δv˙ + δσ × δv˙ . 2 2

(3.160)

Setzt man nun Gl. (3.159) f¨ ur einen der beiden Terme auf der rechten Seite von Gl. (3.160) ein, erh¨ alt man

1 d 1 ˙ ˙ δσ × δv = (δσ × δv ) + δv × δσ + δσ × δv˙ 2 dt 2

1 1 d 1 b δσ × aibb = (δσ × δv ) + δv × ωnb + δσ × aibb . 2 dt 2 2

(3.161)

3.3 Strapdown-Rechnung

57

Durch Substitution von Gl. (3.161) in Gl. (3.157) erh¨alt man
tk C bn (t)

b · a ib dt

=

n C b,k−1

tk−1


tk  1 d b (δσ × δv ) · + a ib 2 dt tk−1

1 1 b + δv × ωnb + δσ × aibb 2 2 und schließlich

⎡


tk C bn (t)

b · a ib dt

=

n C b,k−1

tk−1

⎢ ⎣


tk b a ib

⎢ n = C b,k−1 ⎣

1 2

1 2


tk



⎤

 ⎥ b δσ × aibb − ωnb × δv dt⎦

tk−1


tk

tk−1

+

b a ib dt tk−1

+ ⎡

(3.162)


tk

1 dt + ∆σk × 2

tk−1

dt


tk tk−1

1 b a ib dt + ∆σk × 2 ⎛ ⎜ ⎝


tk b a ib dt tk−1


t


t b b ωnb dτ × aibb − ωnb ×

tk−1

⎞

⎤

⎟ ⎥ b a ib dτ ⎠ dt⎦ ,

tk−1

(3.163) siehe hierzu auch [107]. Der zweite Summand in der eckigen Klammer wird als Rotationsterm bezeichnet, der letzte Summand ist der Sculling-Term. Der Sculling-Term verschwindet, wenn im betrachteten Zeitintervall konstante Drehraten und Beschleunigungen vorliegen. Andererseits kann insbesondere bei Sculling Motion dieser Term einen signifikanten Beitrag liefern. Unter Sculling Motion versteht man das Vorliegen rotatorischer und translatorischer Oszillationen bez¨ uglich zweier orthogonaler Achsen. F¨ ur den Fall, das die IMU in der Lage ist, mit einer ausreichenden Datenrate Messwerte zur Verf¨ ugung zu stellen, die Berechnung einer neuen Fahrzeuggeschwindigkeit aber mit einer niedrigeren Taktrate erfolgen muss, sollten – analog zur Coning-Kompensation – Sculling-Kompensationsalgorithmen verwendet werden. Sculling-Kompensation Zur Herleitung eines Sculling-Kompensationsalgorithmus wird wieder angenommen, dass IMU-Daten im Zeitabstand δt zur Verf¨ ugung stehen, Geschwindigkeits-Updates finden im Zeitabstand T = tk − tk−1 statt. Das Bewegungsmuster Sculling Motion sei durch ⎛ ⎞ ⎛ ⎞ 0 2πf β cos(2πf t) b ⎠ 0  nb =⎝ (3.164) aibb = ⎝ A sin(2πf t) ⎠ ; ω 0 0

58

3 Inertiale Navigation

gegeben. Anhand von Gl. (3.157) erkennt man, dass der Sculling-Kompensationsalgorithmus den Term
tk b δσ (t) × a ib dt

∆vs+r =

(3.165)

tk−1

bestm¨ oglichst approximieren muss. Als analytische, exakte L¨ osung findet man durch Einsetzen von (3.164):
tk ∆vs+r = tk−1

⎛ ⎜ ⎝ ⎡

⎢ = Aβ ⎣


t

⎞ ⎟ b  nb ω dt⎠ × aibb dt

tk−1


tk


tk sin2 (2πf t)dt − sin(2πf tk−1 )

tk−1

⎤ ⎥ sin(2πf t)dt⎦ ez (3.166)

tk−1

Nach einiger Rechnung, siehe hierzu auch [55], erh¨alt man als Mittelwert dieses Ausdrucks ∆vs+r =

Aβ [2πf T − sin(2πf T )]ez . 4πf

(3.167)

F¨ ur die Approximation des Sculling- und Rotationsterms ersetzt man die Integrale der Rotations- und Sculling-Terme in Gl. (3.163) durch Summen und erh¨alt so ∆vˆs+r =

N N N   1 δσi × δvi + kij [δσi × δvj + δvi × δσj ] , 2 i=1 i=1 j=i+1

(3.168)

wobei der Faktor 12 den Koeffizienten kij zugeschlagen wurde. Erneut nach aufw¨andiger Rechnung erh¨ alt man mit λ = 2πf δt als Mittelwert der auftretenden Kreuzprodukte: δσi × δvi+j = δvi × δσi+j (3.169)

Aβ = [2 sin(jλ) − sin((j + 1)λ) − sin((j − 1)λ)]ez (3.170) 4πf Zusammen mit δσi × δvi = 0 l¨ asst sich Gl. (3.168) daher vereinfachen zu N −1 N −1     ∆vˆs+r = ki δσi × δvN + ki δvi × δσN . i=1

(3.171)

i=1

Die Koeffizienten ki werden genau wie bei der Herleitung des Coning-Kompensationsalgorithmus bestimmt, indem die Reihenentwicklung der exakten L¨osung Gl. (3.167) mit der Reihenentwicklung der Approximation Gl. (3.171) verglichen wird.

3.3 Strapdown-Rechnung

59

Auch hier soll exemplarisch der Fall betrachtet werden, dass innerhalb eines Geschwindigkeits-Update-Intervalls drei IMU-Daten-Intervalle vorliegen. Damit erh¨alt man aus Gl. (3.171)

 161λ7 Aβ 3λ5 + − ··· 2k1 2λ3 − 4πf 2 420

5 63λ7 λ 3 +2k2 λ − + − ··· , 4 2520

∆vˆs+r =

(3.172)

aus Gl. (3.167) folgt mit 2πf T = 3λ

∆v s+r

Aβ = 4πf



(3λ)3 (3λ)5 (3λ)7 − + − · · · ez . 6 120 5040

(3.173)

Durch Koeffizientenvergleich erh¨ alt man schließlich k1 =

9 , 20

k2 =

27 , 20

(3.174)

die identischen Koeffizienten wie bei dem Coning-Kompensationsalgorithmus. F¨ ur eine ausf¨ uhrliche Diskussion dieser Tatsache sei auf [55] verwiesen. Sculling- und Rotationsterm lassen also f¨ ur den Fall, dass drei IMU-Daten-Intervalle pro Geschwindigkeits-Update-Intervall vorliegen, anhand von ∆vˆs+r =

9 27 δσ1 + δσ2 20 20



× δvN +

9 27 δv1 + δv2 20 20

× δσN

(3.175)

approximieren. Hierbei bezeichnet δσi wiederum das Orientierungsvektorinkrement des i-ten IMU-Daten-Intervalls, δvi ist das Integral u ¨ ber aibb in diesem IMU-Daten-Intervall. Neben der beschriebenen Vorgehensweise existieren noch eine Vielzahl weiterer Ans¨atze zur Approximation des Integrals (3.165), siehe z.B. [107]. Abschließend soll der Nutzen des Sculling-Kompensationsalgorithmus Gl. (3.175) verdeutlicht werden. Hierzu wurde bei einer IMU-Datenrate von 1000Hz die Frequenz der Sculling Motion zu f = 50Hz angenommen, ferner wurde β = 0.1◦ und A = 9.81m/s2 gew¨ ahlt. Abb. 3.10 zeigt den rms-Wert des Geschwindigkeitsfehlers gegen¨ uber einer Neuberechnung der Geschwindigkeit mit jedem IMU-Daten-Intervall, einmal bei Verwendung des Kompensationsterms Gl. (3.175), einmal bei Ber¨ ucksichtigung des Rotationsterms, aber Vernachl¨ assigung des Sculling-Terms. Man erkennt, dass die Vernachl¨assigung des Sculling-Terms zu einem beschleunigten Anwachsen der Geschwindigkeitsfehler f¨ uhrt.

3 Inertiale Navigation

Abs. Geschwindigkeitsfehler (m/s)

60

♦ 

1

Ohne Sculling-Kompensation Mit Sculling-Kompensation

♦ ♦

0.8

♦ ♦

0.6

♦ ♦

0.4 ♦ ♦

0.2 0 ♦ 0

♦ 





200





400





600

 800

 1000

Zeit (s) Abbildung 3.10: Numerische L¨ osung.

3.3.3

Position

Zur Propagation der Position m¨ ussen die Differentialgleichungen n v eb,n Rn (ϕ) − h n v eb,e λ˙ = (Re (ϕ) − h) cos ϕ ˙h = v n

ϕ˙ =

eb,d

(3.176) (3.177) (3.178)

numerisch gel¨ ost werden. Die Kr¨ ummungsradien der Erde in Nord- und Ostrichtung, Rn ¨ und Re , variieren nur sehr langsam mit der Anderung des Breitengrades. Daher kann deren Berechnung mit einer deutlich niedrigeren Update-Rate erfolgen als die restliche Strapdown-Rechnung.

3.4

Fehlercharakteristik eines Inertialnavigationssystems

Inertiale Navigation ist ein kurzzeitgenaues Navigationsverfahren, d.h. ohne geeignete Gegenmaßnahmen wachsen die Navigationsfehler mit der Zeit an. Ursache daf¨ ur sind – neben numerischen Fehlern und den in den vorherigen Abschnitten angesprochenen

3.4 Fehlercharakteristik eines Inertialnavigationssystems

61

Problemen aufgrund der endlichen Abtastrate der Inertialsensoren – Initialisierungsfehler und Messfehler der Sensoren. Ein Strapdown-Algorithmus propagiert eine Navigationsl¨ osung, damit muss zu Beginn der Strapdown-Rechnung folglich eine initiale Navigationsl¨ osung bekannt sein. Die Fehler dieser initialen Navigationsl¨osung werden nat¨ urlich ebenfalls propagiert. Fehlerhaft gemessene Drehraten bzw. Winkelinkremente f¨ uhren direkt zu Lagefehlern, fehlerhaft gemessene Beschleunigungen bzw. Geschwindigkeitsinkremente f¨ uhren direkt zu Geschwindigkeitsfehlern, die ebenso propagiert werden. ¨ In den folgenden Abschnitten soll daher zun¨achst ein Uberblick u ¨ ber Inertialsensoren und deren Fehler vermittelt werden, bevor die Auswirkungen von Mess- und Initialisierungsfehlern auf ein Inertialnavigationssystem diskutiert werden.

3.4.1

Drehratensensoren

Drehratensensoren werden auch als Kreisel oder Gyroskope bezeichnet. Zu den g¨angigsten Drehratensensoren z¨ ahlen MEMS-Kreisel, Faserkreisel (FOG) und Ringlaserkreisel (RLG). Daneben existieren noch eine Vielzahl weiterer Ans¨atze zur Messung von Drehraten, z.B. der mechanische Wendekreisel oder das Dynamically Tuned Gyroscope, die hier nicht betrachtet werden sollen. Eine umfassende Beschreibung einer Vielzahl von Drehratensensoren ist in [117] zu finden. MEMS-Kreisel MEMS-Kreisel7 z¨ ahlen zu den kosteng¨ unstigsten Drehratensensoren. Daf¨ ur weisen sie in der Regel auch eine geringere G¨ ute auf als z.B. Faserkreisel. Der Begriff MEMSKreisel ist als Oberbegriff zu verstehen, es gibt verschiedene Ans¨atze und Realisierungen. Exemplarisch soll hier lediglich ein Sensortyp beschrieben werden, der die Coriolis-Kraft ausnutzt. Eine Prinzipskizze dieses Sensors ist in Abb. 3.11 zu sehen. Durch elektrostatische Anregung werden zwei flache Probemassen zu gegenphasigen Schwingungen in x-Richtung gezwungen. Liegt nun eine Drehrate ωey vor, so resultiert aufgrund der Coriolis-Kraft eine Beschleunigung ac = 2ωey × va (t)ex = −2va (t)ωez ,

(3.179)

die ein Schwingen der Massen in z-Richtung verursacht. Die damit verbundene Auslenkung kann kapazitiv gemessen werden und ist ein Maß f¨ ur die Drehrate ω. Detailliertere Betrachtungen zu MEMS-Kreiseln sind z.B. in [72],[9] zu finden. Faserkreisel Faserkreisel basieren auf dem Sagnac-Effekt, der anhand von Abb. 3.12 beschrieben werden soll. Eine Glasfaser sei zu einem Ring gebogen, an der Verbindungsstelle soll es die M¨ oglichkeit geben, Laserstrahlen sowohl aus- als auch einzukoppeln. Zum Zeitpunkt t = 0 beginnt ein Laserstrahl im Uhrzeigersinn, ein anderer im Gegenuhrzeigersinn die Glasfaser zu durchlaufen. W¨ ahrend des Durchlaufs wird die Anordnung mit der Drehrate 7 MEMS

= Micro Electro-Mechanical System

62

3 Inertiale Navigation

ωey

erzwungene Schwingung va (t)ex

flexible Halterung

y

x durch Koriolis-Kraft angeregte Schwingung vc (t)ez

z

Abbildung 3.11: Koordinatensysteme f¨ ur die erdnahe Navigation.

Ω gedreht, so dass sich zu einem sp¨ ateren Zeitpunkt t = τ der Auskopplungspunkt weitergedreht hat. Die Laserstrahlen L1 und L2 m¨ ussen daher die Strecken L2 = 2πr − rΩt2

L1 = 2πr + rΩt1 ,

(3.180)

zur¨ ucklegen, wof¨ ur sie die Zeiten t1 =

L1 , u1

t2 =

L2 u2

(3.181)

ben¨ otigen. Hierbei bezeichnen u1 und u2 die Ausbreitungsgeschwindigkeiten der Laserstrahlen im und entgegen dem Uhrzeigersinn. Diese k¨onnen mit dem relativistischen Geschwindigkeitsadditionsgesetz va+b =

va + vb 1 + vac2vb

(3.182)

berechnet werden. Gegen¨ uber einem nichtrotierenden Bezugssystem hat jeder Punkt der Glasfaser die Geschwindigkeit va = rω, die Ausbreitungsgeschwindigkeit in einer ruhenden Glasfaser ist vb = nc , wobei n die Brechzahl der Glasfaser ist. Damit erh¨alt man u1 =

c n

+ rΩ

1+

c n ·rΩ c2

,

u2 =

c n

− rΩ

1−

c n ·rΩ c2

.

(3.183)

3.4 Fehlercharakteristik eines Inertialnavigationssystems

Ω

63

Ω

r L2

L1 t=τ

t=0 Abbildung 3.12: Sagnac-Effekt.

Mit den Wellenzahlen k1 = beiden Laserstrahlen:

ω u1

und k2 =

ω u2

erh¨alt man f¨ ur die Phasenverschiebung der

∆φ = k1 L1 − k2 L2 = ... ≈ 4πr2 ωΩ

1 4πr2 ωΩ ≈ c2 − r 2 Ω2 c2

Mit der Spulenl¨ ange Ls = 2πr, der Wellenl¨ ange λ = Ω=

λc ∆φ . 4πLs r

c f

(3.184)

und ω = 2πf folgt (3.185)

Die Drehrate Ω verursacht also eine Phasenverschiebung zwischen den gegensinnig umlaufenden Laserstrahlen, die von der Brechzahl n der Glasfaser unabh¨angig ist. Dieser Sachverhalt wird als Sagnac-Effekt bezeichnet. Der Sagnac-Effekt ist ein relativistischer Effekt: Verwendet man anstelle des Geschwindigkeitsadditionsgesetzes der speziellen Relativit¨ atstheorie Gl. 3.182 das Geschwindigkeitsadditionsgesetz der klassischen Physik va+b = va + vb , w¨ are die in der Realit¨at vorhandene Phasenverschiebung nicht vorhergesagt worden. Bei der technischen Nutzung des Sagnac-Effektes im Faserkreisel unterscheidet man zwischen aktiven und passiven Faserkreiseln, beim aktiven Faserkreisel wird noch zus¨ atzlich zwischen Open- und Closed-loop-Betrieb unterschieden. Prinzipiell wird die Phasenverschiebung der Laserstrahlen ermittelt, indem sie nach dem Auskoppeln aus der Glasfaserspule zur Interferenz gebracht werden. Tats¨achlich gemessen wird die Intensit¨ at I des Interferenzergebnisses, es gilt  I = I1 + I2 + 2 I1 I2 cos ∆φ , (3.186) at der einzelnen Laserstrahlen bezeichnet. Wird die Anordwobei I1 und I2 die Intensit¨ nung nicht gedreht, w¨ are ohne weitere Maßnahmen der Lichtweg f¨ ur beide Laserstrahlen

64

3 Inertiale Navigation

Glasfaserspule Lichtquelle

IOC

Koppler Photodiode

reflexionsfreier Abschluss

Abbildung 3.13: Aufbau eines aktiven Faserkreisels.

identisch, es k¨ ame zu konstruktiver Interferenz, die Intensit¨at am Detektor w¨are maximal. Das ist nicht w¨ unschenswert, da in diesem Fall die Empfindlichkeit am geringsten ist – die Steigung der Cosinus-Funktion ist an dieser Stelle null – und nicht zwischen Rechts- und Linksdrehung unterschieden werden k¨onnte: Beides w¨are lediglich mit einer Abnahme der Intensit¨ at verbunden. Bei einem passiven FOG wird beim Auskoppeln der Laserstrahlen aufgrund des verwendeten Kopplers eine zus¨ atzliche Phasenverschiebung von 60◦ erzielt. Bei einem aktiven FOG erh¨ alt man eine zus¨ atzliche Phasenverschiebung durch Modulation mit einem integrierten optischen Chip. Der prinzipielle Aufbau eines aktiven FOGs ist in Abb. 3.13 zu sehen. W¨ ahrend im Open-loop-Betrieb die aufgrund des Sagnac-Effektes resultierende Phasenverschiebung gemessen wird, wird im Closed-loop-Betrieb die Modulation so angepasst, dass diese Phasenverschiebung aufgrund des Sagnac-Effektes gerade kompensiert wird. Umfassende Beschreibungen sind in [80], [110] zu finden. Ringlaser-Kreisel Auch der Ringlaser-Kreisel basiert auf dem Sagnac-Effekt. Der ben¨otigte geschlossene Lichtweg wird hier jedoch durch drei8 geeignet angeordnete Spiegel hergestellt, siehe Abb. 3.14. In dem Lichtweg befindet sich ein Helium-Neon-Gasgemisch, das als Lasermedium dient. Durch geeignetes Pumpen bilden sich ein im und ein entgegen dem Uhrzeigersinn umlaufender Laserstrahl aus. Hierbei handelt es sich um stehende Wellen, deren Frequenz mit von der L¨ ange des Lichtweges abh¨angt. Befindet sich die Anordnung in Ruhe, ist der Lichtweg f¨ ur beide Laserstrahlen identisch, beide Laserstrahlen haben somit dieselbe Frequenz. Wird die Anordnung in Rotation versetzt, verl¨angert sich der Lichtweg f¨ ur den einen und verk¨ urzt sich der Lichtweg f¨ ur den anderen Laserstrahl. Ein l¨ angerer Lichtweg bedeutet aber, dass sich die Wellenl¨ange verg¨oßern und somit die Frequenz verringern muss, damit nach wie vor eine stehende Welle vorliegt. Die beiden Laserstrahlen haben nun unterschiedliche Frequenzen, die Frequenzdifferenz h¨angt von der Drehrate ab. Um diese Frequenzdifferenz zu detektieren, ist einer der Spiegel halbdurchl¨ assig ausgef¨ uhrt, so dass die Laserstrahlen ausgekoppelt und zur Interferenz 8 Prinzipiell

k¨ onnen nat¨ urlich auch mehr als drei Spiegel eingesetzt werden.

3.4 Fehlercharakteristik eines Inertialnavigationssystems

65

Spiegel

Anode

Detektor

Kathode

Prisma

Abbildung 3.14: Aufbau eines Ringlaserkreisels.

gebracht werden k¨ onnen. Im Detektor ergibt sich ein Interferenzmuster, das mit zwei Photodioden ausgewertet wird: Bei identischer Frequenz der Laserstrahlen ist das Interferenzmuster ortsfest, bei unterschiedlicher Frequenz beginnen die Interferenzstreifen zu wandern, die Richtung in der sich die Interferenzstreifen bewegen, h¨angt von der Drehrichtung der Anordnung ab. Die Frequenz, mit der die Maxima des Interferenzmusters an den Photodioden vorbeistreichen, h¨angt von der Drehrate ab und wird als Beat-Frequenz bezeichnet. Zwei Photodioden sind n¨otig, um die Bewegungsrichtung der Interferenzstreifen und damit die Drehrichtung erkennen zu k¨onnen. Eine Schwierigkeit bei diesem Messprinzip stellt der Lock-in-Effekt dar. Darunter versteht man, dass sich die Frequenz der Laserstrahlen erst ab einer bestimmten Mindestdrehrate ¨ andert, kleinere Drehraten k¨ onnen nicht gemessen werden. Dieses Problem wird meist durch einen mechanischen Dither gel¨ ost, d.h. die Anordnung wird mechanisch in oszillatorische Rotationsbewegungen versetzt, um so außerhalb des Lock-in-Bereichs zu arbeiten. Ringlaser-Kreisel sind die hochwertigsten der hier beschriebenen Drehratensensoren, eine detaillierte Betrachtung dieses Sensortyps ist in [10] zu finden.

3.4.2

Beschleunigungsmesser

Beschleunigungsmesser verf¨ ugen in der Regel u ¨ ber eine oder mehrere Probemassen, die unter dem Einfluss der zu messenden Beschleunigung ausgelenkt werden. Bei einem

66

3 Inertiale Navigation

I1

10

10

10

10

10

10

10

10

10

10 10

10 10

10

10 10

Q1

10

10

10

10

10

10

10

10

10

10 10

10 10

10

10 10

−Q1

10 10

Q2

10 10

−Q2

U1

10

10 10

10 10

10

10

10

10

10 10

10

10

10

10 10

10

10

10 10

10

10

10 10

10

10

10 U2

10

10 10 10

10

10

10 10 10

10

10 10

01 Probemasse 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10

10 10

10 10

10 10 10 10

10

10

10

10 10 10 10

10 10

sensitive Achse

I2 Abbildung 3.15: Aufbau eines Pendel-Beschleunigungsmessers.

Open-loop-Beschleunigungsmesser wird diese Auslenkung direkt gemessen, w¨ahrend im Closed-loop-Betrieb die Probemasse in ihre Ruhelage zur¨ uckgestellt und die daf¨ ur ben¨otigten Stellgr¨ oßen ermittelt werden. Im Folgenden werden der Pendel-Beschleunigungsmesser und der Vibrating-beam-Beschleunigungsmesser n¨aher beschrieben. Pendel-Beschleunigungsmesser Bei Pendel-Beschleunigungsmessern kommen h¨aufig Techniken aus der Chip-Herstellung zum Einsatz; die elastisch aufgeh¨ angte Probemasse wird sehr exakt aus einem SiliziumWaver ausge¨ azt und auf Ober- und Unterseite metallisiert. Damit kann eine DoppelKondensator-Struktur aufgebaut werden, wie sie in in Abb. 3.15 dargestellt ist. Im Open-loop-Betrieb wird die beschleunigungsbedingte Auslenkung der Probemasse kapazitiv gemessen. Diese Vorgehensweise hat den Nachteil, dass der Zusammenhang zwischen Krafteinwirkung und Auslenkung der Probemasse bestenfalls f¨ ur sehr kleine Auslenkungen linear ist. Dieses Problem wird im Closed-loop-Betrieb umgangen, bei dem die Probemasse in ihrer Ruhelage gehalten wird. In der Ruhelage besitzten beide Kondensatoren die gleiche Kapazit¨ at C1 = C2 , so dass man mit Q1 = C1 U1 ,

Q2 = C2 U2

(3.187)

als Ruhelagebedingung Q1 ! Q2 = U1 U2

(3.188)

alt. Wird die Probemasse durch eine einwirkende Beschleunigung a ausgelenkt, ¨anerh¨ dern sich die Kapazit¨ aten und damit die Spannungen U1 und U2 , die gemessen werden.

3.4 Fehlercharakteristik eines Inertialnavigationssystems

67

Um die Ruhelagebedingung Gl. (3.188) wieder zu erf¨ ullen, werden gezielt Ladungen ∆q = I∆t auf die Kondensatorplatten aufgebracht: Bei unterschiedlichen Ladungen Q1 und Q2 wird eine elektrostatische Kraft F =

Q21 Q22 − 20 A 20 A

(3.189)

auf die Probemasse ausge¨ ubt, A bezeichnet die Kondensatorfl¨ache, 0 ist die Dielektrizit¨ atskonstante. Diese elektrostatische Kraft kompensiert gerade die einwirkende Beschleunigung. Durch Einsetzen von Q1 = Q + ∆q ,

Q2 = Q − ∆q

(3.190)

erh¨ alt man mit F = ma den gesuchten Zusammenhang zwischen aufgebrachter Ladung ∆q und Beschleunigung a: a=

4∆qQ 2m0 A

(3.191)

Hierbei bezeichnet m die Masse der Probemasse. Prinzipiell ist davon auszugehen, dass bei einem Pendel-Beschleunigungsmesser im Closed-loop-Betrieb bessere Ergebnisse erzielt werden k¨ onnen als im Open-loop-Betrieb. Vibrating-beam-Beschleunigungsmesser Die Schwingungsfrequenz eines Quarzes ¨ andert sich – vergleichbar mit einer Gitarrensaite – unter Zug- oder Druckbelastung. Bei einem Vibrating-beam-Beschleunigungsmesser werden nun zwei Quarze so angeordnet, dass einer der Quarze bei Einwirkung einer Beschleunigung von einer Probemasse gestaucht und der andere Quarz gedehnt wird. Es kann wie in Abb. 3.16 eine Probemasse zum Einsatz kommen, h¨aufig besitzt aber jeder Quarz seine eigene Probemasse. W¨ ahrend im beschleunigungsfreien Fall beide Quarze mit der identischen Resonanzfrequenz schwingen, erh¨oht bzw. verringert sich die Schwingungsfrequenz der Quarze bei Einwirkung einer Beschleunigung. Die Beschleunigung wird anhand der Schwebungsfrequenz9 bestimmt, die sehr genau gemessen werden kann. Details zu diesem Sensortyp k¨ onnen [81] entnommen werden.

3.4.3

Generische Inertialsensorfehlermodelle

Drehratensensoren Die Messung von Drehraten und Beschleunigungen bzw. Winkel- und Geschwindigkeitsinkrementen ist immer mit Fehlern behaftet. Ein generisches Fehlermodell einer Drehratensensor-Triade ist gegeben durch ωibb + bω + nω . ω ˜ ibb = MGyro · 

(3.192)

˜ b bezeichnet, ω b ist die reale Drehrate. Idealerweise Die gemessene Drehrate ist mit  ω ib ib sind die sensitiven Achsen der drei Sensoren exakt orthogonal zueinander ausgerichtet. 9 Bei der Uberlagerung ¨ zweier Schwingungen mit unterschiedlichen Frequenzen tritt eine Schwebung auf, deren Frequenz gerade der Frequenzdifferenz der beiden Schwingungen entspricht.

68

3 Inertiale Navigation

10

10 10

10

10

10 10

10

10 10

Quarz Lagerung Probe-

sensitive

masse

Achse

Quarz

10 10

10

10

10

10

10

10

10

10

10

10

10 10

10

10 10

10

10

Abbildung 3.16: Aufbau eines Vibrating-beam-Beschleunigungsmessers.

Die in der Realit¨ at vorliegende Missweisung der Sensorachsen kann u ¨ ber die Nebendiagonalelemente der Misalignment-Matrix ⎞ ⎛ sx δzx −δyx MGyro = ⎝ −δzy sy δxy ⎠ (3.193) δyz −δxz sz beschrieben werden. Meist wird das Misalignment nach dem Zusammenbau vermessen und innerhalb der IMU rechnerisch kompensiert, so dass diese Nebendiagonalelemente als der nach dieser Kompensation verbleibende Fehler zu verstehen sind. Die Hauptdiagonale der Misalignmentmatrix enth¨ alt die Skalenfaktoren der Sensoren. Die Abweichung dieser Skalenfaktoren von dem Idealwert eins sind die Skalenfaktorfehler, die in ppm10 angegeben werden. Diese Skalenfaktorfehler bestehen aus einem konstanten Anteil und einem drehratenabh¨ angigen Anteil, der Skalenfaktorfehlernichtlinearit¨at, f¨ ur die h¨ aufig eine quadratische Abh¨ angigkeit von der Drehrate angenommen wird. Im Term bω sind die Biase, d.h. die Nullpunktsfehler der Sensoren zusammengefasst. Diese werden je nach G¨ ute der Sensoren in Grad pro Stunde oder Grad pro Sekunde ange¨ geben. Ublicherweise unterscheidet man einen konstanten Anteil, der sich zwar von Einschalten zu Einschalten ver¨ andern kann, w¨ahrend des Betriebs aber konstant bleibt, und einen zeitver¨anderlichen Anteil, der als Bias-Drift, Bias-Stabilit¨at oder Bias-Variation 10 ppm

bedeutet parts-per-million, also 1e-6.

3.4 Fehlercharakteristik eines Inertialnavigationssystems

69

Tabelle 3.2: Einheitenumrechnungen Drehratensensorrauschen (◦ ) √ h

◦ /h √ Hz

rad/s rad √ = ˆ √s Hz

1

60

2.909e-4

0.0167

1

4.848e-6

3437.7

206264.8

1

Tabelle 3.3: Ungef¨ ahre Gr¨ oßenordnungen von Drehratensensorfehlern

Fehler

RLG

FOG

g-unabh¨ angiger Bias:

0.001 − 10 ◦/h

MEMS

0.1 −

g-abh¨ angiger Bias:

0 ◦/h/g

< 1 ◦/h/g

1 ◦/h/g

g 2 -abh¨ angiger Bias:

0 ◦/h/g 2

< 0.1 ◦/h/g 2

0.3 ◦/h/g 2

Skalenfaktorfehler:

5 ppm

Angle Random Walk:

0.01 ◦/

50 ◦/h

> 100 ppm √ h

√

0.1 ◦/

h

5 ◦/h − 5 ◦/s

> 400 ppm √ h

1 ◦/

bezeichnet wird. Letzterer wird meistens stochastisch als Gauss-Markov-Prozess erster ¨ Ordnung beschrieben. Ursache f¨ ur zeitliche Anderungen des Bias k¨onnen z.B. Temperatureinfl¨ usse sein. Schließlich weisen Drehratensensoren h¨aufig Biase auf, die als Funktion der einwirkenden Beschleunigung – g-abh¨ angiger Bias – und als Funktion des Quadrates der einwirkenden Beschleunigung – g 2 -abh¨ angiger Bias – beschrieben werden k¨onnen. Bias, Skalenfaktorfehler und Skalenfaktorfehlernichtlinearit¨at sind in Abb. 3.17 veranschaulicht. Der Term nω beschreibt das sensorinherente Rauschen, das als weiß, normalverteilt und mittelwertfrei angenommen wird. Zur Quantifizierung wird die spektrale Leistungsdichte oder deren Wurzel angegeben, da bei integrierenden Sensoren die Varianz im Zeitdiskreten von der Abtastzeit abh¨ angt: Ergibt sich der zeitdiskreter Drehratenmesswert aß ω k aus der realen Drehrate ω(t) gem¨ 1 ωk = ∆t


t k (ω(τ ) + n ω (τ )) dτ ,

(3.194)

t k−1

so findet man f¨ ur den die Messung verf¨ alschende Rauschterm n ωk n ω,k

1 = ∆t


t k n ω (τ ) dτ . t k−1

(3.195)

70

3 Inertiale Navigation

gemessene Drehrate Skalenfaktorfehler

Ausgleichsgerade

Messung

Bias reale Drehrate

Nichtlinearit¨at des Skalenfaktorfehlers

Abbildung 3.17: Bias, Skalenfaktorfehler und Nichtlinearit¨ at des Skalenfaktorfehlers.

Hierbei bezeichnet nω (τ ) das weiße Rauschen11 im Zeitkontinuierlichen, es gilt E [nω (τ1 )nω (τ2 )] = R · δ(τ1 − τ2 ) .

(3.196)

Die Varianz Rk von nω,k ergibt sich zu ⎡


tk
tk



⎢ 1 Rk = E n2ω,k = E ⎣ 2 ∆t

⎤ ⎥ nω (τ1 )nω (τ2 ) dτ1 dτ2 ⎦

tk−1 tk−1

=

1 ∆t2


tk
tk R · δ(τ1 − τ2 ) dτ1 dτ2 tk−1 tk−1

1 = ∆t2


tk R dτ2 =

R ∆t

.

(3.197)

tk−1 11 Weißes Rauschen kann im Zeitkontinuierlichen in der Realit¨ at nicht existieren, ist als theoretisches Konzept aber dennoch sinnvoll. Im Zeitdiskreten existiert weißes Rauschen, siehe auch Abschnitt 5.2.1.

3.4 Fehlercharakteristik eines Inertialnavigationssystems

71 2

Die spektrale Leistungsdichte R besitzt hier die Einheit (rad/s) /Hz, typischerweise wird √ √ 180 rad √ / s angegeben und als Angle deren Wurzel in der Einheit rad/ s oder deg/ h = π·60 Random Walk bezeichnet. Die Bedeutung dieser Angabe kann wie folgt verdeutlicht werden: Es sei µk weißes Rauschen mit der Standardabweichung σµ = 1, ein Drehratenrauschen nω,k mit Varianz Rk ist dann durch #  R nω,k = Rk · µk = · µk (3.198) ∆t gegeben, die zugeh¨ origen Winkelinkremente sind ∆θk = nω,k ∆t =

√ R · ∆t · µk .

(3.199)

Die Varianz eines solchen Winkelinkrements ist 2 σ∆θ = R · ∆t · σµ2 = R · ∆t .

(3.200)

Werden u ¨ ber einen Zeitraum T = m∆t zur Berechnung eines Winkels m dieser Winkelinkremente aufsummiert, erh¨ alt man den aufgrund des Rauschens aufgetretenen Winkelfehler θe . Die Varianz dieses Winkelfehlers ergibt sich durch Summation der Winkelinkrementvarianzen: 2 = m · ∆t · R = T · R σθ2e = m · σ∆θ

(3.201)

Die Standardabweichung des durch Rauschen verursachten Winkelfehlers ist folglich σθe =

√ √ T· R.

(3.202)

Die Angabe des Drehratensensorrauschens als Wurzel aus einer spektralen Leistungsdichte erm¨ oglicht also, schnell auf den in einem bestimmten Zeitraum aufgrund des Rauschens zu erwartenden Winkelfehler zu schließen: Dessen Standardabweichung erh¨ alt man – unabh¨ angig von der Abtastzeit – durch Multiplikation der Wurzel der spektralen Leistungsdichte mit der Wurzel des betrachteten Zeitraums. Ein Angle Random √ Walk von 1◦/ h l¨ asst also in einer Stunde einen rauschbedingten Winkelfehler mit einer Standardabweichung von 1◦ erwarten. Tab. 3.2 listet die Umrechnungen zwischen den g¨angigen Einheiten auf, in denen Inertialsensorrauschen angegeben wird. Die typischen Gr¨ oßenordnungen der beschriebenen Drehratensensorfehler f¨ ur FOG, RLG und MEMS-Gyro sind in Tab. 3.3 angegeben. Diese Zahlenwerte sind als ungef¨ ahre Richtwerte zu verstehen; gerade bei den High-End MEMS-Gyros ist aufgrund der umfangreichen Anstrengungen auf diesem Gebiet in der Zukunft mit einer Steigerung der G¨ ute zu rechnen, so dass Nullpunktsfehler von weniger als einem Grad pro Stunde erreicht werden k¨ onnten, w¨ ahrend momentan die in der Tabelle angegebenen f¨ unf Grad pro Stunde von den meisten Sensoren nicht erreicht werden.

72

3 Inertiale Navigation

Tabelle 3.4: Einheitenumrechnungen Beschleunigungsmesserrauschen √ m/s/ h

√ mg/ Hz

√ √ m/s2/ Hz= ˆ m/s/ s

1

1.703

0.0167

0.588

1

9.807e-3

60

101.97

1

Beschleunigungsmesser Im Prinzip treten bei einer Beschleunigungsmesser-Triade die gleichen Messfehler auf wie bei einer Drehratensensor-Triade, so dass auch hier ein Inertialsensorfehlermodell der Form ˜ b = MAcc · a b + ba + na a ib ib

(3.203)

gew¨ ahlt werden kann. Beschleunigungsmesserbiase werden in mg = 1e – 3g angegeben, wobei die Gravitation zu g = 9.80665m/s2 definiert wird. Beschleunigungsmesserbiase k¨onnen einen als Rectification Error bezeichneten Anteil aufweisen, der von den einwirkenden Vibrationen abh¨ angt12 . Das kann insbesondere dann zu Problemen f¨ uhren, wenn dieser Bias im ¨ Rahmen der Datenfusionsalgorithmik des Navigationssystems gesch¨atzt wird: Andert sich die Vibrationsumgebung, z.B. wenn in einem Transfer-Alignment-Szenario, siehe Abschnitt 9, auf die Alignment-Phase die Freiflug-Phase folgt, sind die in der AlignmentPhase gesch¨ atzten Biase unter Umst¨ anden nicht mehr g¨ ultig. Das Beschleunigungsmesserrauschen wird u ¨ ber die Wurzel der spektrale Leistungsdichte beschrieben und als Velocity Random Walk bezeichnet, die Bedeutung ist dabei analog zum Angle Random Walk. Die Umrechnungen zwischen g¨angigen Einheiten sind in Tab. 3.4 angegeben. ¨ Eine Ubersicht u oßenordnungen von Beschleunigungsmesserfehlern ist ¨ ber typische Gr¨ in Tab. 3.5 zu finden. Neben den bisher beschriebenen Sensorfehlern resultieren bei einer Beschleunigungsmesser-Triade zus¨ atzliche Messfehler, wenn die Positionen der Sensoren nicht ber¨ ucksichtigt werden. Die Achsen der Inertialsensoren definieren die Koordinatenrichtungen des k¨ orperfesten Koordinatensystems, aufgrund ihrer physikalischen Ausdehnung kann sich aber nicht mehr als ein Beschleunigungssensor im Ursprung dieses Koordinatensystems befinden. Wenn das k¨orperfeste Koordinatensystem rotiert, messen die Beschleunigungssensoren daher noch Anteile der Zentripetalbeschleunigung 12 Ein

solcher Rectification Error kann auch bei Drehratensensoren vorliegen.

3.4 Fehlercharakteristik eines Inertialnavigationssystems

73

Tabelle 3.5: Ungef¨ ahre Gr¨ oßenordnungen von Beschleunigungsmesserfehlern

Fehler

Vibrating Beam

Pendel

Konstanter Bias:

0.1 − 1 mg

0.1 − 10 mg

Biasstabilit¨ at:

0.1 mg

1 mg

Skalenfaktorfehler:

100 ppm

1000 ppm √ 0.04 m/s/ h

Velocity Random Walk:

in Richtung ihrer sensitiven Achsen:  ⎞ ⎛ b ωibb × rxb  ex ωib ×    b ωibb × ryb  ey ⎠ ωibb ×  azentr. = ⎝ b ωib ×   ωibb × rzb ez

√ 0.01 m/s/ h

(3.204)

b die Positionen der Beschleunigungsmesser im k¨orperfesten KoHierbei bezeichnen rx,y,z ordinatensystem. Ob eine rechnerische Kompensation dieses als Size Effect bezeichneten Sachverhalts sinnvoll ist, h¨ angt von der G¨ ute der Sensoren und der Trajektoriendynamik ab.

3.4.4

Kurzzeitcharakteristik

Die Fehlerdifferentialgleichungen, die die zeitliche Propagation von Fehlern der Navigationsl¨ osung eines Inertialnavigationssystems beschreiben, sind von besonderem Interesse, da sie in einem integrierten Navigationssystem die Grundlage des Error State Space“” Kalman-Filter-Systemmodells darstellen. Diese werden in Abschnitt 8.2.1 hergeleitet. Im Folgenden sollen anstelle einer detaillierten mathematischen Beschreibung einige grundlegende Mechanismen der Fehlerfortpflanzung in einem Inertialnavigationssystem beschrieben werden. Lagefehler Zur Verdeutlichung der Auswirkung von Lagefehlern wird die in Abb. 3.18 dargestellte Situation betrachtet: Das Fahrzeug befinde sich in Ruhe, so dass die Beschleunigungsmesser lediglich die Schwerebeschleunigung g messen. Der tats¨achlich vorhandene RollWinkel sei null, der tats¨ achliche Pitch-Winkel sei θ. In diesem Szenario messen drei ideale Beschleunigungssensoren ⎛

⎞ g sin θ ˜b = ⎝ 0 ⎠ . a ib −g cos θ

(3.205)

Die Lageinformationen der Navigationsl¨ osung seien fehlerhaft, der Pitch-Winkel wurde zu null berechnet. Bevor die gemessenen Beschleunigungen integriert werden, um auf Geschwindigkeits¨ anderungen zu schließen, muss die Schwerebeschleunigung aus den

74

3 Inertiale Navigation x

reale Lage θ

y

Strapdown-Lage

z

Abbildung 3.18: Auswirkung von Lagefehlern.

Messdaten herausgerechnet werden. Aufgrund der fehlerhaften Lageinformation wird davon ausgegangen, dass der z-Beschleunigungsmesser die volle Schwerebeschleunigung gemessen hat, wohingegen keine Einkopplung in die x- und y-Beschleunigungsmesser vorliegt. Damit ermittelt der Strapdown-Algorithmus f¨ ur den Fall, dass der Lagefehler klein ist, d.h. sin θ ≈ θ und cos θ ≈ 1 gilt, die trajektorienbedingte Beschleunigung zu ⎛ ⎞ ⎛ ⎞ g sin θ gθ b b b ˜ + g = ⎝ ⎠≈⎝0⎠ . 0 aib,dyn. = a (3.206) ib l g(1 − cos θ) 0 Daraus kann geschlossen werden, dass kleine Lagefehler zun¨achst im Wesentlichen zu Geschwindigkeits- und Positionsfehlern in den Horizontalkan¨alen f¨ uhren, w¨ahrend der Vertikalkanal weniger betroffen ist. Da die fehlerhafte Beschleunigungskomponente zur Berechnung der Geschwindigkeit einmal und zur Berechnung der Position zweimal integriert wird, wachsen bedingt durch Lagefehler Geschwindigkeitsfehler proportional mit der Zeit, Positionsfehler quadratisch mit der Zeit an. Inertialsensorbiase Es ist unmittelbar einsichtig, dass Beschleunigungsmesserbiase zu einem Anwachsen der Geschwindigkeitsfehler proportional mit der Zeit und zu einem Anwachsen der Positionsfehler quadratisch mit der Zeit f¨ uhren. Da gemessene Drehraten zur Propagation der Lage einmal integriert werden m¨ ussen, f¨ uhren Drehratensensorbiase in erster N¨ aherung zu einem Anwachsen der Lagefehler proportional mit der Zeit: ⎛ ⎞ ∆φ ⎝ ∆θ ⎠ = bω · t (3.207) ∆ψ

3.4 Fehlercharakteristik eines Inertialnavigationssystems

75

¨ Mit den Uberlegungen des vorherigen Abschnitts erh¨alt man dadurch einen Fehler ⎛

∆gln

⎛ ⎞ ⎞ bω,y ∆θ ≈ gln ⎝−∆φ⎠ = gln ⎝−bω,z ⎠ t 0 0

(3.208)

bei der Kompensation der Schwerebeschleunigung, was letztendlich zu einem Anwachsen der Positionsfehler gem¨ aß ⎞ ⎛  1 n ⎝ bω,y ⎠ 3 (3.209) ∆r bω = gl −bω,z t 6 0 f¨ uhrt.

3.4.5

Langzeitcharakteristik

¨ W¨ ahrend f¨ ur k¨ urzere Zeitr¨ aume die Uberlegungen des vorherigen Abschnittes sicherlich eine qualitative G¨ ultigkeit haben, sind bei der Betrachtung gr¨oßerer Zeitr¨aume zus¨atzlich andere Mechanismen relevant. Instabilit¨ at des H¨ ohenkanals Zun¨ achst soll der H¨ ohenkanal eines Inertialnavigationssystems betrachtet werden. Zur Kompensation der von den Beschleunigungsmessern mitgemessenenen Schwerebeschleunigung gl wird eine gesch¨ atzte Schwerebeschleunigung gˆl einem entsprechenden Modell entnommen. Die Schwerebeschleunigung nimmt mit zunehmender H¨ohe ab. Wird nun aufgrund der Navigationsl¨ osung f¨ alschlicherweise z.B. eine zu große H¨ohe u ¨ ber dem Erdellipsoid angenommen, wird eine zu geringe Schwerebeschleunigung angenommen und daher nur ein Teil der Schwerebeschleunigung kompensiert, siehe Abb. 3.19. Es verbleibt eine unkompensierte Beschleunigung δgl nach oben, die im Strapdown-Algorithmus integriert wird und so den H¨ ohenfehler weiter vergr¨oßert. Bei einer zu gering angenommenen H¨ ohe verbleibt eine Beschleunigungskomponente nach unten, die auch diesen H¨ ohenfehler weiter vergr¨ oßert. Die Auswirkung eines Anfangsh¨ ohenfehlers von zehn Metern – ohne weitere Fehlerquellen, d.h. bei Verwendung idealer Sensoren und ansonsten perfekter Initialisierung – ist in Abb. 3.20 dargestellt. Diese Instabilit¨ at des H¨ ohenkanals ist die Ursache daf¨ ur, dass ein Inertialnavigationssystem, das l¨ angere Zeit autonom navigieren soll, praktisch immer u ¨ ber einen BaroAltimeter verf¨ ugt. Schuler-Oszillationen Wird der H¨ ohenkanal z.B. durch Verwendung eines Baro-Altimeters stabilisiert, so weisen die Horizontalkan¨ ale eine gewisse Stabilit¨at auf: Da die Erde gekr¨ ummt ist, stimmt bei einem horizontalen Positionsfehler ∆r die angenommene Richtung der Schwerebeschleunigung nicht mit deren tats¨ achlicher Richtung u ¨ berein, siehe Abb. 3.21. Dadurch

76

3 Inertiale Navigation

δgl

−gˆl gl angenommene Position reale Position

Abbildung 3.19: Instabilit¨ at des H¨ ohenkanals.

3000

H¨ohenfehlerfehler (m)

2500 2000 1500 1000 500 0 0

10

20

30 Zeit (min)

Abbildung 3.20: Instabilit¨ at des H¨ ohenkanals.

40

50

60

3.4 Fehlercharakteristik eines Inertialnavigationssystems

77

reale Position ∆r Positionsfehler α g l, real ∆a

angenommene Position

g l, angenommen

R

α

Abbildung 3.21: Entstehung der Schuler-Oszillationen.

wird im Strapdown-Algorithmus die Schwerebeschleunigung nicht vollst¨andig kompensiert, ein Anteil der Schwerebeschleunigung wird als Beschleunigung des Fahrzeugs interpretiert. Diese Beschleunigungskomponente ∆a ist gerade so gerichtet, dass sie dem vorliegenden Positionsfehler entgegenwirkt. Anhand von Abb. 3.21 erh¨ alt man n¨ aherungsweise ∆r ∆a =− R gl

(3.210)

und damit Ƭ r+

gl ∆r = 0 . R

(3.211)

Gl. (3.211) ist die Differentialgleichung eines harmonischen Oszillators mit einer Periodendauer von ungef¨ ahr 84 Minuten. Die aus dem beschriebenen Mechanismus resultierenden Oszillationen der Positions- und Geschwindigkeitsfehler werden als SchulerOszillationen bezeichnet.

78

3.5

3 Inertiale Navigation

Initialisierung

Vor Beginn der Strapdown-Rechnung muss eine initiale Navigationsl¨osung bestimmt werden. Im Folgenden soll angenommen werden, dass sich das Fahrzeug dabei in Ruhe befindet und die Anfangsposition bekannt ist. In diesem Szenario muss lediglich die Gewinnung der ben¨ otigten Lageinformationen betrachtet werden. Zun¨ achst wird ein horizontiertes Koordinatensystem mit dem Index h definiert, dessen x- und y-Achse in die Richtungen der Projektionen der x- und y-Achsen des k¨orperfesten Koordinatensystems auf die lokale Horizontale weisen. Die Achsen des horizontierten Koordinatensystems liegen damit in der gleichen Ebene wie die Nord- und Ost-Achse des Navigationskoordinatensystems, die Lage dieser beiden Koordinatensysteme wird u ¨ ber den Yaw-Winkel ψ beschrieben. Die Lage von horizontiertem und k¨orperfestem Koordinatensystem ist durch den Roll-Winkel φ und den Pitch-Winkel θ bestimmt. Roll- und Pitch-Winkel Die Beschleunigungsmesser messen die Schwerebeschleunigung zu aibh = Ch,T (0, 0, −gl) aibb = Ch,T b  b

T

,

(3.212)

wobei man Chb durch Einsetzen von ψ = 0 in Gl. (3.50) erh¨alt. Ausmultiplizieren liefert ⎞ ⎛ ⎞ ⎛ ax sin θgl abib = ⎝ay ⎠ = ⎝ − sin φ cos θgl ⎠ , az − cos φ cos θgl

(3.213)

so dass Roll- und Pitch-Winkel gem¨ aß θ = arcsin

ax gl



,

φ = arctan

ay az

(3.214)

berechnet werden k¨ onnen. Alternativ kann man den Roll-Winkel auch anhand von

−ay φ = arcsin (3.215) gl cos θ berechnen. Bezeichnet man die Fehler der gemessenen Beschleunigungen mit δax , δay und δaz , so erh¨ alt man f¨ ur die Fehler der berechneten Winkel n¨aherungsweise δφ =

−δay , gl

δθ =

δax . gl

(3.216)

Ein Beschleunigungsmesserbias von 1mg verursacht bei dieser Vorgehensweise also einen Lagefehler von 1mrad.

3.5 Initialisierung

79

Ωhor

h ωib,x

ψ

en

h ωib,y ed

Abbildung 3.22: Prinzip der Nordsuche.

Nordsuche Der Yaw-Winkel ψ kann bei ausreichend genauen Drehratensensoren durch Messung der Erddrehrate bestimmt werden. Dieser Vorgang wird als Nordsuche bezeichnet. Die Projektion der Erddrehrate in die lokale Horizontalebene sei mit Ωhor bezeichnet, es gilt Ωhor = Ω cos ϕ. Mit Gl. (3.120) und Abb. 3.22 erh¨alt man die Horizontalkomponenten der Erddrehrate in Koordinaten des horizontierten Koordinatensystems zu

h ωib,x = Ω cos ϕ cos ψ h ωib,y

= −Ω cos ϕ sin ψ .

(3.217) (3.218)

Im allgemeinen Fall φ = 0 und θ = 0 m¨ ussen diese Gr¨oßen aus den gemessenen Drehraten ermittelt werden: h b b b ωib,x = ωib,x cos θ + ωib,y sin φ sin θ + ωib,z cos φ sin θ

(3.219)

h b b = ωib,y cos φ − ωib,z sin φ ωib,y

(3.220)

Ausgehend von Gl. (3.217) – (3.218) berechnet sich der Yaw-Winkel wie folgt: h h , ωib,x ) ψ = arctan2(−ωib,y

(3.221)

Ist der Yaw-Winkel bestimmt, k¨ onnte aus Gl. (3.217) oder (3.218) sogar noch der ungef¨ ahre Breitengrad berechnet werden.

80

3 Inertiale Navigation

Max. Fehler der Nordsuche (◦ )

12  

10

Messfehler 0.01◦ /h Messfehler 0.1◦ /h Messfehler 1.0◦ /h

 

8 6











4



2 0









0

10

20

30





40 50 Breitengrad (◦ )

 60

 70

80

90

Abbildung 3.23: Breitengradabh¨ angigkeit der Nordsuche.

Sind die aufgrund der nicht exakt bekannten Roll- und Pitch-Winkel resultierenden Fehler bei der Berechnung der horizontierten Drehraten Gl. (3.219) – (3.220) vernachl¨assigbar, dann beeinflussen im Wesentlichen drei Faktoren die Genauigkeit der Nordsuche: h 1. Messfehler. Fehlerhafte Messungen der Drehrate ωib f¨ uhren direkt zu YawWinkel-Fehlern. Daher werden meist u ¨ ber einen l¨angeren Zeitraum gemittelte Drehraten verwendet, um den Einfluss des Sensorrauschens zu minimieren. Der Einfluss der Drehratensensorbiase kann nicht verringert werden.

¨ 2. Breitengrad. Je weiter man sich vom Aquator entfernt, desto geringer ist der horizontale Anteil der Erddrehrate, folglich wirken sich Messfehler umso st¨arker aus. Dieser Zusammenhang ist in Abb. 3.23 verdeutlicht, dort sind f¨ ur verschiedene h Gr¨ oßenordnungen der Messfehler von ωib die maximal bei der Nordsuche resultierenden Yaw-Winkelfehler u ¨ ber dem Breitengrad aufgetragen. An den Polen ist eine Nordsuche nat¨ urlich unm¨ oglich. 3. Yaw-Winkel. Der Yaw-Winkel wird u ¨ ber die nichtlineare Arcus-TangensFunktion aus den gemessenen Drehraten bestimmt. Aufgrund dieser Nichtlinearit¨ at resultiert eine unterschiedliche, vom Yaw-Winkel abh¨angige Empfindlichkeit der Nordsuche gegen¨ uber Messfehlern. Dies ist in Abb. 3.24 verdeutlicht, hier ist f¨ ur Messfehler der Gr¨ oßenordnung 1◦/h am 45. Breitengrad der bei einer Nordsuche resultierende Yaw-Winkel-Fehler u ¨ ber dem Yaw-Winkel aufgetragen.

3.5 Initialisierung

81

Max. Fehler der Nordsuche (◦ )

6 5 4 3 2 1 0 -150

-100

-50

0 50 Yaw-Winkel (◦ )

100

150

Abbildung 3.24: Yaw-Winkel abh¨ angige Empfindlichkeit der Nordsuche.

Um eine Nordsuche durchf¨ uhren zu k¨ onnen, werden hochwertige Drehratensensoren ben¨ otigt. F¨ ur kosteng¨ unstige MEMS-Sensoren scheidet diese M¨oglichkeit der Yaw-WinkelBestimmung daher aus. Alternativ zu einer Nordsuche kann in diesem Fall h¨aufig ein Magnetometer verwendet werden oder es besteht die M¨oglichkeit, den Yaw-Winkel z.B. anhand von GPS-Geschwindigkeitsmessungen zu initialisieren. Letzteres ist bei vielen Landfahrzeugen oder Fl¨ achenfl¨ uglern der Fall, solange sich diese im Wesentlichen in einer in k¨ orperfesten Koordinaten bekannten Richtung bewegen – meist in Fahrzeugl¨angsrichtung, d.h. in Richtung der x-Achse des k¨orperfesten Koordinatensystems. Die Bewegungsrichtung in Navigationskoordinaten ist aufgrund des vom GPS-Empf¨anger gelieferten Geschwindigkeitsvektors bekannt. Betrachtet man nun die Horizontalkomponente des Geschwindigkeitsvektors, so kann aus dessen Repr¨asentation in k¨orperfesten Koordinaten und in Navigationskoordinaten auf den Yaw-Winkel geschlossen werden.

4

Satellitennavigation

Ein Satellitennavigationssystem erm¨ oglicht dem Nutzer die Bestimmung seiner Position, Geschwindigkeit und Zeit. Dazu werden von Satelliten abgestrahlte Signale empfangen und ausgewertet. Transit war das erste Satellitennavigationssystem und stand ab 1964 zur Verf¨ ugung. Zur Positionsbestimmung wurde die Frequenzverschiebung der Satellitensignale aufgrund des Doppler-Effekts genutzt, die Positionsgenauigkeit betrug ungef¨ahr 500 Meter. Transit wurde von GPS abgel¨ ost und hat heute keine Bedeutung mehr. Russland betreibt das Satellitennavigationssystem GLONASS. Die Satelliten dieses Systems senden in zwei Frequenzb¨ andern, L1 und L2, auf unterschiedlichen Frequenzen: fL1,n = 1602 MHz + n · 562.5 KHz fL2,n = 1246 MHz + n · 437.5 KHz

(4.1) (4.2)

Die empfangenen Satellitensignale k¨ onnen daher anhand ihrer Signalfrequenzen getrennt werden, man spricht von Frequency-Division Multiple Access (FDMA). Die Positionsbestimmung erfolgt durch Messung der Signallaufzeit vom Satellit zum Empf¨anger. 1996 befanden sich 24 Satelliten im Orbit, bis zum Jahre 2001 sank die Anzahl funktionsf¨ahiger Satelliten jedoch auf sieben St¨ uck ab. Seitdem wird versucht, diesem System wieder Satelliten hinzuzuf¨ ugen, Anfang 2006 waren 13 operationelle Satelliten vorhanden. Eine weltweite, kontinuierlich Abdeckung ist damit nicht gegeben. Details zu GLONASS k¨ onnen [3] entnommen werden. China betreibt das System Beidou, das derzeit u ugt ¨ ber drei geostation¨are Satelliten verf¨ und im Wesentlichen den asiatischen Raum abdeckt, siehe [1]. Das europ¨ aische System Galileo befindet sich noch im Aufbau und wird in Abschnitt 4.5 kurz beschrieben. Das momentan wichtigste System ist das amerikanische Navstar GPS oder kurz GPS. Es ist das derzeit einzige System, das eine kontinuierliche weltweite Abdeckung bietet und f¨ ur das Empf¨ anger kommerziell verf¨ ugbar sind. Dieses System wird daher in den folgenden Abschnitten in gr¨ oßerer Tiefe betrachtet.

4.1

Systemu ¨berblick Navstar GPS

Die Entwicklung des Navstar Global Positioning Systems wurde 1973 vom US-Verteidigungsministerium initiiert. Der erste Satellit wurde bereits 1978 in die Umlaufbahn

84

4 Satellitennavigation

gebracht. Bis 1985 folgten zehn weitere so genannte Block-I-Satelliten, von denen heute keiner mehr aktiv ist. Seit 1989 wurden mit Block II, Block IIA und Block IIR die n¨ achsten Satellitengenerationen ins All geschossen, so dass momentan 27 GPS-Satelliten verf¨ ugbar sind [4]. Im Zuge der Modernisierung des GPS-Systems werden k¨ unftig modifizierte IIR-M-, IIF- und GPS-III-Satelliten zur Verf¨ ugung stehen, siehe Abschnitt 4.4.

Space Segment Das Space Segment besteht aus den GPS-Satelliten, die sich auf sechs verschiedenen Umlaufbahnen um die Erde befinden. Die Bahnebenen sind um 55 ◦ gegen¨ uber der ¨ Aquatorebene geneigt, der Rektaszensionsunterschied1 zweier benachbarter Bahnen betr¨ agt 60 ◦ . Der mittlere Radius der Umlaufbahnen ist 26560 Kilometer. Die Umlaufdauer auf diesen Bahnen betr¨ agt 11 Stunden 58 Minuten, so dass sich eine Relativgeschwindigkeit gegen¨ uber der Erde von ungef¨ ahr 3.9 km/s ergibt. An jedem Punkt der Erde sind zu jeder Zeit mindestens vier Satelliten mit einem Erhebungswinkel von mehr als f¨ unfzehn Grad sichtbar.

Control Segment Das Control Segment besteht aus f¨ unf auf dem Globus verteilten Bodenstationen, wobei sich die Hauptkontrollstation in Colorado Springs befindet. Diese Bodenstationen berechnen aktuelle Ephemeridendaten, anhand derer ein GPS-Empf¨anger die Positionen der Satelliten berechnen kann, und bestimmen den Fehler der Satellitenuhren. Diese Informationen werden an die Satelliten u ¨ bermittelt und von diesen im Rahmen der Navigationsdaten u ¨ bertragen.

User Segment Unter dem User Segment versteht man die Nutzer des GPS-Systems, die mit Hilfe von GPS-Empf¨ angern ihre Position, Geschwindigkeit und die Zeit bestimmen. Die Positionsbestimmung mit GPS beruht auf der Messung der Laufzeit, die die von den Satelliten ausgesandten Signale bis zur Ankunft beim Empf¨anger ben¨otigen. Aus dieser Laufzeit kann auf die Entfernung zwischen Satellit und Empf¨anger geschlossen werden und schließlich, den Empfang einer ausreichenden Anzahl von Satelliten vorausgesetzt, auf die Empf¨ angerposition. F¨ ur den Empfang der Satellitensignale werden unterschiedliche Typen von GPS-Empf¨ angern eingesetzt, auf die im Folgenden kurz eingegangen werden soll. • Einfrequenz-Empf¨ anger sind die kosteng¨ unstigsten GPS-Empf¨anger, die nur den C/A-Code, siehe Abschnitt 4.2.1, nutzen k¨onnen. Die im Stand-aloneBetrieb m¨ ogliche Positionsgenauigkeit ist geringer als bei anderen Empf¨angertypen: Da nur die L1-Tr¨ agerfrequenz empfangen wird, k¨onnen die frequenzabh¨angigen Laufzeitverz¨ ogerungen in der Ionosph¨are nicht kompensiert werden. 1 Der Rektaszensionsunterschied ist die Differenz der L¨ angengrade der beiden Punkte, an denen zwei ¨ Satelliten von unterhalb der Aquatorebene kommend diese durchstoßen.

4.2 Funktionsprinzip eines GPS-Empf¨ angers

85

• Ein Zweifrequenz-Empf¨ anger mit Codeless-Technik nutzt neben dem C/ACode auch das Satellitensignal auf der L2-Tr¨agerfrequenz, ohne jedoch den Y-Code zu kennen. Im einfachsten Fall bedient man sich der Tatsache, dass der Y-Code auf beiden Tr¨ agerfrequenzen zwar unbekannt, aber identisch ist. Durch Berechnung der Kreuzkorrelationsfunktion zwischen den L1- und L2-Signalen kann so die Verschiebung dieser Signale ermittelt werden, was die Kompensation des Ionosph¨arenfehlers erm¨ oglicht. Allerdings ist dies mit einer Verschlechterung des SignalRauschverh¨ altnisses verbunden. Neben dem beschriebenen Verfahren existieren noch weitere, leistungsf¨ ahigere Codeless-Techniken, siehe [53]. Unter Verwendung pr¨ aziser Ephemeriden- und Satellitenuhrenfehlerdaten, die je nach Genauigkeit mit einigen Stunden bis zw¨ olf Tagen Verz¨ogerung zur Verf¨ ugung stehen [6], kann eine Stand-alone-Positionsgenauigkeit im Zentimeterbereich erreicht werden [71]. • C/A-P(Y)-Empf¨ anger stehen den US-Streitkr¨aften und ihren Verb¨ undeten zur Verf¨ ugung. Diesen Empf¨ angern ist der Y-Code bekannt und kann daher auf beiden Tr¨ agerfrequenzen genutzt werden. Der C/A-Code wird f¨ ur einen Kaltstart des Empf¨ angers ben¨ otigt: Da die Y-Code-Pseudozufallsfolge sehr lang ist, w¨are ohne durch Auswertung des C/A-Codes erhaltene Positions- und Zeitinformationen die Suche nach dem Maximum der Kreuzkorrelationsfunktion zwischen empf¨angerinternen Y-Code-Referenzsignal und dem Satellitensignal zu aufw¨andig. Hintergedanke bei diesen Empf¨ angern ist, in einem Konfliktfall den C/A-Code so zu verschlechtern (SA, Selective Availability2 ), dass die frei zug¨anglichen C/A-CodeEmpf¨ anger im Prinzip nutzlos werden, ein Kaltstart der C/A-P(Y)-Empf¨anger aber noch m¨ oglich ist. • Die SAASM Direct P(Y)-Empf¨ anger sind ebenfalls milit¨arischen Anwendungen vorbehalten. Neben anderen Unterschieden zu C/A-P(Y)-Empf¨angern besitzen diese die M¨ oglichkeit, einen Kaltstart ohne Verwendung des C/A-Codes durchzuf¨ uhren. Hierzu gibt es zwei unterschiedliche Vorgehensweisen: Im ersten Fall erfolgt die Suche nach der Verschiebung zwischen Y-Code-Referenzsignal und Satellitensignal mit einer großen Zahl an Korrelatoren, im zweiten Fall wird eine Positions- und Zeitinformation von einer externen Quelle, zum Beispiel einem Empf¨ anger ersteren Typs, zur Verf¨ ugung gestellt. Die zugrunde liegende Philosophie ist hierbei, in einem Konfliktfall den C/A-Code in einem lokalen Bereich durch St¨ orsender vollst¨ andig unbrauchbar zu machen, ohne wie bei einer k¨ unstlichen Verschlechterung des C/A-Codes weltweit auf C/A-Code Empf¨anger verzichten zu m¨ ussen [30].

4.2

Funktionsprinzip eines GPS-Empf¨angers

Ein GPS-Empf¨ anger ermittelt aus den von den Satelliten gesendeten Signalen die Signallaufzeit, die Frequenzverschiebung aufgrund des Dopplereffekts und die Phasenlage. Die Grundz¨ uge der daf¨ ur ben¨ otigten Signalverarbeitung sollen im Folgenden skizziert werden. 2 Bis zum 1.5.2000 wurde mittels SA die Genauigkeit der C/A-Positionsbestimmung auf ungef¨ ahr hundert Meter verschlechtert, es sind aber Verschlechterungen beliebigen Ausmaßes m¨ oglich.

86

4.2.1

4 Satellitennavigation

GPS-Signalstruktur

Die GPS-Satelliten senden Signale auf zwei Tr¨agerfrequenzen L1 (fL1 = 1575.42 MHz, λL1 ≈ 19 cm) und L2 (fL2 = 1227.60 MHz, λL2 ≈ 24.4 cm). Der große Vorteil bei der Verwendung von zwei Sendefrequenzen besteht darin, dass Laufzeitverz¨ogerungen der Signale in der Ionosph¨ are wieder herausgerechnet werden k¨onnen. Um die Signale verschiedener Satelliten voneinander trennen zu k¨onnen, wird ein Bandspreizverfahren (CDMA, Code Division Multiple Access) eingesetzt. Dabei werden die Tr¨agersignale mit bekannten Pseudozufallsfolgen (PRN, Pseudo Random Noise) moduliert. Diese Pseudozufallsfolgen haben die Eigenschaft, dass ihre Autokorrelationsfunktion f¨ ur Verschiebungen ungleich null verschwindet. Zus¨atzlich sind diese Pseudozufallsfolgen orthogonal zueinander, d.h. die Kreuzkorrelationsfunktion zweier Pseudozufallsfolgen verschwindet. Es finden zwei Arten von Pseudozufallsfolgen Verwendung, der C/A-Code (Coarse/Acquisition) und der P-Code (Precise). Jedem Satellit ist sowohl eine C/A-Codeals auch eine P-Code-Pseudozufallsfolge eindeutig zugeordnet. Jede C/A-Code-Pseudozufallsfolge hat eine L¨ange von 1023 Chips3 , die Chiprate betr¨agt 1.023 MHz. Ein Chip entspricht also 977.5 ns, multipliziert mit der Lichtgeschwindigkeit ergibt sich so eine Chipl¨ ange von ungef¨ ahr 300 m. Der C/A-Code ist nur auf das L1Tr¨ agersignal aufmoduliert und wiederholt sich jede Millisekunde. Der C/A-Code ist jedem Nutzer frei zug¨ anglich. Die P-Code-Pseudozufallsfolge schließlich hat eine L¨ange von 266.4 Tagen, jeder Satellit verwendet aber nur ein Segment dieses Codes von der Dauer einer Woche. Der P-Code besitzt eine Chiprate von 10.23 MHz, was einer Chipl¨ange von ungef¨ahr 30 m entspricht. Um die Verwendung des bekannten P-Codes auf autorisierte Nutzer einzuschr¨anken und zu verhindern, dass deren GPS-Empf¨ anger durch vorget¨auschte GPS-Signale in die Irre gef¨ uhrt werden k¨ onnen (spoofing), wird der P-Code mit dem geheimen W-Code verschl¨ usselt. Der resultierende Code wird als Y-Code bezeichnet. Der W-Code besitzt eine Chiprate von 511.5 kHz, so dass auf jeden Chip des W-Codes zwanzig Chips des uber dem P-Codes kommen. Der Y-Code ist auf das L2-Tr¨agersignal und um 90 ◦ gegen¨ C/A-Code phasenverschoben auf das L1-Tr¨agersignal aufmoduliert. Zus¨ atzlich zu den PRN-Codes werden noch Navigationsdaten u ¨ bertragen, die per Biphasenmodulation auf den C/A-Code und den P-Code aufmoduliert werden. Die Navigationsdaten besitzen eine Bitrate von 50 Hz, so dass w¨ahrend der Zeit, die f¨ ur das Senden von einem Bit Navigationsdaten ben¨otigt wird, die 1023 Chips des C/A-Codes zwanzigmal vollst¨ andig wiederholt werden. Die Navigationsdaten werden in Frames zu 1500 Bit eingeteilt, die sich aus f¨ unf Subframes zu 300 Bit zusammensetzten. Die Daten der ersten drei Subframes enthalten unter anderem Informationen u ¨ ber den Satellitenuhrenfehler und die Ephemeriden des sendenden Satelliten und bleiben, von gelegentlichen Anpassungen abgesehen, konstant. Die Subframes vier und f¨ unf enthalten unter anderem Almanach-Daten4 aller Satelliten und Daten zur Korrektur der Laufzeitver3 Da dieser Datenstrom fest vorgegeben ist und somit keine Informationen enth¨ alt, spricht man von Chips; bei einem Informationen enthaltenden Datenstrom spricht man von Bits. 4 Ephemeriden sind genaue Satellitenbahninformationen, die Almanach-Daten sind grobe Bahninformationen die lediglich dazu dienen festzustellen, welche Satelliten sichtbar sind.

4.2 Funktionsprinzip eines GPS-Empf¨ angers

87

C/A(t)

a1 cos(ω L1 t)

L1(t)

Navigationsdaten

Y(t)

a1 sin(ω L1 t) P(t)

Y(t)

W(t) L2(t)

a2 cos(ω L2 t)

Abbildung 4.1: Generierung der GPS-Satellitensignale.

z¨ ogerung in der Ionosph¨ are. Der Inhalt der Subframes vier und f¨ unf ¨andert sich von Frame zu Frame, bis nach 25 Frames5 die Navigationsdaten vollst¨andig u ¨ bertragen sind und wiederholt werden. Die Generierung der GPS-Satellitensignale ist in Abb. 4.1 schematisch dargestellt. Mathematisch l¨ asst sich das von einem Satelliten gesendete Signal vereinfacht wie folgt beschreiben: st,L1 (t) = at,CA · c(t) · d(t) · sin(2πfL1 t) + at,Y 1 · y(t) · d(t) · cos(2πfL1 t) (4.3) st,L2 (t) = at,Y 2 · y(t) · d(t) · cos(2πfL2 t) (4.4) Hierbei bezeichnet st,L1 (t), st,L2 (t) at,CA , at,Y 1 , at,Y 2 c(t) y(t) d(t) 5 25

Sendesignale auf den Tr¨agerfrequenzen L1 und L2 Amplituden von C/A- und Y-Code Chip-Folge des C/A-Codes Chip-Folge des Y-Codes Bit-Folge der Navigationsdaten

Frames entsprechen 12.5 Minuten.

88

4 Satellitennavigation

Die empfangenen Signale sind einer Reihe von Einfl¨ ussen unterworfen und unterscheiden sich daher von den Sendesignalen. Einige wesentliche Einfl¨ usse werden im Folgenden kurz angesprochen. Doppler-Verschiebung Der Doppler-Effekt tritt auf, wenn sich ein Sender und ein Empf¨anger relativ zueinander bewegen. Bewegen sich Sender und Empf¨anger aufeinander zu, so erh¨oht sich die Frequenz des empfangenen Signals gegen¨ uber der nominellen Sendefrequenz. Entfernen sich Sender und Empf¨ anger voneinander, so verringert sich die Frequenz des empfangenen Signals. Hierbei ist lediglich die Komponente der Relativgeschwindigkeit in Ausbreitungsrichtung des Signals vom Sender zum Empf¨anger von Bedeutung6 , die bei einem auf der Erde ruhenden Empf¨ anger maximal vrel = 929 m/s betr¨agt [56]. Dies f¨ uhrt beim L1-Tr¨ agersignal auf eine maximale Verschiebung der Tr¨agerfrequenz von ∆fL1 = fL1

vrel = 4.9 kHz , c

(4.5)

f¨ ur die Chiprate des C/A-Codes ergibt sich eine maximale Dopplerverschiebung von ∆fC/A = fC/A

vrel = 3.2 Hz . c

(4.6)

Dabei bezeichnet c die Lichtgeschwindigkeit. Um die Dopplerverschiebung des C/ACodes bei der empf¨ angerinternen Signalverarbeitung zu ber¨ ucksichtigen, muss die Zeitachse f¨ ur die Erzeugung der Replica-Codes mit mC/A =

fC/A + ∆fC/A fC/A

(4.7)

multipliziert werden, da sich die Zeitdauer eines Chips des empfangenen Codes gerade andert hat. um den Faktor 1/mC/A ver¨ Ist der Empf¨ anger gegen¨ uber der Erdoberfl¨ache bewegt, muss diese Geschwindigkeitskomponente ebenfalls ber¨ ucksichtigt werden. Signallaufzeit Der Abstand zwischen einem Empf¨ anger auf der Erdoberfl¨ache und einem GPS-Satellit variiert zwischen rmin = 20192 km f¨ ur einen senkrecht u ¨ ber der Empf¨angerposition stehenden Satelliten und rmax = 25785 km f¨ ur einen gerade auf- oder untergehenden Satelliten. Setzt man als Ausbreitungsgeschwindigkeit der Satellitensignale die Lichtgeschwindigkeit an, ergibt sich eine minimale Signallaufzeit von τmin =

rmin ≈ 67 ms c

(4.8)

6 Es existiert auch noch ein transversaler Dopplereffekt, f¨ ur den die Geschwindigkeitskomponente senkrecht zur Ausbreitungsrichtung ausschlaggebend ist. Eine Herleitung ist u ¨ber die spezielle Relativit¨ atstheorie m¨ oglich. Bei den hier auftretenden Geschwindigkeiten ist dieser Effekt jedoch vernachl¨ assigbar.

4.2 Funktionsprinzip eines GPS-Empf¨ angers

89

und eine maximale Signallaufzeit von τmax =

rmax ≈ 83 ms . c

(4.9)

Die Erdatmosph¨ are beeinflusst die Ausbreitungsgeschwindigkeit der Satellitensignale und damit die Signallaufzeit. Dies ist – neben der Mehrwegeausbreitung, die an dieser Stelle nicht betrachtet werden soll – eine der Hauptfehlerquellen bei der Positionsbestimmung, siehe Abschnitt 4.3.4. Freiraumd¨ ampfung Mit wachsendem Abstand vom Sender nimmt die Leistung des empfangenen Signals ab. In [56] findet man hierf¨ ur den Zusammenhang Pr =

Pt λ2 . (4πr)2

(4.10)

Die Sendeleistung ist mit Pt bezeichnet, λ ist die Wellenl¨ange des Tr¨agers, r ist der Abstand zwischen Satellit und Empf¨ anger und Pr ist die Empfangsleistung. Die Sendeleistung Pt des C/A-Codes auf L1 betr¨agt 478.63W. H¨aufig gibt man die Leistung auch in [dBW] an, die sich aus der Leistung in Watt wie folgt berechnet: P [dBW] = 10 log10

P [W] , 1[W]

1

P [W] = 10 10 P [dBW]

(4.11)

Damit erh¨ alt man ˆ . Pt = 478.53W=26.8dBW

(4.12)

Der gr¨ oßte Abstand zwischen Satellit und Empf¨anger auf der Erdoberfl¨ache betr¨agt ca. 25785 km, so dass man mit der Wellenl¨ ange des C/A-Codes von 19 cm f¨ ur die minimale Leistung des empfangenen Signals Pr = 1.610−16W = ˆ − 157.8 dBW

(4.13)

erh¨ alt. Die maximale Empfangsleistung beim minimalen Abstand liegt um 2.1 dB h¨oher. Die Freiraumd¨ ampfung betr¨ agt also ungef¨ ahr ∆PF reiraum = Pr − Pt = −184.6 dBW .

(4.14)

Zus¨ atzlich treten noch athmosph¨ arische Verluste und Polarisationsverluste auf, die hier nicht ber¨ ucksichtigt sind. Die in [5] spezifizierte Empfangsleistung des C/A-Codes auf L1 liegt bei −160 dBW, die tats¨ achlichen Signale der Satelliten sind jedoch um 5 − 6 dBW st¨ arker als spezifiziert [23].

90

4 Satellitennavigation

Empfangssystem Die Rauschleistung des thermischen Rauschens im Empfangssystem ist gegeben durch N [W] = k · T0 · B ,

(4.15)

wobei T0 [K] die Temperatur in Kelvin bezeichnet, k = 1.380610−23[ Ws/K] ist die Boltzmannkonstante und B[Hz] ist die relevante Bandbreite, im Falle des C/A-Codes also 2 MHz. Bei einer Temperatur von 290 K erh¨alt man damit N [dBW] = 10 · log10 (k · T0 · B) = −141.0 dBW

(4.16)

H¨ aufig wird die Rauschleistung auch auf die Bandbreite bezogen angegeben: N0 [W/Hz] =

N = k · T0 B

(4.17)

Mit den obigen Zahlenwerten erh¨ alt man N0 [dBW/Hz] = 10 · log10 (k · T0 ) = −204.0 dBW/Hz .

(4.18)

Die Tr¨ agerleistung nach der AD-Wandlung des empfangenen Signals ergibt sich aus der arkung Ga durch die Antenne, der Verluste durch das Empfangsleistung Pr , der Verst¨ Rauschen des Vorverst¨ arkers Dsys und der Verluste durch die AD-Wandlung DAD zu C[W] =

Pr · Ga . Dsys · DAD

Mit einem Antennen-Gewinn von Ga = 1 = ˆ 0 dB Pr = −160[dBW], Dsys = 4 dB, DAD = 2 dB erh¨alt man

(4.19) und

Pr · Ga C[dBW] = 10 log10 Dsys · DAD = Pr [dBW] + Ga [dB] − Dsys [dB] − DAD [dB] = −166 dBW .

den

Zahlenwerten



(4.20)

Zur Beurteilung des empfangenen Signals bildet man das Verh¨altnis von Tr¨agerleistung zu Rauschleistung und erh¨ alt so C [dBW] = C[dBW] − N [dBW] = −25 dBW , N

(4.21)

beziehungsweise auf die Bandbreite bezogen C [dBHz] = C[dBW] − N0 [dBW/Hz] = 38 dBHz . N0

(4.22)

Am Empf¨ anger u ¨ berlagern sich die Signale der sichtbaren Satelliten. Werden n Sa¨ tellitensignale empfangen, verschlechtert sich durch diese Uberlagerung das SignalRauschverh¨ altnis f¨ ur das Signal eines bestimmten Satelliten um ca. (n − 1) · 2 dB.

4.2 Funktionsprinzip eines GPS-Empf¨ angers

91

Mathematische Beschreibung des empfangenen Signals Die empfangenen Signale sind gegen¨ uber den am Satelliten abgestrahlten Signalen um die Signallaufzeit τL1 bzw. τL2 verz¨ ogert. Da der Empf¨anger die exakte Zeit nicht kennt, erscheint von der empf¨ angerinternen Zeitskala aus betrachtet das Empfangssignal zus¨ atzlich verschoben. Diese Verschiebung entspricht gerade dem Fehler δtU der empf¨angerinternen Uhr. Unter Ber¨ ucksichtigung der Dopplerverschiebung erh¨alt man damit aus (4.3), (4.4) mit mL1 =

fL1 + ∆fL1 , fL1

mL2 =

fL2 + ∆fL2 , fL2

usw.

(4.23)

f¨ ur die von einem Satelliten empfangenen Signale sr,L1 (t) und sr,L2 (t) die vereinfachte mathematische Beschreibung   sr,L1 (t) = ar,CA · c mC/A t − τL1 − δtU · d mC/A t − τL1 − δtU  · sin 2πfL1 (mL1 t − τL1 − δtU )   +ar,Y 1 · y mY t − τL1 − δtU · d mY t − τL1 − δtU  · cos 2πfL1 (mL1 t − τL1 − δtU ) + nL1 (t)

(4.24)

  sr,L2 (t) = ar,Y 2 · y mY t − τL2 − δtU · d mY t − τL2 − δtU  · cos 2πfL2 (mL2 t − τL2 − δtU ) + nL2 (t) .

(4.25)

Die Bezeichnungen sind hierbei in Analogie zu Gl. (4.3) und (4.4) gew¨ahlt, nL1 (t) und nL2 (t) sind Rauschterme. Mit den Abk¨ urzungen Φ(t) = 2π∆fL1 t − 2πfL1 (τL1 + δtU ) ∆fC/A
= τL1 + δtU − t τL1 fC/A ∆fY

τL1 = τL1 + δtU − t fY

(4.26) (4.27) (4.28)

kann man das L1-Empfangssignal alternativ auch wie folgt darstellen:      

sr,L1 (t) = ar,CA · c t − τL1 · d t − τL1 · sin 2πfL1 t + Φ(t)      



+ar,Y 1 · y t − τL1 · d t − τL1 · cos 2πfL1 t + Φ(t) + nL1 (t) (4.29)

92

4 Satellitennavigation 1.2 1

AKF

0.8 0.6 0.4 0.2 0 -0.2 -400

-200

0 200 Verschiebung (Chips)

400

Abbildung 4.2: Autokorrelation eines C/A-Codes.

4.2.2

Akquisition

Nach dem Einschalten oder wenn l¨ angere Zeit keine GPS-Signale empfangen werden konnten, f¨ uhrt ein GPS-Empf¨ anger eine Suche nach vorhandenen Satellitensignalen durch, die Akquisition genannt wird. Entscheidend ist, ob g¨ ultige Almanach-Daten vorhanden sind. Almanach-Daten enthalten Satellitenbahninformationen, allerdings weniger genau als die Ephemeriden-Daten7 . Sofern der Empf¨anger zus¨atzlich Zeit und Position ungef¨ ahr kennt, kann anhand der Almanach-Daten auf die theoretisch sichtbaren Satelliten geschlossen werden. Da Signale nicht sichtbarer Satelliten aus der Suche ausgeschlossen werden k¨ onnen, wird die f¨ ur die Akquisition ben¨otigte Zeit erheblich verk¨ urzt, man spricht von einem Warmstart. Fehlen diese Sichtbarkeitsinformationen, muss zun¨ achst nach den Signalen aller Satelliten gesucht werden, was als Kaltstart bezeichnet wird. Ziel der Akquisitionsphase ist, die vorhandenen Satellitensignale, ihre chipweise Verschiebung und ihre dopplerverschobenen Tr¨agerfrequenzen zu bestimmen. Hierzu werden die Korrelationseigenschaften der C/A-Codes8 ausgenutzt: Die Autokorrelationsfunktion verschwindet f¨ ur Chip-Verschiebungen ungleich null nahezu vollst¨andig, siehe Abb. 4.2. Zus¨ atzlich verschwindet die Kreuzkorrelationsfunktion zweier verschiedener C/A-Codes, siehe Abb. 4.3. In der Akquisitionsphase wird das empfangene, herunter7 Ein GPS-Satellit sendet die eigenen Ephemeriden-Daten und die Almanach-Daten aller Satelliten. Selbst wenn nur ein Satellit empfangen werden kann, sind daher nach einiger Zeit die Almanach-Daten aller Satelliten bekannt. 8 Eine direkte Akqusition des P-Codes ist nur mit speziellen Empf¨ angern m¨ oglich.

4.2 Funktionsprinzip eines GPS-Empf¨ angers

93

1.2 1 0.8 KKF

0.6 0.4 0.2 0 -0.2 -400

-200

0 200 Verschiebung (Chips)

400

Abbildung 4.3: Kreuzkorrelation zweier verschiedener C/A-Codes.

gemischte, AD-gewandelte Empfangssignal mit einer vermuteten Tr¨agerfrequenz demoduliert und mit im Empf¨ anger erzeugten Replica-C/A-Codes korreliert. Nur wenn der entsprechende C/A-Code im Empfangssignal vorhanden ist, die Verschiebung des Replica-Codes damit in etwa u ¨ bereinstimmt und auch die vermutete Tr¨agerfrequenz nicht zu weit von der tats¨ achlichen Tr¨ agerfrequenz entfernt ist, liefert die Korrelation ¨ ein Ergebnis oberhalb des Rauschlevels. Ublicherweise wird ausgehend von der nominalen Tr¨ agerfrequenz der Replica-Code so lange verschoben, bis entweder das Korrelationsergebnis u ¨ ber einem Schwellwert liegt oder alle 1023 m¨oglichen Verschiebungen betrachtet worden sind. Wurde der Schwellwert nicht u ¨ berschritten, ver¨andert man die Frequenz einen Schritt nach oben.9 Wird auch bei dieser Frequenz kein Signal gefunden, bewegt man sich zwei Frequenzschritte nach unten, dann drei Schritte nach oben usw., siehe Abb. 4.4. Der Abstand der betrachteten Frequenzen kann dabei einige zehn bis einige hundert Hertz betragen. Um bei dem Vergleich des Korrelationsergebnisses mit dem Schwellwert die Falschalarmrate zu minimieren, werden so genannte Detektoren verwendet. Hierbei ist zwischen Detektoren, die nach einer festgelegten Zeit ein Ergebnis liefern, und Detektoren, deren Zeitbedarf bis zur Entscheidung von der Empfangssituation abh¨angt, zu unterscheiden. Zur ersten Kategorie z¨ ahlt z.B. der m-von-n-Detektor, der bei schlechtem Signal-Rauschverh¨ altnis Vorteile bietet. Bei gutem Signal-Rauschverh¨altnis ist dieser Detektor aber langsamer als z.B. der Tong-Detektor, der zur zweiten Kategorie z¨ ahlt. Die Grundidee des m-von-n-Detektors besteht darin, dass zu einem Punkt im 9 Man

spricht in diesem Zusammenhang von Frequency Bins.

94

4 Satellitennavigation

Frequenz

m¨ogliches Akquisitionsergebnis

Reihenfolge betrachteter Frequenzen

Verschiebung 1023 Chips

Abbildung 4.4: Suchraum bei der Akquisition.

Suchraum f¨ ur n Samples des empfangenen Signals ein Korrelationsergebnis berechnet wird. Wird dabei mindestens m-mal der festgelegt Schwellwert u ¨ berschritten, wird diese Verschiebungs-Frequenz-Kombination akzeptiert, andernfalls wird sie abgelehnt. Beim Tong-Detektor wird ein zu Beginn initialisierter Z¨ahler inkrementiert, wenn das Korrelationsergebnis eines Signal-Samples einen Schwellwert u ¨ berschreitet, andernfalls wird der Z¨ ahler dekrementiert. Sinkt der Z¨ ahlerstand auf null, wird die Verschiebungs-FrequenzKombination abgelehnt. Erreicht der Z¨ ahler einen bestimmten Grenzwert, gilt die Akquisition als erfolgreich. Es existieren eine Vielzahl von Modifikationen und Verfeinerungen dieser beschriebenen Detektoren. Betrachtungen in gr¨ oßerer Tiefe sind z.B. in [65] und [77] zu finden. War die Akquisition erfolgreich und Chip-Verschiebung und Tr¨agerfrequenz sind n¨aherungsweise bekannt, geht der Empf¨ anger f¨ ur diesen Satelliten in die Tracking-Phase u ¨ ber.

4.2.3

Tracking

Es gibt die unterschiedlichsten Ans¨ atze, um ein Tracking von Satellitensignalen zu realisieren. Die prinzipielle Funktionsweise eines m¨oglichen Ansatzes wird im Folgenden beschrieben, es wird nur der C/A-Code betrachtet.

4.2 Funktionsprinzip eines GPS-Empf¨ angers

95

C/A-Code

δts

synchron

δtns

nicht synchron

δts

δtns

Abbildung 4.5: Wahl der Abtastrate.

Das von der GPS-Antenne empfangene Signal wird zun¨achst verst¨arkt, gefiltert, auf eine Zwischenfrequenz von 21.25 MHz heruntergemischt und normalisiert. Die Normalisierung – auch als Automatic Gain Control (AGC) bezeichnet – dient dazu, bei der anschließenden A/D-Wandlung den A/D-Wandler optimal auszunutzen. Bei der A/DWandlung entsteht durch Unterabtastung ein digitaler Datenstrom mit einer Datenrate von 5 MHz. Diese 5 MHz sind bewusst kein Vielfaches der C/A-Code-Chiprate von 1.023 MHz: W¨ urde die A/D-Wandlung mit einem Vielfachen der C/A-Code-Chiprate erfolgen, k¨ onnte eine Verschiebung δts des Signals, die kleiner als die Abtastzeit ist, nicht detektiert werden, da eine solche Verschiebung den digitalen Datenstrom nicht beeinflusst, siehe Abb. 4.5 oben. Erfolgt die Abtastung jedoch nicht synchronisiert, f¨ uhrt auch eine Verschiebung δtns des Signals um weniger als die Abtastzeit nach einiger Zeit zu einer Ver¨ anderung des digitalen Datenstroms und ist damit prinzipiell erkennbar, siehe Abb. 4.5 unten. Die Verarbeitung des so gewonnenen digitalen Datenstroms ist in Abb. 4.6 schematisch dargestellt. Inphasen- und Quadraturkomponente Zun¨ achst wird das Signal in Inphasen- und Quadraturkomponente zerlegt. Die InphasenKomponente erh¨ alt man durch Multiplikation des Datenstroms mit einer um die geˆ sch¨ atzte Phasenverschiebung Φ(t) verschobenen Sinus-Schwingung. Die Frequenz der

96

4 Satellitennavigation

Sinus-Schwingung ist gerade die heruntergemischte Tr¨agerfrequenz des Satellitensignals, in der hier beschriebenen Konfiguration 1.25 MHz. Betrachtet man nur den C/A-Code, so erh¨ alt man mit Gl. (4.29) f¨ ur die Inphasen-Komponente   ˆ I(t) = sr,L1 (t) · sin 2πfL1 t + Φ(t)        

ˆ · d t − τL1 · sin 2πfL1 t + Φ(t) · sin 2πfL1 t + Φ(t) = c t − τL1    1 

= − c t − τL1 · d t − τL1 2      ˆ ˆ · cos 4πfL1 t + Φ(t) + Φ(t) − cos Φ(t) − Φ(t) .

(4.30)

Das Rauschen des Empfangssignals wurde hierbei der Einfachheit halber vernachl¨assigt. Eliminiert man den h¨ oherfrequenten Anteil durch Tiefpassfilterung, erh¨alt man schließlich I(t) =

     1 

ˆ c t − τL1 · d t − τL1 cos Φ(t) − Φ(t) . 2

(4.31)

In anloger Weise findet man durch Multiplikation mit einer um die gesch¨atzte Phasenverschiebung verschobenen Kosinus-Schwingung die Quadratur-Komponente zu Q(t) =

     1 

ˆ c t − τL1 · d t − τL1 sin Φ(t) − Φ(t) . 2

(4.32)

Stimmt die gesch¨ atzte Phasenverschiebung mit der Phasenverschiebung des empfangenen Signals u ¨ berein, verschwindet die Quadratur-Komponente und die InphasenKomponente besteht nur aus C/A-Code und Navigationsdaten: I(t) =

   1 

c t − τL1 · d t − τL1 2

Q(t) = 0

(4.33) (4.34)

Nun werden die beiden Signale aus Gl. (4.31) und Gl. (4.32) mit drei vom Empf¨ anger erzeugten Replica C/A-Codes korreliert, early (E), prompt (P) und late (L). Der prompt Code wird gerade mit der gesch¨atzten Verschiebung τˆ
des empfangenen C/A-Codes generiert, die early und late Codes sind um die Dauer eines halben C/ACode-Chips verfr¨ uht bzw. versp¨ atet. Betrachtet man 20 ms lange Signal-Samples10, entstehen durch das Aufsummieren der jeweiligen, mit 5 MHz anfallenden Korrelationsergebnisse insgesamt sechs Signale mit einer Datenrate von 50 Hz: IE , IP , IL , QE , QP , QL . Diese Signale dienen zur Bestimmung der Fehler der gesch¨atzten Verˆ schiebung τˆ
und der Tr¨ agerphase Φ(t). Zur Veranschaulichung der Bedeutung der 10 Die Navigationsdatenbits k¨ onnen sich alle 20 ms ¨ andern. Idealerweise werden die 20 ms Signal¨ Samples so mit dem empfangenen Signal synchronisiert, dass innerhalb eines Samples keine Anderung des Navigationsdatenbits erfolgen kann.

97

Filter

4.2 Funktionsprinzip eines GPS-Empf¨ angers

Diskriminator Tr¨ ager

QL

QP

ADC Eingangsstufe

Antenne

sin

I

Replica Tr¨ager

cos

Q

E

Filter

P

Replica Code

L

$

QE $

$

IL $

IP $

$

IE

Diskriminator Code

Abbildung 4.6: Generisches Empf¨ anger Blockdiagramm.

98

4 Satellitennavigation

Korrelationsergebnisse wird in Abb. 4.7 ein vereinfachtes Beispiel betrachtet. Auf der linken Seite sind die Ergebnisse der normalisierten Korrelationen eines C/A-Codes mit early, prompt und late Replica C/A-Codes f¨ ur den Fall dargestellt, dass die gesch¨ atzte Verschiebung des C/A-Codes mit der tats¨achlichen Verschiebung u ¨ bereinstimmt. Die Korrelation des prompt Codes liefert den gr¨oßten Beitrag, die Amplitude der early und late Korrelationsergebnisse sind halb so groß. Ist der prompt Replica Code um einen viertel Chip zu fr¨ uh, resultieren die auf der rechten Seite von Abb. 4.7 dargestellten Verh¨ altnisse: Early und promt Codes liefern identische Korrelationsergebnisse, die Amplitude des Korrelationsergebnisses des late Codes ist am geringsten. Delay Lock Loop (DLL) Die Bestimmung des Fehlers δˆ τ
der gesch¨atzten Verschiebung erfolgt mit einem Diskriminator. Es gibt eine Vielzahl von Diskriminatoren, die sich in Leistungsf¨ahigkeit und Rechenzeitbedarf unterscheiden. Ein m¨ oglicher Diskriminator ist gegeben durch $ 2 $ 2 IE + Q2E − I + Q2L 1  L . δˆ τ = $ 2 $ 2 IE + Q2E + IL2 + Q2L


(4.35)

Durch die Addition der Quadrate der jeweiligen Inphasen- und Quadraturkomponente entf¨ allt die St¨ orung durch eine fehlerhaft gesch¨atzte Tr¨agerphase: Der Sinus des Tr¨agerphasensch¨ atzfehlers geht multiplikativ in die Amplituden der Inphasen-Komponenten ein, dessen Kosinus in die Quadraturkomponenten, siehe Gl. (4.31) und Gl. (4.32). Addiert man die Quadrate der Inphasen- und Quadraturkomponente, ist die Amplitude des resultierenden Signals aufgrund von  2  2 ˆ ˆ sin Φ(t) − Φ(t) + cos Φ(t) − Φ(t) =1

(4.36)

vom Tr¨ agerphasensch¨ atzfehler unabh¨ angig. Durch den Nenner in Gl. (4.35) wird eine Beeinflussung des Diskriminatorergebnisses durch die absoluten Signalamplituden verhindert, lediglich die relativen Amplituden von early und late spielen eine Rolle. Das Diskriminatorergebnis wird gefiltert und zur Korrektur der gesch¨atzten Verschieur die Verarbung τˆ
verwendet, die dann Grundlage der Erzeugung der Replica-Codes f¨ beitung des n¨ achsten 20-ms-Signal-Samples ist. Diese Regelschleife wird als Delay Lock Loop (DLL) bezeichnet. Phase Lock Loop (PLL) Zur Bestimmung des Sch¨ atzfehlers der Tr¨agerphase werden die Signale IP und QP herangezogen. Auch hier gibt es eine Vielzahl unterschiedlicher Diskriminatoren. Ein leistungsf¨ ahiger, wenn auch rechenaufw¨ andiger Diskriminator ist durch ˆ = arctan δΦ

QP IP

(4.37)

4.3 GPS-Beobachtungsgr¨ oßen

99

Empfangssignal Code Chip

Empfangssignal Code Chip

Replica Codes

Replica Codes

early

early

prompt

prompt

late

late

Korrelationsergebnis 1 1 2

Korrelationsergebnis P

E −1 − 21

1 L

0

1 2

1 2

1

E

P L

−1 − 21

0

1 2

1

Abbildung 4.7: Korrelation mit Replica Codes.

gegeben, was gerade dem Maximum-Likelyhood-Sch¨atzwert des Tr¨agerphasensch¨atzfehlers entspricht. Da in QP der Kosinus und in IP der Sinus des Tr¨agerphasensch¨atzfehlers eingeht, ist dieser Ansatz unmittelbar einsichtig. Phasenspr¨ unge von 180 Grad aufgrund der Navigationsdaten beeinflussen aufgrund der Mehrdeutigkeit der ArcusTangens-Funktion das Ergebnis nicht. Das Diskriminator-Ergebnis wird gefiltert, der Tr¨ agerphasensch¨ atzwert damit korrigiert und f¨ ur die Gewinnung der Signale Gl. (4.31) und Gl. (4.32) aus dem n¨ achsten 20-ms-Signal-Sample verwendet. Diese Regelschleife wird als Costas Phase Lock Loop bezeichnet. Anstelle eines Costas PLL kann auch ein Frequency Lock Loop (FLL) eingesetzt werden. Da hier nur grundlegende Prinzipien der empf¨angerinternen Signalverarbeitung qualitativ beschrieben werden sollen, wird auf Frequency Lock Loops und die zur Filterung der Diskriminatorergebnisse notwendigen Filter nicht eingegangen. Ausf¨ uhrliche Beschreibungen der Funktionsweise eines GPS-Empf¨angers sind in [56], [23], [97] oder [65] zu finden.

4.3

GPS-Beobachtungsgr¨oßen

In diesem Abschnitt stehen die vom GPS-Empf¨anger gelieferten Messwerte und ihr Zusammenhang mit Position und Geschwindigkeit der Antenne des GPS-Empf¨angers im Vordergrund. Die folgenden Betrachtungen sind dabei bewusst so aufbereitet, dass

100

4 Satellitennavigation

ein Verst¨ andnis der in Abschnitt 4.2 skizzierten, empf¨angerinternen Signalverarbeitung nicht erforderlich ist.

4.3.1

Pseudorange

Die Pseudorange-Messung wird aus der zeitlichen Verschiebung zwischen dem empf¨angerinternen Replica-Code und dem empfangenen Satellitensignal gewonnen. Allerdings ist die zeitliche Verschiebung dieser beiden Signale nicht nur von der Laufzeit des Satellitensignals und somit vom Abstand zwischen Satellit und Empf¨anger abh¨angig, sondern auch vom Fehler der empf¨ angerinternen Uhr. Dieser Uhrenfehler geht daher in die nichtlineare Pseudorange-Messgleichung ein: ρ˜ i = c g,i · τˆ
i = |r Si − r A | + cδt U + eρ,i

(4.38)

Hierbei bezeichnet ρ˜ i c g,i τˆ
i r Si r A c δt U eρ,i

Pseudorange zum Satelliten i Gruppengeschwindigkeit des Satellitensignals i Gemessene zeitliche Verschiebung zwischen empf¨angerinternem Referenzsignal und Satellitensignal i Position des Satelliten i zum Sendezeitpunkt Position der GPS-Antenne zum Empfangszeitpunkt Lichtgeschwindigkeit Uhrenfehler des GPS-Empf¨ angers zum Empfangszeitpunkt Messfehler der Pseudorange-Messung zum Satelliten i.

Werden die Pseudorange-Messgleichungen (4.38) f¨ ur jeden zu einem Zeitpunkt empfangenen Pseudorange aufgestellt, so entsteht ein nichtlineares Gleichungssystem. Liegen mindestens vier Pseudoranges vor, kann dieses Gleichungssystem iterativ nach den vier Unbekannten der Antennenposition (r A,x , r A,y , r A,z )T und Empf¨angeruhrenfehler δt U aufgel¨ ost werden. Dazu wird das nichtlineare Gleichungssystem ⎛

⎞ ⎞ ⎛ ⎞ ⎛ ρ˜ 1 h1 (x) eρ,1 ⎜ ρ˜ 2 ⎟ ⎜ h2 (x) ⎟ ⎜ eρ,2 ⎟ ⎜ . ⎟ = ⎜ . ⎟+⎜ . ⎟ ⎝ . ⎠ ⎝ . ⎠ ⎝ . ⎠ . . . hn (x) ρ˜ n eρ,n % &' ( % &' ( % &' ( ˜ ρ 

 h( x)

(4.39)

 eρ

mit x = (r A,x , r A,y , r A,z , δt U )T

(4.40)

in eine Taylor-Reihe um die gesch¨ atzte Antennenposition und den gesch¨atzten Uhrenfehler entwickelt: ˆ) + H(x − x ˆ) + h.o.t. ρ ˜ = h(x

(4.41)

4.3 GPS-Beobachtungsgr¨ oßen

101

¨ Der Vektor der Messfehler wurde hierbei aus Gr¨ unden der Ubersichtlichkeit weggelassen. Die Matrix H ist die Jacobi-Matrix von h(x): ⎛ ∂h1 ∂h1 ∂rA,x ∂rA,y  ⎜ ∂h2 ∂h2 ⎜ ∂rA,x ∂rA,y ∂h(x)  H= =⎜  .. ⎜ .. ∂x  ˆ ⎝ . .  x= xk ∂hn ∂hn ∂rA,x ∂rA,y

∂h1 ∂rA,z ∂h2 ∂rA,z

∂h1 ∂cδtU ∂h2 ∂cδtU

∂hn ∂rA,z

∂hn ∂cδtU

.. .

.. .

⎞   ⎟ ⎟ ⎟ ⎟ ⎠  

(4.42)

ˆk x=  x

Die Komponenten der Matrix H ergeben sich mit ) |rSi − rA | = (rSi ,x − rA,x )2 + (rSi ,y − rA,y )2 + (rSi ,z − rA,z )2

(4.43)

und ∂hi −(rSi ,x − rA,x ) =  2 ∂rA,x (rSi ,x − rA,x ) + (rSi ,y − rA,y )2 + (rSi ,z − rA,z )2 −(rSi ,y − rA,y ) ∂hi =  ∂rA,y (rSi ,x − rA,x )2 + (rSi ,y − rA,y )2 + (rSi ,z − rA,z )2 ∂hi −(rSi ,z − rA,z ) =  2 ∂rA,z (rSi ,x − rA,x ) + (rSi ,y − rA,y )2 + (rSi ,z − rA,z )2

(4.44) (4.45) (4.46)

sowie eSi

⎛ ⎞ eSi ,x rSi ,x − rA,x = ⎝eSi ,y ⎠ = |rSi ,x − rA,x | e

(4.47)

Si ,z

zu ⎛

−eS1 ,x −eS1 ,y −eS1 ,z −e ⎜ S2 ,x −eS2 ,y −eS2 ,z H= ⎜ .. .. ⎝ .. . . . −eSn ,x −eSn ,y −eSn ,z

⎞ 1   1⎟ ⎟ , .. ⎠ .   1 x=xˆ

(4.48)

wobei eSi den Einheitsvektor bezeichnet, der von der Antennenposition zum i-ten Satelliten weist. Mit ˆ), ∆ ρ=ρ ˜ − h(x

ˆ ∆x = x − x

(4.49)

l¨ asst sich Gl. (4.41) umstellen, wobei Terme h¨oherer Ordnung vernachl¨assigt werden: ∆ ρ = H∆x

(4.50)

102

4 Satellitennavigation

Die Least-Squares-L¨ osung von Gl.(4.50) ist gegeben durch ρ. ∆x = (HT H)−1 HT ∆

(4.51)

Sind Informationen u ute der einzelnen Pseudorange-Messungen vorhanden, ¨ ber die G¨ k¨ onnen diese ber¨ ucksichtigt werden, indem die Weighted-Least-Squares-L¨osung berechnet wird. Mit diesem Korrekturvektor kann die Sch¨ atzung von Antennenposition und Uhrenfehler verbessert werden: ˆ + ∆x xˆ := x

(4.52)

Damit beginnt der n¨ achste Iterationsschritt, die verbesserte Sch¨atzung wird zur Berechnung einer verbesserten Jacobi-Matrix H nach Gl. (4.48) verwendet, diese f¨ uhrt zu einem neuen Korrekturvektor Gl. (4.51), der wieder zur Verbesserung der Positionsund Uhrenfehlersch¨ atzung (4.52) genutzt wird. Die Gleichungen (4.48), (4.51) und (4.52) ¨ werden in der beschriebenen Weise iteriert, bis die Anderung der Sch¨atzwerte in einem Iterationsschritt ein zuvor definiertes Maß unterschreitet. Diese Vorgehensweise zur Berechnung der Antennenposition aus den Pseudoranges wird auch als Snap-Shot-Algorithmus bezeichnet, da nur die zum aktuellen Zeitpunkt vorliegenden Pseudoranges ber¨ ucksichtigt werden. H¨aufig werden bei der Ermittlung der Empf¨ angerposition aus Pseudorange-Messungen aber auch Pseudorange-Messungen zu fr¨ uheren Zeitpunkten mit einbezogen. Durch eine solche Filterung lassen sich zwar h¨oherfrequente Fehler reduzieren, allerdings wird dies durch zus¨atzliche Zeitkorrelationen in den Positionsfehlern und eine zus¨ atzliche Verz¨ogerung in der Positionsl¨osung erkauft. Anhand von Gl. (4.51) k¨ onnen auch prinzipielle Aussagen u ute der Positions¨ ber die G¨ bestimmung getroffen werden. Geht man davon aus, dass jede Pseudorange-Messung mit einem Messrauschen der gleichen Varianz σρ2 behaftet ist, so erh¨alt man f¨ ur die Kovarianz von Position und Uhrenfehler Px : T  Px = (HT H)−1 HT · σρ2 I · (HT H)−1 HT = (HT H)−1 HT · H(HT H)−1,T · σρ2 = (HT H)−1,T σρ2 .

(4.53)

Die Hauptdiagonale der Matrix VT = (HT H)−1,T bzw. V = (HT H)−1 bestimmt folglich, wie sich das Messrauschen der Pseudoranges auf die Varianz von Position und Uhrenfehler auswirkt. Diese Matrix wird daher zur Definition einiger Kenngr¨oßen herangezogen, der DOP-Faktoren11. Hierbei handelt es sich um Indikatoren, die angeben, ob bei der vorliegenden Satellitenkonstellation mit guten oder schlechten Ergebnissen bei der Bestimmung von Position und Uhrenfehler zu rechnen ist. Folgende DOP-Faktoren 11 DOP

steht f¨ ur Dilution of Precision“. ”

4.3 GPS-Beobachtungsgr¨ oßen

103

cδt U

cδt U

ρ˜ 2 ρ˜ 1 ρ˜ 3

cδt U

Empf¨ angerposition Abbildung 4.8: Geometrische Veranschaulichung der Positionsbestimmung mit PseudorangeMessungen im zweidimensionalen Fall.

sind u ¨ blich: VDOP = HDOP =

 V(3, 3) vertical DOP

 V(1, 1) + V(2, 2) horizontal DOP

 PDOP = V(1, 1) + V(2, 2) + V(3, 3) position DOP  GDOP = V(1, 1) + V(2, 2) + V(3, 3) + V(4, 4) geometric DOP

Je kleiner diese DOP-Faktoren ausfallen, desto weniger werden Pseudorange-Messfehler bei der Berechnung von Position und Uhrenfehler verst¨arkt. In Abb. 4.8 ist die Positionsbestimmung mit Pseudorange-Messungen f¨ ur den zweidimensionalen Fall geometrisch veranschaulicht. Jede Pseudorange-Messung definiert den Radius einer Kugelschale um den zugeh¨ origen Satellit. Korrigiert man die Radien aller Kugelschalen um den gleichen Betrag, so dass sich alle Kugelschalen in einem einzigen

104

4 Satellitennavigation

Punkt schneiden, ist mit diesem Schnittpunkt die Position des GPS-Empf¨angers gefunden. Der Betrag, um den die Radien der Kugelschalen korrigiert wurden, entspricht dem Empf¨ angeruhrenfehler multipliziert mit der Lichtgeschwindigkeit.

4.3.2

Tra¨gerphasenmessung

Neben der Messung der Signallaufzeit anhand der zeitlichen Verschiebung von Pseudo¨ zufallsfolgen ist ein GPS-Empf¨ anger in der Lage, die Anderung der Phasenverschiebung zwischen dem Tr¨ ager eines empfangenen Satellitensignals und einem empf¨angerintern erzeugten Referenztr¨ agers zu messen. Die so entstehende Messgr¨oße wird als Tr¨agerphasenmessung bezeichnet, die zugeh¨ orige Messgleichung wird im Folgenden abgeleitet. Die GPS-Satelliten senden Signale auf den Tr¨agerfrequenzen L1 und L2 aus, stellvertretend hierf¨ ur wird eine Sendefrequenz fc eingef¨ uhrt. Da sich die GPS-Satelliten gegenu anger bewegen, stimmt die vom Empf¨anger festgestellte Frequenz ¨ ber dem GPS-Empf¨ fc
eines empfangenen Signals aufgrund des Dopplereffekts nicht mit der Sendefrequenz fc u ¨ berein:  vrel,eS,i
(4.54) = fc · 1 − fc,i c Hierbei bezeichnet vrel,eS,i die Projektion der Relativgeschwindigkeit vrel,i von Satellit i und Empf¨ anger auf eine Gerade, die Empf¨anger und Satellit verbindet. Sie ist bei Ann¨ aherung negativ und bei Vergr¨ oßerung der Distanz positiv, siehe Abb. 4.9. Die skalare Relativgeschwindigkeit vrel,eS,i ist gerade die Ableitung des Abstandes von Empf¨anger und Satellit nach der Zeit. Damit liefert die zeitliche Integration von Gl. (4.54) als Phase12 ϕS,i des empfangenen Satellitensignals

|r Si − r A | ϕS,i = fc · t − . (4.55) c Die Frequenz eines empf¨ angerintern erzeugten Referenztr¨agers h¨angt von der Drift δ t˙ U der empf¨ angerinternen Uhr ab.   (4.56) fE = fc · 1 + δ t˙ U agers folgt daher F¨ ur die Phase ϕE des Referenztr¨ ϕE = fc · (t + δt U )

.

(4.57)

Die Differenz ϕ i der beiden Phasen ϕE und ϕSr,i wird in einem so genannten Phase Lock Loop (PLL) bestimmt und ergibt sich mit c = fc λc zu ϕ i = ϕE − ϕSr,i = 12 Es

|r Si − r A | cδt U + λc λc

ist u ¨blich, hier die Phase in cycles anzugeben.

.

(4.58)

4.3 GPS-Beobachtungsgr¨ oßen

105

vS

−vA

rS

vr

el

vrel,eS

vS

eS rA

Empf¨angerposition vA

Abbildung 4.9: Relativgeschwindigkeit von Satellit und GPS-Empf¨ anger.

Allerdings ist zu beachten, dass zu dem Zeitpunkt, zu dem der Satellit zum ersten Mal sichtbar ist, nur der gebrochene Anteil der Tr¨agerphasendifferenz gemessen werden kann. Die absolute Anzahl der Wellenl¨ angen zwischen Satellit und Empf¨anger ist unbekannt und wird als Tr¨ agerphasenmehrdeutigkeitswert (engl. ambiguity) N bezeichnet. Damit ergibt sich die Messgleichung einer Tr¨ agerphasenmessung, die wie die PseudorangeMessgleichung ebenfalls nichtlinear ist, zu ϕ˜ i (t) = ϕ i (t) − ϕ i (t0 ) |r Si (t) − r A (t)| cδt U (t) eϕ,i (t) + + = λc λc λc * + |r Si (t0 ) − r A (t0 )| cδt U (t0 ) eϕ,i (t0 ) − + + λc λc λc |r Si (t) − r A (t)| cδt U (t) eϕ,i (t) ϕ˜ i (t) = + + −N . (4.59) λc λc λc Der Messfehler der Tr¨ agerphasenmessung ist mit eϕ,i bezeichnet. Aufgrund des unbekannten Mehrdeutigkeitswertes N kann Gl. (4.59) nicht unmittelbar f¨ ur eine Positions-

106

4 Satellitennavigation

cδtU (t1 ) − cδtU (t0 )

ϕi (t0 )λc

cδtU (t2 ) − cδtU (t0 )

ϕi (t1 )λc ϕi (t2 )λc

N λc N λc N λc

Empf¨ angerposition

Abbildung 4.10: Geometrische Veranschaulichung der Tr¨ agerphasenmessung.

bestimmung genutzt werden. Dieser Mehrdeutigkeitswert ist konstant, in ung¨ unstigen Empfangssituationen k¨ onnen jedoch auch als Cycle Slips bezeichnete Spr¨ unge auftreten. ¨ Bei der Tr¨ agerphasenmessung handelt es sich also um eine Messung der Anderung des ¨ Abstandes zwischen Empf¨ anger und Satellit, die um die Anderung des Uhrenfehlers seit dem Zeitpunkt, zu dem der Satellit sichtbar wurde, verf¨alscht ist. In Abb. 4.10 ist die Tr¨ agerphasenmessung geometrisch veranschaulicht. Es gibt mehrere M¨ oglichkeiten, die Tr¨ agerphasenmessung zu nutzen: Beim Carrier Aided Smoothing werden Tr¨ agerphasenmesswerte verwendet, um die zugeh¨origen Pseudorangemessungen zu gl¨ atten. F¨ ur hochgenaue Anwendungen wird der Mehrdeutigkeitswert N bestimmt, um die Tr¨ agerphasenmessung auf die gleiche Weise wie eine Pseudorangemessung nutzen zu k¨ onnen. Dies ist jedoch nur m¨oglich, wenn Differenzen von Tr¨agerphasenmessungen zu verschiedenen Satelliten gebildet werden, so dass sich der Uhrenfehler aus den Messgleichungen herausk¨ urzt. Zus¨atzlich m¨ ussen durch Korrekturdaten einer Referenzstation mit bekannter Position die Fehlerterme eϕ,i m¨oglichst eliminiert werden.

4.3.3

Deltarange

Neben der Tr¨ agerphasenmessung stellen viele GPS-Empf¨anger direkt eine Messung der Relativgeschwindigkeit von Satellit und Empf¨anger zur Verf¨ ugung, die als Deltarange-

4.3 GPS-Beobachtungsgr¨ oßen

107

oder Doppler-Geschwindigkeitsmessung bezeichnet wird. Diese wird aus der Verschiebung der Tr¨ agerfrequenz des empfangenen Satellitensignals gewonnen. Aus Gl. (4.54) folgt
fc,i − fc = −fc

vrel,eS,i = −λc vrel,eS,i c

(4.60)

Die Verschiebung der Tr¨ agerfrequenz muss dabei nat¨ urlich durch Vergleich mit der Frequenz des empf¨ angerintern erzeugten Referenztr¨agers ermittelt werden. 
 v˜rel,eS,i = −λc fc,i − fE + ev,i     vrel,eS,i − fc · 1 + δ t˙ U + ev,i = −λc fc · 1 − c

(4.61)

Hierbei bezeichnet ev,i den Fehler der Deltarange-Messung zum Satellit i. Mit c = fc ·λc folgt v˜rel,eS,i = vrel,eS,i + cδ t˙ U + ev,i

.

(4.62)

Anhand von Abb. 4.9 l¨ asst sich die skalare Relativgeschwindigkeit vrel,eS,i in Abh¨angigkeit von Satellitengeschwindigkeit v S,i und Empf¨angergeschwindigkeit v A formulieren. Die Messgleichung der Deltarange-Messung lautet damit v˜rel,eS,i = e TS,i (v S,i − v A ) + cδ t˙ U + ev,i

.

(4.63)

Durch Umstellen erh¨ alt man T v˜rel,eS,i − e TS,iv S,i = −e S,i v A + cδ t˙ U + ev,i

.

(4.64)

Mit den Deltarange-Messgleichung (4.63) zu verschiedenen Satelliten l¨asst sich ein lineares Gleichungssystem f¨ ur die drei unbekannten Kompenenten der Geschwindigkeit der GPS-Antenne und der unbekannten Uhrenfehlerdrift aufstellen. Sind zu einem Zeitpunkt vier Deltarange-Messwerte vorhanden, kann dieses lineare Gleichungssystem direkt gel¨ ost werden, bei mehr als vier Messwerten wird die Least-Squares-L¨osung verwendet. Da es sich in jedem Fall um ein lineares Gleichungssystem handelt, ist anders als bei der Berechnung der Antennenposition aus Pseudorange-Messungen eine iterative L¨ osung nicht notwendig. F¨ ur die erzielbare Genauigkeit bei einer Deltarange-Messung spielt neben dem SignalRauschverh¨ altnis des Tr¨ agers auch die Messzeit eine Rolle: Je l¨anger das zur Frequenzmessung betrachtete Zeitintervall ist, desto geringer ist der bei der Frequenzmessung resultierende Fehler. Dabei sollte sich allerdings die Frequenz und somit die Relativgeschwindigkeit zwischen Empf¨ anger und Satellit in diesem Zeitintervall nicht ¨andern. Daher k¨ onnen diese Zeitintervalle nicht beliebig lang gew¨ahlt werden, da ansonsten dy¨ namische Vorg¨ ange wie Anderungen der Geschwindigkeit oder Bewegungsrichtung des Empf¨ angers verf¨ alscht w¨ urden.

108

4 Satellitennavigation

4.3.4

Fehlerquellen

Die von einem GPS-Empf¨ anger gelieferten Pseudorange-, Tr¨agerphasen- und Deltarange-Messungen sind mit einer Reihe von Fehlern behaftet. Common-Mode-Fehler treten innerhalb einer bestimmten Region bei jedem Empf¨anger auf, wohingegen NoncommonMode-Fehler vom verwendeten GPS-Empf¨anger selbst oder der spezifischen Empfangssituation abh¨ angen. Zu den Common-Mode-Fehlern z¨ ahlen: • Ionosph¨ arenfehler: Die Ionosph¨ are erstreckt sich in H¨ohen zwischen 50 km und 1000 km und beeinflusst die Ausbreitung elektromagnetischer Wellen. W¨ahrend die Gruppengeschwindigkeit cg verringert wird, vergr¨oßert sich im Gegenzug die Phasengeschwindigkeit cp [119]: cg · cp = c2

(4.65)

Dies hat zur Folge, dass f¨ ur die Pseudorange-Messung zu große Werte und f¨ ur die Tr¨ agerphasenmessung zu kleine Werte resultieren. In erster N¨aherung gilt f¨ ur den Ionosph¨ arenfehler eIon eIon ∼

TEC fc2

.

(4.66)

Die Anzahl der Elektronen pro Quadratmeter TEC (Total Electron Count) ist von der Sonneneinstrahlung abh¨ angig, so dass sich tags¨ uber gr¨oßere Ionosph¨arenfehler ergeben als nachts. Da der Ionosph¨arenfehler frequenzabh¨angig ist, kann diese Fehlerquelle bei Zweifrequenz-Empf¨ angern eliminiert werden. • Troposph¨ arenfehler: Die Troposph¨are erstreckt sich von der Erdoberfl¨ache bis in 10 km H¨ ohe. Laufzeitverz¨ ogerungen h¨angen in erster Linie vom Luftdruck und der Luftfeuchtigkeit ab, eine Frequenzabh¨angigkeit besteht nicht. Der Troposph¨arenfehler ist außerdem vom Erhebungswinkel des Satelliten abh¨angig, da ein niedriger Erhebungswinkel mit einem langen Weg durch die Troposph¨are verbunden ist. • Ephemeridenfehler: Die Bahnen der GPS-Satelliten werden st¨andig von Bodenstationen vermessen und die so bestimmten Ephemeridendaten den Nutzern des GPS-Systems im Rahmen der Navigationsdaten zur Verf¨ ugung gestellt. Die Differenz zwischen angenommener und realer Satellitenbahn wird als Ephemeridenfehler bezeichnet. • Satellitenuhrenfehler: Mit den Navigationsdaten werden auch Korrekturdaten f¨ ur die Uhr des sendenden Satelliten u ¨ bertragen. Die nach der Korrektur der Satellitenuhr verbleibende Differenz zur GPS-Zeit stellt den Satellitenuhrenfehler dar.

4.3 GPS-Beobachtungsgr¨ oßen

109

Zu den Noncommon-Mode-Fehlern z¨ ahlen: • Mehrwegeausbreitung (engl. multipath): Ein Satellitensignal kann die Antenne des GPS-Empf¨ angers nicht nur auf direktem Wege erreichen, sondern auch ¨ nachdem es an Geb¨ auden, der Erdoberfl¨ache, Fahrzeugteilen oder Ahnlichem reflektiert wurde. Bei einer Pseudorange-Messung wird nun anstelle des direkten ¨ Satellitensignals diese Uberlagerung aus zwei oder mehreren Signalen mit dem Referenzsignal korreliert, was die Lage des Maximums der Korrelationsfunktion beeinflusst. Wie stark dieser Einfluss maximal werden kann, h¨angt von der verwendeten Korrelator-Technologie ab. Erreicht das reflektierte Signal mit der H¨alfte der Leistung des direkten Signals den Empf¨anger, kann der maximale PseudorangeFehler durch Mehrwegeausbreitung zwischen 70 m (Wide Correlator) und 5 m (Pulse Aperture Correlator) betragen [8]. In einer typischen Empfangssituation ist mit 0.1 m bis 3 m Fehler zu rechnen [26]. Bei einer Tr¨ agerphasenmessung hingegen kann der Fehler durch Mehrwegeausbreitung 1/4 der Wellenl¨ ange nicht u ¨ berschreiten, siehe [53]. Im Falle der L1Tr¨ agerfrequenz sind das lediglich 4.8 cm. • Empf¨ angeruhrenfehler: Die Differenz zwischen der empf¨angerinternen Uhr und der GPS-Zeit wird als Empf¨ angeruhrenfehler bezeichnet. Durch diese Zeitdifferenz resultiert ein Bias, der f¨ ur alle Pseudorange-Messungen zu einem Zeitpunkt identisch ist. Dieser Bias wird bei der Positionsbestimmung als vierte Unbekannte berechnet und beeinflusst daher die Genauigkeit der Positionsbestimmung nicht. • Empf¨ angerrauschen: Messfehler, die aus dem thermischen Rauschen und den Nichtlinearit¨ aten der elektronischen Komponenten des GPS-Empf¨angers resultieren, sind im Empf¨ angerrauschen zusammengefasst. Bei den heutzutage verf¨ ugbaren Empf¨ angern liegt die Standardabweichung dieses Fehlers bei einer Pseudorange-Messung in der Gr¨ oßenordnung einiger Zentimeter, bei einer Tr¨agerphasenmessung betr¨ agt sie weniger als 1 mm [8].

4.3.5

Differential GPS

Differential GPS (DGPS) ist ein Verfahren, mit dem die Fehler der von einem GPSEmpf¨ anger bestimmten Positionsl¨ osung drastisch reduziert werden k¨onnen. Dazu nimmt eine Referenzsstation Pseudorange- und Tr¨agerphasenmessungen vor. Da die Position der Referenzstation exakt bekannt ist, kann der bei jeder Messung aufgetretene Messfehler ermittelt werden. Diese Informationen werden einem GPS-Empf¨anger mit unbekannter Position zur Verf¨ ugung gestellt, zum Beispiel u ¨ ber eine Funkstrecke. Da die Common-Mode-Fehler den bei weitem gr¨oßten Anteil am Messfehler darstellen, korrigiert der GPS-Empf¨ anger mit diesen Informationen seine eigenen Messwerte. Satellitenuhrenfehler und Ephemeridenfehler werden dadurch vollst¨andig eliminiert. Im Nahbereich der Referenzstation gilt dies auch f¨ ur Ionosph¨arenfehler und Troposph¨arenfehler, mit wachsendem Abstand zwischen Referenzstation und Empf¨anger bleibt jedoch ein gr¨ oßer werdender Anteil unkompensiert. Die Noncommon-Mode-Fehler k¨onnen nicht verringert werden [39]. Unter g¨ unstigen Bedingungen sind mit DGPS bei ausschließlicher Verwendung von Pseudoranges Positionsgenauigkeiten im Bereich einiger Dezimeter m¨ oglich. Gelingt es, die Mehrdeutigkeitswerte der Tr¨agerphasenmessungen zu

110

4 Satellitennavigation

bestimmen, k¨ onnen durch Verarbeitung der Tr¨agerphasenmesswerte Positionsgenauigkeiten im Millimeterbereich erzielt werden. Neben einer Referenzstation am Boden werden auch satellitenbasierte Systeme (Satellite-Based Augmentation System, SBAS) wie WAAS (Wide Area Augmentation System) oder EGNOS eingesetzt. Hierbei werden Korrekturdaten u ¨ ber geostation¨are Satelliten ausgesendet. Die dabei erzielbare Positionsgenauigkeit ist nicht ganz so gut wie bei DGPS und liegt im Bereich von ein bis drei Metern.

4.4

Modernisierung des GPS-Systems

Die USA betreiben seit dem Jahre 2000 ein Programm zu Modernisierung des GPSSystems. Hintergrund hierf¨ ur ist zum einen, dass die grundlegende Planung der GPSSystemarchitektur in den Jahren 1970 bis 1976 erfolgte, seitdem jedoch massive technische Fortschritte erzielt werden konnten. Zum anderen ist man an einer Verbesserung der Anwendungsm¨ oglichkeiten des GPS-Systems interessiert. Als Beispiel seien milit¨ arische Interessen genannt: Sieht man von hochspezialisierten Empf¨angern ab, erfolgt eine Akquisition der milit¨ arischen P(Y)-Codes, indem zun¨achst durch die Akquisition des C/A-Codes Zeit- und Positionsinformationen gewonnen werden. Ein St¨oren13 des frei zug¨ anglichen C/A-Codes, das aus milit¨arischen Gr¨ unden gew¨ unscht sein kann, erschwert dann aber auch die Akquisition des P(Y)-Codes. Von milit¨arischer Seite werden daher Signale gew¨ unscht, die nicht frei zug¨anglich sind und sich einfach direkt akquirieren lassen. Dar¨ uber hinaus ist es nat¨ urlich schwer einzusch¨atzen, ob nicht auch die Bestrebungen der Europ¨aer, mit Galileo ein modernes Satellitennavigationssystem aufzubauen, das GPS-Modernisierungsprogramm mit motiviert haben. Ein wichtiger Punkt des GPS-Modernisierungsprogrammes sind neue Signale, sowohl f¨ ur den zivilen als auch f¨ ur den milit¨ arischen Nutzer. Im ersten Schritt werden modifizierte Block-IIR-M-Satelliten in die GPS-Konstellation integriert. Der erste Block-IIRM-Satellit wurde am 26. September 2005 mit einer Delta-2-Rakete in die Umlaufbahn gebracht. Mit dieser Modernisierungsstufe werden ein neues, ziviles Signal auf L2 und zus¨ atzliche, milit¨ arische Signale auf L1 und L2 eingef¨ uhrt. • L2C. Das neue zivile L2-Signal wird als L2C bezeichnet. Der Code wird aus dem Civil-Moderate-(CM-)Code mit einer Chiprate von 511.5 kHz und einer L¨ange von 10230 Chips, der zus¨ atzlich mit Navigationsdaten moduliert ist, und dem CivilLong-(CL-)Code mit einer Chiprate von ebenfalls 511.5 kHZ und einer L¨ange von 767.250 Chips durch Multiplexen so zusammengesetzt, dass ein Code mit der Chiprate von 1.023 MHz entsteht [130]. Damit stehen zivile Codes auf zwei Frequenzen zur Verf¨ ugung, was die Korrektur des Ionosph¨arenfehlers nun auch f¨ ur zivile Nutzer erm¨ oglicht. Die volle Verf¨ ugbarkeit14 des L2C-Signals wird f¨ ur 2013 erwartet. 13 Das

14 Full

St¨ oren der GPS-Signale wird als jamming bezeichnet. Operational Capability (FOC)

4.5 Galileo System¨ uberblick

111

• M-Codes. Die neuen milit¨ arisches Codes auf L1 und L2 werden als M-Codes bezeichnet. Bei den M-Codes wird das BOC(10,5) Modulationsverfahren eingesetzt. BOC steht f¨ ur Binary Offset Carrier, durch dieses Verfahren wird erreicht, dass sich die haupts¨ achlich belegten Frequenzbereiche ober- und unterhalb der Tr¨agerfrequenz befinden. Dies ist beabsichtigt, um Interferenzen mit dem L1-C/A-Code bzw. L2C-Signal zu minimieren. Details zu diesem Modulationsverfahren sind in [13] zu finden. Die M-Codes k¨ onnen direkt akquiriert werden, mit der vollen Verf¨ ugbarkeit der Signale kann bis 2013 gerechnet werden. Im n¨ achsten Schritt der Modernisierung kommen Block-IIF-Satelliten zum Einsatz, der erste Satellit soll 2007 im Orbit sein. Neben dem L2C-Signal und den M-Codes auf L1 und L2 wird ein zus¨ atzliches ziviles Signal im L5-Band eingef¨ uhrt werden. • L5. Bei dem neuen L5-Signal mit einer Tr¨agerfrequenz von 1176.45 MHz ist eine Chiprate von 10.23 MHz geplant. Die Signalst¨arke auf der Erdoberfl¨ache soll bei –154 dBW liegen. Das Signal verf¨ ugt u ur Inphasen- und ¨ ber unabh¨angige Codes f¨ Quadratur-Komponente. W¨ ahrend die Inphasen-Komponente mit Navigationsdaten moduliert ist, ist die Quadratur-Komponente frei von Navigationsdaten. Dadurch ist eine robustere Messung der Tr¨agerphase und eine leichtere Akquisition des Signals m¨ oglich. Die volle Verf¨ ugbarkeit des Signals ist f¨ ur 2014 prognostiziert. Ab 2012 soll dann die n¨ achste Generation von Satelliten, GPS III, zum Einsatz kommen. Diese sollen weitere, zus¨ atzliche F¨ ahigkeiten aufweisen. Dazu z¨ahlen der direkte Informationsaustausch zwischen Satelliten und die M¨oglichkeit, gezielt die Signalst¨arke der M-Codes um 20dB anzuheben, was als spot-beam bezeichnet wird. Neben den beschriebenen Neuerungen sind eine ganze Reihe weiterer Verbesserungen geplant, die sich nicht auf die Satelliten und ihre Signale beschr¨anken, sondern auch die ¨ Bodenstationen betreffen. Eine Ubersicht u ¨ ber das GPS-Modernisierungsprogramm ist z.B. in [87] zu finden.

4.5

Galileo Systemu ¨berblick

In diesem Abschnitt soll kurz das im Aufbau befindliche, europ¨aische Satellitennavigationssystem Galileo vorgestellt werden. Motivation Die Entscheidung, ein eigenes Satellitennavigationssystem aufzubauen, wurde von der Europ¨ aischen Union bereits 1998 gef¨ allt. Eine erste Definition der beabsichtigten Dienstleistungen, die durch Galileo zur Verf¨ ugung gestellt werden sollen, wurde 2001 ver¨offentlicht. Im Jahre 2003 wurde das Galileo Joint Undertaking (GJU) gegr¨ undet, das seitdem die Entwicklung von Galileo koordiniert. Die Gr¨ unde f¨ ur die Entwicklung eines europ¨aischen Satellitennavigationssystems sind vielf¨ altig. Zum einen soll sicherlich die derzeitige Abh¨angigkeit von den USA und Russland auf dem Gebiet der Satellitennavigation beendet werden. Zum anderen ist f¨ ur

112

4 Satellitennavigation

den Aufbau und Betrieb eines solchen Systems ein immenses Know-how notwendig. Die Erarbeitung dieses Know-hows stellt die Wettbewerbsf¨ahigkeit Europas auf diesem Sektor f¨ ur die Zukunft sicher. Es kann davon ausgegangen werden, dass dadurch eine Vielzahl hochqualifizierter Arbeitspl¨ atze entstehen. Ein weiteres Ziel von Galileo ist die Verbesserung von Genauigkeit, Verf¨ ugbarkeit und Integrit¨at der zur Verf¨ ugung gestellten Navigationsinformationen u ¨ ber das von GPS derzeit erzielte Maß hinaus. So wird Galileo jedem Nutzer Integrit¨ atsinformationen zur Verf¨ ugung stellen, die ihn sofort u ¨ ber auftretende Fehler informieren. Solche Informationen werden von GPS momentan noch nicht geliefert. Zus¨ atzlich wird es Garantien f¨ ur die Verf¨ ugbarkeit von Navigationsinformationen geben. Mit Galileo entsteht auch die M¨oglichkeit, nach eigenen Vorstellungen – kostenpflichtige oder freie – Mehrwertdienste zu generieren, was den Europ¨aern beim GPS-System sicher nicht zugestanden wird. Zeitplan Die Entwicklungs- und Validierungsphase begann 2003 und soll im Laufe des Jahres 2008 abgeschlossen werden. In dieser Phase sollen zwei Testsatelliten sowie die ersten vier Galileo Satelliten in Betrieb genommen werden. Zus¨atzlich muss die daf¨ ur ben¨otigte Infrastruktur in Form von Kontroll- und Monitoring-Stationen aufgebaut werden. Der erste Testsatellit, genannt Giove-A, wurde am 28.12.2005 von einer Sojus-Rakete in die Umlaufbahn gebracht. Der Start des zwieten Testsatelliten, Giove-B, ist f¨ ur Mitte 2007 geplant. Die n¨ achste Phase, der Aufbau des Systems, soll bis 2008 abgeschlossen werden. In dieser Phase werden 26 weitere Galileo Satelliten in Betrieb genommen. Zus¨atzlich wird die Infrastruktur am Boden vervollst¨ andigt werden. Raumsegment Das Raumsegment besteht bei Galileo aus 30 Satelliten in einer H¨ohe von 23616 km auf drei verschiedenen, kreisf¨ ormigen Umlaufbahnen, wobei ein Satellit pro Umlaufbahn als Reserve betrachtet wird. Eine solche Konstellation wird als Walker-27/3/1Konstellation bezeichnet. Die Umlaufbahnen haben eine Inklination von 56 Grad. Die Galileo-Satelliten werden Signale in vier Frequenzb¨andern aussenden: im Band E5a- bei 1176.45 MHz, im Band E5b bei 1207.14 Mhz, im Band E6 bei 1278.75 MHz und im L1-Band bei 1575.42 MHz. In diesen B¨andern stehen insgesamt zehn Signale zur Verf¨ ugung. Im E5a- und E5b-Band sind mit einer AltBOC(15,10)-Modulation Pseudo-Noise-Codes mit einer Chiprate von 10.23 Mcps15 aufmoduliert. Hierbei sind die Inphasen-Komponenten, E5a-I und E5b-I, mit Zusatzinformationen mit einer Datenerate von 50 bzw. 250 sps16 moduliert, die Quadratur-Komponenten E5a-Q und E5b-Q sind so genannte Pilotkan¨ ale, die datenfrei sind. Zu diesen vier Signalen kommen drei Signale im E6-Band: Mit E6-A wird ein Signal bezeichnet, bei dem mit einer BOCcos (10, 5)-Modulation ein Pseudonoise-Code mit einer Chiprate von 5.115 Mcps sowie Zusatzinformationen mit 100 sps aufmoduliert werden. Mit E6-B und E6-C wird die Inphasen- und Quadratur-Komponenete eines BPSK(5)-Signals bezeichnet, wobei 15

Mcps = 1e6 chips per second per second

16 symbols

4.5 Galileo System¨ uberblick

113

Tabelle 4.1: Signale und Frequenzen des Satellitennavigationssystems Galileo.

Signal

Tr¨ agerfrequenz

Modulation

Chiprate

Datenrate

Dienst

E5a-I

1176.45 Mhz

AltBOC(15,10)

10.23 Mcps

50 sps

OS/Sol

–

OS/SoL

250 sps

OS/SoL/CS

–

OS/SoL/CS

100 sps

PRS

1000 sps

CS

–

CS

100 sps

PRS

250 sps

OS/SoL/CS

–

OS/SoL/CS

E5a-Q E5b-I

1207.14 Mhz

E5b-Q E6-A

1278.75 MHz

E6-B

BOCcos (10, 5)

5.115 Mcps

BPSK(5)

E6-C L1-A L1-B L1-C

1575.42 MHz

BOCcos (15, 2.5) BOC(1,1)

5.115 Mcps

die Inphasenkomponente E6-B mit 1000 sps Zusatzinformationen moduliert ist, w¨ahrend die Quadraturkomponente E6-C ebenfalls ein datenfreier Pilotkanal ist. Im L1Band sind drei Signale vorhanden, L1-A ist ein BOCcos (15, 2.5)-Signal mit 100 sps Zusatzinformationen, L1-B und L1-C sind die Inphasen- und Quadraturkomponenten einer BOC(1,1)-Modulation, das L1-B-Signal enth¨alt 250 sps Zusatzinformationen, L1-C ist erneut ein datenfreier Pilotkanal. Die Chiprate der L1-Pseudonoise-Codes betr¨agt 1.023 Mcps. ¨ Eine Ubersicht u ¨ ber die Frequenzen und Signale ist in [25] zu finden und in Tab. 4.1 nochmals zusammengefasst. Eine detaillierte Beschreibung der angesprochenen Modulationsverfahren ist in [103] zu finden. Durch diese zehn Signale werden f¨ unf verschiedene Dienste zu Verf¨ ugung gestellt: • Open Service (OS). Der Open Service ist – vergleichbar mit dem L1-C/A-Code bei GPS – frei zug¨ anglich. Dieser Dienst zielt auf den Massenmarkt wie Automobilnavigationssysteme oder Mobiltelefone. Da der Open Service auf Signalen im E5- und L1-Band basiert, ist eine Korrektur des Ionosph¨arenfehlers wie bei milit¨ arischen GPS-P(Y)-Empf¨ angern m¨oglich. • Commercial Service (CS). Der Zugang zu diesen Signalen soll nur gegen Geb¨ uhr m¨ oglich sein. Daf¨ ur sollen eine h¨ ohere Positionierungsgenauigkeit m¨oglich sein und Mehrwert-Daten zur Verf¨ ugung gestellt werden – denkbar sind Integrit¨atsinforma¨ tionen, Verkehrsinformationen, Wetterwarnungen oder Ahnliches. Zus¨atzlich soll f¨ ur die Verf¨ ugbarkeit dieses Dienstes eine Garantie gegeben werden k¨onnen. • Public Regulated Service (PRS). Dieser Dienst soll nur autorisierten Nutzern zur Verf¨ ugung stehen, dazu werden Ordnungs- und Sicherheitskr¨afte, Nachrichten-

114

4 Satellitennavigation dienste, der Bundesgrenzschutz und vergleichbare Organisationen bzw. Beh¨orden z¨ ahlen. Die Signale dieses Dienstes sind verschl¨ usselt und sollen eine gr¨oßere Resistenz gegen¨ uber unbeabsichtigten oder mutwilligen St¨orungen aufweisen.

• Safety-of-Life Service (SoL). Der Safety-of-Life Service richtet sich an sicherheitskritische Anwendungen wie Flug-, Schienen- oder Schiffsverkehr. Anhand einer digitalen Signatur ist der Nutzer in der Lage, das empfangene Signal eindeutig als Galileo-Signal zu identifizieren. Zus¨atzlich stehen Integrit¨atsinformationen zu Verf¨ ugung, der Nutzer wird also beim Auftreten von Fehlern informiert. • Search-and-Rescue Service (SAR). Im Rahmen dieses Dienstes werden Searchand-Rescue Transponder der Galileo-Satelliten-Signale von bestimmten Notsignalsendern empfangen k¨ onnen. Hierzu sollen Sender des Maritime Distress Security Service und der internationalen Organisation f¨ ur Zivilluftfahrt z¨ahlen. Die Informationen der Notrufsender werden von den Galileo-Satelliten an entsprechende Bodenstationen weitergegeben. Weitere Details zu den Galileo-Diensten sind in [2] zu finden. Bodensegment Das Galileo-Bodensegment hat die Aufgabe, die Konstellation und die Systemzeit aufrecht zu erhalten, die f¨ ur die Nutzer notwendigen Informationen wie z.B. Ephemeridendaten zu ermitteln und diese u ¨ ber die den Satellitensignalen aufmodulierten Zusatzinformationen zur Verf¨ ugung zu stellen. Dazu ist eine umfassende Infrastruktur notwendig, die sich grob wie folgt gliedert: • Galileo-Control-Center (GCC). In den beiden GCC in Oberpfaffenhofen und Fu¨ cino findet die zentrale Kontrolle und Uberwachung des Galileo-Systems statt. Hier werden die Satellitenbahnen berechnet, die Systemzeit aufrecht erhalten und Man¨ over der Satelliten geplant. Die daf¨ ur n¨otigen Daten werden von den GalileoSensor-Stationen geliefert. • Galileo-Sensor-Stationen (GSS). Voraussichtlich vierzig dieser Empfangsstationen empfangen die Signale der Galileo-Satelliten und leiten ihre Messergebnisse and die Galileo-Control-Center weiter. • Satelliten-Kontrollstationen (TCC). Die f¨ unf Telemetry-, Tracking-, and Command-Stationen dienen dazu, Steuersignale an die Galileo-Satelliten zu senden. • Up-Link-Stationen (ULS). Mit Hilfe von neun Up-Link-Stationen werden die ausgestrahlten Navigationssignale aktualisiert.

4.5 Galileo System¨ uberblick

115

Galileo und GPS Ein wichtiger Aspekt von Galileo ist die Kompatibilit¨at und Interoperabilit¨at mit GPS. Unter Kompatibilit¨ at versteht man, dass der Betrieb des einen Systems den Betrieb des anderen Systems nicht st¨ ort. Unter Interoperabilit¨at versteht man, dass Empf¨angerArchitekturen gefunden werden k¨ onnen, die sowohl Signale von GPS-Satelliten als auch von Galileo-Satelliten empfangen k¨ onnen. Es ist zu erwarten, dass verglichen mit reinen GPS- oder Galileo-Empf¨ angern diese hybriden Empf¨anger deutliche Vorteile im Hinblick auf Genauigkeit und Zuverl¨ assigkeit der Navigationsinformationen sowie der Verf¨ ugbarkeit von Integrit¨ atsinformationen bieten. In [24] wird f¨ ur Einfrequenz-Empf¨anger eine Steigerung der Genauigkeit um den Faktor zwei, f¨ ur Zweifrequenzempf¨anger eine Steigerung der Genauigkeit um den Faktor f¨ unf prognostiziert.

5

Grundlagen der Stochastik

Ein stochastisches Filter hat die Aufgabe, anhand von Messwerten den Zustand eines Systems zu sch¨atzen. Diese Messwerte sind – ebenso wie das zur Verf¨ ugung stehende mathematische Modell des Systems – mit Unsicherheiten bzw. Fehlern behaftet, zu deren Beschreibung sich stochastische Konzepte anbieten. Ohne detailliert auf mathematische Grundlagen oder Definitionen einzugehen, werden im Folgenden einige Begriffe aus der Stochastik eingef¨ uhrt.

5.1

Die Zufallsvariable

Eine Zufallsvariable ist mathematisch gesehen eine Funktion. Diese Funktion ordnet den Ergebnissen eines Zufallsexperiments Werte zu, die Realisationen genannt werden. Manche Autoren unterscheiden nicht immer korrekt zwischen Zufallsvariable und Realisation, sondern betrachten eine Zufallsvariable wie eine Variable, die zuf¨allig Werte annimmt. Man unterscheidet zwischen diskreten und kontinuierlichen Zufallsvariablen: Diskrete Zufallsvariablen ordnen Werte aus einer abz¨ahlbaren Menge zu, w¨ahrend kontinuierliche Zufallsvariablen auf beliebige Werte abbilden. Diskrete Zufallsvariablen werden hier nicht weiter betrachtet. Es gibt verschiedene M¨ oglichkeiten, die Charakteristik einer Zufallsvariablen zu beschreiben. Dazu z¨ ahlen die Verteilungsfunktion oder kumulative Wahrscheinlichkeit, die Wahrscheinlichkeitsdichtefunktion, die charakteristische Funktion oder die Momente der Zufallsvariablen.

5.1.1

Wahrscheinlichkeitsdichte

Die Verteilungsfunktion Fx (ξ) einer Zufallsvariable x gibt an, mit welcher Wahrscheinlichkeit P Realisationen kleiner als eine bestimmte Schranke ξ sind: Fx (ξ) = P (x ≤ ξ)

(5.1)

Hierbei wurde der Begriff Wahrscheinlichkeit verwendet, f¨ ur den es unterschiedliche Definitionen gibt: Die Wahrscheinlichkeit eines Ereignisses ist nach der klassische Definition von Laplace die Anzahl der zu einem Ereignis geh¨orenden Elementarereignisse dividiert durch die Anzahl aller m¨ oglichen Ereignisse. Diese Definition wurde von der Definition nach Richard von Mieses abgel¨ ost, die Wahrscheinlichkeit als die relative H¨ aufigkeit des Auftretens eines Ereignisses beschreibt. Die am breitesten akzeptierte Definition stammt von Andrey Kolmogorov, die sich auf drei Axiome st¨ utzt:

118

5 Grundlagen der Stochastik

1. Die Wahrscheinlichkeit eines Ereignisses ist gr¨oßer oder gleich null. 2. Die Wahrscheinlichkeit des sicheren Ereignisses ist eins. 3. F¨ ur zwei sich gegenseitig ausschließende Ereignisse ist die Wahrscheinlichkeit, dass eines von beiden eintritt, gleich der Summe der Einzelwahrscheinlichkeiten. Die Wahrscheinlichkeit, dass eine Realisation in einem bestimmten Intervall auftritt, berechnet sich nach Gl. (5.1) zu P (ξ0 < x ≤ ξ1 ) = Fx (ξ1 ) − Fx (ξ0 ) .

(5.2)

Weiterhin gilt lim Fx (ξ) = 0

(5.3)

lim Fx (ξ) = 1

(5.4)

ξ→−∞ ξ→∞

ξ0 < ξ1 ⇒ Fx (ξ0 ) < Fx (ξ1 ) .

(5.5)

Die Verteilungsfunktion wird auch als kumulative Wahrscheinlichkeit bezeichnet, deren Ableitung ist die Wahrscheinlichkeitsdichte: dFx (ξ) dξ

px (ξ) =

(5.6)

Die Wahrscheinlichkeitsdichte ist eine nicht-negative Funktion, px (ξ) ≥ 0 ,

(5.7)

f¨ ur die weiterhin gilt:
∞ px (ξ)dξ = 1

(5.8)

−∞

Die Wahrscheinlichkeit, dass eine Realisation in einem bestimmten Intervall auftritt, l¨ asst sich anhand von
ξ1 P (ξ0 < x ≤ ξ1 ) =

px (u)du

(5.9)

ξ0

berechnen, die Verteilungsfunktion kann durch Integration u ¨ ber die Wahrscheinlichkeitsdichte ermittelt werden:
ξ Fx (ξ) =

px (u)du −∞

(5.10)

5.1 Die Zufallsvariable

119

Das erste Moment einer Zufallsvariablen x ist der Erwartungswert oder Mittelwert
∞ mx = E[x] =

ξ px (ξ) dξ .

(5.11)

−∞

Die Streuung der Realisationen um den Mittelwert wird durch das zweite zentrale Moment, die Varianz σx2 , beschrieben: σx2


∞   2 = E (x − mx ) = (ξ − mx )2 px (ξ) dξ

(5.12)

−∞

Die Varianz ist eine nicht-negative Zahl, die Wurzel aus der Varianz wird als Standardabweichung bezeichnet. Zwei Zufallsvariablen x und y k¨ onnen anhand der Verbundwahrscheinlichkeitsdichte pxy beschrieben werden, man erh¨ alt
ξ1
η1 P (ξ0 < x ≤ ξ1 ∧ η0 < y ≤ η1 ) =

pxy (u, v) du dv .

(5.13)

ξ0 η0

Die Wahrscheinlichkeitsdichten der einzelnen Zufallsvariablen heißen marginale Wahrscheinlichkeitsdichten, diese k¨ onnen aus der Verbundwahrscheinlichkeitsdichte durch Integration u ¨ ber den gesamten Wertebereich der jeweils anderen Zufallsvariable berechnet werden:
∞ px (ξ) =

pxy (ξ, η) dη

(5.14)

pxy (ξ, η) dξ

(5.15)

−∞
∞

py (η) = −∞

Liegt eine bestimmte Realisation η der Zufallsvariable y vor, stellt sich die Frage nach der Wahrscheinlichkeit von Realisationen ξ der Zufallsvariable x unter dieser Randbedingung. Dazu wird die bedingte Wahrscheinlichkeitsdichtefunktion px|y ben¨otigt, es gilt px|y (ξ |y = η) =

pxy (ξ, η) . py (η)

(5.16)

Eine Herleitung dieses Zusammenhangs, der als Bayes’sche Regel bekannt ist, ist in [85] zu finden. Der entsprechende Zusammenhang f¨ ur den Fall, dass eine bestimmte Realisation ξ vorliegt, ist gegeben durch py|x (η |x = ξ) =

pxy (ξ, η) px (ξ)

(5.17)

120

5 Grundlagen der Stochastik

Damit ergibt sich eine alternative Formulierung der Bayes’schen Regel: pxy (ξ, η) = px|y (ξ |y = η)py (η) = py|x (η |x = ξ)px (ξ)

(5.18)

Der bedingte Erwartungswert E[x|y] kann als Funktion der Zufallsvariable y verstanden werden. Folglich handelt es sich bei E[x|y] ebenfalls um eine Zufallsvariable, w¨ahrend E[x] ein deterministischer Wert ist. Weiterhin gilt E [E [x|y]] = E [x] var(x) = E [var (x|y)] + var (E [x|y]) ,

(5.19) (5.20)

wobei var() die Varianz der entsprechenden Zufallsvariable bezeichnet. Die Eigenschaften zweier Zufallsvariablen werden durch die Begriffe Unabh¨angigkeit, Unkorreliertheit und Orthogonalit¨ at n¨ aher beschrieben. Zwei Zufallsvariablen x und y heißen • statistisch unabh¨ angig, wenn pxy (ξ, η) = px (ξ)py (η) gilt . • unkorreliert, wenn E[xy] = E[x]E[y] gilt . • orthogonal, wenn E[xy] = 0 gilt. Aus Unabh¨ angigkeit folgt Unkorreliertheit, der Umkehrschluss gilt im Allgemeinen aber nicht. Schließlich kann aus mehreren skalaren Zufallsvariablen x1 , x2 ...xn ein Zufallsvektor x gebildet werden. Der Erwartungswert dieses Zufallsvektors berechnet sich gem¨aß
∞ m  x = E[x] =


∞ ...

−∞

  x ξ dξ1 . . . dξn , ξp

−∞

die Kovarianzmatrix ist gegeben durch   T .  x ) (x − m  x) Pxx = E (x − m Die Hauptdiagonale der Kovarianzmatrix besteht aus den Varianzen der Zufallsvariablen x1 , x2 ...xn . Sind diese unkorreliert, verschwinden die Nebendiagonalelemente. Kovarianzmatrizen sind f¨ ur den pathologischen Fall, dass Komponenten des Zufallsvektors die Varianz null besitzen, positiv semi-definit, ansonsten positiv definit.

5.1.2

Gaußverteilung

Im Rahmen der stochastischen Filterung spielt eine bestimmte Verteilungsfunktion, die Gauß- oder Normalverteilung, eine zentrale Rolle. Ein Grund hierf¨ ur ist, dass sich eine

5.1 Die Zufallsvariable

121

Normalverteilung zur Beschreibung vieler Zufallsph¨anomene eignet. Eine beobachtbare Gr¨ oße wird h¨ aufig von vielen St¨ orquellen beeinflusst, die als Zufallsvariablen beschrieben werden k¨ onnen. Vereinfacht dargestellt strebt mit wachsender Anzahl der Zufallsvariablen die Warscheinlichkeitsverteilung ihrer Summe gegen eine Normalverteilung. Die Zufallsvariablen d¨ urfen dabei beliebige Verteilungsfunktionen besitzen, sie m¨ ussen jedoch unabh¨ angig sein. Die exakte mathematische Formulierung dieses Zusammenhangs ist als zentraler Grenzwertsatz bekannt. Ein weiterer Grund f¨ ur die Sonderstellung der Normalverteilung ist die gute mathematische Handhabbarkeit. Dadurch k¨onnen in vielen F¨ allen noch analytische L¨ osungen gefunden werden, was bei Annahme anderer Verteilungsfunktionen nicht m¨ oglich w¨ are. Dar¨ uber hinaus wird die Normalverteilung durch die ersten beiden Momente bereits vollst¨ andig beschrieben. Bei realen Problemstellungen wird man h¨ aufig, auch wenn keine Normalverteilung zu Grunde liegt, kaum u ¨ ber mehr Informationen verf¨ ugen und daher gezwungenermaßen eine Normalverteilung annehmen. Die Wahrscheinlichkeitsdichte ist im skalaren Fall gegeben durch

1

px (ξ) = √ 2πσ 2

(ξ − mx )2 2σ 2 e , −

(5.21)

ein normalverteilter Zufallsvektor der Dimension n × 1 besitzt die Dichtefunktion 1  =  px (ξ) (2π)n | Pxx |

T  1  −m ξ  ξ−m  x P−1 x xx . e 2 −

(5.22)

Werden mit normalverteilten Zufallsvektoren x und y lineare Operationen durchgef¨ uhrt, ist das Ergebnis z ebenfalls wieder ein normalverteilter Zufallsvektor: z = Ax + By + c

(5.23)

Bei A und B handelt es sich um beliebige Gewichtungsmatrizen, c ist ein beliebiger konstanter Vektor. Der Erwartungswert und die Kovarianzmatrix des Vektors z k¨onnen direkt angegeben werden: m  z = Am  x + Bm  y + c T

(5.24) T

T

Pzz = APxx A + APxy B + BPyx A + BPyy B

T

(5.25)

Sind x und y unkorreliert, so folgt Pxy = 0 und Pyx = 0, die Berechnung der Kovarianzmatrix von z vereinfacht sich entsprechend. Im allgemeinen Fall, d.h. bei beliebigen Wahrscheinlichkeitsdichten, m¨ ussen zur Berechnung der Wahrscheinlichkeitsdichte einer Summe von Zufallsvariablen die Wahrschein-

122

5 Grundlagen der Stochastik

lichkeitsdichten der Summanden gefaltet werden, z = x+y
∞ px (l)py (ζ − l) dl , pz (ζ) =

(5.26) (5.27)

−∞

was wesentlich aufw¨ andiger ist. Ein m¨ oglicher Nutzen der Gleichungen (5.23) – (5.25) soll anhand eines Beispiels verdeutlicht werden: Mit einem Zufallszahlengenerator soll ein Rauschvektor mit einer definierten Kovarianzmatrix Pzz erzeugt werden. Der Zufallszahlengenerator liefert normalverteilte, mittelwertfreie Zufallszahlen1 mit der Varianz 1, ein aus diesen Zufallszahlen aufgebauter Vektor ξ kann als Realisation eines Zufallsvektors x mit der Kovarianzmatrix Pxx = I verstanden werden. Nun wird durch Cholesky-Zerlegung von Pzz eine Gewichtungsmatrix A bestimmt: Pzz = AAT

(5.28)

Durch Multiplikation von ξ mit dieser Gewichtungsmatrix A erh¨alt man eine Realisation eines Zufallsvektors z mit der gew¨ unschten Kovarianzmatrix Pzz , denn es gilt: z = Ax     

T T E (z − m  z ) (z − m  z) = E zz T = E (Ax) (Ax) 

= AE xx T AT = APxx AT = AAT = Pzz

(5.29)

(5.30)


Liegt eine Realisation η eines Zufallsvektors y vor, so ist der bedingte Erwartungswert und die bedingte Kovarianzmatrix eines Zufallsvektors x gegeben durch m  x|η = m  x + Pxy P−1 η−m  y) yy ( Pxx|η = Pxx −

Pxy P−1 yy Pyx

.

(5.31) (5.32)

Die Gl. (5.31) und (5.32) k¨ onnen als L¨ osung des Sch¨atzproblems bei normalverteilten Zufallsvektoren aufgefasst werden. In Abschnitt 6 wird unter anderem anhand dieser Gleichungen das Kalman-Filter hergeleitet. 1 Liefert der Zufallszahlengenerator z.B. gleichverteilte Zufallszahlen, erh¨ alt man aufgrund des zentralen Grenzwertsatzes eine n¨ aherungsweise normalverteilte Zufallszahl durch Addition mehrerer gleichverteilter Zufallszahlen.

5.2 Stochastische Prozesse

5.2

123

Stochastische Prozesse

Ein stochastischer Prozess kann als eine Zufallsvariable mit einem zus¨atzlichen Parameter, der Zeit, verstanden werden. Betrachtet man einen stochastischen Prozess zu einem festen Zeitpunkt, erh¨ alt man folglich eine Zufallsvariable. Eine wichtige Kenngr¨ oße eines stochastischen Prozesses x(t) ist die Autokorrelationsfunktion rxx (t1 , t2 ) = E[x(t1 )x(t2 )] ,

(5.33)

anhand derer Aussagen u ahnlichkeit des Prozesses bei zeitlichen Verschie¨ ber die Selbst¨ bungen getroffen werden. Betrachtet man zwei verschiedene Prozesse x(t) und y(t), so erh¨ alt man die Kreuzkorrelationsfunktion: rxy (t1 , t2 ) = E[x(t1 )y(t2 )]

(5.34)

Ein stochastischer Prozess heißt station¨ ar im strengen Sinne, wenn die Charakteristiken des Prozesses, z.B. die Wahrscheinlichkeitsdichte, keine Funktion der Zeit ist. Von Stationarit¨ at im weiteren Sinne spricht man, wenn die ersten beiden Momente keine Funktion der Zeit sind, dadurch h¨ angt z.B. auch die Autokorrelation nur von der zeitlichen Verschiebung, nicht aber vom absoluten Zeitpunkt ab: rxx (t1 , t2 ) = rxx (t1 − t2 ) = rxx (τ ) = E[x(t)x(t − τ )]

(5.35)

Bei einem normalverteilten Zufallsprozess folgt aus Stationarit¨at im weiteren Sinne auch Stationarit¨ at im engen Sinne, da die Normalverteilung durch die ersten beiden Momente vollst¨ andig beschrieben wird. Im weiteren Sinne station¨ are Prozesse k¨ onnen ergodisch sein. In diesem Fall stimmt der Scharmittelwert des Prozesses zu einem beliebigen Zeitpunkt mit dem Zeitmittelwert jeder Realisation ξi (t) u ¨ berein:
∞ E[x(t)] = −∞

1 px (ξ)dξ = lim T →∞ 2T


T ξi (t)dt

(5.36)

−T

Ist ein stochastischer Prozess ergodisch, so kann z.B. die Kovarianz oder die Autokorrelation anhand des Zeitmittelwertes einer einzigen Realisation berechnet werden. Das ist von entscheidender Bedeutung, da in der Praxis h¨aufig nur eine einzige Realisation vorliegt. Die Autokorrelationsfunktion eines ergodischen Prozesses ist beispielsweise anhand von 1 rxx (τ ) = lim T →∞ 2T


T ξi (t)ξi (t − τ )dt

(5.37)

−T

berechenbar. Die Kreuzkorrelationsfunktion kann bei Betrachtung zweier Prozesse in ¨ analoger Weise berechnet werden. Beim Ubergang ins Zeitdiskrete wird das Integral

124

5 Grundlagen der Stochastik

durch eine entsprechende Summe ersetzt. Da in der Praxis nur ein endliches Zeitintervall betrachtet werden kann, erh¨ alt man lediglich einen Sch¨atzwert der Autokorrelationsfunktion. Die spektrale Leistungsdichte Sxx (jω) eines stochastischen Prozesses ist die Fouriertransformierte der Autokorrelationsfunktion, es gilt
∞ Sxx (jω) =

rxx (τ )e−jωτ dτ .

(5.38)

−∞

Die Fourier-R¨ ucktransformierte ist gegeben durch 1 rxx (τ ) = 2π


∞ Sxx (jω)ejωτ dω ,

(5.39)

−∞

dieser Zusammenhang ist als Wiener-Khintchine-Relation bekannt. Die Leistung eines Signals erh¨ alt man als Funktionswert der Autokorrelationsfunktion an der Stelle τ = 0, in diesem Fall besteht der Integrand in Gl. (5.39) lediglich aus der spektralen Leistungsdichte. Die Erweiterung der bisher eingef¨ uhrten Begriffe auf vektorielle stochastische Prozesse stellt keine Schwierigkeit dar und wird daher nicht weiter betrachtet. Stattdessen soll ein spezieller stochastischer Prozess, das weiße Rauschen, angesprochen werden.

5.2.1

Weißes Rauschen

Weißes Rauschen ist durch eine konstante spektrale Leistungsdichte gekennzeichnet: Sxx (jω) = R

(5.40)

Die Signalleistung erh¨ alt man durch Integration u ¨ ber die spektrale Leistungsdichte, folglich besitzt dieser stochastische Prozess unendliche Leistung. Die Existenz von weißem Rauschen ist im Zeitkontinuierlichen daher nicht m¨oglich. Die Autokorrelationsfunktion erh¨ alt man zu rxx (τ ) = Rδ(τ ) ,

(5.41)

¨ wobei δ(τ ) den Dirac-Impuls bezeichnet. F¨ ur τ = 0 wird dieser unendlich, was in Uber¨ einstimmung mit obigen Uberlegungen auf eine unendliche Leistung schließen l¨asst. Da bei mittelwertfreien Prozessen die Autokorrelationsfuktion an der Stelle τ = 0 gleich der Varianz ist, ist die Varianz von weißem Rauschen im Zeitkontinuierlichen nicht definiert. Dennoch ist weißes Rauschen als Konzept sinnvoll: Geht man davon aus, dass man von einem weißem Rauschprozess im Zeitkontinuierlichen durch Mittelwertbildung u ¨ ber den Abtastzeitraum T ins Zeitdiskrete u ¨ bergeht, erh¨alt man auch im Zeitdiskreten einen weißen Rauschprozess. Die Leistung dieses zeitdiskreten Rauschprozesses ist endlich,

5.2 Stochastische Prozesse

125

seine Existenz ist problemlos m¨ oglich. Die Varianz Rk dieses Prozesses ist definiert und h¨ angt, wie in Abschnitt 3.4.3 gezeigt, gem¨ aß Rk =

R T

(5.42)

mit der spektralen Leistungsdichte des nicht existierenden, weißen Rauschens im Zeitkontinuierlichen zusammen. Um z.B. bei der Spezifikation des Rauschens von Inertialsensoren nicht die Varianz bei einer bestimmten Abtastzeit angeben zu m¨ ussen, wird stattdessen die spektrale Leitunsgdichte eines hypothetischen, zeitkontinuierlichen, weißen Rauschprozesses verwendet, die abtastzeitunabh¨angig ist.

5.2.2

Zeitkorreliertes Rauschen

Ein zeitkorrelierter Rauschprozess l¨ asst sich mit FIR2 oder mit IIR3 Systemen beschreiben, an deren Eingang ein weißes Rauschen vorliegt. Im Rahmen der stochastischen Filterung wird u ¨ blicherweise die Beschreibung mit IIR-Systemen bevorzugt, da damit eine einfache Zustandsraumdarstellung resultiert. Ein skalarer Gauß-Markov-Prozess 1. Ordnung ist im Zeitkontinuierlichen gegeben durch # 1 2 w˙ = − w+σ η. (5.43) tcorr tcorr Hierbei bezeichnet w den zeitkorrelierten Rauschterm, η ist ein weißer Rauschprozess mit der spektralen Leistungsdichte Sηη = 1, tcorr ist die Korrelationszeit und σ ist die Standardabweichung des Gauß-Markov-Prozesses. Die Autokorrelationsfunktion dieses Prozesses, |τ |

rww (τ ) = σ 2 e− tcorr ,

(5.44)

ist an der Stelle τ = tcorr folglich auf 1/e ihres Maximalwertes, der bei τ = 0 vorliegt, abgefallen. Durch Fourier-Transformation erh¨alt man die spektrale Leistungsdichte zu ) ) 2 2 σ tcorr σ tcorr · 1 Sηη , (5.45) Sww (jω) = 1 tcorr + jω tcorr − jω % &' ( % &' ( H(jω)

H(−jω)

¨ dabei ist H(jω) die Ubertragungsfunktion des IIR-Systems, offensichtlich handelt es sich hierbei um einen Tiefpass 1. Ordnung. Gauß-Markov-Prozesse 1. Ordnung eignen sich daher zur Beschreibung langsam ver¨ anderlicher Vorg¨ange, wie z.B. der Bias-Drift eines Inertialsensors. ¨ Das zeitdiskrete Aquivalent dieses Rauschprozesses ist gegeben durch wk+1 = e− tcorr wk + ηk , T

2 Finite

Impulse Response Impulse Response

3 Infinite

(5.46)

126

5 Grundlagen der Stochastik

die Abtastzeit ist mit T bezeichnet. F¨ ur die Varianz des weißen Rauschprozesses erh¨alt man 

 2T (5.47) E η 2 = σ 2 1 − e− tcorr . Sch¨ atzung der Rauschprozessparameter Die Sch¨ atzung der Parameter eines zeitkorrelierten Rauschprozesses soll im Folgenden beispielhaft anhand eines vektoriellen Gauß-Markov-Prozesses 1. Ordnung demonstriert werden. Ein solcher Prozess ist gegeben durch w  k+1 = Φw k +  ηk .

(5.48)

alt man Durch Multiplikation mit w  kT erh¨ w  k+1 w  kT = Φw kw  kT +  ηk w  kT .

(5.49)

Die Bildung des Erwartungswertes liefert 





kw  kT = ΦE w  kT + E ηk w  kT . E w  k+1 w

(5.50)

Da das weiße Rauschen  ηk nicht mit dem Rauschterm nk korreliert ist, verschwindet der letzte Erwartungswert. Die verbleibenden Erwartungswerte k¨onnen mit Hilfe der entsprechenden Stichprobenformeln gesch¨atzt werden, liegt eine Stichprobe mit N Messwerten vor, erh¨ alt man so N N 1  1  w  k+1 w  kT = Φ · w kw  kT . N −1 N −1 k=1 k=1 % &' ( % &' ( A

(5.51)

B

Eine Sch¨ atzung der Transitionsmatrix ist daher gegeben durch ˆ = AB−1 . Φ

(5.52)

Mit Hilfe dieser gesch¨ atzten Transitionsmatrix kann f¨ ur jeden Zeitpunkt ein Sch¨atzwert f¨ ur den Rauschterm des weißen Rauschprozesses ermittelt werden: ˆw ηˆk = w  k+1 − Φ k

(5.53)

Die Varianz dieses weißen Rauschprozesses kann ebenfalls durch Anwendung der entsprechenden Stichprobenformel gesch¨ atzt werden.
Die gleiche Vorgehensweise l¨ asst sich auch f¨ ur Prozesse h¨oherer Ordnung anwenden. Eine allgemeine Sch¨ atzvorschrift f¨ ur die Parameter von Gauß-Markov-Prozessen beliebiger Ordnung ist durch die Yule-Walker-Gleichung gegeben, siehe [64]. Ein Gauß-Markov-Prozess n-ter Ordnung wird durch die Differenzengleichung w  k = A1 w  k−1 + A2 w  k−2 + . . . + An w  k−n + ηk

(5.54)

5.2 Stochastische Prozesse beschrieben, die zugeh¨ orige Zustandsraumdarstellung ist gegeben durch ⎞ ⎛ ⎛ ⎞ ⎞ ⎛η ⎞ A1 A2 . . . An ⎛ w  k−1 w k k ⎜ I 0 ... 0 ⎟ 0 ⎟  k−1 ⎟ ⎜  k−2 ⎟ ⎜ ⎟ ⎜w ⎜ w ⎜ ⎟=⎜ ⎟ ⎜.⎟ . ⎟⎜ . ⎟ . .. ⎝ ⎠ ⎜ 0 I . . .. ⎟ ⎝ .. ⎠ + ⎜ ⎝ .. ⎠ . . ⎠ ⎝ .. . . . . 0 w  k−n+1 w  k−n . . .

127

(5.55)

Rauschprozesse, die sich mit diesem Ansatz modellieren lassen, werden auch als Vektorautoregressive (VAR) Prozesse bezeichnet.

6

Das Kalman-Filter

Ein Kalman-Filter ist ein Algorithmus zur Sch¨atzung des Zustandes eines linearen Systems. Dazu werden Messwerte verarbeitet, die in linearer Weise mit dem Systemzustand zusammenh¨ angen m¨ ussen. Ist der Zusammenhang zwischen Messwerten und Systemzustand oder das beobachtete System schwach nichtlinear, kann ein erweitertes Kalman-Filter (EKF) eingesetzt werden, siehe Abschnitt 6.4. Um eine Zustandssch¨ atzung durchf¨ uhren zu k¨onnen, ben¨otigt das Kalman-Filter ein Modell des beobachteten Systems in der Form xk+1 = Φk xk + Bk uk + Gk w k

(6.1)

Gegen¨ uber dem bereits eingef¨ uhrten Systemmodell Gl. (2.71) ist ein zus¨atzlicher Term hinzugekommen, der das System- oder Prozessrauschen w  k enth¨alt. Bei dem Systemrauschen handelt es sich um mittelwertfreies, normalverteiltes, weißes Rauschen; es gilt ,

 Qk i = k T E w  iw . (6.2) k = 0 i = k Das Systemrauschen dient zur Ber¨ ucksichtigung von Modellunsicherheiten. Damit kann der Tatsache Rechnung getragen werden, dass das reale System durch ein Modell der Form Gl. (2.71) nur n¨ aherungsweise beschrieben wird. Des Weiteren ist es vorstellbar, dass die Eingangsgr¨ oßen uk mit Rauschen behaftet sind: auch diese Eingangsgr¨oßen m¨ ussen in der Regel durch Messung ermittelt werden. Das Messrauschen dieser Eingangsgr¨ oßen kann dann dem Systemrauschen zugeschlagen werden. Eine Ber¨ ucksichtigung von Messwerten als Eingangsgr¨ oßen und nicht als Messwerte im eigentlichen Sinne setzt voraus, dass keine Ableitungen dieser Gr¨oßen im Zustandsvektor des Systemmodells auftreten. Der Zusammenhang zwischen Messwerten y˜k und dem Systemzustand xk wird durch ein Messmodell der Form y˜k = Hk xk + vk  (6.3) beschrieben. Die Matrix Hk wird als Messmatrix bezeichnet, bei vk handelt es sich um das Messrauschen. Das Messrauschen wird – genau wie das Systemrauschen – als weiß, mittelwertfrei und normalverteilt angenommen; es gilt ,

T Rk i = k E vivk = . (6.4) 0 i = k Als weitere Voraussetzung wird angenommen, dass die Kreuzkorrelation von Messrauschen und Systemrauschen verschwindet:

 E vk w  kT = 0 (6.5)

130

6 Das Kalman-Filter

Unter den angesprochenen Voraussetzungen – lineares System- und Messmodell, weißes, normalverteiltes, mittelwertfreies System- und Messrauschen – ist das KalmanFilter optimal, d.h. kein anderer Algorithmus liefert im Mittel eine kleinere Summe der quadrierten Sch¨ atzfehler. Neben einer Sch¨atzung des Systemzustandes liefert das Kalman-Filter auch noch Informationen bez¨ uglich der G¨ ute der Sch¨atzung in Form einer Sch¨ atzfehlerkovarianzmatrix. Die Gleichungen des Kalman-Filters werden im Folgenden hergeleitet.

6.1

Kalman-Filter Gleichungen

Rudolf Emil Kalman leitete 1960 die nach ihm benannten Kalman-Filter-Gleichungen u atsprinzip her, siehe [63]. Neben dieser sehr mathematischen Vor¨ ber ein Orthogonalit¨ gehensweise gibt es auch leichter einsichtige Wege der Herleitung dieser Gleichungen, eine M¨ oglichkeit besteht u ¨ ber die Formulierung linearer Operationen mit normalverteilten Zufallsvektoren.

6.1.1

Herleitung u ¨ ber normalverteilte Zufallsvektoren

Prinzipiell k¨ onnen die Kalman-Filter Gleichungen in zwei Gruppen eingeteilt werden, den Estimationsschritt und den Propagationsschritt.

Estimation Im Estimationsschritt des Kalman-Filters werden die zum aktuellen Zeitpunkt vorliegenden Messwerte verarbeitet. Dadurch wird die Sch¨atzung des Systemzustandes verbessert, entsprechend muss auch die Kovarianzmatrix des Sch¨atzfehlers angepasst werden. Es soll folgende Nomenklatur verwendet werden: Der tats¨achliche Systemzustand ˆk . Obere Indizes + atzung des Systemzustandes mit x wird mit xk bezeichnet, die Sch¨ − und geben an, ob die zum aktuellen Zeitpunkt vorliegenden Messwerte bereits verarbeitet wurden ()+ oder die Messwertverarbeitung noch aussteht ()− . Im ersten Fall spricht man von einem a posteriori Sch¨ atzwert, bei noch ausstehender Messwertverarbeitung handelt es sich um einen a priori Sch¨atzwert. Die a priori und a posteriori Kovarianzmatrizen des Sch¨ atzfehlers sind gegeben durch   T ˆ − xk − x ˆ− = E  x −  x (6.6) P− k xx,k k k   T ˆ + xk − x ˆ+  x = E −  x . (6.7) P+ k xx,k k k Grundlage f¨ ur die Herleitung des Estimationsschrittes sind die Gleichungen (5.31) und (5.32), die sich mit der gerade eingef¨ uhrten Nomenklatur wie folgt schreiben lassen:  ˜k − yˆk  y (6.8) xˆk+ = xˆk− + Pxy,k P−1 yy,k − −1 P+ xx,k = Pxx,k − Pxy,k Pyy,k Pyx,k

(6.9)

6.1 Kalman-Filter Gleichungen

131

Um anhand der vorliegenden Messwerte y˜k aus der a priori Zustandssch¨atzung und Sch¨ atzfehlerkovarianzmatrix die entsprechenden a posteriori Gr¨oßen berechnen zu k¨onnen, m¨ ussen die noch unbekannten Gr¨ oßen  yˆk , Pxy,k , Pyy,k und Pyx,k ermittelt werden. Der Erwartungswert des Messwertvektors ergibt sich unter Ber¨ ucksichtigung des Messmodells zu   yˆk = E y˜k = E [Hk xk + vk ] = Hk E [xk ] + E [vk ] ˆ− . yˆk = Hk x  k

(6.10)

Die Kovarianzmatrix Pyy,k ist gegeben durch

Pyy,k

  T = E y˜k − yˆk  yˆk y˜k − 

.

(6.11)

Durch Einsetzen des Messmodells und Gl. (6.10) erh¨alt man:  Pyy,k = E

Hk xk + vk − Hk xˆk−

 T ˆ− Hk xk + vk − Hk x k

   T ˆ − xk − x ˆ − HT = E Hk xk − x k k k     T  ˆ − v T + E vk xk − x ˆ − HT +E Hk xk − x k k k k

 +E vk vkT

(6.12)

Da das Messrauschen vk vor Verarbeitung der Messwerte nicht mit dem Sch¨atzfehler korreliert sein kann, verschwinden in oberiger Gleichung der zweite und der dritte Erwartungswert, es ergibt sich damit

Pyy,k = Hk E

  T

 ˆ− xk − xˆk− xk − x HTk + E vk vkT k

T = Hk P− xx,k Hk + Rk .

(6.13)

132

6 Das Kalman-Filter

Die Matrix Pxy,k ergibt sich analog zu   T ˆ−  y˜k − yˆk Pxy,k = E xk − x k   T ˆ − Hk xk + vk − Hk x ˆ− = E xk − x k k   T   ˆ − xk − x ˆ − HT + E xk − xˆ − v T = E xk − x k k k k k   T ˆ − xk − x ˆ− = E xk − x HTk k k T Pxy,k = P− xx,k Hk .

(6.14)

Setzt man Gl. (6.10), (6.13) und (6.14) in Gl. (6.8) ein, erh¨alt man mit Pyx,k = PTxy,k = Hk P− xx,k

(6.15)

den gesuchten Zusammenhang zur Berechnung des a posteriori Sch¨atzwertes:    ˆ − + P− HT Hk P− HT + Rk −1 y˜ − Hk xˆ − xˆk+ = x xx k k xx,k k k

(6.16)

Es ist u urzung einzuf¨ uhren: ¨ blich, folgene Abk¨  −1 − T T Kk = P− H H P H + R k k k k xx,k xx,k

(6.17)

Die Matrix Kk wird als Kalman-Gain-Matrix bezeichnet. Damit lassen sich der a posteriori Sch¨ atzwert und die a posteriori Kovarianzmatrix des Sch¨atzfehlers wie folgt schreiben:  (6.18) xˆk+ = xˆk− + Kk y˜k − Hk xˆk− − − − P+ xx,k = Pxx,k − Kk Hk Pxx,k = (I − Kk Hk )Pxx,k

(6.19)

Der Kalman-Filter-Estimationsschritt ist folglich durch die Gleichungen (6.17) – (6.19) gegeben. Alternativ zu (6.19) wird h¨ aufig eine andere Form des KovarianzmatrixUpdates verwendet, die so genannte Joseph’s Form. Diese ist gegeben durch − T T P+ xx,k = (I − Kk Hk )Pxx,k (I − Kk Hk ) + Kk Rk Kk

(6.20)

Die Gl. (6.19) und (6.20) sind mathematisch ¨aquivalent: Ausmultiplizieren und Zusammenfassen von Gl. (6.20) liefert − T T P+ xx,k = (I − Kk Hk )Pxx,k (I − Kk Hk ) + Kk Rk Kk − − T T = P− xx,k − Kk Hk Pxx,k − Pxx,k Hk Kk T T T +Kk Hk P− xx,k Hk Kk + Kk Rk Kk − T T = (I − Kk Hk )P− xx,k − Pxx,k Hk Kk T T +Kk (Hk P− xx,k Hk + Rk )Kk .

(6.21)

6.1 Kalman-Filter Gleichungen

133

Setzt man in den letzten Term die Kalman-Gain-Matrix nach Gl. (6.17) ein, erh¨alt man − − T T P+ xx,k = (I − Kk Hk )Pxx,k − Pxx,k Hk Kk  −1 T − T T T +P− (Hk P− xx,k Hk Hk Pxx Hk + Rk xx,k Hk + Rk )Kk − − T T T T = (I − Kk Hk )P− xx,k − Pxx,k Hk Kk + Pxx,k Hk Kk

= (I − Kk Hk )P− xx,k ,

(6.22)

das Kovarianzmatrix-Update nach Gl. (6.19). Die Joseph’s Form des KovarianzmatrixUpdates ist numerisch robuster und sollte daher bevorzugt werden, wenn der zus¨atzliche Rechenaufwand toleriert werden kann.

Propagation Im Propagationsschritt wird der gesch¨ atzte Systemzustand in der Zeit propagiert, d.h. aus dem a posteriori Sch¨ atzwert zum Zeitpunkt k wird der a priori Sch¨atzwert zum n¨ achsten Zeitpunkt, also k + 1, berechnet. Dabei w¨achst die Unsicherheit bez¨ uglich der Zustandssch¨ atzung und die Sch¨ atzfehlerkovarianzmatrix muss entsprechend angepasst werden. Die Zustandssch¨ atzung zum n¨ achsten Zeitpunkt ist gegeben durch − xˆk+1 = E [xk+1 ] ,

(6.23)

durch Einsetzen des Systemmodells erh¨ alt man − xˆk+1 = E [Φk xk + Bk uk + Gk w  k] − xˆk+1 = Φk xˆk+ + Bk uk .

(6.24)

Die zugeh¨ orige Sch¨ atzfehlerkovarianzmatrix ergibt sich mit Gl. (6.1) und Gl. (6.24) zu:   T ˆ − − xk+1 x ˆ − − xk+1 = E x P− xx,k+1 k+1 k+1  ˆ + + Bk uk − (Φk xk + Bk uk + Gk w = E Φk x  k) k  T ˆ + + Bk uk − (Φk xk + Bk uk + Gk w Φk x  ) k k

  T + + T T T ˆ ˆ k  k Gk xk − xk Φk + w = E Φk (xk − xk ) + Gk w    T    T T ˆ + − xk x ˆ + − xk ΦT + E Φk xˆ + − xk w  = E Φk x G k k k k k k   T

 ˆ + − xk ΦT + E Gk w +E Gk w  k x  kw  kT GTk (6.25) k k Da das Systemrauschen w  k erst auf den Systemzustand zum Zeitpunkt k + 1 einen Einfluss hat, siehe Gl. (6.1), ist das Systemrauschen w  k nicht mit dem Sch¨atzfehler zum

134

6 Das Kalman-Filter

ˆ + − xk , korreliert. Damit folgt Zeitpunkt k, also x k P− xx,k+1 = Φk E

  T

 ˆ + − xk x ˆ + − xk x  kw  kT GTk ΦTk + Gk E w k k

T T = Φk P+ xx,k Φk + Gk Qk Gk .

(6.26)

Die Gleichungen (6.24) und (6.26) stellen den Propagationsschritt des Kalman-Filters dar.

6.1.2

Herleitung u ¨ ber Minimierung einer Kostenfunktion

Die Herleitung der Gleichungen des Kalman-Filter-Estimationsschrittes kann auch durch L¨ osung einer Optimierungsaufgabe erfolgen. Dazu wird davon ausgegangen, dass sich der a posteriori Sch¨ atzwert aus dem a priori Sch¨atzwert und der Differenz zwischen ˆ − ergibt, vorliegendem Messwertvektor y˜ und erwartetem Messwertvektor yˆk = Hk x k wobei diese Differenz mit einer zun¨ achst unbekannten Matrix Kk gewichtet wird:  ˆ − + Kk  ˆ− y˜ − Hk x (6.27) xˆk+ = x k k Die eigentliche Optimierungsaufgabe besteht nun darin, die Gewichtungsmatrix Kk so zu w¨ ahlen, dass die Spur der a posteriori Kovarianzmatrix des Sch¨atzfehlers minimal wird. Diese Wahl der Kostenfunktion liegt nahe, da die Spur von P+ xx,k gerade die Summe der Varianzen der Sch¨ atzfehler ist. Allgemein, d.h. f¨ ur eine beliebig gew¨ ahlte Gewichtungsmatrix Kk , erh¨alt man mit Gl. (6.27) f¨ ur P+ xx,k :   T + ˆ + − xk Pxx,k = E xˆk+ − xk x k     T ˆ − − xk + Kk y˜ − Hk x ˆ− x = E xˆk− − xk + Kk y˜ − Hk xˆk− k k   ˆ − − xk + Kk Hk xk + vk − Hk xˆ − = E x k k   T ˆ− xˆk− − xk + Kk Hk xk + vk − Hk x k   ˆ − − xk + Kk vk = E (I − Kk Hk ) x k T   (I − Kk Hk ) xˆk− − xk + Kkvk (6.28) ˆ − − xk ) korreliert sein Da das Messrauschen vk nicht mit dem a priori Sch¨atzfehler (x k kann, ergibt sich    T   + ˆ − − xk xˆ − − xk Pxx,k = E (I − Kk Hk ) x I − HTk KTk + Kk vk vkT KTk k k T = (I − Kk Hk ) P− xx,k (I − Kk Hk ) + Kk Rk Kk . T

(6.29)

6.1 Kalman-Filter Gleichungen

135

Das ist gerade die Jospeh’s Form des Kovarianzmatrix-Updates. W¨ahrend diese offensichtlich f¨ ur beliebige Gewichtsmatrizen Kk gilt, hat Gl. (6.19) nur bei Berechnung der Gewichtsmatrix Kk gem¨ aß Gl. (6.17) G¨ ultigkeit. Gesucht ist nun das Minimum der Spur von P+ xx,k , wobei der beeinflussende Faktor die Gewichtsmatrix Kk ist. Daher muss zur Bestimmung dieses Minimums die Spur von P+ xx,k nach Kk abgeleitet und die Nullstelle dieser Ableitung ermittelt werden:  d Spur P+ xx,k d Kk

!

=0

(6.30)

Dazu werden einige Zusammenh¨ ange aus der Matrizenrechnung ben¨otigt: Bei der Spur einer Matrix handelt es sich um eine skalare Gr¨oße, die Ableitung eines Skalars s nach einer Matrix A ist allgemein gegeben durch ⎛ ds ds ⎞ d a11 d a12 . . . ds ⎜ ds ds ⎟ = ⎝ d a21 d a22 . . .⎠ . (6.31) dA .. . F¨ ur zwei Matrizen A und B gilt unter der Voraussetztung, dass es sich bei dem Produkt AB um eine quadratische Matrix handelt, der Zusammenhang d Spur (AB) = BT . dA

(6.32)

Des Weiteren gilt f¨ ur eine beliebige Matrix A und eine symmetrische Matrix B   d Spur ABAT = 2AB . dA

(6.33)

Da sich beim Transponieren einer Matrix die Hauptdiagonalenelemente nicht ¨andern, folgt sofort f¨ ur die Spur der Transponierten   Spur (A) = Spur AT .

(6.34)

Durch Ausmultiplizieren von Gl. (6.29) findet man nun  − − − − T T T T P+ H = P − K H P − P H K + K P H + R k k xx,k k k xx,k k k Kk . k xx,k xx,k xx,k k (6.35)

136

6 Das Kalman-Filter

Anhand obiger Rechenregeln lassen sich folgende Ableitungen berechnen:  d Spur P− xx,k d Kk



T T d Spur P− xx,k Hk Kk

=0

=

d Kk

 d Spur Kk Hk P− xx,k d Kk T



  T T d Spur Kk Hk P− xx,k Hk + Rk Kk d Kk

= Hk P− xx,k

 T = 2Kk Hk P− H + R k xx,k k

Daraus folgt mit Gl. (6.35)  d Spur P+ xx,k d Kk

T   ! − T H = −2 Hk P− + 2K P H + R . k k xx,k k k = 0(6.36) xx,k

w k

uk

Bk

Gk

+

xk+1

z −1

xk

Hk

Φk

reale Welt

+

Kalman-Filter

Bk

+

vk

+ ˆ− x k+1

− z −1

Φk

Abbildung 6.1: Kalman-Filter-Blockdiagramm.

ˆ+ x k

ˆ− x k

+

Hk

Kk

6.1 Kalman-Filter Gleichungen

137

Aufl¨ osen nach Kk liefert die gesuchte Gewichtungsmatrix  T T = P− H + R Kk Hk P− k xx,k k xx,k Hk  −1 − T T H Kk = P− H P H + R , k k xx,k k xx,k k

(6.37)

die nat¨ urlich mit der im vorigen Abschnitt gefundenen Kalman-Gain-Matrix Gl. (6.17) u ¨ bereinstimmt.

6.1.3

Diskussion der Filtergleichungen

Abb. 6.1 zeigt die prinzipielle Struktur eines Kalman-Filters in Form eines Blockdiagramms. Wie man anhand dieses Blockdiagramms oder den Gleichungen des Estimationsschrittes erkennt, erfolgt eine Anpassung der Zustandssch¨atzung anhand des vorliegenden Messwertvektors durch R¨ uckf¨ uhrung der Differenz zwischen vorliegendem und erwartetem Messwertvektor. Die Grundidee dieser Vorgehensweise soll f¨ ur den Fall eines idealen, nicht mit Rauschen behafteten Messwertvektors, d.h. vk = 0, verdeutlicht werden: Der erwartete Messwertvektor basiert auf der aktuellen Zustandssch¨atzung – ist diese korrekt, werden der erwartete und der vorliegende Messwertvektor u ¨ bereinstimmen und die Zustandssch¨ atzung bleibt unver¨andert. Ist die aktuelle Zustandssch¨atzung nicht korrekt, so stimmen auch der erwartete und der vorliegende Messwertvektor nicht u ¨ berein. In dieser Situation ist es plausibel, basierend auf der verbleibenden Differenz die Zustandssch¨ atzung anzupassen. Beispiel Parametersch¨ atzung Der Einfluss der Kovarianz von Messrauschen und Sch¨atzfehler im Rahmen der Messwertverarbeitung soll anhand eines einfachen Beispiels verdeutlicht werden. Hierzu soll das Kalman-Filter als Parametersch¨ atzer eingesetzt werden, es soll eine konstante, skalare Gr¨ oße gesch¨ atzt werden, die direkt gemessen werden kann. In diesem Szenario sind die Transitionsmatrix Φk und die Messmatrix Hk formal Einheitsmatrizen der Dimension 1 × 1. Da die Transitionsmatrix das Systemverhalten perfekt beschreibt und auch keine mit Rauschen behafteten Eingangsgr¨oßen vorliegen, kann das Systemrauschen zu null angenommen werden. Die Kovarianzmatrizen des Messrauschens und des Sch¨atzfehlers sind hier skalare Gr¨oßen, die Kalman-Gain-Matrix kann daher wie folgt geschrieben werden: Kk =

P− xx,k

(6.38)

P− xx,k + Rk

Damit vereinfachen sich die Gleichungen des Estimationsschrittes zu x ˆ+ k

=

x ˆ− k

+

P− xx,k P− xx,k + Rk

− P+ xx,k = Pxx,k ·

  y˜k − xˆ− k

Rk . + Rk

P− xx,k

(6.39) (6.40)

138

6 Das Kalman-Filter

Im Folgenden soll die resultierende Zustandssch¨atzung f¨ ur einige Sonderf¨alle betrachtet werden: 1. Rk = 0. Es liegt ein perfekter Messwert vor. Durch Einsetzten in (6.39) erkennt man, dass dieser Messwert als neue Zustandssch¨atzung direkt u ¨ bernommen wird. Die bisherige Zustandssch¨ atzung wird verworfen: x ˆ+ ˜. k =y ¨ Da aufgrund der Ubernahme des perfekten Messwertes eine perfekte Zustandssch¨ atzung vorliegt, liefert Gl. (6.40) eine verschwindende Sch¨atzfehlervarianz: P+ xx,k = 0. alt praktisch keine Information. In diesem Fall wird 2. Rk → ∞. Der Messwert enth¨ die Zustandssch¨ atzung nicht ver¨ andert: x ˆ+ ˆ− k =x k. Durch Berechnung des entsprechenden Grenzwertes von Gl. (6.40) erkennt man, dass wie zu erwarten auch keine Anpassung der Sch¨atzfehlervarianz stattfindet: − P+ xx,k = Pxx,k . 3. P− atzung wird als perfekt angesehen, daher xx,k = 0. Die aktuelle Zustandssch¨ wird diese auch bei Vorliegen eines mit Rauschen behafteten Messwertes nicht mehr ver¨ andert: x ˆ+ ˆ− k = x k. Folglich resultiert f¨ ur die Sch¨ atzfehlervarianz: P+ xx,k = 0. atzung enth¨alt praktisch keine Information. 4. P− xx,k → ∞. Die bisherige Zustandssch¨ In dieser Situation ist es sinnvoll, bei Vorliegen eines Messwertes diese Zustandssch¨ atzung zu verwerfen und diesen Messwert direkt zu u ˆ+ ˜. ¨ bernehmen: x k =y Folglich ist die neue Sch¨ atzfehlervarianz durch die Varianz des Messwertes bestimmt: P+ xx,k = Rk . allen ergibt sich der a posteriori Sch¨atzwert als ein gewichteter MittelIn allen anderen F¨ wert von Messwert und a priori Sch¨ atzwert, wobei die Gewichtung von den vorliegenden Messwert- und Sch¨ atzfehlervarianzen abh¨ angt. Dies ist anhand der Wahrscheinlichkeitsdichtefunktionen in Abb. 6.2 illustriert.
Ein großer Vorteil des Kalman-Filters ist dessen Modularit¨at. Es ist keinesfalls notwendig, den Zeitraum zwischen zwei Messwerten mit einem einzigen Propagationsschritt zu u ucken. Meist ist es sinnvoll, stattdessen den Propagationsschritt mit einer festen ¨ berbr¨ Update-Rate aufzurufen und dann zu jedem Zeitpunkt festzustellen, ob Messwerte vorliegen und ein Estimationsschritt ausgef¨ uhrt werden muss. Ferner ist es nicht zwingend notwendig, aus allen zu einem Zeitpunkt vorliegenden Messwerten einen Messwertvektor zu bilden und diesen in einem einzigen Estimationsschritt zu verarbeiten. Sind die einzelnen Messwerte nicht miteinander korreliert1, kann stattdessen jede skalare Einzelmessung im Rahmen eines Estimationsschrittes verarbeitet werden, es werden dann so viele Estimationsschritte durchgef¨ uhrt, wie Messwerte zu diesem Zeitpunkt vorliegen. Dies hat zwei Vorteile: Bei der Berechnung der Kalman-Gain-Matrix muss eine 1 Eine separate Verarbeitung skalarer Einzelmessungen ist auch dann m¨ oglich, wenn diese miteinander in bekannter Weise korreliert sind. In diesem Fall muss vor der Verarbeitung durch eine geeignete Transformation eine Dekorrelation durchgef¨ uhrt werden, siehe [38].

6.1 Kalman-Filter Gleichungen

139

0.5 Messwert WDF A priori WDF A posteriori WDF

0.45 0.4 0.35 0.3 0.25 0.2 0.15 0.1 0.05 0

−4

−2

y˜k

0

x ˆ+ k

2

x ˆ− k

4

6

8

Abbildung 6.2: Wahrscheinlichkeitsdichtefunktionen des Messwertes, der a priori und der a posteriori Zustandssch¨ atzung.

Matrix invertiert werden, deren Dimension durch die Anzahl der Messwerte gegeben ist; liegen viele Messwerte zu einem Zeitpunkt vor, muss eine entsprechend große Matrix invertiert werden, was numerisch kritisch sein kann. Werden hingegen nur skalare Messwerte verarbeitet, reduziert sich die Berechnung der Inversen zu einer einfachen Division, die numerisch unkritisch ist. Des Weiteren ist denkbar, dass zu verschiedenen Zeitpunkten eine unterschiedliche Anzahl von Messwerten vorliegt, z.B. wenn mehrere Sensoren mit verschiedenen Datenraten verwendet werden. Sollten alle zu einem Zeitpunkt vorliegenden Messwerte in einem einzigen Estimationsschritt verarbeitet werden, m¨ usste die Implementierung des Filters alle bez¨ uglich der Messwertverf¨ ugbarkeit m¨oglichen Permutationen abdecken. In diesem Szenario w¨are es sinnvoller, f¨ ur jeden Sensor einen eigenen Estimationsschritt zu implementieren und diesen bei Bedarf aufzurufen.

Eigenschaften Neben den bisher angesprochenen Aspekten sollen im Folgenden die wesentlichen Eigenschaften des Kalman-Filters kurz angesprochen werden. F¨ ur den Fall, dass die angesprochenen Voraussetzungen erf¨ ullt sind, gilt: • Das Kalman-Filter ist der optimale Sch¨atzalgorithmus zur Zustandssch¨atzung dieses linearen Systems. Das bedeutet, das kein anderes Filter bei dieser Aufgabenstellung bessere Ergebnisse erzielen kann.

140

6 Das Kalman-Filter

• Das Kalman-Filter ist asymptotisch stabil: Der Einfluss der notwendigen Initiaˆ − verschwindet f¨ lisierung der Kovarianzmatrix P0 und des Zustandsvektors x ur 0 t → ∞. • Die gelieferten Sch¨ atzwerte sind wirksam (engl. efficient estimate): Kein anderer Sch¨ atzer kann Sch¨ atzwerte mit einer kleineren Varianz liefern. • Die Sch¨ atzwerte sind erwartungstreu (engl. unbiased ): Die Sch¨atzwerte stimmen im Mittel mit den tats¨ achlichen Werten u ¨ berein. • Wird das Kalman-Filter als Parametersch¨atzer eingesetzt, sind die gelieferten Sch¨ atzungen konsistent: Geht die Anzahl der zur Verf¨ ugung stehenden Messungen gegen unendlich, so gehen die Varianzen der Sch¨atzfehler gegen null. • Das Kalman-Filter liefert den wahrscheinlichsten Sch¨atzwert (engl: maximum likelihood estimate): Der vom Kalman-Filter gelieferte Sch¨atzwert ist der Mittelwert des Zustandsvektors xk . Da bei einer Gaußverteilung aber Mittelwert, Median und Modalwert zusammenfallen, ist dies auch der wahrscheinlichste Wert. • Liegt kein Systemrauschen vor, ist das Kalman-Filter identisch zu einem rekursiven Least-Squares-Sch¨ atzer. Ausf¨ uhrliche Diskussionen und weiterf¨ uhrende Betrachtungen des Kalman-Filters sind in [17],[20],[26],[35], [38],[85],[86],[90] oder [76] zu finden. Im Folgenden sollen lediglich einige der wesentlichsten Aspekte, die beim Einsatz eines Kalman-Filters zu ber¨ ucksichtigen sind, angesprochen werden.

6.2

Beobachtbarkeit

Ein System heißt beobachtbar, wenn bei bekannten Eingangsgr¨oßen der initiale Syugung stehenden Messungen stemzustand x0 nach endlicher Zeit anhand der zur Verf¨ y0 , y1 , . . . yk−1 eindeutig ermittelt werden kann. Eine Aussage u  ¨ ber die Beobachtbarkeit eines Systems kann anhand der Beobachtbarkeitsmatrix getroffen werden. F¨ ur skalare Messwerte ist die Beobachtbarkeitsmatrix Mb gegeben durch ⎛ ⎞ ⎛ ⎞ y0 H0 ⎜ y 1 ⎟ ⎜ H 1 Φ1 ⎟ ⎜ . ⎟=⎜ ⎟x0 , .. ⎝ . ⎠ ⎝ ⎠ . . yn−1 Hn Φn−1 &' ( %

(6.41)

Mb

wobei n die Dimension des Zustandsvektors bezeichnet. Das System ist beobachtbar, wenn die Matrix Mb der Dimension n × n den H¨ochstrang n besitzt, d.h. die Determinante von Mb nicht verschwindet. In diesem Fall ist Mb invertierbar, und das Gleichungssystem (6.41) kann nach dem Systemzustand x0 aufgel¨ost werden. Dieses Konzept soll anhand eines einfachen Beispiels verdeutlicht werden.

¨ 6.3 Ubergang kontinuierlich – diskret Beispiel eindimensionale Bewegung Gegeben sei das System





x 1 T 0 x = + u . v k+1 0 1 1 k v k Stehen f¨ ur dieses System Positionsmessungen zur Verf¨ ugung,

  x yk = 1 0 , v k ergibt sich die Beobachtbarkeitsmatrix zu

1 0 Mb = . 1 T

141

(6.42)

(6.43)

(6.44)

Die Determinante dieser Matrix ist ungleich null, die Matrix ist invertierbar und das System damit beobachtbar. Dies ist nicht verwunderlich, da durch Messungen der Position sowohl auf Position als auch auf Geschwindigkeit geschlossen werden kann. Stehen jedoch anstelle von Positionsmessungen nur Geschwindigkeitsmessungen zu Verf¨ ugung,

  x , (6.45) yk = 0 1 v k erh¨ alt man als Beobachtbarkeitsmatrix

0 1 Mb = . 0 1

(6.46)

Diese Matrix ist nicht invertierbar und das System damit unbeobachtbar. Das ist auch anschaulich unmittelbar klar, da nur durch Messung der Geschwindigkeit nicht auf eine unbekannte Anfangsposition geschlossen werden kann.
Beobachtbarkeit im Rahmen der stochastischen Filterung kann als Analogon zur Steuerbarkeit bei einem regelungstechnischen Problem gesehen werden: Steuerbarkeit besagt, dass ein System durch geeignete Eingangsgr¨oßen in endlicher Zeit von einem beliebigen uhrt werden kann. Anfangszustand nach xk = 0 u ¨ berf¨

6.3

¨ Ubergang kontinuierlich – diskret

H¨ aufig ist das einem Kalman-Filter zugrunde liegende Systemmodell nicht in zeitdiskreter Form wie in Gl. (6.1) gegeben, sondern liegt im Zeitkontinuierlichen in Form von Differentialgleichungen vor: x˙ (t) = F(t)x(t) + B(t)u(t) + G(t)w(t) 

(6.47)

142

6 Das Kalman-Filter

¨ Wie ein zeitdiskretes Aquivalent des deterministischen Anteils des Systemmodells Gl. (6.47) gewonnen werden kann, wurde bereits in Abschnitt 2.3 er¨ortert. F¨ ur das Systemrauschen gilt im Zeitkontinuierlichen

 E w(t)  w(τ  )T = Qδ(t − τ ) , (6.48) wobei es sich bei Q um die spektrale Leistungsdichte des Systemrauschens handelt. Es wird angenommen, dass sich der zeitdiskrete Rauschterm als Mittelwert des zeitkontinuierlichen Rauschterms innerhalb des Abtastintervalls ergibt: 1 w k = T

t
k+1

w(τ  ) dτ

(6.49)

tk

Damit findet man f¨ ur das Systemrauschen in analoger Weise zu Gl. (3.197) einen Zusammenhang zwischen Kovarianzmatrix im Zeitdiskreten und spektraler Leistungsdichte im Zeitkontinuierlichen gem¨ aß ⎡⎛ ⎞ ⎛ tk+1 ⎞T ⎤ t
k+1


1 ⎥ ⎢ 1 Qk = E w kw  kT = E ⎣⎝ w(τ  ) dτ ⎠ ⎝ w(τ  ) dτ ⎠ ⎦ T T tk

1 = 2 T

t
k+1 t
k+1



T dα dτ E w(τ  )w(α) 

tk

1 = 2 T

tk

tk

t
k+1 t
k+1

1 Q · δ(τ − α) dα dτ = 2 T

tk

tk

t
k+1

Q dτ =

Q . T

(6.50)

tk

Bei der Berechnung der Matrix Gk sind – wie bei der Eingangsmatrix, siehe Gl. 2.70 sowie Gl. 2.72 und 2.73 – verschiedene Approximationen m¨oglich:

1 (I + Φk ) GT 2 Gk ≈ Φk GT Gk ≈ GT Gk ≈

(6.51) (6.52) (6.53)

Bei der Verwendung der N¨ aherung Gl. (6.51) erh¨alt man f¨ ur das Systemrauschen im Zeitdiskreten   T  k ) (Gk w k) Gk Qk GTk = E (Gk w Q 1 T 1 (I + Φk ) GT · · G (I + Φk )T T 2 T 2 1 = (I + Φk ) GQGT (I + Φk )T T , 4

=

(6.54)

6.4 Nichtlineare System- und Messmodelle

143

bei Verwendung von Gl. (6.52) ergibt sich entsprechend Gk Qk GTk = Φk GQGT ΦTk T ,

(6.55)

und mit Gl. (6.53) erh¨ alt man Gk Qk GTk = GQGT T .

(6.56)

H¨ aufig ist es nicht von entscheidender Bedeutung, welche der angegebenen N¨aherungen verwendet wird: Bei der Modellierung der Unsicherheit des Systemmodells als weißer, normalverteilter Zufallsvektor ist eine gewisse Heuristik unvermeidlich, so dass bei komplexen Systemen das Systemrauschen meist eher als eine M¨oglichkeit des Filter-Tunings ¨ verstanden werden muss und nicht ausschließlich aufgrund mathematischer Uberlegungen gew¨ ahlt werden kann. Neben der unabh¨ angigen Diskretisierung von G(t) und w(t)  ist auch eine gemeinsame ¨ Behandlung m¨ oglich. Das zeitdiskrete Aquivalent von Gl. (6.47) lautet xk+1 = eA(tk+1 −tk ) xk +

t
k+1

eA(tk+1 −τ ) Bu (τ ) dτ

tk t
k+1

eA(tk+1 −τ ) Gw  (τ ) dτ .

+

(6.57)

tk

Nach einiger Rechnung erh¨ alt man f¨ ur die Kovarianzmatrix des Systemrauschens den allgemeinen Ausdruck ⎡⎛ tk+1 ⎞
eA(tk+1 −τ ) Gw(τ  ) dτ ⎠ Gk Qk GTk = E ⎣⎝ tk

⎛ tk+1 ⎞T ⎤
⎥ ⎝ eA(tk+1 −τ ) Gw(τ  ) dτ ⎠ ⎦ tk


T =

 T eAτ GQGT eAτ dτ .

(6.58)

0

Die Berechnung des Integrals ist in den meisten F¨allen nur n¨aherungsweise m¨oglich.

6.4

Nichtlineare System- und Messmodelle

Handelt es sich beim zu beobachtenden System um ein nichtlineares System oder ist der Zusammenhang zwischen Messwerten und Zustandsvektor nichtlinear, kann bei nicht zu

144

6 Das Kalman-Filter

großen Nichtlinearit¨ aten ein linearisiertes oder ein erweitertes Kalman-Filter eingesetzt werden.

6.4.1

Linearisiertes Kalman-Filter

Bei einem linearisierten Kalman-Filter wird ein zu beobachtendes, nichtlineares System x˙ = f (x) + Bu + Gw 

(6.59)

¯ in eine Taylor-Reihe entwickelt, die nach dem linearen um einen Linearisierungspunkt x Glied abgebrochen wird:     ˙x ≈ f x + ∂ f (x)  ∂x 

 · x − x + Bu + Gw 

(6.60)

 x= x

Analog findet man f¨ ur den gesch¨ atzten Systemzustand  ˙ ˆ + Bu xˆ = f x

  ∂ f (x)   ≈ f x +  ∂x 

 ˆ − x + Bu . · x

(6.61)

 x= x

Subtrahiert man Gl. (6.60) von Gl. (6.61), erh¨alt man  ∂ f(x)  ˙ ˆ ˙ x − x =  ∂x 

 · xˆ − x − Gw .

(6.62)

x=  x

ˆ − x und w¨ ˆ als LinearisierungsSetzt man ∆x = x ahlt den gesch¨atzten Systemzustand x 2 punkt , so ergibt sich  (x)  ∂ f ∆x˙ =  ∂x 

· ∆x − Gw .

(6.63)

 x= x

Gleichung (6.63) beschreibt n¨ aherungsweise die zeitliche Propagation der Sch¨atzfehler durch das nichtlineare System und stellt das einem linearisierten Kalman-Filter zugrunde liegende Systemmodell dar. Die Matrix  ∂ f(x)  (6.64) F=  ∂x  ˆ  x= x

wird als Jacobi-Matrix der Funktion f(x) bezeichnet. 2 Es ist auch m¨ oglich, nicht den gesch¨ atzten Systemzustand sondern eine nominale Trajektorie als Linearisierungspunkt zu w¨ ahlen.

6.4 Nichtlineare System- und Messmodelle

145

Das linearisierte Kalman-Filter sch¨ atzt den Systemzustand nicht direkt, stattdessen wird der Fehler eines außerhalb des eigentlichen Filters gespeicherten, vermuteten Systemzustandes gesch¨ atzt. Daher ist auch die Bezeichnung Error State Space oder indirektes Kalman-Filter gebr¨ auchlich. Wird anhand dieses gesch¨atzten Fehlers der vermutete Systemzustand korrigiert, spricht man von einem Closed Loop Error State Space Kalman-Filter. Ein solches Filter verarbeitet auch die vorliegenden Messwerte nicht direkt, statt dessen wird die Differenz aus den vorliegenden Messwerten und den aufgrund des vermuteten Systemzustandes erwarteten Messwerten als Messung“ verarbeitet. Das Messmodell ” wird hierbei in gleicher Weise wie das Systemmodell bestimmt: Der nichtlineare Zusammenhang zwischen Messwertvektor und Systemzustand y˜k = hk (xk ) + vk

(6.65)

wird in eine Taylor-Reihe entwickelt, wobei nach dem linearen Glied abgebrochen wird:  hk (xk )    ∂ y˜k ≈  · xk − x¯k + vk (6.66)  ∂xk  ¯ xk =  xk

Analog kann man schreiben:  hk (xk )  ∂ yˆk ≈   ∂xk 

 ¯k ˆk − x · x

(6.67)

¯k xk =  x

Die Subtraktion dieser beiden Gleichungen liefert mit ∆y˜k = yˆk − y˜k bei Verwendung des vermuteten Systemzustandes als Linearisierungspunkt das Messmodell des linearisierten Kalman-Filters,  hk (xk )  ∂ ∆y˜k = · ∆xk − vk . (6.68)  ∂xk  ˆ  xk = xk

Auf der Grundlage des Systemmodells (6.63) und des Messmodells (6.68) kommen die u ¨ blichen Kalman-Filter-Gleichungen zum Einsatz. Es ist jedoch darauf zu achten, dass die Propagation des vermuteten Systemzustandes durch L¨osung des deterministischen Anteils der nichtlinearen Differentialgleichung (6.59) erfolgt; die Transitionsmatrix, die anhand der Jacobi-Matrix nach Gl. (6.64) und Gl. (2.68) ermittelt wird, wird nur zur Propagation der Kovarianzmatrix des Sch¨atzfehlers ben¨otigt. Die Berechnung des zu verarbeitenden Messwertes ∆y˜k muss ebenfalls auf Grundlage des nichtlinearen Messmodells erfolgen:  ˆk −  ∆y˜k = hk x (6.69) y˜k Die durch die Jacobi-Matrix des nichtlinearen Messmodells gegebene Messmatrix  ∂hk (xk )  Hk = (6.70)  ∂xk  ˆ  xk = xk

146

6 Das Kalman-Filter

wird nur zur Berechnung der Kalman-Gain-Matrix und zur Anpassung der Kovarianzmatrix des Sch¨ atzfehlers verwendet. Wird nach der Messwertverarbeitung der vermutete Systemzustand mit Hilfe der gesch¨ atzten Fehler korrigiert, muss der die Sch¨atzfehler enthaltende Zustandsvektor des Filters zu null gesetzt werden. Daher kann im anschließenden Propagationsschritt auf die Propagation des Filterzustandes verzichtet werden, neben dem vermuteten Systemzustand muss nur die Kovarianzmatrix der Sch¨atzfehler propagiert werden.

6.4.2

Erweitertes Kalman-Filter

Das erweiterte Kalman-Filter3 (EKF) unterscheidet sich von dem linearisierten KalmanFilter dadurch, dass nicht die Fehler eines vermuteten Systemzustandes, sondern der Systemzustand selbst gesch¨ atzt wird. Man spricht daher auch von einem direkten KalmanFilter, die Bezeichnung Total State Space Kalman-Filter ist ebenfalls u ¨ blich. Im Propagationsschritt wird der gesch¨ atzte Systemzustand anhand des nichtlinearen Systemmodells in der Zeit propagiert. Die Propagation der Kovarianzmatrix des Sch¨atzfehlers erfolgt – wie beim linearisierten Kalman-Filter – unter Verwendung einer durch Linearisierung gewonnenen Transitionsmatrix. Ist das nichtlineare Systemmodell im Zeitdiskreten gegeben, xk+1 = f(xk , uk ) + Gk wk ,

(6.71)

erh¨ alt man die Transitionsmatrix unmittelbar durch  ∂ f(xk , uk )  . Φk =   ˆ ∂x

(6.72)

 x= x

Bei einem zeitkontinuierlichen Systemmodell wird die im vorigen Abschnitt beschriebene Vorgehensweise angewandt. Die Messmatrix Hk zur Berechnung der Kalman-Gain-Matrix und zur Anpassung der Kovarianzmatrix des Sch¨ atzfehlers ist durch Gl. (6.70) gegeben, die Pr¨adiktion der Messwerte erfolgt auf Grundlage des nichtlinearen Messmodells Gl. (6.65). Die Gleichungen des erweiterten Kalman-Filters lauten damit insgesamt:   ˆ− = fˆ x+ , uk x k+1 k

(6.73)

+ T T P− xx,k+1 = Φk Pxx,k Φk + Gk Qk Gk  −1 T − T Kk = P− xx,k Hk Hk Pxx Hk + Rk   ˆ+ = xˆ− + Kk  ˆ− x y˜ − hk x k

k

k

− − P+ xx,k = Pxx,k − Kk Hk Pxx,k 3 engl.

extended Kalman filter

(6.74) (6.75) (6.76) (6.77)

6.4 Nichtlineare System- und Messmodelle

147

W¨ ahrend bei linearen Sch¨ atzproblemen mit normalverteiltem System- und Messrauschen das Kalman-Filter die optimale L¨ osung darstellt, sind linearisierte und erweiterte Kalman-Filter suboptimale, im Allgemeinen nicht erwartungstreue Sch¨atzalgorithmen. Sind die Nichtlinearit¨ aten von System- und/oder Messmodell signifikant, k¨onnen diese Filter unzureichende Ergebnisse liefern oder sogar divergieren, so dass andere Filtertypen wie z.B. Partikelfilter eingesetzt werden m¨ ussen. In vielen F¨allen ist der Einsatz eines linearisierten oder erweiterten Kalman-Filtern jedoch gerechtfertigt, da der mit dem Einsatz eines echten“ nichtlinearen Filters verbundene Aufwand in keinem Ver” h¨ altnis zur Verbesserung des Sch¨ atzergebnisses stehen w¨ urde. Eine ausf¨ uhrliche Diskussion von linearisiertem und erweitertem Kalman-Filter ist in [86] zu finden.

6.4.3

Sigma-Point-Kalman-Filter

Die Gruppe der Sigma-Point-Kalman-Filter stellt eine Alternative zum Einsatz eines erweiterten Kalman-Filters dar. Die Grundidee der Sigma-Point-Kalman-Filter besteht darin, Mittelwert und Kovarianz eines normalverteilten Zufallsvektors durch eine Menge deterministisch gew¨ ahlter Sigma-Punkte darzustellen, wobei jeder Sigma-Punkt als ein Zustandsvektor verstanden werden kann. Die Propagation des Zufallsvektors durch eine nichtlineare Funktion wird dadurch realisiert, dass die einzelnen Sigma-Punkte durch diese nichtlineare Funktion propagiert werden und anschließend Mittelwert und Varianz dieser transformierten Sigma-Punkte berechnet wird. Die Umsetzung dieser Idee in einen Filteralgorithmus wird im Folgenden aufgezeigt. Das nichtlineare Systemmodell sei gegeben durch xk = f (xk−1 , uk−1 , w  k) ,

(6.78)

das Messmodell liege in der Form y˜k = h(xk , vk )

(6.79)

vor. Im Rahmen dieser Formulierung besteht die M¨oglichkeit, dass das System- und Messrauschen nichtlinear in das entsprechende Modell eingeht, bei einem erweiterten Kalman-Filter m¨ usste in diesem Fall erst entsprechend linearisiert werden. ¨ Ublicherweise wird zun¨ achst ein erweiterter Zustandsvektor konstruiert, der das Systemund das Messrauschen beinhaltet:  T xˆka = E [xka ] = xTk w  kT vkT

(6.80)

Die Kovarianzmatrix des Sch¨ atzfehlers dieses erweiterten Zustandsvektors ist gegeben durch Paxx,k

⎞ ⎛  Pxx,k 0 0  T ˆ a − x a x ˆ a − x a = E x = ⎝ 0 Qk 0 ⎠ . k k k k 0 0 Rk

(6.81)

148

6 Das Kalman-Filter

Zur Repr¨ asentation von Mittelwert und Kovarianz werden 2L+1 Sigma-Punkte4 gem¨aß a ˆa χ  0,k = x k  a ˆ χ  i,k = xka + ζ Pak i , i = 1 . . . L  a ˆ a − ζ Pa , i = L + 1 . . . 2L = x χ  i,k k ki

(6.82) (6.83) (6.84)

gew¨ ahlt, wobei L die Anzahl der Komponenten des erweiterten Zustandsvektors bezeichnet. Jeder Sigma-Punkt ist folglich ein Vektor der Dimension L × 1, ζ ist ein Skalierungsparameter, der bestimmt, in welchem Abstand sich die Sigma-Punkte vom Mittelwert befinden. Die Wurzel der Matrix Paxx,k , ⎞ ⎛ Pxx,k √0 0 ) (6.85) Paxx,k = ⎝ 0 Qk 0 √ ⎠ 0 0 Rk kann mit Hilfe einer Cholesky-Zerlegung berechnet werden. Wird diese Wurzel so berechnet, dass ) T) Paxx,k = Paxx,k Paxx,k (6.86)  a Pk i einen Vektor der Dimension L × 1, dessen Komponenten ) durch die i-te Zeile der Matrix Paxx,k gegeben sind. gilt, dann bezeichnet

Im Pr¨ adiktionsschritt des Sigma-Point-Kalman-Filters werden diese Sigma-Punkte durch das nichtlineare Systemmodell propagiert:  a,− a,+ (6.87) χ  i,k = f χ  i,k−1 , uk−1 Anschließend werden anhand der transformierten Sigma-Punkte der propagierte Mittelwert und die propagierte Kovarianzmatrix berechnet: ˆ a,− = x k

2L 

a,− wim χ  i,k

(6.88)

i=0

Pa,− xx,k =

2L 2L  

  T a,− a,− c ˆ a,− χ ˆ a,− wij − x − x χ  i,k  j,k k k

(6.89)

i=0 j=0 c Bei wim und wij handelt es sich erneut um Gewichtungsfaktoren.

Im Messschritt des Sigma-Point-Kalman-Filters werden die Sigma-Punkte durch das nichtlineare Messmodell propagiert:  a,−  i,k = h χ Υ (6.90)  i,k 4 Es existieren auch Sigma-Point-Kalman-Filter-Formulierungen, die mit weniger Sigma-Punkten auskommen.

6.4 Nichtlineare System- und Messmodelle

149

Basierend auf dieser Menge pr¨ adizierter Messwerte k¨onnen der Mittelwert, die Kovarianz und die Kreuzkorrelation berechnet werden: yˆk =

2L 

 i,k wim Υ

(6.91)

i=0

Pyy,k =

2L 2L  

  T c  i,k − yˆk Υ  j,k − yˆk wij Υ

(6.92)

  T a,− c ˆ a,− Υ  j,k − yˆk wij − x χ  i,k k

(6.93)

i=0 j=0

Pxy,k =

2L 2L   i=0 j=0

Auf Grundlage dieser Gr¨ oßen ergeben sich die a posteriori Zustandssch¨atzung und Sch¨ atzfehlerkovarianz zu Kk = Pxy,k P−1 yy,k ˆ a,+ x k

 ˆ a,− + Kk y˜k − yˆk = x k

− T P+ xx,k = Pxx,k − Kk Pyy,k Kk .

(6.94) (6.95) (6.96)

Das Kovarianzmatrix-Update Gl. (6.96) erscheint auf den ersten Blick etwas ungew¨ohnlich, hierbei handelt es sich aber lediglich um eine a¨quivalente Schreibweise zu Gl. (5.32): − −1 T P+ xx,k = Pxx,k − Pxy,k Pyy,k Pxy,k −1 −1 T = P− xx,k − Pxy,k Pyy,k Pyy,k Pyy,k Pxy,k − T P+ xx,k = Pxx,k − Kk Pyy,k Kk

(6.97)

Gehen System- und Messrauschen linear in die entsprechenden Modelle ein, kann auf eine Erweiterung des Zustandsvektors verzichtet werden: In diesem Fall kann im Pr¨adiktionsschritt die Kovarianzmatrix des Systemrauschens einfach additiv auf der rechten Seite von Gl. (6.89) ber¨ ucksichtigt werden, das Messrauschen geht durch Addition von Rk auf der rechten Seite von Gl. (6.92) ein. Dadurch kann der Rechenaufwand des Filters unter Umst¨ anden deutlich verringert werden. Bisher wurde noch nicht n¨ aher auf den Skalierungsparameter ζ und die Gewichtsfaktoren c eingegangen. Diese m¨ ussen bestimmten Bedingungen gen¨ ugen, damit ein wim und wij sinnvoller Filteralgorithmus resultiert; ein sinnvoller Mittelwert kann sich beispielsweise nur ergeben, wenn 2L 

wim = 1

(6.98)

i=0

gilt. Die konkrete Wahl dieser Parameter bestimmt den Typ des Sigma-Point-KalmanFilters, die bekanntesten Vertreter sind der Unscented Kalman-Filter und der Central Difference Kalman-Filter.

150

6 Das Kalman-Filter

Eine einfache M¨ oglichkeit der Parameterwahl, die einen Sonderfall des Unscented Kalman-Filters darstellt, ist gegeben durch , w0 < 1 i = 0 m (6.99) wi = 1−w 0 i>0 , 2L 0 i = j c wij (6.100) = 1−w0 i =j 2L # L ζ = . (6.101) 1 − w0 Es l¨ asst sich leicht zeigen, dass mit dieser Wahl der Parameter eine korrekte Repr¨asentation von Mittelwert und Kovarianzmatrix gegeben ist. Durch Einsetzen erh¨alt man 2L 

a wim χ  i,k

= w0 xˆka +

i=0

i=1



 L  a Pk i 2L 1 − w0   # L  1 − w0 ˆ a L  a xk − + Pk i 2L 1 − w0 i=1

L  1 − w0

#

ˆa + x k

1 − w0 ˆ a 1 − w0 ˆ a xk + xk = w0 xˆka + 2 2 ˆa . = x k

(6.102)

F¨ ur die Kovarianz ergibt sich   T 1 − w0  ˆ a ˆ a  ˆ a ˆ a T c ˆa χ ˆa χ  ia − x  ja − x xk − xk xk − xk wij = k k 2L #  # T L  1 − w0 L  a L  a + Pk i Pk i 2L 1 − w0 1 − w0 i=1  # T  # L  1 − w0 L  a L  a + Pk i Pk i − − 2L 1 − w0 1 − w0 i=1

2L 2L   i=0 j=0

1  √ a  a T 1  √ a  a T P i Pk i + P i Pk i 2 i=1 2 i=1 L

=

=

L    a  a T Pk i Pk i = Pak .

L

(6.103)

i=1

Je gr¨ oßer bei dieser Filterformulierung der Parameter w0 < 1 gew¨ahlt wird, desto weiter entfernen sich die Sigma-Punkte vom Mittelwert. Abb. 6.3 illustiert f¨ ur einen zweidimensionalen Zufallsvektor die Lage der Sigma-Punkte sowie die Ellipse, die den 1 − σ Bereich markiert.

6.4 Nichtlineare System- und Messmodelle

5

1σ

x2

x2

5

151

0

−5 −10

−5

0 x

5

0

−5 −10

10

1σ

−5

1

0 x

5

10

1

Abbildung 6.3: Lage der Sigma-Punkte f¨ ur w0 = 0.1 (links) und w0 = 0.7 (rechts) f¨ ur einen zweidimensionalen Zufallsvektor.

Theoretischer Vergleich Erweitertes Kalman-Filter und Sigma-Point-Kalman-Filter verfolgen unterschiedliche Ans¨ atze, um die Propagation von Mittelwert und Kovarianz eines Zufallsvektors durch eine nichtlineare Funktion zu approximieren. Am Beispiel des Mittelwertes sollen die Fehler dieser Approximationen n¨ aher betrachtet werden. Um diese Fehler berechnen zu k¨ onnen, ist es zun¨ achst notwendig, eine Referenzl¨osung zu bestimmen. Die Taylor-Reihenentwicklung einer stetigen, nichtlinearen Funktion f(x) ist gegeben durch   D2 f D3 f D4 f f xˆ + ∆x = f xˆ + D∆x f + ∆x + ∆x + ∆x + · · · . (6.104) 2! 3! 4! Der Operator D∆x f l¨ asst sich mit Hilfe des Nabla-Operators  ∂ T ∂ . . . ∂x∂L ∇ = ∂x1 ∂x 2

(6.105)

darstellen als

    D∆x f = ∆xT ∇ f(x)

ˆ  x= x

,

(6.106)

durch Ausmultiplizieren erh¨ alt man   ∂  ∆xi f(x) D∆x f =  ∂x i i=1 N 

.

(6.107)

ˆ  x= x

F¨ ur die Reihenterme h¨ oherer Ordnung ergibt sich analog  ⎞i ⎛  N i  D∆ 1 ⎝ ∂ ⎠   xf ∆xj . f (x) =  i! i! j=1 ∂xj  ˆ  x= x

(6.108)

152

6 Das Kalman-Filter

Um den Mittelwert eines Zufallsvektors x nach der Propagation durch die nichtlineare Funktion berechnen zu k¨ onnen, muss der Erwartungswert der Reihenentwicklung berechnet werden. Der transformierte Zufallsvektor sei im Folgenden mit y = f(x) bezeichnet, dessen Erwartungswert ergibt sich zu       2  3  4  D D D f f f ∆ x ∆ x ∆ x ˆ + ∆x = f x ˆ + E D∆x f + + + + ··· . yˆ = E f x 2! 3! 4! (6.109) Da die Dichte ∆x als gaußf¨ ormig angenommen ist und per Definition mittelwertfrei ist, verschwinden alle Momente ungerader Ordnung: F¨ ur eine symmetrische Dichtefunktion px (x) einer mittelwertfreien Zufallsvariablen x gilt px (x) = px (−x).

(6.110)

die Momente ungerader Ordnung ergeben sich mit diesen Voraussetzungen zu

2n+1

E x






∞

x2n+1 px (x)dx

= −∞
∞

=


x2n+1 px (x)dx +

0




∞

=

x2n+1 px (x)dx −

0

x2n+1 px (x)dx

−∞
−∞

0

x2n+1 px (x)dx .

(6.111)

0

Durch Substitution von x = −u und folglich dx = −du im zweiten Integralterm erh¨alt man schließlich
2n+1

E[x

∞

]=


2n+1

x 0




∞

=

0


2n+1

x 0




∞

= 0

= 0.

px (x)dx − px (x)dx − 0


x2n+1 px (x)dx −

∞

∞

∞

(−u)2n+1 px (−u)(−1)du (−1)2n+2 u2n+1 px (u)du u2n+1 px (u)du

0

(6.112)

Damit verschwinden alle Reihenglieder ungerader Ordnung in Gl. (6.109) und man kann schreiben:    2  4  D∆ D∆ xf xf ˆ ˆ  y = f x + E  + + ··· (6.113) 2! 4!

6.4 Nichtlineare System- und Messmodelle

153

Im Folgenden soll die Taylor-Reihe nur noch bis zum Glied zweiter Ordnung detailliert betrachtet werden, da die Gleichungen f¨ ur die Glieder h¨oherer Ordnung sehr un¨ ubersichtlich werden. Mit 2  D∆ xf

2!

 D∆x D∆x f =

2!

=

∆xT ∇∆xT ∇ 2!



f =

∇T ∆x∆xT ∇ 2!

f (6.114)

alt man f¨ ur den Mittelwert des transformierten Zufallsvektors und E[∆x∆xT ] = Pa erh¨ ˆ) + yˆ = f(x 

∇T Pa ∇  f + ··· . 2!

(6.115)

Dieser Ausdruck kann mit den von einem erweiterten Kalman-Filter und von einem Sigma Point Kalman-Filter gelieferten Approximationen verglichen werden. Transformation des Mittelwertes beim erweiterten Kalman-Filter Der transformierte Mittelwert ergibt sich bei Verwendung eines erweiterten KalmanFilters zu  ˆ . (6.116) yˆEKF = f x  Offensichtlich geht die Varianz der Zustandssch¨atzung nicht in die Propagation des Mittelwertes ein. Durch Vergleich mit dem exakten Ergebnis Gl. (6.115) erkennt man, dass die beim erweiterten Kalman-Filter verwendete Approximation nur bis zur ersten Ordnung genau ist5 . Transformation des Mittelwertes beim Sigma-Point-Kalman-Filter Die Berechnung des transformierten Mittelwertes erfolgt beim Sigma Point KalmanFilter anhand der transformierten Sigma-Punkte, die mit der Kurzschreibweise σi = √ ζ Pa i wie folgt dargestellt werden k¨ onnen:  ˆ + σi  i = f ( Υ χi ) = f x

(6.117)

Durch eine Taylor-Reihenentwicklung der nichtlinearen Funktion um den Entwicklungsˆ erh¨ puntk x alt man  D2 f D3 f D4 f  i = f xˆ + Dσ f + σi + σi + σi + · · · . Υ i 2! 3! 4!

5 Es

(6.118)

k¨ onnen durch Linearisierung auch Filter h¨ oherer Ordnung gefunden werden, siehe z.B. [35].

154

6 Das Kalman-Filter

Bei einer Wahl der Gewichtungsfaktoren nach Gl. (6.99) ergibt sich folgender transformierter Mittelwert: 2L   0 + 1 − w0 i yˆSP KF = w0 Υ Υ 2L i=1

  2L   1−w  Dσ2 i f Dσ3 i f 0 ˆ ˆ    + + ··· = w0 f x + f x + Dσi f + 2L i=1 2! 3! (6.119)

Da die Sigma-Punkte symmetrisch um den Mittelwert xˆ verteilt sind, entf¨allt in der Summe auf der rechten Seite von Gl. (6.119) der Term erster Ordnung. Die h¨oheren Terme ungerader Ordnung entfallen ebenso, da diese von den entsprechenden Momenten ungerader Ordnung abh¨ angen, die bei symmetrischen Dichtefunktionen verschwinden. Damit erh¨ alt man yˆSP KF

2L  1−w  0 ˆ + = f x 2L i=1



Dσ2 i f Dσ4 i f + + ··· 2! 4!

 .

(6.120)

¨ Aus Gr¨ unden der Ubersichtlichkeit sollen nur Terme bis zur zweiten Ordnung weiter betrachtet werden. Mit Gl. (6.114) ergibt sich

2L T  1−w  ∇ σi σiT ∇  0 ˆ ˆ  ySP KF = f x +  f + ··· 2L i=1 2!   $2L  1−w T T  σ  σ ∇ ∇ i 0 i i=1 ˆ + = f x f + · · · 2L 2!   $L  1−w T T ∇  σ  σ ∇ i 0 i i=1 ˆ + f + · · · . = f x L 2!

(6.121)

Hierbei wurde ausgenutzt, dass aufgrund der Symmetrie der Sigma-Punkte σi = −σL+i √ T gilt und damit σi σiT = σL+i σL+i . Da σi = ζ Pa i einen Vektor der Dimension L × 1 √ bezeichnet, dessen Komponenten sich aus der skalierten i-ten Zeile der Matrix Pa ergeben, kann man schreiben: L  i=1

√ T √ σi σiT = ζ Pa ζ Pa =

L Pa 1 − w0

(6.122)

6.4 Nichtlineare System- und Messmodelle

155

Insgesamt erh¨alt man so f¨ ur den transformierten Mittelwert bei Verwendung eines Sigma-Point-Kalman-Filters

yˆSP KF

  $L  1−w T T ∇  σ  σ ∇ i 0 i i=1 f + · · · = f xˆ + L 2!  ∇T Pa ∇

= f xˆ + f + · · · . 2!

(6.123)

Durch Vergleich mit Gl. (6.115) erkennt man, dass dieses Ergebnis bis zum Term zweiter Ordnung mit dem exakten Ergebnis u ¨ bereinstimmt, und zwar unabh¨angig von der Wahl des Parameters w0 . Da der Term dritter Ordnung sowohl beim Sigma-Point-KalmanFilter als auch bei der exakten Berechnung verschwindet, spricht man bez¨ uglich des transformiertern Mittelwertes von einer Genauigkeit dritter Ordnung. Werden wie z.B. beim Unscented Filter die Gewichtsfaktoren nach einer etwas komplexeren Vorschrift als Gl. (6.99) – (6.101) gew¨ ahlt, kann durch eine geschickte Festlegung der Gewichtsfaktoren zus¨ atzlich versucht werden, den Term vierter Ordnung anzun¨ahern, siehe [61]. Wie gut dies gelingt, h¨ angt von der vorliegenden Nichtlinearit¨at ab. F¨ ur die Transformation der Kovarianz l¨ asst sich in ¨ahnlicher Art und Weise f¨ ur den Mittelwert zeigen, dass ein erweitertes Kalman-Filter bis zur dritten Ordnung, ein SigmaPoint-Kalman-Filter bis zur f¨ unften Ordnung korrekte Approximationen liefert. Weiterf¨ uhrende Betrachtungen zur Klasse der Sigma-Point-Kalman-Filter sind in [59] zu finden. Fazit Ein Sigma-Point-Kalman-Filter ist in der Lage, Mittelwert und Kovarianz eines Zufallsvektors bei nichtlinearen Transformationen genauer zu erfassen als ein erweitertes Kalman-Filter. Diesem Vorteil steht je nach Applikation ein unter Umst¨anden deutlich gr¨ oßerer Rechenaufwand gegen¨ uber. Ob ein Sigma-Point-Kalman-Filter einem erweiterten Kalman-Filter vorzuziehen ist, h¨ angt davon ab, ob in dem betrachteten Szenario die Terme h¨ oherer Ordnung der nichtlinearen Modelle einen signifikanten Einfluss haben oder nicht. Vereinfacht kann man sagen, je nichtlinearer sich die Filteraufgabe gestaltet, desto eher kann ein Sigma-Point-Kalman-Filter vorteilhaft sein. Bei linearen Systemund Messmodellen liefern Kalman-Filter und Sigma-Point-Kalman-Filter identische Ergebnisse. Da am Ende jedes Propagationsschrittes und jedes Messschrittes bei einem erweiterten Kalman-Filter genau wie bei einem Sigma-Point-Kalman-Filter die Wahrscheinlichkeitsdichtefunktion der Zustandssch¨ atzung durch Mittelwert und Varianz repr¨asentiert wird, lassen sich Sigma-Point-Kalman-Filter und erweitertes Kalman-Filter problemlos kombinieren: So kann z.B. bei einem linearen Systemmodell, aber signifikant nichtlinearen Messmodell der Propagationsschritt eines Kalman-Filters und der Messschritt eines Sigma-Point-Kalman-Filters verwendet werden.

156

6.4.4

6 Das Kalman-Filter

Kalman-Filter 2. Ordnung

Bei nichtlinearen Messgleichungen kann die Leistungsf¨ahigkeit eines erweiterten Kalman-Filters in manchen Situationen durch eine einfache Modifikation gesteigert werden: Der Messwert wird wie bei einem EKF u ¨ blich verarbeitet, die Kovarianzmatrix des Sch¨ atzfehlers wird jedoch noch nicht angepasst. Stattdessen wird basierend auf der verbesserten Zustandssch¨ atzung erneut linearisiert und so eine verbesserte Messmatrix bestimmt. Nun wird der Messwert unter Verwendung dieser verbesserten Messmatrix erneut verarbeitet, um eine erneut verbesserte Zustandssch¨atzung zu erhalten. Diese Iteration kann einige Male durchgef¨ uhrt werden, bis sich die Zustandssch¨atzung nicht mehr andert. Ist dies geschehen, schließt die Anpassung der Kovarianzmatrix des Sch¨atzfehlers ¨ den Messschritt ab. Ein solches erweitertes Kalman-Filter wird als Iterated Extended Kalman-Filter (IEKF)6 bezeichnet. Nat¨ urlich kann auch ein erweitertes Kalman-Filter hergeleitet werden, indem die Taylorreihenentwicklung des nichtlinearen System- und Messmodells nicht nach dem linearen Glied, sondern erst nach dem quadratischen Glied abgebrochen wird. Dieses Filter wird manchmal als Kalman-Filter 2. Ordnung bezeichnet. Gegeben sei das nichtlineares Systemmodell xk+1 = f (xk ) + w k ,

(6.124)

wobei  kmittelwertfreies, weißes, normalverteiltes Rauschen mit der Kovarianzmatrix

w  kT = Qk ist. Der Zustandsvektor besitzt die Dimension n × 1. E w kw Der Propagationsschritt des Kalman-Filters 2. Ordnung lautet dann [73]:  n n   1 ∂ 2 f (xk )  − + ˆ ˆ  xk+1 = fk xk +  2 p=1 q=1 ∂xp ∂xq 

· Ppq

(6.125)

ˆ+  xk = x k

+ T P− k+1 = Φk Pk Φk + Ak + Qk  ∂ f (xk )  Φk =  ∂xk  ˆ+

(6.126) (6.127)

 xk = x

k  n  n  n n   1 ∂ 2 f(xk ) ∂ 2 f T (xk )  Ak =  4 p=1 q=1 r=1 s=1 ∂xp ∂xq ∂xr ∂xs 

· (Pqr Pps + Pqs Ppr )

ˆ+  xk = x k

(6.128)

Hierbei bezeichnet xp die p-te Komponente des Zustandsvektors xk , Ppq ist der Eintrag in der p-ten Zeile und q-ten Spalte der Kovarianzmatrix der Zustandssch¨atzung Pk . 6 Es wird zwischen lokalem IEKF und globalem IEKF unterschieden, bei der beschriebenen Vorgehensweise handelt es sich um ein lokales IEKF.

6.5 Filterung bei zeitkorreliertem Rauschen

157

Unter Annahme des Messmodells Gl. (6.65) ist der Messschritt des Filters 2. Ordnung gegeben durch:  −1 − T T Kk = P− k Hk Hk Pk Hk + Sk + Rk ⎛  n n 1   ∂ 2hk (xk )  + − − ˆ ˜ ˆ ˆ  ⎝ xk = xk + Kk  yk − hk (xk ) −  2 p=1 q=1 ∂xp ∂xq 

⎞

(6.129)

· Ppq ⎠ (6.130)

ˆ−  xk = x k

− P+ k = (I − Kk Hk ) Pk  ∂hk (xk )  Hk =  ∂xk  ˆ−

(6.131) (6.132)

 xk = xk

 n n n n 1     ∂ 2hk (xk ) ∂ 2hkT (xk )  Sk =  4 p=1 q=1 r=1 s=1 ∂xp ∂xq ∂xr ∂xs 

· (Pqr Pps + Pqs Ppr )

ˆ−  xk = x k

(6.133) Das Kalman-Filter zeigt ein ¨ ahnliches Verhalten wie das Sigma-Point-Kalman-Filter, die Berechnung der ben¨ otigten 2. Ableitungen kann jedoch umst¨andlich und fehlertr¨ achtig sein. Andererseits kann durch Berechnung einiger zus¨atzlicher Korrekturterme ein bereits bestehendes EKF in ein Filter 2. Ordnung umgewandelt werden. Ist von vorneherein klar, dass ein Filter 2. Ordnung ben¨otigt wird, ist der Entwurf eines SigmaPoint-Kalman-Filters meist weniger aufw¨ andig und daher vorzuziehen.

6.5

Filterung bei zeitkorreliertem Rauschen

Bei vielen praktischen Anwendungen ist die bisher getroffene Voraussetzung, dass das Mess- und Systemrauschen als weiß angenommen werden kann, nicht erf¨ ullt. Ein Beispiel hierf¨ ur w¨aren Inertialsensoren, die Vibrationen ausgesetzt sind. Sind die Inertialsensoren nicht in der Lage, die den Vibrationen zugrunde liegenden oszillatorischen Rotationen und Translationen aufzul¨ osen, erscheinen diese als zus¨atzliches Rauschen in den Inertialsensordaten. Diese Vibrationen lassen sich nicht als weißes Rauschen modellieren, meist handelt es sich um Schwingungen mit bestimmten Eigenfrequenzen. Damit ist das Leistungsdichtespektrum nicht konstant, folglich liegen Zeitkorrelationen vor. Werden diese Zeitkorrelationen nicht im Filteralgorithmus ber¨ ucksichtigt, kann das zu schlechten oder sogar unbrauchbaren Filterergebnissen f¨ uhren. Dar¨ uber hinaus spiegelt die vom Filter gelieferte Kovarianzmatrix der Sch¨atzfehler nicht die tats¨achliche G¨ ute der Zustandssch¨ atzung wieder. Eine u ute der ¨ beroptimistische Einsch¨atzung der G¨ Zustandssch¨ atzung f¨ uhrt zu einer geringeren Gewichtung der vorliegenden Messwerte, was sogar die Divergenz des Filters verursachen kann. Eine M¨oglichkeit zur Ber¨ ucksichtigung von Zeitkorrelationen ist also nicht nur f¨ ur die Genauigkeit der Zustandssch¨atzung, sondern auch zur Sicherstellung deren Zuverl¨assigkeit von Interesse. Im Folgenden sollen daher die g¨ angigen Verfahren zur Ber¨ ucksichtigung von Zeitkor¨ relationen bei Filterproblemen vorgestellt werden. Zur besseren Ubersichtlichkeit wird

158

6 Das Kalman-Filter

ein Systemmodell ohne Eingangsgr¨ oßen betrachtet, xk+1 = Φk xk + Gk w k .

(6.134)

Das zeitkorrelierte Systemrauschen wird durch einen Rauschprozess der Form w  k = Cw  k−1 +  ηk

(6.135)

beschrieben. Mit einem solchen Modell lassen sich bei geeigneter Wahl des Zustandsvektors w  k , der Gewichtungsmatrix C und dem Vektor ηk auch Rauschprozesse h¨oherer Ordnung darstellen. Das in das Messmodell y˜k = Hk xk + vk

(6.136)

eingehende Messrauschen soll in analoger Weise durch das Rauschprozessmodell vk = Dvk−1 +  µk

(6.137)

beschrieben werden.

6.5.1

Erweiterung des Zustandsvektors

Der u ucksichtigung von zeitkorreliertem Rauschen besteht in der ¨ bliche Ansatz zur Ber¨ Erweiterung des Systemmodells um die Zustandsraummodelle dieser zeitkorrelierten Rauschprozesse. In dem hier betrachteten Fall ergibt sich das erweiterte Systemmodell zu ⎞⎛ ⎞ ⎛ ⎛ ⎞ ⎛ ⎞ x Φk Gk 0 0 x ⎝w  ⎠ + ⎝η ⎠ . ⎠ = ⎝ 0 C 0 ⎠ ⎝w (6.138) 0 0 D v k µ k v k+1 Das Systemrauschen ist durch das weiße Rauschen gegeben, das die zeitkorrelierten Rauschprozesse treibt. Zur Verarbeitung der Messwerte wird die Messmatrix ebenfalls erweitert, man erh¨ alt ⎛ ⎞   x ⎠ . y˜k = Hk 0 I ⎝w  v k

(6.139)

Der normalerweise u ¨ bliche, additive Messrauschterm verschwindet in diesem Messmodell, da das zeitkorrelierte Messrauschen im Zustandsvektor enthalten ist. Offensichtlich w¨ are es sehr einfach m¨ oglich, noch zus¨ atzlich einen weißen Rauschterm f¨ ur das Messrauschen einzuf¨ uhren, so dass insgesamt das Messrauschen als Summe des zeitkorrellierten Rauschprozesses und diesem weißen Rauschterm modelliert werden k¨onnte. Da durch diese Erweiterung des Zustandsvektors formal wieder weißes Mess- und Systemrauschen vorliegt, k¨ onnen die u ¨ blichen Kalman-Filter-Gleichungen Anwendung finden.

6.5 Filterung bei zeitkorreliertem Rauschen

159

Werden zeitkorrelierte Rauschprozesse h¨ oherer Ordnung zur Modellierung des Systemund Messrauschens ben¨ otigt, m¨ ussen die Matrizen, die den Zusammenhang zwischen Zustandsvektor bzw. Messwertvektor und dem jeweiligen zeitkorrelierten Rauschprozess beschreiben, entsprechend angepasst werden. Der Nachteil einer Erweiterung des Zustandsvektors besteht, je nachdem wie viele Rauschprozesse welcher Ordnung ber¨ ucksichtigt werden m¨ ussen, in einer unter Umst¨ anden drastischen Erh¨ ohung des Rechenaufwandes. Zus¨atzlich k¨onnen numerische Probleme auftreten. H¨ aufig sind auch die zus¨atzlichen Zust¨ande mit den zu Verf¨ ugung stehenden Messungen kaum beobachtbar7, die Sch¨atzung des Rauschprozesszustandes ist unbrauchbar. Entscheidend bei der Erweiterung des Zustandsvektors ist jedoch nicht der gesch¨ atzte Rauschprozesszustand, sondern der Einfluss dieser Zust¨ande auf die Propagation und das Update der Kovarianzmatrix des Sch¨atzfehlers. Dieser Einfluss l¨asst sich durch die h¨ aufig zu beobachtende Praxis einer einfache Erh¨ohung eines als weiß angenommenen Mess- und Systemrauschens nicht nachbilden. Trotz der angesprochenen Nachteile stellt die Erweiterung des Zustandsvektors, die in der englischsprachigen Literatur als State Augmentation oder Shaping Filter Approach zu finden ist, ein leistungsf¨ ahiges und universell einsetzbares Verfahren zur Ber¨ ucksichtigung von zeitkorreliertem Rauschen dar.

6.5.2

Messwertdifferenzen

Ein als Measurement Differencing bekanntes Verfahren erlaubt es, auf eine Erweiterung des Zustandsvektors zu verzichten. Die Idee dieses Verfahrens besteht darin, durch einen gewichteten Mittelwert zweier aufeinanderfolgender, mit zeitkorreliertem Rauschen behafteter Messwerte ein neue Messgr¨ oße zu konstruieren, deren Messrauschen weiß ist. Diese neue Messgr¨ oße z˜k ist gegeben durch z˜k = y˜k+1 − Dy˜k .

(6.140)

Durch Einsetzten des Messmodells erh¨ alt man z˜k = Hk+1 xk+1 + vk+1 − D (Hk xk + vk ) ,

(6.141)

mit Hilfe des Systemmodells ergibt sich schließlich z˜k = Hk+1 (Φk xk + Gk w  k ) + vk+1 − D (Hk xk + vk ) = (Hk+1 Φk − DHk ) xk + (Hk+1 Gk w  k + Dvk + µk+1 − Dvk ) ˜  k + µk+1 ) . zk = (Hk+1 Φk − DHk ) xk + (Hk+1 Gk w

(6.142)

Unter der Voraussetzung, dass es sich beim Systemrauschen w  k um weißes Rauschen handelt, ist das in der Gr¨ oße z˜k enthaltene Rauschen als Summe des Systemrauschens und des Rauschterms, das den zeitkorrelierten Messrauschprozess treibt, ebenfalls ein 7 In der englischsprachigen Literatur findet man f¨ ur diese Zust¨ ande daher die Bezeichnung Crap ” States“

160

6 Das Kalman-Filter

weißes Rauschen. Allerdings ist dieser Rauschterm mit dem Systemrauschen korreliert, diese Korrelation l¨ asst sich jedoch berechnen und mit einem sog. Generalized KalmanFilter ber¨ ucksichtigen. Details zu dieser Vorgehensweise sind in [18] zu finden. Neben einem unbedeutenden Initialisierungsproblem ist ein Nachteil dieses Verfahrens, dass es sich nur schwer auf zeitkorrelierte Rauschprozesse h¨oherer Ordnung erweitern l¨ asst, da die resultierenden Gleichungen sehr unhandlich werden. Der Hauptnachteil ist jedoch, dass diese Vorgehensweise auf zeitkorreliertes Messrauschen beschr¨ankt ist, zeitkorreliertes Systemrauschen l¨ asst sich nicht ber¨ ucksichtigen. Ein alternatives Verfahren zur Ber¨ ucksichtigung sowohl von zeitkorreliertem Mess- als auch von zeitkorrelierten Systemrauschen, das ohne eine Erweiterung des Zustandsvektors auskommt, wird in Abschnitt 9.3 vorgestellt.

6.6

Covariance Intersection

Der bisher vorgestellte Standard-Kalman-Filter-Algorithmus setzt voraus, dass das Messrauschen nicht mit dem zu sch¨ atzenden Systemzustand korreliert ist:   E (xk − xˆk )vk = 0 (6.143) Ist diese Voraussetzung verletzt, ist der bisher vorgestellte Kalman-Filter suboptimal. Das kann – je nach Sch¨ atzproblem – irrelevant sein, aber auch von einer Verschlechterung der Performance bis zur Divergenz des Filters f¨ uhren. Die Ursachen f¨ ur diese Kreuzkorrelationen k¨onnen unterschiedlich sein. Eine m¨ogliche Ursache ist zeitkorreliertes Messrauschen. Der Messrauschterm vk geht in die Zustandsvk und sch¨ atzung x+ k ein. Liegen nun Zeitkorrelationen vor, so sind die Rauschterme  ¨ vk+1 nicht unabh¨ angig voneinander. Uber den Propagationsschritt des Filters beeinflusst vk aber auch die a priori Zustandssch¨atzung einen Zeitschritt sp¨ater, x− k+1 . Daher k¨ onnen nun x− und  v nicht unabh¨ a ngig voneinander sein, auch wenn x− vk k+1 k+1 k und  noch unkorreliert waren, siehe hierzu auch Abschnitt 9.3. Korrelationen zwischen Messwerten und Zustandssch¨atzung treten auch in SLAM8 Szenarien auf. Als Beispiel soll hier die Navigation eines Roboters in unbekanntem Gel¨ ande angef¨ uhrt werden. Entdeckt dieser Roboter mit einem Laserscanner eine neue Landmarke, so wird anhand der aktuellen Roboterposition die Position dieser Landmarke gesch¨ atzt. Anschließend kann relativ zu dieser Landmarke navigiert werden. Der Fehler der gesch¨ atzten Roboterposition geht damit aber auch in die Positionssch¨atzung der Landmarke ein und folglich in jede Abstandsmessung, die anschließend zu Navigationszwecken zu dieser Landmarke gemacht wird. In dezentralen Datenfusionsproblemen sind Korrelationen ebenfalls praktisch unvermeidlich. Ein Beispiel hierf¨ ur w¨ are ein Sensornetzwerk, bei dem jeder mit einem Sensor ausgestattete Knoten seine Messwerte mit jedem anderen Knoten teilt. Angenommen, Knoten A verteilt eine Messung und die zugeh¨orige Varianz an Knoten B und Knoten C. 8 Simultaneous

Location and Map Building

6.6 Covariance Intersection

161

W¨ urde nun ein Knoten D diese Messungen von Knoten B und Knoten C als unabh¨angig verarbeiten, so w¨ urde eine deutlich zu geringe Varianz der Zustandssch¨atzung resultieren - schließlich enth¨ alt die von Knoten C zur Verf¨ ugung gestellte Messung gegen¨ uber der von Knoten B zur Verf¨ ugung gestellten Messung keine zus¨atzliche Information, es handelt sich ja nur um die eine, urspr¨ unglich von Koten A stammende Messung. Auch wenn in diesem einfachen Szenario eine Mehrfachverarbeitung von Messwerten sehr einfach verhindert werden k¨ onnte, ist dies bei großen, komplexen Netzwerken schwierig. Eine zus¨ atzliche Schwierigkeit besteht nun meist darin, dass Kreuzkorrelationen zwar vorliegen, diese aber unbekannt sind. In diesen F¨allen kann anstelle eines Kalman-Filters der Covariance-Intersection-(CI-)Algorithmus verwendet werden [60][128]. In den meisten Beschreibungen zu CI wird das Problem betrachtet, zwei Zufallsvektoren gleicher Dimension zu fusionieren. Dieses Problem kann als eine Untermenge des in den vorangegangenen Abschitten betrachteten Sch¨atzproblems verstanden werden: Betrachtet man ausschließlich den Messschritt und fordert Hk = I, so resultiert das angesprochene Sch¨ atzproblem, wobei einer der Zufallsvektoren die a-priori Zustandssch¨atzung, der andere der Messwertvektor ist. Im folgenden soll CI angepasst auf das schon beim Kalman-Filter verwendete, allgemeinere Messmodell beschrieben werden. Durch entsprechende Vereinfachungen kann dann zu den u ¨ blicherweise in der Literatur zu findenden CI-Gleichungen u ¨ bergegangen werden. Bevor auf die Filterung bei unbekannten Kreuzkorrelationen eingegangen wird, soll jedoch der Fall betrachtet werden, dass bekannte Kreuzkorrelationen zwischen Messwerten und Zustandssch¨ atzung vorliegen.

6.6.1

Bekannte Kreuzkorrelationen

Die Gleichungen (6.8) und (6.9) stellen eine allgemeing¨ ultige Vorschrift dar, wie bei ˆ − der a posteriori Sch¨atzgaussverteilten Zufallsvektoren aus einem a priori Sch¨atzwert x k wert xˆk+ berechnet werden kann:  ˆ + = x ˆ − + Pxy,k P−1 y˜k − yˆk x k k yy,k − −1 P+ xx,k = Pxx,k − Pxy,k Pyy,k Pyx,k

Zur Herleitung des Kalman-Filter-Messschrittes wurde vom Messmodell Gl. (6.3), y˜k = Hk xk + vk , ausgegangen. Dabei wurde angenommen, dass die Korrelation von Sch¨atzfehler und Messfehler, Pxv,k , verschwindet. Es ist jedoch auch m¨oglich, ohne diese Annahme einen allgemeineren Messschritt herzuleiten, siehe hierzu Abschnitt 9.3. In diesem Fall erh¨alt man T Pxy,k = P− xx,k Hk + Pxv,k

(6.144)

T T Pyy,k = Hk P− xx,k Hk + Hk Pxv,k + Pvx,k Hk + Rk

(6.145)

162

6 Das Kalman-Filter

1

0.5 c

c c c c

0

c

c

c

c

c c

c

-0.5

c

c

c

PC

-1 -1

c

-0.5

c

c

PA und PB PC , wenn PAB = 0 bei verschiedenen PAB

0

0.5

c

1

Abbildung 6.4: Fusion zweier Zufallsvektoren bei unterschiedlichen Kreuzkorrelationen.

und mit (6.8) und (6.9) unter Verwendung der Abk¨ urzung  Kk =

− T P− xx,k Hk + Pxv,k



− − T T Hk P− k Hk + Hk Pxv,k + Pvx,k Hk + Pvv,k

−1

schließlich  xˆk+ = xˆk− + Kk yk − Hk xˆk−  − − − . P+ xx,k = Pxx,k − Kk Hk Pxx,k + Pvx,k Um den Einfluss von Kreuzkorrelationen zu verdeutlichen, wird die Fusion zweier zweidimensionaler Zufallsvektoren a und b betrachtet, die die Kovarianzmatrizen PA und PB besitzen. Das Fusionsergebnis ist ein Vektor c mit der Kovarianzmatrix PC . Die 1Sigma-Kovarianzellipsen der Vektoren a und b, die sich anhand von PA und PB bestimmen lassen, sind in Abb. 6.4 dargestellt. Zus¨atzlich sind in Abb. 6.4 auch noch die aus PC bestimmten Kovarianzellipsen des Fusionsergebnisses f¨ ur unterschiedliche Szenarien zu sehen: Zum einen, wenn a und b unkorreliert sind (PAB = 0); zum anderen, wenn a und b auf unterschiedliche Weise korreliert sind. Man erkennt, dass bei vorliegenden Korrelationen die Unsicherheit in einer von der Korrelation abh¨angigen Vorzugsrichtung geringer ausf¨ allt als im unkorrelierten Fall, senkrecht dazu ergibt sich jedoch eine gr¨oßere Unsicherheit. W¨ urden nun bei der Fusion der Zufallsvektoren die Kreuzkorrelationen

6.6 Covariance Intersection

163

ignoriert, w¨ urde in dieser Richtung eine zu geringe Varianz des Fusionsergebnisses berechnet. Ein Filter w¨ urde dadurch in diesem Szenario mit der Zeit u ¨ ber-optimistisch, was die G¨ ute der Zustandssch¨ atzung anbelangt – die Folge ist eine zu geringe Gewichtung der Messwerte, das Filter h¨ alt zu stark an der eigenen Zustandssch¨atzung fest, was bis zur Divergenz des Filters f¨ uhren kann. Sind die Kreuzkorrelationen anders als in diesem Beispiel unbekannt, muss dennoch sichergestellt werden, dass keine zu geringe Sch¨ atzfehlerkovarianz auftritt, die zu dieser Entkopplung von Filter und realer Welt f¨ uhren kann. Es l¨ asst sich zeigen, dass f¨ ur alle m¨ oglichen Kreuzkorrelationen die Kovarianzellipse des Fusionsergebnisses c innerhalb der Schnittmenge der Kovarianzellipsen von a und b liegen. Die Grundidee von CI besteht darin, bei unbekannten Kreuzkorrelationen die Kovarianzellipse von c so zu w¨ ahlen, dass sie diese Schnittmenge einschließt. Ist das gegeben, spricht man von einer konservativen Sch¨atzung.

6.6.2

Unbekannte Kreuzkorrelationen

Um eine konservative Sch¨ atzung zu erhalten, werden in [48] folgende Vor¨ uberlegungen angestellt: Wenn C eine positiv semi-definite, symmetrische Matrix ist mit

Pxx Pxv C= , Pvx Pvv

(6.146)

Cα ≥ C

(6.147)

dann gilt

mit Cα =

1 1−α Pxx

0

0 1 P α vv

,

(6.148)

wobei 0 ≤ α ≤ 1 gilt. Ein Beweis dieser Tatsache ist ebenfalls in [48] zu finden. Gleichung (6.147) ist dabei so zu verstehen, dass die Matrix Cα − C eine positiv semi-definite Matrix ist. Soll anhand von (6.8) und (6.9) ein Messschritt hergeleitet werden und ist Pxv,k unbekannt, so erh¨ alt man eine konservative Sch¨atzung, wenn man in Gl. (6.144) – (6.145) nicht mit den Eintr¨ agen der Matrix C, sondern mit den Eintr¨agen von Cα arbeitet. Auf diese Weise ergibt sich 1 P− HT 1 − α xx,k k 1 1 T Hk P− Rk = xx,k Hk + 1−α α

Pxy,k =

(6.149)

Pyy,k

(6.150)

164

6 Das Kalman-Filter

und folglich mit der Abk¨ urzung Kα,k = Pxy,k P−1 yy,k =

Kα,k

1 P− HT 1 − α xx,k k



1 1 T Hk P− Rk xx,k Hk + 1−α α

−1



−1 α 1 1−α − − T T P Hk Pxx,k Hk + Rk = H 1 − α xx,k k α(1 − α) α(1 − α)  −1 − T T = αP− (6.151) xx,k Hk αHk Pxx,k Hk + (1 − α)Rk

schließlich  ˆ − − y˜k xˆk+ = xˆk− − Kα,k Hk x k  1 − P+ . P− xx,k = xx,k − Kα,k Hk Pxx,k 1−α

(6.152) (6.153)

Die Gleichungen (6.151) – (6.153) stellen den CI-Messschritt dar. In der Literatur wird nun bei der Beschreibung von CI u ¨ blicherweise nicht nur wie Eingangs erw¨ ahnt eine Untermenge (Hk = I) des hier betrachteten Sch¨atzproblems betrachtet, meist werden die CI-Gleichungen in Invers-Kovarianzform dargestellt. Die Herleitung der Invers-Kovarianzform von Gl. (6.151) – (6.153) soll im Folgenden ¨ skizziert werden, wobei zur besseren Ubersichtlichkeit kurz Pxx,k = Pk geschrieben wird. Einsetzen von Kα,k in Gl. (6.153) liefert

P+ k =

 −1 1  − − T − T . Pk − αP− αH H P H + (1 − α)R H P k k k k k k k k 1−α (6.154)

T Mit den Substitutionen A = (1 − α)Rk , B = I,CT = αHk P− k Hk folgt aus Gl. (A.3)

P+ k =

 1 1 − T R−1 · P− k − αPk Hk 1−α 1−α k 

−1 1 − T −1 − R Hk Pk . I + αHk Pk Hk 1−α k

(6.155)

6.6 Covariance Intersection Mit A−1 = P− k, B=

P+ k

165

α T −1 1−α Hk Rk ,

1 = 1−α

CT = Hk und Gl. (A.2) erh¨alt man

 − −1 + Pk

α HT R−1 Hk 1−α k k

−1

−1 1 α 1 − α  − −1 Pk HTk R−1 + H k k 1−α 1−α 1−α  −1  −1 = (1 − α) P− + αHTk R−1 k k Hk  −1 = (1 − α) P− + αHTk R−1 k k Hk

P+ k = P+ k  + −1 Pk

(6.156)

Die Gain-Matrix l¨ asst sich nun ebenfalls umschreiben:  −1 − T T Kα,k = αP− k Hk αHk Pk Hk + (1 − α)Rk

  + −1 − T 1 + −1 Pk Hk R (1 − α)Rk = α Pk Pk 1−α k −1  T · αHk P− k Hk + (1 − α)Rk

  + −1 − T 1 −1 P R P = α P+ H (1 − α)R k k k k k 1−α k

−1 1 1 −1 − T −1 R R · I + αHk Pk Hk 1−α k 1−α k

Einsetzen von Gl. (6.156) liefert   − −1 T −1 T (1 − α) P P− Kα,k = αP+ + αH R H k k k k k k Hk

−1 1 1 −1 − T −1 I + αHk Pk Hk · R R 1−α k 1−α k

 −1 − T 1 P− (1 − α) P− = αP+ Pk Hk I + αHTk R−1 k k Hk k 1−α k

−1 1 1 − T −1 R−1 R · P H I + αH k k k 1−α k 1−α k

(6.157)

166

6 Das Kalman-Filter

1 − P = I+ HTk R−1 k 1−α k

−1 1 −1 T R · I + αHk P− H k k 1−α k

1 − T −1 T −1 T −1 P = αP+ R + αH R H H R H k k k k k k 1−α k k k

−1 1 − T −1 R · I + αHk Pk Hk 1−α k

1 + T −1 − T −1 P H R = αPk Hk Rk I + αHk 1−α k k k

−1 1 −1 T R H · I + αHk P− k k 1−α k αP+ k

αHTk R−1 k Hk

T −1 Kα,k = αP+ k Hk R k .

(6.158)

 −1 Multiplikation von Gl. (6.152) von links mit P+ ergibt k   + −1 +     ˆ = P+ −1 x ˆ − − P+ −1 Kα,k Hk xˆ − − y˜k Pk x k k k k k   −1 −  + −1 + T −1 ˆ − P ˆ − − y˜k H  x  x αP H R = P+ k k k k k k k k   + −1 − −1 − T ˆ − αH R ˆ − y˜k Hk x = Pk x k k k k  −1 xˆk− + αHTk R−1 = P+ y˜k − αHTk R−1 k k Hk  k Hk   + −1 +   ˆ = (1 − α) P− −1 x ˆ − + αHT R−1 Hk y˜k Pk x k k k k k   − −1 − + + ˆ ˆ ˜k . H xk = Pk (1 − α) Pk xk + αHTk R−1  y k k

(6.159)

Die Invers-Kovarianzform der Gleichungen (6.151) – (6.153) ist durch (6.156) und (6.159) gegeben. In den CI-Gleichungen tritt nun ein hier mit α bezeichneter Parameter auf. Abb. 6.5 zeigt nun f¨ ur das schon im vorigen Abschnitt betrachtete Sch¨atzproblem die Kovarianzellipse des Fusionsergebnisses f¨ ur unterschiedliche Parameter α. Der Parameter α kann als ein Scharparameter f¨ ur die Kovarianzellipse des Fusionsergebnisses gesehen werden; es l¨ asst sich leicht u ¨ berlegen, dass die bei der Fusion resultierende Kovarianzmatrix f¨ ur α = 0 gleich der Kovarianzmatrix des einen Zufallsvektors und f¨ ur α = 1 gleich der Kovarianzmatrix des anderen Zufallsvektors ist. Bei dieser Parameterwahl w¨ are der gefundene Sch¨ atzer zwar sicherlich konservativ, die bei der Fusion gew¨ unschte Verringerung der Sch¨ atzfehlervarianz w¨ urde jedoch ausbleiben. Der Parameter α sollte so gew¨ ahlt werden, dass die Spur der a posteriori Kovarianzmatrix minimal wird – dieses Optimierungsproblem kann numerisch behandelt werden9 , in [48] werden aber 9 Im

einfachsten Fall durch Probieren verschiedener Paramter α.

6.7 Adaptive Filterung

167

1

0.5 c

c c c c

0

-0.5

c

c

c

c

c

c c

c

c

c

c

c

c

PA und PB PC , wenn PAB = 0 PC , CI bei versch. α

-1 -1

-0.5

0

0.5

c

1

Abbildung 6.5: Fusion zweier in unbekannter Weise korrelierter Zufallsvektoren mit dem CI Algorithmus f¨ ur unterschiedliche Parameter α.

auch analytische L¨ osungen f¨ ur α sowohl bei skalaren und als auch bei vektorwertigen Messungen angegeben. Abschließend sei noch darauf hingewiesen, dass eine Vielzahl von Erweiterungen und Verfeinerungen des CI-Algorithmus existieren. So wird z.B. in [49] aufgezeigt, wie bei unbekannten aber beschr¨ ankten Korrelationen eine kleinere Kovarianzmatrix des Fusionsergebnisses erzielt werden kann, als dies mit dem hier vorgestellten CI-Algorithmus m¨ oglich w¨ are.

6.7

Adaptive Filterung

Allgemein gesagt ist ein adaptives Filter in der Lage, sich an aktuell vorliegende Bedingungen anzupassen – das kann notwendig sein, weil diese entweder im Voraus nicht bekannt sind, oder sich w¨ ahrend des Betriebes des Filters ¨andern k¨onnen. In Kapitel 8 zur GPS/INS-Integration werden beispielsweise Filter entwickelt, die im vorhinein unbekannte Parameter, die Biase der Inertialsensoren, sch¨atzen und dadurch ihre Performance verbessern. So gesehen k¨ onnte man diese Filter auch als adaptive Filter bezeichnen. Auf diese Art der Adaptivit¨ at, die Sch¨ atzung unbekannter Parameter durch Erweiterung des Zustandsvektors des Filters, soll hier jedoch nicht weiter eingegangen werden. Ein Grundproblem beim Einsatz eines Kalman-Filters l¨asst sich auf diese Weise n¨amlich nicht l¨ osen: Um ein gut funktionierendes Filter zu erhalten, muss die Kovarianzmatrix des Systemrauschens, Qk , und die Kovarianzmatrix des Messrauschens, Rk , geeignet ge-

168

6 Das Kalman-Filter

w¨ ahlt werden. Die Ans¨ atze zur L¨ osung dieses Problems lassen sich grob in drei Klassen einteilen. Zum einen kann anhand der Sensordaten versucht werden, die unbekannten Kovarianzmatrizen zu sch¨ atzen – das in Abschnitt 9.5 vorgestellte Verfahren ist ein Beispiel hierf¨ ur. Dabei wurde augenutzt, dass bei dem betrachteten Problem das Systemrauschen durch das die Inertialsensordaten behaftende Rauschen gegeben ist, da diese als bekannte Eingangsgr¨ oßen modelliert wurden. Ein anderer Ansatz besteht in der Analyse der Innovation des Filters, also der Differenz zwischen pr¨ adizierten und vorliegenden Messwerten, siehe hierzu beispielsweise [91],[88]. Schließlich ist die Gruppe der Multiple-Model-Algorithmen zu nennen; hierbei wird eine Bank von Filtern betrieben, wobei jeder der elementaren Filter unter einer anderen Annahme entworfen wird – diese unterschiedlichen Annahmen k¨onnen sich auf die Kovarianzmatrizen des System- und/oder Messrauschens beziehen, es k¨onnen aber auch verschiedene Systemmodelle angenommen werden10 . Schließlich wird durch Analyse der Innovationen der elementaren Filter eine Gewichtung bestimmt, die festlegt, wie die Sch¨ atzungen der elementaren Filter zum endg¨ ultigen Sch¨atzergebnis beitragen. Der wichtigste Vertreter dieser Gruppe ist der Interacting Multiple Model (IMM) Filter, der im Folgenden beschrieben werden soll.

6.7.1

Interacting Multiple Model Filter: Problemformulierung

Ein IMM hat das Ziel, den Zustand des Systems xk = Φk (sk )xk−1 + Gk (sk )w k

(6.160)

zu sch¨ atzen. Dabei stehen Messungen zur Verf¨ ugung, deren Beziehung zum Systemzustand durch das Messmodell y˜k = Hk (sk )xk + Ck (sk )vk

(6.161)

beschrieben wird. Bei diesem System- und Messmodell sind die Transitionsmatrix, die Messmatrix sowie die Einwirkung von System- und Messrauschen in Abh¨angigkeit eines Parameters sk formuliert. Bei sk handelt es sich um eine Markov-Kette, die Werte ¨ zwischen 1 und m annehmen kann. Die Markov-Kette wird durch eine Ubergangswahrscheinlichkeitsmatrix M beschrieben, wobei das Element Mij die Wahrscheinlichkeit ¨ angibt, dass sich beim Ubergang von Zeitschritt k − 1 zum Zeitschritt k der Zustand der Markov-Kette von sk−1 = i zu sk = j ¨andert. Diese Markov-Kette wirkt wie ein Schalter, der zwischen verschiedenen Transitionsmatrizen, Messmatrizen usw. hin und herschaltet und so das Systemverhalten beeinflusst. Es handelt sich folglich um ein nichtlineares System, da der xk nicht linear von xk−1 , sk abh¨angt11 . 10 Vor allem im Bereich Target Tracking kommen h¨ aufig Multiple-Model-Ans¨ atze mit verschiedenen Systemmodellen zum Einsatz, z.B. f¨ ur Geradeausflug und Kurvenflug des Ziels. 11 Ist s bekannt, liegt ein lineares System vor. k

6.7 Adaptive Filterung

169

Ein optimaler Sch¨ atzalgorithmus f¨ ur ein solches System w¨ urde den bedingten Erwartungswert k

E [xk |Yk ] =

m 

E [xk |Sq , Yk ] P (Sq |Yk )

(6.162)

q=1

berechnen, wobei Yk f¨ ur alle bis zum Zeitschritt k aufgetretenen Messwerte Yk = y˜k , y˜k−1 , ..., y˜0 steht. Sq bezeichnet die q-te Sequenz von mk m¨oglichen Sequenzen der ur, dass die q-te Zust¨ ande der Markov-Kette, P (Sq |Yk ) ist die Wahrscheinlichkeit daf¨ Sequenz der tats¨ achlich vorliegenden Sequenz von Zust¨anden der Markov-Kette entspricht. Offensichtlich w¨ achst die Anzahl der m¨oglichen Sequenzen exponentiell mit der Zeit, so dass ein praktisch implementierbarer Filter nicht in der Lage ist, alle m¨oglichen Sequenzen f¨ ur alle Zeiten zu betrachten. Die Grundidee des IMM besteht nun darin, Gl. (6.162) anhand von E [xk |Yk ] =

m 

E [xk |sk = j, Yk ] P (sk = j|Yk )

(6.163)

j=1

zu approximieren12. Dazu laufen m elementare Filter parallel, wobei jeder dieser Filter f¨ ur einen bestimmten Zustand sk der Markov-Kette optimal ist, d.h. sein System- und Messmodell f¨ ur ein bestimmtes sk gerade mit Gl. (6.160) und (6.161) u ¨ bereinstimmt. Diese elementaren Filter werden auch als Modelle bezeichnet. Gleichzeitig wird die Wahrscheinlichkeit der einzelnen Modelle gesch¨atzt, was einer Sch¨atzung des Zustandes der Markov-Kette entspricht.

6.7.2

Herleitung der IMM-Filtergleichungen

Zur Herleitung des IMM werden folgende Schreibweisen eingef¨ uhrt, um eine kompakte Darstellung zu erm¨ oglichen: k = P (sk = j|Yk−1 ) µjk−1

j |i

k k−1 µk−1 = P (sk = j|sk−1 = i, Yk−1 )

12 Damit

Bedingte Wahrscheinlichkeit unter der Voraussetzung aller bis zum Zeitpunkt k − 1 eingetroffenen Messungen, dass das Modell j im Zeitschritt k korrekt ist. Bedingte Wahrscheinlichkeit unter der Voraussetzung aller bis zum Zeitpunkt k − 1 eingetroffenen Messungen, dass das Modell j im Zeitschritt k korrekt ist, wenn das Modell i im Zeitschritt k − 1 korrekt war.

ist sofort klar, dass es sich beim IMM um einen suboptimalen Filteralgorithmus handelt.

170

6 Das Kalman-Filter

j |i

k k−1 µk−1 = Mij

¨ Element der Ubergangswahrscheinlichkeitsmatrix

p(xk−1 |sk = i, Yk−1 )

Wahrscheinlichkeitsdichte des Systemzustands, basierend auf Modell i und allen bis zum Zeitpunkt k − 1 vorliegenden Messwerten. Hierbei handelt es sich um die WDF des elementaren Filters i.

Die a posteriori Zustandssch¨ atzung des elementaren Filters i wird mit x+ i,k bezeichnet, + die zugeh¨ orige Kovarianzmatrix mit Pi,k . Die Zustandssch¨atzung der Filterbank baˆ+ orige Kovarianz mit P sierend auf allen elementaren Filtern wird mit xˆ+ k , die zugeh¨ k bezeichnet. Alle anderen im Folgenden verwendeten Bezeichnungen k¨onnen anhand dieser Definitionen erschlossen werden. Der Ablauf des IMM-Algorithmus l¨ asst sich in drei Schritte unterteilen, das Mischen, den Propagationsschritt und den Messschritt. Die nachfolgende Herleitung dieser Schritte orientiert sich an [14]. Mischen Der erste Schritt des IMM-Algorithmus ist das Mischen. Die Markov-Kette propagiert in der Zeit, daher m¨ ussen auch die Sch¨ atzungen des Zustands der Markov-Kette und des ¨ Systemzustandes angepasst werden. Dazu m¨ ussen folgende Uberg¨ ange ermittelt werden: j

p(xk−1 |sk−1

k−1 k → µik−1 , ∀i, j µk−1 = j, Yk−1 ) → p(xk−1 |sk = i, Yk−1 ) ,

(6.164) (6.165)

∀i, j

Die Chapman-Kolmogorov-Gl. (7.7) angewandt auf eine Markov-Kette liefert k µik−1 =

m 

i |j

j

k k−1 k−1 µk−1 µk−1 =

j=1

m 

j

k−1 Mij µk−1 ,

(6.166)

j=1

der gesuchte Zusammenhang Gl. (6.164) ist damit gefunden. Mit der Bayes’schen Regel folgt außerdem j

k−1 µk−1

|ik

i |j

j

j

k k−1 k−1 k−1 k µik−1 = µk−1 µk−1 = Mij µk−1

(6.167)

und damit jk−1 |ik µk−1

j

=

k−1 Mij µk−1 k µik−1

j

k−1 Mij µk−1 = $ . m jk−1 Mij µk−1

(6.168)

j=1

Die gesuchte Dichte Gl. (6.165) l¨ asst sich darstellen als p(xk−1 |sk = i, Yk−1 ) =

m  j=1

j

k−1 p(xk−1 |sk = i, sk−1 = j, Yk−1 )µk−1

|ik

. (6.169)

6.7 Adaptive Filterung

171

Mit Hilfe der Bayes’sche Regel kann man schreiben p(xk−1 |sk = i, sk−1 = j, Yk−1 )P (sk = i|sk−1 = j, Yk−1 ) = P (sk = i|xk−1 , sk−1 = j, Yk−1 )p(xk−1 |sk−1 = j, Yk−1 ) (6.170) und schließlich p(xk−1 |sk = i, sk−1 = j, Yk−1 ) P (sk = i|xk−1 , sk−1 = j, Yk−1 ) p(xk−1 |sk−1 = j, Yk−1 ) .(6.171) = P (sk = i|sk−1 = j, Yk−1 ) ur sk = i. In Setzt man sk−1 = j voraus, so beeinflusst das die Wahrscheinlichkeit f¨ diesem Fall hat jedoch die zus¨ atzliche Voraussetzung xk−1 keinen Einfluss auf die Wahr¨ scheinlichkeit f¨ ur sk = i. Aufgrund dieser Uberlegungen folgt P (sk = i|xk−1 , sk−1 = j, Yk−1 ) = P (sk = i|sk−1 = j, Yk−1 ) ,

(6.172)

Einsetzen in Gl.(6.171) liefert p(xk−1 |sk = i, sk−1 = j, Yk−1 ) P (sk = i|sk−1 = j, Yk−1 ) p(xk−1 |sk−1 = j, Yk−1 ) = P (sk = i|sk−1 = j, Yk−1 ) = p(xk−1 |sk−1 = j, Yk−1 ) .

(6.173)

Damit kann Gl. (6.169) vereinfacht werden: p(xk−1 |sk = i, Yk−1 ) =

m 

j

k−1 p(xk−1 |sk−1 = j, Yk−1 )µk−1

|ik

(6.174)

j=1

Die Wahrscheinlichkeitsdichte des Systemzustandes xk−1 unter Voraussetzung des Modells i ist also durch eine gewichtete Summe von m Gaußverteilungen gegeben. Diese Dichte muss nun durch eine einzelne Gaußverteilung approximiert werden, um mit dieser f¨ ur den auf das Mischen folgenden Propagationsschritt den elementaren Filter i initialisieren zu k¨ onnen. Gesucht sind also Erwartungswert und Kovarianzmatrix einer Summe von Gaußverteilungen. Zur Herleitung wird allgemein ein Zufallsvektor z betrachtet, dessen WDF p(z ) durch eine gewichtete Summe von Gaußverteilungen pi (z ) p(z ) =

m 

αi pi (z )

(6.175)

i=1

gegeben ist, wobei m  i=1

αi = 1

(6.176)

172

6 Das Kalman-Filter

und 1 1 ˆ T −1 ˆ e− 2 (z −zi ) Pi (z −zi ) pi (z ) =  (2π)n |Pi |

(6.177)

gilt, n ist die Anzahl der Komponenten des Zufallsvektors z . Der Erwartungswert von z ergibt sich zu
∞ E[z ] =

z −∞

m 

αi pi (z )dz =

i=1

m 


∞ αi

i=1

z pi (z )dz =

m 

αi zˆi .

(6.178)

i=1

−∞

Die Kovarianzmatrix des Zufallsvektors z l¨asst sich darstellen als   

E (z − E[z ])(z − E[z ])T = E z z T − z zˆ T − zˆ z T + zˆ zˆ T = E[z z T ] − E[z ]zˆ T − zˆ E[z T ] + ˆzzˆ T = E[z z T ] − zˆ zˆ T
∞

z z T

= m  i=1

αi pi (z )dz − zˆ zˆ T

i=1

−∞

=

m 


∞ αi

z z T pi (z )dz − zˆ zˆ T ,

(6.179)

−∞

mit   Pi = E (z − zˆi )(z − zˆi ) T = E[z z T ] − zˆi zˆiT

(6.180)

E[z z T ] = Pi + zˆi zˆiT

(6.181)

folgt

und damit m   

E (z − E[z ])(z − E[z ]) T = αi Pi + zˆi zˆiT − zˆ zˆ T i=1

=

m  i=1

 αi Pi +

m  i=1

 αi zˆi zˆiT

− zˆ zˆ T . (6.182)

6.7 Adaptive Filterung

173

Gl.(6.182) kann weiter vereinfacht werden. Mit m m     T T T αi zˆi zˆi αi zˆi zˆi − zˆ zˆ = − zˆ zˆ T − zˆ zˆ T + zˆ zˆ T i=1

 =

i=1 m 

 αi zˆi zˆiT

i=1

− zˆ



−

m 

 αi zˆi

i=1

m 

T αi zˆi

+ zˆ zˆ T

i=1

=

m  i=1

=

m 

zˆ T m 

αi

i=1

 αi zˆi zˆiT − zˆi zˆ T − zˆ zˆiT zˆ zˆ T   T αi zˆi − zˆ zˆi − zˆ

(6.183)

i=1

erh¨ alt man schließlich m m     T   T αi Pi + αi zˆi − zˆ zˆi − zˆ E (z − E[z ]) (z − E[z ]) = i=1

=

m 

i=1

  T

ˆ ˆ ˆ ˆ αi Pi + zi − z zi − z .

i=1

(6.184) Anhand dieser allgemeing¨ ultigen Formeln f¨ ur Erwartungswert und Kovarianz einer Summe von Gaußverteilungen k¨ onnen die elementaren Filter wie folgt initialisiert werden: 0,+ = xi,k−1

0,+ = Pi,k−1

m  j=1 m 

j

|ik

j

|ik

k−1 µk−1

k−1 µk−1

+ xj,k−1

(6.185)

 T

 0,+ 0,+ ˆ+ ˆ+ + −  x −  x P+  x  x j,k−1 j,k−1 j,k−1 i,k−1 i,k−1

j=1

(6.186) Propagationsschritt Im Propagationsschritt werden die Wahrscheinlichkeitsdichten der einzelnen Modelle in ¨ der Zeit propagiert, es wird also der Ubergang p(xk−1 |sk = i, Yk−1 ) → p(xk |sk = i, Yk−1 ) ,

∀i

(6.187)

¨ durchgef¨ uhrt. Realisiert wird dieser Ubergang, indem jeder elementare Filter einen gew¨ ohnlichen Propagationsschritt durchf¨ uhrt, dies stellt sich wie folgt dar: 0,+ − → xi,k xi,k−1

(6.188)

174

6 Das Kalman-Filter − P0,+ i,k−1 → Pi,k

(6.189)

Die Modellwahrscheinlichkeiten sind davon nicht betroffen. Messschritt Im Messschritt werden die vorhandenen Messwerte von jedem elementaren Filter ver¨ arbeitet. Das Ergebnis davon ist der Ubergang p(xk |sk = i, Yk−1 ) → p(xk |sk = i, Yk ) ,

∀i

(6.190)

bzw. − + → xi,k xi,k

P− i,k

→

P+ i,k

(6.191) .

(6.192)

Zus¨ atzlich m¨ ussen die Modellwahrscheinlichkeiten angepasst werden: k µik−1 → µikk ,

∀i

(6.193)

Mit Hilfe der Bayes’schen Regel erh¨ alt man 

− P sk = i|y˜k , Yk−1 , xi,k



  − − p y˜k |sk = i, Yk−1 , xi,k P sk = i|Yk−1 , xi,k  = . − p y˜k |Yk−1 , xi,k (6.194)

Da alle Messwerte bis zum Zeitpunkt k − 1 bereits in x− ucksichtigt sind, kann man i,k ber¨ schreiben:   − − = p y˜k |xi,k (6.195) p  y˜k |sk = i, Yk−1 , xi,k Damit ergibt sich aus Gl. (6.194)  − k µik−1 p y˜k |xi,k ik µk = $  . − k µik−1 p y˜k |xi,k

(6.196)

i

Der Nenner in Gl. (6.196) dient dabei zur Normierung der Modellwahrscheinlichkeiten, − so dass deren Summe eins ergibt. Um Gl.(6.196) auswerten zu k¨onnen, muss p(y˜k |xi,k ) ermittelt werden, die Wahrscheinlichkeitsdichte von y˜k unter Voraussetzung des Zu− − stands xi,k des elementaren Filters i. Normalerweise spricht man bei p(y˜k |xi,k ) nicht von einer Wahrscheinlichkeitsdichte, sondern von einer Likelihood-Funktion. Diese hat die gleiche Form wie eine Wahrscheinlichkeitsdichte und beschreibt, wie gut“ eine tats¨ach” lich vorliegende Realisation, hier  y˜k , zu einer Wahrscheinlichkeitsdichte, hier die von

6.7 Adaptive Filterung

175

− xi,k , passt“. Damit erschließt sich eine intuitive Interpretation von Gl.(6.196): Passt ” der vorliegende Messwert gut zur WDF von Filter i, so wird die zugeh¨orige Modellwahrscheinlichkeit vergr¨ oßert. War das Auftreten des Messwertes eher unwahrscheinlich, wenn man die WDF von Filter i zugrunde legt, wird die zugeh¨orige Modellwahrscheinlichkeit entsprechend verringert. Der Filter, der auf dem zum tats¨achlichen Zustand der Markov-Kette geh¨ orenden Modell basiert, wird besser in der Lage sein, Messwerte vorherzusagen als ein Filter, dessen System- und/oder Messmodell sich vom tats¨achlichen System-/Messmodell unterscheidet. − Die Likelihood-Funktion p(y˜k |xi,k ) ist gegeben durch

 −1 1 T 1 − =  e− 2 ri,k Si,k ri,k p y˜k |xi,k l (2π) |Si,k |

(6.197)

mit  − T Si,k = cov y˜k |xi,k = Hi,k P− i,k Hi,k + Ri,k Ri,k = cov (Ck (sk = i)vk )   − − ri,k = y˜k − E y˜k |xi,k = y˜k − Hi,k xi,k ,

(6.198) (6.199) (6.200)

die Anzahl der Komponenten von y˜k ist mit l bezeichnet. Die Zustandssch¨ atzung der Filterbank ist durch Erwartungswert und Kovarianz der mit den Modellwahrscheinlichkeiten gewichteten Summe der Wahrscheinlichkeitsdichten der elementaren Filter gegeben: ˆ+ = x k ˆ+ = P k

m  i=1 m 

+ µikk xi,k

µikk

 T

 + + + + + ˆ ˆ xi,k − xk Pi,k + xi,k − xk

(6.201) (6.202)

i=1

Damit ist ein Zyklus des IMM abgeschlossen, die nach diesem Messschritt zur Verf¨ ugung stehenden Gr¨ oßen dienen als Eingangsgr¨ oßen f¨ ur das n¨achste Mischen. Beim IMM wird davon ausgegangen, dass sich die Zust¨ande der Markov-Kette mit der Zeit ¨ andern k¨ onnen. Geht man von einem unbekannten, aber konstanten Zustand aus, so reduziert sich der IMM zum Multiple Model Adaptive Estimator (MMAE). Der MMAE identifiziert das korrekte Modell, dessen Wahrscheinlichkeit – ein entsprechendes Szenario mit aussagekr¨ aftigen Messwerte vorausgesetzt – sich dann ziemlich schnell gegen eins bewegt. Dieses Verhalten kann bei dem hier vorgestellten IMM ¨ durch eine entsprechende Wahl der Ubergangswahrscheinlichkeitsmatrix M erzwungen werden.

176

6 Das Kalman-Filter

Mit wachsender Anzahl von Modellen sinkt im Allgemeinen die Effektivit¨at eines IMM: Zum einen wird Rechenleistung f¨ ur Modelle mit geringer Modellwahrscheinlichkeit verschwendet, die wenig zum Gesamtergebnis beitragen – der dennoch vorhandene Beitrag dieser Modelle bedeutet zudem meist eine Verschlechterung. Dem kann begegnet werden, indem online entschieden wird, welche und wie viele Modelle verwendet werden. Diese als Variable-Structure IMMs (VSIMM) bekannten Algorithmen sollen hier jedoch nicht weiter betrachtet werden.

IMM-Algorithmus ¨ Zur besseren Ubersichtlichkeit werden hier die Gleichungen des IMM nochmals zusammengestellt: Mischen

j

k−1 µk−1

|ik

j

k−1 Mij µk−1 = $ m jk−1 Mij µk−1

j=1

0,+ xi,k−1 =

P0,+ i,k−1 =

m  j=1 m 

j

|ik

j

|ik

k−1 µk−1

k−1 µk−1

+ xj,k−1

 T

 0,+ 0,+ ˆ+ ˆ+ + −  x −  x P+  x  x j,k−1 j,k−1 j,k−1 i,k−1 i,k−1

j=1

Propagationsschritt Durchf¨ uhren gew¨ ohnlicher Propagationsschritte f¨ ur jeden elementaren Filter i, nachdem 0,+ 0,+ diese mit xi,k−1 , Pi,k−1 initialisiert wurden: 0,+ − → xi,k xi,k−1 − P0,+ i,k−1 → Pi,k

Messschritt Durchf¨ uhren gew¨ ohnlicher Messschritte f¨ ur jeden elementaren Filter i: − + → xi,k xi,k + P− i,k → Pi,k .

6.7 Adaptive Filterung

177

Anpassung der Modellwahrscheinlichkeiten:

µikk

 − k µik−1 p y˜k |xi,k = $  − k µik−1 p y˜k |xi,k i

 −1 1 T 1 − e− 2 ri,k Si,k ri,k =  p y˜k |xi,k l (2π) |Si,k | T Si,k = Hi,k P− i,k Hi,k + Ri,k − ri,k =  y˜k − Hi,k xi,k

Berechnung der Ausgangsgr¨ oßen: ˆ+ = x k

m 

+ µikk xi,k

i=1

ˆ+ = P k

m  i=1

µikk

 T

 + + + + + ˆ ˆ xi,k − xk Pi,k + xi,k − xk

7

Monte-Carlo-Methoden

Im vorangegangenen Kapitel wurden mit dem EKF, dem Kalman-Filter zweiter Ordnung und dem Sigma-Point-Kalman-Filter bereits mehrere nichtlineare Filter vorgestellt. Diese Filter beschreiben den Systemzustand als gaußverteilten Zufallsvektor. Dessen Dichtefunktion ist u ¨ ber Mittelwert und Kovarianzmatrix vollst¨andig beschrieben und damit mathematisch gut handhabbar. Eine Gaußverteilung bleibt jedoch nur bei linearen Transformationen erhalten, wird ein gaußverteilter Zufallsvektor durch eine nichtlineare Funktion – sei es ein System- oder ein Messmodell – propagiert, ist das Ergebnis kein gaußverteilter Zufallsvektor mehr. Bei nicht allzu nichtlinearen Sch¨atzproblemen kann die resultierende Dichtefunktion aber durch eine Gaußverteilung gut approximiert werden, wie das eben bei EKF und Sigma-Point-Kalman-Filter geschieht. Liegen massive Nichtlinearit¨ aten vor, ist das nicht mehr m¨oglich. In diesen F¨allen kann ¨ versucht werden, die Dichtefunktion durch die Uberlagerung mehrere Gaußverteilungen zu beschreiben, was auf die Gaussian Sum Filter f¨ uhrt. Einen anderen Ansatz stellen Partikelfilter dar, die die Dichtefunktion durch eine Summe von Partikeln approximieren, wobei jeder Partikel als eine Realisation des Zustandsvektors verstanden werden ¨ kann. Dies hat auf den ersten Blick Ahnlichkeit mit den Sigma-Point-Kalman-Filtern – bei einem Partikelfilter werden die Partikel jedoch zuf¨allig gew¨ahlt, die Sigma-Punkte werden deterministisch vorgegeben. Auch ist bei einem Partikelfilter die Anzahl der Partikel nicht durch die Dimension des Zustandsvektors vorgegeben, sondern eher ein Tuning Parameter“ – viele Partikel erlauben eine genauere Approximation der Dichte” funktion, zu wenige Partikel k¨ onnen zum Scheitern des Filters f¨ uhren. In diesem Kapitel soll nur eine kurze Einf¨ uhrung zum Partikelfilter als Beispiel f¨ ur einen Monte-Carlo Ansatz gegeben werden; eine umfassende Beschreibung der fast u ¨ berabz¨ ahlbar vielen Varianten nichtlinearer Filter, die auf einer numerischen Approximation der Dichtefunktion basieren, wird nicht versucht - f¨ ur weiterf¨ uhrende Betrachtungen sei auf [104][11][47][21] verwiesen.

7.1

Chapman-Kolmogorov-Gleichung

Ein Partikelfilter modelliert den zu sch¨ atzenden Systemzustand als einen Zufallsvektor, dessen Dichtefunktion numerisch approximiert wird. Um mit einem solchen Ansatz einen Filteralgorithmus realisieren zu k¨ onnen, muss zun¨achst einmal klar sein, wie sich die zeitliche Evolution dieser Dichtefunktion beschreiben l¨asst und wie Beobachtungen diese Dichtefunktion ver¨ andern. Ersteres ist durch die Chapman-Kolmogorov-Gleichung gegeben, die im Folgenden hergeleitet werden soll. Bei dem zu sch¨ atzende Systemzustand soll es sich um einen Markov-Prozess handeln. Das bedeutet, dass die Wahrscheinlichkeitsdichtefunktion des aktuellen Systemzustan-

180

7 Monte-Carlo-Methoden

des xk vom Systemzustand einen Zeitschritt zuvor, xk−1 , abh¨angt – eine Abh¨angigkeit von Systemzust¨ anden mehrere Zeitschritte zuvor besteht aber nicht. Damit kann man schreiben p (xk |xk−1 , xk−2 , ..., x0 ) = p (xk |xk−1 )

(7.1)

p (xk |xk−1 , Yk−1 ) = p (xk |xk−1 ) .

(7.2)

sowie

Hierbei bezeichnet Yk−1 alle bis zum Zeitpunkt k − 1 angefallenen Beobachtungen1, yk−1 , yk−2 , ..., y0 . Anschaulich l¨ asst sich Gl. (7.2) so verstehen, dass alle bis Yk−1 =  zum Zeitpunkt k − 1 vorliegenden Beobachtungen Yk−1 bereits in xk−1 eingeflossen atzliche Bedingung darstellt. sind und Yk−1 daher keine zus¨ Ziel ist es nun, die Wahrscheinlichkeitsdichte p (xk |Yk−1 ) als Funktion von p (xk−1 |Yk−1 ) zu formulieren. Durch Integration der Verbunddichte erh¨alt man die marginale Dichte
p (xk , xk−1 , Yk−1 ) dxk−1 . (7.3) p (xk , Yk−1 ) = V xk−1

Mit der Bayes’schen Regel p(A, B) = p(A)p(B|A) = p(B)p(A|B) kann der Integrand umgeschrieben werden:
p (xk |xk−1 , Yk−1 ) p (xk−1 , Yk−1 ) dxk−1 (7.4) p (xk , Yk−1 ) = V xk−1

Erneutes Anwenden der Bayes’schen Regel auf den zweiten Term ergibt
p (xk |xk−1 , Yk−1 ) p (xk−1 |Yk−1 ) p (Yk−1 ) dxk−1 . p (xk , Yk−1 ) = V xk−1

(7.5) angig von xk−1 ist, kann diese Dichte vor das Integral gezogen und Da p (Yk−1 ) unabh¨ durch sie dividiert werden:
p (xk , Yk−1 ) = p (xk |xk−1 , Yk−1 ) p (xk−1 |Yk−1 ) dxk−1 (7.6) p (Yk−1 ) V xk−1

Anwenden der Bayes’schen Regel auf die linke Seite von Gl. (7.6) und Ausn¨ utzen der Markov-Eigenschaft auf der rechten Seite f¨ uhrt auf
p (xk |xk−1 ) p (xk−1 |Yk−1 ) dxk−1 . (7.7) p (xk |Yk−1 ) = V xk−1 1 Auf die explizite Kennzeichung von Messwerten durch eine Tilde (˜ ) soll im Folgenden verzichtet werden.

7.2 Ber¨ ucksichtigung von Beobachtungen

181

Gl. (7.7) is als Chapman-Kolmogorov-Gleichung bekannt und beschreibt die zeitliche Propagation der Wahrscheinlichkeitsdichte des als Zufallsvektor xk modellierten Systemzustandes. Die Auswirkung von Beobachtungen auf diese Dichte wird im n¨achsten Abschnitt behandelt.

7.2

Beru ¨cksichtigung von Beobachtungen

Liegt eine Beobachtung yk vor, kann ausgehend von der Dichtefunktion p (xk |Yk−1 ) die Dichte p (xk |Yk ) berechnet werden. Mit dem Zusammenhang Yk = {yk , Yk−1 } kann diese Dichte unter Verwendung der Bayes’schen Regel wie folgt dargestellt werden: p (xk |yk , Yk−1 ) =

p (xk , yk , Yk−1 ) p (yk , Yk−1 )

(7.8)

Einsetzen der ebenfalls mittels der Bayes’schen Regel gewonnenen Zusammenh¨ange p (xk , yk , Yk−1 ) = p (yk |xk , Yk−1 ) p (xk , Yk−1 )

(7.9)

und p ( yk , Yk−1 ) = p (yk |Yk−1 ) p (Yk−1 )

(7.10)

f¨ uhrt auf p (yk |xk , Yk−1 ) p (xk , Yk−1 ) p (yk |Yk−1 ) p (Yk−1 ) p (yk |xk , Yk−1 ) p (xk |Yk−1 ) = . p (yk |Yk−1 )

p (xk |Yk ) =

(7.11)

Mit Gl. (7.2) folgt daraus p (xk |Yk ) =

p (yk |xk ) p (xk |Yk−1 ) . p ( yk |Yk−1 )

(7.12)

Gl. (7.12) beschreibt, wie Beobachtungen die Dichtefunktion des Systemzustandes beeinflussen; zusammen mit Gl. (7.7) zur zeitlichen Propagation ist damit das nichtlineare Filterproblem in diskreter Zeit formal gel¨ ost. Eine analytische L¨osung dieser Gleichungen ist im Allgemeinen nicht m¨ oglich; unter den einschr¨ankenden Annahmen linearer System- und Messmodelle und gaußverteilten Zufallsvariablen gelingt eine analytische L¨ osung jedoch: das Kalman-Filter2. Eine allgemeine M¨oglichkeit zur numerischen L¨osung dieser Gleichungen stellen die Partikelfilter dar. 2 Eine

Herleitung der Kalman-Filter-Gleichungen aus Gl. (7.7) und (7.12) ist in [89] zu finden.

182

7.3

7 Monte-Carlo-Methoden

Partikelfilter

Im Folgenden soll der einfachste Partikelfilter, der Bootstrap-Filter, beschrieben werden. Hierzu wird von den nichtlinearen System- und Messmodellen k xk = f (xk−1 ) + w  yk = h (xk ) + vk

(7.13) (7.14)

ausgegangen. Bei dem Systemrauschen w  k und dem Messrauschen vk handelt es sich um weißes Rauschen, die zugeh¨ origen Wahrscheinlichkeitsdichtefunktionen werden im Folgenden mit pw k und pvk bezeichnet, hierbei muss es sich nicht um Gaußverteilungen handeln. Es wird vorausgesetzt, dass w  k und vk nicht miteinander korreliert sind.

7.3.1

Repr¨asentation der WDF

Grundlage des Partikelfilters ist die Darstellung einer Wahrscheinlichkeitsdichte mit Hilfe von Partikeln. Eine Wahrscheinlichkeitsdichte p (xk−1 |Yk−1 ) wird approximiert, i mit einem Zufallszahlengenerator gezogen werden, der Zufallsindem N Partikel xk−1 vektoren mit eben dieser Wahrscheinlichkeitsdichte produziert: i xk−1 ∝ p (xk−1 |Yk−1 )

(7.15)

Hierbei beschreibt das Symbol ∝ das Ziehen von Zufallsvektoren entsprechend einer Dichtefunktion. Die Wahrscheinlichkeitsdichte p (xk−1 |Yk−1 ) wird nun dargestellt durch p (xk−1 |Yk−1 ) =

N 

  i , wi · δ xk−1 − xk−1

(7.16)

i=1

wobei wi Gewichte sind, f¨ ur die

N $

wi = 1 gilt. Bei δ handelt es sich um die Dirac’sche

i=1

Delta-Funktion, die durch


∞


∞

δ (x − x0 ) = 0

f¨ ur x = x0

(7.17)

δ (x − x0 ) dx = 1

(7.18)

f (x) δ (x − x0 ) dx = f (x0 )

(7.19)

−∞

−∞

gekennzeichnet ist. Die Gleichungen (7.15)–(7.16) stellen zugleich den Initialisierungsschritt des Filteralgorithmus dar, hierbei wird wi = 1/N gew¨ahlt. Es l¨asst sich zeigen, dass der Approximationsfehler bei dieser Darstellung der Wahrscheinlichkeitsdichte f¨ ur N → ∞ verschwindet. Auf der anderen Seite ist klar, dass bei einer geringen Anzahl von Partikeln die Wahrscheinlichkeitsdichtefunktion nur ungen¨ ugend approximiert wird.

7.3 Partikelfilter

7.3.2

183

Propagationsschritt

Die propagierte Wahrscheinlichkeitsdichte erh¨alt man durch Auswertung der ChapmanKolmogorov-Gl. (7.7). Dazu muss die Dichte p (xk |xk−1 ) bestimmt werden: Setzt man in das Systemmodell Gl. (7.13) einen spezifischen Zustandsvektor xk−1 ein, so erkennt man, dass xk die gleiche Wahrscheinlichkeitsdichte besitzen muss wie das Systemrauschen w k, der Mittelwert ist jedoch um f (xk−1 ) verschoben. Damit ist die gesuchte Dichte gegeben durch  p (xk |xk−1 ) = pw k xk − f (xk−1 ) . (7.20) Einsetzen in Gl. (7.7) liefert
p (xk |Yk−1 ) =

p (xk |xk−1 ) p (xk−1 |Yk−1 ) dxk−1 V xk−1




N     i dxk−1 pw k xk − f (xk−1 ) wi · δ xk−1 − xk−1

= V xk−1

=

N 

i=1

  i  . wi · pw k xk − f xk−1

(7.21)

i=1

Die propagierte Wahrscheinlichkeitsdichtefunktion kann also approximiert werden, ini dem neue Partikel xki entsprechend der Wahrscheinlichkeitsdichte pw k (xk − f(xk−1 )) gezogen werden:   i  xki ∝ pw k xk − f xk−1 (7.22) andert. Die Gewichte wi bleiben hierbei unver¨ i i )) k¨onnen auch Hilfspartikel ωk−1 Alternativ zum Ziehen entsprechend pw k (xk − f(xk−1 entsprechend pw k (xk ) gezogen werden, i ω  k−1 ∝ pw k (xk ) ,

die Partikel xki ergeben sich dann durch  i  i xki = ω .  k−1 + f xk−1

7.3.3

(7.23)

(7.24)

Estimationsschritt

Die Ber¨ ucksichtigung von Messwerten erfolgt durch Auswertung von Gl. (7.12). Auf¨ grund analoger Uberlegungen wie schon beim Propagationsschritt erh¨alt man mit dem Messmodell Gl. (7.14) f¨ ur die bedingte Dichte der Messung  p ( yk |xk ) = pvk yk − h(xk ) , (7.25)

184

7 Monte-Carlo-Methoden

einsetzen in Gl. (7.12) f¨ uhrt auf p (yk |xk ) p (xk |Yk−1 ) p (yk |Yk−1 ) = c · p (yk |xk ) p (xk |Yk−1 ) N      wi · δ xk − xki = c · pvk yk − h(xk )

p (xk |Yk ) =

i=1

=

N 

   yk − h(xki ) · wi · δ xk − xki . c · pvk 

(7.26)

i=1

Im Estimationsschritt werden folglich neue Gewichte wi,+ gem¨aß    wi,+ = c · pvk yk − h xki · wi

(7.27)

berechnet, die Normierungskonstante c wird dabei so gew¨ahlt, dass die Summe der Gewichte eins ergibt: c=

N $ i=1

1    pvk yk − h xki · wi

(7.28)

Die Partikel bleiben im Estimationsschritt unver¨andert.

7.3.4

Resampling

Im Propagationsschritt des Filters, Gl. (7.23) und Gl. (7.24), werden die Partikel anhand des Systemmodells propagiert und entsprechend dem Systemrauschen immer weiter verteilt. Im Messschritt, Gl. (7.27) und (7.28), erhalten nur diejenigen Partikel ein großes Gewicht, die gut zu dem vorliegenden Messwert passen. Ohne weitere Maßnahmen w¨ urden dadurch nach wenigen Iterationsschritten einige wenige Partikel einen Großteil der Gewichtung auf sich vereinen, w¨ ahrend die Gewichte der restlichen Partikel fast verschwinden. Dieser Mechanismus ist als Degeneration der Partikelwolke bekannt und muss verhindert werden, da die Partikel in diesem Fall keine gute Approximation der Wahrscheinlichkeitsdichte mehr darstellen, der Filter divergiert. Da es nicht m¨oglich ist, mit unendlich vielen Partikeln zu arbeiten, m¨ ussen andere Wege gefunden werden, um diese Degeneration zu verhindern. Eine M¨oglichkeit hierzu ist das Resampling. Zur Beschreibung des Resamplings soll von der in Abb. 7.1 oben dargestellten Situation ausgegangen werden. Gezeigt ist dort die tats¨achliche Wahrscheinlichkeitsdichte sowie deren Approximation mit Hilfe von Partikeln, wobei der Durchmesser der Partikel deren Gewicht symbolisieren soll. Hierbei handelt es sich nat¨ urlich um kein realistisches Szenario, in der Regel werden einige hundert bis mehrere tausend Partikel ben¨otigt, um einen funktionierenden Filter zu erhalten. Aufgrund der Verarbeitung von Beobachtungen haben einige wenige Partikel ein großes Gewicht, die Gewichte der restlichen Partikel verschwinden nahezu. Ziel des Resamplings ist es, neue Partikel zu w¨ahlen, die alle das

7.3 Partikelfilter

185

WDF

0

1 1

w w

2

3

w w

4

w

5

w

6

w

7

gezogene Zufallszahlen WDF

Abbildung 7.1: Schematische Darstellung des Resamplings. Oben Partikel und deren Gewichte vor dem Resampling, untern nach dem Resampling.

gleiche Gewicht, 1/N , besitzen. Neben dem Verhindern der Degeneration wird dadurch auch sichergestellt, dass nicht ein Großteil der Rechenzeit auf Partikel verwendet wird, die nichts zur Approximation der Wahrscheinlichkeitsdichte beitragen. Zun¨achst wird nun das Intervall [0,1] in N Intervalle entsprechend der Gewichte der Partikel eingeteilt, siehe Abb. 7.1 Mitte. Anschließend werden N Zufallszahlen entsprechend einer Gleichverteilung aus dem Intervall [0,1] gezogen. Der Partikel, in dessen Intervall eine gezogene Zufallszahl f¨ allt, wird reproduziert. Die Partikel mit großem Gewicht werden h¨ aufiger reproduziert, Partikel mit geringem Gewicht unter Umst¨anden gar nicht. Das Ergebnis des Resamplings f¨ ur den in Abb. 7.1 Mitte gezeigten, hypothetischen Satz Zufallszahlen ist in Abb. 7.1 unten dargestellt. Alle Partikel besitzen nun dasselbe Gewicht und approximieren nach wie vor die tats¨achliche Wahrscheinlichkeitsdichte. Man erkennt aber auch eine potentielle Schwierigkeit, die Vielfalt der Partikelpositionen hat abgenommen. Liegt nur geringes Systemrauschen vor, werden im Propagationsschritt des Filters die Partikel nicht ausreichend verteilt, was dann trotz Resampling zur Divergenz des Filters f¨ uhren kann. Abhilfe schafft in diesen F¨ allen eine als Roughening bekannte Technik, die im Wesentlichen einer k¨ unstlichen Erh¨ ohung des Systemrauschens entspricht. Das beschriebene Resampling muss nicht nach jedem Messchritt durchgef¨ uhrt werden. Einen Hinweis, wann ein Resampling durchgef¨ uhrt werden muss, liefert die Anzahl der

186

7 Monte-Carlo-Methoden

effektiven Pratikel. Diese ist gegeben durch 1

Nef f =

N $

.

(7.29)

2 (wi )

i=1

Haben alle Partikel das gleiche Gewicht 1/N , so ist Nef f = N . Im Extremfall einer degenerierten Partikelwolke, ein einziger Partikel besitzt das gesamte Gewicht, ist Nef f = 1. Ein Resampling wird nur durchgef¨ uhrt, wenn Nef f unter eine bestimmte Schwelle, meist formuliert als ein bestimmter Prozentsatz von N , gesunken ist. Mit dem Resampling und dem zuvor beschriebenen Mess- und Propagationsschritt kann bereits ein einfacher Partikelfilter realisiert werden. Die M¨oglichkeit der Einf¨ uhrung ¨ einer Importance Density wurde aus Gr¨ unden der Ubersichtlichkeit ausgeblendet; deren Einf¨ uhrung kann jedoch je nach Applikation von Vorteil sein.

7.3.5

Simulationsergebnisse

Anhand von zwei Beispielen soll im Folgenden ein Eindruck von den Eigenschaften des Partikelfilters vermittelt werden. Sch¨ atzung der Auslenkung eines Pendels Als erstes Beispiel soll die Auslenkung eines Pendels anhand von Messungen des Abstandes zwischen dem Lot durch den Drehpunkt und dem Ende des Pendels betrachtet

Realer Winkel Partikelfilter Kalman Filter

Pendel-Auslenkung (deg)

100

e

50  e 0

-50 

  e

  e

e    e e e        e  e e e    e e   

e

-100 0

0.5

1

1.5

2

2.5 Zeit (s)

3

3.5

4

4.5

5

Abbildung 7.2: Sch¨ atzung der Pendelschwingung bei einer Anfangsauslenkung von –50 Grad, wobei Partikelfilter und Kalman-Filter mit jeweils +50 Grad initialisiert wurden.

7.3 Partikelfilter

187

werden. Das Systemmodell ist durch die nichtlineare Differentialgleichung des Pendels Gl. (2.109) gegeben, die Messgleichung lautet y˜ = l · sin ϕ ,

(7.30)

siehe hierzu auch Abb. 2.8. F¨ ur dieses Sch¨atzproblem wurden ein Partikelfilter und ein erweitertes Kalman-Filter entworfen. Der MATLAB-Code der beiden Filter ist in Anhang C zu finden. Die Anfangsauslenkung des Pendels wurde nun zun¨achst zu –50 Grad gew¨ahlt, wobei beide Filter mit jeweils +50 Grad initialisiert wurden; die zugeh¨orige Anfangsunsicherheit wurde entsprechend gew¨ ahlt. Abb. 7.2 zeigt die Sch¨atzungen beider Filter. Nach einer kurzen Einschwingphase, die beim Kalman-Filter etwas unruhiger und l¨anger ausf¨ allt, sind beide Filter in der Lage, die Pendelposition zu sch¨atzen. Nun wurdem die Anforderungen weiter versch¨arft, indem die Anfangsauslenkung des Pendels zu –60 Grad gew¨ ahlt wurde, beide Filter wurden mit jeweils +60 Grad initialisiert. Wie man in Abb. 7.3 erkennt, ist der Partikelfilter auch in diesem Szenario in der Lage, die Pendelposition korrekt zu sch¨ atzen – das erweiterte Kalman-Filter divergiert jedoch. Bei nicht zu großen Anfangsfehlern liefert das Kalman-Filter bei diesem Sch¨atzproblem vergleichbare Ergebnisse wie das Partikelfilter – Letzteres ist bei großen Anfangsfehlern jedoch deutlich robuster.

Pendel-Auslenkung (deg)

100 0 -100







































e e

-200

e e

-300

Realer Winkel Partikelfilter Kalman Filter

e

-400

e e

-500 0

0.5

1

1.5

2

2.5 Zeit (s)

3

3.5

4

  e 4.5

5

Abbildung 7.3: Sch¨ atzung der Pendelschwingung bei einer Anfangsauslenkung von –60 Grad, wobei Partikelfilter und Kalman-Filter mit jeweils +60 Grad initialisiert wurden.

188

7 Monte-Carlo-Methoden

Abbildung 7.4: Positionssch¨ atzung eines fahrbaren Roboters mit Hilfe von Abstandsmessungen; erster Zeitschritt obere Reihe links, letzter Zeitschritt untere Reihe rechts. Die Anfangsposition befindet sich im rechten oberen Raum. Die Partikelwolke zeigt m¨ ogliche Positionen auf, bis bei Erreichen des Ganges alle Mehrdeutigkeiten eliminiert sind und die wahre Position gefunden ist.

Positionsbestimmung mit Abstandsmessungen W¨ ahrend bei einem erweiterten Kalman-Filter und den Sigma-Point-Filtern nur unimodale Dichtefunktionen m¨ oglich sind, stellen multimodale Dichten beim Partikelfilter kein Problem dar. Dies soll anhand einer Simulation der Positionsbestimmung mit Hilfe von Abstandsmessungen verdeutlicht werden. Hierzu wurde angenommen, dass von einem Geb¨aude ein Grundrissplan vorhanden ist. Ein fahrbarer Roboter befindet sich an einem unbekannten Ort innerhalb dieses Ge-

7.3 Partikelfilter

189

b¨ audes. Der Roboter ist mit einem Odometer und einem Gyroskop ausger¨ ustet; des Weiteren ist ein Laser-Scanner vorhanden, mit dem Abstandsmessungen innerhalb eines ¨ Offnungswinkels von 180 Grad zu den umgebenden W¨anden vorgenommen werden k¨onnen. Diese Abstandsmessungen werden mit einem Partikelfilter verarbeitet, Odometrie und Gyroskop-Daten werden als bekannte Eingangsgr¨oßen behandelt. Die Partikelwolke und die Position des Roboters ist in Abb. 7.4 f¨ ur verschiedene Zeitschritte zu sehen. F¨ ur den ersten Zeitschritt (obere Reihe links) wurde f¨ ur die Partikelwolke eine Gleichverteilung angenommen, da keinerlei Informationen u ¨ ber die Roboterposition vorlagen. Nach einigen Zeitschritten f¨ uhrt die Verarbeitung der Messungen dazu, dass sich die Partikel zun¨ achst vornehmlich in den R¨ aumen auf der rechten Seite konzentrieren. Die R¨aume auf der linken Seite sind kleiner, so dass diese zun¨achst unwahrscheinlicher werden und schließlich ab dem vierten dargestellten Zeitschritt (untere Reihe links) anhand der vorliegenden Messungen bereits ganz ausgeschlossen werden k¨onnen. Als der Roboter den Gang erreicht, sind alle Mehrdeutigkeiten eliminiert und die Position des Roboters eindeutig bestimmt. Ohne die M¨ oglichkeit, mit multimodalen Dichtefunktionen zu arbeiten, ist in diesem Szenario eine initiale Positionsbestimmung unm¨oglich. Ist die Position des Roboters aber einmal gefunden, kann die Roboterposition auch mit einem Kalman-Filter propagiert und korrigiert werden, was wesentlich weniger Rechenzeit ben¨otigt.

8

Anwendungsbeispiel GPS/INS-Integration

Ein integriertes Navigationssystem ist dadurch gekennzeichnet, dass verschiedene Navigationssensoren und Navigationsverfahren miteinander kombiniert werden. Aufgrund der komplement¨ aren Charakteristiken dieser Informationsquellen k¨onnen hierbei die Nachteile des einen Verfahrens oder Sensors durch die Vorteile eines anderen kompensiert werden. Das Ergebnis ist ein Navigationssystem, dass die Leistungsf¨ahigkeit der einzelnen Subsysteme bei weitem u ¨ bersteigt. H¨aufig kann auch eine gewisse Redundanz geschaffen werden, so dass Verfahren zur Fehlererkennung und Isolation eingesetzt werden k¨ onnen. Weit verbreitet ist die Kombination von inertialer Navigation mit GPS, da sich diese Verfahren hervorragend erg¨ anzen. Das Inertialnavigationssystem kann durch externe Einfl¨ usse nicht gest¨ort werden und stellt somit die kontinuierliche Verf¨ ugbarkeit einer vollst¨andigen Navigationsl¨osung bestehend aus Position, Geschwindigkeit und Lage sicher. Diese Navigationsl¨osung liegt mit einer hohen Datenrate vor, was bei vielen Anwendungen unabdingbar ist, z.B. wenn diese Navigationsl¨ osung als Eingangsgr¨ oße eines Fluglagereglers dient. Wie in Kapitel 3 aufgezeigt handelt es sich bei inertialer Navigation jedoch nur um ein kurzzeitgenaues Navigationsverfahren, die Navigationsfehler wachsen mit der Zeit an. Dies wird durch Kombination mit einem GPS-Empf¨ anger verhindert. Ein GPS-Empf¨ anger stellt langzeitgenaue Positions- und Geschwindigkeitsinformationen bzw. Pseudoranges, Deltaranges und Tr¨agerphasenmessungen zur Verf¨ ugung, Lageinformationen k¨ onnen mit einer einzelnen GPS-Antenne jedoch nicht gewonnen werangers ist vergleichsweise gering, typischerweise den1 . Die Datenrate eines GPS-Empf¨ liegen ein bis vier Messungen pro Sekunde vor, einige hochwertige Empf¨anger liefern bis zu zwanzig Messwerte pro Sekunde oder mehr. Eine kontinuierliche Verf¨ ugbarkeit dieser Messwerte ist jedoch nicht gegeben; sinkt die Anzahl der sichtbaren Satelliten auf weniger als vier, kann ohne zus¨ atzliche Annahmen keine Position oder Geschwindigkeit mehr ermittelt werden2 . Die Ursachen hierf¨ ur k¨onnen vielf¨altig sein. Eine h¨aufige Ursache sind Abschattungen, wenn z.B. Geb¨ aude, Berge oder Teile des den Empf¨anger tragenden Fahrzeugs die Sicht auf Satelliten verdecken. Des Weiteren k¨onnen durch mutwilliges oder unbeabsichtigtes Jamming GPS-Ausf¨alle verursacht werden. W¨ahrend mutwilliges Jamming – ebenso wie Spoofing – eher im milit¨arischen Bereich anzusiedeln 1 Die Genauigkeit von Tr¨ agerphasenmessungen reicht aus, um mit mehreren GPS-Antennen Lageinformationen zu gewinnen, wobei der Abstand der Antennen die Genauigkeit der Lageinformationen bestimmt. 2 Bei bekannter H¨ ohe gen¨ ugen drei Satelliten zur Positionsbestimmung. Bei einigen Anwendungen ist nur eine Positionskoordinate unbekannt, z.B. wenn sich ein Schienenfahrzeug auf einem bekannten Gleis befindet. Hier gen¨ ugen zwei sichtbare Satelliten.

192

8 Anwendungsbeispiel GPS/INS-Integration

ist, kann ein unbeabsichtigtes St¨ oren der Satellitensignale von Fernseh- oder WLANSendern verursacht werden. Zur Fusion der Daten eines Inertialnavigationssystems und eines GPS-Empf¨angers wer¨ den fast ausschließlich Kalman-Filter eingesetzt. Ublicherweise kommen Error State Space Kalman-Filter zum Einsatz, die anhand der vom GPS-Empf¨anger gelieferten St¨ utzinformationen die Fehler der Inertialnavigationsl¨osung sch¨atzen, die daraufhin korrigiert wird. Zus¨ atzlich werden meist deterministische Fehler der Inertialsensorik wie Biase, manchmal auch noch Skalenfaktorfehler und Misalignment, gesch¨atzt. Durch diese Online-Kalibration der IMU verbessert sich die Performance des Inertialnavigationssystems, was insbesondere bei GPS-Ausf¨allen relevant ist. Bei einem solchen integrierten Navigationssystem sind verschiedene Systemarchitekturen m¨ oglich.

8.1

GPS/INS-Integrationsstrategien

Anhand der verwendeten St¨ utzinformationen kann zwischen den verschiedenen Systemarchitekturen unterschieden werden.

8.1.1

Loosely Coupled System

Ein Loosely Coupled System ist dadurch gekennzeichnet, dass GPS-Positions- und Geschwindigkeitsmessungen als St¨ utzinformationen verwendet werden. Diese Integrationsvariante ist am weitesten verbreitet, nicht zuletzt weil die Realisierung eines solchen Systems verglichen mit anderen Ans¨ atzen den geringsten Entwicklungsaufwand erfordert. Der wesentliche Nachteil dieses Ansatzes besteht darin, das bei weniger als vier sichtbaren Satelliten keine St¨ utzung der Inertialnavigation mehr erfolgen kann, die Navigationsfehler wachsen also w¨ ahrend GPS-Ausf¨allen entsprechend der G¨ ute des Inertialnavigationssystems an. Weitere Probleme k¨ onnen auftreten, wenn die vom GPS-Empf¨anger gelieferten Positions- und Geschwindigkeitsinformationen aus einem empf¨angerinternen Kalman-Filter stammen3 . Die gefilterten Positionen und Geschwindigkeiten weisen Zeitkorrelationen auf, die potentiell zu Problemen im Navigationsfilter f¨ uhren k¨onnen. Unter Umst¨ anden liefert der GPS-Empf¨ anger basierend auf dem empf¨angerinternen KalmanFilter auch bei weniger als vier sichtbaren Satelliten noch Messwerte. Die Verarbeitung dieser Messwerte als unabh¨ angig f¨ uhrt dazu, dass der Navigationsfilter zu einer u atzung der G¨ ute der Zustandssch¨atzung gelangt, was bis zur ¨ beroptimistischen Einsch¨ Divergenz des Filters f¨ uhren kann. Prinzipiell kann versucht werden, diesen Problemen mit so genannten Federated-Filter-Architekturen zu begegnen, siehe [19], darauf soll hier jedoch nicht n¨ aher eingegangen werden.

3 Aufgrund der Hintereinanderschaltung zweier Filter ist auch die Bezeichnung Cascaded Integration gel¨ aufig.

8.1 GPS/INS-Integrationsstrategien

8.1.2

193

Tightly Coupled System

F¨ ur die meisten Autoren ist ein Tightly Coupled System dadurch gekennzeichnet, dass im Navigationsfilter direkt Pseudorange- und Deltarange-Messungen als St¨ utzinformaur andere Autoren ist zus¨atzlich die tionen verarbeitet werden [12], [108], [98], [66]. F¨ St¨ utzung des GPS-Empf¨ angers durch das Inertialnavigationssystem ein entscheidender Gesichtspunkt, [123],[99], ohne diese St¨ utzung wird von einem Closely Coupled System gesprochen [74]. Die St¨ utzung des GPS-Empf¨angers kann sich dabei sowohl auf die Akquisition bzw. Re-Akquisition als auch auf das Tracking beziehen. W¨ahrend im ersteren Fall die f¨ ur die Akquisition ben¨ otigte Zeit verk¨ urzt werden soll, ist das Ziel bei der St¨ utzung des Trackings eine geringe Bandbreite der Tracking Loops des GPS-Empf¨angers zu erm¨ oglichen: F¨ ur eine gute Jamming-Robustheit ist eine geringe Bandbreite vorteilhaft, um Trajektoriendynamiken folgen zu k¨onnen ist jedoch eine gr¨oßere Bandbreite notwendig [43]. Wird die Trajektoriendynamik dem Inertialnavigationssystem entnommen, kann auch bei signifikanten Man¨overn eine geringe Bandbreite beibehalten werden. Der entscheidende Vorteil eines Tightly Coupled Systems besteht nun darin, dass auch bei weniger als vier sichtbaren Satelliten eine eingeschr¨ankte St¨ utzung des Inertialnavigationssystems anhand der Pseudorange- und Deltarange-Messungen erfolgt. Dadurch kann das in dieser Situation bei einem Loosely Coupled System vorliegende Anwachsen der Navigationsfehler je nach Szenario verlangsamt oder eventuell sogar verhindert werden. Nachteilig ist der deutlich gr¨ oßere Integrationsaufwand: Abh¨anging von den vom Empf¨ anger zu Verf¨ ugung gestellten Nachrichten m¨ ussen nun GPS-Subframe-Daten dekodiert, Satellitenpositionen und Geschwindigkeiten aus Ephemeriden-Daten berechnet und Korrekturen der Satellitenuhrenfehler sowie relativistische Korrekturen durchgef¨ uhrt werden. Die meisten Empf¨ anger liefern Pseudorange- und Deltarange-Messungen, die zur vollen Sekunde der GPS-Zeit g¨ ultig sind, u ¨ ber den Sendezeitpunkt des Satellitensignals ist in der Regel jedoch nichts bekannt. Da zur Verarbeitung dieser Messungen aber die Satellitenpositionen und Geschwindigkeiten zu den jeweiligen Sendezeitpunkten relevant sind, m¨ ussen diese iterativ berechnet werden.

8.1.3

Ultra-Tight und Deep Integration

Bei diesen Integrationsvarianten werden im Navigationsfilter die in der empf¨angerinternen Signalverarbeitung anfallenden I & Q Samples verarbeitet, die Tracking Loops des GPS-Empf¨ angers werden durch das Navigationsfilter geschlossen. Diese sind damit nicht mehr unabh¨ angig voneinander, wie das bei den bisher angesprochenen Integrationsstrategien der Fall war. Durch diese Verkn¨ upfung profitiert das Tracking eines Satelliten von dem Tracking weiterer Satelliten [123],[46],[12]. Der Vorteil dieser Integrationsvariante besteht in einer gesteigerten Genauigkeit der Navigationsl¨osung, vor allem aber in der F¨ ahigkeit, ein schlechteres Signal-Rauschverh¨altnis des GPS-Signals zu tolerieren. Dies resultiert in einer gr¨ oßeren Jamming-Robustheit, so dass diese Systemarchitektur vor allem bei milit¨ arischen Anwendungen von Interesse ist. Der Nachteil dieser Ans¨atze besteht in einem nochmals gesteigerten Integrationsaufwand, dar¨ uber hinaus sind Eingriffe in die empf¨ angerinterne Signalverarbeitung notwendig, was in der Regel nur den Empf¨ angerherstellern oder in enger Zusammenarbeit mit diesen m¨oglich ist. Diese In-

194

8 Anwendungsbeispiel GPS/INS-Integration

tegrationsvarianten werden daher im Folgenden nicht n¨aher betrachtet, weiterf¨ uhrende Informationen sind z.B. in [41] und [84] zu finden.

8.2

Entwurf eines Navigationsfilters

Im Folgenden soll exemplarisch der Entwurf eines Navigationsfilters zur Fusion von GPS- und Inertialsensordaten gezeigt werden. Aufgrund der vielen Gemeinsamkeiten werden Loosely und Tightly Coupled Integration parallel betrachtet. Es wird ein Error State Space Kalman-Filter entworfen.

8.2.1

Systemmodell

Zu Beginn des Filterentwurfs sind eine Reihe von Designentscheidungen zu treffen. Neben der Wahl einer Error State Space“-Formulierung z¨ahlt dazu die Behandlung der ” Inertialsensordaten als bekannte Eingangsgr¨oßen, die somit im Propagationsschritte des Filters und nicht – obwohl es sich nat¨ urlich um Messwerte handelt – im Messschritt verarbeitet werden. Das Rauschen der Inertialsensoren wird dem Systemrauschen zugeschlagen. Durch diese Vorgehensweise l¨ asst sich ohne merkliche Einbußen in der Performance die Rechenlast deutlich reduzieren: Bei einer Verarbeitung der Inertialsensordaten im Messschritt des Filters m¨ usste dieser mit der Taktrate der Inertialsensordaten ausgef¨ uhrt werden, was aufgrund der Berechnung einer Inversen und der Anpassung der Kovarianzmatrix der Zustandssch¨ atzung mit einer massiven Rechenlast verbunden w¨are. Bei einer Ber¨ ucksichtigung der Inertialsensordaten im Propagationsschritt des Filters muss dieser nicht zwingend mit der Taktrate der Inertialsensordaten ausgef¨ uhrt werden. Bei einem Error State Space Kalman-Filter besteht die Aufgabe des Propagationsschrittes in der Fortschreibung der Kovarianzmatrix des Sch¨atzfehlers, die nach dem letzten Messschritt im Zustandsvektor enthaltenen gesch¨atzten Fehler verschwinden nach deren Korrektur, so dass eine Propagation des Filterzustandsvektors entfallen kann. W¨ahrend die eigentliche Verarbeitung der Inertialsensordaten im Strapdown-Algorithmus nat¨ urlich mit der vollen Datenrate erfolgen muss, kann der Einfluss dieser Propagation auf die Kovarianzmatrix der Zustandssch¨ atzung in der Regel mit einer niedrigeren Taktrate gerechnet werden, ein typischer Wert w¨ aren zehn Hertz. Neben einer Sch¨ atzung der Navigationsfehler sollen auch die Fehler der angenommenen Inertialsensorbiase gesch¨ atzt werden, um eine Online-Kalibration der IMU zu erm¨oglichen. Mit diesen Vorgaben resultiert f¨ ur ein Loosely Coupled System ein Filter mit einem Zustandsvektor der Dimension f¨ unfzehn: ∆x =

n n n ∆xn , ∆xe , ∆xd , ∆veb,n , ∆veb,e , ∆veb,d , ∆α, ∆β, ∆γ , % &' ( % &' ( &' ( % Positionsfehler ∆ p

Geschwindigkeitsfehler ∆ v

∆ba,x , ∆ba,y , ∆ba,z % &' ( Fehler der B.-Messer-Biase ba

,

 Lagefehler ∆ψ

∆bω,x , ∆bω,y , ∆bω,z % &' ( Fehler des Drehratensenor-Biase bω

T (8.1)

8.2 Entwurf eines Navigationsfilters

195

Prinzipiell h¨ atten die Positionsfehler in Nord- und Ostrichtung als Winkelfehler von L¨ angen- und Breitengrad auch in Radian angegeben werden k¨onnen. Dies kann jedoch zu numerischen Problemen f¨ uhren, falls in einem Messschritt eine vollst¨andige GPSPositionsmessung bestehend aus L¨ angengrad, Breitengrad und H¨ohe verarbeitet werden soll: Eine Standardabweichung der GPS-Positionsmessung in Nord- und Ostrichtung uhrt auf eine Standardabweichung in L¨angen- und Breitengrad in der von σx = 10 m f¨ σx 10 m −6 Gr¨ oßenordnung σϕ,λ ≈ RErde = 6378137 rad. Betrachtet man nun den m ≈ 1.57 × 10 Fall, dass die Standardabeichung der Positionssch¨atzung in allen drei Raumrichtungen ebenfalls 10 m betr¨ agt, so muss zur Berechnung der Kalman-Gain-Matrix die Inverse einer Matrix berechnet werden, deren relevante Eintr¨age im Wertebereich zwischen 4.9 × 10−12 und 200 liegen. Die Inversion einer solchen Matrix ist numerisch extrem anspruchsvoll, mit einer Standard-Floating-Point-Arithmetik ohne weitere Maßnahmen sogar unm¨ oglich. Die Angabe der Positionsfehler in Nord- und Ostrichtung in Metern ist eine M¨ oglichkeit, diese Schwierigkeiten sehr einfach zu vermeiden. Das Systemmodell des Kalman-Filters muss – neben den deterministischen Inertialsensorfehlern – die Propagation der Navigationsfehler mit der Zeit beschreiben. Diese Fehlerdifferentialgleichungen k¨ onnen aus den Gleichungen der inertialen Navigation gewonnen werden. Da es sich hierbei um nichtlineare Gleichungen handelt, muss um den gesch¨ atzten Systemzustand linearisiert werden. Positionsfehlerdifferentialgleichungen ¨ Die Anderung des Positionsfehlers in Nordrichtung h¨angt im Wesentlichen von dem Gen schwindigkeitsfehler in Nordrichtung ∆v eb,n und dem H¨ohenfehler ∆xd ab. Die zeitliche Ableitung des Breitengrades ist gegeben durch ϕ˙ =

n v eb,n . Rn − h

(8.2)

Unter der Annahme, dass f¨ ur die Bestimmung der Fehlerdifferentialgleichungen die Kr¨ ummungsradien der Erde als konstant angenommen werden k¨onnen, ist die Taylorreihenentwicklung von Gl. (8.2) gegeben durch ∆ϕ˙ = =

∂ ϕ˙ ∂ ϕ˙ n · ∆xd + · ∆v eb,n n ∂h ∂v eb,n n v eb,n 1 n ∆v eb,n ∆xd + . (Rn − h)2 Rn − h

(8.3)

Da der Positionsfehler in Nordrichtung in Metern angegeben werden soll, muss der Breitengradfehler durch Multiplikation mit dem entsprechenden Faktor umgerechnet werden: ∆x˙ n = (Rn − h) · ∆ϕ˙ n v eb,n n ∆xd + ∆v eb,n = (Rn − h)

(8.4)

196

8 Anwendungsbeispiel GPS/INS-Integration

¨ In der gleichen Weise l¨ asst sich die Anderung des Positionsfehlers in Ostrichtung herleiten. Die Ableitung des L¨ angengrades ist gegeben durch λ˙ =

n v eb,e , (Re − h) cos ϕ

(8.5)

die Taylorreihenentwicklung liefert ∆λ˙ = = =

∂ λ˙ ∂ λ˙ ∂ λ˙ n · ∆ϕ + · ∆xd + · ∆v eb,e n ∂ϕ ∂h ∂v eb,e ∆xn ∂ λ˙ ∂ λ˙ ∂ λ˙ n · + · ∆xd + · ∆v eb,e n ∂ϕ Rn − h ∂h ∂v eb,e n n v eb,e sin ϕ v eb,e ∆xn · + · ∆xd (Re − h) cos2 ϕ Rn − h (Re − h)2 cos ϕ 1 n · ∆v eb,e + . (Re − h) cos ϕ

(8.6)

¨ ¨ Die Anderung des Positionsfehlers in Ostrichtung erh¨alt man aus der Anderung des L¨ angengradfehlers zu ∆x˙ e = (Re − h) cos ϕ · ∆λ˙ n n v eb,e tan ϕ v eb,e n · ∆xn + · ∆xd + ∆v eb,e = . Rn − h (Re − h)

(8.7)

Die zeitliche Ableitung der H¨ ohe h u ¨ ber dem Erdellipsoid ist gleich der Vertikalgeschwindigkeit, daher ist der Zusammenhang zwischen H¨ohenfehler und Vertikalgeschwindigkeitsfehler gegeben durch n . ∆x˙ d = ∆v eb,d

(8.8)

Geschwindigkeitsfehlerdifferentialgleichungen Im Rahmen der Strapdown-Rechnung muss die von den Beschleunigungsmessern mitgemessene Schwerebeschleunigung rechnerisch aus den Beschleunigungsmesserdaten eliminiert werden. Da hierzu Lageinformationen notwendig sind, f¨ uhren Lagefehler zu einer fehlerhaften Interpretation der Beschleunigungsmesserdaten und damit zu Geschwindigkeitsfehlern. Nat¨ urlich beeinflussen die deterministischen Fehler der Beschleunigungsmesser ebenfalls die zeitliche Propagation der Geschwindigkeitsfehler. Ausgangspunkt der Herleitung der Geschwindigkeitsfehlerdifferentialgleichungen ist die aus Abschnitt 3 bekannte Gleichung (3.150) n b n n n = Cnba ib − (2 ω ie +ω  en ) × v eb + g ln . v˙ eb

Zun¨ achst m¨ ussen die Zusammenh¨ ange der darin enthaltenen Gr¨oßen mit den Gr¨oßen, deren Fehler im Zustandsvektor des Kalman-Filters enthalten sind, ermittelt werden.

8.2 Entwurf eines Navigationsfilters

197

Lagefehlerabh¨ angigkeiten Die reale Richtungskosinusmatrix Cnb ist nat¨ urlich nicht bekannt, der Zusammenhang zwischen der realen Richtungskosinusmatrix und der bekannten, gesch¨atzten Richtungsˆ n wird durch eine Richtungskosinusmatrix Cnˆ beschrieben: kosinusmatrix C n b ˆ n Cn ˆn = C C b n b

(8.9)

Die gesch¨ atzte Richtungskosinusmatrix transformiert Gr¨oßen, die in Koordinaten des k¨ orperfesten Koordinatensystems gegeben sind, in ein gesch¨atztes Navigationskoordinatensystem, das sich vom realen Navigationskoordinatensystem unterscheidet. Die Transformation vom realen ins gesch¨ atzte Navigationskoordinatensystem ist durch die Lage = (α, β, γ)T gegeben. Da diese als klein angenommen werden k¨onnen, kann fehler ψ eine entsprechende N¨ aherung der Richtungskosinusmatrix verwendet werden: ⎛ ⎞ 1 γ −β     = (I − Ψ) (8.10) Cnnˆ = ⎝−γ 1 α ⎠ = I − ψ× β −α 1 Insgesamt erh¨ alt man so ˆ n = (I − Ψ)Cn . C b b

(8.11)

Mit dem Zusammenhang (I − Ψ)−1 = (I − Ψ)T = (I + Ψ)

(8.12)

ergibt sich schließlich ˆ nb . Cnb = (I + Ψ)C

(8.13)

Inertialsensorfehlerabh¨ angigkeiten ˜ b werden als Summe der wahren Beschleunigungen Die gemessenen Beschleunigungen a ib aibb , den Beschleunigungsmesser-Biasen ba und einem Rauschterm −na modelliert: ˜ b = a b + ba − na a ib ib Aufl¨ osen nach

aibb

(8.14)

liefert

˜ b − ba + na . aibb = a ib

(8.15)

Taylorreihenentwicklung Die Geschwindigkeitsfehlerdifferentialgleichungen werden durch eine Taylorreihenentwicklung von Gl. (3.150) gewonnen, das Einsetzen der Zusammenh¨ange (8.13) und (8.15) f¨ uhrt auf  n ˜ b − ba + na − (2ω n + ω n ) × v n + g n ˆ n a v˙ eb = (I + Ψ) C b ib ie en eb l  n n ˜ b − ba − (2ω n + ω ˆ n a ≈ (I + Ψ)C  en ) × v eb + g ln + Cˆbn na . (8.16) b ib ie

198

8 Anwendungsbeispiel GPS/INS-Integration

Schließlich muss noch beachtet werden, dass der Coriolis-Term von der Geschwindigkeit n abh¨ angt: v eb ⎛ 2Ω cos ϕ +

⎜ n n −(2 ωien +  ωen ) × v eb = −⎜ ⎝ ⎛ =



vn

n v eb,e Re −h

− Rneb,n −h

−2Ω sin ϕ −

n v eb,e tan ϕ Re −h



⎞ ⎟ ⎟ × v n eb ⎠ ⎞

n n v eb,n v eb,d v n tan ϕ n − 2Ω sin ϕ + eb,e v eb,e R Re −h n −h  ⎟ ⎜ n n ⎜ 2Ω sin ϕ + v eb,e tan ϕ v n + 2Ω cos ϕ + v eb,e n ⎟ v ⎟ ⎜ eb,n eb,d Re −h Re −h

⎝

 − 2Ω cos ϕ +

n v eb,e Re −h



n v eb,e −

n )2 (v eb,n

⎠

(8.17)

Rn −h

Der Coriolis-Term h¨ angt zus¨ atzlich noch vom Breitengrad ϕ ab, diese Abh¨angigkeit kann jedoch vernachl¨ assigt werden. Ebenso k¨onnen die Schwerebeschleunigung und die Kr¨ ummungsradien der Erde als konstant angenommen werden, so dass die Geschwindigkeitsfehlerdifferentialgleichung formal durch n ∆v˙ eb =

n n ˙n ∂v˙ eb ∂v˙ eb n  + ∂v eb ∆ba ∆v eb + ∆Ψ n  ∂v eb ∂ψ ∂ba

(8.18)

gegeben ist. F¨ ur die Ableitung nach der Geschwindigkeit erh¨alt man ⎛ ⎞ n n n v eb,n v eb,d v eb,e tan ϕ −2Ω sin ϕ − 2 Rn −hn Rne −h Rn −h n n ⎜ n ∂v˙ eb v eb,n v tan ϕ tan ϕ v eb,d v eb,e ⎟ ⎟ , =⎜ 2Ω sin ϕ + eb,e + 2Ω cos ϕ + n ⎝ Re −h Re −h Re −h Re −h ⎠ ∂v eb n n v eb,n v eb,e −2 Rn −h −2Ω cos ϕ − 2 Re −h 0 (8.19) Die Ableitung nach den Lagefehlern ergibt sich zu n ∂v˙ eb ∂  ˆ n ˜ b  ΨCb aib − ba =   ∂ψ ∂ψ  ∂  ˆ n ˜ b   =− Cb (aib − ba )× ψ  ∂ψ   ˆ n× , = − a ib

(8.20)

und f¨ ur die Ableitung nach den Beschleunigungsmesserbiasen erh¨alt man  n ∂v˙ eb ∂  ˆ n −ba (I + Ψ)C = b ∂ba ∂ba ˆn . ≈ −C b

(8.21) (8.22)

8.2 Entwurf eines Navigationsfilters

199

Lagefehlerdifferentialgleichungen Auch die Lagefehler selbst sind einer Dynamik unterworfen, die im Systemmodell des Kalman-Filters erfasst werden muss. Zum einen f¨ uhren nicht oder unvollst¨andig kom¨ pensierte Drehratensensorbiase zu einer Anderung der Lagefehler mit der Zeit. Zum anderen wird die Erddrehrate von den Drehratensensoren mitgemessen und muss im Strapdown-Algorithmus rechnerisch kompensiert werden. Fehler bei dieser Kompensa¨ tion tragen ebenfalls zu einer Anderung des Lagefehlers bei. Die entsprechenden Gleichungen werden im Folgenden hergeleitet. Zun¨ achst wird Gl. (8.11) nach der kreuzproduktbildenden Matrix der Lagefehler aufgel¨ ost: ˆ n = Cn − ΨCn C b b b n nT ˆ nb CnT −C =Ψ b + Cb Cb

ˆ n CnT + I Ψ = −C b b

(8.23)

F¨ ur die zeiliche Ableitung erh¨ alt man mit der Produktregel ˙ = −C ˆ˙ nb CnT ˆ n ˙ nT Ψ b − Cb Cb .

(8.24)

Mit der zeitlichen Ableitung der Richtungskosinusmatrix nach Gl. (3.110) ergibt sich ˙ n = Cn Ωb C b b nb   = Cnb Ωbni + Ωbib ,

(8.25)

durch Erweitern mit dem Produkt der Richtungskosinusmatrix und ihrer Transponierten l¨ asst sich die kreuzproduktbildende Matrix der Drehrate zwischen Inertialkoordinatensystem und Navigationskoordinatensystem von k¨orperfesten Koordinaten ins Navigationskoordinatensystem umrechnen: ˙ n = Cn Ωb + Cn Ωb Cb Cn C b b ib b ni % n&' b(

(8.26)

˙ nb = Cnb Ωbib − Ωnin Cnb C

(8.27)

I

In analoger Weise erh¨ alt man f¨ ur die zeitliche Ableitung der gesch¨atzten Richtungskosinusmatrix ˆ bib − Ω ˆ nb . ˆ˙ nb = C ˆ nb Ω ˆ nin C C

(8.28)

200

8 Anwendungsbeispiel GPS/INS-Integration

Einsetzen von Gl. (8.27) und Gl. (8.28) in Gl. (8.24) liefert ˙ = −C ˆ˙ n CnZ − C ˆn ˙ nT Ψ b Cb b b  n b  n n T ˆ bib − Ω ˆ nb CnT ˆ nb Ω ˆ nin C ˆn = − C b − Cb Cb Ωib − Ωin Cb  bT nT  nT nT ˆ bib CnT ˆ nb Ω ˆ n ˆ n nT ˆn = −C b + Ωin Cb Cb − Cb Ωib Cb − Cb Ωin ˆ bib CnT ˆ n ˆ n nT ˆ n b nT ˆ n nT n ˆ nb Ω = −C b + Ωin Cb Cb + Cb Ωib Cb − Cb Cb Ωin  ˙ = −C ˆn Ω ˆ n nT ˆ b − Ωb CnT + Ω ˆn C ˆ n nT n Ψ b ib ib b in b Cb − Cb Cb Ωin .

(8.29)

Diese Gleichung l¨ asst sich unter Verwendung des Zusammenhangs zwischen realer und gesch¨ atzter Richtungskosinusmatrix Gl. (8.11) umformen:  n nT ˙ = −(I − Ψ)Cnb Ω ˆ bib − Ωbib CnT ˆn Ψ b + Ωin (I − Ψ)Cb Cb −(I − Ψ)Cnb CnT Ωnin  b ˆ b − Ωb CnT + Ω ˆ n (I − Ψ) − (I − Ψ)Ωn = −(I − Ψ)Cnb Ω ib ib b in in   ˆ b − Ωb CnT + ΨCn Ω ˆ b − Ωb CnT = −Cnb Ω ib ib b b ib ib b ˆn − Ω ˆ n Ψ − Ωn + ΨΩn +Ω in in in in

(8.30)

Mit ˆ b − Ωb , ∆Ωbib = Ω ib ib

ˆ n − Ωn ∆Ωnin = Ω in in

(8.31)

l¨ asst sich schreiben: ˙ = −Cn ∆Ωb CnT + ΨCn ∆Ωb CnT + ∆Ωn − Ω ˆ n Ψ + ΨΩn Ψ b ib b b ib b in in in

(8.32)

Da sowohl die Lagefehler Ψ als auch die Differenz von gesch¨atzter und realer Drehrate ∆Ωbib als klein angenommen werden kann, kann deren Produkt vernachl¨assigt werden: ΨCnb ∆Ωbib CnT b ≈ 0

(8.33)

n ist als Summe von Erddrehrate und Transportrate durch Position und Die Drehrate ω  in Geschwindigkeit bestimmt. Bei den zu erwartenden Positions- und Geschwindigkeitsfehlern ist der Fehler dieser gesch¨ atzten Drehrate ebenfalls vernachl¨assigbar:

ˆ n ≈ Ωn Ω in in

(8.34)

Damit erh¨ alt man schließlich ˙ = ΨΩnin − Ωnin Ψ + ∆Ωnin − Cnb ∆Ωbib CnT Ψ b .

(8.35)

8.2 Entwurf eines Navigationsfilters

201

Da f¨ ur kreuzproduktbildende Matrizen (kpM) der Zusammenhang AB − BA = kpM(Ab)

(8.36)

gilt, kann wieder zu vektoriellen Gr¨ oßen u ¨ bergegangen werden: n n ˙ = −  + ∆ ψ ωin ×ψ ωin − Cnb ∆ ωibb

(8.37)

Einsetzten von Gl. (8.13) f¨ uhrt auf n n b  + ∆ ˆ n ∆ ˙ = − ×ψ ωin −C ωibb , ψ ωin b ωib + Ψ∆

(8.38)

da auch hier das Produkt aus kleinen Fehlertermen vernachl¨assigt werden kann, ergibt sich n n  + ∆ ˙ = − ˆ nb ∆ ×ψ ωin −C ωibb . ψ ωin

(8.39)

 bekannt, so werden diese unmittelbar zur Sind nach einem Messschritt Lagefehler ψ Korrektur der gesch¨ atzten Richtungskosinusmatrix herangezogen. Anschließend m¨ ussen  = 0 gesetzt werden. die aufgrund dieser Korrektur verschwundenen Lagefehler zu ψ Damit besteht aber vor einem Propagationsschritt kein Unterschied mehr zwischen den  und den totalen Gr¨oßen ψ,  so dass Gl. (8.39) umgeschrieben Fehlern der Lagefehler ∆ψ werden kann: n n  + ∆ ˆ n ∆ω b ˙ = − × ∆ψ ωin −C ∆ψ ωin b ib

(8.40)

˜ b aus der Nimmt man als Sensorfehlermodell an, dass sich die gemessene Drehrate ω ib  Summe der realen Drehrate, einem Bias bω und einem Rauschterm nω ergibt, ω ˜ ibb =  ωibb + bω + nω ,

(8.41)

so l¨ asst sich ∆ωibb wie folgt berechnen: ˜b − ∆ ωibb =  ω ωibb = bω + nω ib

(8.42)

∂∆ ωibb = 1 · ∆bω ∂bω

(8.43)

Mit

erh¨ alt man schließlich ∆ ωibb = ∆bω + nω .

(8.44)

202

8 Anwendungsbeispiel GPS/INS-Integration

n Der Term ∆ωin wird durch eine Taylorreihenentwicklung von ⎛ ⎞ vn Ω cos ϕ + Reeb,e −h ⎜ ⎟ n v eb,n n n ⎟ ωin  =ω  ien +  ωen =⎜ − ⎝ ⎠ R −h n

−Ω sin ϕ −

(8.45)

n v eb,e tan ϕ Re −h

bestimmt: n ∆ ωin



n n ∂ ωin ∂ ωin ∆ϕ n · + = · ∆veb n ∆h ∂ϕ, h ∂veb ⎞ ⎛ n v eb,e −Ω sin ϕ 2 ∆xn

(Re −h) n ⎟ ⎜ v eb,n ⎟ ⎜ = ⎝ 0 − (Rn −h)2 ⎠ · Rn −h ∆xd n n v eb,e v eb,e tan ϕ −Ω cos ϕ − (Re −h) − 2 2 cos ϕ (Re −h) ⎛ ⎞ 1 0 0 Re −h n 0 0⎠ ∆veb + ⎝− Rn1−h tan ϕ 0 − Re −h 0 ⎛ ⎞ n v eb,e ϕ − ΩRnsin 2

−h (Re −h) n ⎜ ⎟ ∆xn v eb,n ⎜ ⎟ = ⎝ 0 − (Rn −h)2 ⎠ · ∆xd n n v eb,e v eb,e tan ϕ Ω cos ϕ − Rn −h − (Rn −h)(Re −h) cos2 ϕ − (R 2 e −h) ⎞ ⎛ 1 0 0 Re −h n 0 0⎠ ∆veb + ⎝− Rn1−h tan ϕ 0 − Re −h 0

(8.46)

Vernachl¨ assigt man die Terme, bei denen durch das Quadrat des Erdradius dividiert wird, erh¨ alt man schließlich ⎞ ⎛ 1 ϕ⎞ 0 − ΩRnsin Re −h 0 −h n 0 0⎠ ∆veb ≈ ⎝ 0 ⎠ ∆xn + ⎝− Rn1−h . Ω cos ϕ tan ϕ 0 − Re −h 0 − Rn −h ⎛

n ∆ ωin

(8.47)

Damit sind alle Komponenten der Lagefehlerdifferentialgleichung bestimmt. Inertialsensorfehlerdifferentialgleichungen Bei der Modellierung der Inertialsensorbiase sind verschiedene Modelle m¨oglich. Handelt es sich um hochwertige Beschleunigungsmesser und hochwertige Drehratensensoren wie Ringlaserkreisel oder Faserkreisel, kann in der Regel die Biasdrift vernachl¨assigt werden, die Biase k¨ onnen als konstant angenommen werden: b˙ a = 0 ,

b˙ ω = 0

(8.48)

8.2 Entwurf eines Navigationsfilters

203

Muss eine Biasdrift ber¨ ucksichtigt werden, bieten sich zur Modellierung Gauß-MarkovProzesse erster Ordnung an, b˙ a = − 1 ba + nb a τba

,

b˙ ω = − 1 bω + nb , ω τbω

(8.49)

eine Modellierung als Random-Walk Prozesse ist ebenfalls m¨oglich: b˙ a = nb a

,

b˙ ω = nb ω

(8.50)

Die Vektoren nba und nbω bezeichnen hierbei das die Rauschprozesse treibende, weiße Rauschen. Uhrenfehlerdifferentialgleichungen Um Pseudorange- und Deltarange-Messungen direkt im Navigationsfilter verarbeiten zu k¨ onnen, muss das Kalman-Filter-Systemmodell um ein Modell des Empf¨angeruhrenfehlers erweitert werden. Ein einfaches Empf¨ angeruhrenfehlermodell ist durch  •= cT

•

cδt cδt 0 1 ncδt = + ncδt˙ 0 0 cδ t˙ cδ t˙

(8.51)

 des Uhrenfehlermodells beinhaltet den Uhrenfehler gegeben. Der Zustandsvektor cT ˙ bei dem Vektor ncT = (ncδt , ncδt˙)T handelt es sich um cδt und die Uhrenfehlerdrift cδ t, weißes, normalverteiltes Rauschen. Eine Alternative zur Erweiterung des Systemmodells um ein Uhrenfehlermodell besteht darin, Differenzen der zum selben Zeitpunkt vorliegenden Pseudorange- und DeltarangeMessungen zu verarbeiten. Da jede Pseudorange- und Deltarange-Messung mit dem selben Uhrenfehler bzw. derselben Uhrenfehlerdrift behaftet ist, werden durch eine Differenzenbildung Uhrenfehler und Uhrenfehlerdrift aus den Gleichungen eliminiert. Ein Vorteil dieser Vorgehensweise besteht darin, dass eine Bestimmung der Uhrenfehlermodelparameter, d.h. der spektralen Leistungsdichte des treibenden Rauschens, nicht notwendig ist. W¨ ahrend ein Uhrenfehlermodell das tats¨achliche Verhalten nur n¨aherungsweise beschreibt, werden durch den Verzicht auf ein Uhrenfehlermodell nat¨ urlich auch Modellierungsfehler vermieden. Um u ¨ berhaupt sinnvoll mit einem Uhrenfehlermodell arbeiten zu k¨ onnen, ist es notwendig, dass der Empf¨anger ein Driften des Uhrenfehlers zul¨ asst und diesen nicht in jedem Zeitschritt korrigiert. Selbst wenn diese Voraussetztung gegeben ist, setzen die meisten Empf¨anger den Uhrenfehler von Zeit zu Zeit zur¨ uck: Genau genommen kann der in den Pseudorange-Messungen enthaltene Uhrenfehler nicht gr¨ oßer werden als die Periodendauer des C/A-Codes, also eine Millisekunde bzw. umgerechnet ungef¨ ahr 300 km, da eine gr¨oßere Laufzeitverschiebung nicht gemes¨ sen werden kann. Ubersteigt der Fehler der Empf¨angeruhr die L¨ange des C/A-Codes, andern sich einfach die empf¨ angerinternen Pseudorange-Mehrdeutigkeitswerte. Da das ¨ auch mit einem großen Sprung in den Pseudorange-Messungen verbunden ist, kann das jedoch mit einer einfachen Heuristik detektiert und entsprechend behandelt werden.

204

8 Anwendungsbeispiel GPS/INS-Integration

Nachteilig bei der Differenzenbildung ist, dass die als Messwerte zu verarbeitenden Differenzen der zum selben Zeitpunkt vorliegenden Pseudoranges miteinander korreliert sind, da derselbe Pseudorange in mehrere Differenzen eingeht. Gleiches gilt nat¨ urlich auch f¨ ur die Deltaranges. Diese Korrelationen k¨onnen ber¨ ucksichtigt werden, indem f¨ ur eine Verarbeitung im Kalman-Filter alle Messwertdifferenzen zu einem Messvektor zusammengefasst werden und eine entsprechende Kovarianzmatrix Rk dieses Vektors bestimmt wird, die diese Korrelationen enth¨alt. Eine sequentielle Verarbeitung der einzelnen Messwertdifferenzen ist auch m¨ oglich, falls vorher durch eine geeignete Transformation eine Dekorrelation durchgef¨ uhrt wird. Die Verarbeitung von Differenzen der zum selben Zeitpunkt vorliegenden Pseudoranges und Deltaranges wird im Folgenden nicht weiter betrachtet, N¨aheres ist in [38] zu finden. Betrachtungen zu Empf¨ angeruhrenfehlermodellen sind in [17] und [26] enthalten. Zusammenfassung der Gleichungen ¨ Der Ubersichtlichkeit halber sind in Gl. (8.59) und (8.60) die vollst¨andigen Systemmodelle eines Loosely und eines Tightly Coupled Systems in Zustandsraumdarstellung angegeben, die Inertialsensorbiase wurden als Random-Walk-Prozesse modelliert. Die auftretenden Submatrizen k¨ onnen den Gl. (8.52) – (8.58) entnommen werden. ⎛

0

0

⎜ n F11 = ⎝ v eb,e tan ϕ 0 Rn −h 0 0

⎞ n v eb,n (Rn −h) n ⎟ v eb,e ⎠ (Re −h) 0

⎛ F22 =

(8.52)

n v eb,d R −hn n ⎜ ⎜2Ω sin ϕ + v eb,e tan ϕ ⎝ Re −h vn −2 Rneb,n −h

n v eb,e tan ϕ Rne −h n v eb,n tan ϕ v + Reeb,d Re −h −h vn −2Ω cos ϕ − 2 Reeb,e −h

−2Ω sin ϕ − 2

n v eb,n Rn −h

2Ω cos ϕ +

⎞ ⎟ n v eb,e ⎟ Re −h ⎠

0 (8.53)

⎛

F23

⎞ ˆn ˆn a 0 a ib,z − ib,y ⎜ ˆn ˆn ⎟ = ⎝−a 0 a ib,z ib,x ⎠ ˆn ˆn −  a 0 a ib,y ib,x

(8.54)

F31

⎞ ϕ − ΩRnsin −h 0 0 0 0⎠ =⎝ 0 ϕ 0 0 − ΩRcos n −h

(8.55)

⎛

⎛

0

1 Re −h

0 F32 = ⎝− Rn1−h ϕ 0 − Rtan e −h

⎞ 0 0⎠ 0

(8.56)

8.2 Entwurf eines Navigationsfilters ⎛ 0

⎜ n veb,e tan(ϕ) F33 = ⎜ ⎝ Ω sin ϕ + n Re −h v − Reb,n n −h

205

−Ω sin ϕ −

n veb,e tan ϕ Re −h

0 −Ω cos ϕ −

n veb,n Rn −h

Ω cos ϕ + n veb,e Re −h

⎞ n veb,e Re −h

⎟ ⎟ ⎠

0 (8.57)

F66 =



01 00

(8.58)

Loosely Coupled System ⎛ ⎞• ⎞ ⎞⎛ ∆ p ∆ p 0 0 F11 I 0 ⎜ 0 F22 F23 −C ⎜ ∆v ⎟ ˆ n 0 ⎟ ⎜ ∆v ⎟ b ⎜ ⎟⎜  ⎟ ⎜ ⎟ ⎜ ∆ψ ⎟ ⎜ ∆ψ ⎟ = ⎜F F F ˆ n⎟ ⎜ 31 32 33 0 −C ⎟ ⎟ ⎜ b⎟⎜ ⎝ 0 0 0 ⎝ ∆ba ⎠ ⎠ ⎝ ∆ba ⎠ 0 0 0 0 0 0 0 ∆bω ∆bω ⎞ ⎛ 0 0 0 0 ⎛ ⎞ ⎜C ˆ n 0 0 0⎟ na ⎟ ⎜ nω ⎟ ⎜ b ˆ n 0 0⎟ +⎜ ⎠ ⎟⎝ ⎜0 C b ⎝ 0 0 I 0⎠ nba nbω 0 0 0 I ⎛

(8.59)

Tightly Coupled System ⎛ ⎞⎛ ⎞• ⎞ ∆ p ∆ p 0 0 0 F11 I 0 ⎜ 0 F22 F23 −C ⎜ ∆v ⎟ ⎜ ∆v ⎟ ˆn 0 0 ⎟ b ⎜ ⎜ ⎟ ⎟⎜  ⎟ ⎜F F F ⎜ ∆ψ ⎟ ⎟ ⎜ ∆ψ ⎟ n ˆ ⎜ 31 32 33 0 −Cb 0 ⎟ ⎜ ⎜ ⎟ ⎟  ⎟ ⎜ ∆ba ⎟ = ⎜ 0 0 0 ⎟ 0 0 0 ⎟⎜ ⎜ ⎜ ⎜ ∆ba ⎟ ⎟ ⎝ 0 0 0 ⎝ ∆bω ⎠ 0 0 0 ⎠ ⎝ ∆bω ⎠   0 0 0 0 0 F66 ∆cT ∆cT ⎞ ⎛ 0 0 0 0 0 ⎛ ⎞ ⎜C ˆ n 0 0 0 0⎟ na ⎟ ⎜ nω ⎟ ⎜ b ⎜0 C ˆ n 0 0 0⎟ ⎟ ⎜ nba ⎟ b +⎜ ⎟ ⎜ 0 0 I 0 0⎟ ⎜ ⎠ ⎟⎝ ⎜ ⎝ 0 0 0 I 0⎠ nbω ncT 0 0 0 0 I ⎛

(8.60)

206

8 Anwendungsbeispiel GPS/INS-Integration

8.2.2

Messmodelle

Die Zusammenh¨ ange zwischen den vom GPS-Empf¨anger gelieferten Messwerten und dem zu sch¨ atzenden Systemzustand sind nichtlinear. Daher muss die zur Berechnung der Kalman-Gain-Matrix und zur Anpassung der Kovarianzmatrix der Sch¨atzfehler im Messschritt ben¨ otigte Messmatrix Hk durch Linearisierung der nichtlinearen Zusammenh¨ ange gewonnen werden. Um diese Linearisierung kompakt und u ¨ bersichtlich gestalten zu k¨onnen, werden Regeln zur Berechnung von Ableitungen definiert. Die Ableitung eines Skalars c nach einem Vektor x ist – hier demonstriert f¨ ur einen Vektor der Dimension zwei – gegeben durch  ∂c ∂c = ∂x 1 ∂x

∂c ∂x2



.

(8.61)

Die Ableitung eines Vektors x nach einem Vektor y ist gegeben durch   ∂x1 ∂x1 ∂x ∂y1 ∂y2 = ∂x2 ∂x2 . ∂y ∂y1 ∂y2

(8.62)

Diese Definitionen erlauben auch die konsistente Berechnung verketteter Ableitungen, die Kettenregel lautet hier ∂c ∂c ∂x = ∂y ∂x ∂y =



∂c ∂c ∂x1 ∂x2

 =

∂c ∂x1 ∂x1 ∂y1





+

∂x1 ∂x1 ∂y1 ∂y2 ∂x2 ∂x2 ∂y1 ∂y2



∂c ∂x2 ∂c ∂x1 ∂x2 ∂y1 ∂x1 ∂y2

+

∂c ∂x2 ∂x2 ∂y2

.

(8.63)

Beschreibt eine nichtlineare Funktion den Zusammenhang zwischen einem Messvektor der Dimension m × 1 und einem Zustandsvektor der Dimension n × 1, liefern diese Definitionen wie gew¨ unscht eine Messmatrix der Dimension m × n. Bei anderen, durchaus u ¨ blichen Definitionen von Ableitungen und Kettenregel ist dies nicht der Fall4 . Positionsmessung Das betrachtete Navigationsfilter sch¨ atzt Positionsfehler in Koordinaten des Navigationskoordinatensystems. Daher soll f¨ ur die Herleitung der Messmatrix zun¨achst angenommen werden, dass die vom GPS-Empf¨anger gelieferte Positionsmessung ebenfalls im Navigationskoordinatensystem gegeben sei. 4 H¨ aufig

Vektoren: ∂ x ∂c ∂ y ∂ x

.

findet man!auch folgende Definitionen der Ableitungen von Skalaren bzw. Vektoren nach ! ∂x ∂x

∂c ∂ x

=

∂c ∂x1 ∂c ∂x2

,

∂ x ∂ y

1

=

∂y1 ∂x1 ∂y2

2

∂y1 ∂x2 ∂y2

. Die Kettenregel lautet mit diesen Definitionen

∂c ∂ y

=

8.2 Entwurf eines Navigationsfilters

207

Ein GPS-Empf¨ anger liefert die Position der GPS-Antenne, diese sei mit rAn bezeichnet. Das Navigationssystem sch¨ atzt die Position des gemeinsamen Ursprungs von k¨orperfestem Koordinatensystem und Navigationskoordinatensystem, die sich von dieser Antennenposition unterscheidet. Diese Position sei mit rUn bezeichnet. Es wird ferner angenommen, dass die Position der GPS-Antenne im k¨orperfesten Koordinatensystem durch einen bekannten, konstanten Vektor l b = (lx , ly , lz )T

(8.64)

beschrieben wird, der von der im Ursprung des k¨orperfesten Koordinatensystems befindlichen IMU zur GPS-Antenne weist. Dieser Vektor l¨asst sich in Koordinaten des Navigationskoordinatensystem umrechnen: l n = (ln , le , ld )T = Cnl b b

(8.65)

Damit erh¨ alt man f¨ ur den Zusammenhang zwischen rAn und rUn : rAn = rUn + Cnbl b

(8.66)

Durch Einsetzen von Gl. (8.13) ergibt sich ˆ nl b rAn = rUn + (I + Ψ)C b ˆ n n = rU + (I + Ψ)l   ˆ ˆ . = r n + l n − l n × ψ U

(8.67)

Die Messmatrix Hpos,k des Loosely Coupled Systems ist durch die Jacobi-Matrix der rechten Seite der Gleichung (8.67) gegeben. Man erh¨alt   (8.68) Hpos,k = Hp1 Hp2 Hp3 Hp4 Hp5 mit ∂rAn =I ∂rUn ∂rAn = n = 0 ∂veb   ∂rAn ˆ = = − l n ×  ∂ψ

Hp1 =

(8.69)

Hp2

(8.70)

Hp3

(8.71)

Hp4 =

∂rAn =0 ∂ba

(8.72)

Hp5 =

∂rAn =0. ∂bω

(8.73)

208

8 Anwendungsbeispiel GPS/INS-Integration

vrot veA GPS-Antenne veb rA

l rU ωeb IMU veb

Abbildung 8.1: Unterschiedliche Geschwindigkeiten von GPS-Antenne und IMU.

Wie in Abschnitt 6.4 gezeigt verarbeitet ein Error State Space Kalman-Filter als Messwert ∆yk die Differenz zwischen dem erwarteten Messwert yˆk , der auf der aktuellen Zustandssch¨ atzung basiert, und der eigentlichen Messung y˜k . Bei der hier gew¨ahlten Formulierung muss diese Differenz in Koordinaten des Navigationskoordinatensystems gegeben sein und die Einheit Meter besitzen. Liefert der GPS-Empf¨anger eine Posi˜ und H¨ohe h, ˜ so ist ∆ypos,k tionsmessung in Form von Breitengrad ϕ, ˜ L¨angengrad λ durch ⎞  (ϕˆ − ϕ) ˜ · Rn − ˆh ⎜ ⎟  ⎜ ˆ ˜ ⎟ =⎜ λ h cos ϕˆ⎟ − λ · Re − ˆ ⎝  ⎠ ˆ−h ˜ h ⎛

∆ypos,k

(8.74) k

ˆ und h ˆ die a priori Sch¨atzungen von Breitengrad, gegeben. Hierbei bezeichnen ϕ, ˆ λ L¨ angengrad und H¨ ohe. Geschwindigkeitsmessung Die Relativposition von GPS-Antenne und IMU muss auch bei Verarbeitung einer GPS¨ Geschwindigkeitsmessung ber¨ ucksichtigt werden: Andert das Fahrzeug seine Lage, haben die damit verdundenen Rotationen zur Folge, dass sich die Geschwindigkeiten von GPS-Antenne und Ursprung des k¨ orperfesten Koordinatensystems unter Umst¨anden signifikant unterscheiden, siehe Abb. 8.1.

8.2 Entwurf eines Navigationsfilters

209

n Die Geschwindigkeit der GPS-Antenne veA bez¨ uglich des erdfesten Koordinatensystems ist in Koordinaten des Navigationskoordinatensystems gegeben durch

 n n  ebb × l b = veb + Cnb ω veA

.

(8.75)

Die f¨ ur die Relativgeschwindigkeit verantwortliche Drehrate ωebb l¨asst sich in die Erddrehrate und die Drehrate des k¨ orperfesten Koordinatensystems bez¨ uglich des Inertialkoordinatensystems zerlegen: ωieb + ω  ibb ω  ebb = −

(8.76)

Die Drehbewegungen des Fahrzeugs k¨ onnen je nach Anwendung mit mehreren hundert Grad pro Sekunde stattfinden, w¨ ahrend die Erddrehrate nur etwas mehr als f¨ unfzehn Grad pro Stunde betr¨ agt. Diese kann daher bei der Ber¨ ucksichtigung der Relativgeschwindigkeit von GPS-Antenne und IMU im Kalman-Filter vernachl¨assigt werden, man erh¨ alt  n n  ibb × l b ≈ veb + Cnb ω veA

.

(8.77)

Durch Einsetzen von Gl. (8.13) und dem Drehratensensorfehlermodell Gl. (8.41) ergibt sich n n ˆn veA ≈ veb + (I + Ψ) C b



˜ ibb − bω − nω × l b . ω

(8.78)

Das Rauschen der Drehratensensoren kann hier in aller Regel ebenfalls vernachl¨assigt ˆb=ω ˜ ibb − bω erh¨ alt man so werden, mit  ω ib n n ˜ b  b ˆ n ˆ b × l b b ˆ n ˆ nω veA ≈ veb +C b ωib × l − Cb bω × l + ΨCb  ib n n˜ b b n b n ˆ b b   ˆ ˆ ˆ = veb + Cb  ωib × l + Cb l × bω + ΨCb Ωib l     n n ˜ b × l b + C  ˆ b l b × ψ ˆ  ˆ n l b × bω − C ˆ nΩ ω = veb + C b ib b b ib   n n ˜ b × l b + C . ˆ nb  ˆ nb l b × bω − [vrot +C ×] ψ ω = veb ib

(8.79)

Die in einem Loosely Coupled System f¨ ur die Verarbeitung einer Geschwindigkeitsmessung ben¨ otigte Messmatrix Hvelo,k ist durch die Jacobi-Matrix der rechten Seite von Gl. (8.79) gegeben, man erh¨ alt   Hvelo,k = Hv1 Hv2 Hv3 Hv4 Hv5

(8.80)

210

8 Anwendungsbeispiel GPS/INS-Integration

Geringe Trajektoriendynamik

Gesamtlagefehler (mrad)

10 Relativposition ber¨ ucksichtigt Relativposition nicht ber¨ ucksichtigt

 8 6

♦ 

♦ ♦  ♦

4 2



0 0

200

400

 ♦ ♦  ♦  ♦  ♦  ♦  ♦  600

800 1000 Zeit (s)

1200

1400

1600

1800

Abbildung 8.2: Gesamtlagefehler bei Ber¨ ucksichtigung bzw. Vernachl¨ assigung der Relativposition von GPS-Antenne und IMU.

mit ∂rAn =0 ∂rUn ∂rAn = n = I ∂veb ∂rAn n ˆ b b ˆ nΩ = vrot ×] = −C b ib l = − [  ∂ψ

Hv1 =

(8.81)

Hv2

(8.82)

Hv3

Hv4 = Hv5 =

∂rAn =0 ∂ba

  ∂rAn ˆ n l b × . =C b ∂bω

(8.83) (8.84) (8.85)

n n verarbeitete Differenz zwischen dem erwarteten Messwert vˆeA,k Die als Messwert ∆veA,k n und dem vorliegenden Messwert v˜eA,k ist gegeben durch n n ˆ b  b v˜ n . ˆ n ∆veA,k = vˆeb +C b ωib × l −  eA,k

(8.86)

Ob die Relativposition zwischen GPS-Antenne und IMU im Navigationsfilter br¨ uckute sichtigt werden muss, h¨ angt im Wesentlichen von der L¨ange des Vektors l b , der G¨

8.2 Entwurf eines Navigationsfilters

211

Trajektoriendynamik bis 2.5 g und 15 ◦ /s

Gesamtlagefehler (mrad)

7

5

♦

Relativposition ber¨ ucksichtigt Relativposition nicht ber¨ ucksichtigt

6 ♦



4 3



2





1

♦

♦

0 0

200

400

♦

 600

♦



 ♦

♦

800 1000 Zeit (s)

 ♦ 1200



♦ 

1400

♦ 1600

 1800

Abbildung 8.3: Gesamtlagefehler bei Ber¨ ucksichtigung bzw. Vernachl¨ assigung der Relativposition von GPS-Antenne und IMU.

der eingesetzten Sensoren und der Trajektoriendynamik ab. Dies wird in Abb. 8.2 und 8.3 illustriert, die den rms-Wert des Gesamtlagefehlers bei zehn Simulationsl¨aufen eines integrierten GPS/INS-Navigationssystems zeigt. Der Gesamtlagefehler wurde hierbei ermittelt, indem in jedem Zeitschritt eine Drehmatrix (I + Ψerr ) berechnet wurde, die die Drehung von der gesch¨ atzten in die – da es sich um eine Simulation handelt, bekannte – tats¨ achliche Lage vermittelt. Der gezeigte Gesamtlagefehler ist durch den rms-Wert der drei in der Drehmatrix (I + Ψerr ) enthaltenen Winkel gegeben. Des Weiteren wurde ein Abstand zwischen GPS-Antenne und IMU von f¨ unf Metern angenommen, es wurden Tactical-Grade-Inertialsensoren simuliert. Bei den in Abb. 8.2 gezeigten Simulationsergebnissen lag eine geringe Trajektoriendynamik vor, die Winkelgeschwindigkeit von Drehbewegungen blieb auf wenige Grad pro Sekunde beschr¨ankt. Man erkennt, dass sich die Vernachl¨ assigung der Relativposition von GPS-Antenne und IMU nicht negativ auf die Lagegenauigkeit auswirkt. Bei den in Abb. 8.3 gezeigten Simulationsergebnissen erkennt man jedoch massive Unterschiede, hier wurden Drehbewegungen bis f¨ unfzehn Grad pro Sekunde und Beschleunigungen bis zur zweieinhalbfachen Schwerebeschleunigung angenommen. Ohne Ber¨ ucksichtigung der Relativposition verschlechtert sich die Lagegenauigkeit deutlich: Zum einen wird der Anfangslagefehler durch die ersten Man¨over langsamer abgebaut, zum anderen wird auch mit fortschreitender Simulationsdauer die bei Ber¨ ucksichtigung der Relativposition resultierende Genauigkeit nicht erreicht. Es ist klar, dass sich durch die Vernachl¨ assigung der Relativposition auch entsprechende Positionsfehler ergeben, die Geschwindigkeit ist durch die w¨ahrend Drehbewegungen vorliegenden systematischen Fehler der Geschwindigkeitsmessung ebenfalls beeintr¨achtigt.

212

8 Anwendungsbeispiel GPS/INS-Integration

Abschließend f¨ allt auf, dass bei gr¨ oßerer Trajektoriendynamik die Lagefehler schneller abgebaut werden und eine insgesamt gr¨ oßere Lagegenauigkeit erzielt wird. Die Ursache daf¨ ur liegt in der Struktur des Systemmodells, dieser Aspekt wird in Abschnitt 10.1 n¨ aher betrachtet. Pseudorangemessung In einem Tightly Coupled System werden GPS-Pseudorange- und DeltarangeMessungen verarbeitet. Hierbei bietet sich eine sequentielle Verarbeitung dieser Messwerte an: Jede Messung wird als skalare Einzelmessung verarbeitet, d.h. liegen zu einem Zeitpunkt n Pseudorange-Messungen vor, werden nacheinander n Messschritte durchgef¨ uhrt. Die Alternative hierzu w¨ are, aus allen zu einem Zeitpunkt vorliegenden Messwerten einen Messwertvektor aufzubauen und diese Messwerte so in einem einzigen Messschritt zu verarbeiten. Dabei m¨ usste jedoch eine Matrix invertiert werden, deren Dimension durch die Anzahl der vorliegenden Messwerte bestimmt ist, was durchaus kritisch und aufw¨ andig sein kann. Außerdem m¨ usste bei der Implementierung einer solchen Parallelverarbeitung der Messwerte die variierende – da von der Anzahl der Messwerte abh¨ angige – Dimension der Messmatrix beachtet werden, was einen zus¨atzlichen Programmieraufwand bedeuten w¨ urde. Der Zusammenhang zwischen Pseudorange ρi und Position der GPS-Antenne rAn ist gegeben durch #  T  n − n − ρi = rAn rAn + cδt , rS,i rS,i

(8.87)

n wobei rS,i = (xS,i , yS,i , zS,i )T die in das Navigationskoordinatensystem umgerechnete Satellitenposition bezeichnet, rAn = (xA , yA , zA )T ist die Position der GPS-Antenne. Die Messmatrix einer Pseudorange-Messung ist durch die Jacobi-Matrix der rechten Seite von Gl. (8.87) gegeben. Bereits in Abschnitt 4.3.1 wurde die Ableitung

−rAn,T ∂ρi n,T  = = −eS,i ∂rAn (xS,i − xA )2 + (yS,i − yA )2 + (zS,i − zA )2

(8.88)

ermittelt, die gesuchte Jacobi-Matrix kann mit der Kettenregel bestimmt werden. Die gesuchte Messmatrix zur Verarbeitung einer Pseudorange-Messung ergibt sich so zu   Hρ,k = Hρ1 Hρ2 Hρ3 Hρ4 Hρ5 Hρ6 (8.89) mit Hρ1 =

∂ρi ∂rAn n,T n,T = −eS,i · I = −eS,i ∂rAn ∂rUn

Hρ2 = 0      ∂ρi ∂rAn ˆn ˆ n,T n,T  l × · − l n × = eS,i Hρ3 = = −eS,i n  ∂rA ∂ ψ

(8.90) (8.91) (8.92)

8.2 Entwurf eines Navigationsfilters Hρ4 = 0 Hρ5 = 0   ∂ρi Hρ6 = = 1 0 .  ∂ cT

213 (8.93) (8.94) (8.95)

Die vom Filter verarbeitete Differenz zwischen erwarteter und vorliegender Pseudorangemessung ist gegeben durch #  T  n − n − ˆ nl b ˆ nl b + cδ tˆ − ρ˜i . ∆ρi,k = rS,i rS,i rˆUn − C rˆUn − C (8.96) b b Deltarange-Messung Die Messmatrix zur Verarbeitung einer Deltarange-Messung wird auf dem gleichen Wege wie die Messmatrix der Pseudorange-Messung bestimmt. Der Zusammenhang zwischen Deltarange vrel,eS,i und der Geschwindigkeit der GPS-Antenne ist gegeben durch  n  n vrel,eS,i = e n,T v eS,i − v eA + cδ t˙ , S,i 

(8.97)

n die ins Navigationskoordinatensystem umgerechnete Satellitengeschwindigwobei v eS,i keit bezeichnet. Die Jacobi-Matrix der rechten Seite von Gl. (8.97) stellt die gesuchte Messmatrix dar, man erh¨ alt   (8.98) Hdr,k = Hdr1 Hdr2 Hdr3 Hdr4 Hdr5 Hdr6

mit Hdr1 = 0 ∂vrel,eS,i Hdr2 = n ∂v eA ∂vrel,eS,i Hdr3 = n ∂v eA

(8.99) n ∂v eA n ∂v eb n ∂v eA

 ∂ψ

n,T n,T = −eS,i · I = −eS,i

(8.100)

 n,T ˆ nΩ ˆ b  b = e n,T [v n ×] · −C = −eS,i b ib l rot S,i

(8.101)

Hdr4 = 0   n ∂vrel,eS,i ∂v eA n,T ˆ n  b = − e · C × Hdr5 = l b S,i n ∂v eA ∂bω   ∂vrel,eS,i Hdr6 = = 0 1 .  ∂ cT

(8.102) (8.103) (8.104)

Die vom Filter verarbeitete Differenz zwischen erwarteter und vorliegender DeltarangeMessung ist gegeben durch  n n ˆ b b ˆn − C ˆ Ω  v + cδ tˆ˙ − v˜rel,eS,i . ∆vdr = e n,T −  v (8.105) l eS,i eb b ib S,i

214

8.2.3

8 Anwendungsbeispiel GPS/INS-Integration

Korrektur der totalen Gr¨oßen

Nachdem die zu einem Zeitpunkt vorliegenden Messwerte verarbeitet wurden, enth¨alt der Zustandsvektor des Kalman-Filters die gesch¨atzten Fehler der Navigationsl¨osung, der angenommenen Inertialsensorbiase und, im Falle eines Tightly Coupled Systems, des Uhrenfehlers und der Uhrenfehlerdrift. Die totalen Gr¨oßen werden daher nach jedem Messschritt mit Hilfe dieser gesch¨ atzten Fehler korrigiert. Die Gleichungen f¨ ur die Korrektur der Position sind gegeben durch ϕˆ+ = ϕˆ− −

∆ˆ x+ n ˆ− Rn − h

(8.106)

ˆ+ = λ ˆ− − λ

∆ˆ x+ e ˆ − ) cos ϕˆ− (Re − h

(8.107)

ˆ − − ∆ˆ ˆ+ = h x+ h d ,

(8.108)

die Korrektur der Geschwindigkeit erfolgt gem¨aß n,+ n,− n,+ = vˆeb − ∆vˆeb . vˆeb

(8.109)

Zur Korrektur der Lage wird aus den gesch¨atzten Lagefehlern ein Korrekturquaternion qc berechnet und mit dem a priori Lagequaternion multipliziert. Hierbei bietet es sich ebenfalls an, die Reihenentwicklungen der trigonometrischen Funktionen zu verwenden, ˆ = 0 auftreten, um eine Division durch null ausschließen zu k¨onnen. Diese w¨ urde bei ∆ψ falls die Funktion si(x) einfach als si(x) = sin(x)/x implementiert w¨ urde. Die Korrektur des Lagequaternions ist somit durch die Gleichungen ˆ (8.110) σc = = −∆ψ ) σc = σcT σc (8.111)



1 2 1 1 4 6 cos σ2c 46080 σc + . . .  11 − 81σc 2+ 3841σc +  (8.112) qc = σc ≈ σc 1 4 σc 2 − 48 σc + 3840 σc − 645120 σc6 + . . . σc sin 2 ˆ n,+ ˆ n,− q = qc • q b b

(8.113)

gegeben. Die Korrektur der Inertialsensorbiase, des Uhrenfehlers und der Uhrenfehlerdrift erfolgt gem¨ aß ˆb+ = ˆb− − ∆ˆb+ a a a bω = ˆb− − ∆ˆb+ ω ω cδ tˆ+ = cδ tˆ− − ∆cδ tˆ+ cδ tˆ˙+ = cδ tˆ˙− − ∆cδ tˆ˙+ .

(8.114) (8.115) (8.116) (8.117)

8.2 Entwurf eines Navigationsfilters

215

Nachdem die totalen Gr¨ oßen mit Hilfe der gesch¨atzten Fehler korrigiert wurden, m¨ ussen die gesch¨ atzten Fehler, d.h. der Zustandsvektor des Kalman-Filters, zu null gesetzt werden: ∆x = 0

(8.118)

W¨ urde dieses R¨ ucksetzen des Zustandsvektors vers¨aumt, w¨ urden die gesch¨atzten Fehler nach dem n¨ achsten Zeitschritt erneut zur Korrektur der totalen Gr¨oßen herangezogen, was nat¨ urlich algorithmisch nicht korrekt w¨are.

8.2.4

Vergleich von Loosely Coupled und Tightly Coupled Systemen

Bereits in den neunziger Jahren wurden die ersten Vergleiche von Loosely Coupled und Tightly Coupled GPS/INS-Systemen ver¨offentlicht, siehe [83],[82], die Vorteile von Tightly Coupled Systemen aufzeigten. Dennoch wird auch heute noch h¨aufig eine Verarbeitung von GPS-Positions- und Geschwindigkeitsmessungen vorgezogen, meist um den f¨ ur die Verarbeitung von Pseudoranges und Deltaranges n¨otigen Entwicklungsaufwand zu umgehen. Im Folgenden soll die Leistungsf¨ ahigkeit der beiden Integrationsans¨atze anhand von numerischen Simulationen verglichen werden. Um eine realistische Konstellation zu garantieren, wurden die Satellitenbahnen anhand von realen Almanach-Daten simuliert. Es wurde eine Vibrationsumgebung angenommen, wie sie z.B. bei manchen Flugk¨orpern anzutreffen ist. Diese wirkte sich nat¨ urlich negativ auf die Inertialnavigation aus, die basierend auf den Parametern einer typischen Tactical Grade IMU simuliert wurde. Zun¨ achst wurde ein Szenario betrachtet, bei dem von 600 Sekunden bis 700 Sekunden nach Simulationsbeginn drei Satelliten und von 850 Sekunden bis 950 Sekunden zwei Satelliten sichtbar waren. Von 1050 Sekunden bis 1150 Sekunden lagen nur von einem Satelliten Pseudorange- und Deltarange-Messungen vor. Zu allen anderen Zeitpunkten waren vier oder mehr Satelliten verf¨ ugbar. Abb. 8.4 zeigt die rms-Werte der w¨ ahrend f¨ unfzig Simulationsl¨ aufen aufgetretenen Positionsfehler. Bei vier oder mehr sichtbaren Satelliten liefern Loosely Coupled und Tightly Coupled Systeme praktisch identische Ergebnisse5. Bei weniger als vier sichtbaren Satelliten wachsen die Navigationsfehler des Loosely Coupled Systems entsprechend der Inertialnavigationsperformance an, das Tightly Coupled System profitiert von der St¨ utzung durch die Pseudorange- und Deltarange-Messungen zu den verbliebenen Satelliten. Bei nur einem sichtbaren Satellit ist hier kein Vorteil mehr zu erkennen, diese Feststellung l¨asst sich jedoch nicht verallgemeinern. Die Performance des Tightly Coupled Systems h¨angt bei weniger als vier sichtbaren Satelliten unter anderem von der G¨ ute der Empf¨angeruhr und von der Satellitenkonstellation ab. W¨ ahrend bei den in Abb. 8.4 gezeigten Simulationsergebnissen bei unvollst¨ andigen Konstellationen keine Satellitenwechsel vorlagen, wurde bei den in Abb. 8.5 gezeigten Simulationsergebnissen angenommen, dass in der Zeit von 600 Sekunden bis 1000 Sekunden zwar zu jedem Zeitpunkt nur ein Satellit sichtbar ist, 5 Das setzt voraus, dass ungefilterte Positions- und Geschwindigkeitsmessungen sowie Informationen bez¨ uglich der G¨ ute der Messerte vom GPS-Empf¨ anger zur Verf¨ ugung gestellt werden.

216

8 Anwendungsbeispiel GPS/INS-Integration 90

b r

80

Tightly Coupled Loosely Coupled

r

Positionsfehler (m)

70 r

60 50

r

40

r

30 20

rbrbrbrbrbrbr rbrbrbrbrbrbrbrbrb

10

r

b

b

b

rb brbrbrbrb

brbrbr

800

1000

b

brbrb

0 0

200

400

600 Zeit (s)

1200

Abbildung 8.4: Mittlere Positionsfehler bei 50 Simulationsl¨ aufen.

1200

Positionsfehler (m)

1000

b r

Tightly Coupled Loosely Coupled

r

800 r

600 400

r r

200 0

r b r b r b r b r b r b r b r b r b 0 200 400 600 Zeit (s)

b 800

b

b

b r b r b r 1000 1200

Abbildung 8.5: Mittlere Positionsfehler bei 50 Simulationsl¨ aufen. W¨ ahrend unvollst¨ andiger Konstellationen zwischen 600 und 1000 Sekunden wurde zu jedem Zeitpunkt nur ein Satellitensignal empfangen, der sichtbare Satellit wechselte jedoch mehrmals.

8.2 Entwurf eines Navigationsfilters

217

dass der sichtbare Satellit jedoch mehrmals wechselt. Solche oder ¨ahnliche Situationen sind in Stadtgebieten nicht ungew¨ ohnlich, hier schr¨anken Geb¨aude die Sicht auf die GPS-Satelliten massiv ein. Die Richtung der befahrenen Straße beeinflusst so, welche ¨ Satelliten sichtbar sind. Andert sich z.B. aufgrund eines Abbiegevorganges die Himmelsrichtung der befahrenen Straße, ¨ andern sich h¨aufig auch die sichtbaren Satelliten: Bisher sichtbare Satelliten werden durch Geb¨aude verdeckt, bisher verdeckte Satelliten werden sichtbar. Man erkennt in Abb. 8.5, dass das Tightly Coupled System in einem solchen Szenario noch eine brauchbare Positionsgenauigkeit aufrechterhalten kann, w¨ahrend die Navigationsfehler des Loosely Coupled Systems aufgrund der fehlenden St¨ utzung unbeschr¨ankt anwachsen. Ein ¨ ahnliches Verhalten wie bei den in Abb. 8.4 und Abb. 8.5 gezeigten Positionsfehlern ist auch f¨ ur die Geschwindigkeits- und Lagefehler zu beobachten. Die in den numerischen Simulationen beobachteten Eigenschaften von Loosely Coupled und Tightly Coupled Systemen konnten auch experimentell best¨atigt werden, die

GPS Positionsmessungen

Start

Ende

c Abbildung 8.6: GPS Positionsmessungen w¨ ahrend der Testfahrt. Kartengrundlage:
Stadt Karlsruhe, Vermessung, Liegenschaften, Wohnen.

218

8 Anwendungsbeispiel GPS/INS-Integration

Ergebnisse einer Testfahrt sind in Abb. 8.6 bis Abb. 8.8 dargestellt. Hierbei wurden w¨ ahrend der Testfahrt die Daten aller Navigationssensoren aufgezeichnet und anschließend offline mit den verschiedenen Navigationsalgorithmen verarbeitet. Dadurch wurde sichergestellt, dass beide Navigationssystemarchitekturen identische Sensordaten erhalten, was die Vergleichbarkeit der Ergebnisse garantiert. Abb. 8.6 zeigt die vom GPS-Empf¨ anger gelieferten Positionsmessungen. Man erkennt im rechten Teil des Bildes eine große L¨ ucke in den GPS-Positionsmessungen, hier wurden so viele Satelliten durch die umstehenden Geb¨aude abgeschattet, dass der GPS-Empf¨anger keine Positions- und Geschwindigkeitsmessungen mehr liefern konnte. Dies wird durch Abb. 8.7 best¨ atigt, die die Anzahl der verf¨ ugbaren Satelliten zeigt: Im Bereich zwischen achtzig und hundertdreisig Sekunden sank die Anzahl der sichtbaren Satelliten teilweise dauerhaft auf unter vier. Abb. 8.8 zeigt die Positionsl¨ osungen eines Loosely Coupled und eines Tightly Coupled Systems, die durch Fusion der GPS-Daten mit den Daten einer Low-cost MEMS IMU erzielt wurden. Gr¨ oßtenteils liefern beide Systeme praktisch identische Ergebnisse. In dem angesprochenen Teilst¨ uck mit weniger als vier sichtbaren Satelliten wachsen die Positionsfehler des Loosely Coupled Systems jedoch – nicht zuletzt aufgrund der geringen G¨ ute der MEMS IMU – schnell an, w¨ahrend die St¨ utzung mit den Pseudorangeund Deltarange-Messungen zu den verbleibenden Satelliten bei dem Tightly Coupled System eine gute Positionsgenauigkeit aufrecht erh¨alt.

10 Anzahl sichtbarer Satelliten 8

6

4

2

0 0

50

100

150

200

250 Zeit (s)

300

350

Abbildung 8.7: Anzahl der sichtbaren Satelliten w¨ ahrend der Testfahrt.

400

450

8.3 Nutzung von Tr¨ agerphasenmessungen 49.016

Breitengrad (◦ )

49.015

b r

Tightly Coupled Loosely Coupled

49.012 49.011

br b br b r b r r b rbb r r r bb r

r

b

br

b

8.414

8.416 8.418 L¨ angengrad (◦ )

b

r

br r r b r b r b r bb r b r

8.412

r b r

rb

49.01 49.009 8.41

rbbr br rb

49.014 49.013

219

b

r

r

b

8.42

b

8.422

b r rb

8.424

Abbildung 8.8: Positionsl¨ osung eines Loosely Coupled und eines Tightly Coupled Systems basierend auf den w¨ ahrend einer Testfahrt aufgezeichneten Navigationssensordaten.

8.3

Nutzung von Tr¨agerphasenmessungen

Bei Anwendungen, bei denen eine hochgenaue Positionsbestimmung im Vordergrund steht, werden GPS-Tr¨ agerphasenmessungen genutzt. Die Messgleichung einer Tr¨agerphasenmessung kann wie folgt formuliert werden: (ϕ˜k + N )λc = |rS,k − rA,k | + cδtk + ecm,k + emp,k + nk ,

(8.119)

siehe auch Abschnitt 4.3.2. Hierbei bezeichnet ecm,k die Common-Mode-Fehler, emp,k sind durch Mehrwegeausbreitung verursachte Fehler und nk ist ein weiterer Rauschterm, der als weiß angenommen werden kann. W¨ ahrend die Common-Mode-Fehler einer Tr¨agerphasenmessung in der gleichen Gr¨ oßenordnung wie bei einer Pseudorange-Messung liegen, k¨ onnen die durch Mehrwegeausbreitung verursachten Fehler ein Viertel der Wellenl¨ ange, beim C/A-Code also ungef¨ ahr f¨ unf Zentimeter, nicht u ¨ bersteigen, siehe [53]. Bei einer Pseudorange-Messung hingegen kann der durch Mehrwegeausbreitung verursachte Fehler leicht mehrere Meter betragen. Das Messrauschen nk schließlich bewegt sich bei einer Tr¨ agerphasenmessung im Millimeterbereich, bei einer PseudorangeMessung ist selbst bei hochwertigen Empf¨ angern mit einigen Dezimetern zu rechnen. Werden nun die Common-Mode-Fehler mit Hilfe einer DGPS-Basestation eliminiert, verbleibt je nach Mehrwegeausbreitung bei einer Pseudorange-Messung ein Fehlerbudget von einigen Dezimetern bis wenigen Metern, bei einer Tr¨agerphasenmessung liegt der Restfehler in der Gr¨ oßenordnung einiger Millimeter bis weniger Zentimeter.

220

8 Anwendungsbeispiel GPS/INS-Integration

Auch wenn die Common-Mode-Fehler mit Hilfe einer DGPS-Base-Station eliminiert werden, verhindert der unbekannte Tr¨ agerphasenmehrdeutigkeitswert N , dass von der gr¨ oßeren Pr¨ azision der Tr¨ agerphasenmessung bei der Positionsbestimmung direkt profitiert werden kann. Die u atze zur Nutzung von Tr¨agerphasenmessungen ¨ blichen Ans¨ versuchen daher, diesen unbekannten Tr¨agerphasenmehrdeutigkeitswert zu sch¨atzen. Der einfachste Ansatz hierzu ist als Carrier Aided Smoothing bekannt.

8.3.1

Carrier Aided Smoothing

Subtrahiert man Gl. (8.119) von der Messgleichung einer Pseudorange-Messung, ρ˜k = |rS,k − rA,k | + cδtk + eρk ,

(8.120)

so ergibt sich ρ˜k − λc ϕ˜k = N λc + eρ,k − eϕ,k .

(8.121)

Anhand der Differenz ρ˜k − λc ϕ˜k kann folglich der Tr¨agerphasenmehrdeutigkeitswert gesch¨ atzt werden. Um den Einfluss des Rauschens von Pseudorange- und Tr¨agerphasenmessung zu minimieren, werden alle bis zum aktuellen Zeitpunkt vorliegenden Messungen zur Bestimmung des Mehrdeutigkeitswertes herangezogen: k  ˆk λc = 1 N ρ˜l − λc ϕ˜l k

(8.122)

l=1

Addiert man den gesch¨ atzten Mehrdeutigkeitswert zur aktuellen Tr¨agerphasenmessung, so ergibt sich eine Messgr¨ oße ˆk λc = |rS,k − rA,k | + cδtk + emp,k + nk + eN , (8.123) ρ˜smooth,k = λc ϕ˜k + N

λϕ˜

− ρ˜

+

Abbildung 8.9: Carrier Aided Smoothing.

$

ˆλ N

ρ˜smooth +

Navigationsfilter

die analog einer Pseudorange-Messung im Navigationsfilter verarbeitet werden kann.

8.3 Nutzung von Tr¨ agerphasenmessungen

221

Nachteilig bei dieser Vorgehensweise ist, dass die Sch¨atzung der Tr¨agerphasenmehrdeutigkeitswerte unabh¨ angig voneinander erfolgt. Die Sch¨atzung des Mehrdeutigkeitswertes eines Satelliten profitiert nicht von anderen, bereits gesch¨atzten Mehrdeutigkeitswerten und der daraus resultierenden Positionsgenauigkeit. Das Grundprinzip des Carrier Aided Smoothing ist in Abb. 8.9 nochmals schematisch dargestellt, detailliertere Betrachtungen sind in [51] und [17] zu finden.

8.3.2

Festlegung der Tr¨agerphasenmehrdeutigkeitswerte

Die angesprochenen Nachteile des Carrier Aided Smoothing k¨onnen umgangen werden, wenn die unbekannten Tr¨ agerphasenmehrdeutigkeitswerte in das Systemmodell des Navigationsfilters aufgenommen werden. Es ergibt sich ein erweitertes Systemmodell der Form ⎛

⎞• ⎛ ∆x F 0 0 ⎜∆N1 ⎟ ⎜0 0 0 ⎜∆N ⎟ = ⎜ 0 0 0 2⎠ ⎝ ⎝ .. .. .. .. . . . .

⎞ ⎛ ⎞ ⎞⎛ ... ∆x G . . .⎟ ⎜∆N1 ⎟ ⎜ 0 ⎟ ⎟ ⎜ ⎟, ⎜ . . .⎟ ⎠ ⎝∆N2 ⎠ + ⎝ 0 ⎠ w .. .. .. . . .

(8.124)

das neben der Verarbeitung von Pseudorange-Messungen die direkte Verarbeitung von Tr¨ agerphasenmessungen erlaubt. Der Vorteil dieser Vorgehensweise besteht darin, dass z.B. bei Konstellations¨ anderungen die Sch¨ atzung des Mehrdeutigkeitswertes eines neu hinzu gekommenen Satelliten von den bereits gesch¨atzten Mehrdeutigkeitswerten anderer Satelliten profitiert. Nachteilig ist der mit der Erweiterung des Zustandsvektors verbundene, gr¨ oßere Rechenaufwand. In der Regel wird man den Zustandsvektor nur um die Mehrdeutigkeitswerte der gerade sichtbaren Satelliten erweitern, das verursacht jedoch einen gr¨ oßeren Aufwand bei der Implementierung, da die Dimension des Systemmodells variiert, Zust¨ ande hinzugef¨ ugt und entfernt werden m¨ ussen. Die Tr¨ agerphasenmehrdeutigkeitswerte sind ganzzahlige Werte, das Kalman-Filter sch¨ atzt jedoch Gleitkommazahlen, man spricht hier auch von Float Ambiguities. Die gr¨ oßtm¨ ogliche Positionsgenauigkeit kann nur erreicht werden, wenn die gesch¨atzten Mehrdeutigkeitswerte auf ganze Zahlen festgelegt werden k¨onnen. Dies ist nur m¨oglich, wenn mit so genannten Doppeldifferenzen gearbeitet wird. Das bedeutet, das neben den Differenzen der Messungen von DGPS Base Station und dem betreffenden GPSEmpf¨ anger noch zus¨ atzlich Differenzen zwischen den zu einem Zeitpunkt vorliegenden Messungen gebildet werden. Da alle diese Messungen mit dem gleichen Empf¨angeruhrenfehler behaftet sind, wird auf diese Weise der Empf¨angeruhrenfehler aus den Gleichungen eliminiert. Dadurch entf¨ allt auch die Notwendigkeit, das Systemmodell um den Empf¨ angeruhrenfehler zu erweitern; andererseits resultieren Kreuzkorrelationen, die bei der Verarbeitung der Doppeldifferenzen im Navigationsfilter beachtet werden m¨ ussen. Die eigentliche Festlegung der vom Filter gesch¨atzten Mehrdeutigkeitswerte auf ganzzahlige Werte kann z.B. mit der LAMBDA-Methode erfolgen, die diese Gleitkommasch¨ atzungen sowie die Kovarianzmatrix des Sch¨atzfehlers des Kalman-Filters als Eingangsgr¨ oßen erh¨ alt. Details zur LAMBDA-Methode sind in [58] zu finden; Systeme die auf der beschriebenen Vorgehensweise beruhen, werden in [27] und [108] vorgestellt.

222

8 Anwendungsbeispiel GPS/INS-Integration

Relevanz der DGPS-Korrekturdaten Selbst wenn nicht versucht wird, die Tr¨ agerphasenmehrdeutigkeitswerte auf ganze Zahlen festzulegen, kann auf die Verwendung von Korrekturdaten einer DGPS Base Station bei den angesprochenen Verfahren nicht verzichtet werden. Dies soll im Folgenden anhand der Ergebnisse eines Hardware-in-the-Loop-(HIL-)Tests illustriert werden. Hierzu wurde das in Abb. 8.10 schematisch dargestellte HIL-Testsystem verwendet. Dieses System erlaubt es, beliebige Fahrzeugtrajektorien vorzugeben, die einem GPS Space Segment Simulator (GSSS) zur Verf¨ ugung gestellt werden. Dieser Space Segment Simulator erzeugt die HF-Signale, die ein GPS-Empf¨anger in diesem Szenario von der GPS-Antenne erhalten w¨ urde. Die Messungen des GPS-Empf¨angers wiederum werden an einen Navigationscomputer weitergegeben. Gleichzeitig werden anhand der Trajektorieninformationen unter Verwendung entsprechender Inertialsensorfehlermodelle realistische IMU-Daten erzeugt, die ebenfalls dem Navigationscomputer zur Verf¨ ugung gestellt werden. Dieser errechnet in Echtzeit eine Navigationsl¨osung, die mit der vorgegebenen Trajektorie verglichen wird. Dadurch k¨onnen die Fehler der Navigationsl¨ osung explizit berechnet werden, was bei einer Testfahrt oder einem Testflug

GSSS

SIMULAB Referenzdaten

Trajektorien-

GPS-

Vorgabe

Ref.

NavigationsINES Filter

Logging

L¨ osung

GPS-HW.

GPS-Rohdaten

HF-Signal

IMU-Daten

a, ω  IMU-Simulation

Abbildung 8.10: Vereinfachtes Blockdiagramm des Hardware-in-the-Loop-Testsystems.

8.3 Nutzung von Tr¨ agerphasenmessungen

223

aufgrund der fehlenden Referenz nicht m¨ oglich ist. Ein weiterer Vorteil eines HIL-Tests ¨ ist dessen Wiederholbarkeit, so k¨ onnen z.B. die Auswirkungen algorithmischer Anderungen im Navigationssystem unter definierten Bedingungen analysiert werden. Daru ¨ ber hinaus erlaubt die synthetische Generierung der Inertialsensordaten, die in der Applikation erwartete Vibrationsumgebung nachzubilden. Die Verwendung eines realen GPS-Empf¨ angers ist jedoch von entscheidender Bedeutung, da sich das Verhalten verschiedener GPS-Empf¨ anger unter Umst¨ anden deutlich unterscheidet. Beispiele hierf¨ ur sind die Re-Akquisition nach Abschattungen von Satelliten oder das Tracking bei gr¨ oßerer Trajektoriendynamik. Diese Eigenschaften des Empf¨angers werden von den Empf¨ angerherstellern in der Regel nicht beschrieben. Mit diesem HIL-Testsystem wurde ein Tightly Coupled GPS/INS-System, das Pseudorange- und Deltarange-Messungen verarbeitet, mit einem System, das Pseudorangeund Tr¨ agerphasenmessungen verarbeitet, verglichen. Zur Verarbeitung der Tr¨agerphasenmessungen wurde der Zustandsvektor des Navigationsfilters wie im vorigen Abschnitt beschrieben um die Tr¨ agerphasenmehrdeutigkeitswerte erweitert. Abb. 8.11 zeigt exemplarisch die Geschwindigkeitsgenauigkeit, die mit diesen beiden Verfahren bei vorhandenen DGPS-Korrekturdaten erzielt werden kann. Die DGPS-Korrekturdaten wurden dabei simuliert, indem die Fehlermodelle des Space Segement Simulators f¨ ur Ionosph¨are und Ephemeriden abgeschaltet wurden. Offensichtlich f¨ uhrt die Nutzung der Tr¨agerphase zu einer deutlichen Steigerung der – bei dem eingesetzten, hochwertigen Empf¨ anger ohnehin schon beeindruckenden – Geschwindigkeitsgenauigkeit. Die hier nicht gezeigte Positionsl¨ osung ist ebenfalls deutlich rausch¨armer. Fehlen jedoch die DGPS-

Geschwindigkeitsfehler (m/s)

0.06 Deltarange Verarbeitung Nutzung der Tr¨agerphase

0.05

e ♦

0.04 e

0.03 0.02

e e

e

e

e e

e

e

e

e

e

e e

e e

e e

e

e e e ♦ ♦ ♦ ♦ ♦ ♦ ♦ ♦ ♦ ♦ ♦ ♦ ♦ ♦ ♦ ♦ ♦ ♦ ♦ ♦ ♦ ♦ ♦ ♦ 0 200 400 600 800 1000 1200 Zeit (s)

0.01

e

e

Abbildung 8.11: Geschwindigkeitsfehler bei Deltarange-Verarbeitung und bei Sch¨ atzung der Tr¨ agerphasenmehrdeutigkeitswerte, mit Simulation von DGPS-Korrekturdaten.

224

8 Anwendungsbeispiel GPS/INS-Integration

0.16 0.14 Geschwindigkeitsfehler (m/s)

e

Deltarange Verarbeitung Nutzung der Tr¨agerphase

♦

0.12 0.1

♦

0.08 ♦ 0.06 0.04 0.02 0

♦ ♦

♦

♦ ♦

♦ ♦ e

e e e e ♦ e♦ e♦ ♦ ♦ e e e e e e e ♦ 200

400

600 Zeit (s)

e♦ e ♦e ♦ e ♦ 800

♦ e

♦ e♦ e

1000

♦

♦ e e

e

1200

Abbildung 8.12: Geschwindigkeitsfehler bei Deltarange-Verarbeitung und bei Sch¨ atzung der Tr¨ agerphasenmehrdeutigkeitswerte, ohne DGPS-Korrekturdaten.

Korrekturdaten, erh¨ alt man die in Abb. 8.12 dargestellten Ergebnisse. Das angesprochene Verfahren zur Nutzung der Tr¨ agerphase liefert hier schlechtere Ergebnisse als die St¨ utzung mit Deltarange-Messungen. Im Folgenden soll daher ein Verfahren vorgestellt werden, mit dem auch ohne Korrekturdaten einer DGPS Base Station die Tr¨ agerphasenmessungen nutzbringend eingesetzt werden k¨ onnen.

8.3.3

Zeitlich differenzierte Tr¨agerphasenmessungen

Bei den mit Hilfe von DGPS-Korrekturdaten eliminierbaren Common-Mode-Fehlern handelt es sich im Wesentlichen um atmosph¨arische Fehler und Ephemeridenfehler. Diese Fehler k¨ onnen daher als langsam ver¨anderlich angesehen werden, d.h. man kann davon ausgehen, dass bei zwei aufeinanderfolgenden Tr¨agerphasenmessungen diese Fehlerterme nahezu identisch sind. Daher k¨onnen durch die Bildung zeitlicher Differenzen der Tr¨ agerphasenmessungen diese Fehler fast vollst¨andig beseitigt werden. Eine einzelne Tr¨ agerphasenmessung stellt eine – mit einem aufgrund der Tr¨agerphasenmehrdeutigkeit unbekannten Offset behaftete – Messung des Abstandes zwischen GPS-Antenne und Satellit dar. Die zeitliche Differenz zweier aufeinanderfolgender Tr¨agerphasenmessungen ist dagegen ein Maß f¨ ur die Abstands¨anderung zwischen GPSAntenne und Satellit in dem betrachteten Zeitintervall, typischerweise eine Sekunde. Diese Differenz kann daher als Geschwindigkeitsmessung interpretiert werden. Neben der Eliminierung der Common-Mode-Fehler ist ein weiterer Vorteil der Differenzen-

8.3 Nutzung von Tr¨ agerphasenmessungen

225

bildung, dass die konstanten Tr¨ agerphasenmehrdeutigkeitswerte aus den Gleichungen verschwinden. Zeitliche Differenzen von Tr¨ agerphasenmessungen werden in Form von Dreifachdifferenzen bei einer Reihe von Anwendungen eingesetzt. Bei Dreifachdifferenzen ist die erste Differenzenbildung die Korrektur der Tr¨agerphasenmessungen mit DGPSKorrekturdaten, die zweite Differenz wird zwischen Tr¨agerphasenmessungen zu verschiedenen Satelliten zur Elimination des Empf¨angeruhrenfehlers gebildet und die dritte Differenzenbildung ist die zwischen den so gewonnenen, aufeinanderfolgenden Doppeldifferenzen. Dreifachdifferenzen werden h¨ aufig zur Detektion von Cycle Slips eingesetzt, aber auch die Lagebestimmung mit Mehrantennenempf¨angern ist m¨oglich [95]. In [45] und [122] werden bei mindestens sieben sichtbaren Satelliten verschiedene Verfahren zur Positionsbestimmung anhand dieser Messwerte vorgeschlagen. Hierbei m¨ ussen vor einer ¨ ersten Positionsbestimmung Anderungen der Satellitengeometrie abgewartet werden. In [79] wird vorgeschlagen, diese Phase durch die Integration eines LEO6 -Satelliten zu verk¨ urzen. Bei den meisten der angesprochenen Anwendungen steht die Sch¨atzung des als Baseline bezeichneten Vektors zwischen DGPS-Basestation und GPS-Empf¨anger im Vordergrund. In [28] und [121] wird jedoch ein Verfahren vorgeschlagen, bei dem mit DGPSKorrekturdaten korrigierte Tr¨ agerphasenmessungen haupts¨achlich zur Steigerung der Geschwindigkeitsgenauigkeit des Navigationssystems eingesetzt werden. Im Folgenden soll aufgezeigt werden, dass sich dieser Ansatz auch bei fehlenden DGPS-Korrekturen einsetzen l¨ asst. Bestimmung der Messgleichung Die Differenz zweier aufeinanderfolgender Tr¨agerphasenmessungen zum selben Satelliten ergibt sich mit Gl. (8.119) zu (ϕ˜k − ϕ˜k−1 )λc = |rSA,k | − |rSA,k−1 | + cδtk − cδtk−1 + n
k .

(8.125)

Hierbei bezeichnen tk−1 und tk die G¨ ultigkeitszeitpunkte der beiden Messungen, deren zeitlicher Abstand ∆t = tk − tk−1 o.B.d.A. zu einer Sekunde angenommen werden kann. Der Term n
k bezeichnet den nicht durch die Differenzenbildung eliminierten Restfehler. F¨ ur die Differenz der Abst¨ ande |rSA,k | und |rSA,k−1 | zwischen GPS-Antenne und Satellit erh¨ alt man mit Abb. 8.13 T rSA,k−1 |rSA,k | − |rSA,k−1 | = ekT rSA,k − ek−1

≈ ekT (rSA,k − rSA,k−1 ) = ekT (δrS,k − δrA,k ) .

(8.126)

¨ der Satellitenposition bzw. der Bei δrS,k und rA,k handelt es sich um die Anderung Antennenposition im betrachteten Zeitintervall, durch Einsetzen in Gl. (8.125) erh¨alt man (ϕ˜k − ϕ˜k−1 )λc − ekT δrS,k = −ekT δrA,k + cδtk − cδtk−1 + n
k . 6 Low

Earth Orbit.

(8.127)

226

8 Anwendungsbeispiel GPS/INS-Integration

δrS,k

rSA,k

rSA,k−1

ek−1

ek δrA,k

Abbildung 8.13: Geometrie zeitlicher Differenzen von Tr¨ agerphasenmessungen.

¨ Die Anderung der Antennenposition setzt sich aus zwei Komponenten zusammen, der ¨ ¨ des Anderung der Position des Navigationskoordinatensystems δrN,k und der Anderung Vektors vom Ursprung des Navigationskoordinatensystems zur Antenne. Formuliert in Koordinaten des Navigationskoordinatensystems ergibt sich   n n δrA,k = δrN,k + Cnb,k − Cnb,k−1 l b (8.128) n Der Vektor δrN,k kann als Integral der Geschwindigkeit formuliert werden, ebenso kann die Differenz der Uhrenfehler zu den Zeitpunkten tk und tk−1 als Integral der Uhrenfehlerdrift dargestellt werden. Insgesamt erh¨alt man so

   n n (ϕ˜k − ϕ˜k−1 )λc − ekT δrS,k = −ekT δrN,k + Cnb,k − Cnb,k−1 l b +cδtk − cδtk−1 + n
k
tk n −ekT veb + cδ t˙ dt

= tk−1

  −ekT Cnb,k − Cnb,k−1 l b + n
k .(8.129)

Nun soll der Term, der aus der Differenz der Richtungskosinusmatrizen zu den Zeitpunkten tk−1 und tk und der Relativposition von GPS-Antenne und Ursprung des

8.3 Nutzung von Tr¨ agerphasenmessungen

227

k¨ orperfesten Koordinatensystems besteht, umgeschrieben werden. Mit Gl. (8.13) erh¨alt man  ˆ n − (I + Ψk−1 )C ˆn b (Cnb,k − Cnb,k−1 )l b = (I + Ψk )C b,k b,k−1 l ˆ ˆn = (I + Ψk )lkn − (I + Ψk−1 )lk−1 .

(8.130)

Erweitern von Gl. (8.130) f¨ uhrt auf  n  ˆ ˆn Cb,k − Cnb,k−1 l b = (I + Ψk ) lkn − (I + Ψk−1 ) lk−1 ˆn ˆn − (I + Ψk ) lk−1 + (I + Ψk ) lk−1  ˆn ˆn ˆ + (Ψk − Ψk−1 ) lk−1 = (I + Ψk ) lkn − lk−1 . (8.131) Interpretiert man die Differenz der kreuzproduktbildenden Matrix der Lagefehler als Differenzenquotient, kann diese durch die zeitliche Ableitung ersetzt werden:   n  ˆn ˆ n ˙ kˆlk−1 Cb,k − Cnb,k−1 l b = (I + Ψk ) lkn − lk−1 + ∆t · Ψ .

(8.132)

Bei diesen Betrachtungen k¨ onnen die Erddrehrate und die Transportrate vernachl¨assigt werden, so dass man als Lagefehlerdifferentialgleichung b ˙ ≈ −C ˆ n ∆ ψ b ωib

(8.133)

erh¨ alt; Einsetzen f¨ uhrt auf    n  ˆn ˆ k × ˆl n − ˆl n ˙ ˆn Cb,k − Cnb,k−1 l b = lkn − lk−1 +ψ k k−1 + ∆t · ψk × lk−1 ˆ k × δˆl n − ∆t · C ˆ n ∆ω b × ˆl n = δlkn + ψ k b ib k−1   ˆn ˆn  ˆn n    ˆ = δ lk − δ lk × ψk + ∆t lk−1 × Cb ∆ωibb .

(8.134)

Die Messgleichung der Differenz aufeinanderfolgender Tr¨agerphasenmessungen ist daher gegeben durch (ϕ˜k − ϕ˜k−1 ) λc −

ekT



n δrS,k


tk ˆn n  − δ lk = −ekT veb + cδ t˙ dt tk−1

   ˆ n b
k + ∆t ˆl n × C ˆ −ekT −δlkn × ψ ∆ ω k−1 b ib + nk . (8.135) Der in dieser Messgleichung auftretende Integralterm enth¨alt jedoch Gr¨oßen, die vom Navigationsfilter gesch¨ atzt werden sollen; damit liegt diese Messgleichung nicht in der

228

8 Anwendungsbeispiel GPS/INS-Integration

u ¨ blichen Form eines Kalman-Filter-Messmodells Gl. (6.3) vor, bei der der Zusammenhang zwischen Messgr¨ oßen und zu sch¨ atzenden Gr¨oßen durch Multiplikation mit einer Messmatrix Hk vermittelt wird. Die Behandlung einer solchen Messgleichung mit Integralterm im Rahmen des Kalman-Filter-Formalismus wird daher im folgenden Abschnitt n¨ aher betrachtet. Verarbeitung im Kalman-Filter Prinzipiell k¨onnte zur Verarbeitung der Tr¨agerphasendifferenzen ein Delayed State Kalman-Filter verwendet werden, wie er in [17] beschrieben wird. Hier soll jedoch ein anderes Verfahren zum Einsatz kommen, das bei Vergleichen geringf¨ ugig bessere Ergebnisse lieferte, sich aber vor allem als numerisch robuster erwiesen hat, siehe [124]. Definiert man in Anlehnung an den Zustandsvektor des Tightly Coupled Systems einen Zustandsvektor x(t), der jeweils drei Zust¨ande f¨ ur Position, Geschwindigkeit, Lage, Beschleunigungsmesserbiase und Drehratensensorbiase enth¨alt sowie Uhrenfehler und Uhrenfehlerdrift beinhaltet, l¨ asst sich der Integralterm in Gl. (8.135) mit  (8.136) H
k = 0 T , −ekT , 0 T , 0 T , 0 T , 0, 1 umschreiben zu
tk


tk n −ekT veb

+ cδ t˙ dt =

tk−1

H
k x(t) dt .

(8.137)

tk−1

Der zeitkontinuierliche Zustandsvektor x(t) kann mit Hilfe geeigneter Transitionsmatrizen in Beziehung zum Zustandsvektor xk zum Zeitpunkt tk gesetzt werden, es gilt x(t) = Φt,tk−1 Φtk−1 ,tk xk .

(8.138)

Der Zustandsvektor xk und die Transitionsmatrix Φtk−1 ,tk sind keine Funktionen der Zeit, setzt man Gl. (8.138) in Gl. (8.137) ein, k¨onnen diese Gr¨oßen daher außerhalb des Integrals gestellt werden:
tk

H
k x(t)

dt =

tk−1

H
k


tk Φt,tk−1 dt · Φtk−1 ,tk xk

(8.139)

tk−1

Dadurch wurde der Integralterm auf die bei einem Kalman-Filter-Messmodell u ¨ bliche Form gebracht, insgesamt erh¨ alt man  ˆ n (ϕ˜k − ϕ˜k−1 )λc − ekT δrS,k − δlkn = Hk xk + n
k (8.140) mit Hk =

H
k


tk Φt,tk−1 dt · Φtk−1 ,tk tk−1

     ˆ ˆn ˆ n , 0, 0 + 0 T , 0 T , ekT δlkn × , 0 T , −ekT ∆t · lk−1 × C b,k

.

(8.141)

8.3 Nutzung von Tr¨ agerphasenmessungen

229

Innerhalb des Zeitintervalls zwischen zwei Tr¨agerphasenmessungen werden mehrere Propagationsschritte des Kalman-Filters stattfinden. Anhand der dabei anfallenden Matrizen k¨ onnen die f¨ ur den Aufbau der Messmatrix Gl. (8.141) ben¨otigten Transitionsmatrizen sukzessive aufgebaut werden. Das in einem Propagationsschritt u uckte ¨ berbr¨ Zeitintervall sei mit δt bezeichnet. Die Transitionsmatrix, die zur Propagation der Kovarianzmatrix der Zustandssch¨ atzung vom Zeitpunkt (i − 1)δt + tk−1 bis zum Zeitpunkt iδt + tk−1 ben¨ otigt wird, ist durch Φiδt+tk−1 ,(i−1)δt+tk−1 = I + Fδt

(8.142)

gegeben, wobei F die f¨ ur dieses Zeitintervall g¨ ultige Systemmatrix bezeichnet. Die Propagation r¨ uckw¨ arts in der Zeit wird in diesem Intervall folglich durch die Matrix Φ(i−1)δt+tk−1 ,iδt+tk−1 = I − Fδt

(8.143)

vermittelt. Damit kann ausgehend von der Einheitsmatrix zum Zeitpunkt tk−1 die in uck f¨ ur St¨ uck aufgebaut werden: der Messmatrix ben¨ otigte Transitionsmatrix Φtk−1 ,tk St¨ Φtk−1 ,iδt+tk−1 = Φtk−1 ,(i−1)δt+tk−1 Φ(i−1)δt+tk−1 ,iδt+tk−1

(8.144)

Das Zeitintegral der Transitionsmatrix Φt,tk−1 erh¨alt man ausgehend von der Nullmatrix zum Zeitpunkt tk−1 mit iδt+t
k−1

(i−1)δt+t
k−1

Φt,tk−1 dt + δt · Φiδt+tk−1 ,(i−1)δt+tk−1 . (8.145)

Φt,tk−1 dt = tk−1

tk−1

Damit verteilt sich der zum Aufbau der Messmatrix ben¨otigte Rechenaufwand gleichm¨ aßig u ¨ ber das Zeitintervall zwischen den zwei Tr¨agerphasenmessungen, was sicherlich ein Vorteil ist. Im Gegensatz zu anderen Verfahren bleibt bei dieser Vorgehensweise außerdem eine Diagonalgestalt der Kovarianzmatrix des Messrauschens, sofern sie vorlag, erhalten. Damit wird f¨ ur eine aus numerischen Gr¨ unden erstrebenswerte, sequentielle Verarbeitung der Tr¨ agerphasendifferenzen keine vorherige Dekorrelation ben¨otigt. Ein weiterer Vorteil des Verfahrens besteht darin, dass die prinzipielle Struktur des Navigationsfilters unver¨ andert bleibt. Es wird lediglich anstelle der Deltarange-Verarbeitung ein anderer Messschritt eingef¨ uhrt. Ergebnisse Ein Vergleich zwischen einem Tightly Coupled Systems mit Pseudorange- und Deltarange-St¨ utzung und dem vorgestellten Verfahren zur Nutzung von Tr¨agerphasenmessungen ohne Verwendung von DGPS-Korrekturen ist in Abb. 8.14 und Abb. 8.15 zu sehen. Hierzu wurde erneut das HIL-Testsystem eingesetzt. Man erkennt, dass durch die Verarbeitung der Tr¨agerphasendifferenzen eine deutliche Steigerung der Geschwindigkeitsgenauigkeit erreicht wird, die bei St¨ utzung mit Pseudoranges und Deltaranges nicht erreicht werden kann. Ohne die Differenzenbildung hatte die St¨ utzung mit Tr¨ agerphasenmessungen zu einer Verschlechterung der Ergebnisse gef¨ uhrt, siehe Abb. 8.12. In Abb. 8.15 sind die zugeh¨origen Gesamtlagefehler dargestellt.

230

8 Anwendungsbeispiel GPS/INS-Integration

0.08 Deltarange Verarbeitung Tr¨agerphasendifferenzen

Geschwindigkeitsfehler (m/s)

0.07

e ♦

0.06 0.05 e

0.04

e

e

e

e e

0.03 0.02

e

e

♦ ♦ ♦ 0.01 ♦ ♦ e♦ 0

e

e

e e e

♦ ♦ ♦ ♦ ♦ ♦ ♦ ♦

200

400

600 Zeit (s)

e e e

e

e

e e

e ♦

e ♦e ♦ ♦ ♦ ♦ ♦ ♦ ♦ 800 1000 1200 ♦

Abbildung 8.14: Geschwindigkeitsfehler bei Deltarange-Verarbeitung und bei Verarbeitung zeitlicher Differenzen von Tr¨ agerphasenmessungen.

0.02

Lagefehler (rad)

0.015

♦ ♦ e

Deltarange Verarbeitung Tr¨agerphasendifferenzen

♦ e e♦ e

0.005

0

♦

e e e e e

0.01 e

e

e

e

e

e e e e e e e e ♦ e ♦ ♦ ♦ ♦ ♦ ♦ ♦ ♦ ♦ ♦ ♦ ♦ ♦ ♦ ♦ ♦ ♦ ♦ ♦ e e

200

400

600 Zeit (s)

800

1000

1200

Abbildung 8.15: Lagefehler bei Deltarange-Verarbeitung und bei Verarbeitung zeitlicher Differenzen von Tr¨ agerphasenmessungen.

8.4 Verz¨ ogerte Verf¨ ugbarkeit von Messwerten

231

-192 -190

♦

♦ ♦ ♦ ♦ ♦e ♦e ♦ e e ♦ ♦ ♦ ♦ e ♦ ♦e e ♦ ♦ ♦ e e e e e e

-188 H¨ ohe (m)

e

Deltarange Verarbeitung Tr¨agerphasendifferenzen

-186 -184

♦ ♦

e

-182 -180

e e

e

e

-178 0

100

200

300 400 Zeit (s)

500

600

700

Abbildung 8.16: H¨ oheninformationen bei Deltarange-Verarbeitung und bei Verarbeitung zeitlicher Differenzen von Tr¨ agerphasenmessungen.

Offensichtlich profitiert die Lagegenauigkeit massiv von der gesteigerten Genauigkeit der St¨ utzinformationen. Die gr¨ oßeren Lagefehler in den ersten dreihundert Sekunden des HIL-Tests wurden durch den Yaw-Winkel verursacht, hier lag keine Trajektoriendynamik vor, was die Unbeobachtbarkeit des Yaw-Winkels zur Folge hat. Mit einsetztender Dynamik wird der Yaw-Winkel-Fehler rasch abgebaut. Abschließend sind in Abb. 8.16 die Ergebnisse eines statischen Tests dargestellt. Hierbei wurde im Freien mit realen Satellitensignalen gearbeitet; da dynamisch keine Referenz zur Verf¨ ugung steht, blieb die Position der GPS-Antenne w¨ahrend des gesamten Zeitraumes unver¨ andert. In Abb. 8.16 sind die bei St¨ utzung mit Deltarange-Messungen und mit Tr¨ agerphasendifferenzen erzielten H¨ oheninformationen zu sehen, offensichtlich ist die Positionsl¨ osung bei Verwendung von Tr¨ agerphasendifferenzen deutlich rausch¨armer. Dies best¨ atigt die Ergebnisse der HIL-Tests. Die zu erkennende, langfristige Drift der Positionsl¨ osung geht im Wesentlichen auf atmosph¨arische St¨orungen und Ephemeridenfehler zur¨ uck und kann ohne Verwendung von DGPS-Korrekturdaten nat¨ urlich nicht verhindert werden.

8.4

Verz¨ogerte Verfu ¨gbarkeit von Messwerten

Das dem Messschritt eines Kalman-Filters zugrunde liegende Messmodell Gl. (6.3) beschreibt mathematisch den Zusammenhang zwischen Zustandsvektor und Messwerten. Hierbei wird davon ausgegangen, dass Systemzustand und Messwert zum selben Zeit-

232

8 Anwendungsbeispiel GPS/INS-Integration

punkt betrachtet werden k¨ onnen. In der Realit¨at wird eine Messung, die zu einem bestimmten Zeitpunkt g¨ ultig ist, immer erst zu einem sp¨ateren Zeitpunkt zur Verf¨ ugung stehen. Diese verz¨ ogerte Verf¨ ugbarkeit kann durch eine sensorinterne Signalverarbeitung ¨ oder durch die zur Ubertragung der Informationen zum Navigationscomputer ben¨otigte Zeit verursacht sein. In einem GPS/INS-System kann der Unterschied zwischen G¨ ultigkeitszeitpunkt und Verf¨ ugbarkeitszeitpunkt bei den Inertialsensoren vernachl¨assigt werden, bei dem GPSEmpf¨ anger in der Regel jedoch nicht. Die Verz¨ogerung der Messwerte eines GPSEmpf¨ angers kann leicht hundert Millisekunden u ¨ bersteigen, so dass innerhalb des Verz¨ ogerungszeitraumes mehrere Propagationsschritte des Filters stattfinden, siehe Abb. 8.17. Die Zustandssch¨ atzung und die zugeh¨orige Kovarianzmatrix ¨andern sich also innerhalb des Verz¨ ogerungszeitraumes. Wird die Verz¨ogerung ignoriert, d.h. werden die GPS-Messwerte so verarbeitet als w¨ aren sie aktuell, so resultieren bei signifikanter Trajektoriendynamik unmodellierte, systematische Messfehler, die zu einer deutlichen Verschlechterung des Filterergebnisses f¨ uhren k¨onnen. Es gibt verschiedene M¨ oglichkeiten, die Verz¨ogerung der GPS-Messungen zu ber¨ ucksichtigen. Eine einfache M¨ oglichkeit w¨ urde darin bestehen, den ganzen Filteralgorithmus mit einer entsprechenden Verz¨ ogerung gegen¨ uber der Realit¨at ablaufen zu lassen. Dadurch steht bei Verf¨ ugbarkeit der GPS-Messung die passende Zustandssch¨atzung und Kovarianzmatrix des Filters zur Verf¨ ugung. Geht man davon aus, dass die Zustandssch¨atzung des Navigationssystems h¨ aufig als Eingangsgr¨ oße z.B. eines Flugreglers dient, ist die bei dieser Vorgehenseweise resultierende Verz¨ogerung der Zustandssch¨atzung nicht tolerierbar. Eine andere theoretische M¨ oglichkeit besteht in der Erweiterung des Systemmodells des Filters. Hierzu wird angenommen, dass das Systemmodell in der u ¨ blichen Form  k−1 xk = Φk,k−1 xk−1 + Bk−1 uk−1 + Gk−1 w

(8.146)

Propagationsschritt des Filters

Zeit G¨ ultigkeitszeitpunkt der GPS-Messung

Verf¨ ugbarkeitszeitpunkt der GPS-Messung

Abbildung 8.17: G¨ ultigkeits- und Verf¨ ugbarkeitszeitpunkt von GPS Messungen.

8.4 Verz¨ ogerte Verf¨ ugbarkeit von Messwerten

233

gegeben ist. Zur eindeutigen Bezeichnung eines Messwertes y˜kk−n werden zwei Indizees verwendet, der obere Index k − n kennzeichnet den G¨ ultigkeitszeitpunkt des Messwertes, der untere Index gibt den Verf¨ ugbarkeitszeitpunkt an. Damit erh¨ alt man als Messmodell y˜kk−n = Hk−n xk−n + vk−n .

(8.147)

Bildet man ein erweitertes Systemmodell, das neben dem aktuellen Systemzustand auch noch die Zustandssch¨ atzungen zu den Zeitpunkten k − 1 bis k − n enth¨alt, ⎛

⎞ ⎛ xk Φk,k−1 ⎜ xk−1 ⎟ ⎜ I ⎜ . ⎟ = ⎜ ⎝ . ⎠ ⎝ . 0 xk−n 0

⎞ ⎞⎛ 0 xk−1 0⎟ ⎜ xk−2 ⎟ ⎟⎜ . ⎟ ⎠⎝ . ⎠ . 0 xk−n−1 ... I 0 ⎛ ⎛ ⎞ ⎞ Gk−1 Bk−1 ⎜ 0 ⎟ ⎜ 0 ⎟ ⎜ ⎟ ⎟  k−1 +⎜ ⎝ .. ⎠ uk−1 + ⎝ .. ⎠ w . . 0 0 0 0 .. .

... ... .. .

(8.148)

so kann mit dem Messmodell ⎞ xk  ⎜ xk−1 ⎟  ⎟ = 0 . . . 0 Hk−n ⎜ ⎝ .. ⎠ + vk−n . xk−n ⎛

y˜kk−n

(8.149)

der vorliegenden Messwert  y˜kk−n korrekt in Beziehung zu Komponenten des Zustandsvektors gesetzt werden. Diese Vorgehensweise kann jedoch nur als eine theoretische L¨osung des Problems der verz¨ ogerten Messwertverf¨ ugbarkeit gesehen werden; der mit der Erweiterung des Zustandsvektors verbundene Rechenaufwand verhindert eine sinnvolle, praktische Umsetzung dieses Ansatzes. Im Folgenden soll daher eine andere M¨ oglichkeit zur L¨osung des Problems aufgezeigt werden, die sich an [78] orientiert. Hierbei handelt es sich zwar nur um ein suboptimales Verfahren, daf¨ ur ist jedoch die praktische Einsetzbarkeit gegeben. Die Grundidee dieses Ansatzes besteht darin, einen extrapolierten Messwert y˜kk∗ zu berechnen, der auf das gleiche Residuum f¨ uhrt, das aufgetreten w¨ are, wenn der betreffende Messwert zum G¨ ultigkeitszeitpunkt auch tats¨ achlich h¨ atte verarbeitet werden k¨onnen. Letzteres Residuum ist gegeben durch ˆ− , k−n = y˜kk−n − Hk−n x k−n

(8.150)

234

8 Anwendungsbeispiel GPS/INS-Integration

f¨ ur das bei der Verarbeitung des extrapolierten Messwertes auftretende Residuum erh¨alt man ˆ− . ∗k = y˜kk∗ − Hk x k

(8.151)

Gleichsetzen und Aufl¨ osen nach dem extrapolierten Messwert liefert ˆ − = y˜ k−n − Hk−n x ˆ− y˜kk∗ − Hk x k k k−n k−n − k∗ ˜ ˜ ˆ ˆ− . + Hk xk − Hk−n x yk = yk k−n

(8.152)

Mit dem Messmodell Gl. (8.147) ergibt sich: ˆ − − Hk−n x ˆ− y˜kk∗ = Hk−n xk−n + vk−n + Hk x k k−n   − − ˆ ˆ = −Hk−n xk−n − xk−n + Hk xk − xk + Hk xk + vk−n − = Hk xk + Hk δxk− − Hk−n δxk−n + vk−n

(8.153)

Damit liegt formal ein gew¨ ohnliches Kalman-Filter-Messmodell vor, y˜kk∗ = Hk xk + vk∗ ,

(8.154)

das Messrauschen des extrapolierten Messwertes ist gegeben durch − vk∗ = Hk δxk− − Hk−n δxk−n + vk−n .

(8.155)

Da in dieses Messrauschen aber die Zustandsvektoren zu den Zeitpunkten k und k − n eingehen, sind Messrauschen und Zustandsvektor korreliert. Um den extrapolierten onnen, m¨ ussen modifizierte Kalman-Filter-Gleichungen Messwert y˜kk∗ verarbeiten zu k¨ hergeleitet werden, die eine Ber¨ ucksichtigung dieser Korrelationen erlauben. Eine m¨ogliche Vorgehensweise hierzu ist die allgemeine Berechnung der a posteriori Kovarianzmatrix des Sch¨ atzfehlers und die anschließende Minimierung deren Spur. Die Anpassung des Zustandsvektors im Messschritt l¨asst sich mit einer noch unbekannten Gewichtungsmatrix K∗k wie folgt formulieren:  ˆ − − K∗ Hk x ˆ − − y˜ k∗ xˆk+ = x k k k k

(8.156)

Einsetzen des Messmodells Gl. (8.154) und Erweitern mit dem Zustandsvektor f¨ uhrt auf   ˆ − − Hk xˆk + v ∗ ˆ − − xk − K∗ Hk x xˆk+ = x k k k k δxk+ = (I − K∗k Hk ) δxk− + K∗kvk∗ .

(8.157)

8.4 Verz¨ ogerte Verf¨ ugbarkeit von Messwerten

235

Damit erh¨ alt man f¨ ur die a posteriori Kovarianzmatrix   +,T + P+ = E δ x δ x k k k

  T ∗ ∗ xk−vk∗,T K∗,T = (I − K∗k Hk ) P− k (I − Kk Hk ) + (I − Kk Hk ) E δ k     T +K∗k E vk∗ δxk−,T (I − K∗k Hk ) + K∗k E vk∗vk∗,T K∗,T .(8.158) k

Bevor die Spur von P+ ussen alle in Gl. (8.158) auftretenden k minimiert werden kann, m¨ Erwartungswerte berechnet werden. F¨ ur den ersten Erwartungswert erh¨alt man     T − E δxk−vk∗,T = E δxk− Hk δxk− − Hk−n δxk−n + vk−n  

− T  −,T − T T H − E δ x δ x xk vk−n = P− (8.159) k k k k−n Hk−n + E δ Der letzte Erwartungswert verschwindet, da das Messrauschen vk−n nicht mit dem Sch¨ atzfehler zum Zeitpunkt k korreliert sein kann, schließlich ist der betreffende Messwert noch nicht verarbeitet worden. Setzt man   −,T , (8.160) Ck,k−n = E δxk− δxk−n so ergibt sich    T T E vk∗ δxk−,T = E δxk−vk∗,T = Hk P− k − Hk−n Ck,k−n .

(8.161)

Mit diesen Ergebnissen kann der letzte Erwartungswert in Gl. (8.158) wie folgt formuliert werden:     − + vk−n E vk∗vk∗,T = E Hk δxk− − Hk−n δxk−n T   − · Hk δxk− − Hk−n δxk−n + vk−n T T T T = Hk P− k Hk − Hk Ck,k−n Hk−n − Hk−n Ck,k−n Hk T +Hk−n P− k−n Hk−n + Rk−n

(8.162)

Was nun noch bleibt ist die Berechnung des Erwartungswertes Gl. (8.160), hierf¨ ur muss der Propagationsschritt des Filters betrachtet werden. W¨ahrend die zeitliche Propagation des Systemzustandes durch Gl. (8.146) beschrieben wird, ist die Propagation der Zustandssch¨ atzung gegeben durch ˆ + + Bk−1 uk−1 . xˆk− = Φk−1 x k−1

(8.163)

¨ Subtrahiert man Gl. (8.146) von Gl. (8.163), erh¨alt man die Anderung des Sch¨atzfehlers im Propagationsschritt: + δxk− = Φk−1 δxk−1 − Gk−1 w  k−1

(8.164)

236

8 Anwendungsbeispiel GPS/INS-Integration

Im Allgemeinen k¨ onnen in dem Zeitraum zwischen G¨ ultigkeit und Verf¨ ugbarkeit der Messung weitere Messwerte anderer Sensoren anfallen. Es soll angenommen werden, dass bei diesen zus¨ atzlichen Messungen keine verz¨ogerte Verf¨ ugbarkeit beachtet werden ¨ muss. Die Anderung des Sch¨ atzfehlers bei Verarbeitung dieser gew¨ohnlichen, unmittelbar verf¨ ugbaren Messung ist in Analogie zu Gl. (8.157) gegeben durch δxk+ = (I − Kk Hk )δxk− + Kkvk# ,

(8.165)

deren Messrauschen ist zur Unterscheidung von dem Messrauschen der verz¨ogert verf¨ ugbaren Messung durch ein hochgestelltes Gatter ()# gekennzeichnet. Mit Gl. (8.165) ist − es nun m¨ oglich, eine Beziehung zwischen den Sch¨atzfehlern δxk− und δxk−n herzustellen. Durch sukzessives Einsetzen von Gl. (8.164) und Gl. (8.165) erh¨alt man: + − Gk−1 w  k−1 δxk− = Φk−1 δxk−1  # − − Gk−1 w = Φk−1 (I − Kk−1 Hk−1 ) δxk−1 + Kk−1vk−1  k−1    + = Φk−1 (I − Kk−1 Hk−1 ) Φk−2 δxk−2 − Gk−2 w  k−2 # +Kk−1vk−1 − Gk−1 w  k−1    − = Φk−1 (I − Kk−1 Hk−1 ) Φk−2 (I − Kk−2 Hk−2 ) δxk−2 # # − Gk−2 w − Gk−1 w +Kk−2vk−2  k−2 + Kk−1vk−1  k−1

= Φk−1 (I − Kk−1 Hk−1 ) Φk−2 (I − Kk−2 Hk−2 ) · ... − ·Φk−n (I − Kk−n Hk−n ) δxk−n  # # + f2 (w +f1 vk−1 , ..., vk−n  k−1 , ..., w  k−n )   n − Φk−i (I − Kk−i Hk−i ) δxk−n = i=1

 # # + f2 (w +f1 vk−1 , ..., vk−n  k−1 , ..., w  k−n )

(8.166)

# # , ..., vk−n der unmittelbar verf¨ ugbaren Messungen und das SyDas Messrauschen vk−1 − stemrauschen w  k−1 , ..., w  sind nicht korreliert mit dem Sch¨atzfehler δxk−n , so dass     k−n −,T −,T die Erwartungswerte E f1 (...)δxk−n und E f2 (...)δxk−n verschwinden. Damit ergibt sich der Erwartungswertes Gl. (8.160) zu

Ck,k−n

  −,T = = E δxk− δxk−n

 

=

n i=1 n i=1



  −,T − Φk−i (I − Kk−i Hk−i ) E δxk−n δxk−n  Φk−i (I − Kk−i Hk−i ) P− k−n ,

(8.167)

8.4 Verz¨ ogerte Verf¨ ugbarkeit von Messwerten

237

f¨ ur die a posteriori Kovarianzmatrix erh¨ alt man − ∗ ∗ P+ k = (I − Kk Hk ) Pk (I − Kk Hk )  ∗,T  T T + (I − K∗k Hk ) P− k Hk − Ck,k−n Hk−n Kk   T T ∗ +K∗k Hk P− k − Hk−n Ck,k−n (I − Kk Hk )  T T T T +K∗k Hk P− k Hk − Hk Ck,k−n Hk−n − Hk−n Ck,k−n Hk  ∗,T T +Hk−n P− k−n Hk−n + Rk−n Kk T

∗,T T = P− − K∗k Hk−n CTk,k−n k − Ck,k−n Hk−n Kk ∗,T T +K∗k Hk−n P− + K∗k Rk−n K∗,T . k−n Hk−n Kk k

(8.168)

Die gesuchte Gewichtungsmatrix erh¨ alt man durch Nullsetzen der Ableitung der Spur der Kovarianzmatrix nach der Gewichtungsmatrix,   d Spur P+ k T ∗ = −2Ck,k−n HTk−n + 2K∗k Hk−n P− k−n Hk−n + 2Kk Rk−n d K∗k (8.169) und damit  −1 T K∗k = Ck,k−n HTk−n Hk−n P− . k−n Hk−n + Rk−n

(8.170)

Abschließend soll die aufgezeigte Vorgehensweise zur Verarbeitung eines verz¨ogert verf¨ ugbaren Messwertes nochmals zusammengefasst werden: Im Zeitraum zwischen G¨ ultigkeit, Zeitpunkt k − n, und Verf¨ ugbarkeit, Zeitpunkt k, der verz¨ ogerten Messung werden unmittelbar anfallende Messwerte wie gew¨ohnlich verarbeitet und auch die Kovarianzmatrix des Sch¨atzfehlers anhand der u ¨ blichen Gleichungen propagiert. Parallel dazu wird jedoch die Matrix Ck,k−n aufgebaut:  Ck,k−n =

n -

 Φk−i (I − Kk−i Hk−i ) P− k−n

(8.171)

i=1

Liegt der verz¨ ogerte Messwert nun vor, wird eine Gewichtungsmatrix K∗k berechnet:  −1 T K∗k = Ck,k−n HTk−n Hk−n P− k−n Hk−n + Rk−n

(8.172)

Durch Einsetzen von (8.152) in Gl. (8.156) erkennt man, dass die explizite Berechnung eines extrapolierten Messwertes nicht notwendig ist, vielmehr kann man schreiben  ˆ − − y˜ k−n . xˆk+ = xˆk− − K∗k Hk−n x k−n k

(8.173)

238

8 Anwendungsbeispiel GPS/INS-Integration

Es gen¨ ugt also, einfach die Innovation basierend auf der zum G¨ ultigkeitszeitpunkt der Messung vorliegenden Zustandssch¨ atzung zu berechnen. Die abschließende Anpassung der Kovarianzmatrix des Sch¨ atzfehlers erfolgt gem¨aß − ∗ T P+ k = Pk − Kk Hk−n Ck,k−n .

(8.174)

Die hier beschriebene Vorgehensweise ist suboptimal in dem Sinne, dass die verz¨ogerte Verf¨ ugbarkeit eines Messwertes prinzipiell einen Einfluss hat: W¨are der Messwert zum ¨ G¨ ultigkeitszeitpunkt verf¨ ugbar gewesen, h¨atte seine Verarbeitung zu einer Anderung der Kovarianzmatrix des Sch¨ atzfehlers gef¨ uhrt, so dass anschließend die unmittelbar verf¨ ugbaren Messwerte mit einer anderen Gewichtung verarbeitet worden w¨aren. Diese Vorgehensweise ist jedoch der optimale Ansatz zur Verarbeitung des extrapolierten Messwertes bzw. der Innovation, die auf der zum G¨ ultigkeitszeitpunkt vorliegenden Zu¨ standssch¨ atzung beruht. Wenn die Anderung des Sch¨atzfehlers im Zeitraum zwischen G¨ ultigkeit und Verf¨ ugbarkeit des Messwertes vernachl¨assigt werden kann – was sicherlich bei einem Großteil der Anwendungen gegeben ist – so kann man n¨aherungsweise − Ck,k−n ≈ P− k ≈ Pk−n

(8.175)

¨ setzen. Das bedeutet, dass die einzige Anderung gegen¨ uber der Verarbeitung eines gew¨ ohnlichen, unmittelbar verf¨ ugbaren Messwertes in der ver¨anderten Berechnung der Innovation in Gl. (8.173) besteht, was praktisch keinen Mehraufwand bedeutet. Nat¨ urlich sind auch Applikationen denkbar, bei denen der G¨ ultigkeitszeitpunkt der Messung im Voraus nicht bekannt ist. In diesem Fall kann eine einfache Ber¨ ucksichtigung der Messwertverz¨ ogerung bei Eintreffen des Messwertes durch Umformulierung der Messgleichung mit Hilfe der Transitionsmatrix Φk−n,k = Φ−1 k,k−n erfolgen, die die Propagation des Systemzustandes vom Zeitschritt k zum Zeitschritt k − n beschreibt: y˜kk−n = Hk−n xk−n + vk−n = Hk−n Φ−1 xk + vk−n k,k−n 

(8.176)

Die neue Messmatrix ist durch Hk−n Φ−1 onnen nun die u ¨ blichen k,k−n gegeben, es k¨ Kalman-Filter-Gleichungen verwendet werden. Dieses Verfahren ber¨ ucksichtigt die verz¨ ogerte Verf¨ ugbarkeit des Messwertes nur n¨aherungsweise, da der Einfluss des Systemrauschens nicht exakt erfasst wird. F¨ ur das Anwendungsgebiet GPS/INS-Integration d¨ urfte das unproblematisch sein. Mit etwas Mehraufwand kann je nach Szenario dieser Einfluss aber auch exakt oder zumindest n¨aherungsweise erfasst werden, hierzu sind in der Target-Tracking-Literatur unter dem Stichwort Out-Of-Sequence Measurements (OOSM) eine Reihe von Verfahren zu finden.

8.5

Integrity Monitoring

In praktisch jedem Integrierten Navigationssystem ist irgendeine Form von Integrity Monitoring realisiert. Darunter versteht man Algorithmen, die die Konsistenz des Filteralgorithmus sicherstellen sollen, indem z.B. fehlerhafte Messwerte abgelehnt werden.

8.6 Sigma-Point-Kalman-Filter

239

Ein verbreitetes Verfahren ist, die Differenz zwischen vorliegendem Messwert y˜k und Messwertvorhersage Hk xk− , sprich die Innovation, zu betrachten. Die Kovarianz der T Innovation ist Hk P− oßer als z.B. die dreifache Stank Hk + Rk ; ist die Innovation gr¨ dardabweichung, sollte der Messwert unter Umst¨anden abgelehnt werden. Dabei muss jedoch darauf geachtet werden, dass es zu keiner Entkopplung des Filters von der Realit¨ at kommt: Werden Messwerte abgelehnt, k¨ onnen je nach Szenario die Navigationsfehler mit der Zeit anwachsen, die Innovationen werden daher ebenfalls gr¨oßer ausfallen. Wird dieses Anwachsen der Navigationsfehler nicht exakt in der P-Matrix abgebildet, kann das zum Ablehnen von weiteren Messwerten f¨ uhren, die keineswegs fehlerhaft waren. In den meisten GPS-Empf¨ angern werden Vorkehrungen getroffen, um zumindest m¨oglichst wenige fehlerhafte Messungen an den Benutzer weiterzugeben, man spricht hier von Receiver Autonomous Integrity Monitoring (RAIM). Die bekanntesten Vertreter hierbei sind die Range-Comparison-Methode, die Least-Square-Methode und die ParitySpace-Methode7 . Diese drei Methoden sind als ¨aquivalent anzusehen, siehe [16], daher soll hier nur die einfachste beschrieben werden: Bei der Range-Comparison-Methode wird zun¨ achst aus vier Pseudoranges die User-Position berechnet. Basierend auf dieser Position k¨ onnen die Pseudoranges zu weiteren Satelliten vorhergesagt und mit den tats¨ achlich gemessenen Pseudoranges verglichen werden. Fallen nun eine oder mehrere dieser Differenzen zwischen Vorhersage und Messung gr¨oßer aus als die aufgrund des Messrauschens erwartete Ungenauigkeit, liegen mit großer Wahrscheinlichkeit fehlerhafussen mindestens te Messungen vor8 . Um einen solchen Fehlerfall erkl¨aren zu k¨onnen, m¨ f¨ unf Pseudoranges vorliegen. Bei mindestens sechs Pseudoranges kann durch Bildung geeigneter Permutationen eine einzelne fehlerhafte Messung identifiziert werden, bei sieben Pseudoranges sind zwei fehlerhafte Messungen identifizierbar. Ein anderer Integrity-Monitoring-Ansatz wird in [96] beschrieben: Hier wird ein IMM verwendet, dessen elementare Filter durch Tightly-Coupled-GPS/INS-Filter gegeben sind, deren Zustandsvektor um jeweils einen Pseudorange-Bias, jeweils f¨ ur einen anderen Satelliten, erweitert wurde. Bei dieser Vorgehensweise kann schon bei nur f¨ unf Satelliten ein fehlerhafter Satellit nicht nur erkannt, sondern auch identifiziert werden – nat¨ urlich ist der Rechenzeitbedarf bei diesem Verfahren, das zur Gruppe der Aircraft ” Autonomous Integrity Monitoring“-(AAIM-)Verfahren z¨ahlt, betr¨achtlich.

8.6

Sigma-Point-Kalman-Filter

Momentan werden zur Fusion der Daten von GPS-Empf¨anger und Inertialsensoren fast ausschließlich erweiterte oder linearisierte Kalman-Filter eingesetzt, ihre prinzipielle Eignung f¨ ur diese Aufgabe ist seit Jahrzehnten unumstritten. Dennoch liegt bei diesem Sch¨ atzproblem ein nichtlineares Systemmodell und zumindest im Falle eines Tightly Coupled Systems auch ein nichtlineares Messmodell vor. Wie in Abschnitt 6.4.3 aus7 F¨ ur eine Implementierung bietet sich die Parity-Space-Methode an, da zur Identifikation einer fehlerhaften Messung nicht verschiedene Permutationen untersucht werden m¨ ussen wie z.B. bei der Range-Comparison-Methode. 8 Gerade in st¨ adtischen Gebieten kann aufgrund der Abschattung von Satelliten ein so schlechtes GDOP auftreten, dass auch ohne fehlerhafte Messungen eine mit unakzeptabel großen Fehlern behaftete Positionsbestimmung resultiert.

240

8 Anwendungsbeispiel GPS/INS-Integration

gef¨ uhrt, approximiert ein Sigma-Point-Kalman-Filter bei nichtlinearen Transformationen die Kovarianz und den Mittelwert einer Zufallsvariable genauer als ein erweitertes Kalman-Filter. In der Literatur werden daher auch eine Reihe von Anwendungen beschrieben, bei denen mit einem Sigma-Point-Kalman-Filter bessere Ergebnisse erzielt werden konnten als mit einem erweiterten Kalman-Filter, dazu z¨ahlen z.B. die Zielverfolgung mittels Radar [129] und das Trainieren neuronaler Netze [118]. Im Folgenden soll untersucht werden, ob auch auf dem Gebiet der GPS/INS-Integration eine Steigerung von Genauigkeit, Zuverl¨ assigkeit und Integrit¨at des Navigationssystem erreicht werden kann, indem anstelle eines erweiterten Kalman-Filters ein Sigma-Point-Kalman-Filter eingesetzt wird.

8.6.1

Nichtlinearita¨t eines Scha¨tzproblems

Einen Unterschied in den Filterergebnissen von Sigma-Point-Kalman-Filter und erweitertem Kalman-Filter kann es nur geben, wenn die Terme h¨oherer Ordnung, die von einem Sigma-Point-Kalman-Filter noch erfasst werden, bei den betreffenden Transformationen einen signifikanten Beitrag zu Mittelwert und Kovarianz liefern. Auf den ersten Blick u ur die nichtlinearen Gleichungen des System- und ¨ berraschend sind daf¨ Messmodells nur von sekund¨ arer Bedeutung, wesentlich entscheidender f¨ ur die Nicht” linearit¨ at“ des Sch¨ atzproblems ist die gew¨ahlte Zeitschrittweite und die Unsicherheit der Zustandssch¨ atzung. Diese Zusammenh¨ange sollen anhand eines einfachen Beispiels illustriert werden. Hierzu wird die zweidimensionale Bewegung eines Fahrzeugs in einer Ebene betrachtet. Das Fahrzeug soll sich mit einer konstanten, bekannten Geschwindigkeit vabs in Fahrzeugl¨ angsrichtung bewegen. Der Systemzustand l¨asst sich mit drei Zustandsvaria blen beschreiben, der Fahrzeugposition in x- und y-Richtung und dem Yaw-Winkel ψ, der die Bewegungsrichtung angibt. Das nichtlineare Systemmodell ist somit durch die Gleichungen x˙ = −vabs sin ψ y˙ = vabs cos ψ ψ˙ = ω

(8.177) (8.178) (8.179)

gegeben, ω ist die Drehrate der Drehung um die Fahrzeughochachse. Wird diese ebenfalls als konstant angenommen, kann sehr leicht eine analytische L¨osung des Differentialgleichungssystems (8.177) – (8.179) bestimmt werden. Anhand dieses Besipiels soll analysiert werden, wie die Kovarianz der Fahrzeugposition w¨ahrend einer neunzig Grad Kreisbogenfahrt von einem erweiterten Kalman-Filter und von einem Sigma-Point-KalmanFilter propagiert wird. Abb. 8.18 zeigt die initialen 1-Sigma-Kovarianzellipsen der beiden Filter, die Standardabweichung der Positionsunsicherheit in Fahrzeugl¨angsrichtung wurde zu 3.9 m, in Fahrzeugquerrichung zu 1.0 m gew¨ ahlt. Die initiale Standardabweichung des YawWinkels wurden zu zehn Grad angenommen, der gesammte Kreisbogen wird in einem einzigen Zeitschritt durchfahren. Abb. 8.18 zeigt zus¨atzlich die propagierten Kovarianzellipsen nach der Kreisbogenfahrt an den von den Filtern bestimmten Positionen,

8.6 Sigma-Point-Kalman-Filter

241

30 b 25 r

20 r

15 y (m)

 b r

Trajektorie

b

b

b r

b

r

1-σ Bereich EKF 1-σ Bereich SPKF

r r



 

b



10

 5

rbr b b r r b b rbr

0 -5 -5

0

5

10 x (m)

15

20

25

Abbildung 8.18: Propagation der Kovarianz mit EKF und SPKF, Standardabweichung Yaw zu Beginn zehn Grad, ein Zeitschritt.

die analytisch berechnete Trajektorie ist ebenfalls eingezeichnet. Offensichtlich unterscheiden sich die propagierten Kovarianzellipsen deutlich. Bestimmt man die korrekte Kovarianzellipse z.B. durch ein Monte-Carlo-Sampling, stellt man fest, dass der SigmaPoint-Kalman-Filter ein sehr gutes Ergebnis liefert, die Kovarianzellipse des erweiterten Kalman-Filters spiegelt die tats¨ achliche Unsicherheit der propagierten Position massiv fehlerhaft wieder, in Fahrzeugquerrichtung wird eine zu große Unsicherheit, in Fahrzeugl¨ angsrichtung eine zu kleine Unsicherheit berechnet. Dadurch w¨ urden Positionsmessungen, die die Position in Fahrzeugl¨ angsrichtung korrigieren k¨onnten, zu gering gewichtet, was bis zur Divergenz des Filters f¨ uhren k¨onnte. Die bei der Kovarianz-Ellipse des Sigma-Point-Kalman-Filters vorliegende Verdrehung l¨asst sich auch anschaulich erkl¨ aren: Denkt man sich die Trajektorie aufgrund der Winkelunsicherheit um einige Grad in die eine oder in die andere Richtung verdreht, so kommt der Endpunkt der Trajektorie in der N¨ ahe des Schnittpunktes der großen Halbachse mit dem Ellipsenrand zu liegen. In dem betrachteten Szenario spielen die vom erweiterten Kalman-Filter nicht erfassten Terme offensichtlich eine wichtige Rolle, ihre Vernachl¨assigung f¨ uhrt zu massiven Fehlern. Das a ¨ndert sich, wenn anstelle einer initialen Winkelunsicherheit von zehn Grad eine Winkelunsicherheit von einem Grad angenommen wird, die restlichen Parameter der Simulation bleiben unver¨ andert. F¨ ur dieses Szenario erh¨alt man die in Abb. 8.19 dargestellten Ergebnisse. Aufgrund der geringeren Anfangsunsicherheit des Winkels, die maßgeblich in die Terme h¨ oherer Ordnung eingeht, k¨onnen diese ohne weiteres vernachl¨ assigt werden. Das Sigma-Point-Kalman-Filter und das erweiterte Kalman-Filter liefern nahezu identische Ergebnisse.

242

8 Anwendungsbeispiel GPS/INS-Integration

30  b r

Trajektorie 25 b r b

20 15

rb r r br

b r b

1-σ Bereich EKF 1-σ Bereich SPKF



 

y (m)



10

 5

rbr b b r r b b rbr

0 -5 -5

0

5

10 x (m)

15

20

25

Abbildung 8.19: Propagation der Kovarianz mit EKF und SPKF, Standardabweichung Yaw zu Beginn ein Grad, ein Zeitschritt.

30 25

r b

b

20

b r

15

b

r b

r

b

r

y (m)

r 

r

b r b

r

b

r

 b

b

r b r

10 5

 b r

0

b r b r b

b

r r

Trajektorie 1-σ Bereich EKF 1-σ Bereich SPKF

-5 -5

0

5

10 x (m)

15

r b 

b r

r b r b b r  r b b r b rr b r b b r r b b rbr 20

25

Abbildung 8.20: Propagation der Kovarianz mit EKF und SPKF, Standardabweichung Yaw zu Beginn zehn Grad, dreißig Zeitschritte.

8.6 Sigma-Point-Kalman-Filter

243

¨ Ahnliche Ergebnisse erh¨ alt man auch f¨ ur die große Anfangswinkelunsicherheit von zehn Grad, wenn man eine geringere Zeitschrittweite w¨ahlt. Wird der Kreisbogen z.B. in dreißig Zeitschritten durchfahren, erh¨ alt man die in Abb. 8.20 dargestellten Ergebnisse. Die durch die große Winkelunsicherheit verursachte Verdrehung der Kovarianzellipse, die in Abb. 8.18 nur vom Sigma-Point-Kalman-Filter erfasst werden konnte, ist auch beim erweiterten Kalman-Filter feststellbar. Dieses einfache Beispiel illustriert eindrucksvoll, dass bei demselben Sch¨atzproblem verglichen mit einem Sigma-Point-Kalman-Filter ein erweitertes Kalman-Filter gleichwertige, aber auch deutlich schlechtere Ergebnisse liefern kann. Ob unterschiedliche Ergebnisse resultieren, h¨ angt in erster Linie von der Zeitschrittweite und der Kovarianz der Zustandssch¨ atzung ab.

8.6.2

Simulationsergebnisse

In diesem Abschnitt werden zwei Tightly Coupled GPS/INS-Systeme, basierend auf einem linearisierten Kalman-Filter und einem Sigma-Point-Kalman-Filter, in numerischen Simulationen verglichen. Grundlage beider Filter sind die nichtlinearen Gleichungen der Inertialnavigation. Bei dem Sigma-Point-Kalman-Filter wurde eine Total State Space“” Formulierung gew¨ ahlt, um die unvermeidlichen Linearisierungsfehler bei der Bestimmung eines Fehlersystemmodells zu vermeiden. Bei dem linearisierten Kalman-Filter konnte mit Eulerwinkelfehlern gearbeitet werden, da diese als klein angenommen werden k¨ onnen und die bei neunzig Grad Pitch auftretenden Singularit¨aten und Mehrdeutigkeiten somit keine Rolle spielen, siehe Abschnitt 8.2.1. Die absoluten Lagewinkel hingegen k¨ onnen nicht als klein angenommen werden, daher wurden in den Zustandsvektor des Sigma-Point-Kalman-Filters anstelle von drei Winkeln die vier Komponenten des Lage-Quaternions aufgenommen.

Tabelle 8.1: Parameter der in den numerischen Simulationen angenommenen Inertialsensoren.

Low-Cost MEMS IMU MEMS IMU B.-Messer const. Bias 10 mg 5 mg B.-Messer Bias Drift* 0.2 mg, 1000 s 1.0 mg, 3600 s √ √ B.-Messer random walk 0.4 mg/ Hz 0.4 mg/ Hz B.-Messer Skalenfaktorf. 0.5% 0.5% B.-Messer Misalignment 3.5 mrad 3.5 mrad Gyroscope const. Bias 360 ◦ /h 75 ◦ /h Gyroscope Bias Drift∗ 72 ◦/h, 1000 s 3 ◦/h, 3600 s √ √ 0.3 ◦/ h Gyroscope Random Walk 3.5 ◦/ h Gyroscope Skalenfaktorf. 0.5% 0.5% Gyroscope Misalignment 3.5 mrad 3.5 mrad Update-Rate 100 Hz 100 Hz * Parameter eines Gauß-Markov Prozesses erster Ordnung.

FOG IMU 5 mg 0.0 √ 0.05 mg/ Hz 200ppm 0.5 mrad 5 ◦ /h 0.0 √ 0.1 ◦/ h 100ppm 0.5 mrad 200 Hz

244

8 Anwendungsbeispiel GPS/INS-Integration

In den numerischen Simulationen wurden Inertialsensoren unterschiedlicher G¨ ute betrachtet, von Low-cost MEMS bis FOG. Die gew¨ahlten Sensorparameter sind in Tab. 8.1 angegeben. Damit wird eine große Bandbreite von Anwendungen abgedeckt, von Flugk¨ orpernavigation bis Micro-Aerial-Vehicles (MAV). Die GPS-Pseudorange- und Deltarange-Messungen wurden ebenfalls synthetisch erzeugt, wobei ein C/A-Code-Empf¨ anger angenommen wurde. Als Messfehler wurden Ionosph¨ arenfehler, Tropsph¨ arenfehler, Ephemeridenfehler, Fehler durch Mehrwegeausbreitung und Empf¨ angerrauschen simuliert. Die daf¨ ur ben¨otigten Fehlermodelle wurden [101] entnommen und anhand von realen Messdaten validiert. F¨ ur das Empf¨angerUhrenfehlermodell wurden typische Werte verwendet, wie sie in [17] angegeben sind. Die Satellitenkonstellation wurde anhand realer Almanach-Daten simuliert. Als Anfangspositionsfehler wurden 30 m angenommen, die Geschwindigkeitsfehler betrugen 1 m/s, die Lagefehler wurden f¨ ur Roll und Pitch zu 35 mrad, f¨ ur Yaw zu 85 mrad gew¨ ahlt. Diese Anfangsfehler sind jeweils als Standardabweichungen zu verstehen, d.h. die tats¨ achlichen Anfangsfehler wurden vor jedem Simulationslauf mit einem Zufallszahlengenerator bestimmt. Zuerst wurden Simulationen mit einer guten Satellitenverf¨ ugbarkeit durchgef¨ uhrt, die Ergebnisse f¨ ur eine Low-cost MEMS IMU und eine FOG IMU sind in Abb. 8.21 – 8.23 und Abb. 8.24 – 8.26 dargestellt. Offensichtlich liefern das linearisierte Kalman-Filter und das Sigma-Point-Kalman-Filter nahezu identische Ergebnisse, lediglich in der ersten Phase der Simulation scheinen die Geschwindigkeits- und Lagesch¨atzungen bei dem auf

18 u e

16

u e

u e

u e

Positionsfehler (m)

14

u e

12

u

u e

10 8 6 4

e u

EKF SPKF

2 0 0

100

200

300

400 Zeit (s)

500

600

700

800

Abbildung 8.21: RMS-Werte der Positionsfehler gemittelt u aufe, gute Sa¨ber 25 Simulationsl¨ tellitenverf¨ ugbarkeit, Low-cost MEMS IMU.

8.6 Sigma-Point-Kalman-Filter

245

Geschwindigkeitsfehler (m/s)

0.5

e u

EKF SPKF

0.4

0.3 u

0.2

e

0.1

e

e e

u

u

u

u e

u

u

e

0 0

100

200

300

400 Zeit (s)

500

600

700

800

Abbildung 8.22: RMS-Werte der Geschwindigkeitsfehler gemittelt u aufe, ¨ber 25 Simulationsl¨ gute Satellitenverf¨ ugbarkeit, Low-cost MEMS IMU.

0.05

0.04 Lagefehler (rad)

e u

EKF SPKF

0.03

0.02 u e

0.01

u e

u

e u

e

u

e

u e

u

600

700

800

0 0

100

200

300

400 Zeit (s)

500

Abbildung 8.23: RMS-Werte der Lagefehler gemittelt u aufe, gute Satelli¨ber 25 Simulationsl¨ tenverf¨ ugbarkeit, Low-cost MEMS IMU.

246

8 Anwendungsbeispiel GPS/INS-Integration

18 u e

16

u e

Positionsfehler (m)

14

u

e

u e

u e

u e

u

12 10 8 6 4

e u

EKF SPKF

2 0 0

100

200

300

400 Zeit (s)

500

600

700

800

Abbildung 8.24: RMS-Werte der Positionsfehler gemittelt u aufe, gute Sa¨ber 25 Simulationsl¨ tellitenverf¨ ugbarkeit, Low-cost MEMS IMU.

Geschwindigkeitsfehler (m/s)

0.5

e u

EKF SPKF

0.4

0.3

0.2

0.1

u e

u e

u

e

u e

u e

u e

u

700

800

0 0

100

200

300

400 Zeit (s)

500

600

Abbildung 8.25: RMS-Werte der Geschwindigkeitsfehler gemittelt u aufe, ¨ber 25 Simulationsl¨ gute Satellitenverf¨ ugbarkeit, Low-cost MEMS IMU.

8.6 Sigma-Point-Kalman-Filter

247

0.05

0.04 Lagefehler (rad)

e u

EKF SPKF

0.03

0.02

0.01 u e

0 0

100

u e 200

u e 300

u e

400 Zeit (s)

500

u e 600

u e

u

700

800

Abbildung 8.26: RMS-Werte der Lagefehler gemittelt u aufe, gute Satelli¨ber 25 Simulationsl¨ tenverf¨ ugbarkeit, Low-cost MEMS IMU.

einer FOG IMU basierendem System bei Verwendung eines Sigma-Point-Kalman-Filters geringf¨ ugig schneller zu konvergieren. Der wesentliche Vorteil eines Tightly Coupled Systems gegen¨ uber einem Loosely Coupled System besteht in der F¨ ahigkeit, bei weniger als vier sichtbaren Satelliten eine – wenn auch eingeschr¨ ankte – St¨ utzung der Inertialnavigation realisieren zu k¨onnen. Um das Verhalten der Filter in einem solchen Szenario beurteilen zu k¨onnen, wurden Simulationen durchgef¨ uhrt, bei denen in der Zeit von 150 Sekunden bis 300 Sekunden nur drei Satelliten, von 300 Sekunden bis 350 Sekunden nur zwei Satelliten, von 450 Sekunden bis 500 Sekunden nur ein Satellit und schließlich von 650 Sekunden bis 700 Sekunden kein Satellit sichtbar war. Außerhalb dieser Zeitr¨aume standen sechs bis sieben Satelliten zur Verf¨ ugung. Die Positions-, Geschwindigkeits- und Lagefehler in diesem Szenario bei Verwendung einer Low-cost MEMS IMU sind in Abb. 8.27 – 8.29 dargestellt. Wie erwartet wachsen die Navigationsfehler bei reduzierter Satellitenverf¨ ugbarkeit zwar mit der Zeit an, dies jedoch deutlich langsamer als in dem letzten Zeitintervall, in dem keinerlei Satelliten sichtbar waren. Das relativ schnelle Anwachsen der Navigationsfehler ist auf die geringe G¨ ute der MEMS IMU zur¨ uckzuf¨ uhren. Wie bei den Simulationen mit vollst¨ andigen Satellitenkonstellationen liefern hier das linearisierte Kalman-Filter und das Sigma-Point-Kalman-Filter praktisch identische Ergebnisse. Diese Simulationsergebnisse konnten anhand realer Sensordaten verifiziert werden. Dazu wurden die in einer Testfahrt aufgezeichneten Daten herangezogen, die bereits in Abschnitt 8.2.4 beim Vergleich von Loosely und Tightly Coupled Systemen verwen-

248

8 Anwendungsbeispiel GPS/INS-Integration

120

e u

Positionsfehler (m)

100

EKF SPKF

80 60 40 e 20

u

u

e

e

u

e

u

u

e

u

e

u

0 0

100

200

300

400 Zeit (s)

500

600

700

800

Abbildung 8.27: RMS-Werte der Positionsfehler gemittelt u aufe, einge¨ber 25 Simulationsl¨ schr¨ ankte Satellitenverf¨ ugbarkeit, Low-cost MEMS IMU.

Geschwindigkeitsfehler (m/s)

6

e u

5

EKF SPKF

4 3 e

2

u

1 u

0 0

e 100

u 200

e

u 300

e

u

400 Zeit (s)

u 500

e 600

e 700

u 800

Abbildung 8.28: RMS-Werte der Geschwindigkeitsfehler gemittelt u aufe, ¨ber 25 Simulationsl¨ eingeschr¨ ankte Satellitenverf¨ ugbarkeit, Low-cost MEMS IMU.

8.6 Sigma-Point-Kalman-Filter

249

0.05

0.04 Lagefehler (rad)

e u

EKF SPKF

0.03

0.02 u e

0.01

u

e u

e e

u

u

u

e e

u

0 0

100

200

300

400 Zeit (s)

500

600

700

800

Abbildung 8.29: RMS-Werte der Lagefehler gemittelt u aufe, eingeschr¨ ank¨ber 25 Simulationsl¨ te Satellitenverf¨ ugbarkeit, Low-cost MEMS IMU.

det wurden. Die Positionsl¨ osungen beider Filter sind in Abb. 8.30 dargestellt, diese sind praktisch identisch. Gleiches gilt f¨ ur die Geschwindigkeits-, Lage- und Bias-Sch¨atzungen, auf deren Darstellung hier verzichtet wurde. Aus diesen Ergebnissen kann geschlossen werden, das f¨ ur realistische Szenarien durch Einsatz eines Sigma-Point-Kalman-Filters nicht mit einer Verbesserung der Navigationsergebnisse zu rechnen ist. Dass es dennoch prinzipielle Unterschiede zwischen den beiden Filtertypen gibt, zeigt sich, wenn z.B. unrealistisch große Anfangspositionsfehler angenommen werden. Diese Situation ist am Beispiel eines auf einer MEMS IMU basierenden Systems in Abb. 8.31 dargestellt. Man erkennt, dass das erweiterte Kalman-Filter deutlich langsamer konvergiert und auch nach l¨angerer Zeit noch nicht die Genauigkeit des Sigma-Point-Kalman-Filters erreicht. Die Ursache hierf¨ ur ist jedoch nicht darin zu sehen, dass ein linearisiertes oder erweitertes Kalman-Filter in diesem Szenario grunds¨ atzlich schlechter geeignet ist – lediglich die getroffenen Designentscheidungen sind hier ungeeignet. Dazu z¨ ahlt vor allem die Wahl einer Mechanisierung in Koordinaten des Navigationskoordinatensystems; ist die Position massiv fehlerhaft, so ergeben sich bei der Umrechnung der Satellitenpositionen in das Navigationskoordinatensystem zus¨atzliche Fehler. Abhilfe k¨ onnte hier eine Mechanisierung in Koordinaten des erdfesten Koordinatensystems schaffen. Dies wird in Abb. 8.32 illustriert, die Simulationsergebnisse einer Positionsbestimmung anhand von Pseudorange-Messungen zeigt. Die Anfangspositionsfehler betrugen 60 km.

250

8 Anwendungsbeispiel GPS/INS-Integration

49.016

Breitengrad (◦ )

49.015

b r

Standard KF Sigma Point KF

rbbr br rb

49.014 49.013 49.012 49.011

br b br b r b r r br b b r rr b br b

br r r b r b r b r bb r b r

8.412

r

b

rb

49.01 49.009 8.41

r b r

8.414

8.416 8.418 L¨angengrad (◦ )

b

r

r

b

8.42

br b r b br br

8.422

8.424

Abbildung 8.30: Positionsl¨ osung eines linearisierten und eines Sigma-Point-Kalman Filters bei Verarbeitung realer Sensordaten.

100 EKF SPKF

Positionsfehler (m)

80

e u

60

40 e 20

e

u

u

20

40

e u

e u

e u

60

80

100

0 0

Zeit (s) Abbildung 8.31: RMS-Werte der Positionsfehler gemittelt u aufe bei unrea¨ber 25 Simulationsl¨ listisch großen Anfangspositionsfehlern von 30 km, gute Satellitenverf¨ ugbarkeit, MEMS IMU.

8.6 Sigma-Point-Kalman-Filter

251

180 EKF Nav.-Ks. SPKF ECEF-Ks. IEKF ECEF-Ks.

160 Positionsfehler (m)

140

e u ♦

e

120 100 80

e

60

e

40 20

♦

0 0

u ♦ 5

e

e

e

u ♦ u ♦ u ♦ u ♦ u ♦ 10 15 20 25 Zeit (s)

e

e

e e u ♦ u♦ u ♦ u ♦ 30 35 40

e u 45

Abbildung 8.32: RMS-Werte der Positionsfehler bei einer Positionsbestimmung anhand von Pseudoranges, 100 Simulationsl¨ aufe, Anfangspositionsfehler 60 km, gute Satellitenverf¨ ugbarkeit.

Verglichen werden ein Sigma-Point-Kalman-Filter, das die Position im erfesten Koordinatensystem bestimmt, ein erweitertes Kalman-Filter, das in Navigationskoordinaten arbeitet, und ein so genanntes iteriertes erweitertes Kalman-Filter, das ebenfalls die Position im erdfesten Koordinatensystem sch¨atzt. Ein iteriertes Kalman-Filter ist dadurch gekennzeichnet, das im Messschritt zun¨achst wie gew¨ohnlich die Gewichtungsmatrix Kk berechnet und anhand der vorliegenden Messwerte die Zustandssch¨atzung angepasst wird. Anschließend erfolgt aber nicht die Anpassung der Kovarianzmatrix des Sch¨ atzfehlers, stattdessen wird um die verbesserte Zustandssch¨atzung erneut linearisiert, eine verbesserte Messmatrix und eine verbesserte Gewichtsmatrix bestimmt und damit eine neue, verbesserte Zustandssch¨ atzung ermittelt. Dieser Vorgang wird einige Male wiederholt, bevor am Ende schließlich die Anpassung der Kovarianzmatrix des Sch¨ atzfehlers den Messschritt abschließt. Betrachtet man die Simulationsergebnisse in Abb. 8.32, so erkennt man, dass auch hier die großen Anfangspositionsfehler bei einem Error State Space Kalman-Filter in Navigationskoordinaten zu einer sehr langsamen Konvergenz und bleibenden Abweichungen f¨ uhren. Das Sigma-Point-Kalman-Filter hingegen kommt mit diesem Szenario sehr gut zurecht. Interessanterweise zeigt das iterierte Kalman-Filter eine noch schnellere Konvergenz als das Sigma-Point-Kalman-Filter. Ebenso relevant wie die Unsicherheit der Zustandssch¨atzung und die Zeitschrittweite ist also die Wahl einer geeigneten Filter-Implementierung. Der Entwurf eines SigmaPoint-Kalman-Filters erfordert hierbei sicherlich weniger Erfahrung, was ein Vorteil ist. Mit einem an das erwartete Szenario angepassten Kalman-Filter hingegen kann in vielen F¨ allen mit deutlich weniger Rechenzeit die gleiche Genauigkeit und Integrit¨at, d.h.

252

8 Anwendungsbeispiel GPS/INS-Integration

¨ Ubereinstimmung zwischen der Kovarianzmatrix der Sch¨atzfehler des Filters und den tats¨ achlich vorliegenden Sch¨ atzfehlern, erzielt werden. Es gibt jedoch auch Anwendungsf¨ alle, bei denen ein Sigma-Point-Kalman-Filter deutlich bessere Ergebnisse liefert als ein erweitertes oder linearisiertes Kalman-Filter, ein Beispiel hierf¨ ur ist die Objektverfolgung mittels Radar.

8.6.3

Theoretischer Vergleich mit Objektverfolgung

Die Messgleichung einer Radar-Abstandsmessung ist gegeben durch  y˜r = h(x, y, z, vr ) = x2 + y 2 + z 2 + vr = r + vr ,

(8.180)

wobei (x, y, z)T den Vektor bezeichnet, der vom Radar zum Ziel weist. Diese Gleichung ist, abgesehen von dem linear eingehenden Uhrenfehler, identisch zu einer PseudorangeMessgleichung. Mehrere Autoren berichten bei der Objektverfolgung mittels Radar von besseren Ergebnissen beim Einsatz eines Sigma-Point-Kalman-Filters anstelle eines erweiterten Kalman-Filters, w¨ ahrend in den vorigen Abschnitten gezeigt wurde, dass bei GPS/INS-Systemen nicht mit einer Leistungssteigerung zu rechnen ist. Die Ursache hierf¨ ur ist, dass sich bei einem Objektverfolgungsszenario die Nichtlinearit¨at der Messgleichung und die Unsicherheiten der Zustandssch¨atzung in einem Bereich bewegen k¨onnen, in dem die Terme h¨ oherer Ordnung, die von einem erweiterten Kalman-Filter nicht erfasst werden, eine Rolle spielen – es muss also eine ¨ahnliche Situation vorliegen wie bei der im vorigen Abschnitt beschrieben GPS/INS-Integration mit unrealistisch großen Anfangspositionsfehlern, siehe Abb. 8.31. Diese Vermutung soll im Folgenden durch die explizite Berechnung des Terms zweiter Ordnung f¨ ur in beiden Anwendungsbereichen typische Zahlenwerte best¨ atigt werden. F¨ ur die Messgleichung (8.180) ergibt sich der Term zweiter Ordnung, siehe Abschnitt 6.4.3, zu

∇ P∇ 2! T



1 ∂ h(x) = 2! ∂x =

∂ ∂ ∂y ∂z



⎛

·

∂ ⎞ ∂x ∂ ⎠ P ⎝ ∂y h(x) ∂ ∂z 2

∂ 2 h(x) 1 ∂ h(x) 1 ∂ 2 h(x) 1 P11 + P22 + P33 ) 2 2 2 ∂x 2 ∂y 2 ∂z 2 ∂ 2 h(x) ∂ 2 h(x) ∂ 2 h(x) +P12 + P23 + P13 . ∂x∂y ∂y∂z ∂x∂z

(8.181)

F¨ ur die darin auftretenden Ableitungen erh¨alt man ∂ 2 h(x) 1 x2 = − 3 , ∂x2 r r 2 xy ∂ h(x) = − 3 , ∂x∂y r

∂ 2 h(x) 1 y 2 = − 3 , ∂y 2 r r 2 yz ∂ h(x) =− 3 , ∂y∂z r

∂ 2 h(x) 1 z 2 = − 3 , ∂z 2 r r 2 xz ∂ h(x) = − 3 . ∂x∂z r

(8.182) (8.183)

8.6 Sigma-Point-Kalman-Filter

253

Bei jedem dieser Terme steht der Abstand bzw. die dritte Potenz des Abstandes im Nenner. Bei einer Pseudorange-Messung ist das der Abstand zwischen GPS-Antenne und Satellit, dieser betr¨ agt ungef¨ ahr 20 000 km. Der Abstand zwischen Radar und Ziel ist bei der Objektverfolgung nat¨ urlich wesentlich kleiner. Gleichzeitig erkennt man an der Gestalt dieser Ableitungen, dass die Terme vierter oder sechster Ordnung deutlich kleiner sein m¨ ussen als der hier betrachtete Term zweiter Ordnung. Setzt man als einen typischen Abstandsvektor zwischen GPS-Antenne und Satellit   x = 7.0e6 m 9.4e6 m 1.7e7 m (8.184) an, so erh¨ alt man bei einer Positionsunsicherheit von zehn Metern, ⎞ 0 0 100m2 0 ⎠ , 100m2 P=⎝ 0 0 0 100m2 ⎛

(8.185)

als Zahlenwert des Terms zweiter Ordnung

∇T P∇ 2!



h(x) = 5.9 × 10−6 m .

(8.186)

Bei der Objektverfolgung ist der Abstand zwischen Radar und Zielobjekt typischerweise kleiner als zweihundert Kilometer, die Positionsunsicherheit ist in Richtung des Zielobjektes sehr viel kleiner als senkrecht dazu. Setzt man hier nun als Zahlenwerte besipielsweise   x = 7.0e4 m 0 m 0 m (8.187) und ⎞ 0 0 100m2 ⎠ , 4.0e5m2 0 P=⎝ 0 0 0 4.0e5m2 ⎛

(8.188)

so ergibt sich f¨ ur den Term zweiter Ordnung

∇T P∇ 2!

h(x) = 5.7 m .

(8.189)

Als Genauigkeit einer Radar-Abstandsmessung k¨onnen zwei Meter angenommen werden. Der Fehler, der bei der Vernachl¨ assigung des Terms zweiter Ordnung resultiert, ist also deutlich gr¨ oßer als die Messunsicherheit. Bei der GPS-Pseudorange-Messung liegt die Messgenauigkeit selbst bei Verwendung von DGPS-Korrekturen im Bereich einiger Dezimeter, der hier bei der Vernachl¨ assigung des Terms zweiter Ordnung resultieren¨ de Fehler von einigen Mikrometern ist also vollkommen irrelevant. Diese Uberlegungen best¨ atigen daher die Ergebnisse der numerischen Simulationen aus Abschnitt 8.6.2.

254

8 Anwendungsbeispiel GPS/INS-Integration

Auch bei der Objektverfolgung mittels Radar k¨onnen Szenarien auftreten, in denen ein erweitertes Kalman-Filter eine vergleichbare Genauigkeit erreicht wie ein SigmaPoint-Kalman-Filter; dies trifft vor allem bei einer guten Winkelaufl¨osung des Radars zu.9 Als interessante Alternative zum Einsatz eines Sigma-Point-Kalman-Filters bei allen Szenarien ist der BLUE-Filter zu nennen, BLUE steht f¨ ur Best Linear Unbiased ” Estimator“. Eine Beschreibung dieses Filtertyps ist z.B. in [132] zu finden.

8.7

Fixed-Interval Smoother

Unter der Voraussetzung, dass die ben¨ otigte Rechenleistung zur Verf¨ ugung gestellt werden kann, sind die bisher vorgestellten Algorithmen in der Lage, in Echtzeit eine Navigationsl¨ osung zu liefern. Ist Echtzeitf¨ ahigkeit kein Kriterium, kann anstelle eines Filters ein Fixed-Interval Smoother10 eingesetzt werden. Hierzu werden alle Navigationssensordaten aufgezeichnet und anschließend offline prozessiert. Dabei kann, verglichen mit der Echtzeit-L¨ osung eines Filters, eine unter Umst¨anden deutliche Genauigkeitssteigerung erzielt werden.

8.7.1

Gleichungen des RTS Smoothers

Prinzipiell werden drei Arten von Smoothern unterschieden, Fixed-Lag, Fixed-Point und Fixed-Interval Smoother. Die Zustandssch¨ atzung eines Filters zum Zeitpunkt k basiert auf allen bis zum Zeitpunkt k vorhandenen Messwerten:   ˜k , y˜k−1 , . . . , y˜0 = E  x |  y xˆ+ k k

(8.190)

Ein Fixed-Lag Smoother hingegen betrachtet zur Berechnung des Sch¨atzwertes zum Zeitpunkt k alle Messwerte bis zum Zeitpunkt k + m, der Sch¨atzwert des Fixed-Lag Smoothers kann daher formuliert werden als   xˆs,k = E xk |y˜k+m , y˜k+m−1 , . . . , y˜0 . (8.191) Dieser Typ Smoother kann so implementiert werden, dass Sch¨atzwerte beinahe in Echtzeit, eben mit m Zeitschritten Verz¨ ogerung, zur Verf¨ ugung stehen. Ein Fixed-Point Smoother hat das Ziel, zu einem festen Zeitpunkt j einen Sch¨atzwert zu liefern, der dann bei Eintreffen weiterer Messwerte st¨andig verbessert wird. Dieser Sch¨ atzwert ist gegeben durch   (8.192) xˆs,j = E xj |y˜k , y˜k−1 , . . . , y˜0 , k > j. 9 Neben der diskutierten Abstandsmessung liegen bei der Objektverfolgung mittels Radar auch noch nichtlineare Winkelmessungen vor. 10 Smoother werden auch als Gl¨ attungsfilter bezeichnet.

8.7 Fixed-Interval Smoother

255

Ein Fixed-Interval Smoother schließlich liefert den Sch¨atzwert   xˆs,k = E xk |y˜N , y˜N −1 , . . . , y˜0 ,

(8.193)

wobei N die Anzahl aller jemals zur Verf¨ ugung stehenden Messwerte ist. Die Genauigkeitssteigerung gegen¨ uber einem Filter resultiert daraus, dass auch die nach dem Zeitpunkt k zur Verf¨ ugung stehenden Messwerte Informationen bez¨ uglich des Systemzustandes zum Zeitpunkt k enthalten; diese Informationen werden vom Smoother genutzt, bei einem Filter bleiben sie aufgrund der geforderten Echtzeitf¨ahigkeit zwangsweise unber¨ ucksichtigt. Ein Fixed-Interval Smoother kann auf unterschiedliche Weisen realisiert werden, am h¨ aufigsten kommt wohl der Rauch-Tung-Striebel (RTS) Smoother [102] zum Einsatz. Eine ausf¨ uhrliche Herleitung ist in [114] zu finden, hier soll nur kurz die Vorgehensweise beschrieben werden: Es sollen Zustandssch¨atzungen des linearen Systems Gl. (6.1) ermittelt werden, das Messmodell ist durch Gl. (6.3) gegeben. Zun¨achst werden nun alle aufgezeichneten Messwerte mit einem herk¨ommlichen Kalman-Filter, Gl. (6.17) – (6.19), (6.24),(6.26) verarbeitet:  −1 − T T Kk = P− k Hk Hk Pk Hk + Rk  ˆ − + Kk y˜k − Hk x ˆ + = x ˆ− x k k k − − − P+ k = Pk − Kk Hk Pk = (I − Kk Hk )Pk ˆ − = Φk xˆ + + Bk uk x

k+1 P− k+1

=

k + T Φk Pk Φk

+ Gk Qk GTk

ˆ + und xˆ − , Dabei werden in jedem Zeitschritt die a posteriori und a priori Sch¨atzwerte, x k k + − sowie die zugeh¨ origen Kovarianzmatrizen, Pk und Pk , gespeichert. Anschließend wird r¨ uckw¨ arts in der Zeit, beginnend mit dem vorletzten Zeitschritt, die Zustandssch¨atzung des Smoothers, xˆs,k , anhand folgender Gleichungen berechnet:  − −1 T Ks,k = P+ k Φk Pk+1  ˆs,k+1 − x ˆs,k = x ˆ + + Ks,k x ˆ− , x k k+1

(8.194) k = N − 1, N − 2, . . . , 1, 0 (8.195)

Die Initialisierung erfolgt bei k = N gem¨ aß ˆ+ xˆs,N = x N

(8.196)

Die Kovarianzmatrix des Sch¨ atzfehlers des Smoothers wird f¨ ur den Ablauf des Algorithmus nicht ben¨ otigt; sie kann ebenfalls r¨ uckw¨arts in der Zeit anhand von   T − Ps,k = P+ k + Ks,k Ps,k − Pk+1 Ks,k , k = N − 1, N − 2, . . . , 1, 0 (8.197)

256

8 Anwendungsbeispiel GPS/INS-Integration

berechnet werden, die Initialisierung erfolgt ebenfalls bei k = N mit Ps,N = P+ N .

(8.198)

Der RTS Smoother kann analog der beim EKF angewandten Vorgehensweise auf nichtlineare System- und Messmodelle erweitert werden, auch eine Error-State-SpaceFormulierung ist m¨ oglich. Diese Aspekte sollen hier jedoch nicht weiter betrachtet werden.

8.7.2

Simulationsergebnisse

Die Verwendung eines RTS Smoothers bei der Prozessierung von aufgezeichneten Navigationssensordaten bringt Vorteile mit sich: Bei einem integrierten GPS/INS-System dauert es in der Regel einige Minuten, bis die Sch¨atzung der Beschleunigungsmesserund Drehratensensorbiase konvergiert ist. Unter der Voraussetzung, dass diese Biase zeitlich konstant sind, kann ein Smoother auch in dieser Phase eine Navigationsl¨osung basierend auf einer bereits konvergierten Bias-Sch¨atzung liefern, was sich in der Regel positiv auf die Lagegenauigkeit auswirkt. Von besonderem Vorteil ist der Einsatz eines Smoothers bei GPS-Ausf¨ allen. Ohne die Verf¨ ugbarkeit von GPS-Messungen wachsen bei Verwendung eines Filters die Navigationsfehler entsprechend der Inertialnavigationsperformance an; stehen nach einem GPS-Ausfall wieder GPS-Messungen zur Verf¨ ugung, erkennt man, wie die Navigationsfehler angewachsen sind – ein Smoother kann aufgrund

60 RTS-Smoother Filter

Positionsfehler (m)

50

b r

r 40 30

r

20 r 10 r b b r b

r rbrbrbrbrbrbrbr

r brbrbrbrbrbrbrbrbrbrbrbr brb b b b brbrbr

brb

0 0

100

200

300

400 Zeit (s)

500

600

700

800

Abbildung 8.33: Positionsfehler der online L¨ osung eines Filters und der durch post-processing ermittelten L¨ osung eines RTS Smoothers.

8.7 Fixed-Interval Smoother

257

dieser Information eine genauere Navigationsl¨osung f¨ ur den Zeitraum des GPS-Ausfalls berechnen. Dieser Mechanismus wird in Abb. 8.33 illustriert. Hierzu wurden GPS- und Inertialsensordaten simuliert, wobei zwischen 300 und 400 Sekunden ein vollst¨andiger GPS-Ausfall angenommen wurde. Abb. 8.33 zeigt die Positionsfehler, die sich bei Verwendung eines Filters und eines RTS Smoothers ergeben. Ein Anwachsen der Positionsfehler w¨ ahrend des GPS-Ausfalls ist bei dem RTS Smoother praktisch noch nicht feststellbar, w¨ ahrend bei der Echtzeitl¨ osung des Filters ein deutlicher Anstieg der Positionsfehler zu verzeichnen ist. Bei l¨ angeren GPS-Ausf¨allen w¨ urden nat¨ urlich auch bei dem RTS Smoother die Navigationsfehler merklich anwachsen, wobei die gr¨oßten Fehler in der Mitte des Ausfallsintervalls zu erwarten sind. Ergebnisse einer Testfahrt, die das Simulationsergebnis Abb. 8.33 best¨atigen, sind in [93] zu finden. Dort wurde auch gezeigt, dass bei Verarbeitung von Tr¨agerphasenmesswerten durch Einsatz eines Smoothers ein gr¨ oßerer Prozentsatz der ganzzahligen Tr¨agerphasenmehrdeutigkeitswerte gefunden werden kann.

9

Anwendungsbeispiel Transfer Alignment

Unter Transfer Alignment versteht man die Bestimmung einer initialen Navigationsl¨ osung eines Navigationssystems unter Verwendung von Informationen eines anderen Navigationssystems. Das zu initialisierende Navigationssystem wird als Slave bezeichnet. Das Navigationssystem, das die zur Initialisierung verwendeten Informationen zur Verf¨ ugung stellt, wird Master oder Host genannt. Die Notwendigkeit, ein Navigationssystem mit Hilfe eines anderen zu initialisieren, tritt bei einer Reihe von Anwendungen auf. Beispiele hierf¨ ur w¨ aren die Initialisierung des Navigationssystems eines Flugk¨orpers, der an einem Flugzeug montiert ist, oder die Bestimmung der Ausrichtung eines flugzeugetragenen SAR-Sensors, die sehr genau erfolgen muss, damit bei der Georeferenzierung der vom SAR-Sensor gelieferten Bilder die Fehler m¨oglichst gering ausfallen. Auch andere an einem Flugzeug montierte Sensoren ben¨otigen hochgenaue Lageinformationen, ein weiteres Beispiel w¨ are ein Warnsensor, der anfliegende Objekte detektiert und deren Position sch¨ atzt. Bei der Initialisierung des Slave ist die Bestimmung von Position und Geschwindigkeit meist unkritisch, die Initialisierung der Lage hingegen ist ein komplexes Problem. Meist werden als erster Schritt vom Slave die Lageinformationen des Masters u ¨ bernommen, dies wird auch als One-shot Alignment“ bezeichnet. Die dabei erzielte Genauigkeit ist ” in der Regel unzureichend, da sich die Ausrichtung des Slave-Systems zwangsl¨aufig von der Ausrichtung des Masters unterscheidet: Unter Umst¨anden sind die mechanischen Gegebenheiten, die die relative Orientierung von Master und Slave bestimmen, nur n¨ aherungsweise bekannt. H¨ aufig variiert diese relative Orientierung aber auch mit der Zeit, z.B. wenn das Slave System an einem Fl¨ ugel oder H¨ohenleitwerk befestigt ist, das unter Krafteinwirkung nat¨ urlich flexibel reagiert. Im Folgenden werden unterschiedliche Transfer-Alignment-Verfahren aufgezeigt und in numerischen Simulationen verglichen.

9.1

Konventionelle Transfer-Alignment-Verfahren

Bei einem konventionellen Transfer Alignment wird die Lage des Slave von einem Kalman-Filter gesch¨ atzt, das die Geschwindigkeit des Masters als Messung verarbeitet. Um den Einfluss von Vibrationen zu minimieren, wird manchmal auch das Integral der Geschwindigkeit des Masters als Messwert verwendet, siehe [106]. Dies soll jedoch, ebenso wie die Verwendung der zweifach integrierten Geschwindigkeit, nicht weiter betrachtet werden.

260

9 Anwendungsbeispiel Transfer Alignment

Das Systemmodell des bei einem solchen Transfer Alignment ben¨otigten Kalman-Filters kann identisch zu dem Systemmodell gew¨ahlt werden, das in Abschnitt 8.2.1 zur GPS/INS-Integration verwendet wurde. Wenn das Alignment Filter anschließend nicht als Navigationsfilter dient, werden nat¨ urlich keine Positionsfehler im Zustandsvektor ben¨ otigt und k¨ onnen eingespart werden. Bei dem zur Verarbeitung der Geschwindigkeit des Masters ben¨otigten Messmodell muss beachtet werden, dass sich die Geschwindigkeiten von Master und Slave bei vorliegenden Drehbewegungen unterscheiden. Das k¨ orperfeste Koordinatensystem des Masters wird durch einen Index bH gekennzeichnet, beim Slave wird die Indizierung bS verwendet. Der Zusammenhang der Geschwindigkeiten von Master und Slave lautet damit n n bS vebH = vebS + CnbS  ωebS × l bS ≈ v n + Cn  ω bS × l bS . ebS

bS

ibS

(9.1)

Der Vektor l bS weist hierbei vom Ursprung des k¨orperfesten Koordinatensystems des Masters zum Ursprung des k¨ orperfesten Koordinatensystems des Slaves. Die Fehler n,bS der gesch¨ ψ atzten Slave-Lage werden u ¨ ber die Gleichung ˆ nbS CnbS = (I + Ψn,bS )C

(9.2)

˜ bS unterscheiden sich definiert, die dem Slave zur Verf¨ ugung stehenden Drehraten ω ibS von den realen Drehraten aufgrund der Drehratensensorbiase bω,S ; das Rauschen der Drehratensensoren soll f¨ ur die Berechnung der Relativgeschwindigkeit vernachl¨assigt werden: bS bS ≈ω  ibS + bω,S ˜ ibS ω

Damit erh¨ alt man schließlich das Messmodell  n n ˜ bS − bω,S × l bS . ˆ n ω vebH = vebS + (I + Ψn,bS ) C bS ibS

(9.3)

(9.4)

Eine weitere Beschreibung dieses Alignment-Filters ist aufgrund der Parallelit¨at zu der in Abschnitt 8.2 beschriebenen GPS/INS-Integration hier nicht notwendig. Der Nachteil eines solchen konventionellen Transfer-Alignment-Verfahrens ist, dass zur vollen Beobachtbarkeit der Lage des Slave-Systems horizontale Beschleunigungen ben¨ otigt werden, das Flugzeug muss daher w¨ahrend des Alignments eine S-Kurve oder ¨ Ahnliches fliegen. Dieser Nachteil kann durch Einsatz eines Rapid-Transfer-AlignmentVerfahrens u uber hinaus eine schnellere Initialisierung der ¨ berwunden werden, das dar¨ Slave-Lage erlaubt.

9.2

Rapid Transfer Alignment

Rapid-Transfer-Alignment-Verfahren sind dadurch gekennzeichnet, dass neben der Geschwindigkeit des Masters auch noch die Lage des Masters als Messung im Alignment-

9.2 Rapid Transfer Alignment

261 CbS bH

CnbH

Master Slave

e CnbS

n d

Abbildung 9.1: K¨ orperfeste Koordinatensysteme und relative Orientierung von Master und Slave.

Filter verarbeitet wird. Dazu muss die relative Orientierung zwischen Master und Slave explizit gesch¨ atzt werden, der Zustandsvektor des Alignment-Filters muss daher um drei Lagefehler der gesch¨ atzten relativen Orientierung erweitert werden. Eine der ersten Beschreibungen eines solchen Rapid-Transfer-Alignment-Verfahrens ist in [62] zu finden, die Ergebnisse von Flugversuchen werden in [36],[37] und [113] beschrieben. F¨ ur die Herleitung der zur Verarbeitung der Lage des Masters ben¨otigten Messgleichung wird die Lage das Masters als perfekt angenommen, diese soll in Form der Richatzlich wird angenommen, dass sich die relative tungskosinusmatrix CnbH vorliegen. Zus¨ Orientierung von Master und Slave nicht ¨ andert, die Sch¨atzung des Slaves dieser relaˆ bH gegeben. Die eigentliche Lagemessung ist durch den tiven Orientierung ist durch C bS Unterschied zwischen der Lage des Masters CnbH und der erwarteten Lage des Masters ˆ n bestimmt, die auf der gesch¨ ˆ bH und der gesch¨atzten C atzten relativen Orientierung C bH bS ˆ bS des Slaves basiert. Die Zusammenh¨ange zwischen den beteiligten KoordiLage C n natensystemen sind in Abb. 9.1 veranschaulicht; genau genommen existieren in diesem Szenario zwei Navigationskoordinatensysteme, deren Urspr¨ unge mit den Urspr¨ ungen der k¨ orperfesten Koordinatensysteme von Master und Slave zusammenfallen. Zur besseren ¨ Ubersichtlichkeit ist in Abb. 9.1 nur ein Koordinatensystem mit den Koordinatenachsen Norden, Osten und Unten dargestellt. Das ist zul¨assig, da durch die Richtungskosinusmatrizen sowieso nur Drehnungen, nicht aber Translationen erfasst werden und somit die Positionen der Koordinatensystemurspr¨ unge irrelevant sind.

262

9 Anwendungsbeispiel Transfer Alignment

Der Unterschied zwischen der Lage des Masters und der vom Slave erwarteten Lage des Masters kann wie folgt beschrieben werden: ˆ nbH CnbH = (I + Ψn,bH )C

(9.5)

n,bH gegeben, Die als Messung verarbeiteten Gr¨ oßen sind durch die drei Winkelfehler ψ die in der kreuzproduktbildenden Matrix Ψn,bH enthalten sind. Um diese berechnen zu k¨ onnen, muss Gl. (9.5) nach dieser Matrix aufgel¨ost werden. Man erh¨alt ˆ bH = (I + Ψn,bH ) CnbH C n ˆ bH − I Ψn,bH = Cn C =

bH n ˆ bH C ˆ bS CnbH C bS n

−I.

(9.6)

Das Messmodell beschreibt die Beziehung zwischen den so gewonnenen Winkelfehlern n,bS der Slave-Lage und der gesch¨atzten relativen Orientierung n,bH und den Fehlern ψ ψ  ψbS,bH von Master und Slave. Mit Gl. (9.2) und dem Zusammenhang ˆ bS CbS bH = (I + ΨbS,bH )CbH

(9.7)

kann man schreiben: CnbH = CnbS CbS bH ˆ n (I + ΨbS,bH )C ˆ bS CnbH = (I + Ψn,bS )C bS bH n ˆ bH n ˆ bH ˆ ˆ CbH Cn = (I + Ψn,bS )CbS (I + ΨbS,bH )CbS bH Cn ˆ bH ˆn ˆ bS ˆ bH CnbH C n − I = (I + Ψn,bS )CbS (I + ΨbS,bH )CbH Cn − I

(9.8)

Durch Vergleich mit Gl. (9.6) erkennt man, dass die linke Seite von Gl. (9.8) gerade der kreuzproduktbildenden Matrix der als Messung verarbeiteten Winkelfehler entspricht. Damit ergibt sich ˆ n (I + ΨbS,bH )C ˆ bS − I Ψn,bH = (I + Ψn,bS )C bS n  n n n ˆ ΨbS,bH + Ψn,bS C ˆ +C ˆ + Ψn,bS C ˆ n ΨbS,bH C ˆ bS − I = C bS bS bS bS n ˆ bS + Ψn,bS + Ψn,bS C ˆ n ΨbS,bH C ˆ bS . ˆ n ΨbS,bH C =C bS n bS n

(9.9)

Da Winkelfehler als klein angenommen werden k¨onnen, ist das Produkt von Winkelfehlern vernachl¨ assigbar, man erh¨ alt ˆ n ΨbS,bH C ˆ bS + Ψn,bS . Ψn,bH = C bS n

(9.10)

¨ Durch Ubergang zur vektoriellen Schreibweise kann man schließlich das Messmodell formulieren: n,bS + [Rauschen] bS,bH + ψ n,bH = C ˆ nbS ψ ψ

(9.11)

9.3 Effiziente Ber¨ ucksichtigung von Zeitkorrelationen

263

Wird neben der Geschwindigkeit des Masters zus¨atzlich mit Hilfe des Messmodells Gl. (9.11) die Lage des Masters als Messung vom Alignment-Filter verarbeitet, ist f¨ ur die vollst¨ andige Beobachtbarkeit der Slave-Lage ein so genanntes Wing-Rock-Man¨over ausreichend, es m¨ ussen also keine S-Kurven mehr geflogen werden. Unter Wing Rock versteht man ein kurzes Rollen in eine Schr¨ aglage und wieder zur¨ uck. Die unter Umst¨anden wenigen Sekunden, die ein Rapid Transfer Alignment dauert, gen¨ ugen oft nicht, um die Inertialsensoren des Slave zu kalibrieren. Stehen nach dem Alignment weiterhin St¨ utzinformationen z.B. durch einen GPS-Empf¨ anger zur Verf¨ ugung, kann diese Kalibration auch im Anschluss an das Alignment erfolgen, siehe [112]. Bei der Entwicklung eines Alignment-Verfahrens muss die Vibrationsumgebung, in der sich das Slave-System befindet, besonders beachtet werden. So wurden beispielsweise in [105] Positionsinformationen im Alignment-Filter verarbeitet, da sich der resultierende Algorithmus als robuster gegen¨ uber den dort vorliegenden Vibrationen herausgestellt hatte. Vibrationen k¨ onnen als rotatorische und translatorische Oszillationen betrachtet werden, die anhand der Inertialsensoren nicht korrekt aufgel¨ost werden. Diese Vibrationen stellen sich daher in den Inertialsensordaten als zus¨atzliches Rauschen dar, das bei der u ucksichtigung der Inertialsensordaten als Eingangsgr¨oßen im ¨ blichen Ber¨ Propagationsschritt des Filters als Systemrauschen in den Filtergleichungen erscheint. Da typischerweise die Eigenfrequenzen des Systems angeregt werden, konzentriert sich die Rauschleistung des vibrationsinduzierten Rauschens bei bestimmten Frequenzen, die von Anwendung zu Anwendung stark variieren k¨onnen. In [62] wird von vibrationsinduziertem Rauschen mit Frequenzen um die zehn Hertz berichtet, in [106] konzentiert sich die Rauschleistung bei f¨ unfzig Hertz nahe der halben Abtastfrequenz der dort eingesetzten Inertialsensorik. In [125] wurden bei der Analyse von Flugversuchsdaten Vibrationen um die achtzig Hertz festgestellt. Hierbei besteht immer die M¨oglichkeit, dass die eigentlichen Vibrationsfrequenzen h¨oher liegen und durch Aliasing in den festgestellten Frequenzbereich abgebildet werden. Auf jeden Fall handelt es sich bei dem vibrationsinduzierten Rauschen um zeitkorreliertes Rauschen. Werden diese Zeitkorrelationen ignoriert, kann das einen negativen Einfluss auf die Dauer und die Genauigkeit des Transfer Alignments haben. In [113] werden daher die Zeitkorrelationen des vibrationsinduzierten Rauschens durch eine Erweiterung des Zustandsvektors mit geeigneten Rauschprozessmodellen ber¨ ucksichtigt. Der Nachteil dieser in Abschnitt 6.5.1 beschriebenen Vorgehensweise zur Ber¨ ucksichtigung von Zeitkorrelationen besteht in einem deutlich gesteigerten Rechenaufwand, numerische Probleme k¨onnen ebenfalls auftreten. Daher wird im Folgenden ein Verfahren aufgezeigt, mit dem zeitkorreliertes Rauschen ber¨ ucksichtigt werden kann, ohne dass eine Erweiterung des Zustandsvektors des Filters notwendig ist.

9.3

Effiziente Beru ¨cksichtigung von Zeitkorrelationen

Um ein effizientes Vefahren zur Ber¨ ucksichtigung von Zeitkorrelationen entwickeln zu k¨ onnen, muss zun¨ achst der Einfluss von Zeitkorrelationen im Rahmen eines Filterproblems verstanden werden.

264

9 Anwendungsbeispiel Transfer Alignment

Propagation xk+1

Propagation xk+2

ˆk x

ˆk+1 x

Pxw = 0

w k

Pxv = 0 w  k+1

Zeitkorrelationen

vk

vk+1 Zeitkorrelationen

Abbildung 9.2: Kreuzkorrelationen zwischen Systemrauschen und Zustandsvektor (links) und zwischen Messrauschen und Zustandssch¨ atzung (rechts) als Folge von zeitkorreliertem Messund Systemrauschen.

Hierzu wird wie in Abschnitt 6.5.1 ein Systemmodell der Form k xk+1 = Φk xk + Gk w

(9.12)

betrachtet, das zeitkorrelierte Systemrauschen wird durch  k−1 +  ηk w  k = Cw

(9.13)

¨ beschrieben. Auf bekannte Eingangsgr¨ oßen wird aus Ubersichtsgr¨ unden verzichtet. Liegt nun Systemrauschen vor, so beeinflusst das Systemrauschen w  k den Systemzustand xk+1 . Ist das Systemrauschen zeitkorreliert, so geht w  k auch in das Systemrauussen aber auch w  k+1 und xk+1 schen zum n¨ achsten Zeitschritt, w  k+1 , ein. Damit m¨ korreliert sein, siehe Abb. 9.2. Diese Korrelationen werden in den u ¨ blichen KalmanFilter-Gleichungen nicht ber¨ ucksichtigt. Die Situation bei zeitkorreliertem Messrauschen gestaltet sich ¨ahnlich, es soll ein Messmodell der Form y˜k = Hk xk + vk

(9.14)

angenommen werden, das zeitkorrelierte Messrauschen wird durch vk = Dvk−1 +  µk

(9.15)

beschrieben. Wird zum Zeitpunkt k ein Messwert verarbeitet, so beeinflusst dessen Messrauschen vk ˆk . Diese wird zum n¨achsten Zeitschritt k + 1 propagiert. Ist das die Zustandssch¨ atzung x ˆk ebenMessrauschen vk mit dem Messrauschen vk+1 korreliert, so muss vk + 1 mit x falls korreliert sein, auch diese Zusammenh¨ange sind in Abb. 9.2 veranschaulicht. Die u ¨ blichen Kalman-Filter-Gleichungen setzen voraus, das keine Korrelationen zwischen Messrauschen und Zustandssch¨ atzung vorliegen.

9.3 Effiziente Ber¨ ucksichtigung von Zeitkorrelationen

265

Zeitkorreliertes Mess- und Systemrauschen l¨asst sich also ber¨ ucksichtigen, in dem die resultierenden Kreuzkorrelationen zwischen Messrauschen und Zustandssch¨atzung bzw. Systemrauschen und Zustandsvektor korrekt behandelt werden. Die entsprechenden Filtergleichungen werden im Folgenden hergeleitet.

9.3.1

Propagationsschritt

Im Propagationsschritt des Filters wird die Zustandssch¨atzung und die zugeh¨orige Kovarianzmatrix in der Zeit propagiert, Grundlage hierf¨ ur ist das Systemmodell Gl. (9.12). W¨ ahrend die Propagation des Systemzustandes wie u ussen bei der Pro¨ blich erfolgt, m¨ pagation der Kovarianzmatrix die Korrelationen des Systemzustandes mit dem Systemrauschen ber¨ ucksichtigt werden. Mit   ˆ+ w xk − x  kT k   ˆ + v T = E xk − x k k

 T =E w kw k

T = E vk vk

P+ xw,k = E

(9.16)

P+ xv,k

(9.17)

Pww,k Pvv,k

(9.18) (9.19)

ergibt sich P− k+1

  T ˆ− ˆ−  x = E xk+1 − x −  x k+1 k+1 k+1   T      ˆ + + Gk w = E Φk xk − xk+ + Gk w  k Φk xk − x  k k + + T T T T = Φk P+ k Φk + Φk Pxw,k Gk + Gk Pwx,k Φk + Gk Pww,k Gk .

(9.20)

Wenn sich durch die Propagation die Zustandssch¨atzung ¨andert, muss nat¨ urlich auch die Korrelation mit dem Mess- und Systemrauschen entsprechend angepasst werden. Die zugeh¨ origen Gleichungen lauten   T ˆ−  k+1 xk+1 − x k+1 w    T ˆ + + Gk w (C w   +  η ) = E Φk xk − x k k k+1 k

P− xw,k+1 = E

T T = Φk P+ xw,k C + Gk Pww,k C

(9.21)

und   ˆ − v T xk+1 − x k+1 k+1    T ˆ + + Gk w  + µ  ) (D v = E Φk xk − x k k k+1 k

P− xv,k+1 = E

T = Φk P+ xv,k D .

(9.22)

266

9 Anwendungsbeispiel Transfer Alignment

Hierbei wurde ausgenutzt, dass die weißen Rauschprozesse ηk+1 und µk+1 , die die zeitkorrelierten System- und Messrauschprozesse treiben, nicht mit dem Systemrauschen w k und dem Messrauschen vk korreliert sind. Die Kovarianz der zeitkorrelierten Rauschprozesse wird ebenfalls propagiert: 

T Pww,k+1 = CPww,k CT + E ηk+1 ηk+1 

Pvv,k+1 = DPvv,k DT + E µ  k+1 µ  Tk+1

(9.23) (9.24)

Sind die Rauschprozessparameter konstant, so kann die Kovarianz der zeitkorrelierten Rauschprozesse auch im Voraus berechnet werden, die Gleichungen (9.23) und (9.24) werden dann nicht ben¨ otigt.

9.3.2

Messwertverarbeitung

Grundlage der Messwertverarbeitung sind die bei Normalverteilungen g¨ ultigen Gleichungen (5.31) und (5.32) f¨ ur den bedingten Mittelwert und die bedingte Kovarianz. Um diese berechnen zu k¨ onnen, muss zun¨achst die Kovarianz der Messwertvorhersage und die Korrelation von Messwertvorhersage und Zustandsvektor bestimmt werden, man erh¨ alt   T yk − yˆk Pyy,k = E yk − yˆk    T ˆ − ) + vk Hk (xk − x ˆ − ) + vk = E Hk (xk − x k k − − T T = Hk P− k Hk + Hk Pxv,k + Pvx,k Hk + Pvv,k

(9.25)

und Pxy,k

  T ˆ−  ˆk = E xk − x − y  y k k   T ˆ − Hk (xk − x ˆ − ) + vk = E xk − x k k − T = P− k Hk + Pxv,k .

(9.26)

Mit der Gewichtsmatrix Kk = Pxy,k P−1 yy,k  −1  − T − − T T Hk P− = Pk Hk + P− xv,k k Hk + Hk Pxv,k + Pvx,k Hk + Pvv,k (9.27) ergibt sich f¨ ur die a posteriori Zustandssch¨atzung  ˆ− , ˆ − + Kk yk − Hk x xˆk+ = x k k

(9.28)

9.3 Effiziente Ber¨ ucksichtigung von Zeitkorrelationen

267

die zugeh¨ orige Kovarianzmatrix ist durch  T − − T − P = P − K H + P P+ k k k k k xv,k − = (I − Kk Hk ) P− k − Kk Pvx,k

(9.29)

gegeben. Genau wie beim Propagationsschritt m¨ ussen aufgrund der Anpassung der Zustandssch¨ atzung die Korrelationen mit Mess- und Systemrauschen aktualisiert werden. Man erh¨ alt   T ˆ+ w  = E  x −  x P+ k k xw,k k    ˆ − − Kk  ˆ− w = E xk − x yk − Hk x  kT k k    T ˆ − − Kk Hk xk + vk − Hk x ˆ− w  = E xk − x k k k     − − ˆ − Kk Hk xk − xˆk + vk w  kT =E xk − x k − = P− xw,k − Kk Hk Pxw,k

= (I − Kk Hk ) P− xw,k

(9.30)

sowie   ˆ + v T xk − x k k     ˆ − − Kk Hk xk − x ˆ − + vk v T = E xk − x k k k

P+ xv,k = E

− = P− xv,k − Kk Hk Pxv,k − Kk Pvv,k

= (I − Kk Hk ) P− xv,k − Kk Pvv,k .

(9.31)

Damit sind die modifizierten Filtergleichungen zur Ber¨ ucksichtigung von zeitkorreliertem Mess- und Systemrauschen vollst¨ andig bekannt.

9.3.3

Diskussion der Filtergleichungen

¨ Zur Ubersicht sollen hier die zu implementierenden Filtergleichungen nochmals zusammengestellt werden: Propagationsschritt − = Φk xk+ xk+1 + T + + T T T P− k+1 = Φk Pk Φk + Φk Pxw,k Gk + Gk Pwx,k Φk + Gk Pww,k Gk + T T P− xw,k+1 = Φk Pxw,k C + Gk Pww,k C + T P− xv,k+1 = Φk Pxv,k D .

268

9 Anwendungsbeispiel Transfer Alignment

Messschritt   −1 − − − T T T Kk = P− Hk P− k Hk + Pxv,k k Hk + Hk Pxv,k + Pvx,k Hk + Pvv,k  xˆk+ = xˆk− + Kk yk − Hk xˆk− − − P+ k = (I − Kk Hk ) Pk − Kk Pvx,k − P+ xw,k = (I − Kk Hk ) Pxw,k − P+ xv,k = (I − Kk Hk ) Pxv,k − Kk Pvv,k

Werden f¨ ur die Matrizen C und D Nullmatrizen angenommen, wird aus dem zeitkorrelierten Mess- und Systemrauschen weißes Rauschen. Man erkennt sofort, dass in obigen Filtergleichungen damit auch s¨ amtliche Korrelationsmatrizen Pxw und Pxv verschwinden, die hergeleiteten Filtergleichungen reduzieren sich zu den u ¨ blichen Kalman-FilterGleichungen. Dieses Verfahren zur Ber¨ ucksichtigung zeitkorrelierten Rauschens hat zwei entscheidende Vorteile gegen¨ uber einer Erweiterung des Zustandsvektors: Zum einen ist der Rechenaufwand deutlich geringer, zum anderen handelt es sich um ein modulares Verfahren: Soll untersucht werden, ob eine Ber¨ ucksichtigung der Zeitkorrelationen eine Verbesserung mit sich bringt und welche Ordnung der Rauschprozessmodelle akzeptable Ergeb¨ nisse liefert, sind bei einer Erweiterung des Zustandsvektors aufgrund der Anderung der Dimension des Zustandsvektors jeweils umfangreiche Modifikationen des Filteralgorithmus notwendig. Bei dem hier beschriebenen Verfahren hingegen k¨onnen einfach die zus¨ atzlichen Gleichungen und Terme implementiert werden, w¨ahrend der Kern des Filteralgorithmus, die u ¨ blichen Kalman-Filter-Gleichungen, unver¨andert bleibt. Abschließend sei darauf hingewiesen, dass es bei anderen Anwendungen als Transfer Alignment und GPS/INS-Integration m¨ oglich sein kann, bei einer Erweiterung des Zustandsvektors tats¨ achlich die Vektoren w  k und vk zu sch¨atzen. In diesem Fall profitiert das Filterergebnis bei Ber¨ ucksichtigung der Zeitkorrelationen nicht nur von der korrekten Berechnung der Kovarianzmatrix des Sch¨atzfehlers, sondern auch von dieser Sch¨atzung der Rauschterme. Da eine Sch¨ atzung der Rauschterme bei obigem Verfahren nicht stattfindet, kann bei solchen Problemstellungen die Erweiterung des Zustandsvektors unter Umst¨ anden bessere Ergebnisse liefern.

9.4

Numerische Simulation

Die bei Ber¨ ucksichtigung der Zeitkorrelationen des Systemrauschens m¨oglichen Verbesserungen wurden f¨ ur verschiedene Transfer-Alignment-Szenarien in numerischen Simulationen untersucht. Um diese Simulationen durchf¨ uhren zu k¨onnen, m¨ ussen unter anderem die Daten der Inertialsensoren von Master und Slave generiert werden. Dabei m¨ ussen deren unterschiedliche Positionen ber¨ ucksichtigt werden.

9.4 Numerische Simulation

269

 ibb ω ˙ ibb × l b ω ωibb × ωibb × l b

Slave Master

Abbildung 9.3: Einfluss von Rotationen auf die von der Inertialsensorik des Slave detektierten Beschleunigungen.

9.4.1

Erzeugung von Inertialsensordaten

Zur Erzeugung der Inertialsensordaten von Master und Slave kann im ersten Schritt davon ausgegangen werden, dass sich die relative Position und Orientierung von Master und Slave nicht ¨ andert; eventuelle rotatorische und translatorische Relativbewegungen z.B. aufgrund der Flexibilit¨ at eines Fl¨ ugels k¨onnen anschließend hinzugef¨ ugt werden. W¨ ahrend unabh¨ angig von der Relativposition von Master und Slave identische Drehraten vorliegen, unterscheiden sich die gemessenen Beschleunigungen je nach Man¨over unter Umst¨ anden deutlich. Exemplarisch soll hier gezeigt werden, welche Beschleunigungen sich am Ort des Slaves in Abh¨ angigkeit von den Beschleunigungen und Rotationen eines Referenzkoordinatensystems ergeben. Dieses Referenzkoordinatensystem soll die gleiche Lage besitzten wie das k¨ orperfeste Koordinatensystem des Slaves und durch den Index b gekennzeichnet sein. Im Inertialkoordinatensystem l¨ asst sich folgender Zusammenhang formulieren: i = rbi + Cibl b rbS

(9.32)

i Hierbei bezeichnet rbS den Vektor vom Ursprung des Inertialkoordinatensystems zum i ist der Vektor zum UrUrsprung des k¨ orperfesten Koordinatensystems des Slaves, rbS b  sprung des Referenzkoordinatensystems und l ist der Vektor, der vom Ursprung des Referenzkoordinatensystems zum Ursprung des Slave-Koordinatensystems weist.

Die im k¨ orperfesten Koordinatensystem des Slaves gemessene Beschleunigung l¨asst sich durch Berechnung der zweifachen Ableitung dieses Zusammenhanges berechnen, man

270

9 Anwendungsbeispiel Transfer Alignment

H¨ ohe (m) Wing Rock 800 S-Kurve 600 S&L 400 200 0 45.2

S&L Kurve

45.1 Breite (◦ ) 45 44.9

10.2 9.8 10 9.6 9.4 9 9.2 L¨ange (◦ ) 8.6 8.8

Abbildung 9.4: Flugtrajektorie, die in den Simulationen verwendet wurde.

erh¨ alt i ˙ i l b r˙bS = r˙bi + C b i i vibS = vib + Cib Ωbibl b i i ˙ b b ˙ i Ωb l b + Ci Ω v˙ ibS = v˙ ib +C b ib b ib l ˙ b l b ai = ai + Ci Ωb Ωb l b + Ci Ω ibS

ib

b

ib

ib

b

ib

˙ b l b abibS = abib + Ωbib Ωbibl b + Ω ib abibS = abib +  ωibb ×  ωibb × l b + ω˙ ibb × l b .

(9.33)

Die Terme, in denen sich die in Referenzkoordinatensystem und Slave anfallenden Beschleunigungen unterscheiden, sind in Abb. 9.3 veranschaulicht.

9.4.2

Ergebnisse

Zur Erzeugung der dem Slave zur Verf¨ ugung gestellten Lage- und Geschwindigkeitsinformationen wurde ein Master-GPS/INS-System basierend auf einer Inertial Grade IMU simuliert. Die Verwendung eines GPS/INS-Systems als Referenzsystem kann zu Problemen f¨ uhren, wenn nach einem GPS-Ausfall Spr¨ unge in den Navigationsinformationen des Masters auftreten, siehe [40]. Daher wurde f¨ ur die hier durchgef¨ uhrten Simulationen von einer kontinuierlichen GPS-Verf¨ ugbarkeit ausgegangen. Eine Beschreibung von Verfahren zur Ber¨ ucksichtigung von GPS-Ausf¨allen in einem solchen Transfer-AlignmentSzenario ist in [44] zu finden. Die in den Simulationen verwendete Flugtrajektorie ist in Abb. 9.4 dargestellt. Zu Beginn wurde f¨ ur f¨ unfundzwanzig Sekunden geradeaus geflogen ( straight & level“, S & ” L), dann folgte ein Wing-Rock-Man¨ over. Dieses Man¨over dauerte zwei Sekunden, der maximale Rollwinkel betrug dabei sechzig Grad. Anschließend folgten f¨ unfzig Sekunden

9.4 Numerische Simulation

271

0.02

Lagefehler (rad)

0.015

u

e u

RTXA, Zeitkorr. ber¨ ucksichtigt RTXA, Zeitkorr. unber¨ ucksichtigt TXA, Zeitkorr. unber¨ ucksichtigt

♦ e u

♦ ♦

♦ 0.01

♦ ♦

♦

♦ 0.005

0 ♦e 0

♦

♦

♦

♦ e u e u e u e u e u e u e u e u e u e u♦ e 100

200

300 Zeit (s)

400

500

600

◦

Abbildung 9.5: Gesamtlagefehler des Slaves, Datenrate 0.2 Hz, 10 /h Slave IMU.

Geradeausflug, bevor hundertdreisig Sekunden lang Kurven geflogen wurden, an die sich f¨ unf Minuten Geradeausflug anschlossen. Das abschließende Man¨over war eine scharfe Neunzig-Grad-Kurve. ◦

◦

F¨ ur die Inertialsensorik des Slave wurde eine 10 /h und eine 2 /h IMU angenommen, die zur Erzeugung der Sensordaten verwendeten Rauschprozessmodelle wurden anhand von Flugversuchsdaten ermittelt. Neben der Sensorg¨ ute ist ein weiterer Faktor von entscheidender Bedeutung, n¨ amlich die Datenrate, mit der der Master Geschwindigkeits- und Lageinformationen zur Verf¨ ugung stellt. Typische Datenraten liegen nach [70] hierbei im Bereich von 1/6 Hz bis 12.5 Hz. Ein erstes Simulationsergebnis ist in Abb. 9.5 dargestellt, das den u unfundzwanzig ¨ ber f¨ ◦ Simulationsl¨ aufe gemittelten Gesamtlagefehler des Slaves bei einer 10 /h Slave IMU und einer Master-Slave-Datenrate von 0.2 Hz f¨ ur verschiedene Alignment-Verfahren zeigt. Aufgrund des Geradeausflugs ist der Yaw-Winkel zun¨achst nicht beobachtbar. Die geringe Datenrate, mit der der Master Geschwindigkeits- und Lageinformationen zur Verf¨ ugung stellt, f¨ uhrt dazu, dass die Rapid-Transfer-Alignment-Verfahren von dem WingRock-Man¨ over nicht profitieren k¨ onnen. Daher nimmt der Lagefehler erst ab, wenn bei 75 Sekunden der Kurvenflug beginnt. Offensichtlich f¨ uhrt die Ber¨ ucksichtigung der Zeitkorrelationen des vibrationsinduzierten Inertialsensorrauschens beim Rapid Transfer Alignment zu einer Verbesserung der Lagesch¨atzung. Zum Vergleich sind auch die Ergebnisse eines konventionellen Alignment-Verfahrens dargestellt. Hierbei f¨allt auf, dass zum einen die Konvergenz der Lagesch¨atzung beim Kurvenflug etwas langsamer ausf¨ allt, zum anderen wachsen beim anschließenden Geradeausflug aufgrund der Unbe-

Fehler der gesch¨ atzen rel. Orientierung (rad)

272

9 Anwendungsbeispiel Transfer Alignment

0.02

e u

RTXA, Zeitkorr. ber¨ ucksichtigt RTXA, Zeitkorr. unber¨ ucksichtigt

u e u 0.015

0.01

0.005 e

u e

u

e

u

e

u

e

u

e

u

u

e

e

u

e

u

e

u

e

0 0

100

200

300 Zeit (s)

400

500

600

Abbildung 9.6: Fehler der gesch¨ atzten relativen Orientierung von Master und Slave, Datenrate ◦ 0.2 Hz, 10 /h Slave IMU.

obachtbarkeit des Yaw-Winkels die Lagefehler wieder an. Dieser Anstieg der Lagefehler wird bei den Rapid-Transfer-Alignment-Verfahren nicht beobachtet. Aufgrund der von diesen Algorithmen gesch¨ atzten relativen Orientierung zwischen Master und Slave profitiert hier die Yaw-Winkelsch¨ atzung des Slaves von der Yaw-Winkelsch¨atzung des Masters. Da f¨ ur den Master sehr hochwertige Inertialsensoren angenommen wurden, die dar¨ uber hinaus auch nicht der gleichen extremen Vibrationsumgebung ausgesetzt sind, w¨ achst der Yaw-Winkelfehler des Masters w¨ahrend des Geradeausfluges kaum an. Abb. 9.6 zeigt den Fehler der von den Rapid-Transfer-Alignment-Verfahren gesch¨atzten Orientierung von Master und Slave. Auch hier f¨ uhrt die Ber¨ ucksichtignug der Zeitkorrelationen zu einer Verbesserung des Sch¨ atzergebnisses. Die gleiche Aussage l¨ asst sich f¨ ur die Sch¨atzung der Drehratensensorbiase treffen. Abb. 9.7 zeigt f¨ ur die verschiedenen Alignmentverfahren die Gesamtfehler der Drehratensensorbiassch¨ atzung. W¨ ahrend ohne Ber¨ ucksichtigung der Zeitkorrelationen das konventionelle Transfer-Alignment-Verfahren ¨ahnliche Ergebnisse liefert wie das RapidTransfer-Alignment-Verfahren, liefert Letzteres bei Ber¨ ucksichtigung der Zeitkorrelationen eine deutlich schnellere und pr¨ azisere Biassch¨atzung. ◦

Schließlich wurden diese Simulationen unter Annahme einer 2 /h Slave IMU wiederholt, Abb. 9.8 zeigt die von den verschiedenen Verfahren erzielten, mittleren Gesamtlagefeh◦ ler. Prinzipiell ergeben sich die gleichen Resultate wie bei der 10 /h Slave IMU, die Unterschiede zwischen den einzelnen Alignment-Verfahren fallen jedoch geringer aus,

Fehler der Drehratensensorbias Sch¨ atzung (deg)

9.4 Numerische Simulation

273

35

u

25

♦ u

20

♦

15

u

♦ u

e

♦

♦ u

10

e u

RTXA, Zeitkorr. ber¨ ucksichtigt RTXA, Zeitkorr. unber¨ ucksichtigt TXA, Zeitkorr. unber¨ ucksichtigt

30 ♦e

♦ u

e e

5

e

e

0 0

100

200

300 Zeit (s)

400

500

600

Abbildung 9.7: Fehler der gesch¨ atzten Drehratensensorbiase bei verschiedenen Transfer Alignment Verfahren.

0.02

Lagefehler (rad)

0.015

e u

RTXA, Zeitkorr. ber¨ ucksichtigt RTXA, Zeitkorr. unber¨ ucksichtigt TXA, Zeitkorr. unber¨ ucksichtigt

u ♦e u

♦

0.01

0.005

0 ♦e 0

♦ ♦ ♦ ♦ ♦ ♦ u ♦ ♦ ♦ ♦ u u u u u u u e e u e u ♦e e e e e e e e 100

200

300 Zeit (s)

400

◦

500

Abbildung 9.8: Gesamtlagefehler des Slaves, Datenrate 0.2 Hz, 2 /h Slave IMU.

600

274

9 Anwendungsbeispiel Transfer Alignment

0.02 ♦ Lagefehler (rad)

0.015

e u

RTXA, Zeitkorr. ber¨ ucksichtigt RTXA, Zeitkorr. unber¨ ucksichtigt TXA, Zeitkorr. unber¨ ucksichtigt

u

♦ ♦

0.01

♦ ♦

0.005

0 ♦e 0

♦

♦

♦ ♦ ♦ ♦ e u e u e u e u e u e u e u e u e u e u e u e ♦

100

♦

200

300 Zeit (s)

400

500

600

◦

Abbildung 9.9: Gesamtlagefehler des Slaves, Datenrate 5 Hz, 10 /h Slave IMU.

insbesondere das beim konventionellen Alignment beobachtete Anwachsen des Lagefehlers w¨ ahrend des Geradeausflugs ist verlangsamt. Abschließend wurden Simulationen durchgef¨ uhrt, bei denen eine Datenrate von 5 Hz f¨ ur die vom Master zur Verf¨ ugung gestellten Informationen angenommen wurde, siehe Abb. 9.9. Mit dieser gr¨ oßeren Messdatenrate sind die Rapid-Transfer-Alignment-Verfahren in der Lage, von dem kurzen Wing-Rock-Man¨over zu profitieren, wohingegen die Lagesch¨ atzung bei Verwendung eines konventionellen Transfer-Alignment-Verfahrens nach wie vor erst mit Beginn des Kurvenfluges konvergiert. Schließlich sein noch darauf hingewiesen, dass neben der Ber¨ ucksichtigung der Zeitkorrelationen des vibrationsinduzierten Rauschens die Erweiterung des Zustandsvektors des Alignment-Filters zur Sch¨ atzung von Skalenfaktoren und Misalignment der Inertialsensoren zu einer Verbesserung der Sch¨ atzergebnisse f¨ uhren kann, siehe [42].

9.5

Adaptive Sch¨atzung der Rauschprozessmodelle

Voraussetzung f¨ ur die Ber¨ ucksichtigung der Zeitkorrelationen des vibrationsinduzierten Rauschens ist, dass geeignete Rauschprozessmodelle zu Verf¨ ugung stehen. H¨aufig h¨angt das vibrationsinduzierte Rauschen von der Flugsituation ab, so dass sich eine adaptive Sch¨ atzung dieser Rauschprozessmodelle anbietet. Da sich in numerischen Simulationen gezeigt hat, dass die Kreuzkorrelationen des vibrationsinduzierten Inertialsensorrauschens vernachl¨assigt werden k¨onnen und eine Mo-

9.5 Adaptive Sch¨ atzung der Rauschprozessmodelle

275

dellierung als Prozess zweiter Ordnung bereits gute Ergebnisse liefert, kann f¨ ur jeden Inertialsensor ein Rauschprozessmodell der Form

 (9.34) wk = a1 wk−1 + a2 wk−2 + nk , E n2k = σn2 angesetzt werden. Die adaptive Sch¨ atzung der Rauschprozessmodelle besteht daher in der Bestimmung der Parameter a1 , a2 und σn2 . Erschwert wird diese Sch¨atzung dadurch, dass neben dem vibrationsinduzierten Rauschen nat¨ urlich auch die Trajektoriendynamik in die Inertialsensordaten eingeht. Eine direkte Sch¨atzung dieser Parameter anhand der Inertialsensordaten z.B. durch Anwendung der Yule-Walker-Gleichung ist daher nicht m¨ oglich.

9.5.1

Identifikation anhand von Messwertdifferenzen

Um den Einfluss der Trajektoriendynamik zu minimieren, k¨onnen zeitliche Differenzen der Inertialsensordaten gebildet werden. Diese zeitlichen Differenzen k¨onnen n¨aherungsweise durch uk = wk − wk−1

(9.35)

beschrieben werden. Hierbei wurde angenommen, dass die Differenzenbildung die Trajektoriendynamik zum gr¨ oßten Teil eliminiert. Nun k¨ onnen f¨ ur unterschiedliche zeitliche Verschiebungen rekursiv Korrelationen E[uk uk−i ] = r(i)

,

i ∈ [0, 1, 2]

(9.36)

dieser Inertialsensordatendifferenzen gesch¨ atzt werden: rk (i) = (1 −

1 1 ) rk−1 (i) + uk uk−i α α

(9.37)

Je gr¨ oßer der Parameter α in Gl. (9.37) gew¨ahlt wird, desto rausch¨armer sind die ge¨ sch¨ atzten Korrelation; andererseits wird eine Anderung der Zeitkorrelationsstruktur entsprechend langsamer erkannt. Um den Zusammenhang zwischen den Korrelationen r(0), r(1) und r(2) und den Parametern a1 , a2 sowie σn2 herzuleiten, wird zun¨achst die z-Transformierte von Gl. (9.34) berechnet: w(z) = a1 z −1 w(z) + a2 z −2 w(z) + n(z) n(z) w(z) = 1 − a1 z −1 − a2 z −2

(9.38)

Damit ergibt sich durch z-Transformation von Gl. (9.35) u(z) = (1 − z −1 )w(z) (1 − z −1 )n(z) = 1 − a1 z −1 − a2 z −2 u(z)(1 − a1 z −1 − a2 z −2 ) = (1 − z −1 )n(z) .

(9.39)

276

9 Anwendungsbeispiel Transfer Alignment

Die R¨ ucktransformation von Gl. (9.39) ins Zeitdiskrete liefert uk − a1 uk−1 − a2 uk−2 = nk − nk−1 uk = a1 uk−1 + a2 uk−2 + nk − nk−1

.

(9.40)

Mit E[uk nk ] = E[(a1 uk−1 + a2 uk−2 + nk − nk−1 )nk ] = a1 E[uk−1 )nk ] +a2 uk−2 nk +E[n2k ] − E[nk−1 nk ] % &' ( % % &' ( &' ( 0

0

=

0

σn2

(9.41)

und E[uk nk−1 ] = E[(a1 uk−1 + a2 uk−2 + nk − nk−1 )nk−1 ] 

= a1 E[uk−1 nk−1 ] + a2 uk−2 nk−1 + E[nk nk−1 ] −E n2k−1 % &' ( % &' ( 0

= (a1 −

0

1)σn2

(9.42)

folgt r(0) = E[(a1 uk−1 + a2 uk−2 + nk − nk−1 )uk ] = a1 r(1) + a2 r(2) + E[nk uk ] − E[nk−1 uk ] r(0) = a1 r(1) + a2 r(2) + (2 − a1 )σn2 .

(9.43)

Analog findet man r(1) = E[(a1 uk−1 + a2 uk−2 + nk − nk−1 )uk−1 ] = a1 r(0) + a2 r(1) + E[nk uk−1 ] −E[nk−1 uk−1 ] % &' ( 0

r(1) = a1 r(0) + a2 r(1) − σn2

(9.44)

und r(2) = E[(a1 uk−1 + a2 uk−2 + nk − nk−1 )uk−2 ] = a1 r(1) + a2 r(0) + E[nk uk−2 ] − E[nk−1 uk−2 ] &' ( % &' ( % 0

r(2) = a1 r(1) + a2 r(2)

.

0

(9.45)

Die Gl. (9.43) – (9.44) stellen ein nichtlineares Gleichungssystem f¨ ur die gesuchten Parameter dar, das zwei L¨ osungen besitzt. Eine n¨ahere Betrachtung dieser L¨osungen

9.5 Adaptive Sch¨ atzung der Rauschprozessmodelle

277

zeigt, dass die zur Bestimmung der gesuchten Parameter ben¨otigten Zusammenh¨ange durch r(0) + 2r(1) + r(2) r(0) + r(1) r(0)r(2) − r(1)r(0) − 2r(1)2 a2 = r(0)(r(0) + r(1)) 2 r(0) + r(0)r(2) − 2r(1)2 σn2 = r(0)

(9.46)

a1 =

(9.47) (9.48)

gegeben sind. ¨ Bei der Ubernahme der gesch¨ atzten Parameter in den Filteralgorithmus sind jedoch 2 zus¨ atzliche Vorsichtsmaßnahmen notwendig. Die Varianz σw des Rauschprozesses Gl. 9.34 ist gegeben durch 2 = σw

σn2 (1 − a2 ) (1 − a2 )(1 − a21 − a22 ) − 2a21 a2

.

(9.49)

F¨ ur a2 = ±(a1 + 1) sowie f¨ ur a2 = −1 geht diese Varianz gegen unendlich. Die Varianz des vibrationsinduzierten Rauschens ist zwar endlich, w¨ahrend der Konvergenz des Verfahrens zu neuen Modellparametern z.B. nach Beginn einer neuen Flugphase

10 z-Beschleunigungsmesser Beschleunigung (m/s2 )

5 0 -5 -10 -15 -20 -25 0

20

40

60

80 Zeit (s)

100

120

140

Abbildung 9.10: Synthetisch erzeugte Messwerte des z-Beschleunigungssensors, bestehend aus Trajektoriendynamik und vibrationsinduziertem Rauschen.

278

9 Anwendungsbeispiel Transfer Alignment

k¨ onnen die gesch¨ atzten Parameter jedoch die angegebenen Bedingungen zumindest n¨aherungsweise erf¨ ullen. W¨ urden in dieser Situation die gesch¨atzten Parameter in den Filteralgorithmus u onnte dies bis zum Scheitern des Datenfusionsalgo¨ bernommen, k¨ rithmus f¨ uhren.

9.5.2

Ergebnisse

Die Leistungsf¨ ahigkeit des dargestellten Verfahrens zur Sch¨atzung der Rauschprozessparameter wurde in numerischen Simulationen untersucht. Hierzu wurden zun¨achst anhand von Flugversuchsdaten typische Trajektoriendynamiken identifiziert. Zu den so gewonnen Daten wurden bekannte, zeitkorrelierte Rauschprozesse addiert, die identifiziert werden sollten. Die Rauschprozesse wurden dabei typischen Vibrationsumgebungen nachempfunden. Der Vorteil dieser Vorgehensweise besteht darin, dass aufgrund der bekannten Rauschprozessmodelle die Korrektheit der Parametersch¨atzung sehr einfach u uft werden kann. ¨ berpr¨ Abb. 9.10 zeigt von den so erzeugten Inertialsensordaten exemplarisch die Messwerte des z-Beschleunigungsmessers, die deutlich den Einfluss von Gravitation, Trajektoriendynamik und vibrationsinduziertem Rauschen erkennen lassen. Abb. 9.11 zeigt die von dem vorgeschlagenen Verfahren gelieferten Sch¨atzungen der Rauschprozessmodellparameter. Offensichtlich stimmen die Sch¨atzungen trotz der signifikanten Trajektoriendynamik sehr gut mit den wahren Werten u ur die zweite ¨ berein. F¨

3

2

Sch¨atzung σn Sch¨atzung a1 Sch¨atzung a2

1.5

Wahre Werte

Gesch¨atzte Parameter

2.5

 

1 0.5 0 -0.5































-1































-1.5 0

20

40

60

80 Zeit (s)

100

Abbildung 9.11: Sch¨ atzung der Parameter des Rauschprozessmodells.

120

140

9.5 Adaptive Sch¨ atzung der Rauschprozessmodelle

279

H¨ alfte des Simulationszeitraumes wurden ver¨anderte Modellparameter angenommen, ¨ diese Anderung wird von dem Sch¨ atzalgorithmus z¨ ugig erfasst. Dieses Identifikationserfahren bietet daher zusammen mit dem modifizierten Filteralgorithmus zur effizienten Ber¨ ucksichtigung von Zeitkorrelationen die M¨oglichkeit, die Leistungsf¨ ahigkeit von Rapid-Transfer-Alignment-Verfahren signifikant zu steigern.

10

Anwendungsbeispiel unbemanntes Flugger¨at

Das Interesse an unbemannten Flugger¨ aten, Unmanned Aerial Vehicles (UAV), ist in den letzten Jahren stetig gestiegen. Im milit¨ arischen Bereich wurden Systeme entwickelt, die eine Nutzlast von mehreren hundert Kilogramm tragen, in sehr großen H¨ohen fliegen und viele Stunden in der Luft bleiben k¨onnen. Beispiele hierf¨ ur w¨aren der Global Hawk und der Predator. F¨ ur eine Reihe von Anwendungen sind aber auch sehr kleine Flugger¨ ate interessant, so genannte Mini und Micro Aerial Vehicles (MAV). Die Grenze zwischen UAV und MAV ist nicht eindeutig definiert; h¨aufig wird bei einem Flugger¨at, dessen Abmessungen unter einem Meter liegen von einem Mini Aerial Vehicle gesprochen. Ist das Flugger¨ at kleiner als f¨ unfzehn Zentimeter, handelt es sich um ein Micro Aerial Vehicle. Die n¨ achsten Abschnitte besch¨ aftigen sich mit einem VTOL MAV, das in Abb. 10.1 zu sehen ist. VTOL MAVs 1 besitzen die F¨ahigkeit zu schweben, was insbesondere ¨ bei Uberwachungsund Aufkl¨ arungsaufgaben vorteilhaft sein kann. Entscheidend bei dem Einsatz eines solchen MAVs ist eine leichte Handhabbarkeit. Gerade schwebeflugf¨ ahige MAVs weisen jedoch anders als viele Fl¨achenfl¨ ugler eine inh¨arente Instabilit¨at auf. Daher ist ein sinnvoller Einsatz eines VTOL MAV nur m¨oglich, wenn zumindest eine automatische Lage- und H¨ ohenregelung vorhanden ist. W¨ahrend H¨oheninformationen vergleichsweise einfach mit einem Baro-Altimeter gewonnen werden k¨onnen, gestaltet sich die Sch¨ atzung der Lage des Flugger¨ates ungleich schwieriger. In der Literatur findet man verschiedene Ans¨ atze zur Lagesch¨atzung. Mit mindestens drei GPS-Empf¨ angern bzw. einem Mehr-Antennen-Empf¨anger k¨onnen Lageinformationen ausschließlich anhand von GPS-Messungen gewonnen werden. Hierzu m¨ ussen zwingend GPS-Tr¨ agerphasenmessungen genutzt und deren Mehrdeutigkeitswerte festgelegt werden k¨ onnen. Die Genauigkeit der Lagebestimmung ist dabei mit abh¨angig von dem Abstand der GPS-Antennen zueinander, aufgrund der geometrischen Gegebenheiten des hier betrachteten MAVs scheidet dieser Ansatz somit aus. Ein anderer Ansatz besteht in der Nutzung der Bilder einer oder mehrerer mitgef¨ uhrter Kameras. Die Ergebnisse entsprechender Bildverabeitungsalgorithmen dienen dann zur Verbesserung der Navigationsl¨ osung oder sogar als alleinige St¨ utzung eines Inertialnavigationssystems, siehe [115],[127],[109],[126]. Bildverarbeitungsalgorithmen sind jedoch sehr rechenaufw¨andig, so dass eine Bildverarbeitung an Bord des MAV hohe Anspr¨ uche an den Navigationscomputer stellt. Eine Alternative hierzu ist die Durchf¨ uhrung der Bildverarbeitung mit einer Bodenstation, die dort errechneten Ergebnisse k¨onnen wieder an das MAV u ugbarkeit einer Funkver¨ bertragen werden. Dazu muss jedoch die kontinuierliche Verf¨ bindung sichergestellt sein, da bei einer solchen Systemarchitektur mit dem Verlust des 1 VTOL

steht f¨ ur Vertical Take-Off and Landing.

282

10 Anwendungsbeispiel unbemanntes Flugger¨at

Abbildung 10.1: VTOL MAV im Flug, Abmessung von Rotormittelpunkt zu Rotormittelpunkt 54 cm. Mit freundlicher Genehmigung der microdrones GmbH.

¨ Ubertragungskanals auch der Verlust der Navigationsinformationen verbunden ist, was unweigerlich einen Absturz zur Folge hat. In der Praxis wird sich eine solche unterbrechungsfreie Datenverbindung nur mit gr¨ oßerem Aufwand realisieren lassen, daher sollen im Folgenden die ben¨ otigten Lageinformationen mit einem GPS/INS-System ermittelt werden. Typische GPS/INS-Systeme f¨ ur UAVs sind in [68],[131],[67] beschrieben, dort werden mehrere GPS-Antennen genutzt, um auch bei Man¨overn die Verf¨ ugbarkeit einer ausreichenden Anzahl von Satelliten zu gew¨ ahrleisten. Trotz dieser Maßnahmen kann nicht davon ausgegangen werden, dass bei einem UAV oder MAV in jeder Situation GPS¨ Messungen verf¨ ugbar sind, z.B. beim Flug in Geb¨auden oder in H¨auserschluchten. Ublicherweise werden bei einem GPS/INS-System GPS-Ausf¨alle anhand der inertialen Navigationsl¨ osung u uckt. Bei MAVs ist das nicht m¨oglich, da aus Gewichts-, Platz¨ berbr¨ und Kostengr¨ unden MEMS-Inertialsensoren geringer G¨ ute verwendet werden m¨ ussen. Dadurch wachsen ohne geeignete St¨ utzinformationen die Navigationsfehler so schnell mit der Zeit an, dass eine Stabilisierung des MAVs nach k¨ urzester Zeit nicht mehr m¨ oglich ist. Um dieses Problem zu l¨ osen, werden in [69],[92],[33] Navigationssystemarchitekturen verwendet, die sich deutlich von den bisher diskutierten GPS/INS-Systemen unterscheiden. Die GPS Positions- oder Geschwindigkeitsmessungen werden hierbei zur Absch¨atzung der trajektorienbedingten Beschleunigung verwendet. Diese gesch¨atzte Beschleunigung wird von den Beschleunigungen, die von den Beschleunigungsmessern geliefert werden, subtrahiert. Was verbleibt ist eine Messung der Schwerebeschleunigung, die dazu verwendet werden kann, Roll- und Pitchwinkel zu st¨ utzen. Der bei der Kompen-

10.1 Beobachtbarkeit des Yaw-Winkels

283

sation der trajektorienbedingten Beschleunigung ben¨otigte Yaw-Winkel wird entweder durch Einsatz eines Magnetometers ermittelt, oder anhand der GPS-Geschwindigkeit und aerodynamischer Eigenschaften des Flugger¨ats bestimmt; Letzteres ist nur bei Fl¨ achenfl¨ uglern m¨ oglich. F¨ allt bei diesem Integrationsansatz nun GPS aus, so kann die trajektorienbedingte Beschleunigung zwar nicht mehr kompensiert werden, die Beschleunigungsmesserdaten sind aber dennoch von der Schwerebeschleunigung dominiert und k¨ onnen weiterhin zur St¨ utzung von Roll- und Pitchwinkel herangezogen werden. Der wesentliche Nachteil dieses Integrationsansatzes besteht darin, dass vorhandene GPS-Informationen nur suboptimal genutzt werden: Eine Kalibration der Beschleunigungsmesserbiase findet nicht statt. Gerade bei der Verwendung von MEMS-Sensoren geringer G¨ ute kann das aber ein massives Problem sein. Die Biase dieser Sensoren sind meist stark temperaturabh¨ angig, so dass z.B. bei Zimmertemperatur ermittelte Biase bei K¨ alte im Freien nicht mehr g¨ ultig sind. Da diese Temperaturabh¨angigkeit von Sensor zu Sensor deutlich variieren kann, m¨ usste jeder einzelne Sensor vermessen werden, um bei diesem Integrationsansatz in Verbindung mit einer Temperaturmessung die sich andernden Biase n¨ aherungsweise kompensieren zu k¨onnen. ¨ Daher soll im Folgenden ein anderer Weg beschritten werden. Zum Einsatz kommt ein GPS/INS-System, das dem in Abschnitt 8.2 beschriebenen Loosely Coupled System stark a auft der Strapdown-Algorithmus jedoch nicht einfach ¨hnelt. Bei GPS-Ausfall l¨ ungest¨ utzt weiter. Stattdessen wird auf ein anderes Filter umgeschaltet, das die Lage anhand der Drehratensensordaten extrapoliert und Beschleunigungsmesserdaten zur Lagest¨ utzung verwendet. Diese Vorgehensweise vereint die Vorteile eines herk¨ommlichen GPS/INS-Systems und die des im vorherigen Abschnitt beschriebenen Ansatzes: Sind GPS-Messungen vorhanden, werden s¨ amtliche Inertialsensoren, auch die Beschleunigungsmesser, kontinuierlich kalibriert. Ohne GPS-Empfang wird durch Messung der Schwerebeschleunigung die Langzeitgenauigkeit der Lagesch¨atzung sichergestellt. Der Umschaltvorgang zwischen den Filtern gestaltet sich problemlos, da zur Initialisierung des einen Filters die Lage- und Drehratensensorbiassch¨atzung des anderen Filters samt zugeh¨ origer Varianzen verwendet werden k¨onnen, so dass die Konvergenzphase praktisch entf¨ allt. In den n¨ achsten Abschnitten werden Besonderheiten des GPS/INS-Systems des VTOL ¨ MAVs sowie der Uberbr¨ uckung von GPS-Ausf¨allen n¨aher betrachtet.

10.1

Beobachtbarkeit des Yaw-Winkels

Bei einem auf MEMS-Sensoren geringer G¨ ute basierenden GPS/INS-System sind gegen¨ uber den in Abschnitt 8.2 entwickelten Gleichungen massive Vereinfachungen m¨oglich. So k¨ onnen die Erddrehrate, die Transportrate und Coriolis-Terme sowohl in der Strapdown-Rechnung als auch im Kalman-Filter-Systemmodell problemlos vernachl¨assigt werden. Aufgrund der geringen Abmessungen ist eine Ber¨ ucksichtigung der Relativposition von GPS-Antenne und Inertialsensorik ebenfalls nicht notwendig. Die geringe G¨ ute der Inertialsensoren und die Schwebeflugf¨ahigkeit des MAVs f¨ uhren jedoch auch zu Problemen, die bei anderen Systemen nicht beachtet werden m¨ ussen. Dies soll anhand des Kalman-Filter-Systemmodells aufgezeigt werden, das sich nach den angesprochenen Vereinfachungen wie folgt darstellt:

284

10 Anwendungsbeispiel unbemanntes Flugger¨at

⎛

⎞• ⎛ ∆xn 000 ⎜ ∆xe ⎟ ⎜000 ⎜ ∆x ⎟ ⎜000 ⎜ ⎜ d ⎟ ⎜ ∆v ⎟ ⎜000 n ⎟ ⎜ ⎜ ⎜ ∆v ⎟ ⎜000 e ⎟ ⎜ ⎜ ⎜ ∆vd ⎟ ⎜000 ⎜ ⎟ ⎜ ⎜ α ⎟ ⎜000 ⎜ ⎟ ⎜ ⎜ β ⎟ = ⎜000 ⎜ ⎟ ⎜ ⎜ γ ⎟ ⎜000 ⎜ ⎟ ⎜ ⎜ ∆ba,x ⎟ ⎜000 ⎜ ⎟ ⎜ ⎜ ∆ba,x ⎟ ⎜000 ⎜ ⎟ ⎜ ⎜ ∆ba,x ⎟ ⎜000 ⎜ ⎟ ⎜ ⎜∆bω,x ⎟ ⎜000 ⎝∆b ⎠ ⎝000 ω,y 000 ∆bω,z

I 000 000 000 000 000 000 000 000 000 000 000 000

000 000 000 0 ad − ae −ad 0 an 0 ae − an 000 000 000 000 000 000 000 000 000

000 000 000 −Cnb 000 000 000 000 000 000 000 000 000

⎞⎛

⎞ ∆xn ⎟ ⎜ ∆xe ⎟ ⎟ ⎜ ∆x ⎟ ⎟⎜ d ⎟ ⎟ ⎜ ∆v ⎟ n ⎟ ⎟⎜ ⎟ ⎜ ∆v ⎟ e ⎟ ⎟⎜ ⎟ ⎜ ∆vd ⎟ ⎟⎜ ⎟ ⎟⎜ α ⎟ ⎜ ⎟ ⎟ ⎜ ⎟ −Cnb ⎟  (10.1) ⎟ ⎜ β ⎟ + Gw ⎟⎜ γ ⎟ ⎟⎜ ⎟ ⎜ ⎟ 000 ⎟ ⎟ ⎜ ∆ba,x ⎟ 000 ⎟ ⎜ ∆ba,x ⎟ ⎟⎜ ⎟ 000 ⎟ ⎜ ∆ba,x ⎟ ⎟⎜ ⎟ 000 ⎟ ⎜∆bω,x ⎟ ⎠ ⎝ ∆b ⎠ 000 000 000 000 000 000 000

ω,y

000

∆bω,z

Der Zusammenhang zwischen Lagefehlern und Geschwindigkeitsfehlern wird durch die kreuzproduktbildende Matrix der in das Navigationskoordinatensystem umgerechneten, gemessenen Beschleunigungen an , ae und ad beschrieben. Offensichtlich f¨ uhrt hierbei ein Lagefehler γ nur zu Geschwindigkeitsfehlern, wenn Beschleunigungen in horizontaler Richtung vorliegen, an oder ae also von null verschieden ist. Andernfalls ist der Lagefehler γ des Yaw-Winkels von den restlichen Zustandsgr¨oßen entkoppelt und somit unbeobachtbar. Dies l¨ asst sich anschaulich leicht erkl¨aren: Betrachtet man das Schweben an einer festen Position oder einen unbeschleunigten, geradlienigen Flug, so kann das Flugger¨ at in Gedanken um die lokale Vertikale gedreht werden, ohne dass dies einen Einfluss auf die Messungen der Beschleunigungsmesser oder den Geschwindigkeitsvek¨ tor des Flugger¨ ates h¨ atte. Diese Drehung um die lokale Vertikale, also eine Anderung des Yaw-Winkels, kann daher nicht mit den St¨ utzinformationen des GPS-Empf¨angers in Verbindung gebracht werden. Zwar detektieren die Drehratensensoren nat¨ urlich eine ¨ Anderung des Yaw-Winkels, da bei der Integration der Drehratensensordaten ohne die M¨ oglichkeit zur St¨ utzung aber die Lagefehler mit der Zeit anwachsen, w¨achst in diesem Szenario ohne weitere Maßnahmen der Yaw-Winkelfehler unbeschr¨ankt an. Zus¨atzlich w¨ achst die Varianz des Yaw-Winkelsch¨ atzfehlers ebenfalls unbeschr¨ankt an, so dass auch das Auftreten numerischer Probleme nur eine Frage der Zeit ist. Bei einem Fl¨ achenfl¨ ugler kann ausgenutzt werden, dass ein solches Flugger¨at nur vorw¨ arts fliegen kann, die Bewegungsrichtung stimmt daher n¨aherungsweise mit der Richtung der x-Achse des k¨ orperfesten Koordinatensystems u ¨ berein. Damit kann anhand des vom GPS-Empf¨ anger gelieferten Geschwindigkeitsvektors auf den Yaw-Winkel geschlossen und so ein unbeschr¨ anktes Anwachsen des Yaw-Winkelfehlers verhindert werden. Diese Vorgehensweise ist bei einem VTOL MAV nicht anwendbar. Daher wurde als zus¨ atzlicher Sensor ein Magnetometer in das Navigationssystem integriert.

10.1 Beobachtbarkeit des Yaw-Winkels

10.1.1

285

Stu ¨tzung mit Erdmagnetfeldmessungen

Ein Magnetometer liefert Messungen des Erdmagnetfeldvektors   hb = hb , hb , hb T x y z in k¨ orperfesten Koordinaten. Dreht man das Magnetometer um eine Achse parallel zu den Feldlinien des Erdmagnetfeldes, so ¨ andern sich die Messwerte des Magnetometers dabei nicht; eine solche Drehung ist daher anhand der Magnetometerdaten nicht beobachtbar. Allgemein gilt, dass f¨ ur die vollst¨ andige Bestimmung der relativen Orientierung zweier Koordinatensysteme zwei nicht-parallele Vektoren in Koordinaten beider Koordinatensysteme ben¨ otigt werden2 . Geht man davon aus, dass der Erdmagnetfeldvektor in Koordinaten des Navigationssystems bekannt ist, so liegt mit den in k¨orperfesten Koordinaten anfallenden Messungen des Magnetometers nur ein Vektor in Koordinaten beider Koordinatensysteme vor. Daher ist eine Bestimmung des Yaw-Winkels nur m¨ oglich, wenn Roll- und Pitchwinkel zumindest n¨aherungsweise gegeben sind. Im Endeffekt ist diese Forderung ¨ aquivalent zu der Forderung nach einem zweiten Vektor, der sowohl in k¨ orperfesten Koordinaten als auch in Navigationskoordinaten bekannt ist. Dieser zweite Vektor ist sowohl bei dem GPS/INS-Filter als auch bei dem im vorigen ¨ Abschnitt erw¨ ahnten Filter zur Uberbr¨ uckung von GPS-Ausf¨allen letztendlich durch den Gravitationsvektor gegeben. Bei einem Kompass wird die Forderung nach einem bekannten Roll- und Pitchwinkel mechanisch gel¨ost, indem eine horizontale Lage der Nadel erzwungen wird. Die Nadel richtet sich somit parallel zur Horizontalkomponente des Erdmagnetfeldes aus. Geht man davon aus, dass das Flugger¨at ebenfalls horizontal ausgerichtet ist, so kann der Yaw-Winkel anhand von   (10.2) ψ = − arctan 2 hby , hbx bestimmt werden. Befinden sich die x- und y-Achsen des k¨orperfesten Koordinatensystems nicht in der Horizontalebene, so f¨ uhrt die Verwendung von Gl. (10.2) sehr schnell zu massiven Fehlern: Abb. 10.2 zeigt den Yaw-Winkelfehler, der sich ergibt, wenn bei einem Roll- und Pitchwinkel von jeweils zwanzig Grad der Yaw-Winkel trotzdem anhand von Gl. (10.2) berechnet wird. Hierbei wurde von einem Erdmagnetfeld von T

T

hn = (hn , he , hd ) = (20536.76, 239.1, 43465.33) nT ausgegangen, wie es laut [7] bei neunundvierzig Grad n¨ordlicher Breite und acht Grad ostlicher L¨ ange vorliegt. Bei dem hier betrachteten VTOL MAV sind Roll- und Pitchwin¨ kel von zwanzig Grad und mehr je nach Agressivit¨at der Man¨over und Wind eher die Regel als die Ausnahme. Die Magnetometerdaten k¨onnten daher ohne Kenntnis von Roll- und Pitchwinkel nicht genutzt werden. Nat¨ urlich w¨are ohne Kenntnis von Rollund Pitchwinkel auch eine Stabilisierung des MAV nicht m¨oglich, auf deren Notwendigkeit zur Interpretation der Magnetometerdaten wird meist jedoch nicht hingewiesen. 2 Die Bestimmung der relativen Orientierung zweier Koordinatensysteme anhand von zwei Vektoren, die in Koordinaten beider Koordinatensysteme bekannt sind, wird Waaba’s Problem genannt.

286

10 Anwendungsbeispiel unbemanntes Flugger¨at

200

Yaw-Winkelfehler (deg)

150 100 50 0 -50 -100 -150 -200 -150

-100

-50 0 50 Yaw-Winkel (deg)

100

150

Abbildung 10.2: Yaw-Winkelfehler, der sich abh¨ angig vom Yaw-Winkel ergibt, wenn vorliegende Roll- und Pitchwinkel von jeweils zwanzig Grad bei der Berechnung des Yaw-Winkels vernachl¨ assigt werden.

Im Folgenden soll die Messgleichung hergeleitet werden, die f¨ ur Verarbeitung einer Erdmagnetfeldmessung im Navigationsfilter ben¨otigt wird. Der Zusammenhang zwischen der in k¨ orperfesten Koordinaten anfallenden Erdmagnetfeldmessung und dem in Koordinaten des Navigationskoordinatensystems bekannten Erdmagnetfeldvektor l¨ asst sich anhand von ˜b h  n vm = Cn,T b h +

(10.3)

beschreiben, vm ist hierbei das Messrauschen. Aus dem bereits aus Abschnitt 8.2 bekannten Zusammenhang ˆ nb Cnb = (I + Ψ) C folgt ˆ n,T (I − Ψ) . = C Cn,T b b

(10.4)

10.2 Stabilisierung bei GPS-Ausfall

287

Durch Einsetzen erh¨ alt man so ˜b h ˆ n,T (I − Ψ) h n + vm =C b  n ˆ n,T  n vm ˆ = Cn,T b h − Cb Ψh +     + vm . ˆ n,T h n + C ˆ n,T h n × ψ =C b b

(10.5)

Die Messgleichung, die den Zusammenhang zwischen der Erdmagnetfeldmessung und den Lagefehlern beschreibt, ist damit gegeben durch   ˜ b ˆ n,T  n h  + vm . ˆ n,T h n × ψ − Cb h = C (10.6) b Leider ist das Erdmagnetfeld unter anderem in der N¨ahe metallischer Objekte gest¨ort. Diese Verzerrungen k¨ onnen zu Fehlern von mehreren Grad bei der Lagebestimmung f¨ uhren. Obwohl der Yaw-Winkel zur Berechnung der Systemmatrix des Navigationsfilters ben¨ otigt wird, gef¨ ahrdet auch ein Yaw-Winkelfehler von z.B. f¨ unfzehn Grad die Funktionsf¨ ahigkeit des Navigationsfilters noch nicht. Roll- oder Pitchwinkelfehler von f¨ unfzehn Grad k¨ onnen jedoch nicht toleriert werden, da dadurch das MAV in einer Lage stabilisiert w¨ urde, die sich von der eigentlich gew¨ unschten Lage deutlich unterscheidet. Um diesem eventuell negativen Einfluss der Magnetometerdaten auf Roll- und Pitchwinkel  zu zu begegnen, k¨ onnen in der Messgleichung (10.6) die Lagefehler ψ  = (0, 0, γ) ψ

(10.7)

gesetzt werden. Dadurch wird f¨ ur die Verarbeitung der Magnetometerdaten angenommen, dass Roll- und Pitchwinkel perfekt bekannt sind. Die endg¨ ultige Form der Messgleichung ergibt sich zu ⎛ ⎞ he ˜ n,T n,T h b − C ˆ h n = C ˆ ⎝−hn ⎠ γ + vm . (10.8) b b 0 Damit wird die Erdmagnetfeldmessung nur zur St¨ utzung des Yaw-Winkels herangezogen, eine Beeinflussung von Roll- und Pitchwinkel findet nicht statt.

10.2

Stabilisierung bei GPS-Ausfall

Liegt ein GPS-Ausfall vor, so wird von dem GPS/INS-Filter auf ein anderes Filter umgeschaltet, das nur die Lage des Flugger¨ ats und die Drehratensensorbiase sch¨atzt. Aufgrund der geringen G¨ ute der Inertialsensoren muss dieses Umschalten bereits wenige Sekunden nach Verf¨ ugbarkeit der letzten GPS-Messung erfolgen. Geschwindigkeitsinformationen k¨onnen dann ebenso wie Positionsinformationen in horizontaler Richtung nat¨ urlich nicht mehr geliefert werden. H¨ oheninformationen stehen durch Einsatz des Baro-Altimeters auch bei einem GPS-Ausfall noch zur Verf¨ ugung. Das Filter, das nun w¨ ahrend eines GPS-Ausfalls zum Einsatz kommt, soll im Folgenden kurz beschrieben werden.

288

10.2.1

10 Anwendungsbeispiel unbemanntes Flugger¨at

Systemmodell des Lagefilters

Auch f¨ ur das Filter, das w¨ ahrend eines GPS-Ausfalls Lageinformationen liefert, wurde eine Error-State-Space-Formulierung gew¨ ahlt. Als St¨ utzinformationen, d.h. als im Messschritt zu verarbeitende Messungen, dienen die Daten der Beschleunigungssensoren und des Magnetometers. Der Zustandsvektor dieses Lagefilters enth¨alt drei Lagefehler und drei Fehler der Drehratensensorbiase. Das Systemmodell ist gegeben durch





 •  ˆn ˆn 0 ∆ψ ∆ψ nω 0 C C b b = + . (10.9) nbω 0 0 0 I ∆bω ∆bω Man erkennt, dass es sich hierbei um eine Untermenge des GPS/INS-Filters handelt. Die Drehratensensorbiase wurden als Random-Walk-Prozesse modelliert, da die Biase von MEMS-Drehratensensoren oftmals einen deutlichen Drift aufweisen. Beschleunigungsmesserbiase sind anhand der bei GPS-Ausfall zur Verf¨ ugung stehenden Messwerte unbeobachtbar und k¨ onnen daher nicht in den Zustandsvektor des Filters aufgenommen werden. Grundlage der Verarbeitung der Messungen des Magnetometers ist genau wie beim GPS/INS-Filter die Messgleichung (10.8). Die Verarbeitung der Beschleunigungsmessungen a ¨hnelt der Verarbeitung der Magnetometerdaten stark, die ben¨otigte Messgleichung wird im Folgenden bestimmt.

10.2.2

Stu ¨tzung mit Beschleunigungsmessungen

Voraussetzung f¨ ur die Lagest¨ utzung mit Beschleunigungsmessungen ist, dass die Daten der Beschleunigungsmesser von der Schwerebeschleunigung dominiert sind. Mit der Schwerebeschleunigung in Koordinaten des Navigationskoordinatensystems gln = (0, 0, g)

(10.10)

k¨ onnen die Messungen der Beschleunigungmesser daher durch ˜ b = −Cn,T g n + na a ib l b

(10.11)

beschrieben werden. Der Fehler bei dieser Annahme, d.h. die trajektorienbedingte Beschleunigung, wird dabei dem Messrauschen na zugeschlagen. Durch Einsetzen von Gl. (10.4) erh¨ alt man ˜ b = −C ˆ n,T (I − Ψ)gln + na a ib b n,T n n,T b ˜ ˆ ˆ aib + Cb gl = Cb Ψgln + na ˜b + C  + na . ˆ n,T g n = −C ˆ n,T [g n ×] ψ a ib

b

l

b

l

(10.12)

Anhand der Beschleunigungsmesserdaten kann nicht auf den Yaw-Winkelfehler γ geschlossen werden, dies zeigt sich bei Berechnung des Kreuzproduktes: ⎛ ⎞⎛ ⎞ ⎛ ⎞ 0 −g 0 α −gβ  = ⎝g 0 0⎠ ⎝β ⎠ = ⎝ gα ⎠ [gln ×] ψ (10.13) 0 0 0 γ 0

10.3 Systemsimulation Die endg¨ ultige Messgleichung ergibt sich damit zu ⎞ ⎛ 0 −g

˜b + C ˆ n,T gln = −C ˆ n,T ⎝g 0 ⎠ α + na . a ib b b β 0 0

289

(10.14)

Dieses Filter nutzt die bez¨ uglich der Lagebestimmung vorliegende Komplementarit¨at von Beschleunigungs- und Drehratensensordaten aus: Mit den Drehratensensoren werden die h¨ oherfrequenten Anteile von Rotationen erfasst. Die Extrapolation der Lage anhand der Drehratensensordaten ist jedoch nur kurzzeitgenau, da z.B. aufgrund der Drehratensensorbiase und deren Drift vor allem die niederfrequenten Anteile von Rotationen nicht korrekt ermittelt werden. Eine einzelne Beschleunigungsmessung hingegen mag deutlich von der Trajektoriendynamik beeinflusst sein, so dass eine direkte Berechnung von Roll- und Pitchwinkel nach Gl. (3.5) zu groben Fehlern f¨ uhren w¨ urde. Die h¨ oherfrequenten Anteile von Rotationen werden bei vorliegender Trajektoriendynamik von den Beschleunigungsmessern daher nur unzureichend erfasst. L¨angerfristig betrachtet wird aber sehr wohl die Schwerebeschleunigung dominant sein, schon allein da das Flugger¨ at nicht beliebig lange beschleunigen kann. Die Beschleunigungssensoren liefern folglich Informationen u ¨ ber die niederfrequenten Rotationsanteile. Zusammenfassend l¨ asst sich sagen, dass die tempor¨ ar vorliegenden trajektorienbedingten Fehler“ der ” Beschleunigungmessungen mit Hilfe der Drehratensensoren u uckt werden, der Ein¨ berbr¨ fluss von Messfehlern der Drehratensensoren wie Drehratensensorbiase und Rauschen wird anhand der Beschleunigungsmesserdaten kompensiert, die – mit einer geringen Gewichtung im Lagefilter verarbeitet – somit die Langzeitgenauigkeit der Lagesch¨atzung sicherstellen.

10.3

Systemsimulation

Um die Leistungsf¨ ahigkeit des VTOL-MAV-Navigationssystems zu untersuchen, sind numerische Simulationen unabdingbar. Hierbei ist es von entscheidender Bedeutung, dass die Dynamik des Flugger¨ ats zumindest n¨aherungsweise nachgebildet wird: F¨ ur das bei GPS-Ausfall verwendete Lagefilter wird die Annahme getroffen, dass die Beschleunigungsmesser im Wesentlichen die Schwerebeschleunigung messen. Diese Annahme ist bei vorliegender Trajektoriendynamik nicht vollst¨andig korrekt. Ob dies zu Problemen bei der Lagesch¨ atzung f¨ uhren kann, muss f¨ ur die zu erwartenden, trajektorienbedingten Beschleunigungen u uft werden. ¨ berpr¨ Die Struktur der VTOL-MAV-Simulation ist in Abb. 10.3 dargestellt. Ausgehend von den vorliegenden Beschleunigungen und Drehraten werden die Lage, Geschwindigkeit und Position des MAV in der Zeit propagiert. Basierend auf dieser Flugtrajektorie werden unter Verwendung entsprechender Fehlermodelle die Daten eines GPS-Empf¨ angers, eines Baro-Altimeters, eines Magnetometers und von MEMSInertialsensoren generiert. Diese synthetischen Navigationssensordaten werden dann der VTOL-MAV-Navigationssystemalgorithmik zur Verf¨ ugung gestellt, die daraus eine Navigationsl¨ osung berechnet. Diese Navigationsl¨osung wird zum einen mit der idealen Referenz verglichen, was die Berechnung von Navigationsfehlern erlaubt. Zum anderen

290

10 Anwendungsbeispiel unbemanntes Flugger¨at

Berechnung der Flugtrajektorie

Mathematisches Modell des Flugger¨ats

Erzeugung von Navigationssensordaten

Berechnung von Kr¨aften und Momenten

Navigationssystem

Flugf¨ uhrung und Flugregelung

Abbildung 10.3: Blockdiagramm der VTOL-MAV-Simulation.

dient diese Navigationsl¨ osung als Eingangsgr¨oße f¨ ur die Flugf¨ uhrungs- und Flugregelungsalgorithmen, die unter Ber¨ ucksichtigung einer vorgegeben Solltrajektorie entsprechende Stellgr¨ oßen zu den Motoren des Flugger¨ats berechnen. Ein Modell, das den Zusammenhang zwischen den Motorstellgr¨oßen und den resultierenden Auftriebskr¨aften beschreibt, wird zur Berechnung der Kr¨afte und Momente verwendet, die wiederum als Eingangsgr¨ oßen f¨ ur ein mathematisches Modell des MAV dienen. Anhand dieses mathematischen Modells werden schließlich die aufgrund der angreifenden Kr¨afte und Momente resultierenden Beschleunigungen und Drehraten ermittelt, was die Simulationsschleife schließt. Im Folgenden soll auf die prinzipielle Funktionsweise des MAV eingegangen und das mathematische Modell des Flugger¨ ats beschrieben werden.

10.3.1

Funktionsprinzip des Flugger¨ats

Das VTOL MAV besitzt, wie in Abb. 10.4 dargestellt, vier Rotoren, von denen zwei Rotoren in Uhrzeigersinn und zwei Rotoren im Gegenuhrzeigersinn drehen. Dadurch wird erreicht, dass sich bei gleicher Drehzahl aller Rotoren alle Drehmomente aufheben. W¨ urden alle Rotoren die gleiche Drehrichtung besitzen, w¨ urde sich das MAV st¨andig um die z-Achse drehen. Dem k¨ onnte zwar prinzipiell begegnet werden, indem zwei Rotoren so verkippt werden, dass deren Schub dieser Drehung entgegenwirkt. Eine solche Vorgehensweise ist aus verschiedenen Gr¨ unden jedoch nicht praktikabel. So w¨ urde

10.3 Systemsimulation

291

F1

F4

F2

F3

x y z

Abbildung 10.4: Funktionsweise des VTOL MAV.

man entweder bei unterschiedlichen Nutzlasten unterschiedliche Verkippungen ben¨otigen oder m¨ usste in Kauf nehmen, dass beim rotationsfreien Schwebeflug die verkippten Rotoren mit einer anderen Drehzahl drehen als die nicht verkippten, was aber ineffizient ist. Daher wird diese Alternative hier nicht weiter betrachtet. Um eine Drehung um die z-Achse zu kommandieren, wird beispielsweise die Drehzahl der im Gegenuhrzeigersinn drehenden Rotoren erh¨oht und die Drehzahl der im Uhrzeigesinn drehenden Rotoren verringert. Der Gesamtauftrieb kann so beibehalten werden, die Drehmomente der Rotoren heben sich jedoch nicht mehr auf, was zu der gew¨ unschten Drehung f¨ uhrt. Eine positive Rollwinkel¨ anderung wird durch die Erh¨ohung der Drehzahl des linken Rotors und Verringerung der Drehzahl des rechten Rotors erreicht, eine positive Pitchwinkel¨ anderung wird dementsprechend durch Erh¨ohung der Drehzahl des vorderen und Verringerung der Drehzahl des hinteren Rotors kommandiert. Eine Translation kann nur erreicht werden, indem ein von null verschiedener Roll- oder Pitchwinkel eingenommen wird. Bei einem negativen Pitchwinkel beispielsweise beginnt ein Vorw¨ artsflug, da der in z-Richtung des k¨orperfesten Koordinatensystems anfallende Schubvektor eine Horizontalkomponente besitzt. Eine H¨ohen¨anderung schließlich wird durch eine kollektive Erh¨ ohung bzw. Verringerung der Drehzahlen aller Rotoren erreicht. Nach diesen Betrachtungen ist klar, das die insgesamt sechs Freiheitsgrade des VTOL MAV, d.h. drei Lagewinkel und drei Positionskoordinaten, nicht unabh¨angig voneinander vorgegeben werden k¨ onnen. Schließlich werden Position und Lage anhand von

292

10 Anwendungsbeispiel unbemanntes Flugger¨at

nur vier Stellgr¨ oßen, den Drehzahlen der Rotoren, beeinflusst. Das VTOL MAV geh¨ort damit zu der Klasse der unteraktuierten Systeme.

10.3.2

Mathematisches Modell

In diesem Abschnitt soll ein mathematisches Modell des VTOL MAV entwickelt werden. Da Transportrate, Erddrehrate, Coriolis-Terme und die Kr¨ ummung der Erde aufgrund der geringen G¨ ute der Inertialsensoren und des begrenzten Aktionsradius vernachl¨assigt werden k¨ onnen, wird ein erdfestes Koordinatensystem mit den Achsen Norden, Osten und Unten eingef¨ uhrt. Dieses Koordinatensystem wird als ein inertiales Koordinatensystem, d.h. unbeschleunigt und nicht rotierend, betrachtet und daher mit dem Index i gekennzeichnet. Dieses Koordinatensystem unterscheidet sich aber bez¨ uglich der Richtung der Koordinatenachsen von dem in Abschnitt 3.1 eingef¨ uhrten, bisher verwendeten Inertialkoordinatensystem. Translation F¨ ur die translatorischen Bewegungen des MAV sind die Beschleunigungen aufgrund des Schubes der Rotoren und die Schwerebeschleunigung entscheidend. Die von den Rotoren erzeugten Auftriebskr¨ afte werden mit F1 bis F4 bezeichnet, siehe Abb. 10.4. Damit erh¨ alt man f¨ ur die Beschleunigung des MAVs v˙ ibi = Cibaibb + g i 4  i 1 F b + g i = Cb m n=1 n ⎛ ⎞ ⎛ ⎞ 0 0 1 i⎝ ⎠ + ⎝0 ⎠ . 0 Cb = m −F1 − F2 − F3 − F4 g

(10.15)

Hierbei bezeichnet m die Masse des Flugger¨ats. Offensichtlich ist die Richtung, in der das Flugger¨ at beschleunigt, wesentlich von der in der Richtungskosinusmatrix Cib gespeicherten Lage bestimmt. Rotation Das mathematische Modell des MAV muss auch einen Zusammenhang zwischen den Auftriebskr¨ aften der Rotoren und den resultierenden Drehraten herstellen. Erzeugen die Rotoren des MAVs unterschiedliche Auftriebskr¨afte, so bewirken diese ein Drehmoment  . In Koordinaten des Inertialkoordinatensystems ist dieses Drehmoment gleich der M  zeitlichen Ableitung des Drehimpulses L: i  i = ∂L M ∂t

(10.16)

Analog zum Impuls, der als Produkt von Masse und Geschwindigkeit gegeben ist, erh¨alt man den Drehimpuls als Produkt von Tr¨ agheitsmoment und Winkelgeschwindigkeit:  i = Ji  L ωibi

(10.17)

10.3 Systemsimulation

293

Da die Auftriebskr¨ afte der Rotoren in k¨ orperfesten Koordinaten anfallen und auch das Tr¨ agheitsmoment in k¨ orperfesten Koordinaten einfacher zu formulieren ist, soll das Drehmoment ebenfalls in k¨ orperfesten Koordinaten betrachtet werden:  b = Cb M  i M i i ∂L = Cbi ∂t

(10.18) (10.19)

Mit  i = Ci L b L b

(10.20)

erh¨ alt man f¨ ur die zeitliche Ableitung des Drehimpulses i ∂L  b + Ci L ˙ b = C˙ ib L b ∂t ˙ b  b + Ci L = Ci Ωb L

(10.21)

 b = Ωbib L b + L ˙ b . M

(10.22)

b

ib

b

und damit

Liegt nur ein rotierender K¨ orper mit dem Tr¨agheitsmoment JbF vor, so erh¨alt man mit b  b = Jb ω L F  ib

(10.23)

durch Einsetzen in Gl. (10.22) die Eulerschen Kreiselgleichungen b b ˙ b  b = Ωb Jb  M ωib , ib F ωib + JF 

(10.24)

die h¨ aufig nur in ausmultiplizierter Form angegeben werden. Bei dem VTOL MAV liegen f¨ unf rotierende K¨orper vor, das eigentliche Flugger¨at und die vier Rotoren mit den zugeh¨ origen L¨ aufern der Motoren. Bezeichnet JbF das Tr¨agheitsmoment des Flugger¨ ats ohne Rotoren und L¨aufer der Motoren sowie JbR das Tr¨agheistmoment eines Rotors mit zugeh¨ origem L¨aufer, so ergibt sich b  b = Jb  L F ωib +

4 

b JbR  ωir,n

n=1 4  b   b ωib + = JbF + 4JbR  JbR ωbr,n .

(10.25)

n=1 b Hierbei ist  ωibb die Drehrate des Flugger¨ ats, ωbr ist die Drehrate eines Rotors gegen¨ uber dem k¨ orperfesten Koordinatensystem.

294

10 Anwendungsbeispiel unbemanntes Flugger¨at

Bezeichnet man den Abstand zwischen Rotormittelpunkt und Ursprung des k¨orperfesten Koordinatensystems im Zentrum des Flugger¨ats mit l, so erh¨alt man mit dem Drehmoment  b= M

4 

l b × F b n n

(10.26)

n=1

und den Gl. (10.22) und (10.25) als Endergebnis 4  n=1

l b × F b n n

  4    b = Ωbib JbF + 4JbR ωibb + JbR ω  br,n n=1 4    b + JbF + 4JbR ω˙ ibb + JbR ω˙ br,n .

(10.27)

n=1

Gleichung (10.27) kann nach  ω˙ ibb aufgel¨ ost werden und stellt damit eine Differentialgleichnug zur Verf¨ ugung, anhand derer die Drehrate des Flugger¨ats in Abh¨angigkeit von den Auftriebskr¨ aften der Rotoren berechnet werden kann. Die darin auftretenden Tr¨ agheitsmomente k¨onnen bestimmt werden, in dem man das Flugger¨ at in Elementark¨ orper zerlegt und deren Tr¨aggheitsmomente der entsprechenden Literatur entnimmt. Alternativ hierzu kann das Flugger¨at in Massepunkte mi zerlegt werden, die sich an den Positionen (xi , yi , zi ) im k¨orperfesten Koordinatensystem befinden. Das Tr¨ agheitsmoment ist dann durch ⎞  2  ⎛$ $ $ 2 − imi xi yi  − $i mi xi zi i yi + zi im $ $ 2 2 ⎠ (10.28) Jb = ⎝ − $i mi xi yi im $i xi + zi $− im2i yi zi2  x − i mi xi zi − i mi y i z i m + y i i i i gegeben. F¨ ur die Berechnung der tats¨ achlichen Flugtrajektorie im Rahmen der Systemsimulation ist ein detailliertes mathematisches Modell nach Gl. (10.27) sinnvoll, hierzu wurden sogar noch weitere experimentell bestimmte Kr¨afte erg¨anzt, die vor allem bei gr¨oßeren Fluggeschwindigkeiten auftreten oder bei Drehungen um die z-Achse aufgrund der Interaktion der Rotoren mit der viskosen Luft relevant sind. F¨ ur den Reglerentwurf ist ein einfacheres Modell des MAV jedoch besser geeignet. Zun¨ achst kann angenommen werden, dass die als Deviatoren bezeichneten Nebendiagonalelemente des Tr¨ agheitsmomentes verschwinden: ⎛ ⎞ Jx 0 0 JbF = ⎝ 0 Jy 0 ⎠ (10.29) 0 0 Jz Dies ist gleichbedeutend mit einem perfekt symmetrischen Aufbau des Flugger¨ats. Zus¨ atzlich kann das Tr¨ agheitsmoment der Rotoren vernachl¨assigt werden. Geht man davon

10.3 Systemsimulation

295

aus, dass das Drehmoment aufgrund der Drehung der Rotoren in der viskosen Luft proportional zur Differenz der Auftriebskr¨ afte der im Uhrzeigersinn und der entgegen dem Uhrzeigersinn drehenden Rotoren ist, so erh¨alt man  b ≈ Ωbib JbF ωibb + JbF ω M ˙ ibb ⎛

⎞ ⎛ ⎞ ⎛ b b b ⎞ ωib,y ωib,z (Jz − Jy ) Jx ω˙ ib,x (F4 − F2 )l b b b ⎠ ⎠ = ⎝ωib,x ⎝ ωib,z (Jx − Jz )⎠ + ⎝Jy ω˙ ib,y (F1 − F3 )l .(10.30) b b b (F1 + F3 − F2 − F4 )lc ωib,x ωib,y (Jy − Jx ) Jz ω˙ ib,z Die Proportionalit¨ atskonstante f¨ ur die Drehung um die z-Achse ist hierbei mit c bezeichnet. Die Differentialgleichung, die den Zusammenhang zwischen Auftriebskr¨aften und Drehraten beschreibt, ist damit gegeben durch Jy − J z (F4 − F2 )l + Jx Jx J − J (F z x 1 − F3 )l b b = ωib,x ωib,z + Jy Jy J − J (F x y 1 + F3 − F2 − F4 )lc b b = ωib,x ωib,y + . Jz Jz

b b b = ωib,y ωib,z ω˙ ib,x

(10.31)

b ω˙ ib,y

(10.32)

b ω˙ ib,z

(10.33)

F¨ ur den Reglerentwurf ist es sinnvoll, nicht direkt die Auftriebskr¨afte der Rotoren als Stellgr¨ oßen zu definieren, sondern geeignete Linearkombinationen der Auftriebskr¨afte zu w¨ ahlen. Eine m¨ ogliche Definition von Stellgr¨oßen u1 bis u4 ist u1 u2 u3 u4

= = = =

F1 + F2 + F3 + F4 F4 − F2 F1 − F3 F1 + F3 − F2 − F4 .

(10.34) (10.35) (10.36) (10.37)

Hierbei bezieht sich u1 auf den Gesamtauftrieb, beeinflusst also das Steigen und Sinken des Flugger¨ ats. Eine Drehung um die x-Achse und damit eine Rollwinkel¨anderung wird durch u2 kommandiert, u3 bestimmt die Drehung um die y-Achse und damit – bei vernachl¨ assigbarem Rollwinkel – den Pitchwinkel. Eine Drehung um die z-Achse wird u oße u4 geregelt, diese wird zur Erzwingung des gew¨ unschten Yaw¨ ber die Stellgr¨ Winkels ben¨ otigt. Insgesamt erh¨ alt man f¨ ur den Reglerentwurf folgendes Modell des VTOL MAV: 1 v˙ ibi = g i − Cibezb u1 m Jy − J z l b b b = ωib,y ωib,z + u2 ω˙ ib,x Jx Jx Jz − J x l b b b ω˙ ib,y = ωib,x ωib,z + u3 Jy Jy J − J lc x y b b b = ωib,x ωib,y + u4 ω˙ ib,z Jz Jz

(10.38) (10.39) (10.40) (10.41)

296

10 Anwendungsbeispiel unbemanntes Flugger¨at

W¨ ahrend f¨ ur modellbasierte Regler die Kopplung der drei die Rotation bestimmenden Differentialgleichungen einfach ber¨ ucksichtigt werden kann, muss f¨ ur den Entwurf von Single-Input-Single-Output-Reglern wie dem PID-Regler diese Kopplung vernachl¨assigt werden.

10.3.3

Einfluss der Trajektoriendynamik

Anhand des mathematischen Modells des VTOL MAVs kann auch der Einfluss trajektorienbedingter Beschleunigungen auf das bei GPS-Ausfall verwendete Lagefilter analysiert werden. Mit Gl. (10.15) und der Abk¨ urzung athrust =

1 (F1 + F2 + F3 + F4 ) m

(10.42)

kann die Beschleunigung des Flugger¨ ats wie folgt formuliert werden: ⎛

i aib,traj

⎞ ⎛ ⎞ sin φ sin ψ + cos φ sin θ cos ψ 0 = − ⎝− sin φ cos ψ + cos φ sin θ sin ψ ⎠ athrust + ⎝0⎠ cos φ cos θ g

(10.43)

Es soll angenommen werden, dass sich das MAV auf einer konstanten H¨ohe bewegt. Damit muss gelten: 0 = − cos φ cos θ · athrust + g g athrust = cos φ cos θ

(10.44)

Solange die Luftreibung, die die Beschleunigung des Flugger¨ats bremst, vernachl¨assigbar ist, erh¨ alt man die Beschleunigung des Flugger¨ats zu ⎛

i aib,traj

⎞ sin φ sin ψ + cos φ sin θ cos ψ g . = − ⎝− sin φ cos ψ + cos φ sin θ sin ψ ⎠ cos φ cos θ 0

(10.45)

Die Beschleunigungsmesser messen diese Beschleunigung zusammen mit der Schwerebeschleunigung. Geht man von idealen Beschleunigungssensoren aus und beachtet, dass die Schwerebeschleunigung als scheinbare Beschleunigung nach oben gemessen wird, so erh¨ alt man i T aib,traj + Ci,T aibb = Ci,T b  b (0, 0, −g) ⎞ ⎛ ⎛ ⎞ − sin θ sin θ = ⎝ sin2φ cos2θ ⎠ · g + ⎝ − sin φ cos θ ⎠ · g cos φ cos θ −1 − cos φ cos θ cos φ cos θ ⎞ ⎛ 0 ⎠ . 0 = ⎝ − cos φgcos θ

(10.46)

10.3 Systemsimulation

297

Das bedeutet, dass sich w¨ ahrend einer Beschleunigungsphase bei vernachl¨assigbarer Luftreibung die aufgrund der Schr¨ aglage einkoppelnde Schwerebeschleunigung und die trajektorienbedingte Beschleunigung f¨ ur den x- und den y-Beschleunigungsmesser gerade aufheben. W¨ ahrend einer solchen Beschleunigungsphase werden folglich systematisch fehlerhafte St¨ utzinformationen verarbeitet, eine verschwindende Beschleunigung in x- oder y-Richtung wird mit einem Roll- bzw. Pitchwinkel von null Grad in Verbindung gebracht. Das bedeutet aber auch, dass dieser systematische Fehler nicht gr¨oßer als der vorliegende Roll- bzw. Pitchwinkel ausf¨allt. Da die Beschleunigungsmessungen vom Lagefilter mit einer niedrigen Gewichtung verarbeitet werden k¨onnen und sich die bremsende Wirkung der viskosen Luft ziemlich schnell bemerkbar macht, werden die Beschleunigungsphasen anhand der Drehratensensordaten problemlos u uckt. ¨ berbr¨

10.3.4

Sch¨atzung von Modellparametern

F¨ ur die H¨ ohenregelung des VTOL MAV ist es wichtig, denjenigen Wert der Motorstellgr¨ oße zu kennen, bei dem das Flugger¨ at gerade schwebt. Dieser Wert ist von vielen Parametern abh¨ angig und daher vor dem Flug nur sehr grob bestimmbar: Das Abfluggewicht ist abh¨ angig von der Nutzlast, die Dichte der Luft variiert wetterabh¨angig und in Abh¨ angigkeit von der H¨ ohe des Abflugortes. Zus¨atzlich l¨asst im Laufe des Fluges die Akkuspannung nach, so dass gegen Ende des Fluges zum Schweben eine gr¨oßere Motorstellgr¨ oße ben¨otigt wird als zu Beginn. Daher wird bei dem Flugger¨at der zum Schweben ben¨ otigte Wert der Motorstellgr¨ oße von einem Kalman-Filter gesch¨atzt. Prinzipiell k¨ onnte hier auch ein H¨ ohenregler mit Integralanteil zum Einsatz kommen. Dabei treten jedoch eine Reihe von Schwierigkeiten auf, in der Startphase soll die zum Schweben ben¨ otigte Motorstellgr¨ oße schnell gefunden werden, sp¨ater sollen nur noch vorsichtige ¨ Anderungen erfolgen. Damit ist es praktisch unm¨oglich, den Integralanteil des Reglers so zu w¨ ahlen, dass sowohl w¨ ahrend der Startphase als auch im weiteren Flugverlauf gute Ergebnisse resultieren. Hinzu kommt, dass sich ein großer Integralanteil, der f¨ ur einen z¨ ugigen Start ben¨ otigt wird, destabilisierend auf die H¨ohenregelung auswirken kann. Im Folgenden sollen zwei Kalman-Filter zur Sch¨atzung der zum Schweben ben¨otigten Motorstellgr¨ oße entworfen und verglichen werden. Dabei wird davon ausgegangen, dass eine vorliegende Schr¨ aglage des Flugger¨ ates durch die im vorigen Abschnitt ermittelten Kosinusterme von Roll- und Pitchwinkel kompensiert wird, so dass sich das Flugger¨at f¨ ur das Filter so darstellt, als w¨ are es horizontal ausgerichtet. Die aktuelle Motorstellg¨oße soll mit sk bezeichnet werden, die zum Schweben ben¨otigte Motorstellgr¨oße sei shov . Der Zusammenhang zwischen Motorstellgr¨oße und Drehzahl kann n¨aherungsweise als linear angenommen werden, ωbr = c1 sk ,

(10.47)

w¨ ahrend die Auftriebskraft n¨ aherunsweise proportional zum Quadrat der Drehzahl ist. Damit erh¨ alt man u1 = F1 + F2 + F3 + F4 = c2 c21 s2k .

(10.48)

Die Beschleunigung des MAVs nach Unten, ad , ist somit gegeben durch ad,k = −

 1  2 2 c2 c1 sk − c2 c21 s2hov . m

(10.49)

298

10 Anwendungsbeispiel unbemanntes Flugger¨at

Diese Beschleunigung kann anhand der Inertialsensoren und der Flugger¨atlage ermittelt werden. Filter A Gl. (10.49) kann daher als Grundlage f¨ ur den Entwurf eines ersten Kalman-Filters mit dem Zustandsvektor 2

s x1 2 = chov (10.50) xk = 2 c1 x2 m

dienen, das im Weiteren als Filter A bezeichnet wird. Dieses Filter sch¨atzt das Quadrat der gesuchten Motorstellgr¨ oße shov , da der Zusammenhang zwischen der Messgr¨oße ad und s2hov weniger nichtlinear ist als der Zusammenhang zwischen ad und shov . Dennoch liegt eine nichtlineare Messgleichung vor, die Messmatrix muss durch Linearisierung gefunden werden. Man erh¨ alt als linearisiertes Messmodell

  s2hov 2 2 ad,k = x2,k −sk + x1,k (10.51) + vk , c2 c1 m

wobei vk das Messrauschen bezeichnet. Das Systemmodell des Filters ist sehr einfach, beide Komponenten des Zustandsvektors k¨onnen als Random-Walk-Prozesse modelliert werden. Filter B Wenn man ausnutzt, dass die Motorstellgr¨oße shov per Definition gerade die Schwerebeschleunigung g kompensiert, l¨ asst sich dieses Sch¨atzproblem sogar als lineares Sch¨atzproblem formulieren: Gl. (10.49) kann umgeschrieben werden zu c2 c21 s2k +g m c2 c21 ad,k − g = −s2k · . m ad,k = −

(10.52)

Addiert man auf der rechten Seite das Messrauschen hinzu, so kann Gl. (10.52) als Messgleichung eines Kalman-Filters, Filter B, mit nur einer einzigen Zustandsvariable x1 verstanden werden: 2

c2 c1 xk = (x1 )k = (10.53) m k Da bei der zum Schweben ben¨ otigten Motorstellgr¨oße die Vertikalbeschleunigung verschwindet, kann man anhand der gesch¨ atzten Zustandvariable x ˆ1,k und Gl. (10.52) die gesuchte Motorstellgr¨ oße shov bestimmen: c2 c21 −g = −s2hov · m # g sˆhov = x ˆ1

(10.54)

10.3 Systemsimulation

299

8 Vertikalbeschleunigung 6 4

(m/s2 )

2 0 -2 -4 -6 -8 0

10

20

30

40

50 Zeit (s)

60

70

80

90

100

Abbildung 10.5: F¨ ur den simulativen Vergleich von Filter A und Filter B generierte Vertikalbeschleunigung.

Vergleich der Filter Die beiden Filter wurden in numerischen Simulationen verglichen. Dabei zeigte sich, dass Filter A eine deutlich gr¨ oßere Trajektoriendynamik in vertikaler Richtung und einen deutlich l¨ angeren Beobachtungszeitraum ben¨otigt als Filter B, um die Motorstellgr¨ oße shov zu sch¨ atzen. Abb. 10.5 zeigt die bei einer solchen numerischen Simulation als Messwert zur Verf¨ ugung gestellte Vertikalbeschleunigung, hier liegt sicherlich eine gr¨oßere Dynamik vor, als in der Praxis erwartet werden kann. Trotz dieser großen Dynamik hat Filter A die zum Schweben ben¨ otigt Motorstellgr¨oße auch nach hundert Sekunden nur unzureichend gesch¨ atzt, siehe Abb. 10.6, wohingegen Filter B innerhalb weniger Sekunden eine brauchbare Sch¨ atzung liefert. Die Ursache hierf¨ ur ist in der Nichtlinearit¨ at der Messgleichung von Filter A zu suchen: Solange die Zustandssch¨ atzung des Filters noch grob falsch ist, ist auch der Linearisierungspunkt f¨ ur die Berechnung der Messmatrix grob falsch. Damit werden die Messwerte nicht korrekt verarbeitet, was eine z¨ ugige Konvergenz der Zustandssch¨atzung verhindert. Bei dem hier betrachteten VTOL MAV wird daher ausschließlich Filter B zur Sch¨ atzung der zum Schweben ben¨ otigten Motorstellgr¨oße eingesetzt.

10.3.5

Ergebnisse der Gesamtsystemsimulation

Die Leistungsf¨ ahigkeit des integrierten VTOL-MAV-Navigationssystems wurde in numerischen Simulationen analysiert. Hierzu wurden einige Wegpunkte vorgegeben, die unter Verwendung einer geeigneten Flugf¨ uhrungsalgorithmik nacheinander angeflogen

300

10 Anwendungsbeispiel unbemanntes Flugger¨at

210

Motorstellgr¨ oße

200



















190 

180 

170







 160 150  





10

20

Wahrer Wert Filter B Filter A



 

140 0

30

40

50 Zeit (s)

60

70

80

90

100

Abbildung 10.6: Sch¨ atzung der zum Schweben ben¨ otigten Motorstellgr¨ oße.

wurden. Die von den Flugregelungsalgorithmen ben¨otigten Navigationsinformationen wurden dabei von dem intergrierten Navigationssystem zur Verf¨ ugung gestellt. Die vom Navigationssystem verarbeiteten, synthetischen Navigationssensordaten wurden anhand von Fehlermodellen generiert, die den im Flugger¨at vorhandenen Sensoren nachempfunden waren. Die u unfundzwanzig Simulationsl¨ aufe gemittelten Roll- und Pitchwinkelfehler sind ¨ ber f¨ f¨ ur verschiedene Szenarien in Abb. 10.7 und Abb. 10.8 zu sehen. Es ist zu erkennen, das bei kontinuierlicher GPS-Verf¨ ugbarkeit nach einer kurzen Konvergenzphase des Filters Lageinformationen gleichbleibender Qualit¨at resultieren. Zus¨ atzlich sind noch die Lagefehler dargestellt, die resultieren, wenn im Zeitraum von dreihundert bis sechshundertf¨ unfzig Sekunden keine GPS-Informationen verarbeitet ¨ werden k¨ onnen. Bei einer Uberbr¨ uckung des GPS-Ausfalls durch rein inertiale Navigation wachsen die Lagefehler kontinuierlich an. Hierbei ist es nur eine Frage der Zeit, wann keine sinnvolle Stabilisierung des Flugger¨ats mehr m¨oglich ist. Wird jedoch w¨ahrend des GPS-Ausfalls auf das Lagefilter umgeschaltet, das die beschriebene, gravitationsvektorbasierte St¨ utzung verwendet, kann eine Lagegenauigkeit aufrechterhalten werden, die zwar schlechter ist als bei GPS-Verf¨ ugbarkeit, aber zur Stabilisierung des Flugger¨ ats immer noch ausreicht. Vor allem bei den Rollwinkelfehlern ist der Einfluss der Trajektoriendynamik zu erkennen, dieser ist jedoch nicht ausreichend, um die Funktionsf¨ ahigkeit des Gesamtsystems zu gef¨ ahrden.

10.3 Systemsimulation

301

Rollwinkelfehler (deg)

20  

15

Volle GPS Verf¨ ugbarkeit Ohne gravitationsbasierte St¨ utzung Mit gravitationsbasierter St¨ utzung  

10



  

5



      

0 0

100

200



300

   

 

400 500 Zeit (s)

    

600

700

800

900

Abbildung 10.7: Mittlere Rollwinkelfehler bei f¨ unfundzwanzig Simulationsl¨ aufen.

Pitchwinkelfehler (deg)

20  

15

Volle GPS Verf¨ ugbarkeit Ohne gravitationsbasierte St¨ utzung Mit gravitationsbasierter St¨ utzung 

10



   

5

0

                     0

100

200

300

400 500 Zeit (s)

600

700

800

Abbildung 10.8: Mittlere Pitchwinkelfehler bei f¨ unfundzwanzig Simualtionsl¨ aufen.

900

302

10.4

10 Anwendungsbeispiel unbemanntes Flugger¨at

Experimentelle Verifikation

Die in den numerischen Simulationen mit synthetischen Sensordaten erzielten Ergebnisse konnten in Flugversuchen best¨ atigt werden. Dazu wurde das Lagefilter und das GPS/INS-Filter auf dem Mikrokontroller des VTOL MAV implementiert. Hierbei ist eine effiziente Umsetzung der Algorithmen von entscheidender Bedeutung, da die zur Verf¨ ugung stehende Rechenleistung begrenzt ist. So ist die Kovarianzmatrix des Sch¨atzfehlers beispielsweise eine sysmmetrische Matrix, d.h. es gen¨ ugt, die obere Dreiecksmatrix zu berechnen. Dadurch werden zum einen eine Vielzahl von Multiplikationen eingespart, zum anderen wird dadurch die Symmetrie der Kovarianzmatrix sichergestellt, die bei vollst¨ andiger Berechnung aufgrund von Rundungsfehlern verloren gehen kann. Diese Vorgehensweise kann als Vorstufe eines Square-Root-Filters betrachtet werden, der mathematisch a ¨quivalent zu einem gew¨ohnlichen Kalman-Filter ist, aber numerisch deutlich robuster ist. Bei diesen Filtern wird die Kovarianzmatrix in eine Dreiecksmatrix und in eine Diagonalmatrix zerlegt. Weiteres Einsparungspotential bietet die schwach besetzte Transitionsmatrix, da Multiplikationen mit null nicht ausgef¨ uhrt werden m¨ ussen. Bei dem im Folgenden betrachteten Flugversuch wurde zu Testzwecken ausschließlich das Lagefilter verwendet. Damit beweist dieser Flugversuch, dass eine Stablisierung des Flugger¨ ats, wie aufgrund der numerischen Simulationen erwartet, auch ohne GPSMessungen gelingt. W¨ ahrend dieses Flugversuchs wurden alle Navigationssensordaten, auch die des GPSEmpf¨ angers, auf eine Speicherkarte aufgezeichnet. Somit konnten anschließend offline diese Sensordaten mit verschiedenen Navigationsalgorithmen verarbeitet werden. Absolute Aussagen bez¨ uglich der vorliegenden Lagefehler k¨onnen nat¨ urlich nicht getroffen werden, da eine ideale Referenz fehlt. Die Verschlechterung der Lagegenauigkeit bei GPS-Ausfall kann hingegen ermittelt werden. Dazu wurde zun¨achst unter Verwendung des GPS/INS-Filters die Navigationsl¨ osung berechnet, die sich bei einer kontinuierlichen GPS-Verf¨ ugbarkeit ergibt. Diese Navigationsl¨osung wurde dann als n¨aherungsweise Referenzl¨ osung verwendet. Schließlich wurde in den Navigationssensordaten ein k¨ unstlicher GPS-Ausfall von 175 bis 375 Sekunden erzwungen. Die Ver¨anderung der Roll- und Pitchwinkelsch¨ atzung gegen¨ uber der n¨aherungsweisen Referenzl¨osung ist sowohl f¨ ur das mit gravitationsvektorbasierter St¨ utzung arbeitende Lagefilter als auch f¨ ur ¨ eine rein inertiale, ungest¨ utzte Uberbr¨ uckung des GPS-Ausfalls in Abb. 10.9 und Abb. 10.10 dargestellt. Es liegt nahe, diese Ver¨ anderung der Lagewinkel als zus¨atzliche Lagefehler zu interpretieren. Damit decken sich die anhand realer Flugversuchsdaten ermittelten Ergebnisse sehr gut mit den Simulationsergebissen aus Abschnitt 10.3.5. Zusammenfassend l¨ asst sich sagen, das ein integriertes Navigationssystem f¨ ur ein VTOL MAV resultiert, das unter allen zu erwartenden Szenarien Navigationsinformationen liefern kann, die zur Lage- und H¨ ohenregelung des Flugger¨ats geeignet sind. Nat¨ urlich kann ein solches Navigationssystem auch bei anderen Anwendungen wie Fl¨achenfl¨ uglern oder fahrbaren Robotern eingesetzt werden.

10.4 Experimentelle Verifikation

303

30  

Rollwinkelfehler (deg)

20

Ohne gravitationsbasierte St¨ utzung Mit gravitationsbasierter St¨ utzung

10  0        

  





     





-10 -20 0

100

200

300

400

500

Zeit (s) Abbildung 10.9: Ver¨ anderung der Rollwinkelsch¨ atzung bei einem k¨ unstlichen GPS-Ausfall von 175 bis 375 Sekunden gegen¨ uber der Rollwinkelsch¨ atzung mit kontinuierlicher GPSVerf¨ ugbarkeit.

30  

Rollwinkelfehler (deg)

20

Ohne gravitationsbasierte St¨ utzung Mit gravitationsbasierter St¨ utzung 

10  0         

 



-10





    



-20 0

100

200

300

400

500

Zeit (s) Abbildung 10.10: Ver¨ anderung der Pitchwinkelsch¨ atzung bei einem k¨ unstlichen GPS-Ausfall von 175 bis 375 Sekunden gegen¨ uber der Pitchwinkelsch¨ atzung mit kontinuierlicher GPSVerf¨ ugbarkeit.

304

10.4.1

10 Anwendungsbeispiel unbemanntes Flugger¨at

Kalibration der Beschleunigungsmesser

Im Endeffekt werden sowohl beim GPS/INS-Filter als auch bei dem Lagefilter die Rollund Pitchwinkel durch die Beschleunigungssensordaten beeinflusst. Ein Beschleunigungsmesserbias ruft daher unmittelbar einen entsprechenden Lagefehler hervor. Da das GPS/INS-Filter durch die Verarbeitung von GPS-Messungen die Beschleunigungsmesserbiase sehr z¨ ugig sch¨ atzen kann, stellt dies bei GPS-Verf¨ ugbarkeit kein Problem dar. Stehen jedoch keine GPS-Informationen zur Verf¨ ugung, k¨onnen die Beschleunigungsmesserbiase nicht gesch¨ atzt werden und die Roll- und Pitchwinkelsch¨atzung des Lagefilters bleibt dauerhaft mit einem Offset behaftet. Aufgrund der Temperaturempfindlichkeit der MEMS-Sensoren sind aber zum Beispiel bei Zimmertemperatur bestimmte Kalibrationsparameter bei K¨ alte im Freien nicht mehr g¨ ultig, bei einem Temperatursprung von zwanzig Grad k¨ onnen sich die Beschleunigungsmesserbiase um 1 m/s2 und mehr ¨ andern. Daher ist es von Vorteil, u ¨ ber eine schnelle und einfache M¨oglichkeit zur Kalibration der Beschleunigungsmesser zu verf¨ ugen. In [34] wird ein Verfahren zur Kalibration von Zweiachs- und Dreiachs-Magnetometern vorgeschlagen. Das im Folgenden beschriebene Verfahren zur Kalibration der Beschleunigungsmessertriade ist diesem Verfahren sehr ¨ahnlich. Es wird ein deterministisches Beschleunigungsmesserfehlermodell der Form ⎛ ⎞ ⎛1 ⎞⎛ ⎞ ⎛ ⎞ a ˜x ax bx sx 0 0 ⎝a ˜y ⎠ = ⎝ 0 s1y 0 ⎠ ⎝ay ⎠ + ⎝by ⎠ a ˜z az bz 0 0 s1z

(10.55)

angenommen, die stochastischen Sensorfehler werden nicht betrachtet. Die gemessenen Beschleunigungen sind mit a ˜x , a ˜y , und a ˜z bezeichnet, die wahren Beschleunigungen mit ax , ay und az . Bei sx , sy und sz handelt es sich um Skalenfaktoren, bx , by und bz sind die Beschleunigungsmesserbiase, diese Gr¨ oßen sollen bestimmt werden. Aufgel¨ost nach den wahren Beschleunigungen ergibt sich ⎞⎛ ⎞ ⎛ ⎞ ⎛ sx 0 0 a ˜ x − bx ax ⎝ ay ⎠ = ⎝ 0 sy 0 ⎠ ⎝ a ˜ y − by ⎠ . (10.56) 0 0 sz az a ˜z − bz W¨ ahrend der Kalibration soll das Flugger¨at in verschiedene Lagen gebracht werden, so dass bei jedem Beschleunigungsmesser die Schwerebeschleunigung einkoppeln kann. Dabei wird eine Beschleunigung des Flugger¨ates nach M¨oglichkeit vermieden. Somit kann ausgenutzt werden, dass die Quadratsumme der gemessenen Beschleunigungen in den drei Raumrichtungen bei korrekt kalibirerten Sensoren gerade dem Quadrat der Schwerebeschleunigung enspricht: g 2 = a2x + a2y + a2z 2

2

2

= s2x (˜ ax − bx ) + s2y (˜ ay − by ) + s2z (˜ az − b z )     ˜2x − 2˜ ˜2y − 2˜ = s2x a ax bx + b2x + s2y a ay by + b2y  2  az bz + b2z +s2z a ˜z − 2˜

(10.57)

10.4 Experimentelle Verifikation

305

Es l¨ asst sich ein Zustandsvektor definieren, der Produkte der gesuchten Biase und Skalenfaktoren enth¨ alt, die dann linear mit dem Quadrat der Schwerebschleunigung zusammenh¨ angen: ⎞ s2x s2x bx ⎟ ⎜ ⎜ ⎟ 2 s ⎟ ⎜ y   ⎜ ⎟ ˜2x −2˜ ax a ˜2y −2˜ ay a ˜2z −2˜ az 1 ⎜ s2y by g2 = a ⎟ . ⎜ ⎟ s2z ⎟ ⎜ ⎝ ⎠ 2 sz b z 2 2 2 2 2 2 sx b x + sy b y + sz b z ⎛

(10.58)

Fasst man das Quadrat der Schwerebeschleunigung als Pseudo-Messwert und die die Beschleunigungsmessungen enthaltende Matrix als Messmatrix auf und erg¨anzt noch einen Messrauschterm, so liegt mit Gleichung (10.58) ein lineares Messmodell in der bei einem Kalman-Filter u ¨ blichen Form vor. Diese Gleichung bildet damit die Grundlage eines Kalman-Filters zur Kalibration der Beschleunigungssensoren. Das Systemmodell kann sehr einfach gew¨ ahlt werden, die Komponenten des Zustandsvektors k¨onnen als konstant angenommen werden. Genau wie bei dem Filter zur Sch¨atzung der zum Schweben ben¨ otigten Motorstellgr¨ oße wird hier durch die Sch¨atzung von Produkten der interessierenden Gr¨ oßen die Nichtlinearit¨ at des Sch¨atzproblems umgangen, die bei einer direkten Sch¨ atzung von Biasen und Skalenfaktoren zu massiven Problemen gef¨ uhrt h¨ atte. Bezeichnet man die Komponenten des Zustandsvektors mit x1 bis x7 , so kann anhand der vom Kalman-Filter gelieferten Zustandssch¨atzung u ¨ ber die Gleichungen sˆx = ˆbx = sˆy = ˆby = sˆz = ˆbz =

 x ˆ1 x ˆ2 x ˆ 1 x ˆ3 x ˆ4 x ˆ 3 x ˆ5 x ˆ6 x ˆ5

(10.59) (10.60) (10.61) (10.62) (10.63) (10.64)

auf die zu bestimmenden Skalenfaktoren und Biase geschlossen werden.

10.4.2

Ergebnisse

Das vorgeschlagene Verfahren zur Kalibration der Beschleunigungsmesser wurde experimentell auf seine Tauglichkeit getestet. Dazu wurden die Daten der unkalibrierten Beschleunigungssensoren aufgezeichnet, w¨ ahrend das Flugger¨at von Hand in verschiedene Lagen gebracht wurde, um ein Einkoppeln der Schwerebeschleunigung in die sensitiven Achsen der Beschleunigungsmesser zu erm¨ oglichen.

306

10 Anwendungsbeispiel unbemanntes Flugger¨at 12 Unkalibrierte Beschleunigungsssensoren

Gemessene Erdbeschl. (m/s2 )

11.5 11 10.5 10 9.5 9 8.5 8 7.5 0

10

20

30

40 Zeit (s)

50

60

70

80

Abbildung 10.11: Schwerebeschleunigung bei Variation der Flugger¨ atlage, berechnet anhand der unkalibrierten Beschleunigunssensoren.

Abb. 10.11 zeigt die Schwerebeschleunigung, die anhand der unkalibrierten Beschleunigungsmesser berechnet wurde. Man erkennt, dass die berechnete Schwerebeschleunigung teilweise deutlich von ihrem Nominalwert 9.81 m/s2 abweicht. Da die Beschleunigungssensoren unterschiedliche Biase und Skalenfaktoren aufweisen, ist die berechnete Schwerebeschleunigung lageabh¨ angig, was die Variation der berechneten Schwerebeschleunigung erkl¨ art. Anschließend wurden die Daten der unkalibrierten Beschleunigungssensoren dem auf Gl. (10.58) basierenden Kalibrationsfilter zur Verf¨ ugung gestellt. Abb. 10.12 zeigt examplarisch die dabei gesch¨ atzten Beschleunigungsmesserbiase, die nach kurzer Zeit auf konstante Werte konvergieren. Abb. 10.13 zeigt die Standardabweichungen der Komponenten des Zustandsvektors, die auch die Beschleunigungsmesserbiase enthalten. Offensichtlich ist die Konvergenz einer Biassch¨ atzung auf einen konstanten Wert auch mit der Abnahme der zugeh¨origen Standardabweichung verbunden. Diese findet statt, sobald die Lage des Flugger¨ats so variiert wurde, dass die Schwerebeschleunigung in die entsprechende Beschleunigunsmesserachse positiv und negativ eingekoppelt hat; ohne diese Einkopplung sind die Biase und Skalenfaktoren unbeobachtbar. Die ermittelten Kalibrationsparameter wurden verifiziert, in dem die w¨ahrend der Kalibrationsphase gemessenen Beschleunigungen mit Hilfe dieser Kalibrationsparameter korrigiert wurden. Die korrigierten Beschleunigungen wurden erneut zur Berechnung der Schwerebeschleunigung herangezogen, die so berechnete Schwerebeschleunigung ist

10.4 Experimentelle Verifikation

307

x y Biassch¨ atzung (m/s2 )

1 ♦

♦

♦



 

♦

 z  ♦ ♦

♦

♦

♦

♦

♦





























0.5

♦  

0



 

-0.5 0

10

20

30

40 Zeit (s)

50

60

70

80

Standardabweichung Biassch¨atzung (m/s2 )

Abbildung 10.12: Gesch¨ atzte Beschleunigungsmesserbiase, ermittelt mit dem Kalibrationsfilter.

0.01

 ♦

 



0.008

x y z



♦  



0.006

0.004

0.002

♦

♦

♦ 



♦ ♦ ♦     ♦  ♦  ♦ ♦  

0 0

10

20

30

40 Zeit (s)

50

60

70

80

Abbildung 10.13: Standardabweichung der Komponenten des Zustandsvektors des Kalibrationsfilters, die auch die Beschleunigungsmesserbiase enthalten.

308

10 Anwendungsbeispiel unbemanntes Flugger¨at 12 Kalibrierte Beschleunigungssensoren

Gemessene Erdbeschl. (m/s2 )

11.5 11 10.5 10 9.5 9 8.5 8 7.5 0

10

20

30

40 Zeit (s)

50

60

70

80

Abbildung 10.14: Schwerebeschleunigung bei Variation der Flugger¨ atlage, berechnet anhand der kalibrierten Beschleunigunssensoren.

in Abb. 10.14 zu sehen. Man erkennt, dass die berechnete Schwerebeschleunigung gut mit ihrem Nominalwert u ¨ bereinstimmt, auch die bei den unkalibrierten Beschleunigungssensoren beobachteten, lagebedingten Variationen sind verschwunden. Damit ist die Praxistauglichkeit diese Verfahrens f¨ ur eine schnelle, einfache Kalibrati¨ on der Beschleunigungssensoren bewiesen. Ublicherweise wird zur Kalibration von Beschleunigungsmessern eine exakt horizontal ausgerichtete Fl¨ache verwendet, diese wird hier nicht ben¨ otigt, da einige m¨ oglichst unbeschleunigte Dreh- und Schwenkbewegungen des Flugger¨ ats gen¨ ugen.

A

Sherman-Morrison-WoodburyFormel

Gegeben ist eine Matrix A der Dimension n × n sowie zwei Matrizen B und C jeweils der Dimension n × m und eine Matrix D der Dimension m × m. Unter der Voraussetzung, dass entsprechende Matrizen invertierbar sind, lautet die Sherman-Morrison-Woodbury-Formel:  −1 A + BDCT

=

 −1 T −1 A−1 − A−1 B D−1 + CT A−1 B C A (A.1)

Dieser Zusammenhang ist auch als Matrix-Inversions-Lemma bekannt. H¨aufig findet man auch eine etwas vereinfachte Darstellung, die sich aus Gl. (A.1) durch Einsetzen von D = I ergibt. −1  A + BCT

 −1 T −1 A−1 − A−1 B I + CT A−1 B C A

=

(A.2)

Eine weitere n¨ utzliche Matrizenidentit¨ at ist gegeben durch −1  B A + BCT

=

 −1 A−1 B I + CT A−1 B .

(A.3)

Ein Beweis f¨ ur Gl. (A.2) und Gl. (A.3) ist in [90] zu finden. Mit Hilfe dieser Zusammenh¨ ange kann z.B. die Invers-Kovarianzform des Kalman Filters aus der Standardform hergeleitet werden und umgekehrt.

Invers-Kovarianz-Form des Kalman-Filters Mit den Substitutionen A = Rk B= I T CT = Hk P− k Hk

310

Sherman-Morrison-Woodbury-Formel

folgt aus Gl. (A.3) −1    T T −1 −1 = R−1 . I + Hk P− Hk P− k Hk + R k k k Hk R k

(A.4)


Setzt man die Kalman-Gain-Matrix  −1 − T T Kk = P− k Hk Hk Pk Hk + Rk

(A.5)

in die Kovarianzmatrix-Update-Gleichung − − P+ k = Pk − Kk Hk Pk

(A.6)

ein, erh¨ alt man  −1 − − T − T P+ Hk P− k = Pk − Pk Hk Hk Pk Hk + Rk k .

(A.7)

Mit Gl. (A.4) ergibt sich   − − T −1 T −1 −1 P+ I + Hk P− Hk P− k = Pk − Pk Hk Rk k Hk R k k .

(A.8)

Mit den Substitutionen A−1 = P− k B = HTk R−1 k CT = H k folgt aus Gl. (A.2) und Gl. (A.8)   − − T −1 T −1 −1 P+ Hk P− I + Hk P− k = Pk − Pk Hk Rk k Hk R k k  − −1 −1 T −1 = (Pk ) + Hk Rk Hk .

(A.9)


+ −1 und I = (R−1 uhrt auf Erweitern von Gl (A.5) mit I = P+ k (Pk ) k Rk ) f¨

−1  + −1 −1 − T T Kk = (P+ )P− k (Pk ) k Hk (Rk Rk ) Hk Pk Hk + Rk   + −1 − T −1 T −1 −1 = P+ Pk Hk Rk Rk R−1 I + Hk P− k (Pk ) k k Hk R k   + −1 − T −1 T −1 −1 = P+ Pk Hk Rk I + Hk P− . k (Pk ) k Hk R k

(A.10)

Sherman-Morrison-Woodbury-Formel

311

Mit dem Zusammenhang  + −1 −1 = (P− + HTk R−1 Pk k) k Hk

(A.11)

aus Gl. (A.9) erh¨ alt man so  − −1  − T −1   T −1 −1 Kk = P+ I + Hk P− + HTk R−1 k (Pk ) k Hk Pk Hk Rk k Hk R k   − −1 − T −1   − T −1 T −1 −1 = P+ Pk Hk Rk I + Hk P− k I + Hk Rk Hk Pk (Pk ) k Hk R k   T −1   − T −1 T −1 −1 = P+ I + Hk P− k I + Hk Rk Hk Pk Hk Rk k Hk R k  T −1   − T −1 T −1 T −1 −1 I + Hk P− = P+ k Hk Rk + Hk Rk Hk Pk Hk Rk k Hk R k    T −1 T −1 T −1 −1 I + Hk P− I + Hk P− = P+ k Hk R k k Hk R k k Hk R k T −1 Kk = P+ k Hk R k .

(A.12)


Durch Multiplikation der Kalman-Filter-Zustandsvektor-Update-Gleichung  ˆ− − Kk Hk x ˆ− − y˜k xˆ+ x k =  k k −1 von links mit (P+ und Einsetzen von Gl. (A.12) erh¨alt man: k)

 + −1 ˆ− + −1 + T −1 − −1 ˆ+ ˜ ˆ (P+ H ) = (P ) − (P ) P H R − y   x  x  x k k k k k k k k k k k  −  + −1 + −1 ˆ+ T −1 T −1 ˜ ˆ (Pk ) xk = (Pk ) − Hk Rk Hk xk + Hk Rk yk Mit Gl. (A.11) folgt −1 ˆ+ −1 ˆ− xk = (P− xk + HTk R−1 y˜k (P+ k) k) k   + −1 − −1 ˆ− T −1 ˜ (P . = (P ) ) + H R xˆ+  x y  k k k k k k k

(A.13)

Die Gleichungen (A.11) und (A.13) stellen den Messschritt der Invers-Kovarianz-Form des Kalman-Filters dar.

B

Differentiation von Spuren von Matrizen

Bei der Minimierung von Kostenfunktionen im Rahmen der Herleitung von MinimumVarianzsch¨ atzern ist es h¨ aufig notwendig, die Ableitung der Spur einer Matrix nach einer Matrix zu berechnen. Die wichtigsten Zusammenh¨ ange sind dabei gegeben durch d Spur(B) dB d Spur(Bi ) dB d Spur(AB−1 C) dB d Spur(AT BCT ) dB d Spur(ABCBT ) dB d Spur(ABCB) dB

= I

(B.1)

= i · Bi−1,T

(B.2)

= −(B−1 CAB−1 )T

(B.3)

=

d Spur(CBT A) = AC dB

(B.4)

= AT BCT + ABC

(B.5)

= AT BT CT + CT BT AT .

(B.6)

Ferner gilt Spur(B) = Spur(BT ) Spur(AB) = Spur(BA) .

(B.7) (B.8)

C

MATLAB-Code zum Beispiel Abschnitt 7.3.5

Im Folgenden ist der MATLAB-Code der Filter, die zum Sch¨atzen der Schwingung eines Pendels verwendet wurden, angegeben, er steht aber auch online u ¨ ber die Homepage des Verlags (www.oldenbourg-wissenschaftsverlag.de) zur Verf¨ ugung. Die Filter verf¨ ugen jeweils u ¨ ber eine Initialisierungsfunktion, die einmal zu Beginn aufgerufen werden muss. Der Propagationsschritt und der Messschritt sind ebenfalls in separaten Funktionen realisiert. Diese Funktionen k¨ onnen von einem Scheduler entsprechend aufgerufen werden.

Initialisierungsschritt des Partikelfilters function pf = pf_filter_init(init_angle) pf.num = 300; pf.X = zeros(2,pf.num); pf.w = 1/pf.num*ones(1,pf.num); pf.Q = [(1/180*pi)^2, 0; 0, (1e-4)^2]; pf.cholQ = chol(pf.Q); pf.R = 0.1^2; pf.Xs = [init_angle/180*pi;0]; pf.Ps = [(init_angle/180*pi)^2,0; 0, (10/180*pi)^2];

% % % %

Anzahl der Partikel Partikel anlegen Gewichte Systemrauschen definieren

% Varianz der Messungen % Initialer Zustand % Initiale Unsicherheit

% Ziehen der Partikel entsprechend initialem Zustand/Unsicherheit cholP = chol(pf.Ps); for k=1:pf.num pf.X(:,k) = cholP*randn(2,1) + pf.Xs; end end

Propagationsschritt des Partikelfilters function pf = pf_filter_propagation(pf,pend,dt) % Propagation der Partikel entsprechend dem Systemmodell % und dem Systemrauschen for k=1:pf.num tmp = pf.X(1,k); pf.X(1,k) = pf.X(1,k) + pf.X(2,k)*dt ...

316

MATLAB-Code zum Beispiel Abschnitt 7.3.5 + randn(1)*pf.cholQ(1,1); pf.X(2,k) = pf.X(2,k) - pend.g/pend.length*sin(tmp)*dt ... + randn(1)*pf.cholQ(2,2); if (pf.X(1,k) pf.X(1,k) end if (pf.X(1,k) pf.X(1,k) end

> pi) % Winkelbereich auf +/- 180◦ beschr¨ anken = pf.X(1,k) - 2*pi; < -pi) = pf.X(1,k) + 2*pi;

end end Bei der Eingangsvariable dt handelt es sich um die Zeitschrittweite, pend.g ist die Schwerebeschleunigung und pend.length ist die L¨ange des Pendels.

Messwertverarbeitung und Resampling beim Partikelfilter function pf = pf_filter_estimation(pf,pend,y) for k=1:pf.num pf.w(k) = exp(-1/2*(y - pend.length*sin(pf.X(1,k)))^2/pf.R); end pf.w = pf.w/sum(pf.w); peff = 1/sum(pf.w.^2); % Anzahl der effektiven Partikel bestimmen if (peffzf) pf.resampled(:,k) = pf.X(:,j); % Partikel break; % reproduzieren end end end pf.X = pf.resampled; end end Bei der Eingangsvariable y handelt es sich um den Messwert.

MATLAB-Code zum Beispiel Abschnitt 7.3.5

317

Initialisierungsschritt des Kalman-Filters function kf = kf_filter_init(init_angle) kf.Q = [(1/180*pi)^2, 0; % 0, (1e-4)^2]; kf.R = 0.1^2; % kf.X = [init_angle/180*pi;0]; % kf.P = [(init_angle/180*pi)^2,0; % 0, (10/180*pi)^2]; end

Systemrauschen definieren Varianz der Messwerte Initialer Zustand Initiale Unsicherheit

Propagationsschritt des Kalman-Filters function kf = kf_filter_propagation(kf,pend,dt) % Zustandssch¨ atzung propagieren tmp = kf.X(1); kf.X(1) = kf.X(1) + kf.X(2)*dt; kf.X(2) = kf.X(2) - pend.g/pend.length*sin(tmp)*dt; % Jacobi-Matrix der Systemmodell-DGL berechnen Phi = [1 , dt; -pend.g/pend.length*cos(tmp)*dt, 1]; % Kovarianzmatrix des Sch¨ atzfehlers propagieren kf.P = Phi*kf.P*Phi’ + kf.Q; end

Messwertverarbeitung des Kalman-Filters function kf = kf_filter_estimation(kf,pend,y) y_pred = pend.length*sin(kf.X(1)); % Messwert-Vorhersage H = [pend.length*cos(kf.X(1)), 0]; % Messmatrix K = kf.P*H’*inv(H*kf.P*H’ + kf.R); % Kalman Gain Matrix kf.X = kf.X - K*(y_pred - y); % Zustandssch¨ atzung updaten % Update der Kovarianzmatrix des Sch¨ atzfehlers (Joseph’s Form) kf.P = (eye(2) - K*H)*kf.P*(eye(2) - K*H)’ + K*kf.R*K’; end

Symbolverzeichnis Abk¨ urzungen AAIM ADR BOC BPSK C/A CDMA CS DCM DGPS DOP ECEF EGNOS EKF E5, E6 FOG GCC GDOP GJU GPS GSS GSSS HDOP HIL IEKF IMM IMU INS IOC L1,L2,L5 MAV MEMS MMAE OS OOSM P-Code PDOP PLL

Aircraft Autonomous Integrity Monitoring Accumulated Deltarange Binary Offset Carrier Binary Phase-shift Keying Coarse/Acquisition Code Division Multiple Access Galileo Commercial Service Direction Cosine Matrix, Richtungskosinusmatrix Differential-GPS Dilution of Precision Earth-centered Earth-fixed European Geostationary Navigation Overlay Service Erweitertes Kalman-Filter (Extended Kalman filter) Galileo Frequenzen Fiber Optic Gyroscope Galileo Control Center Geometric Dilution of Precision Galileo Joint Undertaking Global Positioning System Galileo Sensor Station GPS Space Segment Simulator Horizontal Dilution of Precision Hardware-In-the-Loop Iterated Extended Kalman-Filter Interacting Multiple Model Inertial Measurement Unit Inertial Navigation System Integrated Optics Circuit GPS-Tr¨ agerfrequenzen Mini / Micro Aerial Vehicle Micro Electro-Mechanical System Multiple Model Adaptive Estimator Galileo Open Service Out-Of-Sequence Measurement Precise Code Position Dilution of Precision Phase Lock Loop

320 PRN PRS PSR RAIM RLG RMS SA SBAS SAASM SAR SDA SoL SPKF TCC TEC UAV ULS VDOP VSIMM VTOL WAAS WGN

Symbolverzeichnis Pseudorandom Noise Galileo Public Regulated Service Pseudorange Receiver Autonomous Integrity Monitoring Ring Laser Gyroscope Root-Mean-Square Selective Availability Satellite-based Augmentation System Selective Availability Anti-Spoofing Module Galileo Search- and Rescue Service, Synthetic Aperture Radar Strapdown-Algorithmus Galileo Savety-of-Life Service Sigma-Point-Kalman-Filter Galileo Telemetry, Tracking and Command Station Total Electron Count Unmanned Aerial Vehicle Galileo Uplink Station Vertical Dilution of Precision Variable Structure Interacting Multiple Model Vertical Take-off and Landing Wide Area Augmentation System White Gaussian Noise

Indizes A b e h i n S (...)k ( ...)n ( ...)neb (...) ˜ (...) ˆ (...) ¯ (...)n (...)e (...)d (...)x (...)y (...)z

GPS-Antenne K¨ orperfestes Koordinatensystem Erdfestes Koordinatensystem Horizontales Koordinatensystem Inertialkoordinatensystem Navigationskoordinatensystem Satellit Zeitpunkt tk Vektor in Koordinaten des n-KS. b-KS. bez¨ uglich e-KS., gegeben in Koordinaten des n-KS. Gemessene Gr¨ oße Gesch¨ atzte Gr¨ oße Gemittelte Gr¨ oße Komponente in Nordrichtung Komponente in Ostrichtung Komponente in Richtung der Vertikalen Komponente in Richtung der x-Achse Komponente in Richtung der y-Achse Komponente in Richtung der z-Achse

Symbolverzeichnis [(...)×]

kreuzproduktbildende Matrix von (...)

Lateinische Buchstaben a a A b b Bk B C C c cii cg cp d D e 0 E[...] f F F G(s) Ga Gk G g , gl h h H I I j J K K l L  L

Beschleunigung Quaternionenkomponente, große Halbachse des Erdellipsoids ¨ Gewichtungsmatrix oder Anderung einer DCM Bias Quaternionenkomponente, kleine Halbachse des Erdellipsoids Steuermatrix Steuermatrix, kontinuierlich Kapazit¨ at Richtungskosinusmatrix, Kovarianzmatrix oder Gewichtungsmatrix Lichtgeschwindigkeit oder Quaternionenkomponente Koeffizient der Richtungskosinusmatrix Gruppengeschwindigkeit Phasengeschwindigkeit Quaternionenkomponente Gewichtungsmatrix Exzentrizit¨ at des Erdellipsoids Dielektrizit¨ atskonstante Erwartungswert Frequenz, Abflachung des Erdellipsoids Kummulative Wahrscheinlichkeit oder Kraft Systemmatrix ¨ Ubertragungsfunktion im Laplace-Bereich Antennengewinn Einflussmatrix Einflussmatrix, kontinuierlich Schwerebeschleunigung H¨ohe u ¨ ber dem WGS-84-Erdellipsoid Erdmagnetfeld Messmatrix Einheitsmatrix Intensit¨ at, Strom oder Inphasen-Komponente Imagin¨ are Einheit Tr¨ agheitsmoment Kalman-Gain-Matrix Kostenfunktion Hebelarm zwischen GPS-Antenne und IMU L¨ ange des Lichtweges oder Anzahl der Komponenten eines erweiterten Zustandsvektors Drehimpuls

321

322 m  M M Mb n, n N p P P q ∆q Q Qk Q r rA rS rSA r Rn , Re R0 R R Rt s S t T, δT, δt u U (s) v w  x, y, z x, ∆x X(s) y  z

Symbolverzeichnis Koeffizient der Misalignment-Matrix, Mittelwert oder Masse Drehmoment Misalignment-Matrix Beobachtbarkeitsmatrix Stochastische St¨ orung Tr¨ agerphasenmehrdeutigkeit oder Rauschleistung Wahrscheinlichkeitsdichte Leistung oder Wahrscheinlichkeit Kovarianzmatrix der Zustandssch¨atzung Quaternion Ladungs¨ anderung Ladung oder Quadraturkomponente Kovarianzmatrix des Systemrauschens Spektrale Leistungsdichte des Systemrauschens Ortsvektor oder Quaternion Position der GPS-Antenne Satellitenposition Vektor von der GPS-Antenne zum Satellit Spektrale Leistungsdichte oder Korrelationsfunktion Kr¨ ummungsradien in Nord- und Ostrichtung Durchschnittlicher Kr¨ ummungsradius Widerstand Kovarianzmatrix des Messrauschens Spektrale Leistungsdichte des Messrauschens Komplexe Variable, Motorstellgr¨oße oder Skalenfaktor Spektrale Leistungsdichte Zeit Zeitliche Verschiebung oder Abtastzeit Eingangsgr¨ oßen oder Inertialsensordatendifferenzen Laplace-Transformierte von u(t) oder Spannung Geschwindigkeit oder Messrauschen Systemrauschen Koordinatenachsen oder Ausgangsgr¨oßen Zustandsvektor Laplace-Transformierte von x(t) Vektor der Messwerte Eigenvektor

Griechische Buchstaben α, β, γ χ  ∆, δ

Lagefehler Sigma-Punkt ¨ Fehler, Anderung, Intervall oder Fehlausrichtung

Symbolverzeichnis δt U η λ µ  ∇ ω  ω Ω Ω ψ  ψ Ψ ∆φ Φ Φ φ τ ϕ ρ σ ∆σ c σ θ ∆θ

Uhrenfehler des GPS-Empf¨ angers Weißes Rauschen Geographische L¨ ange, Wellenl¨ ange oder Eigenwert Weißes Rauschen Nabla-Operator Drehrate Kreisfrequenz Erddrehrate Kreuzproduktbildende Matrix eines Drehrate Yaw-Winkel Vektor der Lagefehler α, β, γ Kreuzproduktbildende Matrix der Lagefehler Phasenverschiebung Transitionsmatrix Tr¨agerphase Rollwinkel Zeitdifferenz, zeitliche Verschiebung Geographische Breite oder Phase Pseudorange Orientierungsvektor Coning-Term Standardabweichung oder Betrag des Orientierungsvektors Pitchwinkel Winkelinkrement

323

Literaturverzeichnis [1] Beidou (big dipper). http://www.globalsecurity.org/space/world/china/ beidou.htm. [2] Galileo, europ¨ aisches Satellitennavigationssystem. comm/dgs/energy_transport/galileo/.

http://europa.eu.int/

[3] Glonass interface control document. http://www.glonass-center.ru/. [4] Gps system description. ftp://tycho.usno.navy.mil/pub/gps/gpssy.txt. [5] Icd-gps-200, navstar gps interface control document. http://www.navcen.uscg. gov/pubs/gps/icd200/icd200cw1234.pdf. [6] International gps service. http://igscb.jpl.nasa.gov/index.html. [7] National geophysical data center. struts/calcPointIGRF.

www.ngdc.noaa.gov/seg/geomag/jsp/

[8] Novatel oem4 user’s guide, volume 1, 2001. [9] C. Acar. Robust Micromachined Vibratory Gyroscopes. PhD thesis, University of California, Irvine, 2004. [10] F. Aronowitz. Fundamentals of the ring laser gyro. RTO AGARDograph 339, Optical Gyros and their Application, pages 3–1 – 3–45, 1999. [11] M.S. Arulampalam, S. Maskell, N. Gordon, and T. Clapp. A tutorial on particle filters for online nonlinear/non-gaussian bayesian tracking. IEEE Transactions on Signal Processing, 50(2):174 – 188, 2002. [12] J. u.a. Beser. TrunavT M a low-cost guidance/navigation unit integrating a saasmbased gps and mems imu in a deeply coupled mechanization. Proceedings of the ION GPS 2002, pages 545 – 555, 24-27 September 2002. Portland, OR, USA. [13] J.W. Betz. The offset carrier modulation for gps modernization. Proceedings of the ION 1999 National Technical Meeting, 1999. [14] H.A. Blom and Y. Bar-Shalom. The interacting multiple model algorithm for systems with markovian switching coefficients. IEEE Transactions on Aerospace and Electronic Systems, 33(8):780 – 783, 1988. [15] J.E. Bortz. A new mathematical formulation for strapdown inertial navigation. IEEE Transactions on Aerospace and Electronic Systems, 7(1):61 – 66, 1971.

326

Literaturverzeichnis

[16] R.G. Brown. A baseline gps raim scheme and a note on the equivalence of three raim methods. Journal of the Institute of Navigation, 39(3):301 – 316, 1992. [17] R.G. Brown and P.Y.C. Hwang. Introduction to Random Signals and Applied Kalman Filtering. John Wiley & Sons, New York, 1997. [18] A.E. Bryson Jr. and L.J. Henrikson. Estimation using sampled data containing sequentially correlated noise. Journal of Spacecraft, 5(6):662–665, 1968. [19] N.A. Carlson. Federated filter for distributed navigation and tracking applications. Proceedings of the ION 58th Annual Meeting, pages 340–353, 2002. June 24 - 26, 2002, Albuquerque, NM, USA. [20] C.K. Chui and G. Chen. Kalman Filtering with Real-Time Applications. Springer Verlag, Berlin, 1999. [21] P.M. Djuric, J.H. Kotecha, J. Zhang, Y. Huang, T. Ghirmai, M.F. Bugallo, and J. Miguez. Particle filtering. IEEE Signal Processing Magazine, pages 19 – 38, 2003. [22] K. et. al. Dutton. The Art of Control Engineering. Pearson Education Limited, Harlow, 1997. [23] B. Eissfeller. Ein dynamisches Fehlermodell f¨ ur GPS Autokorrelationsempf¨ anger. PhD thesis, Universit¨ at der Bundeswehr M¨ unchen, 1997. [24] B. Eissfeller. Das europ¨ aische Satellitennavigationssystem Galileo. Fachtagung der ITG Satellitennavigationssysteme, Universit¨ at Bern, 2004. [25] B. Eissfeller. Galileo: Das europ¨ aische Satellitennavigationssystem. Bulletin, SEV ur Elektro-, Energie- und Informationstechnik, 2004. Verband f¨ [26] J. Farrell and M. Barth. The Global Positioning System & Inertial Navigation. McGraw-Hill, New York, 1999. [27] J.A. u.a. Farrell. Real-time differential carrier phase gps-aided ins. IEEE Transactions an Control Systems Technology, 8(4):709 – 720, July 2000. [28] J.L. Farrell. Carrier phase processing without integers. Proceedings of the Institute of Navigation 57th Annual Meeting 2001, pages 423 – 428, June 11-13 2001. Albuquerque, NM, USA. [29] N. Fliege. Systemtheorie. B.G. Teubner, Stuttgart, 1991. [30] H. Fruehauf and S. Callaghan. Saasm and direct p(y) signal acquisition. GPS World, Juli 2002. [31] O. F¨ ollinger. Laplace- und Fourier-Transformation. H¨ uthig Buch Verlag, Heidelberg, 1993. [32] O. F¨ ollinger. Regelungstechnik. H¨ uthig Buch Verlag, Heidelberg, 1994.

Literaturverzeichnis

327

[33] D. Gebre-Egziabher, G.H. Elkaim, J.D. Powell, and B.W. Parkinson. A gyrofree quaternion-based attitude determination system suitable for implementation using low cost sensors. IEEE Position Location and Navigation Symposium 2000, pages 185 – 192, 2000. 13-16 March , San Diego, CA, USA. [34] D. Gebre-Egziabher, G.H. Elkaim, J.D. Powell, and B.W. Parkinson. A non-linear two-step estimation algorithm for calibrating solid-state strapdown magnetometers. International Conference on Integrated Navigation Systems, 2001. May 2001, St. Petersburg, Russia. [35] Gelb, A. Applied Optimal Estimation, 14th printing. The M.I.T. Press, Cambridge, Massachusetts, and London, England, 1996. [36] W.R. Graham and K.J. Shortelle. Advanced transfer alignment for inertial navigators (a-train). Proceedings of the Institute of Navigation Technical Meeting, pages 113 – 124, 1995. [37] W.R. Graham, K.J. Shortelle, and C. Rabourn. Rapid alignment prototype (rap) flight test demonstration. Proceedings of the Institute of Navigation Technical Meeting, pages 557 – 568, 1998. [38] M.S. Grewal and A.P. Andrews. Kalman Filtering: Theory and Practice. Prentice Hall, New Jersey, 1993. [39] M.S. Grewal, L.R. Weill, and A.P. Andrews. Global Positioning Systems, Inertial Navigation and Integration. John Wiley & Sons, New York, 2001. [40] P.D. Groves. Transfer alignment using an integrated ins/gps as the reference. Institute of Navigation: Proceedings of the 55th Annual Meeting, pages 731 – 738, 1999. [41] P.D. Groves. Performance analysis and architectures for ins-aided gps tracking loops. Proceedings of the ION National Technical Meeting 2003, pages 611 – 622, 2003. [42] P.D. Groves and J.C. Haddock. An all-purpose rapid transfer alignment algorithm set. Proceedings of the ION National Technical Meeting 2001, pages 160 – 171, January 2001. Long Beach, California, USA. [43] P.D. Groves and D.C. Long. Adaptive tightly-coupled, a low cost alternative antijam ins/gps integration technique. Proceedings of the ION National Technical Meeting 2003, January 2003. Anaheim, California, USA. [44] P.D. u.a. Groves. Alignment and integration of weapon ins/gps navigation systems. NATO RTO, Emerging Military Capabilities Enabled by Advances in Navigation Sensors, October 2002. Istanbul. [45] X. Gu and A. Lipp. Dgps positioning usinge carrier phase for precision navigation. IEEE Position Location and Navigation Symposium, pages 410 – 417, 1994.

328

Literaturverzeichnis

[46] D. u.a. Gustafson. A high anti-jam gps-based navigator. Proceedings of the ION National Technical Meeting 2000, pages 495 – 503, 26-28 January 2000. Anaheim, CA, USA. [47] F. Gustafsson, F. Gunnarsson, N. Bergman, U. Forssell, J. Jansson, R. Karlsson, and P.-J. Nordlund. Particle filters for positioning, navigation, and tracking. IEEE Transactions on Signal Processing, 50(2):425 – 437, 2002. [48] U.D. Hanebeck and K. Briechle. New results for stochastic prediction and filtering with unknown correlations. Proceedings of the IEEE Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI’2001), pages 147–152, 2001. [49] U.D. Hanebeck and K. Briechle. A tight bound for the joint covariance of two random vectors with unknown but constrained cross-correlation. Proceedings of the IEEE Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI’2001), pages 85–90, 2001. [50] I. Hartmann. Lineare Systeme. Springer-Verlag, Berlin, 1976. [51] R. Hatch. The synergism of gps code and carrier measurements. Proceedings of the Third International Symposium on Satellite Doppler Positioning, pages 1213 – 1231, February 8-12 1982. Las Cruces, USA. [52] M. Heikkinen. Geschlossene Formeln zur Berechnung r¨aumlicher geod¨atischer Koordinaten aus rechtwinkligen Koordinaten. Zeitschrift f¨ ur Vermessungswesen, 5:207 – 211, 1982. [53] B. Hofmann-Wellenhof, H. Lichtenegger, and J. Collins. GPS Theory and Practice. Springer, Wien, 1997. [54] M.B. Ignagni. Efficient class of optimized coning compensation algorithms. Journal of Guidance, Control and Dynamics, 19(2):424 – 429, 1996. [55] M.B. Ignagni. Duality of optimal strapdown sculling and coning compensation algorithms. Navigation: Journal of the Institute of Navigation, 45(2):85 – 95, 1998. [56] Tsui J.B.-Y. Fundamentels of Global Positioning System Receivers. John Wiley & Sons, New York, 2000. [57] C. Jekeli. Inertial Navigation Systems with Geodetic Applications. Walter de Gruyter, Berlin, 2001. [58] P.J. Jonge and C.C.J.M. Tiberius. The LAMBDA method for integer ambiguity estimation: implementation aspects. LGR-Series Publications of the Delft Geodetic Computing Centre No. 12, TU Delft, 1996. [59] S.J. Julier and J.K. Uhlmann. A general method for approximating nonlinear transformations of probability distributions, 1996. [60] S.J. Julier and J.K. Uhlmann. A non-divergent estimation algorithm in the presence of unknown correlations. Proceedings of the 1997 American Control Conference, 1997.

Literaturverzeichnis

329

[61] S.J. Julier and J.K. Uhlmann. Unscented filtering and nonlinear estimation. Proceedings of the IEEE, 92(3):401–422, 2004. Providence, Rhode Island, USA. [62] J.E. Kain and J.R. Cloutier. Rapid transfer alignment for tactical weapon applications. Proceedings of the AIAA Guidance, Navigation and Control Conference, pages 1290 – 1300, 1989. [63] Rudolph Emil Kalman. A new approach to linear filtering and prediction problems. Transactions of the ASME–Journal of Basic Engineering, 82(Series D):35– 45, 1960. [64] K.D. Kammermeyer and K. Kroschel. Digitale Signalverarbeitung. B.G. Teubner, Stuttgart, 1998. [65] E.D. Kaplan. Understanding GPS: Principles and Applications. Artech House, Boston, 1996. [66] S.P. Karatsinides. Enhancing filter robustness in cascaded gps-ins integrations. IEEE Transactions on Aerospace and Electronic Systems, 30(4):1001 – 1008, October 1994. [67] J. Kim, S. Sukkarieh, and S. Wishart. Real-time navigation, guidance and control of a uav using low-cost sensors. International Conference of Field and Service Robotics (FSR’03), pages 95 – 100, 2003. Yamanashi, Japan. [68] J.-H. Kim and S. Sukkarieh. Flight test results of gps/ins navigation loop for an autonomous unmanned aerial vehicle (uav). ION GPS 2002, pages 510 – 517, 2002. 24 – 27 September, Portland, OR, USA. [69] D.B. Kingston and R.W. Beard. Real-time attitude and position estimation for small uavs using low-cost sensors. AIAA 3rd Unmanned Unlimited Systems Conference and Workshop, 2004. [70] H.A. Klotz and C.B. Derbak. Gps-aided navigation and unaided navigation on the joint direct attack munition. Position Location and Navigation Symposium, pages 412 – 419, 1998. [71] J. Kouba and P. Heroux. Gps precise point positioning using igs orbit products. http://www.geod.nrcan.gc.ca/publications/papers/abs3_e.php. [72] A. et.al Kourepenis. Performance of mems inertial sensors. IEEE Position Location and Navigation Symposium, pages 1 – 8, 1998. [73] V. Krebs. Nichtlineare Filterung. Oldenbourg, 1980. [74] C. Kreye and B. u.a. Eissfeller. Improvements of gnss receiver performance using deeply coupled ins measurements. Proceedings of the ION GPS 2000, pages 844 –854, 19-22 September 2000. Salt Lake City, UT, USA. [75] H. Kronm¨ uller. Digitale Signalverarbeitung. Springer-Verlag, Berlin, 1991. [76] K. Kroschel. Statistische Nachrichtentheorie. Springer-Verlag, 1996.

330

Literaturverzeichnis

[77] K. Krumvieda, P. Madhani, C. Cloman, E. Olson, J. Thomas, P. Axelrad, and W. Kober. A complete if software gps receiver: A tutorial about the details. ION GPS 2001, 11 – 14 September 2005. [78] T.D. u.a. Larsen. Incorporation of time delayed measurements in a discrete-time kalman filter. 37th Conference on Decision and Control, CDC’98, 1998. [79] S.-W. Lee and F. van Graas. A preliminary simulation result of integrating a leo satellite for carrier phase differential positioning systems. Proceedings of the Institute of Navigation National Technical Meeting 1998, pages 449 – 457, January 21-23 1998. Long Beach, CA, USA. [80] H. Lef`evre. The Fibre-Optic Gyroscope. Artech House, Inc., Boston, 1993. [81] R. Leonardson and S. Foote. Simma accelerometer for inertial guidance and navigation. IEEE Position Location and Navigation Symposium, 10(4):152 – 160, 20-23 April 1998. [82] Z.H. Lewantowicz. Architectures and gps/ins integration: Impact on mission accomplishment. IEEE Aerospace and Electronics Systems Magazine, 7:16 – 20, 1992. [83] Z.H. Lewantowicz and D.W. Keen. Graceful degradation of gps/ins performance with fewer than four satellites. Proceedings of the Institute of Navigation National Technical Meeting, pages 269 – 276, January 1991. [84] D. Li and J. Wang. Kalman filter design strategies for code tracking loop in ultratight gps/ins/pl integration. Proceedings of the ION National Technical Meeting 2006, 2006. [85] P.S. Maybeck. Stochastic Models, Estimation and Control, volume 1. Academic Press, New York, 1979. [86] P.S. Maybeck. Stochastic Models, Estimation and Control, volume 2. Academic Press, New York, 1982. [87] K.D. McDonald. Gps modernization. ION 61st Annual Meeting, pages 36 – 53, 27-29 June 2005. [88] R.K. Mehra. On the identification of variances and adaptive kalman filtering. IEEE Trans. Automat. Contr., AC-15:175 – 184, 1970. [89] J. Metzger. Optimierung des Akquisitions- und Tracking-Verhaltens zentraler und modularer Terrainnavigationssysteme. Dissertation, Universit¨at Karlsruhe, Karlsruhe, Deutschland, 2006. [90] G. Minkler and J. Minkler. Theory and Application of Kalman Filtering. Magellan Book Company, Palm Bay, Florida, 1993. [91] A. Mohamed and K.P. Schwarz. Adaptive kalman filtering for ins/gps. IAG Journal of Geodesy, 73(4):193 – 203, 1999.

Literaturverzeichnis

331

[92] M. Musial, C. Deeg, V. Remuß, and G. Hommel. Orientation sensing for helicopter uavs under strict resource constraints. First European Micro Air Vehicle Conference and Flight Competition EMAV 2004, 2004. 13 – 14 July, Braunschweig, Germany. [93] R. M¨ onikes, A. Teltschik, J. Wendel, and G.F. Trommer. Post-processing gnss/ins measurements using a tightly coupled fixed-interval smoother performing carrier phase ambiguity resolution. IEEE/ION PLANS 2006, 24.-27. April 2006. [94] N.S. Nise. Control Systems Engineering. John Wiley & Sons, Inc., New York, 2000. [95] S.H. u.a. Oh. Development of the agps/ins integration system using the triple difference technique. Proceedings of the ION GPS 2002, pages 518 – 526, September 24-27 2002. Portland, OR, USA. [96] Y. Oshman and M. Koifman. Robust, imm-based, tightly-coupled ins/gps in the presence of spoofing. Proceedings of the AIAA Guidance, Navigation and Control Conference, 16. -19. August 2004. [97] B.W. Parkinson and J.J. Jr. Spilker. Global Positioning System: Theory and Applications Vol. I. American Institute of Aeronautics and Astronautics, Inc., Washington, DC, 1996. [98] M.G. u.a. Petovello. Development and testing of a real-time gps/ins reference system for autonomous automobile navigation. Proceedings of the ION GPS 2001, pages 2634 – 2641, 11-14 September 2001. Salt Lake City, UT, USA. [99] R.E. Phillips and G.T. Schmidt. Gps/ins integration. NATO AGARD Lecture Series on System Implications and Innovative Applications of Satellite Navigation, LS-207, Paris, France, 1996. [100] W.H. et al. Press. Numerical Recipes in C. Cambridge University Press, Cambridge, 1992. [101] J. Rankin. An error model for sensor simulation gps and differential gps. IEEE Position Location and Navigation Symposium, pages 260–266, 1994. [102] H. Rauch, F. Tung, and C. Striebel. Maximum likelihood estimates of linear dynamic systems. AIAA Journal, 3(8):1445 – 1450, 1965. [103] L. et al. Ries. A software simulation tool for gnss2 boc signals analysis. ION GPS 2002, pages 2225 – 2239, 24 - 27 September 2002. [104] B. Ristic, S. Arulampalam, and N. Gordon. Beyond the Kalman Filter: Particle Filters for Tracking Applications. Artech House Publishers, 2004. [105] R.M. Rogers. Weapon imu transfer alignment using aircraft positions from actual flight tests. Position Location and Navigation Symposium, pages 328 – 335, 1996.

332

Literaturverzeichnis

[106] C.C. Ross and T.F. Elbert. A transfer alignment algorithm study based on actual flight test data from a tactical air to ground weapon launch. Position Location and Navigation Symposium, pages 431 – 438, 1994. [107] P.G. Savage. Strapdown inertial navigation integration algorithm design part 2: Velocity and position algorithms. Journal of Guidance, Control, and Dynamics, 21(2):208 – 221, 1998. [108] B.M. Scherzinger. Precise robust positioning with inertial/gps rtk. Proceedings of the ION GPS 2000, pages 155 – 162, September 19-22 2000. Salt Lake City, UT, USA. [109] C. Schlaile, J. Wendel, and G.F. Trommer. Stabilizing a four-rotor helicopter using computer vision. First European Micro Air Vehicle Conference and Flight Competition EMAV 2004, 2004. 13 – 14 July, Braunschweig, Germany. [110] C. Seidel. Optimierungsstrategien f¨ ur faseroptische Rotationssensoren: Einfluss der spektralen Eigenschaften der Lichtquelle. PhD thesis, Universit¨at Karlsruhe, 2004. [111] S.W. Shepperd. Quaternion from rotation matrix. AIAA Journal of Guidance and Control, 1(3):223 – 224, 1978. [112] K.J. Shortelle and W.R. Graham. Advanced alignment concepts for precisionguided weapons. Proceedings of the Institute of Navigation Technical Meeting, pages 131 – 142, 1995. [113] K.J. Shortelle, W.R. Graham, and C. Rabourn. F-16 flight tests of a rapid transfer alignment procedure. Position Location and Navigation Symposium, pages 379 – 386, 1998. [114] D. Simon. Optimal State Estimation. John Wiley & Sons, Inc., 2006. [115] B. Sinopoli, M. Micheli, G. Donato, and T.J. Koo. Vision based navigation for an unmanned aerial vehicle. IEEE International Conference on Robotics and Automation, 2:1757 – 1764, 2001. [116] D. Sobel. Longitude. Walker & Co, New York, 1996. [117] D.H. Titterton and J.L. Weston. Strapdown inertial navigation technology. Peter Peregrinus Ltd./IEE, London, 1997. [118] B. Todorovic, M. Stankovic, and C. Moraga. On-line learning in recurrent neural networks using nonlinear kalman filters. Proceedings of the 3rd IEEE International Symposium on Signal Processing and Information Technology, pages 802–805, 1417 December 2003. [119] G.F. Trommer. Felder und Wellen. Vorlesungsbegleitendes Manuskript. [120] R. Unbehauen. Systemtheorie. Oldenbourg Verlag, M¨ unchen, 1997.

Literaturverzeichnis

333

[121] F. van Graas and J.L. Farrell. Gps/ins – a very different way. Proceedings of the Institute of Navigation 57th Annual Meeting 2001, pages 715 – 721, June 11-13 2001. Albuquerque, NM, USA. [122] F. van Graas and S.-W. Lee. High-accuracy differential positioning for satellitebased systems without using code-phase measurements. Navigation: Journal of the Institute of Navigation, 42(4):605 – 618, 1995. [123] B. Vik. Nonlinear Design and Analysis of Integrated GPS and Inertial Navigation Systems. Dissertation, Norwegian University of Science and Technology, Trondheim, Norway, 2000. [124] J. Wendel and G.F. Trommer. Enhancement of a tightly coupled gps/ins systsem for high precision attitude determination of land vehicles. Proceedings of the Institute of Navigation 58th Annual Meeting, 2003. June 23-25, Albuquerque, New Mexico, USA. [125] J. Wendel and G.F. Trommer. Improved gps/ins integration in vibration environments. DGON Symposium Gyro Technology, pages 301 – 311, 2003. September 16-17, Stuttgart, Germany. [126] S. u.a. Winkler. Horizon aided low-cost gps/ins integration for autonomous micro air vehicle navigation. First European Micro Air Vehicle Conference and Flight Competition EMAV 2004, 2004. 13 – 14 July, Braunschweig, Germany. [127] S. u.a. Winkler. Improving low-cost gps/mems-based ins integration for autonomous mav navigation by visual aiding. ION GNSS 17th International Technical Meeting of the Satellite Division 2004, pages 1069 – 1075, 2004. 21 – 24 September, Long Beach, CA, USA. [128] C. Working. A culminating advance in the theory and practice of data fusion, 1997. [129] Y. Xu and L. Liping. Single observer bearings-only tracking with the unscented kalman filter. International Conference on Communications, Circuits and Systems, 2:901–905, 27-29 June 2004. [130] C. Yang. Joint acquisition of cm and cl codes for gps l2 civil (l2c) signals. ION 61st Annual Meeting, pages 553 – 562, 27 - 29 June 2005. [131] C.-S. Yoo and I.-K. Ahn. Low cost gps/ins sensor fusion system for uav navigation. The 22nd Digital Avionics Systems Conference 2003, 2, 2003. [132] Z. Zhao, X.R. Li, and V.P. Jilkov. Best linear unbiased filtering with nonlinear measurements for target tracking. IEEE Transactions on Aerospace and Electronic Systems, 40(4):1324 – 1336, October 2001.

Index ¨ Ubertragungsfunktion, 5 AAIM, 239 Autokorrelationsfunktion, 123 Bayes’sche Regel, 119, 171, 174, 180, 181 Beobachtbarkeit, 140, 283 Carrier Aided Smoothing, 220 Chapman-Kolmogorov-Gleichung, 179 Coning, 47, 49 Covariance Intersection, 163

Ephemeriden, 92 I’s & Q’s, 95 PRN-Codes, 86 Pseudorange, 100, 212 Tr¨agerphase, 104 Uhrenfehler, 203 Impulsantwort, 6 Inertialsensoren, 61 Integrity Monitoring, 238 Interacting Multiple Model Filtergleichungen, 176 Herleitung, 169

Fehlercharakteristik eines INS, 73, 75

Kalibration, 192, 304 Kalman-Filter, 129 2. Ordnung, 156 adaptives, 167, 274 diskret, 141 Eigenschaften, 139 erweitertes (EKF), 146 Herleitung, 130, 134 Joseph’s Form, 132 Kreuzkorrelationen, 161 linearisiertes, 144 Messmodell, 129 Systemmodell, 129 unbekannte Kreuzkorrelationen, siehe Covariance Intersection Zeitkorrelationen, 158, 263 Koordinatensysteme, 28 Transformationen, 31 Kreuzkorrelationsfunktion, 123

Galileo, 111 Gauß-Markov-Prozess, 125, 126 Gaußverteilung, 120 GPS Almanach, 92 Deltarange, 106, 213

Lage Eulerwinkel, 37 Fehlerdifferentialgleichung, 199 Quaternion, 38 Richtungskosinusmatrix, 37, 40, 43 Likelihood-Funktion, 174

Deep Integration, 193 Delay Lock Loop, 98 Delayed Measurement Problem, 231 Dirac-Impuls, 124, 182 Doppler-Effekt, 88 Drehimpuls, 292 Drehmoment, 292 Eigenmode, 11 Eigenvektor, 10 Eigenwert, 10 Erddrehrate, 47 ergodisch, 123 Erwartungswert, 119 bedingter, 120, 169 Euler’sche Kreiselgleichungen, 293

336 Lineare Systeme zeitdiskret, 16 Zustandsdifferentialgleichung, 7 Loosely Coupled System, 192 Magnetometer, 285 Matrix Determinante, 10 Differentiation der Spur, 135 Exponentialfunktion, 8 Jacobi, 24 Potenz, 13 Transitions-, 9, 126 Momente einer ZV, 119 Multiple Model Adaptive Estimator, 175 Nichtlinearit¨ at eines Sch¨ atzproblems, 240 Nordsuche, 79 Normalverteilung, siehe Gaußverteilung Numerische Integration, 19 Partikelfilter, 182 Phase Lock Loop, 98 RAIM, 239 Random Walk, 71, 72 Rauschen Erzeugung, 122 weiß, 124 zeitkorreliert, 125 Resampling, 184

Index Schuler-Oszillationen, 75 Schwerebeschleunigung, 30 Sculling, 57 Sigma-Point-Kalman-Filter, 147, 239 Smoother, 254 spektrale Leistungsdichte, 124 station¨ar, 123 Stichprobenformel, 126 stochastischer Prozess, 123 Strapdown-Algorithmus, 45 Taylor-Reihe, 24, 100, 144, 151, 156 Tightly Coupled System, 193 Tr¨agheitsmoment, 292 Transfer Alignment konventionell, 259 rapid, 260 Transportrate, 47 Uhrenfehler, 100 Ultra-Tight Integration, 193 Unmanned aerial vehicle, 281 Unscented Filter, siehe Sigma-PointKalman-Filter Varianz, 119 Verbundwahrscheinlichkeitsdichte, 119 Wahrscheinlichkeitsdichte, 117 WGS-84 Erdmodell, 30 Wiener-Khintchine Relation, 124 z-Transformation, 17 Zufallsvariable, 117