En küçük kareler haritasına katılma

Burada çok fazla arka plan var, soru için aşağıya kaydırın

Ben Doğrusal En Küçük Kareler Sorunundan SLAM Ne Kadar Uzakta anlatılan harita birleştirme algoritması deniyorum ; özellikle formül (36). Yazdığım kod her zaman dönüm noktası konumları için ikinci haritanın değerlerini alıyor gibi görünüyor. Sorum şu: Metni doğru mu anlıyorum yoksa bir tür hata mı yapıyorum? Formülleri anladığım gibi açıklamaya çalışacağım ve kodumun bunu nasıl uyguladığını göstereceğim. Sadece iki yerel haritaya katılmanın basit bir örneğini yapmaya çalışıyorum.

Makaleden (36), iki yerel haritanın birleştirilmesinin en aza indiren bir durum vektörü olduğunu bulduğu belirtilmektedir :$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}))$$

İki yerel haritalar için Expanded ve ^ X L 2 Ben:$\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}))$$

Bunu anlamak gibi, yani bir submap, küresel bir harita için entegre bir gözlem olarak izlenebilir gürültü EKF I yöntem gürültü olmanın aksine submap ilişkilidir (submap yapmak için kullanılan ihtiva edebilir veya farklı olmayabilir).$P^L_j$

vektörü ilk haritadan poz, ikinci haritadan poz ve her iki haritadaki yer işaretlerinin birleşimidir.$X_{join,rel}$

Fonksiyonu olduğu:$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}$$

Aşağıdaki değerlendirmemin doğru olduğuna ikna olmadım:

İlk iki unsur, bir önceki haritanın referans çerçevesinde robotun pozudur. Örneğin, harita 1 için poz, ilk karede olacaktır ; harita 2 için, harita 1 çerçevesinde olacaktır.$t_0$

Bir sonraki eleman grubu, harita 1 ve harita 2 için ortak olan ve harita 1'in referans çerçevesine dönüştürülen elementlerdir.

Son satırlar, ilk harita çerçevesinde harita 2'ye özgü özelliklerdir.

Matlab uygulamam aşağıdaki gibidir:

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

Yukarıda açıklanan uygunluk işlevinin minimumunu bulmak için optimizasyon araç kutusunu kullanıyorum . Fitness işlevinin kendisi oldukça basit olduğunu düşünüyorum. H fonksiyonu, yukarıda tarif edilen H vektörünü döndürür.

Sonuç: İki vektörde join_maps çalıştırdığımda

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)

Çıktı:

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

Soru:

Programım harita 2'nin harita birleştirme fonksiyonunun minimum olduğunu verir. Minimum harita 1 ve harita 2 arasında bir yerde olmalı gibi görünüyor. Sorunun H matrisinde olduğundan eminim. Ne yanlış yapıyorum?

Bu doğru çalışıyor gibi görünüyor ve çok daha basit bir çözüm:

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

Çıktıyı fminunc tanımına daha iyi uyacak şekilde değiştirdim.

Map_1 ve map_2 ile çıktı

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

Bu durumda, H (X) 'u çağırmaya gerek yoktur, çünkü ilk iki poz aynıdır, bu nedenle iki harita aynı referans çerçevesini paylaşır. H fonksiyonu sadece durum tahminini alt haritanın referans çerçevesine dönüştürür.