Filtro de Kalman: implementación, parámetros y ajuste

En primer lugar, esta es la primera vez que intento hacer un filtro de Kalman.

Anteriormente publiqué la siguiente pregunta Filtrar el ruido y las variaciones de los valores de velocidad en StackOverflow que describe los antecedentes de esta publicación. Esta es una muestra típica de valores que estoy tratando de filtrar. No necesariamente tienen que disminuir, que es el caso aquí. Pero la tasa de cambio es típicamente así

X ------- Y
16 --- 233.75
24 --- 234.01
26 --- 234.33
32 --- 234.12
36 --- 233.85
39 --- 233.42
47 --- 233.69
52 --- 233.68
55 --- 233.76
60 --- 232.97
66 --- 233.31
72 --- 233.99

He implementado mi Filtro de Kalman de acuerdo con este tutorial: Filtro de Kalman para Dummies .

Mi implementación se ve así (pseudocódigo).

//Standard deviation is 0.05. Used in calculation of Kalman gain

void updateAngle(double lastAngle){
  if(firsTimeRunning==true)
     priorEstimate = 0;               //estimate is the old one here
     priorErrorVariance = 1.2;        //errorCovariance is the old one
  else
     priorEstimate = estimate;              //estimate is the old one here
     priorErrorVariance = errorCovariance;  //errorCovariance is the old one
  rawValue = lastAngle;          //lastAngle is the newest Y-value recieved
  kalmanGain = priorErrorVariance / (priorErrVariance + 0.05);
  estimate = priorEstimate + (kalmanGain * (rawValue - priorEstimate));
  errorCovariance = (1 - kalmanGain) * priorErrVariance;
  angle = estimate;              //angle is the variable I want to update
}                                //which will be lastAngle next time

Comienzo con una estimación previa de 0. Esto parece funcionar bien. Pero lo que noto es que kalmanGain disminuirá cada vez que se ejecute esta actualización, lo que significa que confío en mis nuevos valores menos cuanto más tiempo se esté ejecutando mi filtro (?). No quiero eso

Pasé de usar solo un promedio móvil (simple y exponencial ponderado) a usar esto. En este momento ni siquiera puedo obtener resultados tan buenos como eso.

Mi pregunta es si esta es la implementación correcta y si mi varianza de error anterior y la desviación estándar se ven bien de acuerdo con los valores de muestra que he publicado. En realidad, mis parámetros se seleccionan al azar para ver si puedo obtener buenos resultados. He intentado varios rangos diferentes pero con malos resultados. Si tiene alguna sugerencia sobre los cambios que puedo hacer, sería muy apreciada. Lo siento si faltan algunas cosas obvias. Publicación por primera vez aquí también.

Los filtros de Kalman son útiles cuando su señal de entrada consiste en observaciones ruidosas del estado de un sistema dinámico lineal. Dada una serie de observaciones del estado del sistema, el filtro de Kalman tiene como objetivo proporcionar recursivamente mejores y mejores estimaciones del estado del sistema subyacente. Para aplicarlo con éxito, debe tener un modelo para la dinámica del sistema cuyo estado está estimando. Como se describe en detalle en Wikipedia , este modelo describe cómo se espera que cambie el estado del sistema subyacente en un paso de tiempo, dado su estado anterior, cualquier entrada al sistema y un componente estocástico distribuido por Gauss llamado ruido de proceso.

Dicho esto, su pregunta no aclara si tiene algún modelo subyacente. La publicación vinculada indica que estás consumiendo valores de velocidad de un sensor. Estos pueden modelarse como observaciones directas del estado de un sistema (donde el estado es su velocidad) u observaciones indirectas de su estado (donde el estado es su posición, por ejemplo). Pero, para usar el marco de Kalman, debe elegir un modelo de cómo se espera que evolucione ese estado a medida que pasa el tiempo; Esta información adicional se utiliza para generar la estimación óptima. Un filtro de Kalman no es una caja negra mágica que simplemente "limpiará" una señal que se le aplica.

Dicho esto, el fenómeno al que aludiste, donde el filtro de Kalman se volverá cada vez más seguro en su propia salida hasta el punto en que las observaciones de entrada se ignoran progresivamente, sucede en la práctica. Esto puede mitigarse aumentando manualmente los valores en la matriz de covarianza de ruido del proceso. Luego, cualitativamente, el modelo para la transición de estado del sistema contiene un componente estocástico más grande, por lo que la capacidad del estimador para predecir con precisión el siguiente estado dado el estado actual disminuye. Esto reducirá su dependencia de su estimación actual del estado del sistema y aumentará su dependencia de observaciones posteriores, evitando el efecto de "ignorar la entrada".

Si lo entendí correctamente, tienes algo que se mueve y puedes observar la velocidad y esta velocidad es ruidosa. A partir de sus mediciones, observa 2 tipos de variaciones.

1. Variaciones causadas por el ruido.
2. Variaciones porque el objeto realmente está cambiando la velocidad (por ejemplo, girar)

La razón por la que su ganancia de Kalman llega a cero es porque ha asumido implícitamente que la velocidad del objeto es constante y todo lo que necesita hacer es estimar esta velocidad verdadera.

" Oye, tengo un objeto que se mueve a una velocidad constante y quiero estimar esta velocidad constante "

$x_k$$k$$y_k$

$$x_k = x_{k-1}$$ $$y_k = x_k + q_k$$

Pero tu objeto no se mueve de esa manera. Su velocidad está cambiando y no sabes cómo y cuándo va a cambiar.

Lo que tienes que decir es:

" Oye, tengo un objeto que se mueve a gran velocidad pero no estoy seguro de cómo está cambiando su velocidad "

Hay muchas maneras de hacer esto: la forma más simple es agregar incertidumbre a su estado.

$$x_{k} = x_{k-1} + \underbrace{v_{k-1}}_{\text{you add uncertainty}}$$ $$y_k = x_k + q_k$$ $q_k$$v_k$

Sus ecuaciones de filtro de Kalman se verán así:

$$\hat y_{k|k-1} = \hat x_{k|k-1}$$ $$K_{k} = \frac{P_{k|k-1}}{P_{k|k-1} + Q^o}$$ $$\hat x_{k|k}= \hat x_{k|k-1} + K_k(y_k - \hat y_{k|k-1})$$ $$P_{k|k} = P_{k|k-1} - K_kP_{k|k-1}$$ $$P_{k+1|k} =P_{k|k} + Q^s$$

0.05$Q^o$$Q^s$

En su código, la ligera modificación sería:

stateVariance = 0.5

errorCovariance = (1 - kalmanGain) * priorErrVariance + stateVariance;

stateVariance$Q^s$

Este stateVariancevalor puede ser lo que quieras. Se basa en su confianza en cuánto cambiará realmente la velocidad. Si cree que la velocidad se mantendrá bastante constante, establezca esto en un número pequeño.

De esta manera, su Kalman Gain no irá a cero.

Necesita un sistema dinámico para usar un filtro de Kalman.

Yo sugeriría

$$y = \sum\limits_{i =0}^n a_i\, x^i$$

$$a[k+1] = a[k] + w$$ $$cov(w) = Q$$ $$z = \sum\limits_{i =0}^n a_i\, x^i = y$$

$x$$a$

Creo que podría usar algunas ideas de la teoría de control clásica, por ejemplo, el controlador PID .

Su señal Y puede ser el punto de ajuste del controlador u (t). La planta de proceso es solo 1 e y (t) será salida filtrada. Todo lo que tendrá que hacer es establecer los parámetros (sintonizar) P, I y D para obtener lo que desea.

La salida y (t) intentará "seguir" la entrada u (t), pero los parámetros controlan cómo será este seguimiento.

La ganancia diferencial D hará que su respuesta sea sensible a los cambios rápidos de error. En su caso, creo que D debería ser pequeño. No desea que y (t) cambie si u (t) cambia abruptamente.

La ganancia integral 'I' hará que su respuesta sea sensible al error acumulado. Deberías poner un alto valor allí. Si u (t) cambia de nivel y lo mantiene allí, el error se acumulará y luego querrá que y (t) haga lo mismo.

La ganancia de P puede dar una buena sintonía. De todos modos, intenta jugar con los parámetros y ver qué obtienes.

Sin embargo, existen métodos de ajuste complicados, pero no creo que lo necesite.

Buena suerte.

Aquí hay una implementación simple y limpia de Kalman Filter que sigue las anotaciones como en la página de Wikipedia. https://github.com/zziz/kalman-filter