Particle Filter iOS implementation fails - ios

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

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):
Create some unit tests to verify your code expectations and make sure that you're doing things right. If you are, then..
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...
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...
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...
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).

Related

How to calculate speed and distance from acceleration in iOS

Okay, this is an odd one - and the question has been asked before in various guises and always closed out with the opinion that a) it won't be accurate and b) that it will get less accurate with time.
I understand this - but I'm doing an experiment to see whether that accuracy can be boosted at all by taking into account other evidential sources (for example, if a map has been plotted in advance then the direction of travel from the compass combined with the route could provide another evidence source).
The problem is that my code is clearly rubbish - so I'd welcome your opinion. I suspect that this might be a brown paper bag error!
My ViewController.h looks like this:
#import <UIKit/UIKit.h>
#interface ViewController : UIViewController <UIAccelerometerDelegate> {
UIAccelerometer *accelerometer;
long last_speed;
long distance_travelled;
long lastAccel;
long long lastTime;
IBOutlet UITextField* speedView;
IBOutlet UITextField* distanceView;
}
#end
And my ViewController.m looks like this:
#import "ViewController.h"
#implementation ViewController
- (void)viewDidLoad {
[super viewDidLoad];
// Do any additional setup after loading the view, typically from a nib.
lastAccel = 0;
last_speed = 0;
distance_travelled = 0;
lastTime = (long)(NSTimeInterval)([NSDate.date timeIntervalSince1970] * 1000.0);
accelerometer = UIAccelerometer.sharedAccelerometer;
accelerometer.updateInterval = 0.1;
accelerometer.delegate = self;
}
- (void)accelerometer:(UIAccelerometer *)meter
didAccelerate:(UIAcceleration *)acceleration
{
long long currentTime = (long)(NSTimeInterval)([NSDate.date timeIntervalSince1970] * 1000.0);
long long deltaTime = currentTime - lastTime;
lastTime = currentTime;
long accel_x = acceleration.x;
long accel_y = acceleration.y;
long accel_z = acceleration.z;
long integrated_acceleration = sqrtl(accel_x*accel_x + accel_y*accel_y + accel_z*accel_z);
long average_acceleration = (lastAccel + integrated_acceleration) / 2;
long speed = last_speed + (average_acceleration * deltaTime);
long average_speed = (speed + last_speed) / 2;
distance_travelled = distance_travelled + (average_speed * deltaTime);
last_speed = speed;
lastAccel = integrated_acceleration;
[speedView setText:[NSString stringWithFormat:#"%ld", speed]];
[distanceView setText:[NSString stringWithFormat:#"%ld", distance_travelled]];
}
#end
When the code is run, the speed and the distance keep going up continually, and deceleration (me slowing down) is never taken into account - so even when I stop, speed and distance keep going up.
Even by the standards of 'this will be a bit inaccurate' that's taking it too far!
All thoughts and opinions gratefully received (well almost - I know that it won't be accurate!)
You can't calculate Speed using UIAccelerometer.
Please find the link for more clarification : How to calculate speed using accelerometer in ios

Random number that doesn't repeat twice in a row [duplicate]

This question already has answers here:
Non repeating random numbers in Objective-C
(6 answers)
Closed 7 years ago.
I'm pretty new to coding, I've been looking for similar questions but none fits my needs. I'm working in a dice rolling app, and I need a a random number generator to "roll" the dice. arc4random seems perfect, but the problems that I can't have the same face occurring twice in a row. I have a method firing when I press the button with a timer
- (IBAction)dieRoll:(id)sender {
self.currentFace = 1;
_timer = [NSTimer scheduledTimerWithTimeInterval:0.25 target:self selector:#selector(roll) userInfo:nil repeats:YES];;
}
but I have to implement the 'roll' method where I get a random number different from the one already selected (the property self.currentFace).
any clue?
this is how your implementation could look like:
#interface ViewController ()
#property (assign, nonatomic) NSInteger currentFace;
#end
#implementation ViewController
- (void)viewDidLoad {
[super viewDidLoad];
self.currentFace = -1;
}
- (IBAction)diceRoll:(id)sender {
NSInteger newFace = -1;
do {
newFace = arc4random_uniform(6) + 1;
} while (newFace == self.currentFace);
self.currentFace = newFace;
}
#end
A simple solution would be to just remember the last rolled number and roll the dice until you get a different one. Pretty simple and you can keep arc4random.
An example:
- (NSUInteger)rollDiceWithLastFaceNumber:(NSUInteger)lastFaceNumber
{
NSUInteger currentFaceNumber;
do {
currentFaceNumber = (arc4random_uniform(6) + 1);
} while (currentFaceNumber == lastFaceNumber);
return currentFaceNumber;
}
and how to use it:
[self rollDiceWithLastFaceNumber:3];
This solution avoids an unknown number of iterations until you get your result.
- (void)roll {
NSUInteger next = [self nextFaceWithPreviousFace:self.currentFace];
NSLog(#"%lu", next);
self.currentFace = next;
}
- (NSUInteger)nextFaceWithPreviousFace:(NSUInteger)previous {
NSMutableArray *candidates = [NSMutableArray arrayWithObjects:#1, #2, #3, #4, #5, #6, nil];
[candidates removeObject:#(previous)];
NSUInteger index = arc4random_uniform((unsigned)candidates.count);
return [candidates[index] unsignedIntegerValue];
}

How to Create Dynamic Value Which Can use Entire App

Basiclly, I have a button which created value according to finish some math process like +, - , /, *. Question is when I get some value, I needed to use that value again when I press button second time. Let me explain Basicly,
When I clicked button process like,
int examplea = [txt_example.text intValue];
int exB = 5000;
int exC = exB - examplea;
This is the first time I push the button, my lastest value is exC When I input text field another value and clicked it same process will start but one difference:
int examplea = [txt_example.text intValue];
int exB = exC;
int exC ( this new value which will be calculated ) = exB - examplea;
How can I create process like this?
Something like this
in the .h file
#interface MyCalculator : NSObject
- (int) doCalculation;
#property (assign, nonatomic) int exB;
#end
in the .m file
#implementation MyCalculator
- (id) init
{
self = [super init];
if (self)
{
_exB = 5000;
}
return self;
}
- (int) doCalculation
{
int examplea = [txt_example.text intValue];
int exC = self.exB - examplea;
self.exB = exC;
return exC;
}
#end
....
MyCalculator* myCalculator = [[MyCalculator alloc] init];
...
[myCalculator doCalculation];
This isn't the full solution as there are several questions about what your code will do, but it will illustrate the principle of how to do it.
The last line is what gets called when the button is pressed.
I don't know where txt_example.text is coming from, but it might be better if that is passed to doCalculation as a parameter.

How to get all CFTree Siblings on same depth?

I am working on CFTree. I need to get all siblings on same depth for example in following figure I need an array of all 8 CFTreeRef on 3rd depth. How can I get that?
Following your depth numbering - i.e. starting with root at 1, not 0:
-(void)allNodesFromTree:(CFTreeRef)node currentDepth:(int)currDepth atDepth:(int)depth nodesFound:(NSMutableArray*)nodes
{
bool atTargetDepth = depth -1 == currDepth;
CFTreeRef curChild = CFTreeGetFirstChild(node);
for (; curChild; curChild = CFTreeGetNextSibling(curChild)) {
if(atTargetDepth) //stop recursion if target depth reached
{
[nodes addObject:(__bridge id)(curChild)];
}
else [self allNodesFromTree:curChild currentDepth:currDepth+1 atDepth:depth nodesFound:nodes];
}
}
To make things a bit cleaner, you might wrap it in a helper function:
-(NSMutableArray*)allNodesFromTree:(CFTreeRef)node atDepth:(int)depth
{
NSMutableArray *nodesArray = [[NSMutableArray alloc]init];
[self allNodesFromTree:node currentDepth:0 atDepth:depth nodesFound:nodesArray];
return nodesArray;
}
Then you can just call
NSMutableArray *nodes = [self allNodesFromTree:root atDepth:3];

NSMutableArray replaceObjectAtIndex:withObject Loop Leakage

I've edited the previous post with some working code for convenience.
The follow code (ARC'ed) seems to leak and will crash the sim or the device after running for a short period of time:
#define kROWS 100
#define kCols 34
void run();
static ViewController *instance;
#interface ViewController ()
#property (nonatomic, strong) NSMutableArray *nsBackColor;
#end
#implementation ViewController
- (void)viewDidLoad
{
[super viewDidLoad];
// Do any additional setup after loading the view, typically from a nib.
self.nsBackColor = [NSMutableArray arrayWithCapacity:1];
instance = self;
// set up a '2D array'
for (int x = 0; x < kROWS; x++) {
[self.nsBackColor addObject:[NSMutableArray arrayWithCapacity:1]];
for (int y = 0; y < kCols; y++) {
[[self.nsBackColor objectAtIndex:x] addObject:[UIColor whiteColor]];
}
}
dispatch_async(dispatch_get_global_queue(DISPATCH_QUEUE_PRIORITY_DEFAULT, 0), ^{
run();
});
}
- (void)plotColor:(UIColor *)color atX:(short)x andY:(short)y {
[[self.nsBackColor objectAtIndex:x] replaceObjectAtIndex:y withObject:color];
}
- (void)didReceiveMemoryWarning
{
[super didReceiveMemoryWarning];
// Dispose of any resources that can be recreated.
}
#end
void plot(short xLoc, short yLoc,
short backRed, short backGreen, short backBlue) {
#autoreleasepool {
[instance plotColor:[UIColor colorWithRed:((float)backRed/100)
green:((float)backGreen/100)
blue:((float)backBlue/100)
alpha:(float)1]
atX:xLoc andY:yLoc];
}
}
void run() {
short x = 0;
short y = 0;
short backRed = 0;
short backGreen = 0;
short backBlue = 0;
while (true) {
x++;
if (x >= kROWS) {
x = 0;
}
y++;
if (y >= kCols) {
y = 0;
}
backRed = arc4random() % 255;
backBlue = arc4random() % 255;
backGreen = arc4random() % 255;
plot(x, y, backRed, backGreen, backBlue);
usleep(1000);
}
}
If I let this run on the device or the simulator long enough (a couple minutes) I'll receive either an mmap malloc error (sim) or a memory warning (device) and crash.
Going through Instruments Allocations I can see +[UIColor colorWithRed:green:blue:alpha:] start to ballon until finally hitting the memory wall.
I can assign the UIColor to a property (directly or by doing a copy), say self.myColor = color, and there is no such leak.
I can do this too:
[[self.nsBackColor objectAtIndex:x] replaceObjectAtIndex:y withObject:[self description]];
and I get the same leak.
It seems to me that object replaced in the array (and yes, this did originally start as a 2D c array but I thought that was the issue) is forever lost and leaked and not properly released.
This would be the Instruments->Allocations after running for a short period of time:
Any help would be much appreciated and more information can be provided.
What is happening is that you are creating an autorelease pool at every iteration.
So the following line runs every time with a new autorelease pool:
[[self.nsBackColor objectAtIndex:x] replaceObjectAtIndex:y withObject:color];
Therefore color increases its reference count within the local pool by one, while [[self.nsBackColor objectAtIndex:x] objectAtIndex:y] decreases its reference count within the local pool by one. But here's the catch: That item was color in the previous iteration, which had its reference count managed by the previous pool that you drained/released earlier.
So what should have happened is as the previous pool was released in the previous iteration, that object was released. Its reference count was 2 (one for [UIColor colorWith...], one for adding to the array), so it should have received 2 release messages as soon as the pool was drained, and the pointer in [[self.nsBackColor objectAtIndex:x] objectAtIndex:y] should be left dangling until you replace it in the current iteration with a pointer to color.
Clearly that's not happening exactly the way it's supposed to be, or the way I understand it. However, the #autoreleasepool {} directive is clearly misplaced. It should be around the while (true) loop, or removed altogether in favor of the thread's pool.

Resources