Convert C++ to Java Code for FusionEKF Algorithm

import java.util.Vector;

import org.ejml.simple.SimpleMatrix;

public class FusionEKF { private boolean is_initialized_; private long previous_timestamp_;

private SimpleMatrix R_laser_;
private SimpleMatrix R_radar_;
private SimpleMatrix H_laser_;

private SimpleMatrix F;
private SimpleMatrix P;
private SimpleMatrix Q;
private Vector x;

private KalmanFilter ekf_;

/**
 * Constructor.
 */
public FusionEKF() {
    is_initialized_ = false;

    previous_timestamp_ = 0;

    // initializing matrices
    R_laser_ = new SimpleMatrix(2, 2);
    R_radar_ = new SimpleMatrix(3, 3);
    H_laser_ = new SimpleMatrix(2, 4);

    //measurement covariance matrix - laser
    R_laser_.set(0, 0, 0.0225);
    R_laser_.set(1, 1, 0.0225);

    //measurement covariance matrix - radar
    R_radar_.set(0, 0, 0.09);
    R_radar_.set(1, 1, 0.0009);
    R_radar_.set(2, 2, 0.09);

    H_laser_.set(0, 0, 1);
    H_laser_.set(1, 1, 1);

    F = new SimpleMatrix(4, 4);
    F.set(0, 0, 1);
    F.set(0, 2, 1);
    F.set(1, 1, 1);
    F.set(1, 3, 1);
    F.set(2, 2, 1);
    F.set(3, 3, 1);

    P = new SimpleMatrix(4, 4);
    Q = new SimpleMatrix(4, 4);

    x = new Vector(4);
    x.add(0.0);
    x.add(0.0);
    x.add(1.0);
    x.add(1.0);

    ekf_ = new KalmanFilter();
    ekf_.Init(x, P, F, H_laser_, R_laser_, Q);
}

/**
 * Destructor.
 */
public void finalize() throws Throwable {}

public void ProcessMeasurement(MeasurementPackage measurement_pack) {
    /**
    * Initialization
    */
    if (!is_initialized_) {
        /**
        *  Initialize the state ekf_.x_ with the first measurement.
        */

        // first measurement
        System.out.println('EKF: ');

        if (measurement_pack.sensor_type_ == MeasurementPackage.SensorType.RADAR) {
            // Convert radar from polar to cartesian coordinates
            float ro = measurement_pack.raw_measurements_.get(0);
            float theta = measurement_pack.raw_measurements_.get(1);
            float ro_dot = measurement_pack.raw_measurements_.get(2);

            float px  = ro * Math.cos(theta);
            float py = ro * Math.sin(theta);
            float vx = ro_dot * Math.cos(theta);
            float vy = ro_dot * Math.sin(theta);

            x.set(0, px);
            x.set(1, py);
            x.set(2, vx);
            x.set(3, vy);

            P.set(0, 0, 1);
            P.set(1, 1, 1);
            P.set(2, 2, 1);
            P.set(3, 3, 1);
        }
        else if (measurement_pack.sensor_type_ == MeasurementPackage.SensorType.LASER) {
            // Initialize state
            float px = measurement_pack.raw_measurements_.get(0);
            float py = measurement_pack.raw_measurements_.get(1);

            x.set(0, px);
            x.set(1, py);
            x.set(2, 0.0);
            x.set(3, 0.0);

            P.set(0, 0, 1);
            P.set(1, 1, 1);
            P.set(2, 2, 1000);
            P.set(3, 3, 1000);
        }

        previous_timestamp_ = measurement_pack.timestamp_;

        // done initializing, no need to predict or update
        is_initialized_ = true;
        return;
    }

    /**
    * Prediction
    */

    float dt = (measurement_pack.timestamp_ - previous_timestamp_) / 1000000.0f;
    previous_timestamp_ = measurement_pack.timestamp_;

    // Modify the F matrix so that the time is integrated
    F.set(0, 2, dt);
    F.set(1, 3, dt);

    // set the process covariance matrix Q
    float dt_2 = dt * dt;
    float dt_3 = dt_2 * dt;
    float dt_4 = dt_3 * dt;

    float noise_ax = 9;
    float noise_ay = 9;

    Q.set(0, 0, dt_4/4 * noise_ax);
    Q.set(0, 2, dt_3/2 * noise_ax);
    Q.set(1, 1, dt_4/4 * noise_ay);
    Q.set(1, 3, dt_3/2 * noise_ay);
    Q.set(2, 0, dt_3/2 * noise_ax);
    Q.set(2, 2, dt_2 * noise_ax);
    Q.set(3, 1, dt_3/2 * noise_ay);
    Q.set(3, 3, dt_2 * noise_ay);

    ekf_.Predict();

    /**
    * Update
    */

    if (measurement_pack.sensor_type_ == MeasurementPackage.SensorType.RADAR) {
        // Radar updates
        ekf_.H_ = tools.CalculateJacobian(ekf_.x_);
        ekf_.R_ = R_radar_;

        ekf_.UpdateEKF(measurement_pack.raw_measurements_);

    } else {
        // Laser updates
        ekf_.H_ = H_laser_;
        ekf_.R_ = R_laser_;

        ekf_.Update(measurement_pack.raw_measurements_);

    }

    // print the output
    System.out.println('x_ = ' + ekf_.x_.toString());
    System.out.println('P_ = ' + ekf_.P_.toString());
}

}