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