private void updateCovariancesAfterMotion() { this.so3LastMotion.transpose(this.updateCovariancesAfterMotionTempM1); Matrix3x3d.mult( this.mP, this.updateCovariancesAfterMotionTempM1, this.updateCovariancesAfterMotionTempM2); Matrix3x3d.mult(this.so3LastMotion, this.updateCovariancesAfterMotionTempM2, this.mP); this.so3LastMotion.setIdentity(); }
public synchronized void processAcc(Vector3d acc, long sensorTimeStamp) { this.mz.set(acc); updateAccelCovariance(this.mz.length()); if (this.alignedToGravity) { accObservationFunctionForNumericalJacobian(this.so3SensorFromWorld, this.mNu); double eps = 1.0E-07D; for (int dof = 0; dof < 3; dof++) { Vector3d delta = this.processAccVDelta; delta.setZero(); delta.setComponent(dof, eps); So3Util.sO3FromMu(delta, this.processAccTempM1); Matrix3x3d.mult(this.processAccTempM1, this.so3SensorFromWorld, this.processAccTempM2); accObservationFunctionForNumericalJacobian(this.processAccTempM2, this.processAccTempV1); Vector3d withDelta = this.processAccTempV1; Vector3d.sub(this.mNu, withDelta, this.processAccTempV2); this.processAccTempV2.scale(1.0D / eps); this.mH.setColumn(dof, this.processAccTempV2); } this.mH.transpose(this.processAccTempM3); Matrix3x3d.mult(this.mP, this.processAccTempM3, this.processAccTempM4); Matrix3x3d.mult(this.mH, this.processAccTempM4, this.processAccTempM5); Matrix3x3d.add(this.processAccTempM5, this.mRaccel, this.mS); this.mS.invert(this.processAccTempM3); this.mH.transpose(this.processAccTempM4); Matrix3x3d.mult(this.processAccTempM4, this.processAccTempM3, this.processAccTempM5); Matrix3x3d.mult(this.mP, this.processAccTempM5, this.mK); Matrix3x3d.mult(this.mK, this.mNu, this.mx); Matrix3x3d.mult(this.mK, this.mH, this.processAccTempM3); this.processAccTempM4.setIdentity(); this.processAccTempM4.minusEquals(this.processAccTempM3); Matrix3x3d.mult(this.processAccTempM4, this.mP, this.processAccTempM3); this.mP.set(this.processAccTempM3); So3Util.sO3FromMu(this.mx, this.so3LastMotion); Matrix3x3d.mult(this.so3LastMotion, this.so3SensorFromWorld, this.so3SensorFromWorld); updateCovariancesAfterMotion(); } else { So3Util.sO3FromTwoVec(this.down, this.mz, this.so3SensorFromWorld); this.alignedToGravity = true; } }
private void magObservationFunctionForNumericalJacobian( Matrix3x3d so3SensorFromWorldPred, Vector3d result) { Matrix3x3d.mult(so3SensorFromWorldPred, this.north, this.mh); So3Util.sO3FromTwoVec(this.mh, this.mz, this.magObservationFunctionForNumericalJacobianTempM); So3Util.muFromSO3(this.magObservationFunctionForNumericalJacobianTempM, result); }
public synchronized void processGyro(Vector3d gyro, long sensorTimeStamp) { float kTimeThreshold = 0.04F; float kdTdefault = 0.01F; if (this.sensorTimeStampGyro != 0L) { float dT = (float) (sensorTimeStamp - this.sensorTimeStampGyro) * 1.0E-09F; if (dT > 0.04F) dT = this.gyroFilterValid ? this.filteredGyroTimestep : 0.01F; else { filterGyroTimestep(dT); } this.mu.set(gyro); this.mu.scale(-dT); So3Util.sO3FromMu(this.mu, this.so3LastMotion); this.processGyroTempM1.set(this.so3SensorFromWorld); Matrix3x3d.mult(this.so3LastMotion, this.so3SensorFromWorld, this.processGyroTempM1); this.so3SensorFromWorld.set(this.processGyroTempM1); updateCovariancesAfterMotion(); this.processGyroTempM2.set(this.mQ); this.processGyroTempM2.scale(dT * dT); this.mP.plusEquals(this.processGyroTempM2); } this.sensorTimeStampGyro = sensorTimeStamp; this.lastGyro.set(gyro); }
public synchronized void setHeadingDegrees(double heading) { double currentHeading = getHeadingDegrees(); double deltaHeading = heading - currentHeading; double s = Math.sin(deltaHeading / 180.0D * 3.141592653589793D); double c = Math.cos(deltaHeading / 180.0D * 3.141592653589793D); double[][] deltaHeadingRotationVals = {{c, -s, 0.0D}, {s, c, 0.0D}, {0.0D, 0.0D, 1.0D}}; arrayAssign(deltaHeadingRotationVals, this.setHeadingDegreesTempM1); Matrix3x3d.mult(this.so3SensorFromWorld, this.setHeadingDegreesTempM1, this.so3SensorFromWorld); }
public double[] getPredictedGLMatrix(double secondsAfterLastGyroEvent) { double dT = secondsAfterLastGyroEvent; Vector3d pmu = this.getPredictedGLMatrixTempV1; pmu.set(this.lastGyro); pmu.scale(-dT); Matrix3x3d so3PredictedMotion = this.getPredictedGLMatrixTempM1; So3Util.sO3FromMu(pmu, so3PredictedMotion); Matrix3x3d so3PredictedState = this.getPredictedGLMatrixTempM2; Matrix3x3d.mult(so3PredictedMotion, this.so3SensorFromWorld, so3PredictedState); return glMatrixFromSo3(so3PredictedState); }
public synchronized void processMag(float[] mag, long sensorTimeStamp) { if (!this.alignedToGravity) { return; } this.mz.set(mag[0], mag[1], mag[2]); this.mz.normalize(); Vector3d downInSensorFrame = new Vector3d(); this.so3SensorFromWorld.getColumn(2, downInSensorFrame); Vector3d.cross(this.mz, downInSensorFrame, this.processMagTempV1); Vector3d perpToDownAndMag = this.processMagTempV1; perpToDownAndMag.normalize(); Vector3d.cross(downInSensorFrame, perpToDownAndMag, this.processMagTempV2); Vector3d magHorizontal = this.processMagTempV2; magHorizontal.normalize(); this.mz.set(magHorizontal); if (this.alignedToNorth) { magObservationFunctionForNumericalJacobian(this.so3SensorFromWorld, this.mNu); double eps = 1.0E-07D; for (int dof = 0; dof < 3; dof++) { Vector3d delta = this.processMagTempV3; delta.setZero(); delta.setComponent(dof, eps); So3Util.sO3FromMu(delta, this.processMagTempM1); Matrix3x3d.mult(this.processMagTempM1, this.so3SensorFromWorld, this.processMagTempM2); magObservationFunctionForNumericalJacobian(this.processMagTempM2, this.processMagTempV4); Vector3d withDelta = this.processMagTempV4; Vector3d.sub(this.mNu, withDelta, this.processMagTempV5); this.processMagTempV5.scale(1.0D / eps); this.mH.setColumn(dof, this.processMagTempV5); } this.mH.transpose(this.processMagTempM4); Matrix3x3d.mult(this.mP, this.processMagTempM4, this.processMagTempM5); Matrix3x3d.mult(this.mH, this.processMagTempM5, this.processMagTempM6); Matrix3x3d.add(this.processMagTempM6, this.mR, this.mS); this.mS.invert(this.processMagTempM4); this.mH.transpose(this.processMagTempM5); Matrix3x3d.mult(this.processMagTempM5, this.processMagTempM4, this.processMagTempM6); Matrix3x3d.mult(this.mP, this.processMagTempM6, this.mK); Matrix3x3d.mult(this.mK, this.mNu, this.mx); Matrix3x3d.mult(this.mK, this.mH, this.processMagTempM4); this.processMagTempM5.setIdentity(); this.processMagTempM5.minusEquals(this.processMagTempM4); Matrix3x3d.mult(this.processMagTempM5, this.mP, this.processMagTempM4); this.mP.set(this.processMagTempM4); So3Util.sO3FromMu(this.mx, this.so3LastMotion); Matrix3x3d.mult(this.so3LastMotion, this.so3SensorFromWorld, this.processMagTempM4); this.so3SensorFromWorld.set(this.processMagTempM4); updateCovariancesAfterMotion(); } else { magObservationFunctionForNumericalJacobian(this.so3SensorFromWorld, this.mNu); So3Util.sO3FromMu(this.mNu, this.so3LastMotion); Matrix3x3d.mult(this.so3LastMotion, this.so3SensorFromWorld, this.processMagTempM4); this.so3SensorFromWorld.set(this.processMagTempM4); updateCovariancesAfterMotion(); this.alignedToNorth = true; } }