Estimando la velocidad a partir de la posición conocida y la aceleración

Estoy atascado en modelar un modelo de sistema, es decir, obtener mi vector de estado y el vector de entrada. Supongo que la posición y la velocidad son vectores de estado y la aceleración es el vector de entrada. Mi segunda suposición es que las tres cantidades están en el vector de estado y ninguna en el vector de entrada.

Entonces ... ¿qué es el vector de estado y qué es el vector de entrada en mi caso?

-

Información adicional:

Recibo mediciones del sensor de posición y el sensor de aceleración. Todo está sucediendo en 1D, por ejemplo, en línea recta. Quiero fusionar estas lecturas (y eliminar el ruido) para obtener una estimación de la velocidad para cada paso de tiempo.

Estas ecuaciones describen el sistema; Sin embargo, no estoy seguro de si están modelados correctamente. Si entiendo correctamente, es seguro predecir que la aceleración es constante (aunque en realidad cambie), porque la matriz de covarianza del proceso corrige esta suposición (¿verdad?).

También tengo algunos datos de muestra para trabajar (los valores de entrada no se mencionan aquí por simplicidad):

 time    pos     acc      what I should get as output (velocity)
[0.0s]  0.000, -0.000  | 18.850
[0.1s]  1.885, -0.113  | 18.850
[0.2s]  3.768, -0.227  | 18.839
[0.3s]  5.650, -0.340  | 18.816
[0.4s]  7.528, -0.452  | 18.782
[0.5s]  9.401, -0.565  | 18.737

---

ADICION 2:

Para una mejor comunicación, estoy creando una nueva respuesta, pero debería tratarse como un comentario a la primera respuesta. Jason, ya me has ayudado enormemente y estoy realmente agradecido por tu tiempo. Sin embargo, todavía tengo problemas con esto: los resultados de Kalman Filter no son los esperados. Puede encontrar el tiempo por favor revise lo siguiente, gracias. Ya te debo una cerveza o dos (o cofres si quieres). Si tienes PayPal, contáctame en primoz [at] codehunter.eu :)

---

Implementé el modelo que Jason había propuesto en la primera respuesta. Agregué el tirón como cuarta variable de estado. Después de horas de revisión, decidí volver aquí por ayuda. Los valores que obtengo de KF no son los esperados. La siguiente tabla representa los datos de las primeras 10 iteraciones del algoritmo. Observe cómo el tirón aumenta cada vez que se equivocan otras estimaciones. ¡Después de un segundo, la diferencia entre la aceleración real y la estimada es de más de 1 m / s² (ver tabla, última fila)!

           real           measured                   estimated                    real 
time   pos     acc       pos    acc        pos      acc    jerk    vel[!]       velocity 
0.0   0.000  -0.000    -0.040  0.030  |  -0.300   -0.060   0.000   18.850  <-->  18.850
0.1   1.885  -0.113     1.965 -0.153  |   1.585   -0.061  -0.006   18.844  <-->  18.844
0.2   3.768  -0.227     3.778 -0.247  |   3.469   -0.066  -0.035   18.835  <-->  18.827
0.3   5.650  -0.340     5.750 -0.370  |   5.351   -0.090  -0.122   18.815  <-->  18.799
0.4   7.528  -0.452     7.358 -0.452  |   7.228   -0.152  -0.291   18.769  <-->  18.759
0.5   9.401  -0.565     9.251 -0.555  |   9.094   -0.282  -0.574   18.673  <-->  18.708
0.6   11.269 -0.677    11.309 -0.717  |   10.938  -0.518  -1.006   18.494  <-->  18.646
0.7   13.130 -0.788    13.260 -0.758  |   12.752  -0.840  -1.490   18.233  <-->  18.573
0.8   14.983 -0.899    15.043 -0.949  |   14.520  -1.286  -2.096   17.854  <-->  18.488
0.9   16.827 -1.009    16.977 -1.089  |   16.235  -1.838  -2.770   17.362  <-->  18.393
1.0   18.661 -1.118    18.831 -1.168  |   17.890  -2.477  -3.476   16.762  <-->  18.287

Mis matrices están aquí:

¿Qué está causando esta adición en cada paso de tiempo para imbécil ? ¿Alguna de mis matrices está mal?

Lo mismo ocurre con la primera solución (solo modelo de 3 estados): la aceleración no está cambiando como debería.

ÚLTIMA EDICIÓN:

Finalmente logré hacerlo funcionar. No estoy seguro de si hubo un error de implementación o matrices P&Q incorrectas.

No haría una suposición de aceleración constante en este caso. Normalmente lo haría si no tuviera medios para medir la aceleración del sistema, pero dice que es observable para su caso. La forma más obvia de modelar este sistema sería usar el vector de estado

$$\mathbf{x_k} = \left [ \begin{array}{c}x_k \\ \dot{x}_k \\ \ddot{x}_k \end{array} \right ] = \left [ \begin{array}{c}x_k \\ v_k \\ a_k \end{array} \right ]$$

donde es la posición, es la velocidad y es la aceleración del sistema en el instante instantáneo . Como usted dice que puede medir la posición y la aceleración del sistema, el vector de medición sería:$x_k$$v_k$$a_k$$k$$\mathbf{z_k}$

$$\mathbf{z_k} = \mathbf{Hx_k} + \mathbf{v_k}$$

con

$$\mathbf{H} = \left[ \begin{array}{cc} 1 && 0 && 0 \\ 0 && 0 && 1 \end{array} \right]$$

resultando en el modelo de medición:

$$\mathbf{z_k} = \left[ \begin{array}{c} x_k \\ a_k \end{array} \right] + \mathbf{v_k}$$

donde es el ruido (gaussiano) inherente a la medición. Ahora, a menos que esté agregando alguna fuerza conocida al sistema que afectaría su dinámica, entonces el vector de entrada . Vea esta pregunta reciente para una discusión más detallada de los diversos términos en el modelo de Kalman, que necesita saber y que no .$\mathbf{v_k}$$\mathbf{u_k} = \mathbf{0}$

Editar: con respecto al comentario a continuación sobre cómo manejar la matriz de transición de estado para el término de aceleración: hay un par de formas diferentes de manejar eso. Podría suponer un modelo cero-tirón (jerk es la derivada del tiempo de la aceleración); esto es equivalente a suponer que . O bien, puede agregar un cuarto elemento a su vector de estado:$a_{k+1}=a_k$

$$\mathbf{x_k} = \left [ \begin{array}{c}x_k \\ \dot{x}_k \\ \ddot{x}_k \\ \dddot{x_k} \end{array} \right ] = \left [ \begin{array}{c}x_k \\ v_k \\ a_k \\ j_k \end{array} \right ]$$

donde es la sacudida del sistema, o la tasa de cambio de aceleración en el tiempo, en el instante instantáneo . Dado que no está midiendo jerk directamente, tendría una matriz de medición de:$j_k$$k$

$$\mathbf{H} = \left[ \begin{array}{cc} 1 && 0 && 0 && 0\\ 0 && 0 && 1 && 0 \end{array} \right]$$

Y la matriz de transición de estado se convertiría en:

$$\mathbf{F} = \left[ \begin{array}{cccc} 1 && dt && \frac{1}{2}dt^2 && \frac{1}{6} dt^3 \\ 0 && 1 && dt && \frac{1}{2}dt^2 \\ 0 && 0 && 1 && dt \\ 0 && 0 && 0 && 1 \end{array} \right]$$

Este es un modelo de "sacudida constante" (similar a los modelos de velocidad constante o aceleración constante que puede ver en ejemplos simples); la matriz de transición de estado supone implícitamente que el tirón es constante para todos los valores de k. Es probable que esto no sea cierto. Para manejar ese aspecto del problema, introduciría un término de ruido de proceso en el componente jerk de la ecuación de transición de estado.

Cualitativamente, esto le permite expresar que no está seguro de cómo va a cambiar el término jerk de vez en cuando, pero que espera que los cambios sean aleatorios con alguna distribución gaussiana. Esta es una herramienta que se usa a menudo para "ajustar" modelos de filtros adaptativos como este hasta que encuentre un conjunto de parámetros que funcione bien para su aplicación.

Supongo que sus variables de estado son según la respuesta de Jason. Si ese es el caso, entonces creo que su problema podría ser su matriz (ruido de proceso): debido a que es cero en todas partes excepto por la actualización jerk, los únicos términos que actualizan los valores de la posición, la velocidad y la aceleración son las partes deterministas de la actualizar las ecuaciones (excepto donde se alimenta la naturaleza estocástica del imbécil).$Q$

Intente usar una matriz como:$Q$

$$Q = \left [ \begin{array}{cccc} 0.00001 & 0& 0& 0\\ 0 & 0.00001 & 0 & 0\\ 0 & 0 & 0.00001 & 0 \\ 0 & 0& 0& 0.001 \end{array} \right ]$$

Si esos elementos diagonales son cero, el modelo dice "No espero que cambie la posición, la velocidad o la aceleración (aparte de las ecuaciones de actualización)", lo cual claramente no es el caso.

Creo que el problema está en Q. Q está cargando a K con sus valores. Logro buenos resultados cuando Q está precargado con poderes de delta_t.

Ver cualquier artículo sobre el modelo wiener. Tiene una buena matriz Q.

A00 tiene que ser múltiplo de la potencia delta_t de 5 A11 potencia delta_t de 3 A22 potencia delta_t de 1 A01 múltiplo de potencia delta_t de 4

Y así

Mirar

Salas de estar