this.measurementModel = measurement; transitionMatrix = processModel.getStateTransitionMatrix(); MathUtils.checkNotNull(transitionMatrix); transitionMatrixT = transitionMatrix.transpose(); if (processModel.getControlMatrix() == null) { controlMatrix = new Array2DRowRealMatrix(); } else { controlMatrix = processModel.getControlMatrix(); RealMatrix processNoise = processModel.getProcessNoise(); MathUtils.checkNotNull(processNoise); RealMatrix measNoise = measurementModel.getMeasurementNoise(); if (processModel.getInitialStateEstimate() == null) { stateEstimation = new ArrayRealVector(transitionMatrix.getColumnDimension()); } else { stateEstimation = processModel.getInitialStateEstimate(); if (processModel.getInitialErrorCovariance() == null) { errorCovariance = processNoise.copy(); } else { errorCovariance = processModel.getInitialErrorCovariance();
/** * Predict the internal state estimation one time step ahead. * * @param u * the control vector * @throws DimensionMismatchException * if the dimension of the control vector does not match */ public void predict(final RealVector u) throws DimensionMismatchException { // sanity checks if (u != null && u.getDimension() != controlMatrix.getColumnDimension()) { throw new DimensionMismatchException(u.getDimension(), controlMatrix.getColumnDimension()); } // project the state estimation ahead (a priori state) // xHat(k)- = A * xHat(k-1) + B * u(k-1) stateEstimation = transitionMatrix.operate(stateEstimation); // add control input if it is available if (u != null) { stateEstimation = stateEstimation.add(controlMatrix.operate(u)); } // project the error covariance ahead // P(k)- = A * P(k-1) * A' + Q errorCovariance = transitionMatrix.multiply(errorCovariance) .multiply(transitionMatrixT) .add(processModel.getProcessNoise()); }
.add(processModel.getProcessNoise());
this.measurementModel = measurement; transitionMatrix = processModel.getStateTransitionMatrix(); MathUtils.checkNotNull(transitionMatrix); transitionMatrixT = transitionMatrix.transpose(); if (processModel.getControlMatrix() == null) { controlMatrix = new Array2DRowRealMatrix(); } else { controlMatrix = processModel.getControlMatrix(); RealMatrix processNoise = processModel.getProcessNoise(); MathUtils.checkNotNull(processNoise); RealMatrix measNoise = measurementModel.getMeasurementNoise(); if (processModel.getInitialStateEstimate() == null) { stateEstimation = new ArrayRealVector(transitionMatrix.getColumnDimension()); } else { stateEstimation = processModel.getInitialStateEstimate(); if (processModel.getInitialErrorCovariance() == null) { errorCovariance = processNoise.copy(); } else { errorCovariance = processModel.getInitialErrorCovariance();
/** * Predict the internal state estimation one time step ahead. * * @param u * the control vector * @throws DimensionMismatchException * if the dimension of the control vector does not match */ public void predict(final RealVector u) throws DimensionMismatchException { // sanity checks if (u != null && u.getDimension() != controlMatrix.getColumnDimension()) { throw new DimensionMismatchException(u.getDimension(), controlMatrix.getColumnDimension()); } // project the state estimation ahead (a priori state) // xHat(k)- = A * xHat(k-1) + B * u(k-1) stateEstimation = transitionMatrix.operate(stateEstimation); // add control input if it is available if (u != null) { stateEstimation = stateEstimation.add(controlMatrix.operate(u)); } // project the error covariance ahead // P(k)- = A * P(k-1) * A' + Q errorCovariance = transitionMatrix.multiply(errorCovariance) .multiply(transitionMatrixT) .add(processModel.getProcessNoise()); }
this.measurementModel = measurement; transitionMatrix = processModel.getStateTransitionMatrix(); MathUtils.checkNotNull(transitionMatrix); transitionMatrixT = transitionMatrix.transpose(); if (processModel.getControlMatrix() == null) controlMatrix = processModel.getControlMatrix(); RealMatrix processNoise = processModel.getProcessNoise(); MathUtils.checkNotNull(processNoise); RealMatrix measNoise = measurementModel.getMeasurementNoise(); if (processModel.getInitialStateEstimate() == null) stateEstimation = processModel.getInitialStateEstimate(); if (processModel.getInitialErrorCovariance() == null) errorCovariance = processModel.getInitialErrorCovariance();
/** * Predict the internal state estimation one time step ahead. * * @param u * the control vector * @throws DimensionMismatchException * if the dimension of the control vector does not match */ public void predict(final RealVector u) throws DimensionMismatchException { // sanity checks if (u != null && u.getDimension() != controlMatrix.getColumnDimension()) { throw new DimensionMismatchException(u.getDimension(), controlMatrix.getColumnDimension()); } // project the state estimation ahead (a priori state) // xHat(k)- = A * xHat(k-1) + B * u(k-1) stateEstimation = transitionMatrix.operate(stateEstimation); // add control input if it is available if (u != null) { stateEstimation = stateEstimation.add(controlMatrix.operate(u)); } // project the error covariance ahead // P(k)- = A * P(k-1) * A' + Q errorCovariance = transitionMatrix.multiply(errorCovariance) .multiply(transitionMatrixT) .add(processModel.getProcessNoise()); }
this.measurementModel = measurement; transitionMatrix = processModel.getStateTransitionMatrix(); MathUtils.checkNotNull(transitionMatrix); transitionMatrixT = transitionMatrix.transpose(); if (processModel.getControlMatrix() == null) { controlMatrix = new Array2DRowRealMatrix(); } else { controlMatrix = processModel.getControlMatrix(); RealMatrix processNoise = processModel.getProcessNoise(); MathUtils.checkNotNull(processNoise); RealMatrix measNoise = measurementModel.getMeasurementNoise(); if (processModel.getInitialStateEstimate() == null) { stateEstimation = new ArrayRealVector(transitionMatrix.getColumnDimension()); } else { stateEstimation = processModel.getInitialStateEstimate(); if (processModel.getInitialErrorCovariance() == null) { errorCovariance = processNoise.copy(); } else { errorCovariance = processModel.getInitialErrorCovariance();