Kalman-Filter mit redundanten Sensoren

Angenommen, ich habe einen Roboter mit zwei 3D-Positionssensoren, die auf unterschiedlichen physikalischen Prinzipien basieren, und ich möchte sie durch einen Kalman-Filter laufen lassen. Ich konstruiere eine Beobachtungsmatrix, zwei repräsentieren meine zwei Sensoren, indem ich zwei Identitätsmatrizen vertikal verkette.

$H = \begin{bmatrix} 1&0&0\\0&1&0\\0&0&1\\1&0&0\\0&1&0\\0&0&1 \end{bmatrix}$ $\hspace{20pt}$ $\overrightarrow x = \begin{bmatrix} x\\y\\z \end{bmatrix}$

damit

$H \overrightarrow x = \begin{bmatrix} x\\y\\z\\x\\y\\z \end{bmatrix}$

Dies stellt beide Sensoren dar, die die genaue Position des Roboters ablesen. Macht bisher Sinn. Das Problem tritt auf, wenn ich die Innovationskovarianz berechne

$S_k = R + HP_{k|k-1}H^T$

Schon seit

$H H^T = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 1 \\ 0 & 1 & 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 & 1 & 0 \\ 0 & 1 & 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 & 1 & 0 \\ 1 & 0 & 0 & 0 & 0 & 1 \\ \end{bmatrix}$

dann, egal was ist, werde ich mit Innovationen vom ersten Sensor enden, die mit Innovationen vom zweiten Sensor korreliert werden , was intuitiv falsch erscheint, wenn ich dieses Recht interpretiere. $P$ $x$ $z$

Wenn ich von hier aus fortfahre, macht meine Verstärkungsmatrix ( ) einige ziemlich seltsame Dinge (Vertauschen von Zeilen und dergleichen), so dass beim Aktualisieren eines statischen Systems ( ) mit einer konstanten Messung Ich zu einem vorhergesagten Zustand . $K = P_{k|k-1} H^T S_k^{-1}$ $A = I_3, B = [0]$ $\overrightarrow z = [1,0,0]$ $\hat x = [0,0,1]$

Wenn ich die Sensoren trenne und den Filter bei jeder Messung separat aktualisiere, ist und ich erhalte vernünftige Ergebnisse. $H H^T = I_3$

Ich glaube, ich bin verwirrt über einige technische Punkte in einem oder mehreren dieser Schritte. Wo gehe ich falsch? Ist es nicht sinnvoll, die Beobachtungsmatrizen vertikal zu verketten?

Ich nehme an, ich könnte einfach die nicht diagonalen 3x3-Blöcke von auf 0 setzen, da ich weiß, dass die Sensoren unabhängig sind, aber gibt es irgendetwas in der Theorie, das diesen Schritt vorschlägt oder beinhaltet? $S_k$

kalman-filter

— evenex_code
quelle

Gute Frage. Ich habe noch nie eine Implementierung gesehen, bei der die Statusvariablen so wiederholt werden wie Sie. Vielmehr segmentiert ein Großteil der Literatur die Sensorfusionslogik als einen Prozess, bevor diese Daten in die EKF eingegeben werden. 1982 verwendete die NASA diesen Ansatz für Entscheidungen zur Sensortoleranz von

— Sensoren

Kommt es nicht auf einen gewichteten Durchschnitt der beiden Messungen an, wobei die Gewichte die Umkehrung der Kovarianz sind? (wie hier beschrieben )

— Rufus

Sind Sie sicher über Ihren Ausdruck für ? $HH^T$

Ich bekomme

H H^{T} = [\begin{matrix} 1 & 0 & 0 & 1 & 0 & 0 \\ 0 & 1 & 0 & 0 & 1 & 0 \\ 0 & 0 & 1 & 0 & 0 & 1 \\ 1 & 0 & 0 & 1 & 0 & 0 \\ 0 & 1 & 0 & 0 & 1 & 0 \\ 0 & 0 & 1 & 0 & 0 & 1 \end{matrix}]

$HH^T = \begin{bmatrix} 1 & 0 & 0 & 1 & 0 & 0 \\ 0 & 1 & 0 & 0 & 1 & 0 \\ 0 & 0 & 1 & 0 & 0 & 1 \\ 1 & 0 & 0 & 1 & 0 & 0 \\ 0 & 1 & 0 & 0 & 1 & 0 \\ 0 & 0 & 1 & 0 & 0 & 1 \end{bmatrix}$ was Ihrer Intuition entspricht.

— DXM
quelle

Huh ... ja, du hast recht. Muss den Ausdruck falsch eingegeben haben. Offensichtlich mehrmals. Wenn ich das nächste Mal etwas so Seltsames sehe, werde ich es von Hand ausarbeiten.

— evenex_code