Filtro Kalman extendido con escaneo láser + mapa conocido

Actualmente estoy trabajando en un proyecto para la escuela donde necesito implementar un filtro Kalman extendido para un robot con un escáner láser. El robot puede girar con un radio de giro de 0 grados y avanzar. Todos los movimientos son lineales por partes (conducir, rotar, conducir).

El simulador que estamos utilizando no admite aceleración, todo el movimiento es instantáneo.

También tenemos un mapa conocido (imagen png) que necesitamos localizar. Podemos rastrear la imagen para simular escaneos láser.

Mi compañero y yo estamos un poco confundidos sobre los modelos de movimiento y sensores que necesitaremos usar.

Hasta ahora estamos modelando el estado como un vector .$(x,y,\theta)$

Estamos utilizando las ecuaciones de actualización de la siguiente manera

void kalman::predict(const nav_msgs::Odometry msg){
    this->X[0] += linear * dt * cos( X[2] ); //x
    this->X[1] += linear * dt * sin( X[2] ); //y
    this->X[2] += angular * dt; //theta

    this->F(0,2) = -linear * dt * sin( X[2] ); //t+1 ?
    this->F(1,2) =  linear * dt * cos( X[2] ); //t+1 ?

    P = F * P * F.t() + Q;

    this->linear = msg.twist.twist.linear.x;
    this->angular = msg.twist.twist.angular.z;
    return;
}

Pensamos que teníamos todo funcionando hasta que nos dimos cuenta de que olvidamos inicializar Py que era cero, lo que significa que no había ninguna corrección. Aparentemente nuestra propagación fue muy precisa ya que aún no hemos introducido ruido en el sistema.

Para el modelo de movimiento estamos utilizando la siguiente matriz para F:

$F = \begin{bmatrix}1 & 0 & -v*\Delta t*sin(\theta) \\ 0 & 1 & v*\Delta t*cos(\theta) \\ 0 & 0 & 1 \end{bmatrix}$

Como es el jacobiano de nuestras fórmulas de actualización. ¿Es esto correcto?

$x$$y$$\theta$

El otro problema era tener cómo inicializar P. Intentamos 1,10,100 y todos colocan el robot fuera del mapa en (-90, -70) cuando el mapa es solo 50x50.

El código para nuestro proyecto se puede encontrar aquí: https://github.com/en4bz/kalman/blob/master/src/kalman.cpp

Cualquier consejo es muy apreciado.

EDITAR:

En este punto, he conseguido que el filtro se estabilice con ruido de movimiento básico pero sin movimiento real. Tan pronto como el robot comienza a moverse, el filtro diverge bastante rápido y sale del mapa.

• Usar EKF para la localización basada en escaneos láser y un mapa conocido es una mala idea y no funcionará porque uno de los principales supuestos de EKF no es válido: el modelo de medición no es diferenciable.

Sugeriría buscar en la localización de Monte Carlo (filtros de partículas). Esto no solo resolverá el problema con su modelo de medición, sino que también permitirá la localización global (la postura inicial dentro del mapa es completamente desconocida).

Editar: lo siento, olvidé mencionar otro punto importante:

• $z_t = h(x_t) + N(0, Q_t)$$h(x_t)$ parte y agregando ruido gaussiano.

En primer lugar, no mencionó nada sobre qué tipo de localización está utilizando. ¿Es con correspondencias conocidas o desconocidas?

Ahora para adquirir el jacobiano en Matlab puedes hacer lo siguiente

>> syms x y a Vtran Vrotat t
>> f1 = x + Vtran*t*cos(a);
>> f2 = y + Vtran*t*sin(a);
>> f3 = a + Vrotat*t;
>> input  = [x y a];
>> output = [f1 f2 f3];
>> F = jacobian(output, input)

El resultado

F =
[ 1, 0, -Vtran*t*sin(a)]
[ 0, 1,  Vtran*t*cos(a)]
[ 0, 0,               1]

El filtro Kalman extendido requiere que el sistema sea lineal y que el ruido sea gaussiano. El sistema aquí significa que los modelos de movimiento y observación deben ser lineales. Los sensores láser proporcionan el rango y el ángulo hacia un punto de referencia desde el marco del robot, por lo que el modelo de medición no es lineal. Aproximadamente P, si supone que es grande, su estimador de EKF será pobre al principio y depende de las mediciones porque el vector de estado anterior no está disponible. Sin embargo, una vez que su robot comience a moverse y detectar, EKF mejorará porque utiliza los modelos de movimiento y medición para estimar la postura del robot. Si su robot no detecta ningún punto de referencia, la incertidumbre aumenta drásticamente. Además, debe tener cuidado con el ángulo. En C ++,cos and sin, el ángulo debe estar en radianes. No hay nada en su código sobre este problema. Si está asumiendo el ruido del ángulo en grados mientras realiza el cálculo en radianes, puede obtener resultados extraños porque un grado como ruido, mientras que los cálculos en radianes se consideran enormes.