-
Notifications
You must be signed in to change notification settings - Fork 1
/
LatitudeLongitudeParticleFilter.java
119 lines (101 loc) · 4.03 KB
/
LatitudeLongitudeParticleFilter.java
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
package com.intel.location.indoor.app.phoneunlockerguan;
import android.location.Location;
import java.util.Random;
import java.util.logging.Level;
import java.util.logging.Logger;
/**
* ParticleFilter class for particle filtering. Adapted from https://github.com/erhs-53-hackers/Particle-Filter/blob/master/src/particlefilter/Particle.java.
*
* Edited by: Grace Guan
* Last Edited: August 2016
*/
public class LatitudeLongitudeParticleFilter {
LatitudeLongitudeParticle[] particles;
int numParticles = 0;
Random gen = new Random();
public LatitudeLongitudeParticleFilter(int numParticles, Location[] landmarks, int width, int height) {
this.numParticles = numParticles;
particles = new LatitudeLongitudeParticle[numParticles];
for (int i = 0; i < numParticles; i++) {
particles[i] = new LatitudeLongitudeParticle(landmarks, width, height);
}
}
public void setNoise(float Fnoise, float Tnoise, float Snoise) {
for (int i = 0; i < numParticles; i++) {
particles[i].setNoise(Fnoise, Tnoise, Snoise);
}
}
public void move(float turn, float forward) throws Exception {
for (int i = 0; i < numParticles; i++) {
particles[i].move(turn, forward);
}
}
public void resample(float[] measurement) throws Exception {
LatitudeLongitudeParticle[] new_particles
= new LatitudeLongitudeParticle[numParticles];
for (int i = 0; i < numParticles; i++) {
particles[i].measurementProb(measurement);
}
float B = 0f;
LatitudeLongitudeParticle best = getBestParticle();
int index = (int) gen.nextFloat() * numParticles;
for (int i = 0; i < numParticles; i++) {
B += gen.nextFloat() * 2f * best.probability;
while (B > particles[index].probability) {
B -= particles[index].probability;
index = circle(index + 1, numParticles);
}
new_particles[i] = new LatitudeLongitudeParticle(particles[index].landmarks, particles[index].worldWidth, particles[index].worldHeight);
new_particles[i].set(particles[index].x, particles[index].y, particles[index].orientation, particles[index].probability);
new_particles[i].setNoise(particles[index].forwardNoise, particles[index].turnNoise, particles[index].senseNoise);
}
particles = new_particles;
}
private int circle(int num, int length) {
while (num > length - 1) {
num -= length;
}
while (num < 0) {
num += length;
}
return num;
}
public LatitudeLongitudeParticle getBestParticle() {
LatitudeLongitudeParticle particle = particles[0];
for (int i = 0; i < numParticles; i++) {
if (particles[i].probability > particle.probability) {
particle = particles[i];
}
}
return particle;
}
public LatitudeLongitudeParticle getAverageParticle() {
LatitudeLongitudeParticle p = new LatitudeLongitudeParticle(particles[0].landmarks, particles[0].worldWidth, particles[0].worldHeight);
float x = 0, y = 0, orient = 0, prob = 0;
for(int i=0;i<numParticles;i++) {
x += particles[i].x;
y += particles[i].y;
orient += particles[i].orientation;
prob += particles[i].probability;
}
x /= numParticles;
y /= numParticles;
orient /= numParticles;
prob /= numParticles;
try {
p.set(x, y, orient, prob);
} catch (Exception ex) {
Logger.getLogger(LatitudeLongitudeParticleFilter.class.getName()).log(Level.SEVERE, null, ex);
}
p.setNoise(particles[0].forwardNoise, particles[0].turnNoise, particles[0].senseNoise);
return p;
}
@Override
public String toString() {
String res = "";
for (int i = 0; i < numParticles; i++) {
res += particles[i].toString() + "\n";
}
return res;
}
}