I have two data file one is for Robot odometry dataset and another one is robot sensor data set. I just show a portion of those two file. Robot odometer
Time forward velocity angular velocity
12.560000 0.067000 0.000000
12.580000 0.067000 0.000000
12.600000 0.067000 0.000000
12.620000 0.067000 0.000000
12.640000 0.067000 0.000000
12.660000 0.067000 0.000000
12.680000 0.033500 0.000000
12.700000 0.067000 0.000000
12.720000 0.067000 0.000000
12.740000 0.067000 0.000000
12.760000 0.067000 0.000000
12.780000 0.067000 0.000000
12.800000 0.067000 0.000000
Sensor Data set
Time CNo: Range Bearing
12.720000 90.000000 2.148000 0.025000
Now, After analysis this data set we can see that 8th(starting from 0) data of odometer data is same as sensor data.
In my program I construct a Omega matrix which add -1 in the off diagonal and +1 in the diagonal of each xt,xt-1 timestamp of corresponding row and column. When it encounter the same timestamp in sensor data set, it fixed a row for this data and update this row and corresponding column of odometer data set. I choose the last row(row no. 13) where I incorporate sensor measurement data. And update that column where this measurement data timestamp same as odometer data(Column No:8).
My code
import org.ujmp.core.Matrix;
import org.ujmp.core.SparseMatrix;
import java.io.File;
import java.io.IOException;
import java.io.PrintWriter;
import java.util.Scanner;
public class Test9 {
public static void main(String args[]) throws IOException {
Matrix Bigomega=Matrix.Factory.zeros(14,14);
Matrix omega = SparseMatrix.Factory.zeros(14, 14);
int i = 0, i1 = 0, k1 = 0, k2 = 0,l=0,l1=0;
double[] timex = new double[14];
double[] forwardx = new double[14];
double[] angularx = new double[14];
double[]x1=new double[14];
double[]y1=new double[14];
double[]theta1=new double[14];
double []landx=new double[1];
double[]landy=new double[1];
double[] timeya = new double[1];
double[] codea = new double[1];
double[] rangea = new double[1];
double[] bearinga = new double[1];
Scanner x = new Scanner(new File("/home/froboticscse/IdeaProjects/UJMPtest/src/main/java/rtest.txt"));
Scanner y = new Scanner(new File("/home/froboticscse/IdeaProjects/UJMPtest/src/main/java/mtese.txt"));
while (x.hasNext()) {
double time = x.nextDouble();
double forward = x.nextDouble();
double angular = x.nextDouble();
timex[i] = time;
forwardx[i] = forward;
angularx[i] = angular;
x1[i] = ((forwardx[i] * 0.006 + Math.cos(0 + (angularx[i] * 0.006) / 2)));
y1[i] = ((forwardx[i] * 0.006 + Math.sin(0 + (angularx[i] * 0.006) / 2)));
theta1[i] = (angularx[i] * 0.006);
i++;
}
while (y.hasNext()) {
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 && k2 < timeya.length) {
if (timex[k1] < timeya[k2]) {
omega.setAsDouble(1, k1, k1);
omega.setAsDouble(-1, k1, k1 + 1);
omega.setAsDouble(-1,k1+1,k1);
omega.setAsDouble(1,k1+1,k1+1);
Bigomega = Bigomega.plus(omega);
omega.clear();
k1++;
l++;
}
else if (timex[k1] == timeya[k2]) {
omega.setAsDouble(1, k1, k1);
omega.setAsDouble(-1, k1, k1 + 1);
omega.setAsDouble(-1,k1+1,k1);
omega.setAsDouble(1,k1+1,k1+1);
Bigomega=Bigomega.plus(omega);
omega.clear();
if (codea[k2] == 90.0) {
omega.setAsDouble(1,k1,k1);
omega.setAsDouble(-1, 13, k1);
omega.setAsDouble(1,13,13);
omega.setAsDouble(-1, k1, 13);
}
Bigomega = Bigomega.plus(omega);
omega.clear();
BigXi=BigXi.plus(Xi);
Xi.clear();
k2++;
l1++;
}
}
System.out.println(Bigomega);
}
}
My output is:
1.0000 -1.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000
-1.0000 2.0000 -1.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000
0.0000 -1.0000 2.0000 -1.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000
0.0000 0.0000 -1.0000 2.0000 -1.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000
0.0000 0.0000 0.0000 -1.0000 2.0000 -1.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000
0.0000 0.0000 0.0000 0.0000 -1.0000 2.0000 -1.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000
0.0000 0.0000 0.0000 0.0000 0.0000 -1.0000 2.0000 -1.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000
0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 -1.0000 2.0000 -1.0000 0.0000 0.0000 0.0000 0.0000 0.0000
0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 -1.0000 3.0000 -1.0000 0.0000 0.0000 0.0000 -1.0000
0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 -1.0000 1.0000 0.0000 0.0000 0.0000 0.0000
0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000
0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000
0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000
0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 -1.0000 0.0000 0.0000 0.0000 0.0000 1.0000
If you compare two data file you can see that odometer data 8th row is same with sensor data row. My program say that if timestamp of odometer data less than(initialize counter k1) timestamp of sensor data do this. If the timestamp of odometer data== timestamp of sensor data(k2 loop counter initialize) do that. After equal to statement execute k2 loop counter increment then the statement again come into < decision. So after execution of 8th row as per logic
if (timex[k1] < timeya[k2]) {
omega.setAsDouble(1, k1, k1);
omega.setAsDouble(-1, k1, k1 + 1);
omega.setAsDouble(-1,k1+1,k1);
omega.setAsDouble(1,k1+1,k1+1);
Bigomega = Bigomega.plus(omega);
omega.clear();
Execute. But the control never go back to this block rather it was terminated.
I cannot find out any logical flaw. But cannot understand why the control terminated. It should go back to if statement.
Aucun commentaire:
Enregistrer un commentaire