I have been working on Orientation estimation and I need to estimate correct heading when I am walking in straight line. After facing some roadblocks, I started from basics again.
I have implemented a Complementary Filter from here, which uses Gravity vector obtained from Android (not Raw Acceleration), Raw Gyro data and Raw Magnetometer data. I am also applying a low pass filter on Gyro and Magnetometer data and use it as input.
The output of the Complementary filter is Euler angles and I am also recording TYPE_ROTATION_VECTOR which outputs device orientation in terms of 4D Quaternion.
So I thought to convert the Quaternions to Euler and compare them with the Euler obtained from Complementary filter. The output of Euler angles is shown below when the phone is kept stationary on a table.
As it can be seen, the values of Yaw are off by a huge margin.
What am I doing wrong for this simple case when the phone is stationary
Then I walked in my living room and I get the following output.
The shape of Complementary filter looks very good and it is very close to that of Android. But the values are off by huge margin.
Please tell me what am I doing wrong?
I don't see any need to apply a low-pass filter to the Gyro. Since you're integrating the gyro to get rotation, it could mess everything up.
Be aware that TYPE_GRAVITY is a composite sensor reading synthesized from gyro and accel inside Android's own sensor fusion algorithm. Which is to say that this has already been passed through a Kalman filter. If you're going to use Android's built-in sensor fusion anyway, why not just use TYPE_ROTATION_VECTOR?
Your angles are in radians by the looks of it, and the error in the first set wasn't too far from 90 degrees. Perhaps you've swapped X and Y in your magnetometer inputs?
Here's the approach I would take: first write a test that takes accel and gyro and synthesizes Euler angles from it. Ignore gyro for now. Walk around the house and confirm that it does the right thing, but is jittery.
Next, slap an aggressive low-pass filter on your algorithm, e.g.
yaw0 = yaw;
yaw = computeFromAccelMag(); // yaw in radians
factor = 0.2; // between 0 and 1; experiment
yaw = yaw * factor + yaw0 * (1-factor);
Confirm that this still works. It should be much less jittery but also sluggish.
Finally, add gyro and make a complementary filter out of it.
dt = time_since_last_gyro_update;
yaw += gyroData[2] * dt; // test: might need to subtract instead of add
yaw0 = yaw;
yaw = computeFromAccelMag(); // yaw in radians
factor = 0.2; // between 0 and 1; experiment
yaw = yaw * factor + yaw0 * (1-factor);
They key thing is to test every step of the way as you develop your algorithm, so that when the mistake happens, you'll know what caused it.
Related
I know that the common way for applying 3d rotation is Roll, then Pitch, then Yaw.
And I know that the order doesn't really matter as long as you chose an order and stick to it.
But it seems reversed of the intuitive way,
If I would've asked you to explain where your face is facing or at which direction you point a camera at, you would probably start with Yaw (the side you look at), then Pitch (how high\low you look) and then Roll (how to rotate it).
Any explanation why it is the common order used?
First, I would like to state that there is no common way of doing rotation. There are just too many ways and each serves its purpose. I would even argue that Euler angles are a bad choice for the majority of cases.
Anyway, your question is probably related to the meaning of applying a rotation. This can be done in several ways. E.g., we could use a rotation matrix and compose it of the individual principal rotations. If we have column vectors, this would be:
R = Yaw * Pitch * Roll
We can interpret this from left to right and we will see that this is exactly what you would interpret it (start to look left/right, then up/down, finally tilt your head). The important thing is that this will also transform the coordinate system, in which we apply subsequent rotations.
If we use this matrix R to transform a point P, then we would calculate P' = R * P. This is:
P' = Yaw * Pitch * Roll * P
We could calculate this also from right to left by introducing parentheses:
P' = Yaw * Pitch * (Roll * P)
So, we could start by applying Roll to P: P_Roll = Roll * P
P' = Yaw * (Pitch * P_Roll)
, then pitch and finally yaw. In this interpretation, however, we would always use the global coordinate system.
So it is all just a matter of perspective.
This is a small background and introduction to the problem:
I have some functionality in my motion- and location-based iOS app, which needs a rotation matrix as an input. Some graphical output is dependent on this matrix. With every movement of the device, graphical output is changed. This is a part of the code which makes that:
[motionManager startDeviceMotionUpdatesUsingReferenceFrame:CMAttitudeReferenceFrameXTrueNorthZVertical
toQueue:motionQueue
withHandler:
^(CMDeviceMotion* motion, NSError* error){
//get and process matrix data
}
In this structure only 4 frames are available:
XArbitraryZVertical
XArbitraryCorrectedZVertical
XMagneticNorthZVertical
XTrueNorthZVertical
I need to have another reference, f.e. gyroscope value instead of North and these frames can not offer me exactly what I want.
In order to reach my goal, I use next structure:
[motionManager startDeviceMotionUpdatesUsingReferenceFrame:CMAttitudeReferenceFrameXArbitraryCorrectedZVertical
toQueue:motionQueue
withHandler:
^(CMDeviceMotion* motion, NSError* error){
//get Euler angles and transform it to rotation matrix
}
You may ask me, why I do not use built in rotation matrix? The answer is simple. I need to make some kind of own reference frame and I can make this via putting inside modified values of angles.
The problem:
In order to get rotation matrix from Euler angles we need to make matrix for each angle and after that multiply them. For 3D case we will have matrix for each axis (3 of them). After that we multiply matrixes. The problem is that the output is dependent on the order of multiplication. XYZ is not equal to ZYX. Wikipedia tells me, that there are 12 variants and I do not know which one is the right one for iOS implementation. I need to know in which order I need to multiply them. In addition, I need to know which angles represents X, Y, Z. For example, X - roll, Y - pitch, Z - yaw.
Actually, this problem was solved by Apple years ago, but I do not have access to .m-files and I do not know which order of multiplication is the right one for iOS device.
Similar question was published here, but order from that math example in the solution does not work for me.
Regarding: Which angles relate to which axis.
See this:
link:https://developer.apple.com/documentation/coremotion/getting_processed_device-motion_data/understanding_reference_frames_and_device_attitude
Regarding rotation order for calculating rotation matrix & Euler angles (Pitch, Roll, Yaw)
Short Answer: ZXY is the rotation order on iOS.
I kept searching for this answer too. Got tired. Not sure why this is not documented somewhere easy to lookup. I decided to collect empirical data and test out which rotation order best matches the values. My values are below.
Methodology:
Wrote a small iPhone App to return quaternion values & corresponding pitch, roll, yaw angles
Computed pitch, roll, yaw values from the quaternions for various rotation orders (XYZ, XZY, YZX, YXZ, ZYX, ZXY)
Calculated RMS error with respect to the pitch, yaw, roll values reported by iOS device motion. Identified the orientation with the least error.
Results:
Rotation orders: ZYX & ZXY both returned values very close to the iOS reported values. However, the Error on ZXY was ~46-597X lower than ZXY for every case. Hence I believe ZXY is the rotation order.
What would be the best way tell the user is shaking the Sphero?
I need to differentiate when the user tilts the Sphero left/right/up/down and when they shake it rapidly a few times in any direction.
Is there a sample project that would be good to look at?
If you're collecting the accelerometer filtered values, and also the "IMU" values, the accelerometer values would be best for detecting shaking, while the IMU values (roll, pitch, yaw) are best for detecting tilt.
If you don't care on which axis it is shaken, then normalize the axis' by getting a square of the sum of their squares: sqrt(x^2 + y^2 + z^2) > 2000. This will give you a magnitude of the acceleration vector. It's a good value for "general acceleration-ness", and it's great for detecting shaking.
If you want to isolate on which axis it is being shaken, then for each axis, evaluate whether its absolute value of acceleration is above a threshold: abs(x) > 2000, since the positive or negative value of an axis is its own vector magnitude.
Then, just use the IMU data's roll, pitch and yaw values to determine the tilt of the Sphero.
I'm trying to estimate the relative camera pose using OpenCV. Cameras in my case are calibrated (i know the intrinsic parameters of the camera).
Given the images captured at two positions, i need to find out the relative rotation and translation between two cameras. Typical translation is about 5 to 15 meters and yaw angle rotation between cameras range between 0 - 20 degrees.
For achieving this, following steps are adopted.
a. Finding point corresponding using SIFT/SURF
b. Fundamental Matrix Identification
c. Estimation of Essential Matrix by E = K'FK and modifying E for singularity constraint
d. Decomposition Essential Matrix to get the rotation, R = UWVt or R = UW'Vt (U and Vt are obtained SVD of E)
e. Obtaining the real rotation angles from rotation matrix
Experiment 1: Real Data
For real data experiment, I captured images by mounting a camera on a tripod. Images captured at Position 1, then moved to another aligned Position and changed yaw angles in steps of 5 degrees and captured images for Position 2.
Problems/Issues:
Sign of the estimated yaw angles are not matching with ground truth yaw angles. Sometimes 5 deg is estimated as 5deg, but 10 deg as -10 deg and again 15 deg as 15 deg.
In experiment only yaw angle is changed, however estimated Roll and Pitch angles are having nonzero values close to 180/-180 degrees.
Precision is very poor in some cases the error in estimated and ground truth angles are around 2-5 degrees.
How to find out the scale factor to get the translation in real world measurement units?
The behavior is same on simulated data also.
Have anybody experienced similar problems as me? Have any clue on how to resolve them.
Any help from anybody would be highly appreciated.
(I know there are already so many posts on similar problems, going trough all of them has not saved me. Hence posting one more time.)
In chapter 9.6 of Hartley and Zisserman, they point out that, for a particular essential matrix, if one camera is held in the canonical position/orientation, there are four possible solutions for the second camera matrix: [UWV' | u3], [UWV' | -u3], [UW'V' | u3], and [UW'V' | -u3].
The difference between the first and third (and second and fourth) solutions is that the orientation is rotated by 180 degrees about the line joining the two cameras, called a "twisted pair", which sounds like what you are describing.
The book says that in order to choose the correct combination of translation and orientation from the four options, you need to test a point in the scene and make sure that the point is in front of both cameras.
For problems 1 and 2,
Look for "Euler angles" in wikipedia or any good math site like Wolfram Mathworld. You would find out the different possibilities of Euler angles. I am sure you can figure out why you are getting sign changes in your results based on literature reading.
For problem 3,
It should mostly have to do with the accuracy of our individual camera calibration.
For problem 4,
Not sure. How about, measuring a point from camera using a tape and comparing it with the translation norm to get the scale factor.
Possible reasons for bad accuracy:
1) There is a difference between getting reasonable and precise accuracy in camera calibration. See this thread.
2) The accuracy with which you are moving the tripod. How are you ensuring that there is no rotation of tripod around an axis perpendicular to surface during change in position.
I did not get your simulation concept. But, I would suggest the below test.
Take images without moving the camera or object. Now if you calculate relative camera pose, rotation should be identity matrix and translation should be null vector. Due to numerical inaccuracies and noise, you might see rotation deviation in arc minutes.
I want to find out the pitch, yaw, and roll on an iPad 1. Since there is no deviceMotion facility, can I get this data from the accelerometer? I assume that I can use the vector that it returns to compare against a reference vector i.e. gravity.
Does iOS detect when the device is still and then take that as the gravity vector? Or do I have to do that?
Thanks.
It's definitely possible to calculate the Pitch and Roll from accelerometer data, but Yaw requires more information (gyroscope for sure but possibly compass could be made to work).
For an example look at Hungry Shark for iOS . Based on how their tilt calibration ui works I'm pretty sure they're using the accelerometer instead of the gyroscope.
Also, here are some formula's I found on a blog post from Taylor-Robotic a for calculating pitch and roll:
Now that we have 3 outputs expressed in g we should be able to
calculate the pitch and the roll. This requires two further equations.
pitch = atan (x / sqrt(y^2 + z^2))
roll = atan (y / sqrt(x^2 + z^2))
This will produce the pitch and roll in radians, to convert them into
friendly degrees we multiply by 180, then divide by PI.
pitch = (pitch * 180) / PI
roll = (roll * 180) / PI
The thing I'm still looking for is how to calibrate the pitch and roll values based on how the user is holding the device. If I can't figure it out soon, I may open up a separate question. Good Luck!