AT523734A4 - Verfahren und System zur Schätzung von Zustandsgrößen eines beweglichen Objekts mit modularer Sensorfusion - Google Patents

Abstract

Die vorliegende Erfindung betrifft ein computer-implementiertes Verfahren zur Schätzung von Zustandsgrößen eines beweglichen Objekts, bei welchem Kernzustandsgrößen des beweglichen Objekts mittels eines rekursiven Bayesschen Filters unter Verwendung von Beobachtungswerten von Sensoren, die seit Aufstarten des beweglichen Objekts eingesetzt werden, propagiert und im Fall von Beobachtungswerten von während der Laufzeit hinzugefügten zusätzlichen Sensoren eine Kovarianzmatrix des rekursiven Bayesschen Filters aus einer auf einen einen Zeitschritt vor dem zweiten Zeitpunkt liegenden Zeitpunkt propagierten Kovarianz der Kernzustandsgrößen des beweglichen Objekts, der neuesten Kovarianz der Kalibrierungszustandsgrößen des zusätzlichen Sensors und den neuesten Kreuzkovarianzen der Kernzustandsgrößen des beweglichen Objekts und der Kalibrierungszustandsgrößen des zusätzlichen Sensors geformt wird, die Kovarianzmatrix mit Hilfe der zu dem zweiten Zeitpunkt von dem zusätzlichen Sensor gebildeten Beobachtungswerte aktualisiert wird und nach Ermitteln der Kernzustandsgrößen die Kovarianz der Kalibrierungszustandsgrößen des zusätzlichen Sensors und die Kreuzkovarianzen der Kernzustandsgrößen des beweglichen Objekts und die Kalibrierungszustandsgrößen des zusätzlichen Sensors wieder von der Kovarianz der Kernzustandsgrößen des zusätzlichen Sensors getrennt werden.

Description

GEBIET DER TECHNIK

Die vorliegende Erfindung betrifft ein computer-implementiertes Verfahren, ein System und ein computerlesbares Speichermedium zur Schätzung von Zustandsgrößen eines beweglichen Objekts, bei welchen ein modularer Sensorfusionansatz verfolgt wird. Sensorfusion wird auch als Sensordatenfusion

bzw. als Multi-Sensor-Datenfusion bezeichnet.

STAND DER TECHNIK

Das Schätzen von Zustandsgrößen bildet einen wesentlichen Teil sowohl der Robotik als auch von anderen ingenieurwissenschaftlichen Disziplinen wie beispielsweise der Regelungstechnik. So ist der genaue Standort einer

Roboterplattform beispielsweise wesentlich für deren Regelung und Navigation.

Bei den meisten Zustandsschätzern handelt es sich um dedizierte Zustandsschätzer, die für eine bestimmte Aufgabe an einer bestimmten Roboterplattform unter bestimmten Bedingungen entwickelt worden sind und somit eine geringe Wiederverwendbarkeit für den Fall haben, dass sich das Szenario, insbesondere die eingesetzten Sensoren oder die eingesetzte Roboterplattform

ändern.

Zwar wird diese Problematik von bekannten sogenannten Extended-Kalman-FilterSystemen adressiert. Bei Extended-Kalman-Filtern handelt es sich um rekursive Filter. Allerdings gehen auch die bekannten Extended-Kalman-Filter-Systeme von

einer Sensorkonfiguration aus, die während der Kompilierungsphase bzw.

Sensordefinition dem System nicht von vornherein bekannt ist.

Dies begrenzt die Anwendbarkeit derartiger Extended-Kalman-Filter-Systeme auf statische Hardwarekonfigurationen, während eine Eignung für modulare Hardwareplattformen, welche während der Laufzeit durch weitere, vorab nicht bekannte Module erweitert werden, eher gering ist. Untereinander verbindbare Schlangenroboter (snake robots) und humanoide Roboter mit austauschbaren Greiforganen stellen Beispiele derartiger modularer Hardwareplattformen dar, deren

Konfiguration/Aufbau sich nach deren Aufstarten ändern kann.

Für die Integration zusätzlicher, während der Laufzeit hinzugefügter Sensoren müssen zusätzlich Kalibrierungszustandsgrößen berücksichtigt werden, da die zusätzlichen Sensoren in der Regel weder auf den Körperrahmen (body frame), d.h. das objektbezogene Koordinatensystem, der Hardware- bzw. Roboterplattform ausgerichtet sind, noch intrinsisch kalibriert sind. Die Anzahl der Kalibrierungszustandsgrößen nimmt mit der Anzahl der zusätzlichen Sensoren zu. Alternativ müssen Annahmen bezüglich der Unveränderbarkeit der anfänglichen Kalibrierung der zusätzlichen Sensoren gemacht werden. Beides macht den regulären, nicht-modularen Extended-Kalman-Filter-Ansatz komplex und unflexibel. Eine zunehmende Anzahl an Zustandsgrößen führt dazu, dass mehr Operationen bzw. Berechnungsschritte für die Zustandsschätzung durchgeführt werden müssen, z.B. bei der Propagierung und Korrektur/Aktualisierung der Systemzustandsgrößen. Bei sogenannten naiven Zustandsschätzern, d.h. Zustandsschätzern, welche weder die Modularität der Sensoren noch vereinfachende Annahmen in Betracht ziehen; nimmt die Bearbeitungszeit aufgrund der erforderlichen Matrizenmultiplikationen kubisch, also mit der Ordnung Ofn®), mit der Anzahl der Sensoren n zu. Für

verzögerte oder außerhalb der Reihe liegende Messsignale erhöht sich dieser Effekt

Neuberechnungsschritte auslösen.

Nicht-rekursive Filtersysteme, die beispielsweise auf Graphenoptimierung (graph optimization) basieren, können zwar vorab unbekannte Sensoren während der Laufzeit des Systems initialisieren. Allerdings ist ihr Bedarf an Rechenleistung in der Regel derart hoch, dass sie sich nicht für den Einsatz auf ressourcenbegrenzten Plattformen wie beispielsweise Drohnen oder generell kleine, leichte Roboter

eignen.

Die Zustandsschätzung mit vordefinierter Konfiguration mehrerer Sensoren und Berücksichtigung ergänzender Kalibrierungszustandsgrößen inklusive deren Selbstkalibrierung und Verzögerungskompensation ist in der einschlägigen Fachliteratur behandelt worden. So behandelt das Single Sensor Fusion Framework (SSF), welches in “Real-time metric state estimation for modular vision-inertial systems,” von S. Weiss und R. Y. Siegwart, Proceedings - IlEEE International Conference on Robotics and Automation, Vol. 231855, S. 4531-4537, 2011, vorgeschlagen wird, sowohl Online-Selbstkalibrierung als auch die genaue Handhabung von Sensorverzögerungen. Eine erweiterte Fassung von SSF wurde in “Long-duration autonomy for small rotorcraft UAS including recharging,” von C. Brommer, D. Malyuta, D. Hentzen, und R. Brockers in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Okt. 2018, S. 7252-7268, in einer Mehrsensorenkonfiguration für lange währende Autonomie eingesetzt. S. Lynen, M. W. Achtelik, S. Weiss, M. Chli, und R. Siegwart, “A robust and modular multi-sensor fusion approach applied to MAV navigation,” in 2013 IEEE/RSJ

basierend auf Sensorverfügbarkeit und Gesundheitsmetriken (health metrics).

C. Tessier, C. Cariou, C. Debain, F. Chausse, R. Chapuis, und C. Rousset, “A realtime, multi-sensor architecture for fusion of delayed observations: application to vehicle localization,” in 2006 IEEE Intelligent Transportation Systems Conference, https /doLora/ 10. 1109tsc 2006. 1707405 schlagen ein Verfahren zur Handhabung von verzögerten Messsignalen vor, welches für rechenleistungsmäßig beschränkte eingebettete Systeme entwickelt worden ist. T. Moore und D. Stouch, “A generalized extended kalman filter implementation for the robot operating system,”

in Proceedings of the 13th

International Conference on Intelligent Autonomous Systems (1AS-13), Springer, Jul. 2014, beschreiben eine generalisierte Extended-Kalman-FilterImplementierung, welche auf dem sogenannten Robot Operating System (ROS) basiert. Die Sensorstruktur wird beim Aufstarten definiert. Modifikationen sind während der Laufzeit nicht möglich. Es wird angenommen, dass sich Sensormesswerte auf den Ursprung des Roboterkoordinatensystems beziehen.

Weder Sensorkalibrierung noch Online-Selbstkalibrierung sind beschrieben. Bei der

von gyroskopischem Bias. Prozessrauschen wird von Hand kompensiert.

M. Darms und H. Winner, “A modular system architecture for sensor data processing of ADAS applications,” IEEE Intelligent Vehicles Symposium, Proceedings, Vol. 2005, S. 729-734, 2005, beschreibt den aktuellen Stand von Wissenschaft und Technik in Bezug auf zentralisierte und dezentralisierte Systeme, welche Sensorfusion zur Fahrunterstützung bei in Fahrzeugen eingesetzten Fahrerassistenzsystemen (advanced driver-assistance systems (ADAS) einsetzen. Zentralisierte Ansätze ermöglichen eine eng gekoppelte Schätzung, aber benötigen eine hohe Kommunikationsbandbreite und sind schwer auf andere Anwendungen auszudehnen. Die Arbeitslast bei der Einbindung neuer Sensoren ist hoch. Dezentralisierte Systeme verwenden lose gekoppelte Sensoren, was aufgrund von unzulänglicher Handhabung der Kreuzkovarianzen von Sensor- und

Kernzustandsgrößen den Nachteil von Inkonsistenzen in sich birgt.

D. A. Cucci and M. Matteucei, “On the Development of a Generic Multi-Sensor Fusion Framework for Robust Odometry Estimation,” Journal of Software Engineering for Robotics, Vol. 5, Maiausgabe, S. 48-2, 2014, und H.-P. Chiu, X. S. Zhou, L. Carlone, F. Dellaert, S. Samarasekera, und R. Kumar, “Constrained optimal selection for multi-sensor robot navigation using plug-and-play factor

graphs,” in 2014 IEEE International

Conference on Robotics and Automation (ICRA). IEEE, Mai 2014, beschäftigen sich mit der Modularisierung von Mehrfachsensorfusion und schlagen ein auf Vektorgraphen basiertes Verfahren vor, welches einen Echtzeit-BatchOptimierungsprozess einsetzt. Der Fokus liegt dabei auf der optimalen Auswahl einer minimalen Untermenge aus der gegebenen Sensorkonfiguration und der Beobachtbarkeit für die Sensorauswahl. Der Einsatz von auf Vektorgraphen basierten Verfahren ist jedoch in Hinblick auf die Skalierbarkeit, insbesondere in

Kombination mit rechenleistungsmäßig beschränkten Ressourcen. von Nachteil

ZUSAMMENFASSUNG DER ERFINDUNG

Ausgehend vom oben genannten Stand der Technik stellt sich die vorliegende Erfindung die Aufgabe, ein Verfahren, ein System und ein computerlesbares Speichermedium zur Schätzung von Zustandsgrößen eines beweglichen Objekts bereitzustellen, welche eine dynamische und effiziente Einbindung während der Laufzeit des beweglichen Objekts hinzugefügter zusätzlicher Sensoren ermöglichen. Es ist ferner Aufgabe der vorliegenden Erfindung, ein Verfahren, ein System und ein computerlesbares Speichermedium zur Schätzung von

Zustandsgrößen eines beweglichen Objekts bereitzustellen, welche eine

beweglichen Objekts zu erlauben.

Die oben genannten Aufgaben werden durch ein computer-implementiertes Verfahren, ein computerlesbares Speichermedium und ein

Datenverarbeitungssystem mit den Merkmalen der unabhängigen Ansprüche gelöst.

Unter der Laufzeit wird die Zeit nach dem Aufstarten des beweglichen Objekts verstanden, wobei beispielsweise ein Anhalten (z.B. zum Auftanken bzw. Aufladen eines als Fahrzeug ausgestalteten beweglichen Objekts) als während der Laufzeit

stattfindend anzusehen ist.

Das Hinzufügen eines zusätzlichen Sensors umfasst neben einem nach dem Aufstarten des beweglichen Objekts erfolgenden physischen Hinzufügens (z.B. ein Montieren) unter anderem auch, dass der zusätzliche Sensor bereits beim Aufstarten des beweglichen Objekts vorhanden ist, aber erst nach dessen Aufstarten zugeschaltet bzw. eingeschaltet wird und/oder erst nach dem Aufstarten Beobachtungswerte liefert. Entsprechend umfasst ein Entfernen eines Sensors ein Ausschalten und/oder ein Nicht-mehr-Liefern von Beobachtungswerten neben

einem physischen Entfernen (beispielsweise einem Abmontieren).

Das erfindungsgemäße computer-implementierte Verfahren zur Schätzung von Zustandsgrößen eines beweglichen Objekts weist die folgenden Schritte auf: In einem ersten Schritt a) erfolgt eine Initialisierung eines zum Schätzen von vorab definierten Kernzustandsgrößen eines beweglichen Objekts eingesetzten rekursiven Bayesschen Filters. Bei dem rekursiven Bayesschen Filter handelt es sich vorzugsweise um einen rekursiven Kalman-Filter, insbesondere um einen sogenannten Extended-Kalman-Filter (EKF; auch erweiterter Kalman-Filter genannt). Bei den Kernzustandsgrößen des beweglichen Objekts handelt es sich bevorzugt um Navigationszustandsgrößen, d.h. für die Navigation relevante

Zustandsgrößen wie beispielsweise Position, Geschwindigkeit und Orientierung.

Eigenschaften und/oder chemische Eigenschaften verstanden.

In Schritt c) werden die Kernzustandsgrößen des beweglichen Objekts und eine Kovarianz der Kernzustandsgrößen mittels eines Zustandsgrößenmodells des rekursiven Bayesschen Filters unter Verwendung derjenigen Beobachtungswerte zeitlich propagiert, welche mit Hilfe von einem oder mehreren Sensoren gebildet worden sind, die seit Aufstarten des beweglichen Objekts eingesetzt werden. Dabei kann es sich beispielweise um einen oder mehrere Propagierungssensoren handeln, d.h. um Sensoren, die für als Navigationszustandsgrößen ausgebildete Kernzustandsgrößen relevante Beobachtungen liefern. Insbesondere kann es sich

um eine inertiale Messeinheit (IMU) handeln.

Wird in einem Schritt d) festgestellt, dass Beobachtungswerte mit Hilfe eines nach dem Aufstarten des beweglichen Objekts hinzugefügten zusätzlichen Sensors gebildet werden, so erfolgt in einem Schritt e1) ein Initialisieren einer Kovarianz von Kalibrierungszustandsgrößen des zusätzlichen Sensors und von Kreuzkovarianzen der Kernzustandsgrößen des beweglichen Objekts und der Kalibrierungszustandsgrößen des zusätzlichen Sensors mit Hilfe der zu einem ersten Zeitpunkt von dem zusätzlichen Sensor gebildeten Beobachtungswerte. Bei den zu dem ersten Zeitpunkt von dem zusätzlichen Sensor gebildeten Beobachtungswerten handelt es sich um die ersten Beobachtungswerte bzw. die

ersten Messwerte, welche der zusätzliche Sensor ausgibt.

Werden von dem zusätzlichen Sensor zu einem zweiten Zeitpunkt, welcher nach dem ersten Zeitpunkt liegt, weitere Beobachtungswerte gebildet bzw. Messwerte ausgegeben, so wird in einem darauffolgenden Schritt e2) eine Kovarianzmatrix des

rekursiven Bayesschen Filters aus (i) der Kovarianz der Kernzustandsgrößen des

Kernzustandsgrößen.

Bei der ersten Durchführung von Schritt e2) handelt es sich bei der neuesten Kovarianz der Kalibrierungszustandsgrößen des zusätzlichen Sensors und den neuesten Kreuzkovarianzen der Kernzustandsgrößen des beweglichen Objekts und der Kalibrierungszustandsgrößen des zusätzlichen Sensors um die im Sensorinitialisierungsschritt e1) gebildete Kovarianz bzw. gebildeten

Kreuzkovarianzen.

Nach dem Formen/Bilden der Kovarianzmatrix des rekursiven Bayesschen Filters in Schritt e2) wird die Kovarianzmatrix in Schritt e3) mit Hilfe der zu dem zweiten Zeitpunkt von dem zusätzlichen Sensor gebildeten Beobachtungswerte aktualisiert bzw. korrigiert. Im anschließenden Schritt e4) werden dann die Kernzustandsgrößen des beweglichen Objekts mit Hilfe der aktualisierten Kovarianzmatrix durch den

rekursiven Bayesschen Filter berechnet bzw. geschätzt.

Danach werden in Schritt e5) die Kovarianz der Kalibrierungszustandsgrößen des zusätzlichen Sensors und die Kreuzkovarianzen der Kernzustandsgrößen des beweglichen Objekts und der Kalibrierungszustandsgrößen des zusätzlichen Sensors von der Kovarianz der Kernzustandsgrößen des zusätzlichen Sensors separiert, sodass eine getrennte Verarbeitung/ein getrenntes Propagieren erfolgen

kann.

Werden nach dem zweiten Zeitpunkt von dem zusätzlichen Sensor bei späteren Beobachtungen weitere Beobachtungswerte gebildet, so werden die obigen Schritte

e2) bis e5) für diese Beobachtungswerte an ihren jeweiligen späteren

Beobachtungszeitpunkten wiederholt. Dabei wird in Schritt e2), in welchem die Kovarianzmatrix gebildet wird, die auf einen einen Zeitschritt vor dem jeweiligen späteren Zeitpunkt liegenden Zeitpunkt propagierte Kovarianz der Kernzustandsgrößen des beweglichen Objekts verwendet. Bei der neuesten Kovarianz der Kalibrierungszustandsgrößen des zusätzlichen Sensors und den neuesten Kreuzkovarianzen der Kernzustandsgrößen des beweglichen Objekts und der Kalibrierungszustandsgrößen des zusätzlichen Sensors handelt es sich dann um die bei der letzten Aktualisierung der Kovarianzmatrix im vorhergehenden Schritt e3) ebenfalls aktualisierte Kovarianz der Kalibrierungszustandsgrößen des zusätzlichen Sensors und die aktualisierten Kreuzkovarianzen der Kernzustandsgrößen des beweglichen Objekts und der

Kalibrierungszustandsgrößen des zusätzlichen Sensors.

Gemäß bevorzugter Ausgestaltung werden für den Fall, dass nach dem Aufstarten des beweglichen Objekts mehrere zusätzlichen Sensoren hinzugefügt werden, die Schritte e1) bis e6) des erfindungsgemäßen Verfahrens für jeden einzelnen der hinzugefügten Sensoren durchgeführt. Dabei werden die mit den jeweiligen zusätzlichen Sensoren assozlierten Kovarianzen und Kreuzkovarianzen vorzugsweise nicht zum gleichen Zeitpunkt aktualisiert. D.h. es werden keine Kreuzkovarianzen zwischen unterschiedlichen, nach dem Aufstarten hinzugefügten

zusätzlichen Sensoren gebildet.

Werden bei der in Schritt e2) des erfindungsgemäßen Verfahrens gebildeten Kovarianzmatrix die Kovarianz der Kernzustandsgrößen des beweglichen Objekts und die mit den Kalibrierungszuständen des zusätzlichen Sensors assoziierte Kovarianz und assoziierten Kreuzkovarianzen zu unterschiedlichen Zeitpunkten gebildet, so beinhalten die Kovarianz der Kernzustandsgrößen und die mit dem zusätzlichen Sensor assoziierte Kovarianz und assoziierten Kreuzkovarianzen nicht die gleiche Menge an Daten, was zu einer nicht-positiven semidefiniten und somit ungültigen Kovarianzmatrix führen kann. Dies ist beispielsweise dann der Fall, wenn sich die mit den Kalibrierungszuständen des zusätzlichen Sensors assoziierte

Kovarianz und assoziierten Kreuzkovarianzen auf den ersten (Beobachtungs-

)Zeitpunkt beziehen, während sich die Kovarianz der Kernzustandsgrößen des beweglichen Objekts auf einen Zeitpunkt bezieht, der einen Zeitschritt vor dem zweiten (Beobachtungs-)Zeitpunkt des zusätzlichen Sensors liegt. Eine nicht-positiv semidefinite und somit ungültige Kovarianzmatrix würde jedoch längerfristig zur Divergenz und zu Fehlverhalten des rekursiven Bayesschen Filters führen, so dass bevorzugterweise wie folgt vorgegangen wird, um eine mit einer nicht-positiv semidefiniten Kovarianzmatrix einhergehende Inkonsistenz und eine sich aus einer solchen Inkonsistenz ergebende Diverganz des rekursiven Bayesschen Filters zu

vermeiden.

Um die im vorhergehenden Absatz genannte Problematik zu adressieren und zu lösen, werden gemäß weiterer bevorzugter Ausgestaltung in Schritt e2) des erfindungsgemäßen Verfahrens die neueste Kovarianz der Kalibrierungszustandsgrößen des zusätzlichen Sensors und die neuesten Kreuzkovarianzen der Kernzustandsgrößen des beweglichen Objekts und der Kalibrierungszustandsgrößen des zusätzlichen Sensors mit Hilfe einer Reihe von einer oder mehreren Zustandsübergangsmatrizen auf denselben Zeitpunkt wie die Kovarianz der Kernzustandsgrößen des beweglichen Objekts propagiert. Auf diese Weise beziehen sich sämtliche der Kovarianzen und Kreuzkovarianzen der

Kovarianzmatrix auf denselben Zeitpunkt.

Dabei ist die Reihe der Zustandsübergangsmatrizen ®(m,n) zwischen zwei

Zeitpunkten {(m) und f(n) wie folgt definiert:

{ us n) A PD, CE H—1 ws Don mit (m) < f(n) und ®x als diskreter Zustandsübergangsmatrix (state-transition matrix) ®Oxik-1, welche die Zustandsdynamik abbildet und basierend auf

Eingangswerten des beweglichen Objekts evaluiert und für den Propagierschritt öf = t(k) — t(k-1) integriert wird (vergleiche S. I. Roumeliotis und G. A. Bekey, “Distributed

multirobot localization,” IEEE Transactions on Robotics and Automation, Vol. 18, Nr. 5, S. 781—795, 2002).

Entsprechend kann die Kreuzkovarianz Pcs zwischen den Kernzustandsgrößen Xc des beweglichen Objekts und den Kalibrierungszustandsgrößen Xs des zusätzlichen Sensors wie folgt von einem Zeitpunkt {(m) auf einen Zeitpunkt £(n)

propagiert werden:

4 g \ 1 DO K T Prsn = Dim, n)Pesm im, n)

z Ks 7

Entsprechendes gilt für die Kovarianz der Kalibrierungszustandsgrößen des zusätzlichen Sensors. Die derart berechnete und beispielsweise in einem Zwischenspeicher hinterlegte Reihe von (sich zeitlich entwickelnden) Zustandsübergangsmatrizen kann vorteilhafterweise verwendet werden, um die mit dem zusätzlichen Sensor assoziierte Kovarianz und assoziierten Kreuzkovarianzen auf denselben Zeitpunkt zu propagieren wie die propagierte Kovarianz der Kernzustandsgrößen des beweglichen Objekts. Die mit dem zusätzlichen Sensor assozlierte Kovarianzen und assoziierten Kreuzkovarianzen „erben“ während des Propagierens sozusagen diejenigen Informationen, die ihnen für den Zeitraum fehlten, während dem allein die Kovarianz der Kernzustandsgrößen des beweglichen Objekts propagiert worden war. Durch das vorerst alleinige Propagieren der Kovarianz der Kernzustandsgrößen wird deren Komplexität vorteilhafterweise unabhängig von der Anzahl zusätzlicher, nach dem Aufstarten hinzugefügter Sensoren und somit konstant gehalten. Die mit den zusätzlichen Sensoren assoziierten Kovarianzen und Kreuzkovarianzen werden vorzugsweise

erst beim Bilden der Kovarianzmatrix propagiert.

Werden mehrere zusätzliche Sensoren nach dem Aufstarten des beweglichen Objekts hinzugefügt, so kann es dazu kommen, dass Beobachtungen eines ersten zusätzlichen Sensors die Kernzustandsgrößen des beweglichen Objekts und die

Kalibrierungszustandsgrößen eines zweiten zusätzlichen Sensors indirekt aufgrund

von Kreuzkorrelationen beeinflussen, obgleich der erste zusätzliche Sensor vor dem Hinzufügen des zweiten zusätzlichen Sensors bereits entfernt und erst nach Entfernen des zweiten zusätzlichen Sensors wieder hinzugefügt worden ist. Diese unerwünschte Beeinflussung kann zu einer nicht-positiv semidefiniten und damit ungültigen Kovarianzmatrix (einer sogenannten Pseudokovarianzmatrix) führen. Kovarianzmatrizen sollen definitionsgemäß symmetrisch und positiv semidefinit

sein. Dies garantiert, dass ihre Korrelationen bzw. Kovarianzen kohärent sind.

Um sicherzustellen, dass die bei dem erfindungsgemäßen Verfahren gebildete Kovarianzmatrix positiv semidefinit ist, wird gemäß weiterer bevorzugter Ausgestaltung die Kovarianzmatrix vor der Schätzung der Kernzustandsgrößen des beweglichen Objekts in Schritt e3) des erfindungsgemäßen Verfahrens in eine

positiv semidefinite Kovarianzmatrix korrigiert.

P. J. Rousseeuw und G. Molenberghs, “Transformation of non positive semidefinite correlation matrices,” Communications in Statistics - Theory and Methods, Vol. 22, Nr. 4, S. 965-984, Jan. 1993, verfügbar unter:

https /doiora/ 10. 1080/036109285308331068, und R. Rebonato und P. Jaeckel, “The

most general methodology to create a valid correlation matrix for risk management

and option pricing purposes,” SSRN Electronic Journal, 2011, verfügbar unter:

hites:i/dolLara/10.2139/ sem. 1969689, beschreiben verschiedene Verfahren, um die

naheste positiv semidefinite Kovarianzmatrix einer gegebenen Pseudokovarianzmatrix zu schätzen. Beschrieben sind unter anderem das Eigenwert-Verfahren (Eigenvalue method) und das Skalier-/HypersphäreDekompositionsverfahren (scaling/hypersphere decomposition) mit Winkelparametrisierung (angular parametrization). Das Skalier-/HypersphäreDekompositionsverfahren verwendet ein Optimierungsverfahren zur Minimierung

der sogenannten Frobeniusnorm A:

. , TE Er a x 3 A= X 5 — Dig)“

Zi Zug E43

in Bezug auf eine gegebene Matrix, wobei op Elemente der tatsächlichen,

gegebenen Matrix und 7 Elemente ihrer nahesten Approximation sind.

Das Eigenwert-Verfahren approximiert eine positiv semidefinite Matrix mittels Korrektur der Eigenwerte einer gegebenen Matrix. Gemäß N. J. Higham, “Computing a nearest symmetric positive semidefinite matrix,” Linear Algebra and its Applications, vol. 103, pp. 103-118, Mai 1988, verfügbar unter: hitps/doLorg/10.1016/0024-37095(82)90223-6, minimiert das Eigenwert-Verfahren

ebenfalls die Frobeniusnorm.

Aufgrund seiner deterministischen Eigenschaften und geringeren Komplexität wird gemäß bevorzugter Ausgestaltung des erfindungsgemäßen Verfahrens die gebildete Kovarianzmatrix mit Hilfe eines Eigenwert-Verfahrens in eine positiv semidefinite Kovarianzmatrix korrigiert. Bei dem Eigenwert-Verfahren wird die Kovarianzmatrix in einem ersten Schritt in ihre Eigenwerte und Eigenvektoren zerlegt. In einem zweiten Schritt werden gegebenenfalls negative Eigenwerte korrigiert, und in einem dritten Schritt wird die Kovarianzmatrix mit den korrigierten Eigenwerten und den Eigenvektoren rekonstruiert. D.h. im ersten Schritt wird eine

Kovarianzmatrix P wie folgt zerlegt: P = DRDY,

wobei R eine diagonale Matrix mit den Eigenwerten ist und D die Eigenvektoren sind. Wenn die Kovarianzmatrix nicht-positiv semidefinit ist, dann sind die Eigenwerte R(<0) negativ und werden bevorzugterweise korrigiert. Die negativen Eigenwerte können beispielsweise durch eines der folgenden Eigenwert-Verfahren korrigiert werden: das Absolute-Eigenwert-Korrekturverfahren, das Null-Eigenwert-

Korrekturverfahren und das Delta-Eigenwert-Korrekturverfahren.

Bei dem Absolute-Eigenwert-Korrekturverfahren wird ein negativer Eigenwert durch seinen absoluten Wert ersetzt, sodass die durch die Eigenvektoren aufgespannten Dimensionen erhalten bleiben. Bei dem Null-Eigenwert-Korrekturverfahren wird ein

negativer Eigenwert durch den Wert Null ersetzt, was die minimale Änderung

darstellt, um eine positiv semidefinite Kovarianzmatrix zu erhalten. Bei dem DeltaEigenwert-Korrekturverfahren werden negative Eigenwerte durch positive empirische Werte ersetzt. Aufgrund von Konsistenzvorteilen wird das Absolute-

Eigenwert-Korrekturverfahren bevorzugt eingesetzt.

Nach Korrektur der Eigenwerte wird die Kovarianzmatrix basierend auf den korrigierten Eigenwerten und den Eigenvektoren rekonstruiert und im Rahmen des rekursiven Bayesschen Filters zur Schätzung der Kernzustandsgrößen des beweglichen Objekts eingesetzt. Eine nicht-positiv semidefinite und somit ungültige Kovarianzmatrix würde längerfristig zur Divergenz und Fehlverhalten des rekursiven Bayesschen Filters führen, was den hier beschriebene Korrekturschritt in diesem erfindungsgemäßen Verfahren unabdingbar macht, um die Fusion von mehreren, auch nach Aufstarten des beweglichen Objekts hinzugefügten Sensoren bzw. von

deren Beobachtungswerten auf dynamische und effiziente Weise zu ermöglichen.

Bei dem erfindungsgemäßen Verfahren zur Schätzung von (Kern-)Zustandsgrößen eines beweglichen Objekts handelt es sich um ein rekursives Verfahren, welches die Fusion von mehreren, auch nach Aufstarten des beweglichen Objekts hinzugefügten Sensoren bzw. von deren Beobachtungswerten auf dynamische und effiziente Weise ermöglicht. Entsprechendes gilt für das Entfernen von Sensoren während der Laufzeit eines beweglichen Objekts. Die vorliegende Erfindung bietet einen robusten, modularen Ansatz für eine Multi-Sensor-Datenfusion (auch) für Sensoren, die weder dem zur Schätzung der Kernzustandsgrößen eingesetzten rekursiven Bayesschen Filters noch dem beweglichen Objekt a priori bekannt sind. Eine asynchrone und dynamische Verarbeitung der zu unterschiedlichen Zeitpunkten von den jeweiligen zusätzlichen Sensoren gemachten Beobachtungen ist ohne weiteres möglich. Dies ist insbesondere bei Langzeiteinsätzen von

beweglichen Objekten, beispielsweise Roboterplattformen, von erheblichem Vorteil.

Figur 1 verdeutlicht den modularen Ansatz der vorliegenden Erfindung. Figur 1a) zeigt einen Multi-Sensor-Datenfusionsansatz gemäß dem Stand der Technik. Die

verwendete Kovarianzmatrix enthält zusätzlich zu der Kovarianz der

Kernzustandsgrößen des beweglichen Objekts, die Kovarianzen der Kalibrierungszustände zweier zusätzlicher Sensoren A und B und die Kreuzkorrelationen bzw. Kreuzkovarianzen zwischen den Kernzustandsgrößen des beweglichen Objekts und den Kalibrierungszustandsgrößen der Sensoren A und B. Beim in Figur 1a) gezeigten Stand der Technik nimmt also die Größe und Komplexität der Kovarianzmatrix mit jedem zusätzlichen Sensor zu, und es werden bei jedem Aktualisierungs-/Korrekturschritt die mit sämtlichen zusätzlichen Sensoren assoziierten Kovarianzen und Kreuzkovarianzen zusätzlich zu der Kovarianz der Kernzustandsgrößen des beweglichen Objekts aktualisiert bzw.

korrigiert.

Figur 1b) veranschaulicht den modularen Sensorfusionansatz der vorliegenden Erfindung, bei welchem eine Segmentierung der Kovarianzmatrix für jeden Sensor erfolgt. Nur für denjenigen zusätzlichen Sensor, von welchem gerade Beobachtungswerte erhalten werden, wird die Kovarianzmatrix gebildet. Von daher werden im Aktualisierungsschritt nur die Kovarianz der Kernzustandsgrößen des beweglichen Objekts und die mit dem jeweils aktiven zusätzlichen Sensor assoziierte Kovarianz und assoziierten Kreuzkovarianzen bzw. Kreuzkorrelationen mit den neuen Beobachtungen dieses zusätzlichen Sensors aktualisiert/korrigiert. Entsprechendes gilt für den Propagierungsschritt. Beides, Propagieren und Aktualisieren/Korrigieren, wird modular, d.h. separat für jeden einzelnen zusätzlichen Sensor durchgeführt, wobei die Zeitdauer für die Ausführung sowohl des Propagierungsschrittes (Propagierungsphase) als auch des Aktualisierungsschrittes (Aktualisierungsphase bzw. Korrekturphase) konstant und

unabhängig von der Anzahl der zusätzlichen Sensoren bleibt.

In Figur 1b) werden — von links nach rechts — zuerst nur Beobachtungen von dem zusätzlichen Sensor A, dann nur von dem zusätzlichen Sensor B und schließlich wieder nur von dem Sensor A geliefert und für die Aktualisierung der Kovarianzmatrix verwendet. Komplexität und Größe der Kovarianzmatrix bleiben dabei vorteilhafterweise unabhängig von der Anzahl zusätzlicher Sensoren und

konstant.

Bei dem erfindungsmäßen Verfahren wird das effiziente, nachträgliche Hinzufügen von zusätzlichen Sensoren durch ein Entkoppeln der Kernzustandsgrößen des beweglichen Objekts von den Kalibrierungszuständen der jeweiligen zusätzlichen Sensoren ermöglicht, wodurch die Kernzustandsgrößen des beweglichen Objekts und die Kalibrierungszustandsgrößen der jeweiligen zusätzlichen Sensoren jeweils unabhängig voneinander propagiert werden können. Bei dem erfindungsgemäßen Verfahren werden die nachträglich hinzugefügten, zusätzlichen Sensoren kontinuierlich selbst kalibriert. Dadurch kennzeichnet sich das erfindungsgemäße

Verfahren durch eine hohe Flexibilität bei niedriger benötigter Rechenleistung aus.

Bei dem erfindungsgemäßen Verfahren ist somit die Komplexität insgesamt nur linear von der Anzahl der zusätzlichen Sensoren abhängig. Im Propagierungsschritt bleibt die Komplexität sogar im Wesentlichen konstant. Das erfindungsgemäße Verfahren benötigt also wesentlich weniger Rechenleistung als die aus dem Stand der Technik bekannten Verfahren und kann folglich Beobachtungen schneller verarbeiten als bekannte Verfahren, was wiederum eine genauere Navigation bzw. Regelung des beweglichen Objekts bei gleicher Rechenleistung ermöglicht. Alternativ oder zusätzlich kann die eingesparte Rechenleistung beispielsweise eingesetzt werden, die Reichweite des beweglichen Objekts zu erhöhen. Alternativ können leistungsschwächere und in der Regel kostengünstigere Prozessoren

eingesetzt werden.

Gleichzeitig bleiben Konsistenz und Beobachtbarkeit des eingesetzten rekursiven Bayesschen Filters bei dem erfindungsgemäßen Verfahren erhalten, dies insbesondere dadurch, dass die Kovarianzmatritzen trotz modularer Behandlung konsistent und positiv semidefinit gehalten werden. Mit Hilfe der Erfindung können zusätzliche, nach dem Aufstarten hinzugefügte Sensoren effizient initialisiert und in Bezug auf das jeweilige bewegliche Objekt kalibriert werden. Insgesamt konnte bei dem erfindungsgemäßen Verfahren eine Effizienzsteigerung von 30% gegenüber bekannten Verfahren beobachtet werden. Eine dezentralisierte Implementierung des erfindungsgemäßen Verfahrens, die weniger Bandbreite als eine zentralisierte

Implementierung benötigt, ist ohne weiteres möglich. Kalibrierungszustandsgrößen

der zusätzlichen Sensoren können dabei lokal gespeichert und propagiert und

aktualisiert werden.

Die vorliegende Erfindung kann beispielsweise auf den technischen Gebieten der Robotik, Automation und in der Automobil- und Fahrzeugindustrie eingesetzt werden, wobei der Begriff „Fahrzeug“ neben Automobilen auch Luft- und Wasserfahrzeuge umfasst. Wegen seiner vergleichsweise geringen Komplexität eignet sich das erfindungsgemäße Verfahren besonders für den Einsatz in autonomen Fahrzeugen mit begrenzten Dimensionen und begrenzter Rechenleistung wie beispielsweise Drohnen (unbemanntes Flugobjekt, UAV -

unmanned aerial vehicles).

Aufgrund der oben beschriebenen Segmentierung der Kovarianzmatrix für jeden zusätzlich hinzugefügten Sensor und der damit einhergehenden Eignung für die Verarbeitung asynchroner Beobachtungen eignet sich die vorliegende Erfindung auch besonders für die Verarbeitung verzögerter Beobachtungen bzw. Messdaten, nicht ursprünglich geplante, „außerhalb der Reihe“ liegende Aktualisierungen/Korrekturen der Kovarianzmatrix und die Überwachung von

Sensorgesundheit (sensor health).

Die vorliegende Erfindung betrifft weiter ein computerlesbares Speichermedium, welches Befehle umfasst, welche bei der Ausführung durch einen Computer diesen veranlassen, das erfindungsgemäße Verfahren auszuführen. Mit dem computerlesbaren Speichermedium werden die Vorteile des erfindungsgemäßen Verfahrens realisiert. Ferner betrifft die vorliegende Erfindung ein System zur Datenverarbeitung, welches Mittel zur Ausführung des erfindungsgemäßen Verfahrens umfasst. Die Mittel sind ausgestaltet, eine oder mehrere Sensorkomponenten zu instantiieren, welche einen oder mehrere nach Aufstarten eines beweglichen Objekts hinzugefügte zusätzliche Sensoren repräsentieren, und ferner eine Filterkomponente zu instantiieren, welche einen rekursiven Bayesschen Filter repräsentiert, wobei die Filterkomponente abhängig von der Ausführung des

rekursiven Bayesschen Filter ist.

Mit dem erfindungsgemäßen Datenverarbeitungssystem lässt sich eine modulare Implementierung des erfindungsgemäßen Verfahrens realisieren, die es auf einfache und effiziente Weise ermöglicht, zusätzliche Sensoren durch Instantiierung entsprechender Komponenten (auch Module, Instanzen genannt) zu berücksichtigen und wieder zu entfernen, wenn die jeweiligen Sensoren entfernt werden. Ferner ermöglicht das erfindungsgemäße Datenverarbeitungssystem einen schnellen und unkomplizierten Wechsel des verwendeten rekursiven Bayesschen Filter durch einfache Änderung bzw. Neuinstantiieren der ihn repräsentierenden

Filterkomponente.

KURZE BESCHREIBUNG DER ZEICHNUNGEN

Weitere vorteilhafte Ausgestaltungen der Erfindung ergeben sich aus den abhängigen Ansprüchen und den anhand der Zeichnungen nachfolgend dargestellten Ausführungsbeispielen. Es zeigen:

Fig. 1 einen schematischen Vergleich der Kovarianzmatrix eines für die Schätzung von Kernzustandsgrößen eingesetzten als Kalman-Filter ausgeführten rekursiven Bayesschen Filters nach dem Stand der Technik (Figur 1a)) und nach dem erfindungsgemäßen Verfahren (Figur 1b)),

Fig. 2 ein Ausführungsbeispiel des erfindungsgemäßen Verfahrens mit beispielhaft zwei zusätzlichen Sensoren,

Fig. 3 eine beispielhafte Kalibrierung von Kalibrierungszustandsgrößen zusätzlicher Sensoren gemäß dem erfindungsgemäßen Verfahren in schematischer Darstellung,

Fig. 4 eine beispielhaftes Flugprofil eines Anwendungsbeispiels des erfindungsgemäßen Verfahrens in Form einer Drohne,

Fig. 5 eine beispielhafte Implementierung des erfindungsgemäßen Verfahrens und

Fig. 6 einen beispielhaften Aufbau eines Zwischenspeichereintrags.

Figur 1 ist im Zusammenhang mit der Beschreibung der Vorteile der Erfindung in der obigen Beschreibungseinleitung beschrieben. Es wird auf die dortigen Textteile

verwiesen.

BESCHREIBUNG DER AUSFÜHRUNGSBEISPIELE

Zur Steuerung und Regelung eines beweglichen Objekts werden typischerweise Kernzustandsgrößen des beweglichen Objekts vorab definiert, die die wesentlichen Variablen des beweglichen Objekts beschreiben. Für ein bewegliches Objekts, welches beispielsweise eine inertiale Messeinheit (IMU) umfasst bzw. an welchem eine IMU vorgesehen ist, sind die Kernzustandsgrößen insbesondere Navigationszustandsgrößen wie die Position des objektbezogenen Koordinatensystems (Körperrahmen, body frame) pw, welche in Bezug auf ein Referenzkoordinatensystem (world frame, Weltkoordinatensystem) definiert ist, die Geschwindigkeit vw der inertialen Messeinheit in Bezug auf das

Referenzkoordinatensystem, die Orientierung der inertialen Messeinheit in Bezug

auf das Referenzkoordinatensystem Im das gyroskopische Bias bw und das Beschleunigungsbias ba. Das objektbezogene Koordinatensystem und das Referenzkoordinatensystem sind gleichhändig, vorzugsweise rechtshändig definiert. Die Position und Orientierung des objektbezogenen Koordinatensystems entspricht bevorzugt der Position und Orientierung der inertialen Messeinheit in Bezug auf das Referenzkoordinatensystem. Es ergibt sich die folgende (Kern-

)Zustandsgrößenmatrix Xc:

LAT OT T T TI 45 Xc — Pyers Yayıı Qu: LS bb} * (D

Bewegliche Objekte können grundsätzlich durch mathematische, zeitabhängige Modelle abgebildet werden, welche die Kernzustandsgrößen und deren Kovarianzen auf den nächsten Zeitpunkt propagieren und die auf den von der inertialen Messeinheit gemessenen Kernzustandsgrößen basieren. Die folgenden Differentialgleichungen beschreiben ein derartiges mathematisches Modell, auch

Zustandsraummodell genannt, welches die Kernzustandsgrößen abbildet:

. = Day 7 War {2} Vu — Bawr) (Bay a Da m 114} X (3) er — Sm — Das Die yo {4} Des — Ka N = (S)

Dabei ist am ein Messwert der inertialen Messeinheit der linearen Beschleunigung des beweglichen Objekts, ba das Bias der linearen Beschleunigung des beweglichen Objekts, na Messrauschen der linearen Beschleunigung, g die Gravitätskonstante, wm ein Messwert der inertialen Messeinheit der Winkelgeschwindigkeit des beweglichen Objekts, bw das Bias der Winkelgeschwindigkeit des beweglichen Objekts und nw das Messrauschen der Winkelgeschwindigkeit. Ferner sind R und Q(w) wie folgt definiert, wobei Q(w) die

rechtsseitige Quaternionen-Multiplikations-Matrix ist:

„2 3 A A fen Ofen u Os + a m) U — x AG AA v ”“ © dw 4} e} Ad x Sr} DE a a aA 8 va R = 2 ( Gede or Ahetfs } Ge T 5 EN + 7 N U Ss 2 Eh vd T 7 del 2} CE NM Ka AR Ars — God) AUdy + bw) } Eh Er - 7 + Ö AU Ay U

OQw) S wur = © To wx 0 Wi a “

üb — u ® Up OR 0 Dr Kal Way ung 2 {} In Bezug auf R gilt: cPas © Bf )APun

Falls während der Laufzeit des beweglichen Objekts zusätzliche Sensor hinzugefügt bzw. berücksichtigt werden sollen, so sind diese in der Regel nicht auf das bewegliche Objekt ausgerichtet. Derartige zusätzliche Sensoren bzw. deren extrinsische Eigenschaften können als Kalibrierungszustandsgrößen in das obige Modell eingefügt werden, wobei die Kalibrierungszustandsgrößen geschätzt werden können. Werden zum Beispiel zwei zusätzliche Sensoren S1 und Sz während der Laufzeit des beweglichen Objekts, d.h. nach dessen Aufstarten, zu diesem hinzugefügt, können die Kernzustandsgrößen Xc wie folgt um die

Kalibrierungszustandsgrößen Xs1 und Xsz erweitert werden: X = [Xe; Xs; Xeil. (6)

Die Beobachtung der zusätzlichen Sensoren bzw. von deren Kalibrierungszustandsgrößen, beispielsweise mittels eines als Extended-KalmanFilter ausgeführten rekursiven Bayesschen Filters, führt Kreuzkorrelationen zwischen den Kernzustandsgrößen und den Kalibrierungszustandsgrößen der zusätzlichen Sensoren und somit zu Kreuzkovarianzen in der Kovarianzmatrix P des rekursiven Bayesschen Filters. Nach Beobachtungen durch die während der Laufzeit hinzugekommenen zusätzlichen Sensoren S1 und Sz ergibt sich die

folgende Kovarianzmatrix P:

Po Posi Pos P= Pac Rıy 0 (7)

Posc 0 P.,

— T Pos = (Pac) ist (entsprechend für den zusätzlichen Sensor S1) und die

wobei Gleichung (7) auf der Annahme basiert, dass die zusätzlichen Sensoren S1 und Sz unabhängig voneinander sind, d.h. sich nicht gegenseitig beeinflussen. Dies ist beispielsweise für einen GNSS-Sensor (GNSS -— Globales Navigationssatellitensystem) und einen als Kamera ausgebildeten Sensor der Fall.

Position und positionsmäßige Kalibrierung des GNSS-Sensors (oder eines GPS-

Sensors) mit Bezug auf das bewegliche Objekt (z.B. einem Fahrzeug) sind unabhängig von der Position der Kamera an dem beweglichen Objekt. Entsprechendes gilt für ein Magnetometer mit drei Freiheitsgraden und einen GNSS-Sensor mit drei Freiheitsgraden. Die rotationsmäßige Kalibrierung des Magnetometers in Bezugs auf das objektbezogene Koordinatensystem, in welchem die Position der inertialen Messeinheit definiert ist, und die Translation des GNSSSensors in Bezug auf das objektbezogene Koordinatensystem bzw. die inertiale Messeinheit stehen in keiner physikalischen Beziehung zueinander. Selbst wenn aus analytischen Gesichtspunkten Kreuzkorrelationen bestehen sollten, so sind diese und die mit deren Berücksichtigung einhergehende Genauigkeitsabnahme im Vergleich zu der Zunahme der Rechengeschwindigkeit bei Weglassen dieser

Kreuzkorrelationen vernachlässigbar.

Figur 2 zeigt ein Ausführungsbeispiel eines erfindungsgemäßes Verfahrens für den Fall von zwei zusätzlichen, nach dem Aufstarten hinzugefügten Sensoren 1 und 2. Bespielhaft ist der Zeitraum zwischen den Zeitpunkten t = 0 und t = 31 dargestellt. In Figur 2 beziehen sich Felder mit einer Schraffur mit positiver Steigung auf den zusätzlichen Sensor 1, Felder mit einer Schraffur mit negativer Steigung auf den zusätzlicher Sensor 2 und Felder mit Rauten-Schraffur auf die Kernzustandsgrößen. Die Blöcke 3, 6 und 9 der Figur 2 zeigen jeweils die Kovarianzmatrix zu unterschiedlichen Zeitpunkten unterteilt in ihre einzelnen (Kovarianz- und Kreuzkovarianz-) Segmente, die gemäß dem erfindungsgemäßen Verfahren separat verarbeitet werden (vgl. auch Figur 1). Die in Figur 2 beispielhaften Beobachtungszeitpunkte der Sensoren 1 und 2 sind von den Messdaten der

eingesetzten zusätzlich Sensoren abhängig.

Zum Zeitpunkt t=0 wird das bewegliche Objekt aufgestartet, und der zum Schätzen der Zustandsgrößen eingesetzte rekursive Bayessche Filter und die vorab definierten Kernzustandsgrößen des beweglichen Objekts werden initialisiert (Block 1 in Figur 2).

24 / 40

Nach dem Aufstarten werden während der Laufzeit des beweglichen Objekts zwei zusätzliche Sensoren hinzugefügt: Der Sensor 1 wird zum Zeitpunkt t = 5 hinzugefügt. Der Sensor 2 wird zum Zeitpunkt t = 9 hinzugefügt. Wird festgestellt, dass zusätzliche Sensoren hinzugefügt worden sind bzw. werden Beobachtungswerte von diesen zusätzlichen Sensoren gebildet, so werden die Kovarianz der Kalibrierungszustandsgrößen des jeweiligen zusätzlichen Sensors und die Kreuzkovarianzen der Kernzustandsgrößen des beweglichen Objekts und der Kalibrierungszustandsgrößen des jeweiligen zusätzlichen Sensors mit Hilfe der zum Zeitpunkt der Hinzufügung gebildeten Beobachtungswerte initialisiert bzw. kalibriert. D.h. im vorliegenden Bespiel werden die mit dem zusätzlichen Sensor 1 assoziierte Kovarianz und assozlierten Kreuzkovarianzen zum Zeitpunkt t = 5 initialisiert/kalibriert (ebenfalls Block 1 der Figur 2), während die mit dem zusätzlichen Sensor 2 assoziierte Kovarianz und assoziierten Kreuzkovarianzen zum Zeitpunkt t = 9 initialisiert/kalibriert werden (ebenfalls Block S1 der Figur 2). Handelt es sich bei dem zusätzlichen Sensor 1 beispielsweise um einen GPSSensor, so wird zum Zeitpunkt t = 5 die Position des GPS-Sensors im

objektbezogenen Koordinatensystems des beweglichen Objekts kalibriert.

Unabhängig von der Initialisierung der zusätzlichen Sensoren 1 und 2 werden die Kernzustandsgrößen und deren Kovarianz bis einen Zeitschritt vor denjenigen nächsten Zeitpunkt propagiert, an welchem einer der zusätzlichen Sensoren 1 und 2 nächste Beobachtungswerte ausgibt/bildet. Im vorliegenden Beispiel ist dies der Zeitpunkt t = 20, denn zum Zeitpunkt t = 21 bildet der zusätzliche Sensor 1 erneut

Beobachtungswerte (Block 2 in Figur 2).

Zum Zeitpunkt t = 21 werden nun die mit dem zusätzlichen Sensor 1 assoziierte neueste Kovarianz Ps1 und die assoziierten neuesten Kreuzkovarianzen Pcs1 mit der neuesten Kovarianz Pc der Kernzustandsgrößen des beweglichen Objekts zur Bildung der Kovarianzmatrix P kombiniert. Dabei sind die mit dem zusätzlichen Sensor 1 assoziierte neueste Kovarianz Ps1 und die neuesten Kreuzkovarianzen Pes1 die zum Zeitpunkt t = 5 initialisierte Kovarianz Ps1 und die zum Zeitpunkt t = 5

initialisierten Kreuzkovarianzen Pcs1. Die Kovarianz Pc der Kernzustandsgrößen

entspricht der auf den Zeitpunkt t = 20 propagierten Kovarianz Pc (vgl. Block 3 in Figur 2).

Vorzugsweise wird für die Bildung der Kovarianzmatrix P die mit dem zusätzlichen Sensor 1 assoziierte neueste Kovarianz Ps1 und die assozilerten neuesten Kreuzkovarianzen Pcs1 wie oben beschrieben mit Hilfe einer Reihe von Zustandsübergangsmatrizen auf den gleichen Zeitpunkt wie die Kovarianz der Kernzustandsgrößen, d.h. auf t = 20, propagiert. Ferner wird die Kovarianzmatrix P bevorzugterweise, insbesondere durch Einsatz eines Eigenwert-Verfahrens, in eine positiv semidefinite Kovarianzmatrix korrigiert, falls erforderlich, wie oben beschrieben. Schließlich wird die gebildete Kovarianzmatrix (bzw. deren Segmente)

zwischengespeichert (ebenfalls Block 3 der Figur 2).

Mit der in Block 3 gebildeten Kovarianzmatrix wird nun in Block 4 mit den zum Zeitpunkt t = 21 gebildeten Beobachtungen des zusätzlichen Sensors 1 eine Aktualisierung bzw. Korrektur der Kovarianzmatrix durchgeführt. Mit der aktualisierten Kovarianzmatrix werden dann Kernzustandsgrößen geschätzt. Danach werden die mit dem zusätzlichen Sensor 1 assoziierte Kovarianz und die assoziierten Kreuzkovarianzen von der Kovarianz der Kernzustandsgrößen getrennt (Block 4 in Figur 2).

In Block 5 von Figur 2 werden die Kernzustandsgrößen bzw. deren Kovarianz bis einen Zeitschritt vor Erhalt der nächsten Beobachtungswerte zum Zeitpunkt t = 26 propagiert. D.h. die Kovarianz der Kernzustandsgrößen wird vom Zeitpunkt 22 auf den Zeitpunkt t = 25 propagiert. Im vorliegenden Beispiel werden die nächsten Beobachtungswerte zum Zeitpunkt t = 26 von dem zusätzlichen Sensor 2

ausgegeben.

In Block 6 wird zum Aktualisieren der Kovarianzmatrix mittels der zum Zeitpunkt t = 26 erhaltenen Beobachtungswerte die Kovarianzmatrix erneut gebildet aus: der auf den Zeitpunkt t = 25 propagierten Kovarianz der Kernzustandsgrößen und den zum Zeitpunkt t = 9 initialisierten, mit dem zusätzlichen Sensor 2 assoziierten Kovarianz

und assozlierten Kreuzkovarianzen (Block 6 von Figur 2). Auch in Block 6 wird

vorzugsweise für die Bildung der Kovarianzmatrix die mit dem zusätzlichen Sensor 2 assozlierte neueste Kovarianz und assoziierten neuesten Kreuzkovarianzen mit Hilfe einer Reihe von Zustandsübergangsmatrizen auf den gleichen Zeitpunkt wie die Kovarianz der Kernzustandsgrößen, d.h. auf t = 25, propagiert. Ferner wird die Kovarianzmatrix bevorzugterweise, insbesondere durch Einsatz des EigenwertVerfahrens, in eine positiv semidefinite Kovarianzmatrix korrigiert, falls erforderlich. Schließlich wird die gebildete Kovarianzmatrix (bzw. deren Segmente)

zwischengespeichert (ebenfalls Block 6 der Figur 2).

In Block 7 wird die in Block 6 gebildete Kovarianzmatrix dann mit Hilfe der zum Zeitpunkt t = 26 erhaltenen Beobachtungswerte des zusätzlichen Sensors 2 aktualisiert bzw. korrigiert, und die Kernzustandsgrößen werden mit der aktualisierten Kovarianzmatrix geschätzt. Danach werden die mit dem zusätzlichen Sensor 2 assoziierte Kovarianz und assoziierten Kreuzkovarianzen wieder von der

Kovarianz der Kernzustandsgrößen getrennt (Block 7 in Figur 2).

In den Blöcken 8 bis 10 von Figur 2 setzt sich das erfindungsgemäße Verfahren wie oben beschrieben fort. Als nächstes werden zum Zeitpunkt t = 31 Beobachtungswerte vom zusätzlichen Sensor 1 erhalten. Entsprechend wird die Kovarianz der Kernzustandsgrößen vom Zeitpunkt t = 27 auf den Zeitpunkt t = 30 propagiert. Aus der derart propagierten Kovarianz der Kernzustandsgrößen und den neuesten mit dem Sensor 1 assoziierten Kovarianz und Kreuzkovarianzen (vom Zeitpunkt t = 21; siehe Beschreibung von Block 4) wird die Kovarianzmatrix gebildet, welche dann in Block 10 mit den Beobachtungswerten von zusätzlichen Sensor 1 zum Zeitpunkt t = 31 aktualisiert bzw. korrigiert und zur Schätzung der

Kernzustandsgrößen herangezogen wird.

Auch in Block 9 wird vorzugsweise für die Bildung der Kovarianzmatrix die mit dem Sensor 1 assoziierte neueste Kovarianz und assozlierten neuesten Kreuzkovarianzen mit Hilfe der Reihe von Zustandsübergangsmatrizen auf den gleichen Zeitpunkt wie die Kovarianz der Kernzustandsgrößen, d.h. auf t = 30,

propagiert. Ferner wird die Kovarlanzmatrix bevorzugterweise, insbesondere durch

Einsatz des Eigenwert-Verfahrens, in eine positiv semidefinite Kovarianzmatrix korrigiert, falls erforderlich. Schließlich wird die gebildete Kovarianzmatrix (bzw. deren Segmente) vor ihrer Aktualisierung in Block 10 zwischengespeichert (Block 9 der Figur 2).

D.h. bei dem erfindungsgemäßen Verfahren werden vorteilhafterweise die Kernzustandsgrößen (bzw. deren Kovarianz) getrennt von den Kalibrierungszustandsgrößen der Sensoren (bzw. deren Kovarianz und Kreuzkovarianzen mit den Kernzustandsgrößen) propagiert. Die Kalibrierungszustandsgrößen der zusätzlichen Sensoren (bzw. deren Kovarianz und Kreuzkovarianzen mit den Kernzustandsgrößen) werden wiederum getrennt von den Kernzustandsgrößen initialisiert/kalibriert und getrennt voneinander aktualisiert. Mit der vorliegenden Erfindung können also nach dem Aufstarten hinzugefügte zusätzliche Sensoren berücksichtigt werden, ohne die Kovarianzen und mit den Kernzustandsgrößen assozlierten Kreuzkovarianzen anderer Sensoren zu

beeinflussen.

Figur 3 zeigt schematisch eine Initialisierung/(Selbst-)Kalibrierung von beispielhaften Kalibrierungszustandsgrößen zusätzlicher Sensor, welche bei einem beweglichen Objekt 10, welches eine inertiale Messeinheit IMU umfasst, nachträglich hinzugefügt werden. Das bewegliche Objekt 10 und die inertiale Messeinheit IMU (inertial measurement unit) sind in einem Referenzkoordinatensystem „Nav World“ angeordnet. Nach dem Aufstarten des beweglichen Objekts 10 werden zusätzliche Sensoren in Form eines Sichtsensors („Vision“, beispielsweise eine Kamera), eines Drucksensors („Pressure“, beispielsweise ein Barometer) und eines GPS-Sensors hinzugefügt. Die Beobachtungen dieser Sensoren beziehen sich auf die jeweiligen Sensorkoordinatensysteme („Vision Ref.“, „Pressure Ref.“, „GPS Ref.“), wie in Figur 3 durch durchgezogene Linien angegeben ist. Bei dem Koordinatensystem „GPS Ref.“ des GPS-Sensors handelt es sich um ein vorgegebenes globales Koordinatensystem, das in Bezug auf das Referenzkoordinatensystem „Nav World“

fixiert und in Bezug auf dieses definiert ist (strich-punktierte Linie in Figur 3). Die

gestrichelten Linien zwischen den Sensorkoordinatensystemen „Vision Ref.“ und „Pressure Ref“ geben deren Position und Orientierung in Bezug auf das Referenzkoordinatensystem „Nav World“ an. Die gestrichelte Linie zwischen der IMU und dem Referenzkoordinatensystem gibt die Position und Orientierung des objektbezogenen Koordinatensystems in Bezug auf das Referenzkoordinatensystem „Nav World“ an. Die gepunkteten Linien zwischen den Sicht-, Druck- und GPS-Sensoren und der das bewegliche Objekt 10 repräsentierenden IMU geben die im Rahmen des erfindungsgemäßen Verfahrens erfolgende, oben im Zusammenhang mit Block 1 der Figur 2 beschriebene Initialisierung/Kalibrierung der Kalibrierungszustandsgrößen der zusätzlichen Sensoren in Bezug auf das objektbezogene Koordinatensystem des beweglichen

Objekts 10 bzw. von dessen IMU an.

Figur 4 zeigt ein zu Figur 3 gehörendes Anwendungsbeispiel, bei welchem das bewegliche Objekt als Drohne 10 ausgebildet ist. Figur 4 zeigt ein beispielhaftes Flugprofil der Drohne 10. Das Flugprofil enthält verschiedene Phasen 1, 2, 3, 4, 5, in welchen unterschiedliche zusätzliche Sensoren (bzw. deren Messdaten/Beobachtungswerte) hinzugefügt oder entfernt werden. So wird in der Phase 1 (Start von der Landeplattform (landing pad)) und der Phase 2 (Geradeausflug) der Sichtsensor eingesetzt. In der Phase 2 kommen zusätzlich der Drucksensor (Barometer) und der GPS-Sensor hinzu, welche auch in der Phase 3 (Wenden) eingesetzt werden. In der Phase 3 wird der Sichtsensor dagegen nicht eingesetzt und ist ausgeschaltet. Er wird erst wieder bei der Rückkehr in die Phase 2 zugeschaltet und auf die jetzige Position der Drohne 10 kalibriert/initialisiert. Danach werden nach einer kurzen Überlappungsperiode der Drucksensor und der GPS-Sensor deaktiviert, sodass sie beim Eintritt in die finale Phase 5 nicht mehr berücksichtigt werden. In der Phase 5 erfolgt in diesem Beispiel also die Landung auf der Landeplattform allein mittels des Sichtsensor. Anstelle eines GPS-Sensors

kann selbstverständlich auch ein anderer GNSS-Sensor eingesetzt werden.

Ein zusätzlicher Sensor kann auch als eine andere Drohne bzw. ein auf einer

anderen Drohne vorgesehener oder mit einer solchen assoziierter Sensor

ausgebildet sein, z.B. als eine an einer anderen Drohne installierte Kamera, welche

diejenige Drohne, deren Kernzustandsgrößen geschätzt werden sollen, filmt.

Figur 5 zeigt eine beispielhafte modulare Implementierung des erfindungsgemäßen Verfahrens. Die Implementierung kann mittels Softwarekomponenten und/oder Hardwarekomponenten erfolgen. Jede der Komponenten (auch Modul, Instanz oder Einheit genannt) ist vorzugsweise eigenständig mit klar definierten Schnittstellen ausgeführt, um eine einfache und effiziente Austauschbarkeit der jeweiligen

Komponente zu ermöglichen.

Bevorzugt ist eine Kernlogikkomponente 20 (auch Hauptlogikkomponente genannt) vorgesehen, welche bei der Ausführung des erfindungsgemäßen Verfahrens für den organisatorischen Teil verantwortlich ist und die Schnittstelle zwischen Zwischenspeicher 22 und den Sensorkomponenten 24, 26, 28 bildet. Die Sensorkomponenten 24 und 26 repräsentieren bzw. instantiieren zusätzliche, nach dem Aufstarten zugeschaltete Sensoren wie beispielsweise einem GPS-Sensor 24 und einem Sichtsensor 26 (z.B. eine Kamera). Die Sensorkomponente 28 repräsentiert vorzugsweise einen bereits beim Aufstarten zugeschalteten Sensor wie beispielsweise einen Propagierungssensor, d.h. einen Sensor, der für die Kernzustandsgrößen relevante Beobachtungen liefert. Handelt es sich bei den Kernzustandsgrößen um Navigationszustandsgrößen so kann der

Propagierungssensor beispielsweise als IMU ausgebildet sein.

Die Kernlogikkomponente 20 ist insbesondere derart ausgestaltet, dass sie den Nutzen der von den Sensoren gebildeten Beobachtungswerte überprüft und diese gegebenenfalls nicht für eine Schätzung der Zustandsgrößen berücksichtigt, wenn kein Nutzen mehr gegeben ist, beispielsweise bei stark verzögerten Beobachtungen/Messwerten, deren Verzögerung eine vordefinierte Zeitdauer überschreitet oder deren Beobachtungszeitpunkt (Zeitstempel) älter ist als der letzte

Zwischenspeichereintrag ist.

Figur 6 zeigt einen bespielhaften Eintrag im Zwischenspeicher (buffer) 22 für einen

zusätzlichen Sensor. Neben einer Identifikation des jeweiligen zusätzlichen Sensors

sind im Zwischenspeicher der Zeitpunkt der letzten Beobachtungen (Zeitstempel) und die folgenden an diesem Zeitstempel ermittelten Daten/Werte hinterlegt: die Kernzustandsgrößen, die Kovarianz der Kernzustandsgrößen, die Kalibrierungszustandsgrößen des jeweiligen zusätzlichen Sensors, die Kovarianz der Kalibrierungszustandsgrößen, die Kreuzkovarianzen zwischen den Kernzustandsgrößen und den Kalibrierungszustandsgrößen und die

Zustandsübergangsmatrizen. Ferner sind Metadaten hinterlegt.

Wieder bezugnehmend auf Figur 5 ist bevorzugt ein Sensormanager 30 vorgesehen, welcher bevorzugterweise über eine Liste sämtlicher Sensoren verfügt und auf einem höheren Level für die Verwaltung der Sensorkomponenten 24, 26, 28

zuständig ist.

Ferner ist vorzugsweise eine eigenständige Kernzustandsgrößenkomponente 32 vorgesehen, an welche die Kernlogikkomponente 20 Beobachtungen des bereits beim Aufstarten des beweglichen Objekts zugeschalteten Sensors 28 weiterleitet. Die Kernzustandsgrößenkomponente 32 ist ausgestaltet, basierend aus diesen Beobachtungen die Kernzustandsgrößen (Kernzustandsgrößenvektor) und deren Kovarianz zu propagieren, wie oben beschrieben ist. Die propagierten Kernzustandsgrößen und deren Kovarianzen werden in dem Zwischenspeicher 22

gespeichert.

Kommen aktuelle Beobachtungen von einem zusätzlichen, nachträglich hinzugefügten Sensor, so berechnet die Kernzustandsgrößenkomponente 32 außerdem individuelle Zustandsübergangsmatrizen für jeden einzelnen Propagationsschritt. Die individuellen Zustandsübergangsmatrizen werden dann

ebenfalls in dem Zwischenspeicher 22 hinterlegt.

Bei Empfang von aktuellen Beobachtungswerten von einem zusätzlichen Sensor fordert die Kernlogikkomponente 20 den letzten bzw. neuesten Eintrag in dem Zwischenspeicher 22 an und berechnet die Reihe der Zustandsübergangsmatrizen beginnend vom Zeitstempel des abgerufenen Zwischenspeichereintrags bis einen

Zeitschritt vor den Zeitpunkt der aktuellen Beobachtungen. Die dem jeweiligen

zusätzlichen Sensor entsprechende Sensorkomponente 24, 26 propagiert die aus dem Zwischenspeicher 22 abgerufene Kovarianz der Kalibrierungszustandsgrößen des zusätzlichen Sensors und die abgerufenen Kreuzkovarianzen zwischen Kalibrierungszustandsgrößen und Kernzustandsgrößen mit Hilfe der berechneten Reihe von Zustandsübergangsmatrizen auf einen Zeitschritt vor den

Beobachtungszeitpunkt der aktuellen Beobachtungswerte.

Aus dieser propagierten Kovarianz der Kalibrierungszustände des zusätzlichen Sensors und den entsprechend propagierten, mit dem zusätzlichen Sensor assoziierten Kreuzkovarianz und der propagierten Kovarianz der Kernzustandsgrößen wird dann, vorzugsweise in der Kernlogikkomponente 20, die Kovarianzmatrix gebildet, welche dann, ebenfalls durch die Kernlogikkomponente 20, gegebenenfalls mit dem Eigenwert-Verfahren korrigiert wird, um eine positive semidefinite Kovarianzmatrix zu erhalten, die dann an die jeweilige Sensorkomponente 24, 26 weitergegeben wird. Danach wird die Kovarianzmatrix mit den neuesten Beobachtungen des jeweiligen zusätzlichen Sensors in der mit diesem assozlierten Sensorkomponente 24, 26 aktualisiert/korrigiert. Vorteilhafterweise können dabei zusätzliche statische Tests wie beispielsweise ein X?-Test von der jeweiligen Sensorkomponente 24, 26 durchgeführt werden. Die in der jeweiligen Sensorkomponente 24, 26 aktualisierte Kovarianzmatrix bzw. deren (Kovarianz- und Kreuzkovarianz-) Segmente werden danach via den Sensormanager 30 an die Kernlogikkomponente 20 übermittelt, welche die

Segmente an den Zwischenspeicher 22 zur Speicherung weiterleitet.

Ferner ist eine Filterkomponente 36 vorgesehen, welche einen rekursiven Bayesschen Filter repräsentiert bzw. instantiiert. Beispielhaft repräsentiert die Filterkomponente 36 einen Kalman-Filter, insbesondere einen Extended-KalmanFilter. Durch die modulare Implementierung/Instantiierung des rekursiven Bayesschen Filters kann dessen konkrekte Ausgestaltung auf einfache und effiziente Weise beispielsweise von einem Extended-Kalman-Filter in einen anderen rekursiven Bayesschen Filter, beispielsweise einen sogenannten Unscented

Kalman-Filter, geändert werden.

Zur Schätzung der Kernzustandsgrößen werden die Sensorkomponenten 24, 26, 28 und die Filterkomplemente 34, vorzugsweise softwaretechnisch, aus einer Abstrakte-Sensor-Komponente 34 abgeleitet. Dies hat den Vorteil, dass einfach und effizient Sensorkomponenten für zusätzliche Sensoren hinzugefügt, entfernt und

ersetzt werden können.

Die in Figur 5 gezeigte Implementierung des erfindungsgemäßen Verfahrens hat den Vorteil, dass die Kernzustandslogikkomponente 20 keinerlei Kenntnisse bezüglich der Sensoren zu haben braucht, sozusagen unabhängig von diesen implementiert sein kann. Alles „Sensorenwissen“, wie z.B. mathematische Definitionen der jeweiligen Sensormodelle und die für die jeweiligen Sensoren einzusendenden Verfahren zur Initialisierung/Kalibrierung, Erzeugung und Handhabung von deren Kalibrierungszuständen einschließlich deren Propagieren, Aktualisieren und Korrektur, ist in den Sensorkomponenten 24, 26, 28 enthalten. Dies erlaubt vorteilhafterweise das effiziente und elegante Hinzufügen bzw. Zuschalten von zusätzlichen Sensoren auch nach dem Aufstarten. Jede Sensorkomponente ist eigenständig und berechnet selbständig die mit ihr assoziierten Kovarianz und Kreuzvarianzen sowohl bei der Initialisierung, als auch beim Propagieren, als auch beim Aktualisieren/Korrigieren, wobei insbesondere bei der Initialisierung die zu deren Zeitpunkt geltenden Kernzustandsgrößen via die

Kernlogikkomponente 20 erhalten und somit berücksichtigt werden können.

Claims (1)

1. (43664)

   Patentansprüche

   1. Computer-implementiertes Verfahren zur Schätzung von Zustandsgrößen eines beweglichen Objekts, gekennzeichnet durch die folgenden Schritte: a) Initialisieren eines rekursiven Bayesschen Filters zum Schätzen von vorab definierten Kernzustandsgrößen des beweglichen Objekts, b) Beobachten von technischen Eigenschaften des beweglichen Objekts mit Hilfe von einem oder mehreren Sensoren unter Bildung von Beobachtungswerten, c) zeitliches Propagieren der Kernzustandsgrößen des beweglichen Objekts und einer Kovarianz der Kernzustandsgrößen mittels eines Zustandsgrößenmodells des rekursiven Bayesschen Filters unter Verwendung derjenigen Beobachtungswerte, welche mit Hilfe von einem oder mehreren Sensoren gebildet worden sind, die seit Aufstarten des beweglichen Objekts eingesetzt werden, d) Feststellen, ob Beobachtungswerte mit Hilfe eines nach dem Aufstarten des beweglichen Objekts hinzugefügten zusätzlichen Sensors gebildet werden, e) falls in Schritt d) festgestellt wurde, dass Beobachtungswerte mit Hilfe eines nach dem Aufstarten des beweglichen Objekts hinzugefügten zusätzlichen Sensors gebildet werden,

   e1) Initialisieren einer Kovarianz von Kalibrierungszustandsgrößen des zusätzlichen Sensors und von Kreuzkovarianzen der Kernzustandsgrößen des beweglichen Objekts und der Kalibrierungszustandsgrößen des zusätzlichen Sensors mit Hilfe der zu einem ersten Zeitpunkt von dem zusätzlichen Sensor gebildeten Beobachtungswerte,

   e2) bei Bilden von Beobachtungswerten durch den zusätzlichen Sensor an einem nach dem ersten Zeitpunkt liegenden zweiten Zeitpunkt: Formen einer Kovarianzmatrix des rekursiven Bayesschen Filters aus der auf einen einen Zeitschritt vor dem zweiten Zeitpunkt liegenden Zeitpunkt propagierten Kovarianz

   der Kernzustandsgrößen des beweglichen Objekts, der neuesten Kovarianz der

   e3) Aktualisieren der Kovarianzmatrix mit Hilfe der zu dem zweiten Zeitpunkt von dem zusätzlichen Sensor gebildeten Beobachtungswerte,

   e4) Ermitteln der Kernzustandsgrößen des beweglichen Objekts mit Hilfe der aktualisierten Kovarianzmatrix des rekursiven Bayesschen Filters,

   e5) Separieren der Kovarianz der Kalibrierungszustandsgrößen des zusätzlichen Sensors und der Kreuzkovarianzen der Kernzustandsgrößen des beweglichen Objekts und der Kalibrierungszustandsgrößen des zusätzlichen Sensors von der Kovarianz der Kernzustandsgrößen des zusätzlichen Sensors,

   e6) Wiederholen der Schritte e2) bis e5) für Beobachtungswerte des zusätzlichen Sensors, die an nach dem zweiten Zeitpunkt liegenden spätere Zeitpunkten gebildet werden, wobei in Schritt e2) die Kovarianz der Kernzustandsgrößen des beweglichen Objekts auf einen einen Zeitschritt vor dem

   jeweiligen späteren Zeitpunkt liegenden Zeitpunkt propagiert wird.

   2. Verfahren nach Anspruch 1 oder 2, wobei die Schritte e1) bis e6) für jeden nach dem Aufstarten des beweglichen Objekts hinzugefügten zusätzlichen Sensors

   durchgeführt werden.

   3. Verfahren nach Anspruch 1, wobei in Schritt e2) die neueste Kovarianz der Kalibrierungszustandsgrößen des zusätzlichen Sensors und die neuesten Kreuzkovarianzen der Kernzustandsgrößen des beweglichen Objekts und der Kalibrierungszustandsgrößen des zusätzlichen Sensors mit Hilfe einer Reihe aus einer oder mehreren Zustandsübergangsmatrizen auf denselben Zeitpunkt wie die

   Kovarianz der Kernzustandsgrößen des beweglichen Objekts propagiert werden.

   4. Verfahren nach Anspruch 3, wobei vor Ausführung von Schritt e3) die gebildete Kovarianzmatrix in eine positiv semidefinite Kovarianzmatrix korrigiert

   wird.

   rekonstruiert wird.

   6. Verfahren nach Anspruch 5, wobei die negativen Eigenwerte durch die Absolute-Eigenwert-Korrektur, die Null-Eigenwert-Korrektur oder die Delta-

   Eigenwert-Korrektur korrigiert werden.

   7. Verfahren nach einem der vorhergehenden Ansprüche, wobei der rekursive Bayessche Filter als Kalman-Filter, insbesondere als Extended-Kalman-Filter,

   ausgestaltet ist.

   8. Computerlesbares Speichermedium, umfassend Befehle, welche bei der Ausführung durch einen Computer diesen veranlassen, ein Verfahren nach einem

   der vorhergehenden Ansprüche auszuführen.

   9. System zur Datenverarbeitung, umfassend Mittel zur Ausführung eines Verfahrens nach einem der vorhergehenden Ansprüche 1 bis 7, wobei die Mittel ausgestaltet sind, eine oder mehrere Sensorkomponenten zu instantiieren, welche einen oder mehrere nach Aufstarten eines beweglichen Objekts hinzugefügte zusätzliche Sensoren repräsentieren, und

   eine Filterkomponente zu instantiieren, welche einen rekursiven Bayesschen Filter repräsentiert, wobei die Filterkomponente abhängig von der Ausführung des

   rekursiven Bayesschen Filter ist.