@Override
  }

  @Override
<<<<<<< HEAD
  public AdjMultivariateGaussian createInitialLearnedObject() {
    return new TruncatedRoadGaussian(this.model.getState().clone(),
        this.modelCovariance);
  }

  public boolean equals(Object obj) {
    if (this == obj) {
      return true;
    }
    if (obj == null) {
      return false;
    }
    if (this.getClass() != obj.getClass()) {
      return false;
    }
    final TruncatedRoadKalmanFilter other =
        (TruncatedRoadKalmanFilter) obj;
    if (this.model == null) {
      if (other.model != null) {
        return false;
      }
    } else if (!StatisticsUtil.vectorEquals(
        this.model.convertToVector(), other.model.convertToVector())) {
      return false;
    }
    return true;
  }

  public SvdMatrix getMeasurementCovariance() {
    return this.measurementCovariance;
  }

  /**
   * Getter for model
   * 
   * @return Motion model of the underlying system.
   */
  public LinearDynamicalSystem getModel() {
    return this.model;
  }

  public SvdMatrix getModelCovariance() {
    return this.modelCovariance;
  }

  @Override
  public int hashCode() {
    final int prime = 31;
    int result = super.hashCode();
    result =
        prime
            * result
            + ((this.model == null) ? 0 : StatisticsUtil
                .hashCodeVector(this.model.convertToVector()));
    return result;
  }

  public void
      measure(MultivariateGaussian belief, Vector observation) {
    
    measure_old(belief, observation);
    
    final SvdMatrix covar = new SvdMatrix(belief.getCovariance());
    final AbstractSingularValueDecomposition svd =
        covar.getSvd();
    if (belief.getInputDimensionality() == 2) {
      svd.getS().setElement(1, 1, 0d);
    } else {
      svd.getS().setElement(1, 1, 0d);
      svd.getS().setElement(2, 2, 0d);
    }
    final SvdMatrix newCovar =
        new SvdMatrix(new SimpleSingularValueDecomposition(
            svd.getU(), svd.getS(), svd.getU().transpose()));

    belief.setCovariance(newCovar);

  }
  
  public void measure_old(MultivariateGaussian belief, Vector observation) {
    
    final Matrix F = this.model.getC();
    final Vector a = belief.getMean();
    final Matrix R = belief.getCovariance();
    final Matrix Q2 =
        F.times(R).times(F.transpose())
            .plus(this.getMeasurementCovariance());
    /*
     * This is the source of one major improvement:
     * uses the solve routine for a positive definite matrix
     */
    final UpperSPDDenseMatrix Qspd =
        new UpperSPDDenseMatrix(
            ((AbstractMTJMatrix) Q2).getInternalMatrix(), false);
    final no.uib.cipr.matrix.Matrix CRt =
        ((AbstractMTJMatrix) F.times(R.transpose()))
            .getInternalMatrix();

    final DenseMatrix Amtj =
        new DenseMatrix(Qspd.numRows(), CRt.numColumns());
    Qspd.transSolve(CRt, Amtj);

    final DenseMatrix AtQt =
        new DenseMatrix(Amtj.numColumns(), Qspd.numRows());
    Amtj.transABmult(Qspd, AtQt);

    final DenseMatrix AtQtAMtj =
        new DenseMatrix(AtQt.numRows(), Amtj.numColumns());
    AtQt.mult(Amtj, AtQtAMtj);

    final Matrix AtQtA =
        ((DenseMatrixFactoryMTJ) MatrixFactory.getDenseDefault())
            .createWrapper(AtQtAMtj);

    final DenseVector e2 =
        new DenseVector(
            ((gov.sandia.cognition.math.matrix.mtj.DenseVector) observation
                .minus(F.times(a))).getArray(), false);

    final DenseVector AteMtj = new DenseVector(Amtj.numColumns());
    Amtj.transMult(e2, AteMtj);
    final Vector Ate =
        ((DenseVectorFactoryMTJ) VectorFactory.getDenseDefault())
            .createWrapper(AteMtj);

    final Matrix CC = R.minus(AtQtA);
    final Vector m = a.plus(Ate);

    belief.setCovariance(CC);
    belief.setMean(m);
=======
  public void
      measure(MultivariateGaussian belief, Vector observation) {
//    DlmUtils.svdForwardFilter(observation, belief, this);
    DlmUtils.schurForwardFilter(observation, belief, this);
    // TODO FIXME: hack for a numerical issue
    if (!belief.getCovariance().isSymmetric()) {
      UpperSymmBandMatrix symmat = new UpperSymmBandMatrix(
          ((AbstractMTJMatrix) belief.getCovariance()).getInternalMatrix(), 
          belief.getInputDimensionality());
      belief.setCovariance(((DenseMatrixFactoryMTJ) MatrixFactory.getDenseDefault())
          .createWrapper(new DenseMatrix(symmat)));
    }
//    super.measure(belief, observation);
>>>>>>> 5dc42c8383471f2d958e551871142e409cb7e710
  }

  @Override

  }

  @Override
  public void
      measure(MultivariateGaussian belief, Vector observation) {
//    DlmUtils.svdForwardFilter(observation, belief, this);
    DlmUtils.schurForwardFilter(observation, belief, this);
    // TODO FIXME: hack for a numerical issue
    if (!belief.getCovariance().isSymmetric()) {
      UpperSymmBandMatrix symmat = new UpperSymmBandMatrix(
          ((AbstractMTJMatrix) belief.getCovariance()).getInternalMatrix(), 
          belief.getInputDimensionality());
      belief.setCovariance(((DenseMatrixFactoryMTJ) MatrixFactory.getDenseDefault())
          .createWrapper(new DenseMatrix(symmat)));
    }
//    super.measure(belief, observation);
  }

  @Override