我有两个数据文件，一个是机器人里程计数据集，另一个是机器人传感器数据集。我只展示了这两个文件的一部分。机器人里程表
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
传感器数据集
Time CNo: Range Bearing
12.720000 90.000000 2.148000 0.025000
现在，在分析这个数据集后，我们可以看到里程表数据的第 8 个（从 0 开始）数据与传感器数据相同。
在我的程序中，我构造了一个 Omega 矩阵，它在对应行和列的每个 xt,xt-1 时间戳的非对角线中添加 -1，在对角线中添加 +1。当它在传感器数据集中遇到相同的时间戳时，它为该数据固定一行并更新该行和里程表数据集的相应列。我选择合并传感器测量数据的最后一行（第 13 行）。并更新该测量数据时间戳与里程表数据相同的列（列号：8）。
我的代码
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);
}
}
我的输出是：
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
如果比较两个数据文件，您可以看到里程表数据第 8 行与传感器数据行相同。我的程序说，如果里程表数据的时间戳小于（初始化计数器 k1）传感器数据的时间戳do this。如果里程表数据的时间戳==传感器数据的时间戳（k2循环计数器初始化）do that。在等于语句执行 k2 循环计数器递增之后，该语句再次进入 < 决策。所以在按照逻辑执行第 8 行之后
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 语句。