本文整理了Java中us.ihmc.kalman.YoKalmanFilter类的一些代码示例，展示了YoKalmanFilter类的具体用法。这些代码示例主要来源于Github/Stackoverflow/Maven等平台，是从一些精选项目中提取出来的代码，具有较强的参考意义，能在一定程度帮忙到你。YoKalmanFilter类的具体详情如下：
包路径：us.ihmc.kalman.YoKalmanFilter
类名称：YoKalmanFilter

YoKalmanFilter介绍

[英]Adapted from http://code.google.com/p/efficient-java-matrix-library/wiki/KalmanFilterExamples
[中]改编自http://code.google.com/p/efficient-java-matrix-library/wiki/KalmanFilterExamples

代码示例

代码示例来源：origin: us.ihmc/KalmanProject

private void populateAndConfigureYoKalmanFilter(double controlDT)
{
 yoKalmanFilter = new YoKalmanFilter(name + "YoKalman", registry);
 updateYoKalmanFilterConfiguration(controlDT);
}

代码示例来源：origin: us.ihmc/KalmanProject

public void update(DenseMatrix64F y)
{
 checkForNaN(y);
 getVariablesForUpdateFromYoVariables();
 if (updateCovarianceAndGain.getBooleanValue())
 {
   updateKalmanGainMatrixK();
 }
 
 updateAPosterioriState(x, y, K);
 storeInYoVariablesVector(x, yoX);
 if (updateCovarianceAndGain.getBooleanValue())
 {
   updateAPosterioriStateCovariance();
   storeInYoVariablesSymmetric(P, yoP);
 }
}

代码示例来源：origin: us.ihmc/ihmc-kalman-project

public void computeSteadyStateGainAndCovariance(int numberOfIterations)
  {
   getVariablesForPredictFromYoVariables();
   getVariablesForUpdateFromYoVariables();

   for (int i = 0; i < numberOfIterations; i++)
   {
     updateAPrioriCovariance();
     updateKalmanGainMatrixK();
     updateAPosterioriStateCovariance();
   }

   storeInYoVariablesSymmetric(P, yoP);
  }
}

代码示例来源：origin: us.ihmc/KalmanProject

private void doKalmanFiltering()
  {
   yoKalmanFilter.predict(inputs);
   yoKalmanFilter.update(measurements);
  }
}

代码示例来源：origin: us.ihmc/KalmanProject

public void predict(DenseMatrix64F u)
{
 if (doChecks)
 {
   MatrixTools.checkMatrixDimensions(u, nInputs.getIntegerValue(), 1);
 }
 checkForNaN(u);
 getVariablesForPredictFromYoVariables();
 updateAPrioriState(x, u);
 storeInYoVariablesVector(x, yoX);
 if (updateCovarianceAndGain.getBooleanValue())
 {
   updateAPrioriCovariance();
   storeInYoVariablesSymmetric(P, yoP);
 }
}

代码示例来源：origin: us.ihmc/SensorProcessing

protected void configure()
{
  processModel.updateMatrices();
  measurementModel.updateMatrices();
  super.configure(processModel.getStateMatrix(), processModel.getInputMatrix(), measurementModel.getOutputMatrix());
  setProcessNoiseCovariance(processModel.getProcessCovarianceMatrix());
  setMeasurementNoiseCovariance(measurementModel.getMeasurementCovarianceMatrix());
}

代码示例来源：origin: us.ihmc/KalmanProject

private void setMeasurementNoiseCovariance()
{
 if (sensors.isPositionMeasurementUpdated())
   measurementCovariance.set(0, 0, defaultCovariance);
 else
   measurementCovariance.set(0, 0, Double.POSITIVE_INFINITY);
 yoKalmanFilter.setMeasurementNoiseCovariance(measurementCovariance);
}

代码示例来源：origin: us.ihmc/KalmanProject

private void setProcessNoiseCovariance()
  {
   
   double maxModeledJerk = 4.0e-3;
   processCovariance.set(positionIndex, positionIndex, Math.pow(maxModeledJerk * dt * dt * dt / 6.0, 2.0));
   processCovariance.set(velocityIndex, velocityIndex, Math.pow(maxModeledJerk * dt * dt / 2.0, 2.0));
//      processCovariance.set(positionIndex, positionIndex, positionModelCovariance);
//      processCovariance.set(velocityIndex, velocityIndex, velocityModelCovariance);
   yoKalmanFilter.setProcessNoiseCovariance(processCovariance);
  }

代码示例来源：origin: us.ihmc/ihmc-kalman-project

public void predict(DenseMatrix64F u)
{
 if (doChecks)
 {
   MatrixTools.checkMatrixDimensions(u, nInputs.getIntegerValue(), 1);
 }
 checkForNaN(u);
 getVariablesForPredictFromYoVariables();
 updateAPrioriState(x, u);
 storeInYoVariablesVector(x, yoX);
 if (updateCovarianceAndGain.getBooleanValue())
 {
   updateAPrioriCovariance();
   storeInYoVariablesSymmetric(P, yoP);
 }
}

代码示例来源：origin: us.ihmc/ihmc-kalman-project

private void doKalmanFiltering()
  {
   yoKalmanFilter.predict(inputs);
   yoKalmanFilter.update(measurements);
  }
}

代码示例来源：origin: us.ihmc/KalmanProject

private void updateYoKalmanFilterConfiguration(double controlDT)
{
 DenseMatrix64F modelStateEvolutionF = new DenseMatrix64F(nStates, nStates);
 modelStateEvolutionF.set(positionIndex, positionIndex, 1.0);
 modelStateEvolutionF.set(positionIndex, velocityIndex, controlDT);
 modelStateEvolutionF.set(velocityIndex, velocityIndex, 1.0);
 DenseMatrix64F modelInputInfluenceG = new DenseMatrix64F(nStates, nInputs);
 modelInputInfluenceG.set(velocityIndex, positionIndex, controlDT * controlDT / 2.0);
 modelInputInfluenceG.set(velocityIndex, positionIndex, controlDT);
 DenseMatrix64F modelOutputH = new DenseMatrix64F(nMeasurements, nStates);
 modelOutputH.set(positionIndex, positionIndex, 1.0);
 yoKalmanFilter.configure(modelStateEvolutionF, modelInputInfluenceG, modelOutputH);
}

代码示例来源：origin: us.ihmc/ihmc-kalman-project

private void setMeasurementNoiseCovariance()
{
 if (sensors.isPositionMeasurementUpdated())
   measurementCovariance.set(0, 0, defaultCovariance);
 else
   measurementCovariance.set(0, 0, Double.POSITIVE_INFINITY);
 yoKalmanFilter.setMeasurementNoiseCovariance(measurementCovariance);
}

代码示例来源：origin: us.ihmc/ihmc-kalman-project

private void setProcessNoiseCovariance()
  {
   
   double maxModeledJerk = 4.0e-3;
   processCovariance.set(positionIndex, positionIndex, Math.pow(maxModeledJerk * dt * dt * dt / 6.0, 2.0));
   processCovariance.set(velocityIndex, velocityIndex, Math.pow(maxModeledJerk * dt * dt / 2.0, 2.0));
//      processCovariance.set(positionIndex, positionIndex, positionModelCovariance);
//      processCovariance.set(velocityIndex, velocityIndex, velocityModelCovariance);
   yoKalmanFilter.setProcessNoiseCovariance(processCovariance);
  }

代码示例来源：origin: us.ihmc/KalmanProject

public void computeSteadyStateGainAndCovariance(int numberOfIterations)
  {
   getVariablesForPredictFromYoVariables();
   getVariablesForUpdateFromYoVariables();

   for (int i = 0; i < numberOfIterations; i++)
   {
     updateAPrioriCovariance();
     updateKalmanGainMatrixK();
     updateAPosterioriStateCovariance();
   }

   storeInYoVariablesSymmetric(P, yoP);
  }
}

代码示例来源：origin: us.ihmc/ihmc-kalman-project

public void update(DenseMatrix64F y)
{
 checkForNaN(y);
 getVariablesForUpdateFromYoVariables();
 if (updateCovarianceAndGain.getBooleanValue())
 {
   updateKalmanGainMatrixK();
 }
 
 updateAPosterioriState(x, y, K);
 storeInYoVariablesVector(x, yoX);
 if (updateCovarianceAndGain.getBooleanValue())
 {
   updateAPosterioriStateCovariance();
   storeInYoVariablesSymmetric(P, yoP);
 }
}

代码示例来源：origin: us.ihmc/KalmanProject

private void doKalmanFiltering()
  {
   yoKalmanFilter.predict(inputs);
   yoKalmanFilter.update(measurements);
  }
}

代码示例来源：origin: us.ihmc/ihmc-kalman-project

private void populateAndConfigureYoKalmanFilter(double controlDT)
{
 yoKalmanFilter = new YoKalmanFilter(name + "YoKalman", registry);
 updateYoKalmanFilterConfiguration(controlDT);
}

代码示例来源：origin: us.ihmc/ihmc-kalman-project

private void updateYoKalmanFilterConfiguration(double controlDT)
{
 DenseMatrix64F modelStateEvolutionF = new DenseMatrix64F(nStates, nStates);
 modelStateEvolutionF.set(positionIndex, positionIndex, 1.0);
 modelStateEvolutionF.set(positionIndex, velocityIndex, controlDT);
 modelStateEvolutionF.set(velocityIndex, velocityIndex, 1.0);
 DenseMatrix64F modelInputInfluenceG = new DenseMatrix64F(nStates, nInputs);
 modelInputInfluenceG.set(velocityIndex, positionIndex, controlDT * controlDT / 2.0);
 modelInputInfluenceG.set(velocityIndex, positionIndex, controlDT);
 DenseMatrix64F modelOutputH = new DenseMatrix64F(nMeasurements, nStates);
 modelOutputH.set(positionIndex, positionIndex, 1.0);
 yoKalmanFilter.configure(modelStateEvolutionF, modelInputInfluenceG, modelOutputH);
}

代码示例来源：origin: us.ihmc/KalmanProject

private void setMeasurementNoiseCovariance()
{
 if (sensors.isPositionMeasurementUpdated())
   measurementCovariance.set(positionMeasurementIndex, 0, ultrasonicsVariance);
 else
   measurementCovariance.set(positionMeasurementIndex, 0, Double.POSITIVE_INFINITY);
 
 measurementCovariance.set(accelerationMeasurementIndex, 1, imuVariance);
 yoKalmanFilter.setMeasurementNoiseCovariance(measurementCovariance);
}

代码示例来源：origin: us.ihmc/ihmc-kalman-project

private void setProcessNoiseCovariance()
{      
 // 1.5g change received in a single tick used as Variance
 //double estimatedAccelerationDifferenceInOneTick = 1.5 * 9.81 / dt;
 double maxModeledJerk = 4.0e-3; //theoretical result 4.0e-6
 
 processCovariance.set(positionIndex, positionIndex, Math.pow(maxModeledJerk * dt * dt * dt / 6.0, 2.0));
 processCovariance.set(velocityIndex, velocityIndex, Math.pow(maxModeledJerk * dt * dt / 2.0, 2.0));
 processCovariance.set(accelerationIndex, accelerationIndex, Math.pow(maxModeledJerk * dt , 2.0));
 yoKalmanFilter.setProcessNoiseCovariance(processCovariance);
}