K̂

Kalman Filter

Advanced
Lancer l’exemple en direct Gratuit · sans inscription · dans votre navigateur

Le 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 (QQ) et de mesure (RR), le bloc estime les états cachés (ex. position et vitesse) tout en suivant l'incertitude statistique (σ=Pii\sigma = \sqrt{P_{ii}}). 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 2×22 \times 2 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 x∈R2\mathbf{x} \in \mathbb{R}^2 à partir de la mesure scalaire z∈Rz \in \mathbb{R} et de l'entrée de commande optionnelle u∈Ru \in \mathbb{R}.

Pour les modèles continus (x˙=Acx+Bcu+w\dot{\mathbf{x}} = \mathbf{A}_c \mathbf{x} + \mathbf{B}_c u + \mathbf{w} avec densité spectrale Qc\mathbf{Q}_c), le système est discrétisé exactement sur la période d'échantillonnage 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

À chaque pas de temps discret kk, le filtre exécute deux étapes séquentielles :

1. Prédiction (Mise à jour temporelle) :

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. Correction (Mise à jour de mesure) :

Mesure prédite z^k=Cx^k−+Duk+offset\hat{z}_k = \mathbf{C}\hat{\mathbf{x}}_k^- + D u_k + \text{offset}, innovation yk=zk−z^ky_k = z_k - \hat{z}_k et variance d'innovation SkS_k :

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

Le gain de Kalman optimal Kk\mathbf{K}_k et l'état estimé x^k\hat{\mathbf{x}}_k s'écrivent :

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

Mise à jour de covariance sous forme stabilisée de Joseph :

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

Surveillance de cohérence via l'innovation normalisée au carré : NISk=yk2/Sk\text{NIS}_k = y_k^2 / S_k.

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 xx et de la vitesse vv à partir d'un radar bruité (R=4.0 m2R = 4.0\text{ m}^2). 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 2σ2\sigma régulières et réduit le bruit de position de plus de 70%70\%.

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 σ\sigma croît progressivement jusqu'au rétablissement du flux.

Remarques & Bonnes Pratiques

  • Grilles matricielles 2×22 \times 2 & vectorielles visuelles : Éditez directement chaque cellule de A,B,C,Q,x0,P0\mathbf{A}, \mathbf{B}, \mathbf{C}, \mathbf{Q}, \mathbf{x}_0, \mathbf{P}_0 dans l'interface graphique sans manipulation fastidieuse de chaînes de caractères.
  • Diagnostic NIS & réglage : L'innovation quadratique normalisée (NIS=y2/S\text{NIS} = y^2 / S) doit présenter une moyenne temporelle proche de 1.01.0. Une valeur nettement supérieure traduit une confiance excessive (QQ ou RR 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 P\mathbf{P} reste définie semi-positive et prévenant les dérives numériques.
  • Point de fonctionnement affine : Le paramètre et port offset permettent de suivre un point de linéarisation non nul sans altérer la matrice d'état A\mathbf{A}.
  • Architecture de calcul découplée : Le filtre s'exécute une fois par période TsT_s, 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?

RR is directly obtained from sensor characterization (variance of stationary sensor noise σ2\sigma^2). QQ is a design tuning parameter representing unmodeled dynamics and input disturbances. If Q≫RQ \gg R, the filter places high trust in sensor measurements (fast dynamic tracking, less noise filtering). If R≫QR \gg Q, 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.