Exemplo n.º 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;
  }
Exemplo n.º 2
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;
  }