Kalman Filter
AdvancedHet 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- () en meetruiscovariantiematrices (), schat het blok verborgen toestanden (zoals positie en snelheid) terwijl het gelijktijdig de schattingsonzekerheid monitort (). In de Eigenschappen-editor worden alle matrices en toestandsvectoren (a, b, c, q, xInit, pInit) bewerkt via visuele 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 uit een scalaire meting en een optionele stuuringang .
Voor continue modellen ( met ruisspectraaldichtheid ) wordt het systeem exact gediscretiseerd over de bemonsteringstijd :
Bij elke discrete tijdstap doorloopt het filter twee opeenvolgende fasen:
1. Tijd-update (Predictie):
2. Meet-update (Correctie):
Voorspelde meting , innovatie , en innovatievariantie :
De optimale Kalman-versterking en bijgewerkte toestandsschatting luiden:
Covariantie-actualisatie via de Joseph-gestabiliseerde vorm:
Consistentiebewaking via de Normalized Innovation Squared: .
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 en snelheid uit radarmetingen met aanzienlijke ruis (). 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 -onzekerheidsgrenzen en reduceert de positieruis met meer dan .
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 groeit realistisch totdat data weer beschikbaar is.
Opmerkingen & Best Practices
- Visuele matrix- & vectorroosters: In plaats van handmatig puntkomma's in te voeren, bewerk je elke cel van direct in het interactieve tabelrooster.
- NIS-diagnostiek: De genormaliseerde innovatiekwadraten () dienen gemiddeld rond te liggen. Waarden aanzienlijk boven duiden op overmatig zelfvertrouwen ( of te klein); waarden onder op overmatige terughoudendheid.
- Joseph-gestabiliseerde covariantievorm: De meetupdate berekent covariantie via de symmetrische Joseph-formule, wat numerieke positief-semidefinietheid van waarborgt en afrondingsfouten voorkomt.
- Affiene linearisatie-offset: De parameter en ingang
offsetmaken tracking rond niet-nul werkpunten mogelijk zonder matrix te vervuilen. - Ontkoppelde solvers: Het discrete Kalman-filter stapt eenmaal per sampletijd , 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?
wordt direct bepaald uit de sensorkarakteristiek (de variantie van stationaire meetruis). is een ontwerpparameter die onge-modelleerde dynamica en procesverstoringen vertegenwoordigt. Als , vertrouwt het filter sterk op de meting (snelle respons, meer ruis). Als , 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.