¿Cómo elijo los parámetros de un filtro Kalman?

Supongamos que quiero rastrear la posición de un automóvil en 2D. Lo que obtengo como datos del sensor es mi posición actual. Entonces mi estado es

$$\mathbf{x} = \begin{pmatrix}x\\y\\\dot{x}\\\dot{y}\end{pmatrix}$$

donde es la posición en m lejos de algún punto predefinido, es la velocidad en m / s en el momento de inicio y es la aceleración en . Las medidas son$x \in \mathbb{R}$$\dot{x} \in \mathbb{R}$$\ddot{x} \in \mathbb{R}$$m/s^2$

$$\mathbf{z} = \begin{pmatrix}x^{(M)}\\y^{(M)}\end{pmatrix}$$

Lo que puedo elegir es mi aceleración en cada paso de tiempo (los pasos de tiempo tienen la longitud ):$i$$t$

$$u = \begin{pmatrix}\ddot{x}^{(u)}\\\ddot{y}^{(u)}\end{pmatrix}$$

Como el filtro de Kalman es un filtro lineal, mi modelo de estado es:

$$\mathbf{x}^{(P)} = A x + Bu$$

La medición depende del estado, con algo de ruido :$v$

$$\mathbf{z} = H \mathbf{x} + v$$

con , . Como uno puede descomponer la aceleración / velocidad en las direcciones y la ecuación para la nueva posición es$A \in \mathbb{R}^{4 \times 4}$$H \in \mathbb{R}^{2 \times 4}$

$$\begin{align}x_{new}(t) &= x + \dot{x} t + 0.5 \ddot{x} t^2\\ y_{new}(t) &= y + \dot{y} t + 0.5 \ddot{y} t^2\\ \dot{x}_{new}(t) &= \dot{x} + \ddot{x} t\\ \dot{y}_{new}(t) &= \dot{y} + \ddot{y} t\end{align}$$

Entonces, dado nuestro modelo de estado, obtenemos:

$$\mathbf{x}^{(P)} = \underbrace{\begin{pmatrix}1& 0 & t & 0\\ 0& 1 & 0 & t\\ 0& 0 & 1 & 0\\ 0& 0 & 0 & 1\end{pmatrix}}_{A_i} \mathbf{x} + \underbrace{\begin{pmatrix}0.5t^2 & 0\\ 0 & 0.5t^2\\ t & 0\\ 0 & t\end{pmatrix}}_{B_i} \cdot u_i$$

1. ¿Es este hasta ahora un escenario / enfoque razonable para el filtro de Kalman?

2. ¿Cómo elijo la matriz de covarianza de incertidumbre inicial / el estado inicial ? He oído que uno hace que los valores de la matriz sean "grandes", sea lo que sea que eso signifique. Por ejemplo, si se trata de una matriz diagonal para algunos ? Por ejemplo, ya que el diámetro de la tierra es aproximadamente y ya que ir a más de nunca va a suceder para un automóvil.$P_0 \in \mathbb{R}^{4 \times 4}$$\mathbf{x}$

   $$P_0 = \begin{pmatrix}a_1 & 0 & 0 & 0\\ 0 & a_2 & 0 & 0\\ 0 & 0 & a_3 & 0\\ 0 & 0 & 0 & a_4\end{pmatrix}$$ $a \in \mathbb{R}^+$$a_1 = a_2 = 20000000$$40000\textrm{ km}$$a_3=a_4=90$$324\textrm{ km/h}$

   Para el parámetro de estado inicial, esperaría dos pasos de tiempo:

   $$\mathbf{x}_0 = \begin{pmatrix}x^{(M)}_{-1}\\ y^{(M)}_{-1}\\ x^{(M)}_{-1} - x^{(M)}_{-2}\\ y^{(M)}_{-1} - y^{(M)}_{-2}\end{pmatrix}$$

Paso de predicción

La predicción de estado funciona como anteriormente:

$$\mathbf{x}^{(P)}_{i+1} = A_i \mathbf{x}_{i} + B_i u_i$$

Predicción de covarianza:

$$P_{i+1}^{(P)} = A P_i A^T + Q \quad \text{with}\quad Q \in \mathbb{R}^{4 \times 4}. \tag{P}$$

3. ¿De dónde obtengo la covarianza de error de proceso ? ¿Qué propiedades tiene que tener? Supongo positivo definitivo? ¿Qué significa esta matriz?$Q$

Paso de innovación

Innovación, que compara la medición con la predicción:

$$\tilde{y}_{i+1} = z_{i+1} - H \mathbf{x}^{(P)}_{i+1}$$

4. (resuelto) : ¿De dónde obtengo la matriz de observación ? Qué significa eso?$H \in \mathbb{R}^{2 \times 4}$

EDITAR :

Lo tengo. En mi ejemplo, ya que codifica la relación entre el estado y la medición.

$$H = \begin{pmatrix}1 & 0 & 0 & 0\\0 & 1 & 0 & 0\end{pmatrix},$$

Covarianza de innovación:

$$S_{i+1} = H P_{i+1}^{(P)} H^T + R$$

Para la covarianza de error de medición tengo que saber algo sobre la forma en que funcionan mis sensores. Supongo que generalmente será una matriz diagonal, ya que los sensores serán independientes (?).$R \in \mathbb{R}^{2 \times 2}$

Kalman Gain:

$$K_{i+1} = P_{i+1}^{(P)} H^T S^{-1}_{i+1}$$

Ahora, finalmente, el estado y la actualización de covarianza:

$$x_{i+1} = \mathbf{x}^{(P)}_{i+1} + K_{i+1} \tilde{y}$$ $$P_{i+1} = (I - K_{i+1} H) P_{i+1}^{(P)}$$

Fuentes:

• http://greg.czerniak.info/guides/kalman1/ (Encontré esto mientras escribía esta pregunta, ya respondió a un par de preguntas que, con suerte, eliminé).
• Diversas conferencias en KIT

¿Es este hasta ahora un escenario / enfoque razonable para el filtro de Kalman?

Respuesta 1: Sí, su modelo se ve razonable. Sin embargo, estás tratando la aceleración como constante. Si va a cambiar en su experimento, debe incluirlo en la matriz de error su sistema .$Q$

¿Cómo elijo la matriz de covarianza de incertidumbre inicial / el estado inicial ?$P_0 \in \mathbb{R}^{4 \times 4}$$\mathbf{x}$

Respuesta 2: es su covarianza de estado inicial . Eso expresa cuánto sabe sobre la estimación inicial del estado . Si no tiene idea, es habitual configurar donde es grande. Ver, por ejemplo, esta respuesta que establece .$P_0$$\mathbf{x}_0$

$$P_0 = \sigma^2I_4$$ $\sigma^2$$\sigma^2 = 1000$

¿De dónde obtengo la covarianza de error de proceso ? ¿Qué propiedades tiene que tener? Supongo positivo definitivo? ¿Qué significa esta matriz?$Q$

Respuesta 3: Esta matriz expresa su error del sistema. Las entradas en esta matriz representan la covarianza de los valores correspondientes en su modelo de sistema. Si supone, por ejemplo, que la aceleración es tan constante como lo hizo, pero que va a cambiar en el mundo real, entonces puede incluir aquí la covarianza correspondiente.

Aparte de eso, siempre es una buena idea experimentar mucho con sus matrices yHacer que más grande dependerá más de sus datos en vivo, hacer que más grande dependerá más de su predicción. Rara vez un modelo es perfecto sin sintonizar.$Q$$P$$Q$$P$