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

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$(x_k-x_{k-1})/dt$${\bf H}$${\bf H} = [1\ \ \ 0]$${\bf x}_k$$x_k$$\dot{x}_{k}$

$$\begin{align*} \mathbf{x}_{k} & =\left(\begin{array}[c]{c}x_{k}\\ \dot{x}_{k}\end{array} \right) \end{align*}$$

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$k$$\hat{x}_k$$k-1$$x_{k-1}$$\dot{x}_{k-1}$$a$$k-1$$k$$\hat{x}$$a$

$$\hat{x}_k = x_{k-1} + \dot{x}_{k-1} dt + \frac{1}{2} a dt^2$$

Esto implica que en el tiempo , una medida de la velocidad viene dada por˙ x$k$$\hat{\dot{x}}_k$

$$\hat{\dot{x}}_k = \dot{x}_{k-1} + a dt = 2 \frac{\hat{x}_k - {x}_{k-1}}{dt} - \dot{x}_{k-1}$$

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-1$\hat{x}_k$$x_{k-1}$$\dot{x}_{k-1}$$\bf R$

$$\begin{align*} \mathbf{\hat{x}}_{k} & =\left(\begin{array}[c]{c}\hat{x}_{k}\\ \hat{\dot{x}}_{k}\end{array} \right) \end{align*}$$

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

¿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:$x_k$

$$\begin{array} xx_{k+1} &=& x_{k} + \dot{x}_{k} \Delta t + \frac{1}{2} a (\Delta t)^2\\ \dot{x}_{k+1} &=& \dot{x}_{k} + a \Delta t \end{array}$$

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.

$$\begin{align*} \mathbf{x}_{k} & =\left(\begin{array}[c]{c}x_{k}\\ \dot{x}_{k}\end{array} \right) \end{align*}$$ $$\mathbf{x}_{k+1} = \left(\begin{array}[c]{c} 1\ \ \Delta t\\ 0\ \ 1\end{array} \right) \mathbf{x}_{k} + \left(\begin{array}[c]{c} \frac{(\Delta t)^2}{2} \\ \Delta t \end{array} \right) a_k$$ $a_k$

Eso proporciona una matriz de la versión anterior, pero las matrices y deberían ser las mismas.$\mathbf{G}$$\mathbf{F}$$\mathbf{H}$

---

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!).$y$$y$

Luego, las siguientes gráficas muestran el resultado.

Gráfico 1 : y versus tiempo.$y$$x_k$

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

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

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

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.

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

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 por$\mathbf{z}_{k+1}=x_{k+1}$$\mathbf{P}_k=0$$\mathbf{P}_{k+1}^{-}=\mathbf{Q}$$\mathbf{K}_{k+1}$

$$\mathbf{K}_{k+1} = \left(\begin{array}[c]{c}1\\ 2/dt\end{array} \right)$$

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

$$\begin{align*} \mathbf{\hat{x}}_{k+1} & = \mathbf{F}_{k+1}\mathbf{x}_k + \mathbf{K}_{k+1}\left(\mathbf{z}_{k+1} - \mathbf{H}_{k+1} \mathbf{F}_{k+1}\mathbf{x}_k\right)\\ & = \left(\begin{array}[c]{c}x_k + \dot{x}_k dt\\ \dot{x}_k\end{array} \right) + \left(\begin{array}[c]{c}1\\ 2/dt\end{array} \right) \left(x_{k+1} - \left( x_k + \dot{x}_k dt\right) \right)\\ & = \left(\begin{array}[c]{c}x_{k+1}\\ 2 \left(x_{k+1} - x_k \right) /dt - \dot{x}_k\end{array} \right) \end{align*}$$

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.$(x_k-x_{k-1})/dt$