public /*synchronized*/ void processGyro(float[] gyro, long sensorTimeStamp) { float kTimeThreshold = 0.04F; float kdTdefault = 0.01F; if (sensorTimeStampGyro != 0L) { float dT = (float)(sensorTimeStamp - sensorTimeStampGyro) * 1.0E-09F; if (dT > kTimeThreshold) { dT = gyroFilterValid ? filteredGyroTimestep : kdTdefault; } else { filterGyroTimestep(dT); } mu.set(gyro[0] * -dT, gyro[1] * -dT, gyro[2] * -dT); So3Util.sO3FromMu(mu, so3LastMotion); processGyroTempM1.set(so3SensorFromWorld); Matrix3x3d.mult(so3LastMotion, so3SensorFromWorld, processGyroTempM1); so3SensorFromWorld.set(processGyroTempM1); updateCovariancesAfterMotion(); processGyroTempM2.set(mQ); processGyroTempM2.scale(dT * dT); mP.plusEquals(processGyroTempM2); } sensorTimeStampGyro = sensorTimeStamp; lastGyro[0] = gyro[0]; lastGyro[1] = gyro[1]; lastGyro[2] = gyro[2]; }
public /*synchronized*/ void processAcc(float[] acc, long sensorTimeStamp) { mz.set(acc[0], acc[1], acc[2]); if (sensorTimeStampAcc != 0L) { accObservationFunctionForNumericalJacobian(so3SensorFromWorld, mNu); double eps = 1.0E-07D; for (int dof = 0; dof < 3; dof++) { Vector3d delta = processAccVDelta; delta.setZero(); delta.setComponent(dof, eps); So3Util.sO3FromMu(delta, processAccTempM1); Matrix3x3d.mult(processAccTempM1, so3SensorFromWorld, processAccTempM2); accObservationFunctionForNumericalJacobian(processAccTempM2, processAccTempV1); Vector3d withDelta = processAccTempV1; Vector3d.sub(mNu, withDelta, processAccTempV2); processAccTempV2.scale(1.0D / eps); mH.setColumn(dof, processAccTempV2); } mH.transpose(processAccTempM3); Matrix3x3d.mult(mP, processAccTempM3, processAccTempM4); Matrix3x3d.mult(mH, processAccTempM4, processAccTempM5); Matrix3x3d.add(processAccTempM5, mRaccel, mS); mS.invert(processAccTempM3); mH.transpose(processAccTempM4); Matrix3x3d.mult(processAccTempM4, processAccTempM3, processAccTempM5); Matrix3x3d.mult(mP, processAccTempM5, mK); Matrix3x3d.mult(mK, mNu, mx); Matrix3x3d.mult(mK, mH, processAccTempM3); processAccTempM4.setIdentity(); processAccTempM4.minusEquals(processAccTempM3); Matrix3x3d.mult(processAccTempM4, mP, processAccTempM3); mP.set(processAccTempM3); So3Util.sO3FromMu(mx, so3LastMotion); Matrix3x3d.mult(so3LastMotion, so3SensorFromWorld, so3SensorFromWorld); updateCovariancesAfterMotion(); } else { So3Util.sO3FromTwoVec(down, mz, so3SensorFromWorld); } sensorTimeStampAcc = sensorTimeStamp; }
private static void rodriguesSo3Exp(Vector3d w, double kA, double kB, Matrix3x3d result) { double wx2 = w.x * w.x; double wy2 = w.y * w.y; double wz2 = w.z * w.z; result.set(0, 0, 1.0D - kB * (wy2 + wz2)); result.set(1, 1, 1.0D - kB * (wx2 + wz2)); result.set(2, 2, 1.0D - kB * (wx2 + wy2)); double a, b; a = kA * w.z; b = kB * (w.x * w.y); result.set(0, 1, b - a); result.set(1, 0, b + a); a = kA * w.y; b = kB * (w.x * w.z); result.set(0, 2, b + a); result.set(2, 0, b - a); a = kA * w.x; b = kB * (w.y * w.z); result.set(1, 2, b - a); result.set(2, 1, b + a); }
public static void arrayAssign(double[][] data, Matrix3x3d m) { //assert(3 == data.length); //assert(3 == data[0].length); //assert(3 == data[1].length); //assert(3 == data[2].length); m.set(data[0][0], data[0][1], data[0][2], data[1][0], data[1][1], data[1][2], data[2][0], data[2][1], data[2][2]); }
public static void generatorField(int i, Matrix3x3d pos, Matrix3x3d result) { result.set(i, 0, 0.0D); result.set((i + 1) % 3, 0, -pos.get((i + 2) % 3, 0)); result.set((i + 2) % 3, 0, pos.get((i + 1) % 3, 0)); }