Filtro Kalman esteso con scansione laser + mappa nota

Attualmente sto lavorando a un progetto per la scuola in cui devo implementare un filtro Kalman esteso per un robot punto con uno scanner laser. Il robot può ruotare con raggio di sterzata di 0 gradi e avanzare. Tutti i movimenti sono lineari a tratti (guida, ruota, guida).

Il simulatore che stiamo utilizzando non supporta l'accelerazione, tutto il movimento è istantaneo.

Abbiamo anche una mappa nota (immagine png) in cui dobbiamo localizzarci. Possiamo tracciare i raggi nell'immagine per simulare le scansioni laser.

Il mio compagno e io siamo un po 'confusi per quanto riguarda i modelli di movimento e sensore che dovremo usare.

Finora stiamo modellando lo stato come vettore .$(x,y,\theta)$

Stiamo usando le equazioni di aggiornamento come segue

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;
}

Pensavamo di avere tutto funzionato fino a quando non ci siamo resi conto che ci siamo dimenticati di inizializzare Pe che era zero, il che significa che non si stava verificando alcuna correzione. Apparentemente la nostra propagazione è stata molto accurata poiché non abbiamo ancora introdotto rumore nel sistema.

Per il modello di movimento stiamo usando la seguente matrice per F:

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

Essendo il giacobino delle nostre formule di aggiornamento. È corretto?

$x$$y$$\theta$

L'altro problema di come inizializzare P. Abbiamo provato 1,10,100 e tutti posizionano il robot fuori dalla mappa a (-90, -70) quando la mappa è solo 50x50.

Il codice per il nostro progetto è disponibile qui: https://github.com/en4bz/kalman/blob/master/src/kalman.cpp

Ogni consiglio è molto apprezzato.

MODIFICARE:

A questo punto ho ottenuto il filtro per stabilizzarsi con un rumore di movimento di base ma nessun movimento reale. Non appena il robot inizia a muoversi, il filtro diverge abbastanza rapidamente ed esce dalla mappa.

• L'uso di EKF per la localizzazione basata su scansioni laser e una mappa nota è una cattiva idea e non funzionerà perché uno dei presupposti principali di EKF non è valido: il modello di misurazione non è differenziabile.

Suggerirei di esaminare la localizzazione di Monte Carlo (filtri antiparticolato). Ciò non solo risolverà il problema con il modello di misurazione, ma consentirà anche la localizzazione globale (posizione iniziale all'interno della mappa completamente sconosciuta).

Modifica: mi dispiace, ho dimenticato di menzionare un altro punto importante:

• $z_t = h(x_t) + N(0, Q_t)$$h(x_t)$ parte e l'aggiunta di rumore gaussiano.

Prima di tutto, non hai menzionato nulla del tipo di localizzazione che stai utilizzando. È con corrispondenze conosciute o sconosciute?

Ora per acquisire il giacobino in Matlab puoi fare quanto segue

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

Il risultato

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

Il filtro Kalman esteso richiede che il sistema sia lineare e che il rumore sia gaussiano. Il sistema qui significa che i modelli di movimento e di osservazione devono essere lineari. I sensori laser forniscono la distanza e l'angolo verso un punto di riferimento dal telaio del robot, quindi il modello di misurazione non è lineare. A proposito P, se si assume che sia grande, lo stimatore EKF sarà scarso all'inizio e si basa sulle misurazioni perché il vettore di stato precedente non è disponibile. Tuttavia, una volta che il robot inizia a muoversi e rilevare, EKF migliorerà perché utilizza i modelli di movimento e misurazione per stimare la posa del robot. Se il tuo robot non rileva alcun punto di riferimento, l'incertezza aumenta drasticamente. Inoltre, devi stare attento all'angolo. In C ++,cos and sin, l'angolo dovrebbe essere in radianti. Non c'è nulla nel tuo codice su questo problema. Se stai assumendo il rumore dell'angolo in gradi durante il tuo calcolo in radianti, potresti ottenere strani risultati perché un grado come rumore mentre i calcoli in radianti sono considerati enormi.