Esempio n. 1
0
  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);
    }
  }
Esempio n. 2
0
  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);
    }
  }