Эта проблема сводит меня с ума, и мне действительно нужна помощь.
Мне нужно реализовать фильтр частиц в iOS, и я начал с рабочего кода на Java.
Алгоритм очень близок к описанному в курсе "Искусственный интеллект для робототехники" Труна на Udacity (https://www.udacity.com/course/artificial-intelligence-for-robotics--cs373).)
Реализация Java имеет такое поведение: я перемещаю робота, частицы все ближе и ближе приближаются к роботу, а затем следуют за ним с очень низкой ошибкой. Я думаю, это ожидаемое поведение. Меняя количество частиц, ориентиров и шума, я получаю лучшие или худшие результаты.
Поведение iOS-реализации совсем другое: я двигаю робота, частицы приближаются к роботу, но после нескольких итераций начинают удаляться от робота. Первоначально среднее значение все еще близко к роботу, но затем оно прыгает далеко. Меняя количество частиц, количество ориентиров и шума, я получаю почти тот же результат.
Я дважды проверил код iOS и, похоже, он правильный.
Я подозревал, что проблема связана с генерацией случайных чисел и/или гауссовской случайной генерацией.
Я изменил свой код, чтобы использовать именно тот код, который использует библиотека Java (http://docs.oracle.com/javase/7/docs/api/java/util/Random.html#nextGaussian()) с теми же результатами.
Я переместил генератор гауссовых случайных чисел в синглтон, чтобы использовать один и тот же генератор для всех частиц, но ничего не изменилось.
Я пытался изменить drand48() на ((double)arc4random() / (double)UINT32_MAX), получая те же результаты.
У меня нет других идей.
Я не прошу вас отлаживать мой код, но, пожалуйста, дайте мне совет, чтобы решить эту проблему.
ИЗМЕНИТЬ 1
Может эти картинки помогут
С этого шага частицы удаляются от робота
ИЗМЕНИТЬ 2
Это мой класс частиц:
@interface Particle ()
@property (nonatomic, strong) MyPoint *point;
@property double orientation;
@property NSInteger worldWidth;
@property NSInteger worldHeight;
@property double probability;
@property double forwardNoise;
@property double turnNoise;
@property double senseNoise;
@property (nonatomic,strong) Utils *utils;
@end
@implementation Particle
-(instancetype)initWithWorldWidth:(float)worldWidth worldHeight:(float)worldHeight {
self = [super init];
if (self) {
_worldWidth = worldWidth;
_worldHeight = worldHeight;
_point = [[MyPoint alloc] initWithX:drand48() * _worldWidth
Y:drand48() * _worldHeight];
_orientation = drand48() * 2. * M_PI;
_forwardNoise = 0;
_turnNoise = 0;
_senseNoise = 0;
_utils = [Utils sharedInstance];
}
return self;
}
-(void)setPosition:(MyPoint *)p orientation:(float)orientation probability:(double)probability {
_point.x = p.x;
_point.y = p.y;
_orientation = orientation;
_probability = probability;
}
-(void)setNoise:(double)forwardNoise turnNoise:(double)turnNoise senseNoise:(double)senseNoise {
_forwardNoise = forwardNoise;
_turnNoise = turnNoise;
_senseNoise = senseNoise;
}
-(MyPoint *)getPosition {
return _point;
}
-(double)getOrientation {
return _orientation;
}
-(double)getProbability {
return _probability;
}
-(double)getForwardNoise {
return _forwardNoise;
}
-(double)getTurnNoise {
return _turnNoise;
}
-(double)getSenseNoise {
return _senseNoise;
}
-(NSArray<NSNumber *>*)sense:(NSArray<Landmark*>*)landmarks {
NSMutableArray<NSNumber*> *measures = [[NSMutableArray alloc] init];
for(int i=0; i<landmarks.count; i++) {
Landmark *landmark = landmarks[i];
double distance = [Utils distanceBetweenP1:_point andP2:landmark];
double measure = distance + [_utils box_muller:0 :1.] * _senseNoise;
[measures addObject:[NSNumber numberWithDouble:measure]];
}
return measures;
}
-(void)moveForward:(double)forward withTurn:(double)turn {
//NSLog(@"---- Move ---- forward: %f ---- %f",forward,turn);
double a1 = [_utils box_muller:0. :1.];
//NSLog(@"\ta1=%.8f",a1);
_orientation = _orientation + turn + a1 * _turnNoise;
_orientation = [Utils circle:_orientation :2*M_PI];
double a2 = [_utils box_muller:0. :1.];
//NSLog(@"\ta2=%.8f",a2);
double dist = forward + a2 * _forwardNoise;
_point.x += cos(_orientation) * dist;
_point.y += sin(_orientation) * dist;
_point.x = [Utils circle:_point.x :_worldWidth];
_point.y = [Utils circle:_point.y :_worldHeight];
}
-(double)measurementProb:(NSArray<NSNumber *> *)measurements landmarks:(NSArray<Landmark *>*)landmarks {
double prob = 1.0;
for(int i=0; i<measurements.count; i++) {
Landmark *landmark = landmarks[i];
double measurement = [measurements[i] doubleValue];
double dist = [Utils distanceBetweenP1:_point andP2:landmark];
prob *= [Utils gaussian:dist :_senseNoise :measurement];
}
_probability = prob;
return prob;
}
Это мой фильтр частиц:
#import "ParticleFilter.h"
@interface ParticleFilter ()
@property (nonatomic,strong) NSMutableArray<Particle *> *particles;
@property (nonatomic,strong) NSArray<Landmark *> *landmarks;
@property NSInteger worldWidth;
@property NSInteger worldHeight;
@end
@implementation ParticleFilter
-(instancetype)initWithLandmarks:(NSArray<Landmark*>*)landmarks numberOfParticles:(NSInteger)numberOfParticles worldWidth:(float)worldWidth worldHeight:(float)worldHeight {
self = [super init];
if (self) {
_worldWidth = worldWidth;
_worldHeight = worldHeight;
_particles = [[NSMutableArray alloc] init];
for (int i = 0; i < numberOfParticles; i++) {
[_particles addObject:[[Particle alloc] initWithWorldWidth:worldWidth worldHeight:worldHeight]];
}
_landmarks = [NSArray arrayWithArray:landmarks];
}
return self;
}
-(void)setNoise:(double)forwardNoise turnNoise:(double)turnNoise senseNoise:(double)senseNoise {
for (Particle *p in _particles) {
[p setNoise:forwardNoise turnNoise:turnNoise senseNoise:senseNoise];
}
}
-(void)moveForward:(double)forward withTurn:(double)turn {
for (Particle *p in _particles) {
[p moveForward:forward withTurn:turn];
}
}
-(void)resample:(NSArray<NSNumber *>*)measurements {
NSMutableArray<Particle *> *newParticles = [[NSMutableArray alloc] init];
for (Particle *p in _particles) {
[p measurementProb:measurements landmarks:_landmarks];
}
double B = 0;
Particle *bestParticle = [self getBestParticle];
NSInteger index = drand48() * _particles.count;
for (int i = 0; i < _particles.count; i++) {
B += drand48() * 2. * [bestParticle getProbability];
while (B > [_particles[index] getProbability]) {
B -= [_particles[index] getProbability];
index = [self circle:index+1 :_particles.count];
}
[newParticles addObject:_particles[index]];
}
[_particles removeAllObjects];
[_particles addObjectsFromArray:newParticles];
}
-(NSInteger)circle:(NSInteger) num :(NSInteger)length {
while(num > length - 1)
num -= length;
while(num < 0)
num += length;
return num;
}
-(Particle *)getAverageParticle {
Particle *p = [[Particle alloc] initWithWorldWidth:_worldWidth worldHeight:_worldHeight];
double x = 0;
double y = 0;
double orient = 0;
double prob = 0;
for(int i=0; i<_particles.count; i++) {
x += [_particles[i] getPosition].x;
y += [_particles[i] getPosition].y;
orient += [_particles[i] getOrientation];
prob += [_particles[i] getProbability];
}
x /= _particles.count;
y /= _particles.count;
orient /= _particles.count;
prob /= _particles.count;
[p setPosition:[[MyPoint alloc] initWithX:x Y:y]
orientation:orient
probability:prob];
[p setNoise:[_particles[0] getForwardNoise]
turnNoise:[_particles[0] getTurnNoise]
senseNoise:[_particles[0] getSenseNoise]];
return p;
}
Каждое движение это:
[_robot moveForward:2. withTurn:0];
[_particleFilter moveForward:2. withTurn:0];
NSLog(@"%@",_particleFilter);
NSLog(@"Mean %@",[_particleFilter getAverageParticle]);
NSArray<NSNumber*> *measurements = [_robot sense:_landmarks];
[_particleFilter resample:measurements];
NSLog(@"%@",_particleFilter);
NSLog(@"Mean %@",[_particleFilter getAverageParticle]);
NSLog(@"Robot %@",_robot);
NSLog(@"Estimated Robot %@",[_particleFilter getAverageParticle]);
NSLog(@"Best Robot %@",[_particleFilter getBestParticle]);
Здесь код со случайными числами
#import "Utils.h"
@interface Utils ()
@property BOOL haveNextNextGaussian;
@property double nextNextGaussian;
@end
@implementation Utils
+ (instancetype) sharedInstance {
static id sharedInstance = nil;
static dispatch_once_t onceToken;
dispatch_once(&onceToken, ^{
sharedInstance = [[super alloc] initInstance];
});
return sharedInstance;
}
-(instancetype)initInstance {
self = [super init];
if (self) {
srand48(arc4random());
}
return self;
}
+(double)distanceBetweenP1:(MyPoint *)p1 andP2:(MyPoint *)p2 {
return sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y));
}
+(double)gaussian:(double)mu :(double)sigma :(double)x {
return exp(-(pow(mu - x, 2.)) / pow(sigma, 2.) / 2.0) / sqrt(2.0 * M_PI * pow(sigma, 2.));
}
-(double)box_muller:(double)m :(double)s {
if (_haveNextNextGaussian) {
_haveNextNextGaussian = NO;
return _nextNextGaussian;
} else {
double v1, v2, s;
do {
v1 = 2 * drand48() - 1; // between -1.0 and 1.0
v2 = 2 * drand48() - 1; // between -1.0 and 1.0
s = v1 * v1 + v2 * v2;
}
while (s >= 1 || s == 0);
double multiplier = sqrt(-2 * log(s)/s);
_nextNextGaussian = v2 * multiplier;
_haveNextNextGaussian = YES;
return v1 * multiplier;
}
}
+(double)circle:(double) num :(double)length {
while(num > length - 1)
num -= length;
while(num < 0)
num += length;
return num;
}
@конец
