Filtro Kalman estendido com varredura a laser + mapa conhecido

Atualmente, estou trabalhando em um projeto para a escola em que preciso implementar um filtro Kalman estendido para um robô pontual com um scanner a laser. O robô pode girar com raio de giro de 0 grau e avançar. Todos os movimentos são lineares por partes (acionar, girar, acionar).

O simulador que estamos usando não suporta aceleração, todo movimento é instantâneo.

Também temos um mapa conhecido (imagem png) em que precisamos localizar. Podemos traçar um raio na imagem para simular digitalizações a laser.

Meu parceiro e eu estamos um pouco confusos quanto aos modelos de movimento e sensor que precisaremos usar.

Até agora, estamos modelando o estado como um vetor .$(x,y,\theta)$

Estamos usando as equações de atualização da seguinte maneira

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 tínhamos tudo funcionando até percebermos que esquecemos de inicializar Pe que era zero, significando que não havia nenhuma correção acontecendo. Aparentemente, nossa propagação foi muito precisa, pois ainda não introduzimos ruído no sistema.

Para o modelo de movimento, estamos usando a seguinte 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 é o jacobiano das nossas fórmulas de atualização. Isso está correto?

Para o modelo de sensor, estamos aproximando o jacobiano (H) tomando diferenças finitas das posições , e dos robôs e traçado de raios no mapa. Conversamos com a AT que disse que isso iria funcionar, mas ainda não tenho certeza. Nosso professor está fora, então não podemos perguntar a ele infelizmente. Estamos usando 3 medições a laser por etapa de correção, portanto H é 3x3.$x$$y$$\theta$

A outra questão foi sobre como inicializar P. Tentamos 1.10.100 e todos colocam o robô fora do mapa em (-90, -70) quando o mapa tem apenas 50x50.

O código para nosso projeto pode ser encontrado aqui: https://github.com/en4bz/kalman/blob/master/src/kalman.cpp

Qualquer conselho é muito apreciado.

EDITAR:

Neste ponto, consegui estabilizar o filtro com ruído de movimento básico, mas sem movimento real. Assim que o robô começa a se mover, o filtro diverge rapidamente e sai do mapa.

• Usar o EKF para localização com base em digitalizações a laser e um mapa conhecido é uma péssima idéia e não funcionará porque uma das principais suposições do EKF não é válida: O modelo de medição não é diferenciável.

Eu sugeriria olhar para a localização de Monte Carlo (filtros de partículas). Isso não apenas resolverá o problema com o seu modelo de medição, mas também permitirá a localização global (pose inicial dentro do mapa completamente desconhecida).

Edit: Me desculpe, eu esqueci de mencionar outro ponto importante:

• Se você deseja aplicar o EKF, seu modelo de medição e seu modelo de movimento podem incluir apenas ruído gaussiano . Isso significa que você precisa anotar seu modelo de medição como . Essa é uma limitação severa, pois não permite um modelo um pouco mais complexo, como o beam_range_finder_model na Probabilistic Robotics, que também considera objetos dinâmicos na frente do robô, medições inválidas (máx) e leituras completamente aleatórias. Você ficaria preso com raios de para a parte e adicionando ruído gaussiano.$z_t = h(x_t) + N(0, Q_t)$$h(x_t)$

Antes de tudo, você não mencionou nada sobre o tipo de localização que está usando. É com correspondências conhecidas ou desconhecidas?

Agora, para adquirir o jacobiano em Matlab, você pode fazer o seguinte

>> 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)

O resultado

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

O filtro Kalman estendido exige que o sistema seja linear e que o ruído seja gaussiano. O sistema aqui significa que os modelos de movimento e observação devem ser lineares. Os sensores a laser fornecem o alcance e o ângulo em direção a um ponto de referência a partir da estrutura do robô, para que o modelo de medição não seja linear. Sobre P, se você assumir que ele é grande, seu estimador de EKF será ruim no início e dependerá das medições porque o vetor de estado anterior não está disponível. No entanto, quando o robô começar a se mover e sentir, o EKF ficará melhor, pois usa os modelos de movimento e medição para estimar a pose do robô. Se o seu robô não estiver detectando pontos de referência, a incerteza aumenta drasticamente. Além disso, você precisa ter cuidado com o ângulo. Em C ++,cos and sin, o ângulo deve estar em radiano. Não há nada no seu código sobre esse problema. Se você estiver assumindo o ruído do ângulo em grau enquanto calcula em radiano, poderá obter resultados estranhos, porque um grau como ruído enquanto os cálculos em radiano são considerados enormes.