Exemple #1
0
  // if we don't have anything to filter, just use Dead Reckoning
  public double[][] estimate() {
    // use new velocity estimates to estimate xyzrpy
    double[] Exyzrpy = LinAlg.scale(Vxyzrpy, T);

    // convert back to RBT
    double[][] Erbt = LinAlg.xyzrpyToMatrix(Exyzrpy);
    // XXX there is a 2nd version of the function above which one is better
    return Erbt;
  }
  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);
    }
  }
Exemple #3
0
 // weight A by alpha and B by 1-alpha if alpha == 1 then returns A
 private double[] weightedSum(double[] A, double[] B, double[] W) {
   assert ((A.length == B.length) && (A.length == W.length)) : "Warning!";
   double[] C = new double[A.length];
   for (int i = 0; i < A.length; i++) {
     C[i] = (1 - W[i]) * A[i] + W[i] * B[i];
     if (C[i] == Double.NaN) {
       System.out.println("Warning generated NaN: T = " + T);
       LinAlg.print(W);
     }
   }
   return C;
 }
Exemple #4
0
  // incorporate new information and return new estimate
  public double[][] estimate(double[][] Urbt) {
    // what rigid body transformation occured over the last frame
    double[] DUxyzrpy = LinAlg.matrixToXyzrpy(Urbt);

    // convert to velocity
    double[] Uxyzrpy;
    if (T != 0) {
      Uxyzrpy = LinAlg.scale(DUxyzrpy, 1 / T);
    } else {
      Uxyzrpy = new double[6];
    }
    /*
    System.out.println("Our observation velocity");
    LinAlg.print(Uxyzrpy);

    System.out.println("Our best estimate of velocity");
    LinAlg.print(Vxyzrpy);
     */
    // calculate relative weighting
    double[] W = constructW(Uxyzrpy);
    /*
    System.out.println("Our weighting matrix");
    LinAlg.print(W);
     */
    // incorporatenew information
    Vxyzrpy = weightedSum(Vxyzrpy, Uxyzrpy, W);
    /*
    System.out.println("After in corporating information");
    LinAlg.print(Vxyzrpy);
     */
    // use new velocity estimates to estimate xyzrpy
    double[] Exyzrpy = LinAlg.scale(Vxyzrpy, T);

    // convert back to RBT
    double[][] Erbt = LinAlg.xyzrpyToMatrix(Exyzrpy);
    // XXX there is a 2nd version of the function above which one is better
    return Erbt;
  }
  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);
    }
  }
  public void read(StructureReader ins) throws IOException {
    int ncomponents = ins.readInt();
    components = new GEdge[ncomponents];
    weights = new double[ncomponents];

    for (int i = 0; i < components.length; i++) {
      weights[i] = ins.readDouble();
      String cls = ins.readString();
      components[i] = (GEdge) ReflectUtil.createObject(cls);
      ins.blockBegin();
      components[i].read(ins);
      ins.blockEnd();
    }

    attributes = Attributes.read(ins);

    nodes = LinAlg.copy(components[0].nodes);
  }
  /** Components should all be of the same edge type. Weights should sum to 1. * */
  public GEdgeMixture(GEdge components[], double weights[]) {
    this.components = components;
    this.weights = weights;

    double total = 0;
    assert (components.length == weights.length);
    for (int i = 0; i < components.length; i++) {
      total += weights[i];
      assert (components[i].getClass().equals(components[0].getClass()));

      assert (components[i].nodes.length == components[0].nodes.length);
      for (int j = 0; j < components[0].nodes.length; j++) {
        assert (components[i].nodes[j] == components[0].nodes[j]);
      }
    }

    assert (total >= 0.99 && total <= 1.01);

    this.nodes = LinAlg.copy(components[0].nodes);
  }