I am working on a driver behavior app and I am using SOMotionDetector (Thanks to MIT). Its giving speed and Motion Type (Not Moving, Walking, Running, Automotive) of device. I will use Automotive in my case as I need to detect driver behavior. This is detecting Motion Type based on speed with some thresholds set for Walking, Running, Automotive or if available it uses M7 Chip. It updates location approximately after every second (time varies based on GPS) in [SOMotionDetector sharedInstance].locationChangedBlock To detect Aggressive speed or break I am checking is that the increase/decrease of speed in last second. If it increases from a certain threshold (I am using kAggressiveSpeedIncrementFactor 8.0f) then its aggressively increasing speed, and if there is decreasing speed (difference factor is negative in this case) then its aggressive break. For turn I am playing with angle of latitude and longitude points, following is code for my logic:
#define kAggressiveSpeedIncrementFactor 8.0f // if 8 km/h speed was increased in last second
#define kAggressiveAngleIncrementFactor 30.0f // 30 degree turn angle
#define kAggressiveTurnIncrementFactor 5.0f . // while turn the increasing speed factor in last second
SOMotionDetector *motionDetector = [SOMotionDetector sharedInstance];
motionDetector.locationChangedBlock = ^(CLLocation *location) {
if (motionDetector.motionType == MotionTypeAutomotive) {
SOLocationManager *locationManager = [SOLocationManager sharedInstance];
float currSpeed = motionDetector.currentSpeed * 3.6f;
float lastSpeed = motionDetector.lastSpeed * 3.6f;
float currAngle = locationManager.currAngle;
float lastAngle = locationManager.lastAngle;
self.speedDiff = currSpeed-lastSpeed;
self.angleDiff = currAngle-lastAngle;
if (fabs(self.speedDiff)>kAggressiveSpeedIncrementFactor && fabs(self.angleDiff)<kAggressiveAngleIncrementFactor) {
NSString *msg = #"Aggressive Speed";
if (self.speedDiff < 0)
msg = #"Aggressive Break";
NSLog(#"%#", msg);
}
if (fabs(self.angleDiff)>kAggressiveAngleIncrementFactor && currSpeed>kAggressiveTurnIncrementFactor) {
NSLog(#"aggressive turn");
}
}
};
I have created currentSpeed and lastSpeed in SOMotionDetector class (for my speed difference) and currAngle and lastAngle in SOLocationManager. Please have a look at code,
Aggressive Speed some times work perfect
My question is:
Is this right approach what I am doing?
For detecting aggressive turn with the angle some times this happens that if
my vehicle is going 50 degrees angle (calculated with current and last lat, longs) on a strait road, some times the GPS detect location right or left side of road that give a big difference to the angle (like the path becomes a zig zag). any suggestion for this?
Related
I apologize for not knowing the proper terminology for everything here. I'm a fairly new programmer, and entirely new to Swift. The task I'm trying to accomplish is to display a current speed in MPH. I've found that using the "CoreLocation" and storing locations in an array and using "locations.speed "to display the speed is quite slow and does not refresh as often as I want.
My thought was to get an initial speed value using the "MapKit" and "CoreLocation" method, then feed that initial speed value into a function using the accelerometer to provide a quicker responding speedometer. I would do this by integrating the accelerometer values and adding the initial velocity. This was the best solution I could come up with to get a more accurate speedometer with a better refresh rate.
I'm having a couple of issues currently:
First Issue: I don't know how to get an initial speed value from a function using location data as parameters into a function using accelerometer data as parameters.
Second Issue: Even when assuming an initial speed of 0, my current program displays a value that keeps increasing infinitely. I'm not sure what the issue is that is causing this.
I will show you the portion of my code responsible for this, and would appreciate any insight any of you may have!
For my First Issue, here is my GPS data Function:
func provideInitSpeed(_ manager: CLLocationManager, didUpdateLocations locations: [CLLocation])->Double {
let location = locations[0]
return ((location.speed)*2.23693629) //returns speed value converted to MPH
}
I'm not sure how to make a function call to retrieve this value in my Accelerometer function.
For my Second Issue, here is my Accelerometer Function with assumed starting speed of 0:
motionManager.startAccelerometerUpdates(to: OperationQueue.current!) {
(data,error) in
if let myData = data {
//getting my acceleration data and rounding the values off to the hundredths place to reduce noise
xAccel = round((myData.acceleration.x * g)*100)/100
yAccel = round((myData.acceleration.y * g)*100)/100
zAccel = round((myData.acceleration.z * g)*100)/100
// Integrating accel vals to get velocity vals *Possibly where error occurs* I multiply the accel values by the change in time, which is currently set at 0.2 seconds.
xVel += xAccel * self.motionManager.accelerometerUpdateInterval
yVel += yAccel * self.motionManager.accelerometerUpdateInterval
zVel += zAccel * self.motionManager.accelerometerUpdateInterval
// Finding total speed; Magnitude of Velocity
totalSpeed = sqrt(pow(xVel,2) + pow(yVel,2) + pow(zVel,2))
// if-else statment for further noise reduction. note: "zComp" just adjusts for the -1.0 G units z acceleration value that the phone reads by default for gravity
if (totalSpeed - zComp) > -0.1 && (totalSpeed - zComp) < 0.1 {
self.view.reloadInputViews()
self.speedWithAccelLabel.text = "\(0.0)"
} else {
// Printing totalSpeed
self.view.reloadInputViews()
self.speedWithAccelLabel.text = "\(abs(round((totalSpeed - zComp + /*where initSpeed would go*/)*10)/10))"
}
}//data end
}//motionManager end
I'm not sure why but the speed this function displays is always increasing by about 4 mph every refresh of the label.
This is my first time using Stack Overflow, so I apologize for any stupid mistakes I might have made!
Thanks a lot!
Interesting case: if we move iPhone to iBeacon device, value of accuracy changed much faster than when we move iPhone from device.
How can I make this process faster?
As you have noted, CoreLocation averages past signal measurements to come up with an accuracy calculation (distance estimate in meters). The algorithm used to do the averaging is unpublished, but my measurements have shown a lag that stabilizes after about 20 seconds. I have not noticed a difference in lag between getting closer or further away.
You have no control over this averaging interval. The only thing you can do is average RSSI yourself over whatever time period you wish. You can then use a custom calculation to convert average RSSI to distance.
To do this you will need to have beacons that all have identical transmitter power, as you will not have access to the measured power calibration constant in the beacon advertisement. (Apple does not allow reading this value.). Instead, this constant must be hard coded in your customer distance calculation.
You can see sample code that does this here:
+(double) distanceForRSSI:(double)rssi forPower:(int)txPower {
// use coefficient values from spreadsheet for iPhone 4S
double coefficient1 = 2.922026; // multiplier
double coefficient2 = 6.672908; // power
double coefficient3 = -1.767203; // intercept
if (rssi == 0) {
return -1.0; // if we cannot determine accuracy, return -1.0
}
double ratio = rssi*1.0/txPower;
double distance;
if (ratio < 1.0) {
distance = pow(ratio,10);
}
else {
distance = (coefficient1)*pow(ratio,coefficient2) + coefficient3;
}
if (distance < 0.1) {
NSLog(#"Low distance");
}
return distance;
}
https://github.com/AltBeacon/ios-beacon-tools/blob/master/ios-beacon-tools/RNLBeacon%2BDistance.m
I'm trying to detect three actions: when a user begins walking, jogging, or running. I then want to know when the stop. I've been successful in detecting when someone is walking, jogging, or running with the following code:
- (void)update:(CMAccelerometerData *)accelData {
[(id) self setAcceleration:accelData.acceleration];
NSTimeInterval secondsSinceLastUpdate = -([self.lastUpdateTime timeIntervalSinceNow]);
if (labs(_acceleration.x) >= 0.10000) {
NSLog(#"walking: %f",_acceleration.x);
}
else if (labs(_acceleration.x) > 2.0) {
NSLog(#"jogging: %f",_acceleration.x);
}
else if (labs(_acceleration.x) > 4.0) {
NSLog(#"sprinting: %f",_acceleration.x);
}
The problem I run into is two-fold:
1) update is called multiple times every time there's a motion, probably because it checks so frequently that when the user begins walking (i.e. _acceleration.x >= .1000) it is still >= .1000 when it calls update again.
Example Log:
2014-02-22 12:14:20.728 myApp[5039:60b] walking: 1.029846
2014-02-22 12:14:20.748 myApp[5039:60b] walking: 1.071777
2014-02-22 12:14:20.768 myApp[5039:60b] walking: 1.067749
2) I'm having difficulty figuring out how to detect when the user stopped. Does anybody have advice on how to implement "Stop Detection"
According to your logs, accelerometerUpdateInterval is about 0.02. Updates could be less frequent if you change mentioned property of CMMotionManager.
Checking only x-acceleration isn't very accurate. I can put a device on a table in a such way (let's say on left edge) that x-acceleration will be equal to 1, or tilt it a bit. This will cause a program to be in walking mode (x > 0.1) instead of idle.
Here's a link to ADVANCED PEDOMETER FOR SMARTPHONE-BASED ACTIVITY TRACKING publication. They track changes in the direction of the vector of acceleration. This is the cosine of the angle between two consecutive acceleration vector readings.
Obviously, without any motion, angle between two vectors is close to zero and cos(0) = 1. During other activities d < 1. To filter out noise, they use a weighted moving average of the last 10 values of d.
After implementing this, your values will look like this (red - walking, blue - running):
Now you can set a threshold for each activity to separate them. Note that average step frequency is 2-4Hz. You should expect current value to be over the threshold at least few times in a second in order to identify the action.
Another helpful publications:
ERSP: An Energy-efficient Real-time Smartphone Pedometer (analyze peaks and throughs)
A Gyroscopic Data based Pedometer Algorithm (threshold detection of gyro readings)
UPDATE
_acceleration.x, _accelaration.y, _acceleration.z are coordinates of the same acceleration vector. You use each of these coordinates in d formula. In order to calculate d you also need to store acceleration vector of previous update (with i-1 index in formula).
WMA just take into account 10 last d values with different weights. Most recent d values have more weight, therefore, more impact on resulting value. You need to store 9 previous d values in order to calculate current one. You should compare WMA value to corresponding threshold.
if you are using iOS7 and iPhone5S, I suggest you look into CMMotionActivityManager which is available in iPhone5S because of the M7 chip. It is also available in a couple of other devices:
M7 chip
Here is a code snippet I put together to test when I was learning about it.
#import <CoreMotion/CoreMotion.h>
#property (nonatomic,strong) CMMotionActivityManager *motionActivityManager;
-(void) inSomeMethod
{
self.motionActivityManager=[[CMMotionActivityManager alloc]init];
//register for Coremotion notifications
[self.motionActivityManager startActivityUpdatesToQueue:[NSOperationQueue mainQueue] withHandler:^(CMMotionActivity *activity)
{
NSLog(#"Got a core motion update");
NSLog(#"Current activity date is %f",activity.timestamp);
NSLog(#"Current activity confidence from a scale of 0 to 2 - 2 being best- is: %ld",activity.confidence);
NSLog(#"Current activity type is unknown: %i",activity.unknown);
NSLog(#"Current activity type is stationary: %i",activity.stationary);
NSLog(#"Current activity type is walking: %i",activity.walking);
NSLog(#"Current activity type is running: %i",activity.running);
NSLog(#"Current activity type is automotive: %i",activity.automotive);
}];
}
I tested it and it seems to be pretty accurate. The only drawback is that it will not give you a confirmation as soon as you start an action (walking for example). Some black box algorithm waits to ensure that you are really walking or running. But then you know you have a confirmed action.
This beats messing around with the accelerometer. Apple took care of that detail!
You can use this simple library to detect if user is walking, running, on vehicle or not moving. Works on all iOS devices and no need M7 chip.
https://github.com/SocialObjects-Software/SOMotionDetector
In repo you can find demo project
I'm following this paper(PDF via RG) in my indoor navigation project to determine user dynamics(static, slow walking, fast walking) via merely accelerometer data in order to assist location determination.
Here is the algorithm proposed in the project:
And here is my implementation in Swift 2.0:
import CoreMotion
let motionManager = CMMotionManager()
motionManager.accelerometerUpdateInterval = 0.1
motionManager.startAccelerometerUpdatesToQueue(NSOperationQueue.mainQueue()) { (accelerometerData: CMAccelerometerData?, error: NSError?) -> Void in
if((error) != nil) {
print(error)
} else {
self.estimatePedestrianStatus((accelerometerData?.acceleration)!)
}
}
After all of the classic Swifty iOS code to initiate CoreMotion, here is the method crunching the numbers and determining the state:
func estimatePedestrianStatus(acceleration: CMAcceleration) {
// Obtain the Euclidian Norm of the accelerometer data
accelerometerDataInEuclidianNorm = sqrt((acceleration.x.roundTo(roundingPrecision) * acceleration.x.roundTo(roundingPrecision)) + (acceleration.y.roundTo(roundingPrecision) * acceleration.y.roundTo(roundingPrecision)) + (acceleration.z.roundTo(roundingPrecision) * acceleration.z.roundTo(roundingPrecision)))
// Significant figure setting
accelerometerDataInEuclidianNorm = accelerometerDataInEuclidianNorm.roundTo(roundingPrecision)
// record 10 values
// meaning values in a second
// accUpdateInterval(0.1s) * 10 = 1s
while accelerometerDataCount < 1 {
accelerometerDataCount += 0.1
accelerometerDataInASecond.append(accelerometerDataInEuclidianNorm)
totalAcceleration += accelerometerDataInEuclidianNorm
break // required since we want to obtain data every acc cycle
}
// when acc values recorded
// interpret them
if accelerometerDataCount >= 1 {
accelerometerDataCount = 0 // reset for the next round
// Calculating the variance of the Euclidian Norm of the accelerometer data
let accelerationMean = (totalAcceleration / 10).roundTo(roundingPrecision)
var total: Double = 0.0
for data in accelerometerDataInASecond {
total += ((data-accelerationMean) * (data-accelerationMean)).roundTo(roundingPrecision)
}
total = total.roundTo(roundingPrecision)
let result = (total / 10).roundTo(roundingPrecision)
print("Result: \(result)")
if (result < staticThreshold) {
pedestrianStatus = "Static"
} else if ((staticThreshold < result) && (result <= slowWalkingThreshold)) {
pedestrianStatus = "Slow Walking"
} else if (slowWalkingThreshold < result) {
pedestrianStatus = "Fast Walking"
}
print("Pedestrian Status: \(pedestrianStatus)\n---\n\n")
// reset for the next round
accelerometerDataInASecond = []
totalAcceleration = 0.0
}
}
Also I've used the following extension to simplify significant figure setting:
extension Double {
func roundTo(precision: Int) -> Double {
let divisor = pow(10.0, Double(precision))
return round(self * divisor) / divisor
}
}
With raw values from CoreMotion, the algorithm was haywire.
Hope this helps someone.
EDIT (4/3/16)
I forgot to provide my roundingPrecision value. I defined it as 3. It's just plain mathematics that that much significant value is decent enough. If you like you provide more.
Also one more thing to mention is that at the moment, this algorithm requires the iPhone to be in your hand while walking. See the picture below. Sorry this was the only one I could find.
My GitHub Repo hosting Pedestrian Status
You can use Apple's latest Machine Learning framework CoreML to find out user activity. First you need to collect labeled data and train the classifier. Then you can use this model in your app to classify user activity. You may follow this series if are interested in CoreML Activity Classification.
https://medium.com/#tyler.hutcherson/activity-classification-with-create-ml-coreml3-and-skafos-part-1-8f130b5701f6
I'm making an app that (among other things) displays a simplified compass image that rotates according to the device's rotation. The problem is that simply doing this:
float heading = -1.0f * M_PI * trueHeading / 180.0f; //trueHeading is always between 0 and 359, never 360
self.compassNeedle.transform = CGAffineTransformMakeRotation(heading);
inside CLLocationManager's didUpdateHeading method makes the animation ugly and choppy.
I have already used Instruments to find out whether its simply my app not being able to render at more than 30-48 fps, but that's not the case.
How can I smooth out the image view's rotation so that it's more like Apple's own Compass app?
Instead of using the current instant value, try using the average of the last N values for the true heading. The value may be jumping around a lot in a single instant but settle down "in the average".
Assuming you have a member variable storedReadings which is an NSMutableArray:
-(void)addReading(float):newReading
{
[storedReadings addObject:[NSNumber numberWithFloat:newReading]];
while([storedReadings count] > MAX_READINGS)
{
[storedReadings removeObjectAtIndex:0];
}
}
then when you need the average value (timer update?)
-(float)calcReading
{
float result = 0.0f;
if([storedReadings count] > 0)
{
foreach(NSNumber* reading in storedReadings)
{
result += [reading floatValue];
}
result /= [storedReadings count];
}
return result;
}
You get to pick MAX_READINGS a priori.
NEXT LEVEL(S) UP
If the readings are not jumping around so much but the animation is still choppy, you probably need to do something like a "smooth" rotation. At any given time, you have the current angle you are displaying, theta (store this in your class, start it out at 0). You also have your target angle, call it target. This is the value you get from the smoothed calcReading function. The error is defined as the difference between the two:
error = target-theta;
Set up a timer callback with a period of something like 0.05 seconds (20x per second). What you want to do is adjust theta so that the error is driven towards 0. You can do this in a couple of ways:
thetaNext += kProp * (target - theta); //This is proportional feedback.
thetaNext += kStep * sign(target-theta); // This moves theta a fixed amount each update. sign(x) = +1 if x >= 0 and -1 if x < 0.
The first solution will cause the rotation to change sharply the further it is from the target. It will also probably oscillate a little bit as it swings past the "zero" point. Bigger values of kProp will yield faster response but also more oscillation. Some tuning will be required.
The second solution will be much easier to control...it just "ticks" the compass needle around each time. You can set kStep to something like 1/4 degree, which gives you a "speed" of rotation of about (1/4 deg/update) * (20 updates/seconds) = 5 degrees per second. This is a bit slow, but you can see the math and change kStep to suit your needs. Note that you may to "band" the "error" value so that no action is taken if the error < kStep (or something like that). This prevents your compass from shifting when the angle is really close to the target. You can change kStep when the error is small so that it "slides" into the ending position (i.e. kStep is smaller when the error is small).
For dealing with Angle Issues (wrap around), I "normalize" the angle so it is always within -Pi/Pi. I don't guarantee this is the perfect way to do it, but it seems to get the job done:
// Takes an angle greater than +/- M_PI and converts it back
// to +/- M_PI. Useful in Box2D where angles continuously
// increase/decrease.
static inline float32 AdjustAngle(float32 angleRads)
{
if(angleRads > M_PI)
{
while(angleRads > M_PI)
{
angleRads -= 2*M_PI;
}
}
else if(angleRads < -M_PI)
{
while(angleRads < -M_PI)
{
angleRads += 2*M_PI;
}
}
return angleRads;
}
By doing it this way, -pi is the angle you reach from going in either direction as you continue to rotate left/right. That is to say, there is not a discontinuity in the number going from say 0 to 359 degrees.
SO PUTTING THIS ALL TOGETHER
static inline float Sign(float value)
{
if(value >= 0)
return 1.0f;
return -1.0f;
}
//#define ROTATION_OPTION_1
//#define ROTATION_OPTION_2
#define ROTATION_OPTION_3
-(void)updateArrow
{
// Calculate the angle to the player
CGPoint toPlayer = ccpSub(self.player.position,self.arrow.position);
// Calculate the angle of this...Note there are some inversions
// and the actual image is rotated 90 degrees so I had to offset it
// a bit.
float angleToPlayerRads = -atan2f(toPlayer.y, toPlayer.x);
angleToPlayerRads = AdjustAngle(angleToPlayerRads);
// This is the angle we "wish" the arrow would be pointing.
float targetAngle = CC_RADIANS_TO_DEGREES(angleToPlayerRads)+90;
float errorAngle = targetAngle-self.arrow.rotation;
CCLOG(#"Error Angle = %f",errorAngle);
#ifdef ROTATION_OPTION_1
// In this option, we just set the angle of the rotated sprite directly.
self.arrow.rotation = CC_RADIANS_TO_DEGREES(angleToPlayerRads)+90;
#endif
#ifdef ROTATION_OPTION_2
// In this option, we apply proportional feedback to the angle
// difference.
const float kProp = 0.05f;
self.arrow.rotation += kProp * (errorAngle);
#endif
#ifdef ROTATION_OPTION_3
// The step to take each update in degrees.
const float kStep = 4.0f;
// NOTE: Without the "if(fabs(...)) check, the angle
// can "dither" around the zero point when it is very close.
if(fabs(errorAngle) > kStep)
{
self.arrow.rotation += Sign(errorAngle)*kStep;
}
#endif
}
I put this code into a demo program I had written for Cocos2d. It shows a character (big box) being chased by some monsters (smaller boxes) and has an arrow in the center that always points towards the character. The updateArrow call is made on a timer tick (the update(dt) function) regularly. The player's position on the screen is set by the user tapping on the screen and the angle is based on the vector from the arrow to the player. In the function, I show all three options for setting the angle of the arrow:
Option 1
Just set it based on where the player is (i.e. just set it).
Option 2
Use proportional feedback to adjust the arrow's angle each time step.
Option 3
Step the angle of the arrow each timestep a little bit if the error angle is more than the step size.
Here is a picture showing roughly what it looks like:
And, all the code is available here on github. Just look in the HelloWorldLayer.m file.
Was this helpful?
I have an app that records angles as user is walking around an object, while pointing device (preferably) at the center of the object.
Angle gets reset on user's command - so reference attitude gets reset.
Using Euler angles produces Gimbal lock, so I am currently using quaternions and calculating angles this way:
double angleFromLastPosition = acos(fromLastPositionAttitude.quaternion.w) * 2.0f;
This gives off good precision and it works perfectly IF device's pitch and yaw does not change. In other words, as the angle shows 360 degrees I end up in the same place as the start of the circle.
Problem 1: if device's yaw and pitch change slightly (user not pointing directly at center of the object), so does the angleFromLastPosition.
I understand this part, as my angle formula just shows the angle in between two device attitudes in 3D space.
Scenario:
I mark the start of rotation attitude and start moving in a circle around the object while pointing at the center
I stop at, say, 45 degrees and change pitch of the device by pointing it higher or lower. Angle changes accordingly.
What I would love to see is: angle stays at 45 degrees, even if pitch or yaw changes.
Question 1 is, how can I calculate only the Roll of the device using quaternions, and disregard changes in other two axes (at least within some reasonable number of degrees).
Problem 2: if I rotate for a bit and then freeze the device completely (on tripod so there's no shaking at all), the angleFromLastPosition drifts at a rate of 1 degree per about 10-20 seconds, and it appears not to be linear. In other words, it drifts fast at first, then slows down considerably. Sometimes I get no drift at all - angle is rock-solid if device is stationary. And this makes me lost in understanding what's going on.
Question 2, what is going on here, and how can I take care of the drift?
I went through quite a few articles and tutorials, and quaternion math is beyond me at the moment, hope someone will be able to help with a tip, link, or few lines of code.
I have tested this and it seems to work according to what you're looking for in Question 1, Andrei.
I set the homeangle initially 0, and immediately after the first pass I store the angle returned from walkaroundAngleFromAttitude:fromHomeAngle: in homeangle, for future use.
My testing included starting the device updates using a reference frame:
[_motionManager
startDeviceMotionUpdatesUsingReferenceFrame:CMAttitudeReferenceFrameXArbitraryZVertical
toQueue:operationQueue
withHandler:handler];
and using the following methods called within handler:
- (CMQuaternion) multiplyQuanternion:(CMQuaternion)left withRight:(CMQuaternion)right {
CMQuaternion newQ;
newQ.w = left.w*right.w - left.x*right.x - left.y*right.y - left.z*right.z;
newQ.x = left.w*right.x + left.x*right.w + left.y*right.z - left.z*right.y;
newQ.y = left.w*right.y + left.y*right.w + left.z*right.x - left.x*right.z;
newQ.z = left.w*right.z + left.z*right.w + left.x*right.y - left.y*right.x;
return newQ;
}
-(float)walkaroundRawAngleFromAttitude:(CMAttitude*)attitude {
CMQuaternion e = (CMQuaternion){0,0,1,1};
CMQuaternion quatConj = attitude.quaternion;
quatConj.x *= -1; quatConj.y *= -1; quatConj.z *= -1;
CMQuaternion quat1 = attitude.quaternion;
CMQuaternion quat2 = [self multiplyQuanternion:quat1 withRight:e];
CMQuaternion quat3 = [self multiplyQuanternion:quat2 withRight:quatConj];
return atan2f(quat3.y, quat3.x);
}
-(float)walkaroundAngleFromAttitude:(CMAttitude*)attitude fromHomeAngle:(float)homeangle {
float rawangle = [self walkaroundRawAngleFromAttitude:attitude];
if (rawangle <0) rawangle += M_PI *2;
if (homeangle < 0) homeangle += M_PI *2;
float finalangle = rawangle - homeangle;
if (finalangle < 0) finalangle += M_PI *2;
return finalangle;
}
This is using some modified and extended code from Finding normal vector to iOS device
Edit to deal with Question 2 & Problem 2:
This may not be solvable. I've seen it in other apps (360 pano for example) and have read about faulty readings in gyro and such. If you tried to compensate for it, of course you'll end up with a jittery experience when some authentic rotational movement gets tossed by the compensation code. So far as I've been interpreting for the last few years, this is a hardware-based issue.