private void getVariablesForUpdateFromYoVariables() { int nMeasurements = this.nMeasurements.getIntegerValue(); int nStates = this.nStates.getIntegerValue(); H.reshape(nMeasurements, nStates); R.reshape(nMeasurements, nMeasurements); x.reshape(nStates, 1); P.reshape(nStates, nStates); getFromYoVariablesMatrix(H, yoH); getFromYoVariablesSymmetric(R, yoR); getFromYoVariablesVector(x, yoX); getFromYoVariablesSymmetric(P, yoP); }
public void setProcessNoiseCovariance(DenseMatrix64F Q) { if (doChecks) { checkPositiveSemiDefinite(Q); MatrixTools.checkMatrixDimensions(Q, nStates.getIntegerValue(), nStates.getIntegerValue()); } populateYoVariablesSymmetricMatrix(yoQ, Q.getNumRows(), "Q", registry); storeInYoVariablesSymmetric(Q, yoQ); }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout=300000) public void testBackAndForthSymmetricMatrix() { List<List<YoDouble>> yoArray = new ArrayList<List<YoDouble>>(); int size = 50; String prefix = "test"; YoVariableRegistry registry = new YoVariableRegistry("test"); Random random = new Random(1235612L); DenseMatrix64F matrix = RandomMatrices.createSymmPosDef(size, random); DenseMatrix64F matrixBack = new DenseMatrix64F(size, size); MatrixYoVariableConversionTools.populateYoVariablesSymmetricMatrix(yoArray, size, prefix, registry); MatrixYoVariableConversionTools.storeInYoVariablesSymmetric(matrix, yoArray); MatrixYoVariableConversionTools.getFromYoVariablesSymmetric(matrixBack, yoArray); EjmlUnitTests.assertEquals(matrix, matrixBack, 1e-12); } }
public void configure(DenseMatrix64F F, DenseMatrix64F G, DenseMatrix64F H) { nStates.set(F.getNumRows()); nInputs.set(G.getNumCols()); nMeasurements.set(H.getNumRows()); if (doChecks) { MatrixTools.checkMatrixDimensions(F, nStates.getIntegerValue(), nStates.getIntegerValue()); MatrixTools.checkMatrixDimensions(G, nStates.getIntegerValue(), nInputs.getIntegerValue()); MatrixTools.checkMatrixDimensions(H, nMeasurements.getIntegerValue(), nStates.getIntegerValue()); } populateYoVariablesMatrix(yoF, F.getNumRows(), F.getNumCols(), "F", registry); populateYoVariablesMatrix(yoG, G.getNumRows(), G.getNumCols(), "G", registry); populateYoVariablesMatrix(yoH, H.getNumRows(), H.getNumCols(), "H", registry); populateYoVariablesVector(yoX, nStates.getIntegerValue(), "x", registry); populateYoVariablesSymmetricMatrix(yoP, nStates.getIntegerValue(), "P", registry); storeInYoVariablesMatrix(F, yoF); storeInYoVariablesMatrix(G, yoG); storeInYoVariablesMatrix(H, yoH); }
public void setState(DenseMatrix64F x, DenseMatrix64F P) { if (doChecks) { MatrixTools.checkMatrixDimensions(x, nStates.getIntegerValue(), 1); MatrixTools.checkMatrixDimensions(P, nStates.getIntegerValue(), nStates.getIntegerValue()); } storeInYoVariablesVector(x, yoX); storeInYoVariablesSymmetric(P, yoP); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testBackAndForthVector() { List<YoDouble> yoArray = new ArrayList<YoDouble>(); int size = 50; String prefix = "test"; YoVariableRegistry registry = new YoVariableRegistry("test"); DenseMatrix64F matrix = new DenseMatrix64F(size, 1); Random random = new Random(1235612L); RandomMatrices.setRandom(matrix, random); DenseMatrix64F matrixBack = new DenseMatrix64F(size, 1); MatrixYoVariableConversionTools.populateYoVariablesVector(yoArray, size, prefix, registry); MatrixYoVariableConversionTools.storeInYoVariablesVector(matrix, yoArray); MatrixYoVariableConversionTools.getFromYoVariablesVector(matrixBack, yoArray); EjmlUnitTests.assertEquals(matrix, matrixBack, 1e-12); }
@ContinuousIntegrationTest(estimatedDuration = 0.3) @Test(timeout=300000) public void testBackAndForthMatrix() { List<List<YoDouble>> yoArray = new ArrayList<List<YoDouble>>(); int nRows = 50; int nColumns = 60; String prefix = "test"; YoVariableRegistry registry = new YoVariableRegistry("test"); DenseMatrix64F matrix = new DenseMatrix64F(nRows, nColumns); Random random = new Random(1235612L); RandomMatrices.setRandom(matrix, random); DenseMatrix64F matrixBack = new DenseMatrix64F(nRows, nColumns); MatrixYoVariableConversionTools.populateYoVariablesMatrix(yoArray, nRows, nColumns, prefix, registry); MatrixYoVariableConversionTools.storeInYoVariablesMatrix(matrix, yoArray); MatrixYoVariableConversionTools.getFromYoVariablesMatrix(matrixBack, yoArray); EjmlUnitTests.assertEquals(matrix, matrixBack, 1e-12); }
public DenseMatrix64F getCovariance() { int nStates = this.nStates.getIntegerValue(); P.reshape(nStates, nStates); getFromYoVariablesSymmetric(P, yoP); return P; }
public DenseMatrix64F getState() { int nStates = this.nStates.getIntegerValue(); x.reshape(nStates, 1); getFromYoVariablesVector(x, yoX); return x; }
public void computeSteadyStateGainAndCovariance(int numberOfIterations) { getVariablesForPredictFromYoVariables(); getVariablesForUpdateFromYoVariables(); for (int i = 0; i < numberOfIterations; i++) { updateAPrioriCovariance(); updateKalmanGainMatrixK(); updateAPosterioriStateCovariance(); } storeInYoVariablesSymmetric(P, yoP); } }
public void configure(DenseMatrix64F F, DenseMatrix64F G, DenseMatrix64F H) { nStates.set(F.getNumRows()); nInputs.set(G.getNumCols()); nMeasurements.set(H.getNumRows()); if (doChecks) { MatrixTools.checkMatrixDimensions(F, nStates.getIntegerValue(), nStates.getIntegerValue()); MatrixTools.checkMatrixDimensions(G, nStates.getIntegerValue(), nInputs.getIntegerValue()); MatrixTools.checkMatrixDimensions(H, nMeasurements.getIntegerValue(), nStates.getIntegerValue()); } populateYoVariablesMatrix(yoF, F.getNumRows(), F.getNumCols(), "F", registry); populateYoVariablesMatrix(yoG, G.getNumRows(), G.getNumCols(), "G", registry); populateYoVariablesMatrix(yoH, H.getNumRows(), H.getNumCols(), "H", registry); populateYoVariablesVector(yoX, nStates.getIntegerValue(), "x", registry); populateYoVariablesSymmetricMatrix(yoP, nStates.getIntegerValue(), "P", registry); storeInYoVariablesMatrix(F, yoF); storeInYoVariablesMatrix(G, yoG); storeInYoVariablesMatrix(H, yoH); }
public void setState(DenseMatrix64F x, DenseMatrix64F P) { if (doChecks) { MatrixTools.checkMatrixDimensions(x, nStates.getIntegerValue(), 1); MatrixTools.checkMatrixDimensions(P, nStates.getIntegerValue(), nStates.getIntegerValue()); } storeInYoVariablesVector(x, yoX); storeInYoVariablesSymmetric(P, yoP); }
public DenseMatrix64F getCovariance() { int nStates = this.nStates.getIntegerValue(); P.reshape(nStates, nStates); getFromYoVariablesSymmetric(P, yoP); return P; }
public DenseMatrix64F getState() { int nStates = this.nStates.getIntegerValue(); x.reshape(nStates, 1); getFromYoVariablesVector(x, yoX); return x; }
public void computeSteadyStateGainAndCovariance(int numberOfIterations) { getVariablesForPredictFromYoVariables(); getVariablesForUpdateFromYoVariables(); for (int i = 0; i < numberOfIterations; i++) { updateAPrioriCovariance(); updateKalmanGainMatrixK(); updateAPosterioriStateCovariance(); } storeInYoVariablesSymmetric(P, yoP); } }
private void getVariablesForPredictFromYoVariables() { int nStates = this.nStates.getIntegerValue(); int nInputs = this.nInputs.getIntegerValue(); F.reshape(nStates, nStates); G.reshape(nStates, nInputs); Q.reshape(nStates, nStates); x.reshape(nStates, 1); P.reshape(nStates, nStates); getFromYoVariablesMatrix(F, yoF); getFromYoVariablesMatrix(G, yoG); getFromYoVariablesSymmetric(Q, yoQ); getFromYoVariablesVector(x, yoX); getFromYoVariablesSymmetric(P, yoP); }
public void setProcessNoiseCovariance(DenseMatrix64F Q) { if (doChecks) { checkPositiveSemiDefinite(Q); MatrixTools.checkMatrixDimensions(Q, nStates.getIntegerValue(), nStates.getIntegerValue()); } populateYoVariablesSymmetricMatrix(yoQ, Q.getNumRows(), "Q", registry); storeInYoVariablesSymmetric(Q, yoQ); }
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); } }
private void getVariablesForUpdateFromYoVariables() { int nMeasurements = this.nMeasurements.getIntegerValue(); int nStates = this.nStates.getIntegerValue(); H.reshape(nMeasurements, nStates); R.reshape(nMeasurements, nMeasurements); x.reshape(nStates, 1); P.reshape(nStates, nStates); getFromYoVariablesMatrix(H, yoH); getFromYoVariablesSymmetric(R, yoR); getFromYoVariablesVector(x, yoX); getFromYoVariablesSymmetric(P, yoP); }
public void setMeasurementNoiseCovariance(DenseMatrix64F R) { if (doChecks) { checkPositiveSemiDefinite(R); MatrixTools.checkMatrixDimensions(R, nMeasurements.getIntegerValue(), nMeasurements.getIntegerValue()); } populateYoVariablesSymmetricMatrix(yoR, R.getNumRows(), "R", registry); storeInYoVariablesSymmetric(R, yoR); }