/** * Predict the internal state estimation one time step ahead. */ public void predict() { predict((RealVector) null); }
/** * Correct the current state estimate with an actual measurement. * * @param z * the measurement vector * @throws NullArgumentException * if the measurement vector is {@code null} * @throws DimensionMismatchException * if the dimension of the measurement vector does not fit * @throws SingularMatrixException * if the covariance matrix could not be inverted */ public void correct(final double[] z) throws NullArgumentException, DimensionMismatchException, SingularMatrixException { correct(new ArrayRealVector(z, false)); }
transitionMatrix = processModel.getStateTransitionMatrix(); MathUtils.checkNotNull(transitionMatrix); transitionMatrixT = transitionMatrix.transpose(); if (processModel.getControlMatrix() == null) { controlMatrix = new Array2DRowRealMatrix(); } else { controlMatrix = processModel.getControlMatrix(); measurementMatrix = measurementModel.getMeasurementMatrix(); MathUtils.checkNotNull(measurementMatrix); measurementMatrixT = measurementMatrix.transpose(); RealMatrix processNoise = processModel.getProcessNoise(); MathUtils.checkNotNull(processNoise); RealMatrix measNoise = measurementModel.getMeasurementNoise(); MathUtils.checkNotNull(measNoise); 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(measurementModel.getMeasurementNoise());
transitionMatrix = processModel.getStateTransitionMatrix(); MathUtils.checkNotNull(transitionMatrix); transitionMatrixT = transitionMatrix.transpose(); if (processModel.getControlMatrix() == null) { controlMatrix = new Array2DRowRealMatrix(); } else { controlMatrix = processModel.getControlMatrix(); measurementMatrix = measurementModel.getMeasurementMatrix(); MathUtils.checkNotNull(measurementMatrix); measurementMatrixT = measurementMatrix.transpose(); RealMatrix processNoise = processModel.getProcessNoise(); MathUtils.checkNotNull(processNoise); RealMatrix measNoise = measurementModel.getMeasurementNoise(); MathUtils.checkNotNull(measNoise); if (processModel.getInitialStateEstimate() == null) { stateEstimation = new ArrayRealVector(transitionMatrix.getColumnDimension()); } else { stateEstimation = processModel.getInitialStateEstimate(); if (processModel.getInitialErrorCovariance() == null) { errorCovariance = processNoise.copy(); } else { errorCovariance = processModel.getInitialErrorCovariance();
.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 fit */ public void predict(final double[] u) throws DimensionMismatchException { predict(new ArrayRealVector(u, false)); }
.add(measurementModel.getMeasurementNoise());
/** * Correct the current state estimate with an actual measurement. * * @param z * the measurement vector * @throws NullArgumentException * if the measurement vector is {@code null} * @throws DimensionMismatchException * if the dimension of the measurement vector does not fit * @throws SingularMatrixException * if the covariance matrix could not be inverted */ public void correct(final double[] z) throws NullArgumentException, DimensionMismatchException, SingularMatrixException { correct(new ArrayRealVector(z, false)); }
transitionMatrix = processModel.getStateTransitionMatrix(); MathUtils.checkNotNull(transitionMatrix); transitionMatrixT = transitionMatrix.transpose(); if (processModel.getControlMatrix() == null) { controlMatrix = new Array2DRowRealMatrix(); } else { controlMatrix = processModel.getControlMatrix(); measurementMatrix = measurementModel.getMeasurementMatrix(); MathUtils.checkNotNull(measurementMatrix); measurementMatrixT = measurementMatrix.transpose(); RealMatrix processNoise = processModel.getProcessNoise(); MathUtils.checkNotNull(processNoise); RealMatrix measNoise = measurementModel.getMeasurementNoise(); MathUtils.checkNotNull(measNoise); 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()); }
/** * Predict the internal state estimation one time step ahead. */ public void predict() { predict((RealVector) null); }
.add(measurementModel.getMeasurementNoise());
/** * Correct the current state estimate with an actual measurement. * * @param z * the measurement vector * @throws NullArgumentException * if the measurement vector is {@code null} * @throws DimensionMismatchException * if the dimension of the measurement vector does not fit * @throws SingularMatrixException * if the covariance matrix could not be inverted */ public void correct(final double[] z) throws NullArgumentException, DimensionMismatchException, SingularMatrixException { correct(new ArrayRealVector(z, false)); }
this.measurementModel = measurement; transitionMatrix = processModel.getStateTransitionMatrix(); MathUtils.checkNotNull(transitionMatrix); transitionMatrixT = transitionMatrix.transpose(); if (processModel.getControlMatrix() == null) controlMatrix = processModel.getControlMatrix(); measurementMatrix = measurementModel.getMeasurementMatrix(); MathUtils.checkNotNull(measurementMatrix); measurementMatrixT = measurementMatrix.transpose(); RealMatrix processNoise = processModel.getProcessNoise(); MathUtils.checkNotNull(processNoise); RealMatrix measNoise = measurementModel.getMeasurementNoise(); MathUtils.checkNotNull(measNoise); 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()); }
/** * Predict the internal state estimation one time step ahead. */ public void predict() { predict((RealVector) null); }
/** * 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 fit */ public void predict(final double[] u) throws DimensionMismatchException { predict(new ArrayRealVector(u, false)); }
/** * 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 fit */ public void predict(final double[] u) throws DimensionMismatchException { predict(new ArrayRealVector(u, false)); }