Java Code Examples for org.ejml.data.DenseMatrix64F#copy()

public static double det(DenseMatrix64F mat) {
    int numCol = mat.getNumCols();
    int numRow = mat.getNumRows();
    if (numCol != numRow) {
        throw new IllegalArgumentException("Must be a square matrix.");
    } else if (numCol <= 6) {
        return numCol >= 2 ? UnrolledDeterminantFromMinor.det(mat) : mat.get(0);
    } else {
        LUDecompositionAlt_D64 alg = new LUDecompositionAlt_D64();
        if (alg.inputModified()) {
            mat = mat.copy();
        }

        return !alg.decompose(mat) ? 0.0D : alg.computeDeterminant().real;
    }
}
 

public static double invertAndGetDeterminant(DenseMatrix64F mat, DenseMatrix64F result, boolean log) {

        final int numCol = mat.getNumCols();
        final int numRow = mat.getNumRows();
        if (numCol != numRow) {
            throw new IllegalArgumentException("Must be a square matrix.");
        }

        if (numCol <= 5) {

            if (numCol >= 2) {
                UnrolledInverseFromMinor.inv(mat, result);
            } else {
                result.set(0, 1.0D / mat.get(0));
            }

            double det = numCol >= 2 ?
                    UnrolledDeterminantFromMinor.det(mat) :
                    mat.get(0);
            return log ? Math.log(det) : det;

        } else {

            LUDecompositionAlt_D64 alg = new LUDecompositionAlt_D64();
            LinearSolverLu_D64 solver = new LinearSolverLu_D64(alg);
            if (solver.modifiesA()) {
                mat = mat.copy();
            }

            if (!solver.setA(mat)) {
                return Double.NaN;
            }

            solver.invert(result);

            return log ? computeLogDeterminant(alg) : alg.computeDeterminant().real;

        }
    }
 

@Override
public DenseMatrix64F getGradientVarianceWrtVariance(NodeRef node,
                                                     ContinuousDiffusionIntegrator cdi,
                                                     ContinuousDataLikelihoodDelegate likelihoodDelegate,
                                                     DenseMatrix64F gradient) {
    if (tree.isRoot(node)) {
        return super.getGradientVarianceWrtVariance(node, cdi, likelihoodDelegate, gradient);
    } else {
        DenseMatrix64F result = gradient.copy();
        if (hasDiagonalActualization()) {
            actualizeGradientDiagonal(cdi, node.getNumber(), result);
        } else {
            actualizeGradient(cdi, node.getNumber(), result);
        }
        return result;
    }
}
 

private DenseMatrix64F scaleGradient(double scalar, DenseMatrix64F gradient) {
    DenseMatrix64F result = gradient.copy();
    if (scalar == 0.0) {
        CommonOps.fill(result, 0.0);
    } else {
        CommonOps.scale(scalar, result);
    }
    return result;
}
 

@Override
DenseMatrix64F getGradientDisplacementWrtDrift(NodeRef node,
                                               ContinuousDiffusionIntegrator cdi,
                                               ContinuousDataLikelihoodDelegate likelihoodDelegate,
                                               DenseMatrix64F gradient) {
    DenseMatrix64F result = gradient.copy();
    if (hasDiagonalActualization()) {
        actualizeDisplacementGradientDiagonal(cdi, node.getNumber(), result);
    } else {
        actualizeDisplacementGradient(cdi, node.getNumber(), result);
    }
    return result;
}
 

private KalmanOutput filter() {

        int n = 0;
        double f;
        predictionError[0] = y[0];
        // f[t] is always the first element of the first column of the predicted covariance matrix,
        // because f[t] = Z * M, where Z is a row vector with a 1 in the first (index 0) position and zeros elsewhere,
        // and M is the first column of the predicted covariance matrix.
        mult(Z, predictedStateCovariance, ZP);
        transpose(Z, Zt);
        predictionErrorVariance[0] = dot(ZP, Zt);
        f = predictionErrorVariance[0];

        double ssq = 0.0;
        double sumlog = 0.0;
        if (f < 1E4) {
            n++;
            ssq = ((predictionError[0] * predictionError[0]) / f);
            sumlog = log(f);

        }
        // Initialize filteredState.
        DenseMatrix64F newInfo = new DenseMatrix64F(rd, 1, true, new double[rd]);
        transpose(ZP, newInfo);
        divide(newInfo, f);
        PZtf = newInfo.copy();
        scale(predictionError[0], newInfo);
        add(predictedState, newInfo, filteredState);

        // Initialize filteredCovariance.
        final RowD1Matrix64F adjustedPredictionCovariance = new DenseMatrix64F(rd, rd);
        mult(PZtf, Z, PZtfZ);
        mult(PZtfZ, predictedStateCovariance, adjustedPredictionCovariance);
        //multOuter(predictedCovarianceFirstColumn, adjustedPredictionCovariance);
        //divide(adjustedPredictionCovariance, predictionErrorVariance[0]);
        subtract(predictedStateCovariance, adjustedPredictionCovariance, filteredStateCovariance);

        final RowD1Matrix64F filteredCovarianceTransition = new DenseMatrix64F(rd, rd);
        final RowD1Matrix64F stateCovarianceTransition = new DenseMatrix64F(rd, rd);
        final DenseMatrix64F transitionTranspose = transitionMatrix.copy();
        transpose(transitionTranspose);

        predictionError[0] /= Math.sqrt(f);

        for (int t = 1; t < y.length; t++) {

            // Update predicted mean of the state vector.
            mult(transitionMatrix, filteredState, predictedState);

            // Update predicted covariance of the state vector.
            mult(transitionMatrix, filteredStateCovariance, filteredCovarianceTransition);
            mult(filteredCovarianceTransition, transitionTranspose, stateCovarianceTransition);
            add(stateCovarianceTransition, stateDisturbance, predictedStateCovariance);

            predictionError[t] = y[t] - dot(Z, predictedState);
            mult(Z, predictedStateCovariance, ZP);
            predictionErrorVariance[t] = dot(ZP, Zt);
            f = predictionErrorVariance[t];
            if (f < 1E4) {
                n++;
                ssq += ((predictionError[t] * predictionError[t]) / f);
                sumlog += log(f);
            }

            // Update filteredState.
            transpose(ZP, newInfo);
            //mult(predictedStateCovariance, Zt, newInfo);
            divide(newInfo, f);
            PZtf = newInfo.copy();
            scale(predictionError[t], newInfo);
            add(predictedState, newInfo, filteredState);

            // Update filteredCovariance.
            mult(PZtf, Z, PZtfZ);
            mult(PZtfZ, predictedStateCovariance, adjustedPredictionCovariance);
            //multOuter(predictedCovarianceFirstColumn, adjustedPredictionCovariance);
            //divide(adjustedPredictionCovariance, predictionErrorVariance[0]);
            subtract(predictedStateCovariance, adjustedPredictionCovariance, filteredStateCovariance);

            predictionError[t] /= Math.sqrt(f);
        }
        return new KalmanOutput(n, ssq, sumlog, predictionError);
    }