/** * 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()); }
RealMatrix processNoise = processModel.getProcessNoise(); MathUtils.checkNotNull(processNoise); RealMatrix measNoise = measurementModel.getMeasurementNoise();
.add(processModel.getProcessNoise());
/** * 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()); }
/** * 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()); }
RealMatrix processNoise = processModel.getProcessNoise(); MathUtils.checkNotNull(processNoise); RealMatrix measNoise = measurementModel.getMeasurementNoise();
RealMatrix processNoise = processModel.getProcessNoise(); MathUtils.checkNotNull(processNoise); RealMatrix measNoise = measurementModel.getMeasurementNoise();
RealMatrix processNoise = processModel.getProcessNoise(); MathUtils.checkNotNull(processNoise); RealMatrix measNoise = measurementModel.getMeasurementNoise();