ios - 粒子过滤器 iOS 实现失败

Question

这个问题快把我逼疯了，我真的需要一些帮助。

我必须在 iOS 中实现粒子过滤器，我从 Java 中的工作代码开始。

该算法与 Thrun 在 Udacity (https://www.udacity.com/course/artificial-intelligence-for-robotics--cs373) 类(class)“机器人人工智能”中描述的算法非常接近

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

每个 Action 是:

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

@结束

Answer 1

在 OP 发布任何代码之前，这个答案被发布为“我如何找到 x 的问题”。

对于要查看的代码，这可能不再相关，特别是因为 OP 断言已遵循下面列出的所有步骤。

这是一个模糊的答案，但这是一个模糊的问题..

我们不可能在这里提供任何建议或指导，因为读者真正可以获得的唯一信息是您怀疑问题出在随机数生成 。

这是你的儿子吗？

为了作为读者回答这个问题，我们基本上是在比较互联网上陌生人(您)犯错的几率VS一个 ios 开发人员团队犯了错误，而这个错误却被忽视了。不幸的是，我们不得不将赌注押在您的错这个错误上。

程序员的经验法则:在指责别人的代码之前先测试您的代码。如果您认识要责备的人，请再次测试。如果你要责备的人每天都看到你，那就测第三次。如果你在你要责备的人的攻击范围内，再测试一次。

解决问题(又名如何找到并修理你的狗)

解决任何编码问题的第一步是识别问题。

与其一次又一次地随意查看您的代码以查找错误(StackOverflow 用户同意(c)，您将永远找不到错误):

1. 创建一些单元测试来验证您的代码预期并确保您做的事情是正确的。 如果你是，那么..
2. 如果您测试的代码单元都运行正常，请检查它们在组合时是否运行正确。创建检查操作链的测试。链越长，您将测试的应用程序的端到端功能就越多。 还是不行？您可以考虑...
3. 在您的应用程序运行时对其进行远程调试，以准确了解发生了什么。 无法进行远程调试？你让这件事变得很困难，但是......
4. 从您的应用中记录一小组数据。创建一个测试以通过您的代码运行您的数据集。验证期望。 您仍然无法让它工作？然后尝试...
5. 在 StackOverflow 上发布您的问题，并附上相关代码和数据集。

基本上测试，测试，测试

如果您的应用处于积极开发阶段，从长远来看，进行的每项测试都会为您节省时间(非常成熟和稳定的代码将受益较少)。