private void magObservationFunctionForNumericalJacobian(Matrix3x3d so3SensorFromWorldPred, Vector3d result)
{
Matrix3x3d.mult(so3SensorFromWorldPred, north, mh);
So3Util.sO3FromTwoVec(mh, mz, magObservationFunctionForNumericalJacobianTempM);
So3Util.muFromSO3(magObservationFunctionForNumericalJacobianTempM, result);
}
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;
}
public double[] getPredictedGLMatrix(double secondsAfterLastGyroEvent)
{
double dT = secondsAfterLastGyroEvent;
Vector3d pmu = getPredictedGLMatrixTempV1;
pmu.set(lastGyro[0] * -dT, lastGyro[1] * -dT, lastGyro[2] * -dT);
Matrix3x3d so3PredictedMotion = getPredictedGLMatrixTempM1;
So3Util.sO3FromMu(pmu, so3PredictedMotion);
Matrix3x3d so3PredictedState = getPredictedGLMatrixTempM2;
Matrix3x3d.mult(so3PredictedMotion, so3SensorFromWorld, so3PredictedState);
return(glMatrixFromSo3(so3PredictedState));
}
public void processMag(float[] mag, long sensorTimeStamp)
{
mz.set(mag[0], mag[1], mag[2]);
mz.normalize();
Vector3d downInSensorFrame = new Vector3d();
so3SensorFromWorld.getColumn(2, downInSensorFrame);
Vector3d.cross(mz, downInSensorFrame, processMagTempV1);
Vector3d perpToDownAndMag = processMagTempV1;
perpToDownAndMag.normalize();
Vector3d.cross(downInSensorFrame, perpToDownAndMag, processMagTempV2);
Vector3d magHorizontal = processMagTempV2;
magHorizontal.normalize();
mz.set(magHorizontal);
if (sensorTimeStampMag != 0L)
{
magObservationFunctionForNumericalJacobian(so3SensorFromWorld, mNu);
double eps = 1.0E-07D;
for (int dof = 0; dof < 3; dof++)
{
Vector3d delta = processMagTempV3;
delta.setZero();
delta.setComponent(dof, eps);
So3Util.sO3FromMu(delta, processMagTempM1);
Matrix3x3d.mult(processMagTempM1, so3SensorFromWorld, processMagTempM2);
magObservationFunctionForNumericalJacobian(processMagTempM2, processMagTempV4);
Vector3d withDelta = processMagTempV4;
Vector3d.sub(mNu, withDelta, processMagTempV5);
processMagTempV5.scale(1.0D / eps);
mH.setColumn(dof, processMagTempV5);
}
mH.transpose(processMagTempM4);
Matrix3x3d.mult(mP, processMagTempM4, processMagTempM5);
Matrix3x3d.mult(mH, processMagTempM5, processMagTempM6);
Matrix3x3d.add(processMagTempM6, mR, mS);
mS.invert(processMagTempM4);
mH.transpose(processMagTempM5);
Matrix3x3d.mult(processMagTempM5, processMagTempM4, processMagTempM6);
Matrix3x3d.mult(mP, processMagTempM6, mK);
Matrix3x3d.mult(mK, mNu, mx);
Matrix3x3d.mult(mK, mH, processMagTempM4);
processMagTempM5.setIdentity();
processMagTempM5.minusEquals(processMagTempM4);
Matrix3x3d.mult(processMagTempM5, mP, processMagTempM4);
mP.set(processMagTempM4);
So3Util.sO3FromMu(mx, so3LastMotion);
Matrix3x3d.mult(so3LastMotion, so3SensorFromWorld, processMagTempM4);
so3SensorFromWorld.set(processMagTempM4);
updateCovariancesAfterMotion();
}
else
{
magObservationFunctionForNumericalJacobian(so3SensorFromWorld, mNu);
So3Util.sO3FromMu(mx, so3LastMotion);
Matrix3x3d.mult(so3LastMotion, so3SensorFromWorld, processMagTempM4);
so3SensorFromWorld.set(processMagTempM4);
updateCovariancesAfterMotion();
}
sensorTimeStampMag = sensorTimeStamp;
}
