Same code written in JAVA and Python gives different results (particle filter)

Question

I am working on particle filter:

I have a 3D position observation, from acceleration and angular rate I compute new particles' positions adding some Gaussian noise, and depending of how far the particles are from the observation, I resample them.

I first develop the software in Python (3.7.3) but I now need to write it in JAVA (I am pretty new to JAVA). I have a problem with the particles selection, in Python it works just fine, the resample process yields good particles which tend to stay around the observation but in JAVA it diverges.

To reproduce this divergence, I tried to write the same code in Python and JAVA, modeling a static evolution (starting point is [0,0,0], acceleration and angular rate are null at each timestamp) using only 10 particles. I unit-tested all the function I used to make sure they do what they claim to do.

The Python code:

import numpy as np
def pf(particles,acc,gyr,obs):
    sigma = .5 #define the trust you have in your observation 
    weights = np.zeros((10,))
    n = len(particles)
    for i in range(n):
        #Compute new position adding a Gaussian noise
        particles[i][0] += np.random.normal()
        particles[i][1] += np.random.normal()
        particles[i][2] += np.random.normal()
        #Compute particles' weights
        p = np.exp(-np.linalg.norm(obs-particles[i][:3])**2/(2*sigma*sigma))/(np.sqrt(2*np.pi)*sigma)
        weights[i] = p
        print(p)
    #Normalize weights
    weights = weights/sum(weights)

    #Resampling using sytematic resampling
    new_particles = np.zeros((n,10))
    j=0
    sum_w = weights[0]
    u = np.random.rand()/n
    for i in range(n):
        while sum_w < u :
            j+=1
            sum_w += weights[j]
        new_particles[i] = particles[j]
        u+=1/n
    return new_particles

#Simple test
particles = np.zeros((10,10))
for i in range(100):
    particles = pf(particles,np.zeros(3),np.zeros(3),np.zeros(3))

The JAVA code:

import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
import java.util.Random;

public class Main {

    public static void main(String[] args) {
        double[][] particles = new double[10][10];
        for (int i = 0; i < 100; i++) {
            particles = generateNewParticles(particles, new double[3],new double[3], new double[3]);
        }
    }
    private static double[][] generateNewParticles(double[][] particles, double[] acc, double[] gyr, double[] observation) {
        Vector3D obs = new Vector3D(observation[0], observation[1], observation[2]);
        double sigma = 0.5;
        int n = particles.length;
        double[] weights = new double[n];

        for (int i = 0; i < n; i++) {

            particles[i][0] += new Random().nextGaussian();
            particles[i][1] += new Random().nextGaussian();
            particles[i][2] += new Random().nextGaussian();

            Vector3D diff = obs.subtract(new Vector3D(particles[i][0],particles[i][1],particles[i][2]));
            double p = Math.exp(-Math.pow(diff.getNorm(),2) / (2 * sigma * sigma)) / (Math.sqrt(2 * Math.PI) * sigma);
            weights[i] = p;
            System.out.println(p);
        }

        //Normalize the weights
        double ss = sum(weights);
        for (int i = 0; i < n; i++) {
            weights[i] /= ss;
        }

        //Resampling
        double[][] newParticles = new double[n][10];
        int j = 0;
        double sum_w = weights[0];
        double u = Math.random() / n;
        for (int i = 0; i < n; i++) {
            while (sum_w < u) {
                j+=1;
                sum_w += weights[j];
            }
            newParticles[i] = particles[j];
            u += 1. / n;
        }
        return newParticles;
    }
    private static double sum(double[] array){
        double s = 0;
        for (double value : array) {
            s += value;
        }
        return s;
    }
}

I printed the particles' weights before normalization as a divergence indicator. (You can also monitor mean particles' positions). As you can see the Python code yields proper weights (the particle cloud tends to stay around the origin) while the JAVA code yields weights converging to 0.

EDIT: I wrote the same code in C++ and it works just fine too.. Then I used a converter tool to get a JAVA code from my C++ code and it still does not work.

Answer 1

I solved it by using a function to copy the return of my generateNewParticles function ! I don't know why it doesn't work before (some reference problem going on I suppose, I am not used to JAVA...). Here is the code:

particles = makeClone(generateNewParticles(particles, new double[3],new double[3], new double[3]));

....

private static double[][] makeClone(double[][] in) {
        int n = in.length;
        double[][] out = new double[n][in[0].length];
        for (int i = 0; i < n; i++) {
            out[i] = in[i].clone();
        }
        return out;
    }