List of usage examples for org.apache.commons.math3.linear MatrixUtils checkAdditionCompatible
public static void checkAdditionCompatible(final AnyMatrix left, final AnyMatrix right) throws MatrixDimensionMismatchException
From source file:com.bolatu.gezkoncsvlogger.GyroOrientation.RotationKalmanFilter.java
/** * Creates a new Kalman filter with the given process and measurement * models./* w ww .j av a 2s. c o m*/ * * @param process * the model defining the underlying process dynamics * @param measurement * the model defining the given measurement characteristics * @throws NullArgumentException * if any of the given inputs is null (except for the control * matrix) * @throws NonSquareMatrixException * if the transition matrix is non square * @throws DimensionMismatchException * if the column dimension of the transition matrix does not * match the dimension of the initial state estimation vector * @throws MatrixDimensionMismatchException * if the matrix dimensions do not fit together */ public RotationKalmanFilter(final ProcessModel process, final MeasurementModel measurement) throws NullArgumentException, NonSquareMatrixException, DimensionMismatchException, MatrixDimensionMismatchException { MathUtils.checkNotNull(process); MathUtils.checkNotNull(measurement); this.processModel = process; this.measurementModel = measurement; transitionMatrix = processModel.getStateTransitionMatrix(); MathUtils.checkNotNull(transitionMatrix); transitionMatrixT = transitionMatrix.transpose(); // create an empty matrix if no control matrix was given if (processModel.getControlMatrix() == null) { controlMatrix = new Array2DRowRealMatrix(); } else { controlMatrix = processModel.getControlMatrix(); } measurementMatrix = measurementModel.getMeasurementMatrix(); MathUtils.checkNotNull(measurementMatrix); measurementMatrixT = measurementMatrix.transpose(); // check that the process and measurement noise matrices are not null // they will be directly accessed from the model as they may change // over time RealMatrix processNoise = processModel.getProcessNoise(); MathUtils.checkNotNull(processNoise); RealMatrix measNoise = measurementModel.getMeasurementNoise(); MathUtils.checkNotNull(measNoise); // set the initial state estimate to a zero vector if it is not // available from the process model if (processModel.getInitialStateEstimate() == null) { stateEstimation = new ArrayRealVector(transitionMatrix.getColumnDimension()); } else { stateEstimation = processModel.getInitialStateEstimate(); } if (transitionMatrix.getColumnDimension() != stateEstimation.getDimension()) { throw new DimensionMismatchException(transitionMatrix.getColumnDimension(), stateEstimation.getDimension()); } // initialize the error covariance to the process noise if it is not // available from the process model if (processModel.getInitialErrorCovariance() == null) { errorCovariance = processNoise.copy(); } else { errorCovariance = processModel.getInitialErrorCovariance(); } // sanity checks, the control matrix B may be null // A must be a square matrix if (!transitionMatrix.isSquare()) { throw new NonSquareMatrixException(transitionMatrix.getRowDimension(), transitionMatrix.getColumnDimension()); } // row dimension of B must be equal to A // if no control matrix is available, the row and column dimension will // be 0 if (controlMatrix != null && controlMatrix.getRowDimension() > 0 && controlMatrix.getColumnDimension() > 0 && controlMatrix.getRowDimension() != transitionMatrix.getRowDimension()) { throw new MatrixDimensionMismatchException(controlMatrix.getRowDimension(), controlMatrix.getColumnDimension(), transitionMatrix.getRowDimension(), controlMatrix.getColumnDimension()); } // Q must be equal to A MatrixUtils.checkAdditionCompatible(transitionMatrix, processNoise); // column dimension of H must be equal to row dimension of A if (measurementMatrix.getColumnDimension() != transitionMatrix.getRowDimension()) { throw new MatrixDimensionMismatchException(measurementMatrix.getRowDimension(), measurementMatrix.getColumnDimension(), measurementMatrix.getRowDimension(), transitionMatrix.getRowDimension()); } // row dimension of R must be equal to row dimension of H if (measNoise.getRowDimension() != measurementMatrix.getRowDimension()) { throw new MatrixDimensionMismatchException(measNoise.getRowDimension(), measNoise.getColumnDimension(), measurementMatrix.getRowDimension(), measNoise.getColumnDimension()); } }
From source file:lirmm.inria.fr.math.BigSparseRealMatrix.java
/** * Compute the sum of this matrix and {@code m}. * * @param m Matrix to be added.//from ww w . j a v a 2s .c o m * @return {@code this} + {@code m}. * @throws MatrixDimensionMismatchException if {@code m} is not the same * size as {@code this}. */ public BigSparseRealMatrix add(BigSparseRealMatrix m) throws MatrixDimensionMismatchException { MatrixUtils.checkAdditionCompatible(this, m); final BigSparseRealMatrix out = new BigSparseRealMatrix(this); for (OpenLongToDoubleHashMap.Iterator iterator = m.entries.iterator(); iterator.hasNext();) { iterator.advance(); final int row, col; if (m.isTransposed) { col = (int) (iterator.key() / m.columns); row = (int) (iterator.key() - col * m.columns); } else { row = (int) (iterator.key() / m.columns); col = (int) (iterator.key() - row * m.columns); } out.setEntry(row, col, getEntry(row, col) + iterator.value()); } return out; }
From source file:lirmm.inria.fr.math.BigSparseRealMatrix.java
/** * Subtract {@code m} from this matrix./*from ww w. ja v a 2 s .c o m*/ * * @param m Matrix to be subtracted. * @return {@code this} - {@code m}. * @throws MatrixDimensionMismatchException if {@code m} is not the same * size as {@code this}. */ public BigSparseRealMatrix subtract(BigSparseRealMatrix m) throws MatrixDimensionMismatchException { MatrixUtils.checkAdditionCompatible(this, m); final BigSparseRealMatrix out = new BigSparseRealMatrix(this); for (OpenLongToDoubleHashMap.Iterator iterator = m.entries.iterator(); iterator.hasNext();) { iterator.advance(); final int row, col; if (m.isTransposed) { col = (int) (iterator.key() / m.columns); row = (int) (iterator.key() % m.columns); } else { row = (int) (iterator.key() / m.columns); col = (int) (iterator.key() % m.columns); } out.setEntry(row, col, getEntry(row, col) - iterator.value()); } return out; }
From source file:lirmm.inria.fr.math.BigSparseRealMatrix.java
/** * Subtract {@code A*B} from this matrix. * * @param A Latent feature matrix 1/*from w w w . j av a 2 s . c o m*/ * @param B Latent feature matrix 2 * @return {@code this} - {@code A} * {@code B}. */ public BigSparseRealMatrix specialOperation(BigSparseRealMatrix A, BigSparseRealMatrix B) { MatrixUtils.checkMultiplicationCompatible(A, B); MatrixUtils.checkAdditionCompatible(this, new BigSparseRealMatrix(A.getRowDimension(), B.getColumnDimension())); BigSparseRealMatrix out = new BigSparseRealMatrix(getRowDimension(), getColumnDimension()); for (OpenLongToDoubleHashMap.Iterator iterator = entries.iterator(); iterator.hasNext();) { iterator.advance(); final double value = iterator.value(); final long key = iterator.key(); final int i, j; if (isTransposed) { j = (int) (key / columns); i = (int) (key % columns); } else { i = (int) (key / columns); j = (int) (key % columns); } double v = value - A.getRowVector(i).dotProduct(B.getColumnVector(j)); out.setEntry(i, j, v); } return out; }