Convert C++ to Java: Tools Class for RMSE and Jacobian Calculation

import org.ejml.simple.SimpleMatrix;
import java.util.List;

public class Tools {
    public Tools() {}

    public SimpleMatrix CalculateRMSE(List<SimpleMatrix> estimations, List<SimpleMatrix> ground_truth) {
        SimpleMatrix rmse = new SimpleMatrix(4, 1);
        rmse.set(0, 0, 0.0);
        rmse.set(1, 0, 0.0);
        rmse.set(2, 0, 0.0);
        rmse.set(3, 0, 0.0);

        if (estimations.size() != ground_truth.size() || estimations.size() == 0) {
            System.out.println('Invalid estimation or ground_truth data');
            return rmse;
        }

        for (int i = 0; i < estimations.size(); ++i) {
            SimpleMatrix residual = estimations.get(i).minus(ground_truth.get(i));

            // coefficient-wise multiplication
            residual.elementMult(residual);
            rmse = rmse.plus(residual);
        }

        // calculate the squared root of mean
        rmse = rmse.divide(estimations.size()).elementPower(0.5);

        return rmse;
    }

    public SimpleMatrix CalculateJacobian(SimpleMatrix x_state) {
        SimpleMatrix Hj = new SimpleMatrix(3, 4);

        // recover state parameters
        double px = x_state.get(0, 0);
        double py = x_state.get(1, 0);
        double vx = x_state.get(2, 0);
        double vy = x_state.get(3, 0);

        // pre-compute a set of terms to avoid repeated calculation
        double c1 = px*px + py*py;
        double c2 = Math.sqrt(c1);
        double c3 = c1*c2;

        // avoid division by zero
        if (Math.abs(c1) < 0.0001) {
            System.out.println('CalculateJacobian () - Error - Division by Zero');
            return Hj;
        }

        // compute the Jacobian matrix
        Hj.set(0, 0, px/c2);
        Hj.set(0, 1, py/c2);
        Hj.set(0, 2, 0);
        Hj.set(0, 3, 0);
        Hj.set(1, 0, -py/c1);
        Hj.set(1, 1, px/c1);
        Hj.set(1, 2, 0);
        Hj.set(1, 3, 0);
        Hj.set(2, 0, py*(vx*py - vy*px)/c3);
        Hj.set(2, 1, px*(px*vy - py*vx)/c3);
        Hj.set(2, 2, px/c2);
        Hj.set(2, 3, py/c2);

        return Hj;
    }
}