Dead reckoning script.
I was working on an dead reckoning script, despite hearing from just about everybody that the phone had too much noise/too low a frequency for sampling (Which turned out to be true, on my ipod touch 5th generation it drifts around 40cm per 5 seconds).
With the preset time constants, it should run on an iphone 4s/ ipod touch 5 directly. Each while loop takes approximately 0.08 seconds (you need 0.01 seconds to even start thinking about seeing the results real time). And to that end, I avoided using arrays/map/for loops/numpy.cos because my timing showed that it was slower than just setting var1=something, and using math.cos. Despite my best efforts, it cannot present accurate results. If people could check it for errors, that would be fabulous, and maybe a solution using pickle, and later analysis would work. Coded mostly in pycharm on the desktop.
The numbers it returns is relative to your initial position according to this page(not the phones, assuming you are facing towards the short side of the phone)
Angle drift depended on how long the update has started, the longer, the less drift, that's why update is started and we wait 10 seconds.
Acceleration drift is dependent on phone position, that's why you need to "calibrate it".
Array creation is not worth it, if you're just using three numbers, also don't bother with numpy.
Additional benchmarking showed that the print statement was taking up 98.5% of the 0.01 seconds, so delete that, and you can easily reach 100Hz, that reduces the noise so that you only have about 20m of drift in each direction every 10 seconds, an significant improvement. That means that we might be able to start doing some really fancy filtering on the data in real time to reduce the noise.
Where did you get your strapdown equations... They seem dont seem correct to me(perhsps for some degenerate case of no actual motion?). For instance, you are using cosines only, generally anything involving Euler angles is going to require sin and cos! Also, just adding gyro rates is not correct, since the orientation is changing.
A simple implementation is described here
Section 6.1.2 is what you'd use for gyro rate propagation. This uses Euler angles, which can be problematic and is not the most efficient, but is easy to understand conceptually.
For position updates, you would need to convert user accel to global frame using eq 42, then propagate using 45 and 46.
Actually, you may be better off forgetting about rotation_rate, and use attitude to derive the Euler angles for use in eq 42. iOS is already doing some fusion of sensors to give you an absolute attitude, which has the advantage of properly accounting for initial attitude, and using magnometer reduces gyro drift. note for stationary device, the attitude hardly drifts at all, compared with just adding gyro rates.
I'm also concerned about not using actual time, but hard coding timing values. Perhaps using time.time() will be more accurate? especially in conjunction with a threading.Timer to try for more controlled timing. we arent trying to measure microsecinds here. it is a shame we dont get time tagged measurements, but I guess this is only important for fast rates, in. Which case the lack of a scale factor cal probably is the deal breaker anyway.
I was using the equivalent of an direction cosine matrix, where the actual x vector would have to be "projected" on the plane global x, global y are on, then projected onto the x axis itself. Where cos(theta1)|x2|=|x1|, cos(theta2)|x1|=|x|. Please correct me if I made a mistake.
The motion module returns <roll, pitch, yaw>. roll is around the y axis, pitch is around the x axis, and yaw is around the z axis. That means, roll does not affect y, pitch does not affect x, and yaw does not affect z. Assume that the gyro/accelerometer are in the center of the device, and the phone is flat on a table. We rotate the phone counter clockwise around the z axis for 1 pi. And move the phone forwards at constant acceleration m, if we want to get the motion relative to the original phone, we would need an value that is the negative of the original acceleration m, and the negative of the original speed, because we are moving away from the direction the accelerometer is telling us, at an increased speed(instead of decreasing as it's telling us). Only cos is an even function that would return cos(pi)= -1. Now imagine that in addition to that, we flip the phone over, front over end. That would cause the top side to be pointing towards the top again, a rotation of pi, (cos(pi))^2= (-1)^2=1, returning the correct vector again.
The problem with attitude is the large amount of drift initially, after starting updates, it can drift by nearly 10 degrees, which makes it necessary to zero before using in each loop, which is marginally more difficult to compensate and slower to call than just using the cumsum of the rotation_rate.
I share your concern with timing, but actually with a little bit of tweaking, I have gotten the error down to 1.2cm of drift per second for the first ten seconds for the x and y axis. The z axis is unusable due to the huge amount of noise still present.
Your equations only are approx correct if the angles are near zero.
Think through the case where all angles are pi/2! You equations would say nothing gets projected to global frame!
In general, the projection will be a linear combo of all 3 axes. See the Wikipedia entry on Euler angles.
Not sure what convention ios uses, but probably zyx which is fairly standard. You have to take care whether you use the DCM that converts from global vectors to body, or vice versa, which is the transpose.
You have to compute your sin and cos of each angle, and 9 combos of these terms.
Using DCMs have problems with gimbal lock, quaternions are generally more numerically stable.
If you plot out ios attitude vs time, it does not have 10s of degrees of drift, it has sub degree drift, if stationary.
You may want to prompt user to do the figure 8 maneuver, which calibrates the mag to gyro/accel, and improves yaw accuracy...
I was thinking about it last night, and I did in fact miss the other two axis' contributions to the third axis, which would explain why my script showed zero movement if I rotated the device by 90 degrees and moved in the original direction. I will be correcting that soon.
Attitude does drift by a huge amount right after you start updating, and the lack of a magnetometer on the ipod touch makes using the third sensor impossible.