Kalman Filter
AdvancedLe bloc KalmanFilter implémente un filtre de Kalman linéaire en temps discret pour l'estimation optimale d'état des systèmes dynamiques linéaires à deux états avec une mesure scalaire unique. En combinant les prédictions du modèle physique avec les observations capteurs bruitées, pondérées par leurs covariances de bruit de processus () et de mesure (), le bloc estime les états cachés (ex. position et vitesse) tout en suivant l'incertitude statistique (). Dans l'Éditeur de Propriétés, toutes les matrices et vecteurs d'état (a, b, c, q, xInit, pInit) s'éditent via des grilles matricielles visuelles et vectorielles à 2 états, regroupées en trois sections (_Timing & type de modèle_, _Modèle d'espace d'état_, et _Bruit & covariance initiale_).
Tutoriel vidéo
Modèle Mathématique
Le filtre estime l'état à partir de la mesure scalaire et de l'entrée de commande optionnelle .
Pour les modèles continus ( avec densité spectrale ), le système est discrétisé exactement sur la période d'échantillonnage :
À chaque pas de temps discret , le filtre exécute deux étapes séquentielles :
1. Prédiction (Mise à jour temporelle) :
2. Correction (Mise à jour de mesure) :
Mesure prédite , innovation et variance d'innovation :
Le gain de Kalman optimal et l'état estimé s'écrivent :
Mise à jour de covariance sous forme stabilisée de Joseph :
Surveillance de cohérence via l'innovation normalisée au carré : .
Entrées & Sorties
| 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 |
Paramètres
| Paramètres | Label | Type | Défaut | Description |
|---|---|---|---|---|
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 |
Exemples d'Utilisation
Exemple 1 : Poursuite de position et vitesse sur signal radar bruité
Estimation conjointe de la position et de la vitesse à partir d'un radar bruité (). Avec a = "0 1; 0 0", b = "0; 1", c = "1 0" et bruit de processus q = "0.01 0.1". Le filtre estime la vitesse avec des bandes d'incertitude régulières et réduit le bruit de position de plus de .
Exemple 2 : Estimation d'attitude gyroscopique tolérante aux pertes de trames
Estimation de l'orientation lors de coupures temporaires de télémétrie. En reliant un capteur d'angle sur z et un indicateur de liaison sur Dropout. Lors d'une déconnexion (Dropout > 0.5), le filtre ignore l'étape de correction et continue à prédire sur son modèle ; l'incertitude croît progressivement jusqu'au rétablissement du flux.
Remarques & Bonnes Pratiques
- Grilles matricielles & vectorielles visuelles : Éditez directement chaque cellule de dans l'interface graphique sans manipulation fastidieuse de chaînes de caractères.
- Diagnostic NIS & réglage : L'innovation quadratique normalisée () doit présenter une moyenne temporelle proche de . Une valeur nettement supérieure traduit une confiance excessive ( ou trop faibles) ; une valeur inférieure traduit un sous-dimensionnement.
- Forme stabilisée de Joseph : La mise à jour de covariance s'appuie sur la formulation symétrique de Joseph, garantissant que reste définie semi-positive et prévenant les dérives numériques.
- Point de fonctionnement affine : Le paramètre et port
offsetpermettent de suivre un point de linéarisation non nul sans altérer la matrice d'état . - Architecture de calcul découplée : Le filtre s'exécute une fois par période , permettant l'intégration transparente des parties continues par des solveurs ODE performants (RK4, TR-BDF2).
Questions Fréquemment Posées (FAQ)
Q: How to tune process noise covariance Q vs measurement noise covariance R?
is directly obtained from sensor characterization (variance of stationary sensor noise ). is a design tuning parameter representing unmodeled dynamics and input disturbances. If , the filter places high trust in sensor measurements (fast dynamic tracking, less noise filtering). If , the filter places high trust in the system model (smooth estimates, but slower response to unexpected physical maneuvers).
Composants Associés
- StateSpace : Modèle d'état continu multivariable général.
- RandomNumber : Source de bruit blanc gaussien pour simuler des signaux réalistes.
- NumericDisplay : Afficheur numérique en direct.
- uPlotDisplay : Oscilloscope multivoie pour tracer les états estimés et les fuseaux d'incertitude.