# EIF slam algorithm implementation as per Probabilistic Robotics

I want to implement EIF slam from Probabilistic Robotics Table 11.2. This is the algorithm for EIF slam.

To construct $$\Omega and \xi$$ matrix I write down a Java code. Here is my code

public class Part1Jama {
public static void main(String args[]) throws IOException {
Matrix Bigomega = new Matrix(11116,11116);//last 15 row and column define landmarks x and y axis
Matrix omega = new Matrix(11116,11116);
Matrix BigXi = new Matrix(11116, 1);
Matrix Xi = new Matrix(11116, 1);

int i = 0, i1 = 0, k1 = 0, k2 = 0, l = 0, l1 = 0,k3=0,j=0;
double[] timex = new double[11101];//11101 odometer data denoting by u_1;
double[] xcoa = new double[11101];
double[] ycoa = new double[11101];
double[]thetacoa=new double[11101];

double[] landx = new double[3095];//3095 measurment data z
double[] landy = new double[3095];
double[] timeya = new double[3095];
double[] codea = new double[3095];
double[] rangea = new double[3095];
double[] bearinga = new double[3095];
Scanner x = new Scanner(new File("/home/froboticscse/IdeaProjects/UJMPtest/src/main/java/measurmentF.txt"));
Scanner y = new Scanner(new File("/home/froboticscse/IdeaProjects/UJMPtest/src/main/java/uniquemeasure.txt"));

while (x.hasNext()) {
//Data from odometry data file which consist of time,forward velocity angular velocity
double time = x.nextDouble();
double xco = x.nextDouble();
double yco = x.nextDouble();
double theta=x.nextDouble();
timex[i] = time;
xcoa[i] = xco;
ycoa[i] = yco;
thetacoa[i]=theta;
i++;
}

while (y.hasNext()) {
//Data from measurement file consist of time,correspondence Number(landmark number),range,bearing
double timey = y.nextDouble();
double code = y.nextDouble();
double range = y.nextDouble();
double bearing = y.nextDouble();
timeya[i1] = timey;
codea[i1] = code;
rangea[i1] = range;
bearinga[i1] = bearing;

i1++;

}

while(k1<timex.length-1) {
if (timex[k1] < timeya[k2]) {

omega.set( k1, k1,1);
omega.set(k1, k1 + 1,-1);
omega.set( k1 + 1, k1,-1);
omega.set( k1 + 1, k1 + 1,1);

Bigomega = Bigomega.plus(omega);
omega.set( k1, k1,0);// clear value store in omega
omega.set(k1, k1 + 1,0);
omega.set( k1 + 1, k1,0);
omega.set( k1 + 1, k1 + 1,0);

Xi.set( k1, 0,-(ycoa[k1]));
Xi.set(k1 + 1, 0,ycoa[k1]);
BigXi = BigXi.plus(Xi);
Xi.set( k1, 0,0); //clear value store in Xi
Xi.set(k1 + 1, 0,0);

}
// //For line number 10 to 20 as per algorithm
else if (timex[k1] == timeya[k2]) {
landy[l1] = ycoa[k1] + rangea[k2] * (Math.cos(bearinga[k2] + thetacoa[k1]));
omega.set( k1, k1,1);
omega.set(k1, k1 + 1,-1);
omega.set( k1 + 1, k1,-1);
omega.set( k1 + 1, k1 + 1,1);
Bigomega = Bigomega.plus(omega);
omega.set( k1, k1,0);
omega.set(k1, k1 + 1,0);
omega.set( k1 + 1, k1,0);
omega.set( k1 + 1, k1 + 1,0);

Xi.set( k1, 0,-(ycoa[k1]));
Xi.set(k1 + 1, 0,ycoa[k1]);
BigXi = BigXi.plus(Xi);
Xi.set( k1, 0,0);
Xi.set(k1 + 1, 0,0);
if (codea[k2] == 90.0) {

omega.set(k1, k1,1);
omega.set( 11101, k1,-1);
omega.set(11101, 11101,1);
omega.set(k1, 11101,-1);
Bigomega = Bigomega.plus(omega);
omega.set(k1, k1,0);
omega.set( 11101, k1,0);
omega.set(11101, 11101,0);
omega.set(k1, 11101,0);

Xi.set( k1, 0,((-landy[l1]) + ycoa[k1]));
Xi.set(k1 + 1, 0,(landy[l1] + ycoa[k1]));
BigXi = BigXi.plus(Xi);
Xi.set( k1, 0,0);
Xi.set(k1 + 1, 0,0);

}
else if (codea[k2] == 18.0) {
omega.set(k1, k1,1);
omega.set( 11102, k1,-1);
omega.set(11102, 11102,1);
omega.set(k1, 11102,-1);
Bigomega = Bigomega.plus(omega);
omega.set(k1, k1,0);
omega.set( 11102, k1,0);
omega.set(11102, 11102,0);
omega.set(k1, 11102,0);

Xi.set( k1, 0,((-landy[l1]) + ycoa[k1]));
Xi.set(k1 + 1, 0,(landy[l1] + ycoa[k1]));
BigXi = BigXi.plus(Xi);
Xi.set( k1, 0,0);
Xi.set(k1 + 1, 0,0);

}
else if (codea[k2] == 81.0) {

omega.set(k1, k1,1);
omega.set( 11103, k1,-1);
omega.set(11103, 11103,1);
omega.set(k1, 11103,-1);
Bigomega = Bigomega.plus(omega);
omega.set(k1, k1,0);
omega.set( 11103, k1,0);
omega.set(11103, 11103,0);
omega.set(k1, 11103,0);

Xi.set( k1, 0,((-landy[l1]) + ycoa[k1]));
Xi.set(k1 + 1, 0,(landy[l1] + ycoa[k1]));
BigXi = BigXi.plus(Xi);
Xi.set( k1, 0,0);
Xi.set(k1 + 1, 0,0);

}
else if (codea[k2] == 9.0) {

omega.set(k1, k1,1);
omega.set( 11104, k1,-1);
omega.set(11104, 11104,1);
omega.set(k1, 11104,-1);
Bigomega = Bigomega.plus(omega);
omega.set(k1, k1,0);
omega.set( 11104, k1,0);
omega.set(11104, 11104,0);
omega.set(k1, 11104,0);

Xi.set( k1, 0,((-landy[l1]) + ycoa[k1]));
Xi.set(k1 + 1, 0,(landy[l1] + ycoa[k1]));
BigXi = BigXi.plus(Xi);
Xi.set( k1, 0,0);
Xi.set(k1 + 1, 0,0);

}
else if (codea[k2] == 25.0) {

omega.set(k1, k1,1);
omega.set( 11105, k1,-1);
omega.set(11105, 11105,1);
omega.set(k1, 11105,-1);
Bigomega = Bigomega.plus(omega);
omega.set(k1, k1,0);
omega.set( 11105, k1,0);
omega.set(11105, 11105,0);
omega.set(k1, 11105,0);

Xi.set( k1, 0,((-landy[l1]) + ycoa[k1]));
Xi.set(k1 + 1, 0,(landy[l1] + ycoa[k1]));
BigXi = BigXi.plus(Xi);
Xi.set( k1, 0,0);
Xi.set(k1 + 1, 0,0);

}
else if (codea[k2] == 61.0) {

omega.set(k1, k1,1);
omega.set( 11106, k1,-1);
omega.set(11106, 11106,1);
omega.set(k1, 11106,-1);
Bigomega = Bigomega.plus(omega);
omega.set(k1, k1,0);
omega.set( 11106, k1,0);
omega.set(11106, 11106,0);
omega.set(k1, 11106,0);

Xi.set( k1, 0,((-landy[l1]) + ycoa[k1]));
Xi.set(k1 + 1, 0,(landy[l1] + ycoa[k1]));
BigXi = BigXi.plus(Xi);
Xi.set( k1, 0,0);
Xi.set(k1 + 1, 0,0);

}
else if (codea[k2] == 72.0) {

omega.set(k1, k1,1);
omega.set( 11107, k1,-1);
omega.set(11107, 11107,1);
omega.set(k1, 11107,-1);
Bigomega = Bigomega.plus(omega);
omega.set(k1, k1,0);
omega.set( 11107, k1,0);
omega.set(11107, 11107,0);
omega.set(k1, 11107,0);

Xi.set( k1, 0,((-landy[l1]) + ycoa[k1]));
Xi.set(k1 + 1, 0,(landy[l1] + ycoa[k1]));
BigXi = BigXi.plus(Xi);
Xi.set( k1, 0,0);
Xi.set(k1 + 1, 0,0);

}
else if (codea[k2] == 36.0) {

omega.set(k1, k1,1);
omega.set( 11108, k1,-1);
omega.set(11108, 11108,1);
omega.set(k1, 11108,-1);
Bigomega = Bigomega.plus(omega);
omega.set(k1, k1,0);
omega.set( 11108, k1,0);
omega.set(11108, 11108,0);
omega.set(k1, 11108,0);

Xi.set( k1, 0,((-landy[l1]) + ycoa[k1]));
Xi.set(k1 + 1, 0,(landy[l1] + ycoa[k1]));
BigXi = BigXi.plus(Xi);
Xi.set( k1, 0,0);
Xi.set(k1 + 1, 0,0);

}
else if (codea[k2] == 70.0) {

omega.set(k1, k1,1);
omega.set( 11109, k1,-1);
omega.set(11109, 11109,1);
omega.set(k1, 11109,-1);
Bigomega = Bigomega.plus(omega);
omega.set(k1, k1,0);
omega.set( 11109, k1,0);
omega.set(11109, 11109,0);
omega.set(k1, 11109,0);

Xi.set( k1, 0,((-landy[l1]) + ycoa[k1]));
Xi.set(k1 + 1, 0,(landy[l1] + ycoa[k1]));
BigXi = BigXi.plus(Xi);
Xi.set( k1, 0,0);
Xi.set(k1 + 1, 0,0);

}
else if (codea[k2] == 27.0) {

omega.set(k1, k1,1);
omega.set( 11110, k1,-1);
omega.set(11110, 11110,1);
omega.set(k1, 11110,-1);
Bigomega = Bigomega.plus(omega);
omega.set(k1, k1,0);
omega.set( 11110, k1,0);
omega.set(11110, 11110,0);
omega.set(k1, 11110,0);

Xi.set( k1, 0,((-landy[l1]) + ycoa[k1]));
Xi.set(k1 + 1, 0,(landy[l1] + ycoa[k1]));
BigXi = BigXi.plus(Xi);
Xi.set( k1, 0,0);
Xi.set(k1 + 1, 0,0);

}
else if (codea[k2] == 54.0) {

omega.set(k1, k1,1);
omega.set( 11111, k1,-1);
omega.set(11111, 11111,1);
omega.set(k1, 11111,-1);
Bigomega = Bigomega.plus(omega);
omega.set(k1, k1,0);
omega.set( 11111, k1,0);
omega.set(11111, 11111,0);
omega.set(k1, 11111,0);

Xi.set( k1, 0,((-landy[l1]) + ycoa[k1]));
Xi.set(k1 + 1, 0,(landy[l1] + ycoa[k1]));
BigXi = BigXi.plus(Xi);
Xi.set( k1, 0,0);
Xi.set(k1 + 1, 0,0);

}
else if (codea[k2] == 16.0) {

omega.set(k1, k1,1);
omega.set( 11112, k1,-1);
omega.set(11112, 11112,1);
omega.set(k1, 11112,-1);
Bigomega = Bigomega.plus(omega);
omega.set(k1, k1,0);
omega.set( 11112, k1,0);
omega.set(11112, 11112,0);
omega.set(k1, 11112,0);

Xi.set( k1, 0,((-landy[l1]) + ycoa[k1]));
Xi.set(k1 + 1, 0,(landy[l1] + ycoa[k1]));
BigXi = BigXi.plus(Xi);
Xi.set( k1, 0,0);
Xi.set(k1 + 1, 0,0);

}
else if (codea[k2] == 45.0) {

omega.set(k1, k1,1);
omega.set( 11113, k1,-1);
omega.set(11113, 11113,1);
omega.set(k1, 11113,-1);
Bigomega = Bigomega.plus(omega);
omega.set(k1, k1,0);
omega.set( 11113, k1,0);
omega.set(11113, 11113,0);
omega.set(k1, 11113,0);

Xi.set( k1, 0,((-landy[l1]) + ycoa[k1]));
Xi.set(k1 + 1, 0,(landy[l1] + ycoa[k1]));
BigXi = BigXi.plus(Xi);
Xi.set( k1, 0,0);
Xi.set(k1 + 1, 0,0);

}
else if (codea[k2] == 7.0) {

omega.set(k1, k1,1);
omega.set( 11114, k1,-1);
omega.set(11114, 11114,1);
omega.set(k1, 11114,-1);
Bigomega = Bigomega.plus(omega);
omega.set(k1, k1,0);
omega.set( 11114, k1,0);
omega.set(11114, 11114,0);
omega.set(k1, 11114,0);

Xi.set( k1, 0,((-landy[l1]) + ycoa[k1]));
Xi.set(k1 + 1, 0,(landy[l1] + ycoa[k1]));
BigXi = BigXi.plus(Xi);
Xi.set( k1, 0,0);
Xi.set(k1 + 1, 0,0);

}
else if (codea[k2] == 63.0) {

omega.set(k1, k1,1);
omega.set( 11115, k1,-1);
omega.set(11115, 11115,1);
omega.set(k1, 11115,-1);
Bigomega = Bigomega.plus(omega);
omega.set(k1, k1,0);
omega.set( 11115, k1,0);
omega.set(11115, 11115,0);
omega.set(k1, 11115,0);

Xi.set( k1, 0,((-landy[l1]) + ycoa[k1]));
Xi.set(k1 + 1, 0,(landy[l1] + ycoa[k1]));
BigXi = BigXi.plus(Xi);
Xi.set( k1, 0,0);
Xi.set(k1 + 1, 0,0);

}

if(k2<timeya.length-1) {
k2++;

}
l++;
} else  if(timex[k1] > timeya[k2]) {
omega.set(k1, k1,1);
omega.set(k1, k1 + 1,-1);
omega.set(k1 + 1, k1,-1);
omega.set( k1 + 1, k1 + 1,1);
Bigomega = Bigomega.plus(omega);
omega.set(k1, k1,0);
omega.set(k1, k1 + 1,0);
omega.set(k1 + 1, k1,0);
omega.set( k1 + 1, k1 + 1,0);

Xi.set( k1, 0,-(ycoa[k1]));
Xi.set(k1 + 1, 0,ycoa[k1]);
BigXi = BigXi.plus(Xi);
Xi.set( k1, 0,0);
Xi.set(k1 + 1, 0,0);

}
k1++;
}
Bigomega.set(0,0,2);

Matrix mu=Bigomega.inverse().times(BigXi);
mu.print(3,6);

}

}

In the java code xcoa,ycoa,thetacoa get from line no.5(Algorithm EIF-construct). I create another java file to save the values that I get from line no: 5.Then I use it here. I am not very sure how to construct line no:6 and 7 so I assume that our world is noise free and use 1 and -1 for to represent it between two pose. Say , Robot go from $$x_t to x_{t+1}$$ so the cell between them the diagonal cell represent with 1 and off diagonal -1.

while(k1<timex.length-1) {
if (timex[k1] < timeya[k2]) {

omega.set( k1, k1,1);
omega.set(k1, k1 + 1,-1);
omega.set( k1 + 1, k1,-1);
omega.set( k1 + 1, k1 + 1,1);

Bigomega = Bigomega.plus(omega);
omega.set( k1, k1,0); //clear value store in omega
omega.set(k1, k1 + 1,0);
omega.set( k1 + 1, k1,0);
omega.set( k1 + 1, k1 + 1,0);

Xi.set( k1, 0,-(ycoa[k1]));// construct $$\xi$$ vector for y co - ordinate. Alternation of line 19. As fail to understand how to create $$\mu_{tx},\mu_{ty},\mu_{t\theta},\mu_{jx}\mu_{jy}$$
Xi.set(k1 + 1, 0,ycoa[k1]);
BigXi = BigXi.plus(Xi);
Xi.set( k1, 0,0); //clear value store is xi
Xi.set(k1 + 1, 0,0);

}

90.0,54.0,27.0,81.0.... they are the landmark numbers. As per my code same code run thrice for x,y,theta and then plot all of them to get a map and path. The code I write here is run for y co because $$\xi$$ is set as * Xi.set( k1, 0,-(ycoa[k1])); Xi.set(k1 + 1, 0,ycoa[k1]); If I write down Xi.set( k1, 0,-(xcoa[k1])*); Xi.set(k1 + 1, 0,xcoa[k1]); it will give me x coordinates. So I modify this code in this way.

The problem with this code is Matrix inversion is difficult.

Looking for the suggestion how to implement the code as describe in algorithm?

What is the need of Algorithm EIF_reduce as per table 11.3? I have only 15 landmark. So I calculate $$\mu$$ at the end of this code block. Am I wrong?

• I'm having trouble following along with your question, but you say the problem with this code is Matrix inversion is difficult, but I'm not seeing an matrix inversion happening here offhand. I'm assuming the code and if...elseif statements in your code are all for the for all observed features line in the algorithm? – Chuck Sep 25 '18 at 13:45
• Yes there is a matrix inversion just before the last line. Matrix mu=Bigomega.inverse().times(BigXi); – Encipher Sep 25 '18 at 17:46