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?). ingrese la descripción de la imagen aquí

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.

kalman-filters

— Primož Kralj
fuente

Para obtener una respuesta útil, debe proporcionar más información. Describe el sistema. Muéstranos qué ecuaciones has reunido.

— Jim Clay

Gracias por su respuesta. He proporcionado información adicional.

— Primož Kralj

Por favor, que alguien me ayude con esto. Siento que estoy perdiendo la cabeza.

— Primož Kralj

No veo ningún problema obvio. Es probable que tenga un error de implementación.

— Jason R

@Phonon, sin resentimientos. Aunque siento que he estado en una montaña rusa :)

— Primož Kralj

Respuestas:

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

x_{k} = [\begin{matrix} x_{k} \\ {\dot{x}}_{k} \\ {\ddot{x}}_{k} \end{matrix}] = [\begin{matrix} x_{k} \\ v_{k} \\ a_{k} \end{matrix}]

$\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}$

z_{k} = H x_{k} + v_{k}

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

con

H = [\begin{array}{cc} 1 & 0 & 0 \\ 0 & 0 & 1 \end{array}]

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

resultando en el modelo de medición:

z_{k} = [\begin{matrix} x_{k} \\ a_{k} \end{matrix}] + v_{k}

$\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$

x_{k} = [\begin{matrix} x_{k} \\ {\dot{x}}_{k} \\ {\ddot{x}}_{k} \\ \overset{⃛}{x_{k}} \end{matrix}] = [\begin{matrix} x_{k} \\ v_{k} \\ a_{k} \\ j_{k} \end{matrix}]

$\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$

H = [\begin{array}{cc} 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \end{array}]

$\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:

F = [\begin{array}{cccc} 1 & d t & \frac{1}{2} d t^{2} & \frac{1}{6} d t^{3} \\ 0 & 1 & d t & \frac{1}{2} d t^{2} \\ 0 & 0 & 1 & d t \\ 0 & 0 & 0 & 1 \end{array}]

$\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.

— Jason R
fuente

Gracias, realmente encendiste una luz en mi cabeza. Pero todavía no entiendo cuál es mi aceleración que espera estar en el siguiente paso (si no es constante): esto es importante para construir la matriz de transición A, la última fila para ser precisos. ¿Qué sugieres?

— Primož Kralj

Hay todo un lío con las preguntas migratorias. Parte de tu respuesta se ha ido. Espero que los mods reparen esto. Además, mire la Adición 2 en mi pregunta original.

— Primož Kralj

Pude recuperar el texto editado y volver a aplicarlo a mi respuesta.

— Jason R

¿Podría explicar cómo obtener la covarianza del proceso para la primera solución (3 variables de estado)? No puedo encontrar un enlace decente en línea que lo explique exhaustivamente en la práctica.

— Primož Kralj

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

$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.

— Peter K.
fuente

Buena explicación, pero ¿por qué elegir 0.001, por qué no 2.84 por ejemplo?

— Primož Kralj

¡Gracias! La razón para elegir 0.00001 sobre 2.84: Bueno, 2.84 es MUY grande para una variación. Supuse que tenía un razonamiento válido detrás del número jerk (0.001), y que los otros números deberían ser significativamente más pequeños que esto. Si no hay una razón real para elegir el número jerk, intente el mismo valor en todos los elementos diagonales.

— Peter K.

Pero ¿qué significa esto en la práctica? Si digo que mi sensor de posición tiene una variación de 0.00001 (primer elemento de diagnóstico en la matriz Q), ¿eso significa que su error es + - 0.00001 metros, por ejemplo (supongo que no)? Supongo que elige una variación menor para un sensor que es más preciso (posición) y una variación mayor para un sensor con menos precisión (aceleración).

— Primož Kralj

Si dice que la varianza de una medición es , en el marco del filtro de Kalman, está diciendo que es gaussiana distribuida con una desviación estándar de . Entonces, ~ 68% del tiempo, la magnitud del error de medición será menor que , ~ 95% del tiempo, será menor que , y así sucesivamente.

0.00001

$0.00001$

\sqrt{0.00001}

$\sqrt{0.00001}$

\sqrt{0.00001}

$\sqrt{0.00001}$

2 \sqrt{0.00001}

$2\sqrt{0.00001}$

— Jason R

Gracias a ambos, espero que este hilo también ayude a los futuros visitantes.

— Primož Kralj

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

— Jm Rams
fuente