// 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; }
// 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; }