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 .

Funciones gaussianas

Kalman filters utilize a Gaussian distribution to model the noise in a process [1]. In the case of a Kalman filter, the estimated state of the system is the mean, while the variance is a measure of how certain (or uncertain) the filter is about the true state.

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.

To summarize, Kalman filters (and all Bayesian filters) have two parts: prediction and correction. Prediction projects our state estimate forward in time according to our system’s dynamics, and correct steers the estimated state towards the measured state. While filters often perform both in the same timestep, it’s not strictly necessary – For example, WPILib’s pose estimators call predict frequently, and correct only when new measurement data is available (for example, from a low-framerate vision system).

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.

Java

  // The observer fuses our encoder data and voltage inputs to reject noise.
  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);

C++

#include <numbers>

#include <frc/DriverStation.h>
#include <frc/Encoder.h>
#include <frc/TimedRobot.h>
#include <frc/XboxController.h>
#include <frc/controller/LinearQuadraticRegulator.h>
#include <frc/drive/DifferentialDrive.h>
#include <frc/estimator/KalmanFilter.h>
#include <frc/motorcontrol/PWMSparkMax.h>
#include <frc/system/LinearSystemLoop.h>
#include <frc/system/plant/DCMotor.h>
#include <frc/system/plant/LinearSystemId.h>
#include <units/angular_velocity.h>

  // The observer fuses our encoder data and voltage inputs to reject noise.
  frc::KalmanFilter<1, 1, 1> m_observer{
      m_flywheelPlant,
      {3.0},   // How accurate we think our model is
      {0.01},  // How accurate we think our encoder data is
      20_ms};

Python

        # The observer fuses our encoder data and voltage inputs to reject noise.
        self.observer = wpimath.estimator.KalmanFilter_1_1_1(
            self.flywheelPlant,
            [3],  # How accurate we think our model is
            [0.01],  # How accurate we think our encoder data is
            0.020,
        )

Footnotes

[1]

In a real robot, noise comes from all sorts of sources. Stray electromagnetic radiation adds extra voltages to sensor readings, vibrations and temperature variations throw off inertial measurement units, gear lash causes encoders to have inaccuracies when directions change… all sorts of things. It’s important to realize that, by themselves, each of these sources of «noise» aren’t guaranteed to follow any pattern. Some of them might be the «white noise» random vibrations you’ve probably heard on the radio. Others might be «pops» or single-loop errors. Others might be nominally zero, but strongly correlated with events on the robot. However, the Central Limit Theorem shows mathematically that regardless of how the individual sources of noise are distributed, as we add more and more of them up their combined effect eventually is distributed like a Gaussian. Since we do not know the exact individual sources of noise, the best choice of a model we can make is indeed that Gaussian function.