iPhone4 iOS5 Sensor fusion for short range inertial navigation - ios5

Question

I've taken a look around, and there aren't that many talks or examples on inertial navigation for iOS5. I know that iOS5 introduced some very cool sensor fusion algorithms:
motionQueue = [[NSOperationQueue alloc] init];
[motionQueue setMaxConcurrentOperationCount:1];
motionManager = [[CMMotionManager alloc] init];
motionManager.deviceMotionUpdateInterval = 1/20.0;
[motionManager startDeviceMotionUpdatesUsingReferenceFrame:CMAttitudeReferenceFrameXTrueNorthZVertical toQueue:motionQueue withHandler:^(CMDeviceMotion *motion, NSError *error) {
}];
I've taken a look at both videos from WWDC that deal with the block above.
The resulting CMDeviceMotion contains a device attitude vector, along with the acceleration separated from the user induced acceleration.
Are there any open source inertial navigation projects specifically for iOS 5 that take advantage of this new sensor fusion ? I'm talking about further integrating this data with the GPS and magnetometer output to get a more accurate GPS position.
A bonus question: is this kind of fusion even possible from a hardware standpoint? Will I melt my iPhone4 if I start to do 20hz processing of all available sensor data over extended periods of time?
I'm ready to start tinkering with these, but would love to get something more solid to start with than the empty block above :)
Thank you for any pointers!

Answer 1

I am writing an app for scuba divers and hoped to add inertial navigation since GPS and other radio based navigation is unavailable underwater. I did quite a bit of research and found that there is just too much jitter in the sensor data on the iPhone for accurate inertial navigation. I did a quick experiment and found that even when the device is perfectly still, the "drift" due to noise in the signal showed that the device "moved" many meters after only a few minutes. Here is the code I used in my experiment. If you can see something I am doing wrong, let me know. Otherwise, I want my afternoon back!
- (void)startCoreMotion {
CMMotionManager *manager = [[CMMotionManager alloc] init];
if ([manager isAccelerometerAvailable]) {
manager.deviceMotionUpdateInterval = 1.0/updateHz;
[manager startDeviceMotionUpdatesToQueue:[NSOperationQueue mainQueue] withHandler:^(CMDeviceMotion *motion, NSError *error) {
xVelocity += (( 9.8 * motion.userAcceleration.x ) / updateHz);
yVelocity += (( 9.8 * motion.userAcceleration.y ) / updateHz);
zVelocity += (( 9.8 * motion.userAcceleration.z ) / updateHz);
xPosition += ( xVelocity * ( 1 / updateHz ));
yPosition += ( yVelocity * ( 1 / updateHz ));
zPosition += ( zVelocity * ( 1 / updateHz ));
self.xPositionLabel.text = [NSString stringWithFormat:#"x = %f m", xPosition];
self.yPositionLabel.text = [NSString stringWithFormat:#"y = %f m", yPosition];
self.zPositionLabel.text = [NSString stringWithFormat:#"z = %f m", zPosition];
self.xVelocityLabel.text = [NSString stringWithFormat:#"vx = %f m/s", xVelocity];
self.yVelocityLabel.text = [NSString stringWithFormat:#"vy = %f m/s", yVelocity];
self.zVelocityLabel.text = [NSString stringWithFormat:#"vz = %f m/s", zVelocity];
self.distanceLabel.text = [NSString stringWithFormat:#"dist = %f m", sqrt( pow(xPosition, 2) + pow(yPosition, 2) + pow(zPosition, 2))];
}];
}
}