Observateurs d’état et filtres de Kalman

Les observateurs d’état combinent des informations sur le comportement d’un système et des mesures externes pour estimer l’état du système. Un observateur commun utilisé pour les systèmes linéaires est le filtre de Kalman. Les filtres de Kalman sont avantageux par rapport aux autres filtres car ils fusionnent les mesures d’un ou plusieurs capteurs avec un modèle d’espace d’état du système pour estimer de manière optimale l’état d’un système.

Cette image montre les mesures de vitesse d’un volant d’inertie au fil du temps, exécutées à travers une variété de filtres différents. Notez qu’un filtre de Kalman bien réglé ne montre aucun décalage de mesure pendant la rotation du volant tout en rejetant les données bruyantes et en réagissant rapidement aux perturbations lorsque les billes le traversent. Vous trouverez plus d’informations sur les filtres dans la :ref:` section filtres <docs/software/advanced-controls/filters/index:Filters>`.

Comparaison de filtres entre: Kalman, Median et RII.

Les fonctions gaussiennes

Les filtres de Kalman utilisent des distributions gaussiennes (ou courbes en cloche) pour modéliser le bruit dans un processus. Le graphique d’une fonction gaussienne est une forme de « courbe en cloche ». Cette fonction est décrite par sa moyenne (l’emplacement du «pic» de la courbe en cloche) et sa variance (une mesure de «l’étalement» de la courbe en cloche). Dans le cas d’un filtre de Kalman, l’estimation de l’état du système est la moyenne, tandis que la variance est une mesure du degré de certitude (ou d’incertitude) du filtre par rapport à l’état réel.

../../../../_images/normal-distribution.png

L’idée de variance et de covariance est au cœur de la fonction d’un filtre de Kalman. La covariance est une mesure de la manière dont deux variables aléatoires sont corrélées. Dans un système à un seul état, la matrice de covariance est simplement \(\mathbf{\text{cov}(x_1, x_1)}\), ou une matrice contenant la variance \(\mathbf{\text{var}(x_1)}\) de l’état \(x_1\). L’amplitude de cette variance est le carré de l’écart type de la fonction gaussienne décrivant l’estimation de l’état actuel. Des valeurs relativement élevées de covariance peuvent indiquer des données bruyantes, tandis que de petites covariances peuvent indiquer que le filtre est plus sûr de son estimation. Rappelez-vous que les valeurs « grandes » et « petites » de la variance ou de la covariance sont relatives à l’unité de base utilisée - par exemple, si \(\mathbf{x_1}\) a été mesurée en mètres, \(\mathbf{\text{cov}(x_1, x_1)}\) serait en mètres carrés.

Les matrices de covariance sont écrites sous la forme suivante:

\[\begin{split}\mathbf{\Sigma} &= \begin{bmatrix} \text{cov}(x_1, x_1) & \text{cov}(x_1, x_2) & \ldots & \text{cov}(x_1, x_n) \\ \text{cov}(x_2, x_1) & \text{cov}(x_2, x_2) & \ldots & \text{cov}(x_1, x_n) \\ \vdots & \vdots & \ddots & \vdots \\ \text{cov}(x_n, x_1) & \text{cov}(x_n, x_2) & \ldots & \text{cov}(x_n, x_n) \\ \end{bmatrix}\end{split}\]

Les filtres de Kalman

Important

Il est important de développer une intuition de ce que fait réellement un filtre de Kalman. Le livre Kalman and Bayesian Filters in Python par Roger Labbe fournit une excellente introduction visuelle et intéractive aux filtres bayésiens. Les filtres de Kalman de la WPILib utilisent l’algèbre linéaire pour rendre plus accessible les mathématiques, mais les idées sont similaires au cas unidimensionnel. Nous vous suggérons de lire le chapitre 4 pour avoir une idée de ce que font ces filtres.

Pour résumer, les filtres de Kalman (et tous les filtres bayésiens) comportent deux parties: la prédiction et la correction. La prédiction projette notre estimation d’état vers l’avant dans le temps en fonction de la dynamique de notre système, et correcte oriente l’état estimé vers l’état mesuré. Bien que les filtres préforment souvent les deux dans le même pas de temps, ce n’est pas strictement nécessaire - Par exemple, les estimateurs de pose de WPILib appellent souvent prédire et ne corrigent que lorsque de nouvelles données de mesure sont disponibles (par exemple, à partir d’un système de vision à faible fréquence d’images).

Ce qui suit montre les équations d’un filtre de Kalman à temps discret:

\[\begin{split}\text{Predict step} \nonumber \\ \hat{\mathbf{x}}_{k+1}^- &= \mathbf{A}\hat{\mathbf{x}}_k + \mathbf{B} \mathbf{u}_k \\ \mathbf{P}_{k+1}^- &= \mathbf{A} \mathbf{P}_k^- \mathbf{A}^T + \mathbf{\Gamma}\mathbf{Q}\mathbf{\Gamma}^T \\ \text{Update step} \nonumber \\ \mathbf{K}_{k+1} &= \mathbf{P}_{k+1}^- \mathbf{C}^T (\mathbf{C}\mathbf{P}_{k+1}^- \mathbf{C}^T + \mathbf{R})^{-1} \\ \hat{\mathbf{x}}_{k+1}^+ &= \hat{\mathbf{x}}_{k+1}^- + \mathbf{K}_{k+1}(\mathbf{y}_{k+1} - \mathbf{C} \hat{\mathbf{x}}_{k+1}^- - \mathbf{D}\mathbf{u}_{k+1}) \\ \mathbf{P}_{k+1}^+ &= (\mathbf{I} - \mathbf{K}_{k+1}\mathbf{C})\mathbf{P}_{k+1}^-\end{split}\]
\[\begin{split}\begin{array}{llll} \mathbf{A} & \text{system matrix} & \hat{\mathbf{x}} & \text{state estimate vector} \\ \mathbf{B} & \text{input matrix} & \mathbf{u} & \text{input vector} \\ \mathbf{C} & \text{output matrix} & \mathbf{y} & \text{output vector} \\ \mathbf{D} & \text{feedthrough matrix} & \mathbf{\Gamma} & \text{process noise intensity vector} \\ \mathbf{P} & \text{error covariance matrix} & \mathbf{Q} & \text{process noise covariance matrix} \\ \mathbf{K} & \text{Kalman gain matrix} & \mathbf{R} & \text{measurement noise covariance matrix} \end{array}\end{split}\]

L’estimation d’état \(\mathbf{x}\), avec \(\mathbf{P}\), décrivent la moyenne et la covariance de la fonction gaussienne qui décrit l’estimation par notre filtre de l’état réel du système.

Les matrices de covariance du bruit de processus et de mesure

Les matrices de covariance du bruit de processus et de mesure \(\mathbf{Q}\) et \(\mathbf{R}\) décrivent la variance de chacun de nos états et mesures. N’oubliez pas que pour une fonction gaussienne, la variance est le carré de l’écart type de la fonction. Dans un WPILib, Q et R sont des matrices diagonales dont les diagonales contiennent leurs variances respectives. Par exemple, un filtre de Kalman avec les états \(\begin{bmatrix}\text{position} \\ \text{velocity} \end{bmatrix}\) et les mesures \(\begin{bmatrix}\text{position} \end{bmatrix}\) avec les écarts-types d’état \(\begin{bmatrix}0.1 \\ 1.0\end{bmatrix}\)begin{bmatrix}0.01end{bmatrix}` aurait les matrices suivantes \(\mathbf{Q}\) et \(\mathbf{R}\) :

\[\begin{split}Q = \begin{bmatrix}0.01 & 0 \\ 0 & 1.0\end{bmatrix}, R = \begin{bmatrix}0.0001\end{bmatrix}\end{split}\]

Matrice de covariance d’erreur

La matrice de covariance d’erreur \(\mathbf{P}\) décrit la covariance de l’estimation d’état \(\mathbf{\hat{x}}\). De manière informelle, \(\mathbf{P}\) décrit notre certitude quant à l’état estimé. Si \(\mathbf{P}\) est grand, notre incertitude sur l’état vrai est grande. Inversement, un \(\mathbf{P}\) avec des éléments plus petits impliquerait moins d’incertitude sur notre véritable état.

Au fur et à mesure que nous projetons le modèle vers l’avant, \(\mathbf{P}\) augmente à mesure que notre certitude sur l’état réel du système diminue.

L’étape de prédiction

En prédiction, notre estimation d’état est mise à jour selon la dynamique du système linéaire \(\mathbf{\dot{x} = Ax + Bu}\). De plus, notre covariance d’erreur \(\mathbf{P}\) augmente selon la matrice de covariance du bruit de processus \(\mathbf{Q}\). Des valeurs plus grandes de:math:mathbf{Q} feront croître notre covariance d’erreur \(\mathbf{P}\) plus rapidement. Ceci \(\mathbf{P}\) est utilisé dans l’étape de correction pour pondérer le modèle et les mesures.

L’étape correcte

À l’étape correcte, notre estimation d’état est mise à jour pour inclure de nouvelles informations de mesure. Cette nouvelle information est pondérée par rapport à l’estimation d’état \(\mathbf{\hat{x}}\) par le gain de Kalman \(\mathbf{K}\). Les grandes valeurs de \(\mathbf{K}\) influencent plus fortement les mesures entrantes, tandis que les valeurs plus petites de \(\mathbf{K}\) influencent plus fortement notre prédiction d’état. Parce que \(\mathbf{K}\) est lié à \(\mathbf{P}\), des valeurs plus grandes de \(\mathbf{P}\) augmenteront \(\mathbf{K}\) et influencer encore plus les mesures. Si, par exemple, un filtre est utilisé pour une longue période, une valeur élevée de \(\mathbf{P}\) pondérerait fortement les nouvelles informations.

Enfin, la covariance d’erreur \(\mathbf{P}\) diminue pour augmenter notre confiance dans l’estimation de l’état.

Le réglage des filtres Kalman

Les constructeurs des classes de filtre Kalman de WPILib utilisent un système linéaire, un vecteur des écarts-types de bruit de processus et des écarts-types de bruit de mesure. Ceux-ci sont convertis en matrices \(\mathbf{Q}\) et \(\mathbf{R}\) en remplissant les diagonales avec le carré des écarts-types, ou variances, de chaque état ou mesure. En diminuant l’écart type d’un état (et donc son entrée correspondante dans \(\mathbf{Q}\)), le filtre se fera moins confiance aux mesures entrantes. De même, l’augmentation de l’écart type d’un état fera davantage confiance aux mesures entrantes. Il en va de même pour les écarts-types de mesure - la diminution d’une entrée augmentra la confiance du filtre par rapport à la mesure entrante pour l’état correspondant, tandis que son augmentation diminuera la confiance dans la mesure.

49  // The observer fuses our encoder data and voltage inputs to reject noise.
50  private final KalmanFilter<N1, N1, N1> m_observer =
51      new KalmanFilter<>(
52          Nat.N1(),
53          Nat.N1(),
54          m_flywheelPlant,
55          VecBuilder.fill(3.0), // How accurate we think our model is
56          VecBuilder.fill(0.01), // How accurate we think our encoder
57          // data is
58          0.020);