DE102018104310A1 - Verfahren und System zum Nachverfolgen eines Objekts mittels Doppler-Messinformation - Google Patents

Abstract

Die Erfindung betrifft ein Verfahren zum Nachverfolgen eines dynamischen Objekts (16) unter Ausnutzung von messinformationsbasierten Objektdaten, wobei die Messinformationen mindestens eine Doppler-Messinformation umfassen. Es ist vorgesehen, dass die auf einer Doppler-Messinformation beruhenden Objektdaten zum Nachverfolgen des Objekts (16) mittels einer den Zusammenhang zwischen der Doppler-Messung und dem Zustand des Objekts angebenden impliziten Messfunktion und eines zur Verarbeitung impliziter Messfunktionen eingerichteten Kalman-Filters verarbeitet werden.
Die Erfindung betrifft weiterhin ein entsprechendes Computerprogrammprodukt, eine Verwendung des Verfahrens sowie ein entsprechendes System (14) zum Nachverfolgen eines dynamischen Objekts (16).

Description

• Die vorliegende Erfindung betrifft ein Verfahren zum Nachverfolgen eines dynamischen Objekts unter Ausnutzung von messinformationsbasierten Objektdaten, bei dem die Messinformationen mindestens eine Doppler-Messinformation umfassen, wobei die auf einer Doppler-Messinformation beruhenden Objektdaten zum Nachverfolgen des Objekts mittels eines Filters verarbeitet werden.
• Die Erfindung betrifft weiterhin ein entsprechendes Computerprogrammprodukt, eine Verwendung des Verfahrens sowie ein entsprechendes System zum Nachverfolgen eines Objekts.
• Aus dem wissenschaftlichen Artikel » A. Scheel, C. Knill, S. Reuter und K. Dietmayer, „Multi-Sensor Multi-Object Tracking of Vehicles Using High-Resolution Radars“ in Proceedings of the 2016 IEEE Intelligent Vehicles Symposium, 2016, S. 558-565« ist ein Verfahren zum Nachverfolgen eines dynamischen Objekts unter Ausnutzung von messinformationsbasierten Objektdaten, wobei die Messinformationen Doppler-Messinformationen und weitere Messinformationen umfassen und wobei die auf einer Doppler-Messinformation beruhenden Objektdaten zum Nachverfolgen des Objekts mittels eines Filters verarbeitet werden. Es ergibt sich eine Assoziationsunsicherheit, an welcher Stelle des Objekts gemessen wird. Um sich Beeinträchtigungen dieser Assoziationsunsicherheit zu entledigen, wird bei diesem Ansatz die Wahrscheinlichkeit faktorisiert. Dies erfordert jedoch eine willkürliche Annahme der Verteilung einer der Messvariablen des entsprechenden Sensors (in diesem Artikel wird angenommen, dass der gemessene Azimut eine trapezförmige Wahrscheinlichkeitsdichte aufweist) und ein Partikelfilter wird obligatorisch, um die Form dieser willkürlichen Verteilung zu berücksichtigen.
• Ausgehend von dem Stand der Technik liegt der Erfindung somit die Aufgabe zugrunde, Maßnahmen anzugeben, die eine Nutzung der Doppler-Messinformation ermöglichen ohne dass derartige Annahmen getroffen werden müssen.
• Die Lösung der Aufgabe erfolgt erfindungsgemäß durch die Merkmale der unabhängigen Ansprüche. Vorteilhafte Ausgestaltungen der Erfindung sind in den Unteransprüchen angegeben.
• Bei dem erfindungsgemäßen Verfahren zum Nachverfolgen eines Objekts unter Ausnutzung von messdatenbasierten Objektdaten, bei dem zumindest ein Teil der Messdaten mittels eines Doppler-Messverfahrens erstellt sind, ist vorgesehen, dass die auf der Doppler-Messinformation beruhenden Objektdaten zum Nachverfolgen des Objekts mittels (i) einer den Zusammenhang zwischen der Doppler-Messung und dem Zustand des Objekts angebenden impliziten Messfunktion und (ii) eines Kalman-Filters verarbeitet werden, der zur Verarbeitung impliziter Messfunktionen eingerichtet ist. Auf diese Weise können die auf Doppler-Messinformation beruhenden Objektdaten in ein Verfahren zum Nachverfolgen eines Objekts unter Ausnutzung von messdatenbasierten Objektdaten integriert werden. Ein in Form einer impliziten Messfunktion beschreibbares Modell der Doppler-Messung lässt sich beispielsweise aus dem eingangs erwähnten Artikel von Scheel et al. ableiten. Auf diese Weise kann man den Zusammenhang zwischen der Dopplermessung und dem Zustand als eine implizite Funktion ausdrücken, ohne zu wissen, wo die Messung am Objekt stattfindet. Der besagte zur Verarbeitung impliziter Messfunktionen eingerichtete Kalman-Filter ist beispielsweise aus der Druckschrift DE 10 2016 105 023 A1 bekannt, wobei die implizite Messfunktion dort Innovationsfunktion h genannt wird. In dieser Druckschrift werden neben der Wirkungsweise dieses speziellen Kalman-Filters auch dessen Hauptvorteile gegenüber herkömmlichen Kalman-Filtern diskutiert.
• Der erfindungsgemäße Ansatz hat den Vorteil, dass er eine vollständige Bayessche Rekursion ohne irgendeine willkürliche Annahme bezüglich der Verteilung irgendeiner Messvariable des entsprechenden Sensors ermöglicht und dass kein Partikelfilter verwendet werden muss.
• Hauptanwendungsgebiet dieses Verfahrens ist die Erkennung von Objekten, insbesondere Fahrzeugen, im Straßenverkehr. Zur Beschreibung der Bewegung solcher Objekte benötig man die folgenden Parameter: die zentrale Position x, y des Objekts, die Geschwindigkeit v, die Gierrate (Giergeschwindigkeit) θ sowie den Winkel α zwischen Geschwindigkeitsvektor v und einer Längsachse des Objekts. Im Zusammenhang mit der Dopplermessung beziehungsweise der resultierenden Doppler-Messinformation ist die Ausdehnung des Objekts zwar nicht relevant, aber für Objektdaten, die auf anderen Messinformationen beruhen, muss diese Ausdehnung angegeben werden, beispielsweise mit dem Parameter a zur Angabe der halben Breite und einem Parameter b zur Angabe der halben Länge des Objekts. Bemerkenswert ist hier der Parameter α, der ein Maß dafür ist, wie weit der Abstand zwischen tatsächlicher Gierachse und der zentralen Position x, y ist. Bei Fahrzeugen ergibt sich die Gierachse in der Regel in der Mitte der nichtgelenkten Achse. Diese fällt im Allgemeinen nicht mit der zentralen Position zusammen, sodass in der Regel α ≠ 0 ist.
• Gemäß einer bevorzugten Ausgestaltung der Erfindung ist vorgesehen, dass sich die Parameter der impliziten Messfunktion aus einem Bewegungsmodell (engl.: motion model) ergeben, bei dem berücksichtigbar ist, dass ein an einer zentralen Position des Objekts ansetzender Geschwindigkeitsvektor v des Objekts einen Winkel α ≠ 0 mit der Längsachse des Objekts bildet.
• Gemäß einer weiteren bevorzugten Ausgestaltung der Erfindung hat die implizite Messfunktion die folgende Form oder eine Form äquivalent zu der folgenden Form: $$\begin{array}{c}h\left(X,Y\right)\\ =\frac{\left(v\text{ cos}\left(\theta +\alpha \right)-\dot{\theta }\left(y_P-y\right)-v_x\right)\left(x_P-x_S\right)}{\sqrt{{\left(x_P-x_S\right)}^2+{\left(y_P-y_S\right)}^2}}\\ +\frac{\left(v\text{ sin}\left(\theta +\alpha \right)+\dot{\theta }\left(x_P-x\right)-v_y\right)\left(y_P-x_S\right)}{\sqrt{{\left(x_P-x_S\right)}^2+{\left(y_P-y_S\right)}^2}}-\rho \end{array}$$

  
• Dabei geben die Parameter x_P und y_P die durch den Sensor gemessene Position, die Parameter x_S und y_S die Sensorposition im Bezugssystem der Umwelt (world frame), und ρ die vom Sensor gemessene Relativgeschwindigkeit des Objekts bezüglich dieses Sensors an.
• Gemäß einer weiteren bevorzugten Ausgestaltung der Erfindung ist vorgesehen, dass zum Abschätzen der Form des Objekts und zu dessen Nachverfolgung weitere messinformationsbasierte Objektdaten mittels eines Erwartungs-Maximierungs-Algorithmus verarbeitet werden.
• Aus dem wissenschaftlichen Artikel »H. Kaulbersch, M. Baum and P. Willet „EM Approach for Tracking Star-Convex Extended Objects“; 2017, 20th International Conference on Information Fusion (Fusion)« ist ein Verfahren zum Abschätzen der Form eines dynamischen Objekts und zum gleichzeitigen Nachverfolgen dieses Objekts basierend auf mehrfach räumlich verteilten Messungen und einem Erwartungs-Maximierungs-Algorithmus (EM: Expectation-Maximization) bekannt. Dabei wird das Objekt als - im mathematischen Sinne - „sternförmige Menge“ beschrieben. In der Mathematik versteht man unter einer sternförmigen Menge eine Teilmenge M des ℝⁿ, hier insbesondere ℝ², zu der es einen Punkt x₀ gibt (ein Sternzentrum bzw. einen Sternmittelpunkt), von dem aus alle Punkte der Menge „sichtbar“ sind, das heißt, jede gerade Verbindungsstrecke von x₀ zu einem beliebigen Punkt x ∈ M liegt vollständig in M. Dabei gilt: Jede nichtleere konvexe Menge ist sternförmig. Das Objekt wird bei diesem Ansatz somit als stern-konvexes Objekt (engl.: star convex object) beschrieben. Dies bedeutet, dass die äußere Form bzw. Gestalt des Objekts stern-konvex, also über die Punkte der Menge M, die die äußere Form bestimmen, definiert wird. In diesem Zusammenhang spricht man auch von einer stern-konvexen Form (engl.: star convex shape).
• Gemäß noch einer weiteren bevorzugten Ausgestaltung der Erfindung ist vorgesehen, dass die messinformationsbasierten Objektdaten auf Messung mittels eines Radar-Sensors basieren.
• Bei dem erfindungsgemäßen Computerprogrammprodukt ist vorgesehen, dass dieses Programmteile umfasst, die in einem Prozessor einer computerbasierten Auswerteeinrichtung geladen zur Durchführung des vorstehend genannten Verfahrens eingerichtet sind.
• Bei der erfindungsgemäßen Verwendung des vorstehend genannten Verfahrens zum Nachverfolgen eines Objekts ist vorgesehen, dass es sich um ein Objekt in einem Fahrzeug-Umfeld handelt. In der Regel ist die Verwendung also eine Verwendung für eine Automotive-Anwendung.
• Bei dem erfindungsgemäßen System zum Nachverfolgen eines dynamischen Objekts unter Ausnutzung von messinformationsbasierten Objektdaten, wobei die Messinformationen mindestens eine Doppler-Messinformation umfassen und wobei das System eine computerbasierte Auswerteeinrichtung umfasst, ist vorgesehen, dass die Auswerteeinrichtung eingerichtet ist (i) die auf einer Doppler-Messinformation beruhenden Objektdaten zum Nachverfolgen des Objekts mittels einer den Zusammenhang zwischen der Doppler-Messung und dem Zustand des Objekts angebenden impliziten Messfunktion und (ii) eines zur Verarbeitung impliziter Messfunktionen eingerichteten Kalman-Filters zu verarbeiten.
• Gemäß einer bevorzugten Ausführungsform der Erfindung ist das System eingerichtet, das vorstehend genannte Verfahren durchzuführen. Die im Zusammenhang mit dem Verfahren diskutierten optionalen Merkmale ergeben sich analog auch für das System.
• Gemäß einer weiteren bevorzugten Ausführungsform der Erfindung ist die Auswerteeinrichtung weiterhin eingerichtet zum Abschätzen der Form des Objekts und zu dessen Nachverfolgung weitere messinformationsbasierte Objektdaten mittels eines Erwartungs-Maximierungs-Algorithmus zu verarbeiten.
• Gemäß noch einer weiteren bevorzugten Ausführungsform der Erfindung weist die computerbasierte Auswerteeinrichtung einen Prozessor, einen Datenspeicher und ein Ein- und/oder Ausgabe-Interface auf.
• Nachfolgend wird die Erfindung unter Bezugnahme auf die anliegenden Zeichnungen anhand einer bevorzugten Ausführungsform näher erläutert.
• Es zeigt
• 1 eine Szene aus dem Straßenverkehr mit zwei sich bewegenden Fahrzeugen, von denen eines dieser Fahrzeuge das andere unter Ausnutzung von Doppler-Messinformationen nachverfolgt.
• 2 ein Fahrzeug auf einer Kurvenfahrt entlang einer Kreisbahn zur Visualisierung von Parametern eines entsprechenden Bewegungsmodells und
• 3 ein rechteckiges Objekt und die seinen Zustand beschreibenden Zustandsparameter.
• In 1 ist in Draufsicht eine Szene aus dem Straßenverkehr mit zwei sich bewegenden Fahrzeugen 10, 12 gezeigt, von denen das eine Fahrzeug 10 ein System 14 zum Nachverfolgen von dynamischen Objekten 16 unter Ausnutzung von messinformationsbasierten Objektdaten aufweist, wobei die entsprechenden Messinformationen mindestens eine Doppler-Messinformation umfassen. Das entsprechende dynamische Objekt 16 ist im gezeigten Fall das andere Fahrzeug 12. Das Objektnachverfolgungs-System 14 weist als zentrale Einrichtung eine computerbasierte Auswerteeinrichtung 18 mit den für computerbasierte Systeme üblichen Komponenten Prozessor, Datenspeicher und Ein- und Ausgabe-Interfaces (diese Komponenten sind jedoch nicht gezeigt) auf. Das eine Fahrzeug 10 weist weiterhin als Radar-Sensoren 20 ausgebildete Sensoren 22 auf. Diese Sensoren sind Sensoren zum Überwachen des Fahrzeug-Umfelds 24. Im Beispiel sind es zwei Sensoren 22, die sich im Frontbereich und im Heckbereich des einen Fahrzeugs 10 befinden. Jeder dieser Sensoren 22 überwacht einen entsprechenden Teil des Fahrzeug-Umfelds 24. Die Radar-Sensoren 20 sind Doppler-fähige Radarsensore, auch Doppler-Radar genannt, deren Messdaten Messinformationen inklusive zumindest einer Doppler-Messinformation umfassen. Jedes der Fahrzeuge 10, 12 weist eine Längsachse 26 auf, in der sich das entsprechende Fahrzeug 10, 12 bevorzugt bewegt.
• Die Auswerteeinrichtung 18 kann aus diesen Messdaten Objektdaten zum Nachverfolgen des Objekts 16 erstellen. Zumindest ein Teil dieser Objektdaten beruht auf der Doppler-Messinformation. Um nun diese Doppler-Messinformation nutzen zu können ist die Auswerteeinrichtung 18 dazu eingerichtet, die auf einer Doppler-Messinformation beruhenden Objektdaten zum Nachverfolgen des Objekts 16 mittels einer den Zusammenhang zwischen der Doppler-Messung und dem Zustand des Objekts 16 angebenden impliziten Messfunktion und eines zur Verarbeitung impliziter Messfunktionen eingerichteten Kalman-Filters zu verarbeiten. Dieser zur Verarbeitung impliziter Messfunktionen eingerichtete Kalman-Filter wird im Folgenden auch als „innovativer Kalman-Filter“ bezeichnet, analog zu dem in der Druckschrift DE 10 2016 105 023 A1 benutzten Begriff für die implizite Messfunktion, nämlich die Innovationsfunktion h.
• Der Vollständigkeit halber und um ein besseres Verständnis dieses innovativen Kalman-Filters zu ermöglichen, soll an dieser Stelle eine Herleitung des innovativen Kalman-Filters, der auch implizite Messfunktionen nutzen kann, aus dem Bayes-Theorem erfolgen.
• Da eine zeitliche Aktualisierung durch die Verwendung eines impliziten Messmodells nicht beeinflusst wird, geht es nur um ein Mess-Update. Es sei X die Zufallsvariable des zu schätzenden Zustands und Z die Zufalls-Variable der Messung.
• Dabei sei zu beachten, dass die Messung hier nicht als fester Wert definiert wird, sondern gerade umgekehrt zu dieser üblichen Annahme ist es eine Zufallsvariable, deren Erwartung der gemessene Wert ist, wobei die Varianz die Varianz des Messrauschens ist. Es wird eine Relation zwischen dem Zustand und der Messung als implizite Funktion definiert, die die Innovation gemäß der folgenden Relation ergibt: $$v=h\left(x,z\right)$$

  
• Wenn der Status beispielsweise eine Linie darstellt und die Messung ein Punkt ist, repräsentiert die Innovationsbeziehung die Entfernung vom Punkt zur Linie. Man kann sich den Zustand und die Messung als Orange und Apfel vorstellen, die Beziehung zwischen diesen beiden Objekten wird mit der Innovation definiert. Der zur Verarbeitung impliziter Messfunktionen eingerichtete Kalman-Filter (also der innovativer Kalman-Filter) bietet die Möglichkeit, eine aktualisierte Orange oder einen aktualisierten Apfel zu erhalten: aus diesem Grund werden sowohl Zustände als auch Messungen als Zufallsvariablen definiert, da ihre Rolle beliebig umgeschaltet werden kann.
• Wenn die Messung von dem Objekt (als Target) stammt, muss die Innovation für den wahren Wert des Zustands und den wahren nicht notierten Wert der Messung gleich 0 sein: In unserem vorherigen Beispiel bedeutet dies physikalisch, dass der Sensor 22 tatsächlich einen Punkt auf der Linie gemessen hat. Es soll daher die Wahrscheinlichkeitsdichtefunktion (PDF: probability density function) des Zustands geschätzt werden, die auf den Fall konditioniert ist, dass die nach (1) definierte Zufallsvariablen-Innovation den Wert 0 angenommen hat: $$P\left(x/v=0\right)$$

  
• Wendet man nun Bayes Regel auf (2) an, so erhält man: $$P\left(x/v=0\right)=\frac{P\left(v=0/x\right)P\left(x\right)}{P\left(v=0\right)}\propto P\left(v=0/x\right)P\left(x\right)$$

  
• Sowohl Zustand als auch Messung werden als normalverteilt angenommen: $$\begin{array}{c}P\left(x\right)=N\left\{x;\widehat{x};P_x\right\}\\ P\left(z\right)=N\left\{z;\widehat{z};P_z\right\}\end{array}$$

  
• Weiterhin kann angenommen werden, dass Zustand und Messung unabhängig voneinander sind. Es ist klar, P(v = 0/x) ist keine Wahrscheinlichkeitsdichtefunktion PDF, es ist die Wahrscheinlichkeit. Diese Notation ist tatsächlich weniger mehrdeutig als das übliche P(z/x), wo sowohl die Messung als auch der Zustand ähnliche Notationen haben, und es daher nicht so leicht zu sehen ist, dass dieser Ausdruck eine Wahrscheinlichkeit und keine bedingte Wahrscheinlichkeitsdichtefunktion PDF darstellt. Zunächst soll die bedingte Wahrscheinlichkeitsdichtefunktion PDF P(v/x) gefunden (es ist eine durch x parametrisierte PDF) und danach auf 0 gesetzt werden, um die Wahrscheinlichkeit P(v = 0/x) zu erhalten, die der Wert der Wahrscheinlichkeitsdichte bei 0 als Funktion von x ist.
• Wenn die Innovationsfunktion linearisiert wird, wird auch P(v/x) normal verteilt, wenn eine Zeilentransformation von normalverteilten Zufallsvariablen erfolgt. Es wird daher nach dem Mittelwert und der Kovarianz von P(v/x) gesucht, die P(v/x) vollständig bestimmen. Die Linearisierung ergibt: $$\begin{array}{c}h\left(x,z\right)\approx h\left(\widehat{x},\widehat{z}\right)+H_x\left(x-\widehat{x}\right)+H_z\left(z-\widehat{z}\right)\\ H_x=\frac{\partial h}{\partial x}\left(\widehat{x},\widehat{z}\right)\\ H_z=\frac{\partial h}{\partial z}\left(\widehat{x},\widehat{z}\right)\end{array}$$

  
• Zur Vereinfachung werden Zufallsvariablen eingeführt: $$\begin{array}{c}z\text{'}=H_z\left(z-\widehat{z}\right)\\ y=H_x\left(x-\widehat{x}\right)+z\text{'}\\ v=y+h\left(\widehat{x},\widehat{z}\right)\end{array}$$

  
• Aus (4) und (6) ist es einfach zu überprüfen, dass $$z\text{'}\sim N\left\{z\text{'};0;H_z;P_z;H_z^T\right\}$$

  
• Da x und z unabhängig sind, sind x und z' auch unabhängig. Die Zufallsvariable y als Summe normalverteilter Zufallsvariablen ist ebenfalls normalverteilt. Gesucht wird nun ihre bedingte Wahrscheinlichkeitsdichtefunktion PDF P (y/x), für die die Erwartung und die Kovarianz benötigt wird. Wenn P (y/x) bekannt ist, ist es einfach, P (v/x) zu erhalten. Per Definition der Erwartung gilt: $$\begin{array}{c}E\left[y/x\right]={\displaystyle \int y{f}_Y\left(Y=y/X=x\right)dy}\\ ={\displaystyle \int y\frac{f_{Y,X}\left(Y=y,X=x\right)}{F_X\left(X=x\right)}dy}\end{array}$$

  

  per Definition (6) von y = H_x(x - x̂) + z' ist $$\left\{Y=y,X=x\right\}$$

  

  das gleich wie $$\left\{X=x,Z\text{'}=y-H_x\left(x-\widehat{x}\right)\right\}$$

  
• Daher ist: $$f_{Y,X}\left(Y=y,X=x\right)=f_{X,Z\text{'}}\left(X=x,Z\text{'}=y-H_x\left(x-\widehat{x}\right)\right)$$

  
• Durch die Unabhängigkeit von x und z' ergibt sich $$\begin{array}{c}{f}_{X,Z\text{'}}=\left(X=x,Z\text{'}=y-H_x\left(x-\widehat{x}\right)\right)\\ =f_X\left(X=x\right)f_{Z\text{'}}\left(Z\text{'}=y-H_x\left(x-\widehat{x}\right)\right)\end{array}$$

  
• Durch Einfügen von (11) und (12) in (8) erhält man: $$\begin{array}{c}E\left[y/x\right]={\displaystyle \int y\frac{f_X\left(X=x\right)F_{Z\text{'}}\left(Z\text{'}=y-H_x\left(x-\widehat{x}\right)\right)}{f_X\left(X=x\right)}dy}\\ ={\displaystyle \int y{f}_{Z\text{'}}\left(Z\text{'}=y-H_x\left(x-\widehat{x}\right)\right)dy}\end{array}$$

  
• Mit t = y - H_x(x - x̂) ergibt sich: $$\begin{array}{c}E\left[y/x\right]={\displaystyle \int \left(t+H_x\left(x-\widehat{x}\right)\right)f_{Z\text{'}}\left(Z\text{'}=t\right)dt}\\ ={\displaystyle \int t{f}_{Z\text{'}}\left(Z\text{'}=t\right)dt+H_x\left(x-\widehat{x}\right){\displaystyle \int f_{Z\text{'}}\left(Z\text{'}=t\right)dt}}\\ =E\left[z\text{'}\right]+H_x\left(x-\widehat{x}\right)=H_x\left(x-\widehat{x}\right)\end{array}$$

  
• Man erhält die Kovarianz auch durch Kombination von (14) mit (6): $$\begin{array}{c}E\left[\left(y-\widehat{y}\right){\left(y-\widehat{y}\right)}^T/x\right]=E\left[z\text{'},z{\text{'}}^T/x\right]=E\left[z\text{'},z{\text{'}}^T\right]\\ =H_z{P}_z{H}_z^T\end{array}$$

  

  wenn z' die Erwartung 0 hat und unabhängig von x ist. Es ergibt sich daher: $$P\left(y/x\right)N\left\{y;H_x\left(x-\widehat{x}\right);H_z{P}_z{H}_z^T\right\}$$

  
• Aus der letzten Gleichung von (6) folgt: $$P\left(v/x\right)=N\left\{v;h\left(\widehat{x},\widehat{z}\right)+H_x\left(x-\widehat{x}\right);H_z{P}_z{H}_z^T\right\}$$

  

  da der Ausdruck h(x, y) nur den Mittelwert von (16) verschiebt. Durch Kombinieren von (17) und (4) mit (3) erhält man: $$\begin{array}{c}P\left(x/v=0\right)\propto P\left(v=0/x\right)P\left(x\right)\\ =N\left\{0;h\left(\widehat{x},\widehat{z}\right)+H_x\left(x-\widehat{x}\right);H_z{P}_z{H}_z^T\right\}N\left\{x;\widehat{x};P_x\right\}\\ \begin{array}{c}\propto \text{exp}\left(-\frac{1}{2}\lfloor {\left(-h\left(\widehat{x},\widehat{z}\right)-H_x\left(x-\widehat{x}\right)\right)}^T{\left(H_z{P}_z{H}_z^T\right)}^{-1}\left(-h\left(\widehat{x},\widehat{z}\right)-H_x\left(x-\widehat{x}\right)\right)+{\left(x-\widehat{x}\right)}^T{P}_x^{-1}\left(x-\widehat{x}\right)\rfloor \right)\\ =e^{-1}\end{array}\end{array}$$

  
• Der Ausdruck J unter dem Exponenten in x quadratisch ist, daher ist P(x/v = 0) gaußverteilt: $$P\left(x/v=0\right)=P\left(x_n\right)=N\left\{x_n;\widehat{x_n;}{P}_n\right\}$$

  
• Ein quadratischer Ausdruck wird vollständig durch seine erste und zweite Ableitung bestimmt, bis zu einem Multiplikationsfaktor, der in den Normierungsfaktor verschwindet.
• Der Mittelwert der Gaußverteilung ist, wo die erste Ableitung von J 0 ist, und das Inverse seiner Kovarianz ist die zweite Ableitung von J: $$\begin{array}{c}\frac{\partial J}{\partial x}=\\ -H_x^T{\left(H_z{P}_z{H}_z^T\right)}^{-1}\left(-h\left(\widehat{x},\widehat{z}\right)-H_x\left(x-\widehat{x}\right)\right)+P_x^{-1}\left(x-\widehat{x}\right)\end{array}$$

  

  $$\frac{{\partial }^{2}J}{\partial x^2}=H_x^T{\left(H_z{P}_z{H}_z^T\right)}^{-1}{H}_x+P_x^{-1}=P_n^{-1}$$

  
• Man erhält den Mittelwert der späteren Wahrscheinlichkeit (19) durch Setzen von (20) auf 0: $$\begin{array}{c}\frac{\partial J}{\partial x}=0\\ \iff H_x^T{\left(H_z{P}_z{H}_z^T\right)}^{-1}\left(-h\left(\widehat{x},\widehat{z}\right)-H_x\left(\widehat{x_n}-\widehat{x}\right)\right)\\ =P_x^{-1}\left(\widehat{x_n}-\widehat{x}\right)\\ \iff H_x^T{\left(H_z{P}_z{H}_z^T\right)}^{-1}\left(h-\left(\widehat{x},\widehat{z}\right)\right)\\ =\left(H_x^T{\left(H_z{P}_z{H}_z^T\right)}^{-1}{H}_x+P_x^{-1}\right)\left(\widehat{x_n}-\widehat{x}\right)\\ \iff H_x^T{\left(H_z{P}_z{H}_z^T\right)}^{-1}\left(-h\left(\widehat{x},\widehat{z}\right)\right)=P_x^{-1}\left(\widehat{x_n}-\widehat{x}\right)\\ \iff \widehat{x_n}=\widehat{x}+P_n{H}_x^T{\left(H_z{P}_z{H}_z^T\right)}^{-1}\left(-h\left(\widehat{x},\widehat{z}\right)\right)\end{array}$$

  
• Durch definieren der Kalman-Verstärkung als: $$K=-P_n{H}_x^T{\left(H_z{P}_z{H}_z^T\right)}^{-1}$$

  

  erhält man schließlich: $$\widehat{x_n}=\widehat{x}+Kh\left(\widehat{x},\widehat{z}\right)$$

  
• Die Gleichungen (21), (23) und (24) bilden die Innovative Kalman-Filter-Messungsaktualisierung in der Informationsform, die das Inverse der Kovarianz-Matrix verwendet. Eine kleine algebraische Manipulation ermöglicht die Wiederherstellung der Standardform des Kalman-Filters: $$\begin{array}{c}K=-P_n{H}_x^T{\left(H_z{P}_z{H}_z^T\right)}^{-1}=\\ -P_n{H}_x^T{\left(H_z{P}_z{H}_z^T\right)}^{-1}\left(H_x{P}_x{H}_x^T+H_z{P}_z{H}_z^T\right){\left(H_x{P}_x{H}_x^T+H_z{P}_z{H}_z^T\right)}^{-1}\\ =-P_n\left(H_x^T{\left(H_z{P}_z{H}_z^T\right)}^{-1}{H}_x{P}_x{H}_x^T+H_x^T\right){\left(H_x{P}_x{H}_x^T+H_z{P}_z{H}_z^T\right)}^{-1}\\ =-P_n\left(H_x^T{\left(H_z{P}_z{H}_z^T\right)}^{-1}{H}_x+P_x^{-1}\right)P_x{H}_x^T{\left(H_x{P}_x{H}_x^T+H_z{P}_z{H}_z^T\right)}^{-1}\end{array}$$

  
• Das Einfügen von (21) in (25) führt zur Innovativen Kalman-Verstärkung in der Form: $$K=-P_x{H}_x^T{\left(H_x{P}_x{H}_x^T+H_z{P}_z{H}_z^T\right)}^{-1}$$

  
• Für die Aktualisierung der Kovarianz-Matrix muss man das Matrixinversions-Lemma verwenden: Angewandt auf (21) mit $$A=P_x^{-1},C={\left(H_z{P}_z{H}_z^T\right)}^{-1},U=H_x^T\text{ and }V=H_x$$

  

  ergibt sich: $${\left(A+UCV\right)}^{-1}=A^{-1}-A^{-1}U{\left(C^{-1}+V{A}^{-1}U\right)}^{-1}V{A}^{-1}$$

  

  $$\begin{array}{c}{P}_n={\left(H_x^T{\left(H_z{P}_z{H}_z^T\right)}^{-1}{H}_x+P_x^{-1}\right)}^{-1}\\ =P_x-P_x{H}_x^T{\left(H_z{P}_z{H}_z^T+H_x{P}_x{H}_x^T\right)}^{-1}{H}_x{P}_x\\ =P_x+K{H}_x{P}_x\end{array}$$

  
• Aus (26) und (28) ist wiederum zu erkennen, dass es sich bei dem Zustand und der Messung um beliebig umschaltbare mathematische Objekte handelt, je nachdem, ob eine Schätzung in Form der Messung oder des Zustandes gewünscht ist.
• Bevor die Beziehung zwischen der Doppler-Messung von Radar und der Bewegung eines sich bewegenden Objekts 16 hergeleitet wird, soll zunächst das Bewegungsmodell des Objekts 16 abgeleitet werden.
• Das Bewegungsmodell basiert auf der realen Bewegung eines Fahrzeugs 12 (als Objekt 16), das mit konstanter Geschwindigkeit mit einem konstanten Lenkradwinkel fährt. 2 zeigt ein entsprechendes Fahrzeug 12, wobei die zentrale Position(x, y) des Fahrzeugs 12 auf einem Kreis fährt: Zu jedem Zeitpunkt ist der Radius, der sich aus der zentralen Position des Fahrzeugs 12 ergibt: $$r_k=\frac{v_k}{{\dot{\theta }}_k}$$

  
• Mit Gleichung (29) können wir die Position des Rotationszentrums als Funktion des Fahrzeugzentrums x, y, des Winkels und des Winkels des Geschwindigkeitsvektors zum Fahrzeug 12 beschreiben (wobei zu beachten ist, dass r_k in der vorherigen Abbildung negativ ist): $$\begin{array}{l}{x}_c=x_k-r_k\text{sin}\left({\theta }_k+{\alpha }_k\right)=x_k-\frac{v_k}{{\dot{\theta }}_k}\text{sin}\left({\theta }_k+{\alpha }_k\right)\\ y_c=y_k+r_k\text{cos}\left({\theta }_k+{\alpha }_k\right)=x_k+\frac{v_k}{{\dot{\theta }}_k}\text{cos}\left({\theta }_k+{\alpha }_k\right)\end{array}$$

  
• Da sich der Rotationsmittelpunkt unter der Annahme eines konstanten Lenkradwinkels nicht zwischen zwei Zeitschritten k und k+1 ändert, kann man die Gleichungen (30) für zwei aufeinanderfolgende Zeitschritte aufstellen und das Rotationszentrum eliminieren.
• Die Annahme einer konstante Geschwindigkeit und eines konstanten Lenkradwinkels implizieren eine konstante Wenderate θ̇ wie zum Beispiel $${\theta }_{k+1}={\theta }_k+{\dot{\theta }}_k\Delta t.$$

  
• Resultierend: $$\begin{array}{c}{x}_{k+1}=x_c+\frac{v_{k+1}}{{\dot{\theta }}_{k+1}}\text{sin}\left({\theta }_{k+1}+{\alpha }_{k+1}\right)\\ =x_c+\frac{v_k}{{\dot{\theta }}_k}\text{sin}\left({\theta }_k+{\dot{\theta }}_k\Delta t+{\alpha }_k\right)\\ y_{k+1}=y_c-\frac{v_{k+1}}{{\dot{\theta }}_{k+1}}\text{cos}\left({\theta }_{k+1}+{\alpha }_{k+1}\right)\\ =y_c-\frac{v_k}{{\dot{\theta }}_k}\text{cos}\left({\theta }_k+{\dot{\theta }}_k\Delta t+{\alpha }_k\right)\end{array}$$

  
• Wenn man (30) in (31) einfügt, um das Rotationszentrum zu eliminieren, erhält man: $$\begin{array}{c}{x}_{k+1}=x_k-\frac{v_k}{{\dot{\theta }}_k}\left[\text{sin}\left({\theta }_k+{\alpha }_k\right)-\text{sin}\left({\theta }_k+{\dot{\theta }}_k\Delta t+{\alpha }_k\right)\right]\\ x_{k+1}=y_k+\frac{v_k}{{\dot{\theta }}_k}\left[\text{cos}\left({\theta }_k+{\alpha }_k\right)-\text{cos}\left({\theta }_k+{\dot{\theta }}_k\Delta t+{\alpha }_k\right)\right]\end{array}$$

  
• Es gilt zu beachten, dass (32) eine Division durch die Rate θ̇_k enthält, die problematisch wird, wenn das Fahrzeug geradeaus fährt, da es sich um eine Division durch 0 handelt. Die Lösung dieses Problems besteht darin, Gleichung (32)zu linearisieren, indem man feststellt, dass θ_̇kΔt klein ist bei kleinen Zeitschritten: $$\begin{array}{c}{x}_{k+1}=x_k+\frac{v_k}{{\dot{\theta }}_k}\left(\text{sin}\left({\theta }_k+{\dot{\theta }}_k\Delta t+{\alpha }_k\right)-\text{sin}\left({\theta }_k+{\alpha }_k\right)\right)\\ \approx x_k+\frac{v_k}{{\dot{\theta }}_k}{\dot{\theta }}_k\Delta t\text{ cos}\left({\theta }_k+{\alpha }_k\right)\\ =x_k+v_k\Delta t\text{ cos}\left({\theta }_k+{\alpha }_k\right)\\ y_{k+1}=y_k+\frac{v_k}{{\dot{\theta }}_k}\left(\text{cos}\left({\theta }_k+{\alpha }_k\right)-\text{cos}\left({\theta }_k+{\dot{\theta }}_k\Delta t+{\alpha }_k\right)\right)\\ \approx y_k+\frac{v_k}{{\dot{\theta }}_k}{\dot{\theta }}_k\Delta t\text{ sin}\left({\theta }_k+{\alpha }_k\right)\\ =y_k+v_k\Delta t\text{ sin}\left({\theta }_k+{\alpha }_k\right)\end{array}$$

  
• Die Gleichungen (33) sind die linearisierte Version des Kreisbewegungsmodells und enthalten keine Division durch die Gierrate. Experimente haben gezeigt, dass mit der Aktualisierungsrate, die normalerweise genutzt werden (Δt = 40 ms), der Linearisierungsfehler (selbst bei voll eingeschlagener Lenkung des Fahrzeugs) vernachlässigbar ist und dass sich kein Nutzen durch die Implementierung der nicht linearisierten Variante ergibt.
• Zusätzlich zu den zuvor beschriebenen Variablen zur Beschreibung der Bewegung eines Fahrzeugs (Position des Zentrums x, y, Geschwindigkeit v, Gierrate θ, Fahrzeugwinkel und Winkel des Geschwindigkeitsvektors α) sind Parameter notwendig, um die Form des Fahrzeugs 12 in dem Zustand zu beschreiben, der als halbe Breite a und halbe Länge b der Bounding Box gemäß der 3 und den folgenden Gleichungen definiert ist: $$X={\left[\begin{array}{cccccccc}x& y& \theta & a& b& v& \alpha & \dot{\theta }\end{array}\right]}^T$$

  
• Die Parameter zur Beschreibung der Form des Fahrzeugs 12 sind für die Doppler-Messung nicht notwendig, wie später gezeigt wird, sind aber hier der Vollständigkeit halber enthalten.
• Wir verwenden daher (33), um das Bewegungsmodell zu erstellen, indem wir „verrauschte“ Eingaben hinzufügen, um die Beschleunigung, die Änderung des Lenkradwinkels und die Ungenauigkeit des Modells selbst abzudecken: $$\begin{array}{c}{X}_{k+1}=f\left(X_k,q\right)\\ q=\left[\begin{array}{c}\tilde{a}\\ \dot{\alpha }\\ \ddot{\theta }\\ r\end{array}\right]\\ \widehat{q}=0\\ E\left[q{q}^T\right]=Q=\left[\begin{array}{cccc}\tilde{A}& 0& 0& 0\\ 0& \dot{\text{A}}& 0& 0\\ 0& 0& \ddot{\Theta }& 0\\ 0& 0& 0& R\end{array}\right]\\ \left[\begin{array}{c}{x}_{k+1}\\ y_{k+1}\\ {\theta }_{k+1}\\ a_{k+1}\\ b_{k+1}\\ v_{k+1}\\ {\alpha }_{k+1}\\ {\dot{\theta }}_{k+1}\end{array}\right]=\left[\begin{array}{c}{x}_k+\left(v_k\Delta t+\tilde{a}\frac{\Delta t^2}{2}\right)\text{cos}\left({\theta }_k+{\alpha }_k\right)-r\text{ sin }{\theta }_k\\ y_k+\left(v_k\Delta t+\tilde{a}\frac{\Delta t^2}{2}\right)\text{sin}\left({\theta }_k+{\alpha }_k\right)-r\text{ cos }{\theta }_k\\ {\theta }_k+{\dot{\theta }}_k\Delta t+\ddot{\theta }\frac{\Delta t^2}{2}\\ a_k\\ b_k\\ v_k+\tilde{a}\Delta t\\ {\alpha }_k+\dot{\alpha }\Delta t\\ {\dot{\theta }}_k+\ddot{\theta }\Delta t\end{array}\right]\end{array}$$

  
• Die letzte Komponente r des Rauschvektors ist hier, um etwas zusätzliches Rauschen senkrecht zur Fahrzeugbewegung hinzuzufügen. Ohne diese Komponente wird der größte Teil des Rauschens in Längsrichtung oder in einem Winkel hinzugefügt, und die Vorhersage wird senkrecht zur Richtung der Box überkonfident, was bewirkt, dass der Filter divergiert. Der für das erweiterte (innovativen) Kalman-Filter verwendete Jacobi-Status lautet daher: $$\begin{array}{c}{F}_X=\frac{\partial f}{\partial X_k}|{}_{{}_{{}_{{}_{{}_{{}_{\widehat{X_k},q}}}}}}\\ =\left[\begin{array}{cccccccc}1& 0& \frac{\partial x_{k+1}}{\partial {\theta }_k}|{}_{{}_{{}_{{}_{{}_{{}_{\widehat{X_k},\widehat{q}}}}}}}& 0& 0& \frac{\partial x_{k+1}}{\partial v_k}|{}_{{}_{{}_{{}_{{}_{{}_{\widehat{X_k},\widehat{q}}}}}}}& \frac{\partial x_{k+1}}{\partial {\alpha }_k}|{}_{{}_{{}_{{}_{{}_{{}_{\widehat{X_k},\widehat{q}}}}}}}& 0\\ 0& 1& \frac{\partial y_{k+1}}{\partial {\theta }_k}|{}_{{}_{{}_{{}_{{}_{{}_{\widehat{X_k},\widehat{q}}}}}}}& 0& 0& \frac{\partial y_{k+1}}{\partial v_k}|{}_{{}_{{}_{{}_{{}_{{}_{\widehat{X_k},\widehat{q}}}}}}}& \frac{\partial y_{k+1}}{\partial {\alpha }_k}|{}_{{}_{{}_{{}_{{}_{{}_{\widehat{X_k},\widehat{q}}}}}}}& 0\\ 0& 0& 1& 0& 0& 0& 0& \Delta t\\ 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right]\\ \frac{\partial x_{k+1}}{\partial {\theta }_k}|{}_{{}_{{}_{{}_{{}_{{}_{\widehat{X_k},\widehat{q}}}}}}}=-\widehat{v_k}\Delta t\text{ sin}\left(\widehat{{\theta }_k}+\widehat{{\alpha }_k}\right)\\ \frac{\partial x_{k+1}}{\partial v_k}|{}_{{}_{{}_{{}_{{}_{{}_{\widehat{X_k},\widehat{q}}}}}}}=\Delta t\text{ cos}\left(\widehat{{\theta }_k}+\widehat{{\alpha }_k}\right)\\ \frac{\partial x_{k+1}}{\partial {\alpha }_k}|{}_{{}_{{}_{{}_{{}_{{}_{\widehat{X_k},\widehat{q}}}}}}}=-\widehat{v_k}\Delta t\text{ sin}\left(\widehat{{\theta }_k}+\widehat{{\alpha }_k}\right)\\ \frac{\partial y_{k+1}}{\partial {\theta }_k}|{}_{{}_{{}_{{}_{{}_{{}_{\widehat{X_k},\widehat{q}}}}}}}=\widehat{v_k}\Delta t\text{ cos}\left(\widehat{{\theta }_k}+\widehat{{\alpha }_k}\right)\\ \frac{\partial y_{k+1}}{\partial v_k}|{}_{{}_{{}_{{}_{{}_{{}_{\widehat{X_k},\widehat{q}}}}}}}=\Delta t\text{ sin}\left(\widehat{{\theta }_k}+\widehat{{\alpha }_k}\right)\\ \frac{\partial y_{k+1}}{\partial {\alpha }_k}|{}_{{}_{{}_{{}_{{}_{{}_{\widehat{X_k},\widehat{q}}}}}}}=\widehat{v_k}\Delta t\text{ cos}\left(\widehat{{\theta }_k}+\widehat{{\alpha }_k}\right)\end{array}$$

  
• Die Jacobi-Matrix in Bezug auf die Rauscheingabe ist: $$\begin{array}{c}{F}_q=\frac{\partial f}{\partial q}|{}_{{}_{{}_{{}_{{}_{{}_{\widehat{X_k},\widehat{q}}}}}}}\\ =\left[\begin{array}{cccc}\frac{\Delta t^2}{2}\text{cos}\left(\widehat{{\theta }_k}+\widehat{{\alpha }_k}\right)& 0& 0& -\text{sin}\widehat{{\theta }_k}\\ \frac{\Delta t^2}{2}\text{sin}\left(\widehat{{\theta }_k}+\widehat{{\alpha }_k}\right)& 0& 0& \text{cos}\widehat{{\theta }_k}\\ 0& 0& \frac{\Delta t^2}{2}& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\\ \Delta t& 0& 0& 0\\ 0& \Delta t& 0& 0\\ 0& 0& \Delta t& 0\end{array}\right]\end{array}$$

  
• Mit dieser Linearisierung kann die aktualisierte Erwartung und Kovarianz des Zustands berechnet werden mit: $$\begin{array}{c}\widehat{X_{k+1}}=f\left(\widehat{X_k},\widehat{q}\right)\\ P_{k+1}=F_X{P}_k{F}_X^T+F_{a}Q{F}_a^T\end{array}$$

  
• Nun wird das Innovationsmodell für die Doppler-Messung abgeleitet. Die Eingaben (Inputs) für das Innovationsmodell sind die Zustandsparameter (34); Die Positionsmessung im Welt-Bezugssystem wird mit Doppler erweitert. Der Sensorgeschwindigkeitsvektor über Grund wird auch als bekannt vorausgesetzt: $$Y=\left[\begin{array}{c}{x}_P\\ y_P\\ \rho \end{array}\right]$$

  

  $$V_s=\left[\begin{array}{c}{v}_x\\ v_y\end{array}\right]$$

  

  Nimmt man nun an, dass man die Position der Messung relativ zum Zielkoordinatensystem kennen würden, d. h. dass man weiß, woher die Messung auf dem Objekt 16 stammt, wird diese Position notiert als: $$Y^L=\left[\begin{array}{c}{x}_P^L\\ y_P^L\end{array}\right]$$

  
• Bei einem Koordinatensystemwechsel beträgt die Position des gemessenen Punktes in der Welt: $$\left[\begin{array}{c}{x}_P^W\\ y_P^W\end{array}\right]=\left[\begin{array}{c}x+x_P^L\text{cos}\theta -y_P^L\text{sin}\theta \\ y+x_P^L\text{sin}\theta -y_P^L\text{cos}\theta \end{array}\right]$$

  
• Der Geschwindigkeitsvektor über Grund dieses Punktes ist daher die zeitliche Ableitung von (42): $$\begin{array}{c}{V}_P=\left[\begin{array}{c}{\dot{x}}_P^W\\ {\dot{y}}_P^W\end{array}\right]=\left[\begin{array}{c}\dot{x}-\dot{\theta }{x}_P^L\text{sin}\theta -\dot{\theta }{y}_P^L\text{cos}\theta \\ \dot{y}-\dot{\theta }{x}_P^L\text{cos}\theta -\dot{\theta }{y}_P^L\text{sin}\theta \end{array}\right]\\ =\left[\begin{array}{c}\dot{x}-\dot{\theta }\left(x_P^L\text{sin}\theta +y_P^L\text{cos}\theta \right)\\ \dot{y}+\dot{\theta }\left(x_P^L\text{cos}\theta -y_P^L\text{sin}\theta \right)\end{array}\right]\end{array}$$

  
• Ausgehend von der Messung im vom Sensor 22 gelieferten Bezugssystem können wir die Position relativ zum Targetpunkt des Objekts 16 berechnen: $$\begin{array}{c}\left[\begin{array}{c}{x}_P^L\\ y_P^L\end{array}\right]=\left[\begin{array}{cc}\text{cos}\theta & \text{sin}\theta \\ -\text{sin}\theta & \text{cos}\theta \end{array}\right]\left[\begin{array}{c}{x}_P-x\\ y_P-y\end{array}\right]\\ =\left[\begin{array}{c}\left(x_P-x\right)\text{cos}\theta +\left(y_P-y\right)\text{sin}\theta \\ -\left(x_P-x\right)\text{sin}\theta +\left(y_P-y\right)\text{cos}\theta \end{array}\right]\end{array}$$

  
• Die Schlüsselidee ist nun, dass obwohl die Variablen x, y und θ aus dem Zustand in (44) Funktionen der Zeit sind, die Kombination dieser Variablen gemäß (44) keine Funktion der Zeit mehr ist, wenn der Punkt vom Targetpunkt kommt, weil $x_P^L\text{ und }{y}_P^L$

  

  Konstanten sein müssen. Wir können daher (44) in (43) injizieren und wir bekommen: $$V_P=\left[\begin{array}{c}\dot{x}-\dot{\theta }\left(y_P-y\right)\\ \dot{y}+\dot{\theta }\left(x_P-x\right)\end{array}\right]$$

  
• Sei (x_sy_s) die Sensorposition im Weltframe, so ist der Einheitsvektor vom Sensor 22 zum gemessenen Punkt: $$u=\left[\begin{array}{c}\frac{x_P-x_S}{\sqrt{{\left(x_P-x_S\right)}^2+{\left(y_P-y_S\right)}^2}}\\ \frac{y_P-y_S}{\sqrt{{\left(x_P-x_S\right)}^2+{\left(y_P-y_S\right)}^2}}\end{array}\right]$$

  
• Das vom Sensor 22 gemessene Dopplersignal ist daher: $$\begin{array}{c}\rho =\left(V_P-V_S\right)\cdot u=\frac{\left(\dot{x}-\dot{\theta }\left(y_P-y\right)-v_x\right)\left(x_P-x_S\right)}{\sqrt{{\left(x_P-x_S\right)}^2+{\left(y_P-y_S\right)}^2}}\\ +\frac{\left(\dot{y}+\dot{\theta }\left(x_P-x\right)-v_y\right)\left(y_P-y_S\right)}{\sqrt{{\left(x_P-x_S\right)}^2+{\left(y_P-y_S\right)}^2}}\end{array}$$

  
• Nun müssen x und y durch Werte ersetzen, die sich tatsächlich im Zustand befinden: $$\begin{array}{c}\dot{x}=v\text{ cos}\left(\theta +\alpha \right)\\ \dot{y}=v\text{ sin}\left(\theta +\alpha \right)\end{array}$$

  
• Nach dem Einfügen von (48) in (47) erhalten wir das Innovationsmodell für Doppler: $$\begin{array}{c}h\left(X,Y\right)\\ =\frac{\left(v\text{ cos}\left(\theta +\alpha \right)-\dot{\theta }\left(y_P-y\right)-v_x\right)\left(x_P-x_S\right)}{\sqrt{{\left(x_P-x_S\right)}^2+{\left(y_P-y_S\right)}^2}}\\ +\frac{\left(v\text{ sin}\left(\theta +\alpha \right)+\dot{\theta }\left(x_P-x\right)-v_y\right)\left(y_P-y_S\right)}{\sqrt{{\left(x_P-x_S\right)}^2+{\left(y_P-y_S\right)}^2}}-\rho \end{array}$$

  
• Dieses Doppler-Innovationsmodell (49) ist ein weiteres Beispiel dafür, wie leistungsfähig der Innovative Kalman Filter ist. Wir können die Abhängigkeit zwischen der Dopplermessung und dem Zustand als eine implizite Funktion ausdrücken, ohne zu wissen, wo sich die Messung in Bezug auf das Target befindet. Der innovative Kalman-Filter umgeht das Problem der Assoziationsunsicherheit. Ohne den innovativen Kalman-Filter müsste man $x_P^L\text{ und }{y}_P^L$

  

  a priori kennen und (43) verwenden, um ein Messmodell zu erstellen. Das Jacobi-Matrix in Bezug auf den Zustand ist: $$\begin{array}{c}{H}_X^T=\\ \left[\begin{array}{c}-\widehat{\dot{\theta }}\left(\widehat{y_P}-y_S\right)\\ \widehat{\dot{\theta }}\left(\widehat{x_P}-x_S\right)\\ -\widehat{v}\text{ sin}\left(\widehat{\theta }+\widehat{\alpha }\right)\left(\widehat{x_P}-x_S\right)+\widehat{v}\text{ cos}\left(\widehat{\theta }+\widehat{\alpha }\right)\left(\widehat{y_P}-y_S\right)\\ 0\\ 0\\ \text{cos}\left(\widehat{\theta }+\widehat{\alpha }\right)\left(\widehat{x_P}-x_S\right)+\text{sin}\left(\widehat{\theta }+\widehat{\alpha }\right)\left(\widehat{y_P}-y_S\right)\\ -\widehat{v}\text{ sin}\left(\widehat{\theta }+\widehat{\alpha }\right)\left(\widehat{x_P}-x_S\right)+\widehat{v}\text{ cos}\left(\widehat{\theta }+\widehat{\alpha }\right)\left(\widehat{y_P}-y_S\right)\\ -\left(\widehat{y_P}-\widehat{y}\right)\left(\widehat{x_P}-x_S\right)+\left(\widehat{x_P}-x\right)\left(\widehat{y_P}-y_S\right)\\ \Vert u\Vert =\sqrt{{\left(\widehat{x_P}-x_S\right)}^2+{\left(\widehat{y_P}-y_S\right)}^2}\end{array}\right]\raisebox{1ex}{$$}\!\left/ \!\raisebox{-1ex}{$\Vert u\Vert $}\right.\end{array}$$

  
• Die Jacobi-Matrix in Bezug auf die Messung ist: $$\begin{array}{c}{H}_y^T=\\ \left[\begin{array}{c}\frac{\left(\widehat{v}\text{ cos}\left(\widehat{\theta }+\widehat{\alpha }\right)+\widehat{\dot{\theta }}\left(\widehat{y}-y_S\right)-v_x\right)\left({\Vert u\Vert }^2-\widehat{x_P}\left(\widehat{x_P}-x_S\right)\right)}{{\left({\left(\widehat{x_P}-x_S\right)}^2+{\left(\widehat{y_P}-y_S\right)}^2\right)}^{3/2}}\\ \begin{array}{c}\frac{\left(\widehat{v}\text{ sin}\left(\widehat{\theta }+\widehat{\alpha }\right)+\widehat{\dot{\theta }}\left(x_S-x\right)-v_y\right)\left({\Vert u\Vert }^2-\widehat{y_P}\left(\widehat{y_P}-y_S\right)\right)}{{\left({\left(\widehat{x_P}-x_S\right)}^2+{\left(\widehat{y_P}-y_S\right)}^2\right)}^{3/2}}\\ -1\end{array}\end{array}\right]\end{array}$$

  
• Bezugszeichenliste

• Fahrzeug	10
  Fahrzeug, anderes	12
  System zum Nachverfolgen dynamischer Objekte	14
  Objekt	16
  Auswerteeinrichtung	18
  Radar-Sensor	20
  Sensor	22
  Fahrzeug-Umfeld	24
  Längsachse	26
  Position	x, y
  gemessense Position	xP, yP
  Sensorposition	xS, yS
  Geschwindigkeitsvektor	v
  Winkel Geschwindigkeitsvektor/Längsachse	α
  Gierrate	θ
  Relativgeschwindigkeit	ρ
  halbe Breite	a
  halbe Länge	b

• ZITATE ENTHALTEN IN DER BESCHREIBUNG
• Diese Liste der vom Anmelder aufgeführten Dokumente wurde automatisiert erzeugt und ist ausschließlich zur besseren Information des Lesers aufgenommen. Die Liste ist nicht Bestandteil der deutschen Patent- bzw. Gebrauchsmusteranmeldung. Das DPMA übernimmt keinerlei Haftung für etwaige Fehler oder Auslassungen.
• Zitierte Patentliteratur
• DE 102016105023 A1 [0006, 0024]

Claims (10)

1. Verfahren zum Nachverfolgen eines dynamischen Objekts (16) unter Ausnutzung von messinformationsbasierten Objektdaten, bei dem die Messinformationen mindestens eine Doppler-Messinformation umfassen, wobei die auf einer Doppler-Messinformation beruhenden Objektdaten zum Nachverfolgen des Objekts mittels - einer den Zusammenhang zwischen der Doppler-Messung und dem Zustand des Objekts (16) angebenden impliziten Messfunktion und - eines zur Verarbeitung impliziter Messfunktionen eingerichteten Kalman-Filters verarbeitet werden.
2. Verfahren nach Anspruch 1, dadurch gekennzeichnet, dass sich die Parameter der impliziten Messfunktion aus einem Bewegungsmodell für das Objekt (16) ergeben, bei dem berücksichtigbar ist, dass ein an einer zentralen Position des Objekts (16) ansetzender Geschwindigkeitsvektor v des Objekts (16) einen Winkel α ≠ 0 mit der Längsachse (26) des Objekts (16) bildet.
3. Verfahren nach Anspruch 1 oder 2, dadurch gekennzeichnet, dass die implizite Messfunktion eine Form äquivalent zu der folgenden Form hat: $$\begin{array}{c}h\left(X,Y\right)\\ =\frac{\left(v\text{ cos}\left(\theta +\alpha \right)+\dot{\theta }\left(y_P-y\right)-v_x\right)\left(x_P-x_S\right)}{\sqrt{{\left(x_P-x_S\right)}^2+{\left(y_P-y_S\right)}^2}}\\ +\frac{\left(v\text{ sin}\left(\theta +\alpha \right)+\dot{\theta }\left(x_P-x\right)-v_y\right)\left(y_P-y_S\right)}{\sqrt{{\left(x_P-x_S\right)}^2+{\left(y_P-y_S\right)}^2}}-\rho \end{array}$$

   

   wobei die Parameter wie folgt definiert sind: x, y Ortsparameter der zentralen Position des Objekts (16), v die Geschwindigkeit, θ die Gierrate, α der Winkel zwischen Geschwindigkeitsvektor v und einer Längsachse (26) des Objekts (16), sowie x_P, y_P die gemessene Position, x_S, y_S die Sensorposition und ρ die vom Sensor gemessene Relativgeschwindigkeit des Objekts (16) bezüglich dieses Sensors (22).
4. Verfahren nach einem der Ansprüche 1 bis 3, dadurch gekennzeichnet, dass zum Abschätzen der Form des Objekts (16) und zu dessen Nachverfolgung weitere messinformationsbasierte Objektdaten mittels eines Erwartungs-Maximierungs-Algorithmus verarbeitet werden.
5. Verfahren nach einem der Ansprüche 1 bis 4, dadurch gekennzeichnet, dass die messinformationsbasierten Objektdaten auf Messungen mittels eines als Radar-Sensor (20) ausgebildeten Sensors (22) basieren.
6. Computerprogrammprodukt umfassend Programmteile, die in einem Prozessor einer computerbasierten Auswerteeinrichtung (18) geladen zur Durchführung des Verfahrens nach einem der Ansprüche 1 bis 5 eingerichtet sind.
7. Verwendung des Verfahrens nach einem der Ansprüche 1 bis 5 zum Nachverfolgen eines Objekts (16) in einem Fahrzeug-Umfeld (24).
8. System (14) zum Nachverfolgen eines dynamischen Objekts (16) unter Ausnutzung von messinformationsbasierten Objektdaten, wobei die Messinformationen mindestens eine Doppler-Messinformation umfassen und wobei das System (14) eine computerbasierte Auswerteeinrichtung (18) umfasst, dadurch gekennzeichnet, dass die Auswerteeinrichtung (18) eingerichtet ist die auf einer Doppler-Messinformation beruhenden Objektdaten zum Nachverfolgen des Objekts (16) mittels einer den Zusammenhang zwischen der Doppler-Messung und dem Zustand des Objekts (16) angebenden impliziten Messfunktion und eines zur Verarbeitung impliziter Messfunktionen eingerichteten Kalman-Filters zu verarbeiten.
9. System nach Anspruch 8, dadurch gekennzeichnet, dass die Auswerteeinrichtung (18) eingerichtet ist zum Abschätzen der Form des Objekts (16) und zu dessen Nachverfolgung weitere messinformationsbasierten Objektdaten mittels eines Erwartungs-Maximierungs-Algorithmus zu verarbeiten.
10. System nach Anspruch 8 oder 9, dadurch gekennzeichnet, dass das die computerbasierte Auswerteeinrichtung (18) einem Prozessor, einen Datenspeicher und ein Ein- und/oder Ausgabe-Interface aufweist.

Images