Question

我正在研究颗粒过滤器：

我有一个3D位置观测，我从加速度和角速率中计算出新粒子的位置并加上一些高斯噪声，然后根据粒子与观测之间的距离，对其进行重新采样。

我首先用Python（3.7.3）开发该软件，但是现在我需要用JAVA编写它（我对JAVA还是很陌生）。我在粒子选择方面遇到问题，在Python中它工作得很好，重采样过程会产生良好的粒子，这些粒子倾向于保留在观察范围内，但在JAVA中会发散。

为了重现这种差异，我尝试使用Python和JAVA编写相同的代码，对静态演化进行建模（起始时间戳为[0,0,0]，每个时间戳的加速度和角速率均为零），仅使用10个粒子。我对所有我用来确保他们按照自己的意愿做的功能进行了单元测试。

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代码，但它仍然无法正常工作。< / p>

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

用JAVA和Python编写的相同代码给出不同的结果（粒子过滤器）

1 个答案: