K̂

Kalman Filter

Advanced
Live-Beispiel starten Kostenlos · ohne Anmeldung · läuft im Browser

Der KalmanFilter-Block implementiert ein lineares zeitdiskretes Kalman-Filter zur optimalen Zustandsschätzung zweidimensionaler linearer dynamischer Systeme mit einer skalaren Messgröße. Durch die Kombination modellbasierter Vorhersagen mit verrauschten Sensorbeobachtungen, gewichtet über die jeweiligen Prozess- (QQ) und Messrauschkovarianzen (RR), schätzt der Block verborgene Zustände (z. B. Position und Geschwindigkeit) und überwacht die Schätzunsicherheit (σ=Pii\sigma = \sqrt{P_{ii}}). Im Eigenschaftseditor werden alle Matrizen und Vektoren (a, b, c, q, xInit, pInit) über visuelle 2×22 \times 2-Matrix- und 2-Zustands-Vektorgitter bearbeitet, strukturiert in drei Paramatergruppen (_Timing & Modelltyp_, _Zustandsraummodell_ und _Rauschen & Anfangskovarianz_).

Video-Tutorial

Mathematisches Modell

Das Filter schätzt den Zustand x∈R2\mathbf{x} \in \mathbb{R}^2 aus der skalaren Messung z∈Rz \in \mathbb{R} und dem optionalen Steuereingang u∈Ru \in \mathbb{R}.

Bei kontinuierlichen Modellen (x˙=Acx+Bcu+w\dot{\mathbf{x}} = \mathbf{A}_c \mathbf{x} + \mathbf{B}_c u + \mathbf{w} mit Rauschleistungsdichte Qc\mathbf{Q}_c) wird das System über die Abtastzeit TsT_s exakt diskretisiert:

A=eAcTs,B=∫0TseAcτdτ Bc,Q=∫0TseAcτQceAcTτdτ\mathbf{A} = e^{\mathbf{A}_c T_s}, \quad \mathbf{B} = \int_0^{T_s} e^{\mathbf{A}_c \tau} d\tau \, \mathbf{B}_c, \quad \mathbf{Q} = \int_0^{T_s} e^{\mathbf{A}_c \tau} \mathbf{Q}_c e^{\mathbf{A}_c^T \tau} d\tau

Zu jedem diskreten Zeitschritt kk führt das Filter zwei Phasen aus:

1. Zeit-Update (Prädiktion):

x^k−=Ax^k−1+Buk−1Pk−=APk−1AT+Q\begin{aligned} \hat{\mathbf{x}}_k^- &= \mathbf{A}\hat{\mathbf{x}}_{k-1} + \mathbf{B}u_{k-1} \\ \mathbf{P}_k^- &= \mathbf{A}\mathbf{P}_{k-1}\mathbf{A}^T + \mathbf{Q} \end{aligned}

2. Mess-Update (Korrektur):

Erwartete Messung z^k=Cx^k−+Duk+offset\hat{z}_k = \mathbf{C}\hat{\mathbf{x}}_k^- + D u_k + \text{offset}, Innovation yk=zk−z^ky_k = z_k - \hat{z}_k und Innovationsvarianz SkS_k:

Sk=CPk−CT+RS_k = \mathbf{C}\mathbf{P}_k^-\mathbf{C}^T + R

Die optimale Kalman-Verstärkung Kk\mathbf{K}_k und der geschätzte Zustand x^k\hat{\mathbf{x}}_k lauten:

Kk=Pk−CTSk−1,x^k=x^k−+Kkyk\mathbf{K}_k = \mathbf{P}_k^-\mathbf{C}^T S_k^{-1}, \quad \hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + \mathbf{K}_k y_k

Kovarianzaktualisierung über die Joseph-stabilisierte Form:

Pk=(I−KkC)Pk−(I−KkC)T+KkRKkT\mathbf{P}_k = (\mathbf{I} - \mathbf{K}_k\mathbf{C})\mathbf{P}_k^-(\mathbf{I} - \mathbf{K}_k\mathbf{C})^T + \mathbf{K}_k R \mathbf{K}_k^T

Filterkonsistenzprüfung über die normalisierte quadrierte Innovation: NISk=yk2/Sk\text{NIS}_k = y_k^2 / S_k.

Ein- & Ausgänge

Direction ID Label Typ Status
→ In in1 z number Required
→ In in2 u number Optional
→ In in3 Reset number Optional
→ In in4 Dropout number Optional
→ In in5 Offset number Optional
← Out x_hat_0 x̂₀ number Output
← Out x_hat_1 x̂₁ number Output
← Out sigma_0 σ₀ number Output
← Out sigma_1 σ₁ number Output
← Out k_gain_0 K₀ number Output
← Out k_gain_1 K₁ number Output
← Out z_hat ẑ number Output
← Out innovation Innov. number Output
← Out innovation_cov S number Output
← Out nis NIS number Output

Parameter

Parameter Label Typ Standardwert Beschreibung
sampleTime Sample Time number 1 Filter period in seconds
model Model select continuous Continuous: dx/dt = A·x + B·u, discretised exactly for the sample time. Discrete: x[k+1] = A·x[k] + B·u[k], already for this sample time
a A (2x2) vector 0 1; 0 0 State matrix, row by row
b B (2x1) vector 0 1 Input matrix
c C (1x2) vector 1 0 Measurement row
d D number 0 Direct feedthrough from u to the measurement
offset Measurement Offset number 0 Constant term in z = C·x + D·u + offset, e.g. the intercept of a linearised curve
q Q vector 0 0.01 Process noise: two numbers for the diagonal, four for the full matrix. Per second (a density) for a continuous model, per sample for a discrete one
r R number 1 Measurement noise variance
xInit Initial Estimate vector 0 0
pInit Initial Covariance vector 1 1 Two numbers for the diagonal, four for the full matrix

Anwendungsbeispiele

Beispiel 1: Positions- und Geschwindigkeitsverfolgung bei verrauschtem Radar

Schätzung von Fahrzeugposition xx und Geschwindigkeit vv aus verrauschten Radardaten (R=4.0 m2R = 4.0\text{ m}^2). Mit a = "0 1; 0 0", b = "0; 1", c = "1 0" und Prozessrauschen q = "0.01 0.1". Das Filter rekonstruiert die Geschwindigkeit exakt mit glatten 2σ2\sigma-Unsicherheitsbändern und reduziert das Positionsrauschen um über 70%70\%.

Beispiel 2: Sensorausfalltolerante Gyroskop-Winkelschätzung

Schätzung des Neigungswinkels bei temporärem Ausfall der Sensortelemetrie. Der Winkelsensor speist z, ein Statusbit speist Dropout. Bei Verbindungsabbruch (Dropout > 0.5) überspringt das Filter die Messkorrektur und prädiziert rein modellbasiert; die Unsicherheit σ\sigma wächst stetig an, bis die Messung wieder verfügbar ist.

Hinweise & Best Practices

  • Visuelle 2×22 \times 2-Matrix- & Vektorgitter: Anstelle fehleranfälliger Zeichenketten bearbeiten Sie jede Zelle von A,B,C,Q,x0,P0\mathbf{A}, \mathbf{B}, \mathbf{C}, \mathbf{Q}, \mathbf{x}_0, \mathbf{P}_0 direkt im interaktiven Gittereditor.
  • NIS-Diagnose & Tuning: Die normalisierte quadrierte Innovation (NIS=y2/S\text{NIS} = y^2 / S) sollte im Mittel bei 1.01.0 liegen. Werte deutlich über 1.01.0 weisen auf überhöhtes Vertrauen hin (QQ oder RR zu klein); Werte unter 1.01.0 auf übervorsichtige Schätzungen.
  • Joseph-stabilisierte Kovarianzform: Das Messupdate berechnet die Kovarianz über die symmetrische Joseph-Gleichung, was strikte positive Semidefinitheit von P\mathbf{P} sichert und numerische Divergenz verhindert.
  • Affiner Linearisierungs-Offset: Über den Parameter und Port offset lassen sich Arbeitspunkte ungleich Null abbilden, ohne Matrix A\mathbf{A} zu verändern.
  • Entkoppelte Solver-Architektur: Das diskrete Kalman-Filter schaltet exakt einmal pro TsT_s, sodass kontinuierliche Teilsysteme ohne Schrittweiten-Interferenzen von modernen ODE-Solvern (RK4, TR-BDF2) integriert werden können.

Häufig gestellte Fragen (FAQ)

Q: Wie stimmt man Prozessrauschen Q und Messrauschen R beim Kalman-Filter ab?

RR wird direkt aus dem Sensordatenblatt bestimmt (Varianz σ2\sigma^2 des stationären Rauschens). QQ repräsentiert unmodellierte Prozessstörungen. Ein hohes Q/RQ/R-Verhältnis führt zu schnellerer Nachführung bei geringerer Rauschunterdrückung; ein kleines Q/RQ/R liefert stark geglättete Zustandsschätzungen.

Verwandte Blöcke

  • StateSpace: Allgemeines kontinuierliches Mehrgrößen-Zustandsraummodell.
  • RandomNumber: Gaußsche Rauschquelle zur Simulation realistischer Mess- und Prozessstörungen.
  • NumericDisplay: Digitale LCD-Messwertanzeige in Echtzeit.
  • uPlotDisplay: Mehrkanalige Signal-Scope-Anzeige für Zustandsschätzungen und Konfidenzbänder.