Convert C++ Kalman Filter to Java using EJML Library

import org.ejml.simple.SimpleMatrix; import org.ejml.dense.row.CommonOps_DDRM;

public class KalmanFilter {

private SimpleMatrix x_;
private SimpleMatrix P_;
private SimpleMatrix F_;
private SimpleMatrix H_;
private SimpleMatrix R_;
private SimpleMatrix Q_;

private static final double PI = 3.14159265;

public KalmanFilter() {}

public void Init(SimpleMatrix x_in, SimpleMatrix P_in, SimpleMatrix F_in,
                 SimpleMatrix H_in, SimpleMatrix R_in, SimpleMatrix Q_in) {
    x_ = x_in.copy();
    P_ = P_in.copy();
    F_ = F_in.copy();
    H_ = H_in.copy();
    R_ = R_in.copy();
    Q_ = Q_in.copy();
}

public void Predict() {
    /**
     * predict the state
     */
    x_ = F_.mult(x_);
    SimpleMatrix Ft = F_.transpose();
    P_ = F_.mult(P_).mult(Ft).plus(Q_);
}

public void Update(SimpleMatrix z) {
    /**
     * update the state by using Kalman Filter equations
     */
    SimpleMatrix y = z.minus(H_.mult(x_));
    SimpleMatrix S = H_.mult(P_).mult(H_.transpose()).plus(R_);
    SimpleMatrix K = P_.mult(H_.transpose()).mult(S.invert());

    x_ = x_.plus(K.mult(y));
    int x_size = x_.getNumElements();
    SimpleMatrix I = SimpleMatrix.identity(x_size);
    P_ = (I.minus(K.mult(H_))).mult(P_);
}

public void UpdateEKF(SimpleMatrix z) {
    /**
     * update the state by using Extended Kalman Filter equations
     */
    double px = x_.get(0);
    double py = x_.get(1);
    double vx = x_.get(2);
    double vy = x_.get(3);

    double ro = Math.sqrt(px*px + py*py);
    double theta = Math.atan2(py, px);
    double ro_dot;

    // check division by zero
    if (ro < 0.0001) {
        ro_dot = 0;
    } else {
        ro_dot = (px*vx + py*vy)/ro;
    }

    SimpleMatrix z_pred = new SimpleMatrix(3, 1, true, new double[] {ro, theta, ro_dot});
    SimpleMatrix y = z.minus(z_pred);

    if (y.get(1) > PI) {
        y.set(1, y.get(1) - 2*PI);
    }

    if (y.get(1) < -PI) {
        y.set(1, y.get(1) + 2*PI);
    }

    SimpleMatrix Ht = H_.transpose();
    SimpleMatrix S = H_.mult(P_).mult(Ht).plus(R_);
    SimpleMatrix Si = S.invert();
    SimpleMatrix K = P_.mult(Ht).mult(Si);

    x_ = x_.plus(K.mult(y));
    int x_size = x_.getNumElements();
    SimpleMatrix I = SimpleMatrix.identity(x_size);
    P_ = (I.minus(K.mult(H_))).mult(P_);
}

}