Beibehaltung einer positiv definierten Eigenschaft für die Kovarianz in einem nicht parfümierten Kalman-Filter-Update

Ich habe einen nicht parfümierten Kalman-Filter (UKF), der den Zustand eines Roboters verfolgt. Der Zustandsvektor hat 12 Variablen. Jedes Mal, wenn ich einen Vorhersageschritt durchführe, wirkt meine Übertragungsfunktion (natürlich) auf den gesamten Zustand. Meine Sensoren liefern jedoch Messungen verschiedener Teile des Roboterzustands, sodass ich bei einer Messung möglicherweise Roll-, Nick-, Gier- und ihre jeweiligen Geschwindigkeiten und bei einer anderen lineare Geschwindigkeit erhalten kann.

Mein bisheriger Ansatz bestand darin, einfach Untermatrizen für die Kovarianz zu erstellen, meine Standard-UKF-Aktualisierungsgleichungen auszuführen und die resultierenden Werte dann wieder in die vollständige Kovarianzmatrix einzufügen. Nach einigen Aktualisierungen schreit mich die UKF jedoch an, weil ich versucht habe, eine Matrix, die nicht eindeutig positiv ist, in eine Cholesky-Zerlegungsfunktion zu übergeben. Offensichtlich verliert die Kovarianz ihre positiv-definierten Eigenschaften, und ich vermute, dass dies mit meinen Versuchen zu tun hat, Teilmengen der vollständigen Kovarianzmatrix zu aktualisieren.

Als Beispiel aus einer tatsächlichen Protokolldatei ist die folgende Matrix (nach dem UKF-Vorhersageschritt) positiv definit:

   1.1969            0            0            0            0            0      0.11567            0            0            0            0            0
        0       1.9682            0            0            0            0            0      0.98395            0            0            0            0
        0            0       1.9682            0            0            0            0            0      0.98395            0            0            0
        0            0            0       1.9682            0            0            0            0            0      0.98395            0            0
        0            0            0            0       1.9682            0            0            0            0            0      0.98395            0
        0            0            0            0            0       1.9682            0            0            0            0            0      0.98395
  0.11567            0            0            0            0            0      0.01468            0            0            0            0            0
        0      0.98395            0            0            0            0            0            1            0            0            0            0
        0            0      0.98395            0            0            0            0            0            1            0            0            0
        0            0            0      0.98395            0            0            0            0            0            1            0            0
        0            0            0            0      0.98395            0            0            0            0            0            1            0
        0            0            0            0            0      0.98395            0            0            0            0            0            1

Nach dem Verarbeiten der Korrektur für eine Variable (in diesem Fall lineare X-Geschwindigkeit) wird die Matrix jedoch zu:

   1.1969            0            0            0            0            0      0.11567            0            0            0            0            0
        0       1.9682            0            0            0            0            0      0.98395            0            0            0            0
        0            0       1.9682            0            0            0            0            0      0.98395            0            0            0
        0            0            0       1.9682            0            0            0            0            0      0.98395            0            0
        0            0            0            0       1.9682            0            0            0            0            0      0.98395            0
        0            0            0            0            0       1.9682            0            0            0            0            0      0.98395
  0.11567            0            0            0            0            0         0.01            0            0            0            0            0
        0      0.98395            0            0            0            0            0            1            0            0            0            0
        0            0      0.98395            0            0            0            0            0            1            0            0            0
        0            0            0      0.98395            0            0            0            0            0            1            0            0
        0            0            0            0      0.98395            0            0            0            0            0            1            0
        0            0            0            0            0      0.98395            0            0            0            0            0            1

Der Unterschied zwischen den beiden obigen Matrizen ist

        0            0            0            0            0            0            0            0            0            0            0            0
        0            0            0            0            0            0            0            0            0            0            0            0
        0            0            0            0            0            0            0            0            0            0            0            0
        0            0            0            0            0            0            0            0            0            0            0            0
        0            0            0            0            0            0            0            0            0            0            0            0
        0            0            0            0            0            0            0            0            0            0            0            0
        0            0            0            0            0            0     -0.00468            0            0            0            0            0
        0            0            0            0            0            0            0            0            0            0            0            0
        0            0            0            0            0            0            0            0            0            0            0            0
        0            0            0            0            0            0            0            0            0            0            0            0
        0            0            0            0            0            0            0            0            0            0            0            0
        0            0            0            0            0            0            0            0            0            0            0            0

Wie Sie sehen können, ist der einzige Unterschied zwischen den beiden der Wert an der Stelle der Varianz der linearen X-Geschwindigkeit, die die Messung ist, die ich gerade verarbeitet habe. Dieser Unterschied reicht aus, um meine Kovarianzmatrix zu "brechen".

Ich habe zwei Fragen:

1. Das Aktualisieren einer Teilmenge des Filters scheint nicht der richtige Weg zu sein. Gibt es eine bessere Lösung?

2. Fehlt mir alternativ ein Schritt, der meine Kovarianzmatrix als positiv-definitiv hält?

Vielen Dank!

BEARBEITEN:

Es sieht so aus, als würde ich die Werte nicht richtig in die ursprüngliche Kovarianzmatrix zurücksetzen. Das einfache Zurückkopieren der Werte reicht nicht aus. Ich muss die Korrelationskoeffizienten für die Kovarianzmatrix verfolgen und sicherstellen, dass beim Aktualisieren eines Varianzwerts alle Werte in seiner Zeile / Spalte aktualisiert werden, um den Korrelationskoeffizientenwert beizubehalten. Ich muss noch einige Tests durchführen, um zu überprüfen, ob dies mein Problem ist, aber einige erste Analysen in Matlab legen nahe, dass dies der Fall ist. Wenn ich richtig liege, beantworte ich meine eigene Frage.

EDIT 2:

Angesichts der Antwort unten und nachdem ich es ausprobiert habe, kann ich sehen, dass meine ursprüngliche Bearbeitungsidee nicht funktioniert. Ich habe jedoch noch eine Frage:

Da dies eine UKF ist, habe ich eigentlich keine Jacobi-Matrizen. Ich denke, ich sehe, wie ich es innerhalb der UKF-Aktualisierungsgleichungen zum Laufen bringen würde, aber selbst in einer EKF - und ich frage, weil ich auch eine davon habe - meine State-to-Measurement-Funktion$h$wird am Ende die Identitätsmatrix sein, da ich meine Zustandsvariablen direkt messe. In dem Fall nehme ich an, mein "Jacobianer" wäre nur ein$m \times n$ Matrix mit denen in der $(i, i)$ Ort, wo $i$ ist der Index der gemessenen Werte im Messvektor?

Ein paar Anmerkungen zuerst,

Erstens können Sie, wie Sie bereits erwähnt haben, nicht einfach eine Submatrix herausziehen und ein Update durchführen . Sie können jedoch einen Propagierungsschritt für eine Submatrix ausführen.

Dies liegt an den kovarianzübergreifenden Begriffen (die Informationen über verschiedene Teile des Staates "verbreiten"). Aus diesem Grund führt eine genauere Schätzung Ihres Kurses beispielsweise zu genaueren Positionsschätzungen.

Wenn Sie jedoch nach der Bearbeitung die Kreuzkorrelationsterme (Kovarianz) einfach nicht aktualisieren, müssen Sie auch die gesamte Matrix aktualisieren (es sei denn, Sie wissen sicher, dass einige Elemente unabhängig sind, abhängig von der Zustandsschätzung.

Bitte schön:

Bilden Sie dazu die Jacobi-Matrizen wie zuvor, beachten Sie jedoch, dass in allen nicht diagonalen Elementen Nullen vorhanden sein sollten, wenn kein Teil dieses Zustands gemessen wird. Dann wird die Magie der Matrixinversion die Innovationskorrekturen auf die richtigen Teile des Staates übertragen. Die Jacobi-Matrix muss groß sein$n\times m$ zum $m$ Messwerte und $n$ Zustandsvariablen (oder $m\times n$abhängig von Ihrer Definition von Jacobian). Nichts von diesem "Update-Teil des Kovarianz-Mülls", es sei denn, Sie wissen sicher, dass die jakobianischen Elemente der Identität entsprechen. Am sichersten ist es, den vollen Jacobian zu verwenden.

Andere Hacks (sobald alles theoretisch korrekt ist)

Dies stellt jedoch immer noch keine PD-Kovarianzmatrizen sicher. Ich empfehle dringend , die folgenden Hacks erst durchzuführen, wenn Sie alle anderen Fehler behoben haben. Letztendlich habe ich jedoch festgestellt, dass für ein vor Ort einsetzbares System, das nicht trivial lange arbeitet, fast immer Folgendes erforderlich ist:

1. Lassen Sie nach allen Updates Kovarianz $P$ Sein $P\gets \frac{1}{2}P + \frac{1}{2}P^{T}$, nur um die nicht diagonalen Terme "auszugleichen" - für Symmetrie

2. Lassen $P\gets P + \epsilon I_{n\times n}$, wo $\epsilon$ist ein kleiner Skalar, um sicherzustellen, dass Sie nicht unterlaufen (und die Bedingungsnummer Ihrer Matrix zerstören).