BlockWerk Documentatie
ENNLDEFRESZH
Start Canvas
K̂

Kalman Filter

Advanced

Het KalmanFilter-blok schat de verborgen toestand van een lineair systeem op basis van een ruizige meting. Bij elke sample voorspelt het blok waar de toestand zich zou moeten bevinden volgens het model en de bekende ingang, en corrigeert het die voorspelling vervolgens met de meting. Daarbij weegt het de twee af op basis van betrouwbaarheid: de procesruis Q tegenover de meetruis R.

Beschrijf het systeem zoals op papier — continu-tijd dx/dt = A·x + B·u, met Q als een ruisdichtheid per seconde — en het blok discretiseert dit exact voor de gekozen bemonsteringstijd, inclusief ruis. Het verwerkt twee toestanden en één meting, wat dekking biedt voor positie/snelheids-tracking, eerste-orde processen met onbekende bias en gelineariseerde batterijmodellen. Naast de toestandsschatting levert het blok de eigen onzekerheid (σ), de Kalman-versterkingen en de genormaliseerde innovatiekwadraten (NIS), het getal dat aantoont of Q en R correct zijn getuned. Het rekent één keer per sample ongeacht welke solver het diagram aanstuurt, blijft voorspellen wanneer metingen uitvallen, en kan direct in een terugkoppelingslus (regelkring) worden geplaatst.

Ingangen. z is de meting en u de bekende ingang. Reset herstart het filter vanaf de initiële schatting, terwijl een Valid-ingang onder 0.5 het filter laat voorspellen zonder meetcorrectie. Beide zijn optioneel — een niet-aangesloten Valid betekent dat de meting betrouwbaar is.

Offset is de constante term van het meetmodel. De meetvergelijking luidt z = C·x + offset. Een gelineariseerde curve rond een werkpunt is affien: het lineariseren van V(SOC) geeft V(s) + H·(SOC − s), oftewel een richtingscoëfficiënt H plus een constante V(s) − H·s. Sluit de Offset-ingang aan om deze direct dynamisch vanuit het diagram te sturen via een LookupTable en Gain.

Simuleren in BlockWerk

Interactieve sandbox direct in je browser — geen installatie, aangedreven door WebAssembly.

Simuleren in BlockWerk →

Wiskundig Model

Continu model:x˙=Acx+Bcu+w,E[ww⊤]=Qc δ(t−s)A=eAcTs,B=∫0TseAcs ds Bc,Q=∫0TseAcsQc eAc⊤s dsPredictie:x^−=Ax^+Buk−1,P−=APA⊤+QCorrectie:S=CP−C⊤+R,K=P−C⊤/Sx^=x^−+K (z−z^),z^=Cx^−+Du+offset,P=(I−KC)P−(I−KC)⊤+KRK⊤σi=Pii,NIS=(z−z^)2/S\text{Continu model:}\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{Predictie:}\quad \hat{x}^- = A\hat{x} + B u_{k-1},\qquad P^- = A P A^\top + Q \text{Correctie:}\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

Waarbij:

  • x̂ de tweedimensionale toestandsschatting is en P de covariantiematrix.
  • z de meting is en u de bekende ingang (nulde-orde-hold over elke sample).
  • Een continu model exact wordt gediscretiseerd via de matrixexponentiaal en Van Loan's methode voor Q.
  • K de Kalman-versterking is en S de innovatievariantie (scalair bij één meting).
  • De eerste sample de predictiestap overslaat en de initiële toestand direct corrigeert; een sample met Valid < 0.5 slaat juist de correctiestap over.

Ingangen & Uitgangen

Direction ID Label Type 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

Parameters

Parameters Label Type Standaard Beschrijving
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

Gebruiksvoorbeelden

Volgen van een Ruisige Sinusoïde

Een langzame sinusgolf met Gaussische ruis (σ = 0.15) stuurt een continu constante-snelheid Kalman-filter aan met een bemonsteringsfrequentie van 50 Hz. De toestandsschatting volgt de sinus met slechts een kwart van de meetfout (RMS 0.040 ten opzichte van 0.150).

Snelheidsinschatting zonder Tachometer

Meting van een positietransducer met kwantisatieruis wordt gefilterd om zowel een zuivere positie als de niet rechtstreeks gemeten snelheid te reconstrueren zonder ruisversterking door differentiatie.

Opmerkingen & Best Practices

  • σ uitgangen: x̂₀ ± 2σ₀ vormt een 95%-betrouwbaarheidsinterval wanneer het model accuraat is. Controleer dit via de NIS-uitgang: het tijdgemiddelde is 1 voor een consistent filter.
  • Valid ingang: Koppel hier de status van een sensor aan. Onder 0.5 voert het blok enkel voorspellingen uit en groeien de covarianties totdat er weer geldige metingen binnenkomen.
  • Terugkoppeling: De schatting kan direct een regelaar aansturen waarvan de stuuractie terugkeert als u. Doordat de predictie u_{k-1} gebruikt, ontstaat er bij d = 0 geen algebraïsche lus.
  • Numerieke robuustheid: Slecht geformatteerde matrices vallen automatisch terug op het standaardmodel. Een gedegenereerde innovatievariantie (S ≤ 0) slaat de correctiestap veilig over zonder NaN te genereren.

Gerelateerde Componenten