private void benchmarkFunction(String name, int elements, DoubleSupplier f) { System.out.println("--- " + name + " ---"); long start = System.nanoTime(); double compressionFactor = f.getAsDouble(); long duration = System.nanoTime() - start; System.out.println("Compression factor: " + compressionFactor); System.out.println("Duration: " + Conversions.nanosecondsToSeconds(duration) + "s"); System.out.println("Time per data line: " + Conversions.nanosecondsToMilliseconds((double) (duration / elements)) + "ms"); System.out.println(); }
@Override public void receivedTimestampAndData(long timestamp) { if(++counter % displayOneInNPackets == 0) { long delay = Conversions.nanosecondsToMilliseconds(lastTimestamp - timestamp); delayValue.setText(delayFormat.format(delay)); for (int i = 0; i < jointUpdaters.size(); i++) { jointUpdaters.get(i).update(); } scs.setTime(Conversions.nanosecondsToSeconds(timestamp)); updateLocalVariables(); scs.tickAndUpdate(); } }
public void predict() { long startTime = System.nanoTime(); // State prediction. state.predict(); // Get linearized plant model and predict error covariance. state.getFMatrix(F); state.getQMatrix(Q); filterMatrixOps.predictErrorCovariance(Pprior, F, Pposterior, Q); predictionTime.set(Conversions.nanosecondsToMilliseconds((double) (System.nanoTime() - startTime))); }
public void correct() { long startTime = System.nanoTime(); // From the sensor get the linearized measurement model and the measurement residual sensor.assembleFullJacobian(H, residual, robotState); // Compute the kalman gain and correct the state sensor.getRMatrix(R); filterMatrixOps.computeKalmanGain(K, Pprior, H, R); state.getStateVector(Xprior); filterMatrixOps.updateState(Xposterior, Xprior, K, residual); // Update the error covariance. filterMatrixOps.updateErrorCovariance(Pposterior, K, H, Pprior); // Update the state data structure after the correction step. state.setStateVector(Xposterior); correctionTime.set(Conversions.nanosecondsToMilliseconds((double) (System.nanoTime() - startTime))); }
actualEstimatorDT.set(Conversions.nanosecondsToMilliseconds((double) (nanoTime - lastReadSystemTime))); lastReadSystemTime = nanoTime;
orientationEstimationTime.set(Conversions.nanosecondsToMilliseconds((double) (System.nanoTime() - startTime)));
actualControlDT.set(Conversions.nanosecondsToMilliseconds((double) (nanoTime - lastReadSystemTime))); lastReadSystemTime = nanoTime;