Mappa dei minimi quadrati

C'è molto sfondo qui, scorri fino in fondo per la domanda

Sto provando l'algoritmo di unione delle mappe descritto in Quanto dista SLAM da un problema dei minimi quadrati lineari ; in particolare, formula (36). Il codice che ho scritto sembra prendere sempre i valori della seconda mappa per le posizioni di riferimento. La mia domanda è: sto capendo il testo correttamente o sto facendo una sorta di errore. Proverò a spiegare le formule quando le capirò e mostrerò come il mio codice lo implementa. Sto cercando di fare il semplice caso di unire solo due mappe locali.

Dal documento (36) dice che l'unione di due mappe locali sta trovando un vettore di stato che minimizza:$X_{join,rel}$

$$\sum_{j=1}^{k}(\hat{X_j^L} - H_{j,rel}(X_{join,rel}))^T(P_j^L)^{-1}(\hat{X_j^L} - H_{j,rel}(X_{join,rel}))$$

Espanso per due mappe locali e ^ X L 2 Ho:$\hat{X_1^L}$$\hat{X_2^L}$

$$(\hat{X_1^L} - H_{j,rel}(X_{join,rel}))^T(P_1^L)^{-1}(\hat{X_1^L} - H_{j,rel}(X_{join,rel})) + (\hat{X_2^L} - H_{j,rel}(X_{join,rel}))^T(P_2^L)^{-1}(\hat{X_2^L} - H_{j,rel}(X_{join,rel}))$$

A quanto ho capito, una mappa secondaria può essere vista come un'osservazione integrata per una mappa globale, quindi è il rumore associato alla mappa secondaria (invece di essere il rumore di processo nell'EKF che ho usato per creare la mappa secondaria, che può o potrebbe non essere diverso).$P^L_j$

Il vettore è la posa della prima mappa, la posa della seconda mappa e l'unione dei punti di riferimento in entrambe le mappe.$X_{join,rel}$

La funzione è:$H_{j,rel}$

$$\begin{bmatrix} X_{r_{je}}^{r_{(j-1)e}}\\ \phi_{r_{je}}^{r_{(j-1)e}}\\ R(\phi_{r_{(j-1)e}}^{r_{m_{j1}e}}) (X^{r_{m_{j1}e}}_{f_{j1}} - X^{r_{m_{j1}e}}_{r_{(j-1)e}})\\.\\.\\.\\ R(\phi_{r_{(j-1)e}}^{r_{m_{jl}e}}) (X^{r_{m_{jl}e}}_{f_{jl}} - X^{r_{m_{jl}e}}_{r_{(j-1)e}})\\ X_{f_{j(l+1)}}^{r_{j-1e}}\\ .\\.\\.\\ X_{f_{jn}}^{r_{j-1e}} \end{bmatrix}$$

Non sono convinto che la mia valutazione di seguito sia corretta:

$t_0$

Il prossimo gruppo di elementi sono quelli comuni alla mappa 1 e alla mappa 2, che vengono trasformati nella cornice di riferimento della mappa 1.

Le righe finali sono le caratteristiche uniche della mappa 2, nel riquadro della prima mappa.

La mia implementazione di matlab è la seguente:

function [G, fval, output, exitflag] = join_maps(m1, m2)
    x = [m2(1:3);m2];
    [G,fval,exitflag,output] = fminunc(@(x) fitness(x, m1, m2), x, options);
end

function G = fitness(X, m1, m2)
    m1_f = m1(6:3:end);
    m2_f = m2(6:3:end);
    common = intersect(m1_f, m2_f);
    P = eye(size(m1, 1)) * .002;
    r = X(1:2);
    a = X(3);
    X_join = (m1 - H(X, common));
    Y_join = (m2 - H(X, common));
    G = (X_join' * inv(P) * X_join) + (Y_join' * inv(P) * Y_join);
end

function H_j = H(X, com)
    a0 = X(3);
    H_j = zeros(size(X(4:end)));
    H_j(1:3) = X(4:6);
    Y = X(1:2);
    len = length(X(7:end));
    for i = 7:3:len
        id = X(i + 2);
        if find(com == id)
            H_j(i:i+1) = R(a0) * (X(i:i+1) - Y);
            H_j(i+2) = id;
        else  % new lmk
            H_j(i:i+2) = X(i:i+2);
        end
    end
end

function A = R(a)
    A = [cos(a) -sin(a); 
         sin(a)  cos(a)];
end

Sto usando la casella degli strumenti di ottimizzazione per trovare il minimo della funzione fitness descritta sopra. La funzione di fitness in sé è piuttosto semplice, credo. La funzione H restituisce il vettore H sopra descritto.

Il risultato è: quando eseguo join_maps sui due vettori

map_1 = [3.7054;1.0577;-1.9404; %robot x, y, angle
      2.5305;-1.0739;81.0000]; % landmark x, y, id
map_2 = [3.7054;1.0577;-1.9404;
         2.3402;-1.1463;81.0000]; % note the slightly different x,y

[G,fv,output,exitflag] = join_maps(map_1, map_2)

L'output è:

Warning: Gradient must be provided for trust-region algorithm;
  using line-search algorithm instead. 
> In fminunc at 341
  In join_maps at 7

Local minimum found.

Optimization completed because the size of the gradient is less than
the default value of the function tolerance.

<stopping criteria details>


Local minimum possible.

fminunc stopped because it cannot decrease the objective function
along the current search direction.

<stopping criteria details>

G = 
      3.7054
      1.0577
     -1.9404
      3.7054
      1.0577
     -1.9404
      2.3402
     -1.1463
      81.0000

 fv =
     1.3136e+07
  output = 
     iterations: 1
      funcCount: 520
       stepsize: 1.0491e-16
  firstorderopt: 1.6200e+05
      algorithm: 'medium-scale: Quasi-Newton line search'
        message: [1x362 char]
  exitflag =
   5

La domanda:

Il mio programma indica che la mappa 2 è il minimo della funzione di unione della mappa. Sembra che il minimo dovrebbe essere compreso tra la mappa 1 e la mappa 2. Sono abbastanza sicuro che il problema sia con la matrice H. Cosa sto facendo di sbagliato?

Questo sembra funzionare correttamente ed è una soluzione molto più semplice:

function [X, FVAL, EXITFLAG, OUTPUT, GRAD] = join_maps(m1, m2)
    p = [m1(1:3);m2(1:3)];
    x1 = [p;m1(4:end)];
    x2 = [p;m2(4:end)];
    guess_0 = zeros(size(x1,1),1);
    q = @(x)x'*eye(length(x))*x;
    fit = @(x)q(x1-x)+q(x2-x);
    [X,FVAL,EXITFLAG,OUTPUT,GRAD] = fminunc(fit ,guess_0);
end

Ho modificato l'output per adattarlo meglio alla descrizione di fminunc.

L'output con map_1 e map_2 è

X =
 3.7054
 1.0577
-1.9404
 3.7054
 1.0577
-1.9404
 2.4353
-1.1101
 81.0000

In questo caso, non è necessario chiamare H (X), poiché le prime due pose sono identiche, quindi le due mappe condividono lo stesso quadro di riferimento. La funzione H trasforma semplicemente la stima dello stato nel quadro di riferimento della mappa secondaria.