Observadores de estado y filtros de Kalman

Los observadores de estados combinan información sobre el comportamiento de un sistema y mediciones externas para estimar el verdadero estado del sistema. Un observador común utilizado para sistemas lineales es el filtro de Kalman. Los filtros de Kalman son ventajosos sobre otros filtros ya que fusionan las mediciones de uno o más sensores con un modelo de espacio de estado del sistema para estimar de manera óptima el estado de un sistema.

Esta imagen muestra las mediciones de la velocidad del volante a lo largo del tiempo, ejecutadas a través de una variedad de filtros diferentes. Tenga en cuenta que un filtro de Kalman bien ajustado no muestra ningún retraso de medición durante el giro del volante, mientras que sigue rechazando datos ruidosos y reacciona rápidamente a las perturbaciones cuando las bolas pasan a través de él. Puede encontrar más información sobre filtros en sección de filtros .

Filter comparison between: Kalman, Median, and IIR.

Funciones gaussianas

Los filtros Kalman utilizan Distribuciones gaussianas (o curvas de campana) para imitar el ruido en un proceso. La gráfica de una función gaussiana tiene una figura de «curva de campana». Esta función es descrita por su nombre (la ubicación del «pico» de la curva de campana) y varianza (una medida de que tan «extendida» esta la curva). En el caso de un filtro Kalman, el estimado estado de sistema es el medio, mientras la varianza es la medida de que tan cierto (o incierto) el filtro es sobre su verdadero estado.

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

La idea de varianza y covarianza es fundamental para la función de un filtro de Kalman. La covarianza es una medida de cómo se correlacionan dos variables aleatorias. En un sistema con un solo estado, la matriz de covarianza es simplemente \(\ mathbf {\ text {cov} (x_1, x_1)}\), o una matriz que contiene la varianza:math:mathbf {text {var } (x_1)} `del estado: matemáticas: x_1`. La magnitud de esta varianza es el cuadrado de la desviación estándar de la función gaussiana que describe la estimación del estado actual. Los valores relativamente grandes de covarianza pueden indicar datos ruidosos, mientras que las pequeñas covarianzas pueden indicar que el filtro tiene más confianza en su estimación. Recuerde que los valores «grandes» y «pequeños» para la varianza o covarianza son relativos a la unidad base que se está utilizando, por ejemplo, si \(\ mathbf{x_1}\) se midió en metros, \(\mathbf{\text{cov}(x_1, x_1)}\) estaría en metros al cuadrado.

Las matrices de covarianza se escriben de la siguiente forma:

\[\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}\]

Filtros de Kalman

Importante

Es importante desarrollar una intuición de lo que realmente hace un filtro de Kalman. El libro Kalman and Bayesian Filters in Python de Roger Labbe <https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python> __ proporciona una excelente introducción visual e interactiva a los filtros bayesianos. Los filtros de Kalman en WPILib usan álgebra lineal para gentrificar las matemáticas, pero las ideas son similares al caso unidimensional. Sugerimos leer el Capítulo 4 para hacerse una idea de lo que hacen estos filtros.

En resumen, los filtros de Kalman (y todos los filtros bayesianos) tienen dos partes: predicción y corrección. La predicción proyecta nuestra estimación de estado hacia adelante en el tiempo de acuerdo con la dinámica de nuestro sistema, y la corrección dirige el estado estimado hacia el estado medido. Si bien los filtros a menudo se preforman ambos en el mismo paso de tiempo, no es estrictamente necesario; por ejemplo, los estimadores de pose de WPILib llaman a predecir con frecuencia y corrigen solo cuando hay nuevos datos de medición disponibles (por ejemplo, de un sistema de visión de baja frecuencia de cuadros).

A continuación se muestran las ecuaciones de un filtro de Kalman de tiempo discreto:

\[\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}\]

La estimación del estado \(\mathbf {x}\), junto con \(\mathbf{P}\), describe la media y la covarianza de la función gaussiana que describe la estimación de nuestro filtro del estado verdadero del sistema.

Matrices de covarianza de proceso y medición de ruido

Las matrices de covarianza de ruido de proceso y medición \(\mathbf{Q}\) y \(\mathbf{R}\) describen la varianza de cada uno de nuestros estados y medidas. Recuerde que para una función gaussiana, la varianza es el cuadrado de la desviación estándar de la función. En un WPILib, Q y R son matrices diagonales cuyas diagonales contienen sus respectivas varianzas. Por ejemplo, un filtro de Kalman con estados: matemáticas begin{bmatrix}text{position} \ text{velocity} end{bmatrix} y medidas \(\begin{bmatrix}\text{position} \end{bmatrix}\) con desviaciones de estado estándar \(\begin{bmatrix}0.1 \\ 1.0\end{bmatrix}\) y desviación estándar de medición \(\begin{bmatrix}0.01\end{bmatrix}\) tendría las siguientes \(\mathbf{Q}\) y \(\mathbf{R}\) matrices:

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

Matriz de covarianza de errores

La matriz de covarianza de error \(\mathbf{P}\) describe la covarianza de la estimación del estado \(\mathbf{\hat{x}}\). De manera informal, \(\mathbf{P}\) describe nuestra certeza sobre el estado estimado. Si \(\mathbf{P}\) es grande, nuestra incertidumbre sobre el estado real es grande. A la inversa, a \(\mathbf{P}\) con elementos más pequeños implicaría menos incertidumbre sobre nuestro verdadero estado.

A medida que proyectamos el modelo hacia adelante, :math:mathbf{P}` aumenta a medida que disminuye nuestra certeza sobre el verdadero estado del sistema.

Predecir paso

En la predicción, nuestra estimación de estado se actualiza de acuerdo con la dinámica del sistema lineal \(\mathbf{\dot{x} = Ax + Bu}\). Además, nuestra covarianza de error \(\mathbf{P}\) aumenta por la matriz de covarianza de ruido del proceso \(\mathbf{Q}\). Los valores más grandes de \(\mathbf{Q}\) harán que nuestra covarianza de error \(\mathbf{P}\) crezca más rápidamente. Esto \(\mathbf{P}\) se usa en el paso de corrección para ponderar el modelo y las medidas.

Paso correcto

En el paso correcto, nuestra estimación de estado se actualiza para incluir nueva información de medición. Esta nueva información se pondera contra la estimación del estado \(\mathbf{\hat{x}}\) por la ganancia de Kalman \(\mathbf{K}\). Los valores grandes de \(\mathbf{K}\) ponderan más las medidas entrantes, mientras que los valores más pequeños de \(\mathbf{K}\) ponderan más nuestra predicción de estado. Porque \(\mathbf{K}\) está relacionado con \(\mathbf{P}\), los valores más grandes de \(\mathbf{P}\) aumentarán \(\mathbf{K}\) y medidas más pesadas. Si, por ejemplo, se predice un filtro para una duración prolongada, el grande \(\mathbf{P}\) ponderaría mucho la nueva información.

Finalmente, la covarianza de error \(\mathbf{P}\) disminuye para aumentar nuestra confianza en la estimación del estado.

Ajuste de los filtros de Kalman

Los constructores de las clases de filtros Kalman de WPILib toman un sistema lineal, un vector de desviaciones estándar de ruido de proceso y desviaciones estándar de ruido de medición. Éstos se convierten en matrices \(\mathbf{Q}\) y \(\mathbf{R}\) llenando las diagonales con el cuadrado de las desviaciones estándar, o varianzas, de cada estado o medida. Al disminuir la desviación estándar de un estado (y por lo tanto su entrada correspondiente en \(\mathbf{Q}\)), el filtro desconfiará más de las mediciones entrantes. De manera similar, aumentar la desviación estándar de un estado confiará más en las mediciones entrantes. Lo mismo se aplica a las desviaciones estándar de la medición: disminuir una entrada hará que el filtro confíe más en la medición entrante para el estado correspondiente, mientras que aumentarla disminuirá la confianza en la medición.

54
55
56
57
58
59
60
  private final KalmanFilter<N1, N1, N1> m_observer = new KalmanFilter<>(
        Nat.N1(), Nat.N1(),
        m_flywheelPlant,
        VecBuilder.fill(3.0), // How accurate we think our model is
        VecBuilder.fill(0.01), // How accurate we think our encoder
        // data is
        0.020);