BlockWerk Documentation
ENNLDEFRESZH
Lancer l’application
K̂

Kalman Filter

Advanced

Le bloc KalmanFilter estime l'état caché d'un système linéaire à partir d'une mesure bruitée. À chaque pas d'échantillonnage, il prédit l'évolution de l'état selon le modèle et la commande connue, puis corrige cette prédiction par la mesure acquise, en pondérant les deux sources selon leur fiabilité respective : le bruit de processus Q contre le bruit de mesure R.

Décrivez le système sous forme continue usuelle (dx/dt = A·x + B·u) avec Q en densité spectrale de puissance par seconde, et le bloc le discrétise de façon exacte pour la période d'échantillonnage choisie, bruit inclus. Il prend en charge deux états et une mesure, ce qui couvre la poursuite position/vitesse, les systèmes du premier ordre avec dérive inconnue et les modèles de batterie linéarisés. Outre l'estimation de l'état, il fournit son incertitude (σ), les gains de Kalman et le carré de l'innovation normalisée (NIS), indicateur statistique permettant de vérifier le réglage optimal de Q et R. Il s'exécute à chaque échantillon, poursuit la prédiction en cas de perte de signal capteur et s'intègre naturellement dans les boucles fermées de régulation.

Entrées. z est la mesure et u la commande connue. Reset réinitialise le filtre à son état initial, tandis qu'un signal Valid inférieur à 0,5 fait prédire sans correction de mesure.

Offset représente le terme constant du modèle de mesure z = C·x + offset. La linéarisation d'une courbe non linéaire autour d'un point de fonctionnement étant affine, connecter l'entrée Offset permet de piloter dynamiquement ce décalage depuis le schéma de simulation.

Simuler dans BlockWerk

Bac à sable interactif directement dans votre navigateur — aucune installation, moteur WebAssembly.

Simuler dans BlockWerk →

Modèle Mathématique

Modeˋle continu :x˙=Acx+Bcu+w,E[ww⊤]=Qc δ(t−s)A=eAcTs,B=∫0TseAcs ds Bc,Q=∫0TseAcsQc eAc⊤s dsPreˊdiction :x^−=Ax^+Buk−1,P−=APA⊤+QMise aˋ jour :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{Modèle continu :}\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édiction :}\quad \hat{x}^- = A\hat{x} + B u_{k-1},\qquad P^- = A P A^\top + Q \text{Mise à jour :}\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

Où :

  • x̂ est l'estimation des deux états et P la matrice de covariance.
  • z est la mesure et u l'entrée connue (bloqueur d'ordre zéro sur la période d'échantillonnage).
  • Le modèle continu est discrétisé de manière exacte par exponentielle de matrice et la méthode de Van Loan pour Q.
  • K est le gain de Kalman et S la variance scalaire de l'innovation.
  • Le premier échantillon omet la prédiction et corrige directement l'état initial ; un échantillon avec Valid < 0,5 omet la mise à jour.

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

Filtrage d'une sinusoïde bruitée

Une onde sinusoïdale lente bruitée (σ = 0,15) est traitée par un filtre de Kalman continu échantillonné à 50 Hz. L'estimation réduit l'erreur quadratique moyenne de 0,150 à 0,040.

Estimation de vitesse sans tachymètre

Reconstitution conjointe de la position et de la vitesse à partir d'un encodeur de position quantifié, sans amplification de bruit par dérivation.

Remarques & Bonnes Pratiques

  • Sorties σ : x̂₀ ± 2σ₀ définit un intervalle de confiance à 95 % lorsque le modèle est exact. Vérifiez-le avec le signal NIS (moyenne temporelle proche de 1).
  • Entrée Valid : À relier au statut du capteur. En dessous de 0,5, le bloc prédit sans corriger et la variance augmente jusqu'au retour de données valides.
  • Régulation en boucle fermée : L'état estimé peut attaquer directement un régulateur ; comme la prédiction utilise la commande précédente, il n'y a pas de boucle algébrique pour d = 0.
  • Robustesse numérique : Les matrices mal formées basculent automatiquement sur le modèle par défaut pour éviter tout plantage.

Composants Associés