Filtro de Kalman para posición y velocidad: introducción de estimaciones de velocidad

24

Gracias a todos los que publicaron comentarios / respuestas a mi consulta ayer ( Implementando un filtro de Kalman para posición, velocidad, aceleración ). He estado mirando lo que se recomendó, y en particular tanto en (a) el ejemplo de Wikipedia en posición y velocidad unidimensionales como en otro sitio web que considera algo similar .

Actualización 26-abr-2013 : la pregunta original aquí contenía algunos errores, relacionados con el hecho de que no había entendido correctamente el ejemplo de Wikipedia en posición y velocidad unidimensionales . Con mi mejor comprensión de lo que está sucediendo, ahora volví a redactar la pregunta y la enfocé más estrechamente.

Ambos ejemplos a los que me refiero en el párrafo introductorio anterior suponen que solo se mide la posición. Sin embargo, ninguno de los ejemplos tiene ningún tipo de cálculo para la velocidad. Por ejemplo, el ejemplo de Wikipedia especifica la matriz como , lo que significa que solo se ingresa la posición. Centrándose en el ejemplo de Wikipedia, el vector de estado del filtro de Kalman contiene la posición y la velocidad , es decirH H = [ 1 0 ] x k x k ˙ x k(xkxk1)/dtHH=[1   0]xkxkx˙k

xk=(xkx˙k)

Suponga que la medida de la posición en el tiempo es . Entonces, si la posición y la velocidad en el tiempo fueron y , y si es una aceleración constante que se aplica en el intervalo de tiempo a , de la medición de es posible deducir un valor para uso de la fórmulax k k - 1 x k - 1 ˙ x k - 1 un k - 1 k x unakx^kk1xk1x˙k1ak1kx^a

x^k=xk1+x˙k1dt+12adt2

Esto implica que en el tiempo , una medida de la velocidad viene dada por˙ x kkx˙^k

x˙^k=x˙k1+adt=2x^kxk1dtx˙k1

Todas las cantidades en el lado derecho de esa ecuación (es decir, , y ) son variables aleatorias normalmente distribuidas con medias conocidas y desviaciones estándar , entonces la matriz para el vector de mediciónxk-1 ˙ x k-1Rx^kxk1x˙k1R

x^k=(x^kx˙^k)

Se puede calcular. ¿Es esta una forma válida de introducir estimaciones de velocidad en el proceso?

Estocástico
fuente
2
No revisé todos tus cálculos. Sin embargo, hablando del ejemplo de Wikipedia, parece estar un poco confundido sobre su estructura. Tienes razón en que solo se mide la posición. Sin embargo, se utiliza el llamado modelo de "velocidad constante". Esto significa que la velocidad se considera constante en la matriz de transición de estado.
Jason R
3
Los cambios en la velocidad se modelan utilizando la matriz de ruido del proceso. Por lo tanto, asume inherentemente que la velocidad cambiará aleatoriamente con alguna covarianza especificada. Sorprendentemente, esto a menudo funciona bien. Es común utilizar el ruido del proceso una derivada por encima de su derivada más alta variable de estado de esta manera. Por ejemplo, si incluyó la aceleración en su modelo, entonces podría tener un componente jerk aleatorio incluido en el ruido de su proceso.
Jason R
@JasonR con el modelo de Wikipedia (suponiendo cero covarianza inicial entre la posición y la velocidad), la estimación de la velocidad es siempre su valor inicial (como usted dice, un modelo de "velocidad constante"). Sin embargo, la variación de la velocidad crece monotónicamente a través del ruido del proceso, y no hay mediciones que puedan reducirla. ¿Cuál es la ventaja de esto sobre un modelo que solo modela la posición y asume una velocidad constante?
Estocástico
2
La varianza de la estimación de velocidad no debería aumentar monotónicamente. El ruido del proceso simplemente introduce un componente estocástico en la ecuación de transición de estado, lo que le permite expresar cierta incertidumbre sobre cómo evolucionará exactamente el estado del sistema paso a paso. Si no incluye el ruido del proceso, su filtro realmente generará una velocidad constante. Eso probablemente no es lo que quieres.
Jason R
Bueno, @JasonR, si miras el modelo de wikipedia, verás que la variación de velocidad monotínicamente creciente es lo que te da.
Estocástico

Respuestas:

24

¿Es esta una forma válida de introducir estimaciones de velocidad en el proceso?

Si elige su estado de manera apropiada, las estimaciones de velocidad son "gratis". Vea la derivación del modelo de señal a continuación (para el caso simple 1-D que hemos estado viendo).

Modelo de señal, toma 2

Entonces, realmente necesitamos acordar un modelo de señal antes de poder avanzar. Desde su edición, parece que su modelo de la posición, , es:xk

xk+1=xk+x˙kΔt+12a(Δt)2x˙k+1=x˙k+aΔt

Si nuestro estado es como antes: entonces la ecuación de actualización de estado es solo: donde ahora nuestro es la aceleración normalmente distribuida.

xk=(xkx˙k)
xk+1=(1  Δt0  1)xk+((Δt)22Δt)ak
ak

Eso proporciona una matriz de la versión anterior, pero las matrices y deberían ser las mismas.GFH


Si implemento esto en scilab(lo siento, no hay acceso a matlab), se ve así:

// Signal Model
DeltaT = 0.1;
F = [1 DeltaT; 0 1];
G = [DeltaT^2/2; DeltaT];
H = [1 0];

x0 = [0;0];
sigma_a = 0.1;

Q = sigma_a^2;
R = 0.1;

N = 1000;

a = rand(1,N,"normal")*sigma_a;

x_truth(:,1) = x0;
for t=1:N,
    x_truth(:,t+1) = F*x_truth(:,t) + G*a(t);
    y(t) = H*x_truth(:,t) + rand(1,1,"normal")*sqrt(R);
end

Entonces, puedo aplicar las ecuaciones de filtro de Kalman a esta (las mediciones ruidosas).y

// Kalman Filter
p0 = 100*eye(2,2);

xx(:,1) = x0;
pp = p0;
pp_norm(1) = norm(pp);
for t=1:N,
    [x1,p1,x,p] = kalm(y(t),xx(:,t),pp,F,G,H,Q,R);
    xx(:,t+1) = x1;
    pp = p1;
    pp_norm(t+1) = norm(pp);
end

Así que tenemos nuestras mediciones ruidosas , y les aplicamos el filtro de Kalman y usamos el mismo modelo de señal para generar que hacemos para aplicar el filtro de Kalman (¡una suposición bastante grande, a veces!).yy

Luego, las siguientes gráficas muestran el resultado.

Gráfico 1 : y versus tiempo.yxk

ingrese la descripción de la imagen aquí

Gráfico 2 : una vista ampliada de las primeras muestras:

ingrese la descripción de la imagen aquí

Trama 3 : Algo que nunca obtienes en la vida real, la posición real frente a la estimación estatal de la posición.

ingrese la descripción de la imagen aquí

Trama 4 : Algo que nunca obtienes en la vida real, la velocidad real frente a la estimación estatal de la velocidad.

ingrese la descripción de la imagen aquí

Gráfica 5 : La norma de la matriz de covarianza de estado (¡algo que siempre debes monitorear en la vida real!). Tenga en cuenta que pasa rápidamente de su valor inicial muy grande a algo muy pequeño, por lo que solo he mostrado las primeras muestras.

ingrese la descripción de la imagen aquí

Gráfico 6 : Gráficos del error entre la posición y velocidad reales y sus estimaciones.

ingrese la descripción de la imagen aquí

Si estudia el caso donde las mediciones de posición son exactas, entonces encuentra que las ecuaciones de udpate de Kalman producen resultados exactos para AMBAS posiciones y velocidad. Matemáticamente es sencillo ver por qué. Usando la misma notación que el artículo de Wikipedia , las medidas exactas significan que . Si supone que la posición inicial y la velocidad son conocidas de modo que , entonces y la matriz de ganancia de Kalman viene dado porzk+1=xk+1Pk=0Pk+1=QKk+1

Kk+1=(12/dt)

Esto significa que el procedimiento de actualización de Kalman produce

x^k+1=Fk+1xk+Kk+1(zk+1Hk+1Fk+1xk)=(xk+x˙kdtx˙k)+(12/dt)(xk+1(xk+x˙kdt))=(xk+12(xk+1xk)/dtx˙k)

Como puede ver, el valor de la velocidad viene dado exactamente por la fórmula que estaba proponiendo usar para la estimación de la velocidad. Entonces, aunque no pudo ver ningún tipo de cálculo para la velocidad, de hecho, está oculto allí después de todo.(xkxk1)/dt

Peter K.
fuente
Gracias por toda su ayuda hasta ahora. Mi pregunta original contenía algunos malentendidos, así que traté de reenfocarla y con un intento de responder a su pregunta sobre z_k.
Estocástico
1
gracias vm por todos tus esfuerzos :-). Su trabajo me impulsó a hacer un poco de matemáticas que he agregado a su respuesta, espero que no le importe. De todos modos, ahora estoy 100% convencido de que el método estándar es bueno, porque después de todo, puedo ver fórmulas allí para la velocidad. Gracias de nuevo
Estocástico
Me alegra poder ayudar! No hay problema con agregar a la respuesta.
Peter K.