K̂

Kalman Filter

Advanced
Start het live voorbeeld Gratis · geen account · draait in je browser

Het KalmanFilter-blok implementeert een lineair discreet-tijd Kalman-filter voor optimale toestandsschatting in tweedimensionale lineaire dynamische systemen met één scalaire meting. Door modelgebaseerde fysische voorspellingen te combineren met ruizige sensormetingen, gewogen op basis van procesruis- (QQ) en meetruiscovariantiematrices (RR), schat het blok verborgen toestanden (zoals positie en snelheid) terwijl het gelijktijdig de schattingsonzekerheid monitort (σ=Pii\sigma = \sqrt{P_{ii}}). In de Eigenschappen-editor worden alle matrices en toestandsvectoren (a, b, c, q, xInit, pInit) bewerkt via visuele 2×22 \times 2 matrix- en 2-toestandsvectorroosters, overzichtelijk gegroepeerd in drie secties (_Timing & modeltype_, _Toestandsruimtemodel_, en _Ruis & initiële covariantie_).

Video-uitleg

Wiskundig Model

Het filter schat toestand x∈R2\mathbf{x} \in \mathbb{R}^2 uit een scalaire meting z∈Rz \in \mathbb{R} en een optionele stuuringang u∈Ru \in \mathbb{R}.

Voor continue modellen (x˙=Acx+Bcu+w\dot{\mathbf{x}} = \mathbf{A}_c \mathbf{x} + \mathbf{B}_c u + \mathbf{w} met ruisspectraaldichtheid Qc\mathbf{Q}_c) wordt het systeem exact gediscretiseerd over de bemonsteringstijd TsT_s:

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

Bij elke discrete tijdstap kk doorloopt het filter twee opeenvolgende fasen:

1. Tijd-update (Predictie):

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. Meet-update (Correctie):

Voorspelde meting z^k=Cx^k−+Duk+offset\hat{z}_k = \mathbf{C}\hat{\mathbf{x}}_k^- + D u_k + \text{offset}, innovatie yk=zk−z^ky_k = z_k - \hat{z}_k, en innovatievariantie SkS_k:

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

De optimale Kalman-versterking Kk\mathbf{K}_k en bijgewerkte toestandsschatting x^k\hat{\mathbf{x}}_k luiden:

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

Covariantie-actualisatie via de Joseph-gestabiliseerde vorm:

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

Consistentiebewaking via de Normalized Innovation Squared: NISk=yk2/Sk\text{NIS}_k = y_k^2 / S_k.

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

Voorbeeld 1: Snelheids- en positietracking bij ruizige radarmetingen

Tracking van voertuigpositie xx en snelheid vv uit radarmetingen met aanzienlijke ruis (R=4.0 m2R = 4.0\text{ m}^2). Met continue matrices a = "0 1; 0 0", b = "0; 1", c = "1 0" en procesruis q = "0.01 0.1". Het filter schat de snelheid nauwkeurig met soepele 2σ2\sigma-onzekerheidsgrenzen en reduceert de positieruis met meer dan 70%70\%.

Voorbeeld 2: Gyroscoophoekschatting bestand tegen sensoruitval

Schatten van de oriëntatiehoek tijdens tijdelijk telemetrieverlies. Verbind een hoeksensor met poort z en een statusvlag met poort Dropout. Wanneer de verbinding wegvalt (Dropout > 0.5), slaat het filter de meetupdate over en vertrouwt het op het procesmodel; de onzekerheid σ\sigma groeit realistisch totdat data weer beschikbaar is.

Opmerkingen & Best Practices

  • Visuele 2×22 \times 2 matrix- & vectorroosters: In plaats van handmatig puntkomma's in te voeren, bewerk je elke cel van A,B,C,Q,x0,P0\mathbf{A}, \mathbf{B}, \mathbf{C}, \mathbf{Q}, \mathbf{x}_0, \mathbf{P}_0 direct in het interactieve tabelrooster.
  • NIS-diagnostiek: De genormaliseerde innovatiekwadraten (NIS=y2/S\text{NIS} = y^2 / S) dienen gemiddeld rond 1.01.0 te liggen. Waarden aanzienlijk boven 1.01.0 duiden op overmatig zelfvertrouwen (QQ of RR te klein); waarden onder 1.01.0 op overmatige terughoudendheid.
  • Joseph-gestabiliseerde covariantievorm: De meetupdate berekent covariantie via de symmetrische Joseph-formule, wat numerieke positief-semidefinietheid van P\mathbf{P} waarborgt en afrondingsfouten voorkomt.
  • Affiene linearisatie-offset: De parameter en ingang offset maken tracking rond niet-nul werkpunten mogelijk zonder matrix A\mathbf{A} te vervuilen.
  • Ontkoppelde solvers: Het discrete Kalman-filter stapt eenmaal per sampletijd TsT_s, waardoor continue dynamische processen storingsvrij door hoogwaardige ODE-solvers (RK4, TR-BDF2) kunnen worden geïntegreerd.

Veelgestelde Vragen van Ingenieurs (FAQ)

Q: Hoe stel je de procesruis Q en meetruis R in bij een Kalman Filter?

RR wordt direct bepaald uit de sensorkarakteristiek (de variantie σ2\sigma^2 van stationaire meetruis). QQ is een ontwerpparameter die onge-modelleerde dynamica en procesverstoringen vertegenwoordigt. Als Q≫RQ \gg R, vertrouwt het filter sterk op de meting (snelle respons, meer ruis). Als R≫QR \gg Q, vertrouwt het filter op het model (zeer gladde schatting, maar vertraagde reactie op plotse bewegingen).

Gerelateerde Componenten

  • StateSpace: Algemeen continu meervoudig toestandruimtemodel.
  • RandomNumber: Voegt Gaussiaanse witte ruis toe om sensor- en procesruis realistisch te testen.
  • NumericDisplay: Real-time digitaal numeriek afleesdisplay.
  • uPlotDisplay: Meerkanaals golfvormscope voor visualisatie van toestandsschattingen en onzekerheidsbanden.