@Override public void measure( MultivariateGaussian belief, Vector observation) { // Figure out what the model says the observation should be Vector xpred = belief.getMean(); Vector ypred = this.observationModel.evaluate( xpred ); // The only difference between the EKF and the KF is that we have // to estimate the output-Jacobian, the derivative of the estimated // output with respected to the current estimated state. Matrix C = NumericalDifferentiator.MatrixJacobian.differentiate( xpred, this.observationModel ); // Update step... compute the difference between the observation // and what the model says. // Then compute the Kalman gain, which essentially indicates // how much to believe the observation, and how much to believe model Vector innovation = observation.minus( ypred ); this.computeMeasurementBelief(belief, innovation, C); }
@Override public void measure( MultivariateGaussian belief, Vector observation) { // Figure out what the model says the observation should be Vector xpred = belief.getMean(); Vector ypred = this.observationModel.evaluate( xpred ); // The only difference between the EKF and the KF is that we have // to estimate the output-Jacobian, the derivative of the estimated // output with respected to the current estimated state. Matrix C = NumericalDifferentiator.MatrixJacobian.differentiate( xpred, this.observationModel ); // Update step... compute the difference between the observation // and what the model says. // Then compute the Kalman gain, which essentially indicates // how much to believe the observation, and how much to believe model Vector innovation = observation.minus( ypred ); this.computeMeasurementBelief(belief, innovation, C); }
@Override public void measure( MultivariateGaussian belief, Vector observation) { // Figure out what the model says the observation should be Vector xpred = belief.getMean(); Vector ypred = this.observationModel.evaluate( xpred ); // The only difference between the EKF and the KF is that we have // to estimate the output-Jacobian, the derivative of the estimated // output with respected to the current estimated state. Matrix C = NumericalDifferentiator.MatrixJacobian.differentiate( xpred, this.observationModel ); // Update step... compute the difference between the observation // and what the model says. // Then compute the Kalman gain, which essentially indicates // how much to believe the observation, and how much to believe model Vector innovation = observation.minus( ypred ); this.computeMeasurementBelief(belief, innovation, C); }
@Override public void predict( MultivariateGaussian belief) { // The only difference between the KF and EKF is that // in EKF we have to estimate the Jacobian (A), whereas the KF just // accesses the Jacobian directly. Vector x = belief.getMean(); Vector xpred = this.getMotionModel().evaluate( this.currentInput, x ); Matrix A = NumericalDifferentiator.MatrixJacobian.differentiate( x, new ModelJacobianEvaluator() ); Matrix P = this.computePredictionCovariance(A, belief.getCovariance()); // Load the updated belief belief.setMean( xpred ); belief.setCovariance( P ); }
@Override public void predict( MultivariateGaussian belief) { // The only difference between the KF and EKF is that // in EKF we have to estimate the Jacobian (A), whereas the KF just // accesses the Jacobian directly. Vector x = belief.getMean(); Vector xpred = this.getMotionModel().evaluate( this.currentInput, x ); Matrix A = NumericalDifferentiator.MatrixJacobian.differentiate( x, new ModelJacobianEvaluator() ); Matrix P = this.computePredictionCovariance(A, belief.getCovariance()); // Load the updated belief belief.setMean( xpred ); belief.setCovariance( P ); }
@Override public void predict( MultivariateGaussian belief) { // The only difference between the KF and EKF is that // in EKF we have to estimate the Jacobian (A), whereas the KF just // accesses the Jacobian directly. Vector x = belief.getMean(); Vector xpred = this.getMotionModel().evaluate( this.currentInput, x ); Matrix A = NumericalDifferentiator.MatrixJacobian.differentiate( x, new ModelJacobianEvaluator() ); Matrix P = this.computePredictionCovariance(A, belief.getCovariance()); // Load the updated belief belief.setMean( xpred ); belief.setCovariance( P ); }
/** * Static access to the numerical differentiation procedure. * @param input * Input about which to approximate the derivative. * @param f * Function of which to approximate the derivative. * @return * Approximated Jacobian, of the same dimension as input */ public static Matrix differentiate( Vector input, Evaluator<? super Vector,Vector> f ) { return MatrixJacobian.differentiate( input, f, DEFAULT_DELTA ); }
/** * Static access to the numerical differentiation procedure. * @param input * Input about which to approximate the derivative. * @param f * Function of which to approximate the derivative. * @return * Approximated Jacobian, of the same dimension as input */ public static Matrix differentiate( Vector input, Evaluator<? super Vector,Vector> f ) { return MatrixJacobian.differentiate( input, f, DEFAULT_DELTA ); }
/** * Static access to the numerical differentiation procedure. * @param input * Input about which to approximate the derivative. * @param f * Function of which to approximate the derivative. * @return * Approximated Jacobian, of the same dimension as input */ public static Matrix differentiate( Vector input, Evaluator<? super Vector,Vector> f ) { return MatrixJacobian.differentiate( input, f, DEFAULT_DELTA ); }
public Matrix differentiate( Vector input ) { return MatrixJacobian.differentiate( input, this.getInternalFunction(), this.getDelta() ); }
public Matrix differentiate( Vector input ) { return MatrixJacobian.differentiate( input, this.getInternalFunction(), this.getDelta() ); }
public Matrix differentiate( Vector input ) { return MatrixJacobian.differentiate( input, this.getInternalFunction(), this.getDelta() ); }