Particle Filter iOS implementation fails

Question

This problem is driving me crazy and I actually need some help.

I have to implement a particle filter in iOS and I started from a working code in Java.

The algorithm is very close to which is described in the course "Artificial Intelligence for Robotics" of Thrun on Udacity (https://www.udacity.com/course/artificial-intelligence-for-robotics--cs373)

The Java implementation has this behavior: I move the robot, the particles get closer and closer to the robot and then they follow it with a very low error. This is the expected behavior, I think. Changing number of particles, number of landmarks and noise, I get better or worse results.

The iOS implementation behavior is quite different: I move the robot, the particles get closer to the robot but after few iterations they start moving far from the robot. Initially the mean value is still close to the robot but then it jumps far. Changing number of particles, number of landmarks and noise, I get to almost the same result.

I double checked the iOS code and apparently it's correct.

I suspected that the problem was related to random number generation and/or gaussian random generation.

I have changed my code to use exactly the code that Java library uses (http://docs.oracle.com/javase/7/docs/api/java/util/Random.html#nextGaussian()) getting the same results.

I moved the gaussian random generator to a singleton in order to share the same generator across all the particles, but nothing changed.

I have tried to change drand48() with ((double)arc4random() / (double)UINT32_MAX) getting the same results.

I have no other ideas.

I am not asking you to debug my code but please, give me any advice to sort this problem out.

EDIT 1

Maybe these pictures may help

From this step particles get far from the robot

EDIT 2

This is my Particle class:

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

This is my Particle filter:

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

Each movement is:

    [_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]);

Here the code involving random numbers

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

@end

Answer 1

This answer was posted as a "how do I find the problem with x" before the OP posted any code.

With code to look at, this may no longer be relevant, particularly since OP asserts that all steps listed below have been followed.

This is a vague answer, but it's a vague question..

It's not possible for us to give any advice or guidance here since the only piece of information really available to the reader is that you suspect that the problem lies in the random number generation.

This yor folt?

To answer this question as a reader, we're basically comparing the chances of a stranger (YOU) on the internet having made a mistake VS the chances of a mistake being made by a team of ios developers, and having that mistake go unnoticed. Unfortunately we have to put our bets on the mistake being your fault.

PROGRAMMER'S RULE OF THUMB: Test your code before blaming someone else's code. If you know the person you are about to blame, test it again. If the person you are about to blame sees you every day, test it a third time. If you are within punching distance of the person you are about to blame, test it once more.

Problem Solving (AKA How to Find and Fix Your Pooch Up)

The first step in solving any coding problem is identifying the problem.

Rather than haphazardly looking at your code for errors again and again (StackOverflow Users Agree(c) you will never find it):

1. Create some unit tests to verify your code expectations and make sure that you're doing things right. If you are, then..
2. If the units of code that you've tested are all behaving correctly, check that they behave correctly when combined. Create tests that check chains of operations. The longer the chains, the more of the end to end functionality of your app you will be testing. Still not working? You may consider...
3. Remote debug your application while it is running to see exactly what is going on. Can't get remote debug to work? You're making this hard, but...
4. Log a small set of data from your app. Create a test to run your data set through your code. Verify expectations. You still can't get it to work? Then try...
5. Post your question on StackOverflow, with relevant code and data set.

Basically TEST, TEST, TEST

Every test made will save you time in the long run if your app is in active development (very mature and stable code will benefit less).