K̂

Kalman Filter

Advanced

Der KalmanFilter-Block schätzt den verborgenen Zustand eines linearen Systems aus verrauschten Messungen. Bei jedem Zeitschritt prognostiziert er den Zustand anhand des Systemmodells sowie des bekannten Stellsignals und korrigiert diese Vorhersage anschließend mit dem Messwert. Dabei werden Modell und Messung gewichtet nach ihrer Zuverlässigkeit: Prozessrauschen Q gegenüber Messrauschen R.

Beschreiben Sie das System im kontinuierlichen Zeitbereich (dx/dt = A·x + B·u) mit Q als spektrale Rauschleistungsdichte, und der Block diskretisiert es exakt für die Abtastzeit inklusive Rauschanteil. Er verarbeitet zwei Zustände und eine Messung (geeignet für Positions-/Geschwindigkeitstracking, PT1-Strecken mit unbekanntem Bias oder linearisierte Batteriemodelle). Neben dem Zustandsschätzwert gibt er die Unsicherheit (σ), die Kalman-Verstärkung und das quadratische normalisierte Innovationsmaß (NIS) aus, das anzeigt, ob Q und R korrekt abgestimmt sind. Er rechnet zeitschrittsynchron, prognostiziert bei Sensorausfällen stabil weiter und lässt sich direkt in Regelkreise einbinden.

Eingänge. z ist das Messsignal und u das bekannte Stellsignal. Reset setzt das Filter auf den Anfangszustand zurück. Ein Valid-Signal unter 0,5 lässt das Filter reine Prädiktionen ohne Messkorrektur ausführen.

Offset repräsentiert den konstanten Offset der Messgleichung z = C·x + offset. Bei Linearisierung einer Kennlinie wie V(SOC) um einen Arbeitspunkt entsteht eine affine Tangente. Durch Anschluss des Offset-Eingangs kann dieser dynamisch von einer Kennlinientabelle vorgegeben werden.

Mathematisches Modell

\text{Kontinuierliches Modell:}\quad \dot{x} = A_c x + B_c u + w,\quad E[w w^\top] = Q_c\,\delta(t-s) A = e^{A_c T_s},\quad B = \int_0^{T_s} e^{A_c s}\,ds\,B_c,\quad Q = \int_0^{T_s} e^{A_c s} Q_c\, e^{A_c^\top s}\,ds \text{Prädiktion:}\quad \hat{x}^- = A\hat{x} + B u_{k-1},\qquad P^- = A P A^\top + Q \text{Korrektur:}\quad S = C P^- C^\top + R,\qquad K = P^- C^\top / S \hat{x} = \hat{x}^- + K\,(z - \hat{z}),\quad \hat{z} = C\hat{x}^- + D u + \text{offset},\quad P = (I - KC)P^-(I - KC)^\top + K R K^\top \sigma_i = \sqrt{P_{ii}},\qquad \text{NIS} = (z - \hat{z})^2 / S

Wobei:

x̂ die 2-Zustands-Schätzung und P die Kovarianzmatrix darstellt.
z der Messwert und u das Stellsignal ist (Nullte-Ordnung-Halteglied über jeden Abtastschritt).
Die kontinuierliche Diskretisierung exakt über die Matrixexponentialfunktion und die Van-Loan-Methode für Q erfolgt.
K die Kalman-Verstärkung und S die skalare Innovationsvarianz ist.
Der erste Zeitschritt die Prädiktion überspringt und den Anfangszustand direkt korrigiert; ein Zeitschritt mit Valid < 0,5 überspringt hingegen die Korrektur.

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

Schätzung eines verrauschten Sinussignals

Eine Sinusschwingung mit überlagertem Gaußschen Rauschen (σ = 0,15) wird von einem kontinuierlichen Kalman-Filter mit 50 Hz Abtastung gefiltert. Der Zustandsschätzwert folgt dem Sinus mit nur einem Viertel des Messfehlers (RMS 0,040 gegenüber 0,150).

Geschwindigkeitsbeobachtung ohne Tachogenerator

Filterung eines quantisierten Positionssignals zur zeitgleichen Rekonstruktion von Position und Geschwindigkeit ohne differenzierungsbedingte Rauschverstärkung.

Hinweise & Best Practices

σ-Ausgänge: x̂₀ ± 2σ₀ bildet ein 95%-Konfidenzintervall bei zutreffendem Systemmodell. Überprüfen Sie dies über den NIS-Ausgang (zeitlicher Mittelwert nahe 1).
Valid-Eingang: Signalisiert Sensorverfügbarkeit. Bei Werten unter 0,5 schaltet das Filter auf reine Prädiktion; die Kovarianz wächst bis zur Rückkehr valider Messwerte.
Rückführung: Die Zustandsschätzung kann direkt zur Regelung genutzt werden. Da die Prädiktion u_{k-1} nutzt, entsteht bei d = 0 keine algebraische Schleife.
Stabilität: Numerisch ungültige Eingaben fallen fehlertolerant auf das Standardmodell zurück. Eine semidefinite Innovationsvarianz überspringt das Update sicher ohne NaN-Fehler.