public static void mixture() {
Graph g = new Graph();
int cnt = 5;
for (int i = 0; i < cnt; i++) {
GXYTNode gn = new GXYTNode();
gn.init = new double[] {i, 0, 0};
gn.state = LinAlg.copy(gn.init);
gn.truth = new double[3];
g.nodes.add(gn);
if (i > 0) {
double sig = 0.06;
// wheel slippage mode
GXYTEdge ge1 = new GXYTEdge();
ge1.z = new double[] {0, 0, 0};
ge1.P = LinAlg.diag(new double[] {sig, sig, sig});
ge1.nodes = new int[] {i - 1, i};
// nominal odometry
GXYTEdge ge2 = new GXYTEdge();
ge2.z = new double[] {1, 0, 0};
ge2.P = LinAlg.diag(new double[] {sig, sig, sig});
ge2.nodes = new int[] {i - 1, i};
GEdgeMixture ge = new GEdgeMixture(new GEdge[] {ge1, ge2}, new double[] {0.05, 0.95});
g.edges.add(ge);
}
}
if (true) {
GXYTEdge ge = new GXYTEdge();
ge.z = new double[] {cnt, 0, 0};
ge.P = LinAlg.diag(new double[] {0.1, 0.1, 0.1});
ge.nodes = new int[] {0, g.nodes.size() - 1};
g.edges.add(ge);
}
try {
g.write("mixture.graph");
} catch (IOException ex) {
System.out.println("ex: " + ex);
}
}
public static void nomixture() {
Graph g = new Graph();
int cnt = 5;
double u = 0.95;
double sig = 0.1075;
for (int i = 0; i < cnt; i++) {
GXYTNode gn = new GXYTNode();
gn.init = new double[] {i, 0, 0};
gn.state = LinAlg.copy(gn.init);
g.nodes.add(gn);
if (i > 0) {
GXYTEdge ge = new GXYTEdge();
ge.z = new double[] {0.95, 0, 0};
ge.P = LinAlg.diag(new double[] {sig, sig, sig});
ge.nodes = new int[] {i - 1, i};
g.edges.add(ge);
}
}
if (true) {
GXYTEdge ge = new GXYTEdge();
ge.z = new double[] {0, 0, 0};
ge.P = LinAlg.diag(new double[] {0.1, 0.1, 0.1});
ge.nodes = new int[] {0, g.nodes.size() - 1};
g.edges.add(ge);
}
try {
g.write("nomixture.graph");
} catch (IOException ex) {
System.out.println("ex: " + ex);
}
}