用JAVA和Python編寫的相同代碼給出不同的結果（粒子過濾器）

Question

我正在研究粒子過濾器：

我有一個3D位置觀測值，從加速度和角速率中計算出新粒子的位置並加上一些高斯噪聲，然后根據粒子與觀測值之間的距離對它們進行重新采樣。

我首先用Python（3.7.3）開發該軟件，但現在需要用JAVA編寫它（我對JAVA還是很陌生）。 我對粒子選擇存在問題，在Python中它工作得很好，重采樣過程會產生良好的粒子，這些粒子傾向於保留在觀察范圍內，但在JAVA中會發散。

為了重現這種差異，我嘗試使用Python和JAVA編寫相同的代碼，僅使用10個粒子對靜態演化（起始時間為[0,0,0]，加速度和角速度在每個時間戳均為零）進行建模。 我對所有我用來確保他們按要求做的功能進行了單元測試。

Python代碼：

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))

JAVA代碼：

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;
    }
}

我在歸一化之前將粒子的權重打印為散度指標。 （您也可以監視平均粒子的位置）。 如您所見，Python代碼產生適當的權重（粒子雲傾向於留在原點附近），而JAVA代碼產生的權重收斂為0。

編輯：我在C ++中編寫了相同的代碼，它也可以正常工作..然后，我使用了一個轉換工具，從我的C ++代碼中獲取了JAVA代碼，但它仍然不起作用。

Answer 1

我通過使用一個函數復制了generateNewParticles函數的返回值來解決了這個問題！ 我不知道為什么它以前不起作用（我想可能是一些參考問題，我不習慣JAVA ...）。 這是代碼：

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;
    }