I have a set of acceleration values along the x, y, z directions (sensor body frame). The sensor tilted and changed orientation many times during operation. Using the gyroscope, magnetometer and accelerometer, I was able to fuse and extract the Roll, Pitch and Yaw angles all in quaternion format ( cal it Q).
Now, to derotate the acceleration values into global X, Y, Z positions. Do I just simply
1. convert the acceleration values into quaternion by setting the scalar part to 0 (call it Acc)
2. Multiply the acceleration quaternion as follows : Qtranspose * Acc * Q??
Or is my conclusion wrong? how do we derotate the values?
thanks
If you already have an accurate quaternion orientation q, then the 'unrotated' vector of the accelerations is as you said:
Ag = q* A q
or
Ag = q A q*
depending on which convention you are using for quaternion multiplication. (just try both and see which one give you the right gravity vector).
NOTE that A and Ag are 'pure quaternions' - this is a quaternion with the scalar component as 0, and the axis component as your acceleration vector.
In other words if your quaternion is q = [w x y z], then A = [0 ax ay az]. You can then discard the scalar component of Ag to get the unrotated vector.
Make sure the order of the acceleration vector is consistent with your quaternion vector order.
Related
I am trying to determine a players depth position on a plane, which defines the walkable ground in a 2D brawler game. The problem is depictured in the following drawing:
C represents the players current position. I need to find the magnitude of vector V. Since I am not strong on linear algebra, the one thing I can think of is: determining the intersection point P of L1 and L2, and then take the magnitude from AP. However, I get the feeling there must be an easier way to find V, since I already know the angle the vector should have, given by vector from AB.
Any input would be appreciated, since I am looking forward to step up my linear algebra game.
Edit: As it is unclear thanks to my lack of drawing skills: the geometry depicted above is a parallelogram. The vector V I am looking for is parallel to the left and right side of the parallelogram. Depth does not mean, that I am looking for the vector perpendicular to the top side, but it refers to the fake depth of a purely 2D game. The parallelogram is therefore used as a means for creating the feeling of walking along a z axis.
The depth of your player (length of V) as measured from the top line in your drawing, is just the difference between A.y and C.y. This is seperate from the slant in the parralelogram, as we're just looking at depth.
example:
float v;
Vector2 a = new Vector2(100, 100); //The point you're measuring from
Vector2 c = new Vector2(150, 150); //Your character position
v = c.y - a.y; // This is the length of V.
//In numbers: 50 = 150 - 100
Illustrated: image not to scale
This works for any coördinate in your plane.
Now if you'd want to get the length of AC is when you'd need to apply some pythagoras, which is a² + b² = c². In the example that would mean in code:
Vector2 a = new Vector2(100, 100);
Vector2 c = new Vector2(150, 150);
float ac1 = Mathf.Sqrt(Mathf.Pow(c.x - a.x, 2) + Mathf.Pow(c.y - a.y, 2));
Now that is quite a sore to have to type out every time, and looks quite scary. But Unity has you covered! There is a Vector method called Distance
float ac2 = Vector2.Distance(a, c);
Which both return 70.71068 which is the length of AC.
This works because for any point c in your area you can draw a right angled triangle from a to c.
Edit as per comment:
If you want your "depth" vector to be parallel with the sides of the paralellogram we can just create a triangle in the parallelogram of which we calculate the hypotenuse.
Since we want the new hypotenuse of our triangle to be parallel to the parallelogram we can use the same angle θ as point B has in your drawing (indicated by pink in mine), of which I understood you know the value.
We also know the length of the adjacent (indicated in blue) side of this new triangle, as that is the height we calculated earlier (c.y - a.y).
Using these two values we can use cosine to find the length of hypotenuse (indicated in red) of the triangle, which is equal to the vector V, in parallel with the parallelogram.
the formula for that is: hypotenuse = adjacent/cos(θ)
Now if we were to put some numbers in this, and for my example I took 55 for the angle θ. It would look like this
float v = 50/(cos(55));
image not to scale
Let's call the lower right vertex of the parallelogram D.
If the long sides of the parallelogram are horizontal, you can find magnitude of V vector by:
V.magnitude = (c.y - a.y) / sin(BAD)
Or if you prefer:
V.magnitude = AB.magnitude * (c.y - a.y)/(b.y - a.y)
I'm trying to understand quaternions better, but I don't know how the math behind FromToRotation works. I tried looking this up but couldn't find any results.
You may know that a rotation can be represented by a quaternion of the following form:
cos (phi / 2)
sin (phi / 2) * axis.x
sin (phi / 2) * axis.y
sin (phi / 2) * axis.z
axis is the rotation axis and phi is the rotation angle. These are the two measures you need to define your quaternion.
There are multiple rotations that map a vector from to another vector to. The shortest rotation is the one where the axis is perpendicular to both of the vectors. Hence, the axis is:
axis = normalize(from x to)
x denotes the cross product.
And the angle is the angle between the two vectors:
phi = acos(dot(from, to) / (norm(from) * norm(to))
norm is the vector norm or vector length.
With these values, you can then calculate the quaternion.
I am building a small device that also uses magnetometer data in order to calculate the compass heading. The LSM9DS0 IMU sensor works great if the heading is calculated as a yaw (if the sensor is on a flat surface).
I have 3D printed a shell in which i am going to assemble all the electronics. My problem is that it is poorly designed and the IMU sensor is not on a flat surface, but it has to stay on 90 degrees. So by this, the Z axis is no more my way to calculate the yaw (or heading), but it changed to Y.
In order to calculate the heading on Z, i was using this formula:
heading.value = atan2((float)dof.my, (float)dof.mx);
if(heading.value < 0) heading.value += 2*PI;
if(heading.value > 2*PI) heading.value -= 2*PI;
heading.value *= 180/PI;
...where my is the magnetometer Y and mx the magnetometer X
Now, I don't know how to calculate the heading based on other axis.
I know this thread hasn't been active for a while, but during my search I came across this publication by NXP which explains the solution really nicely.
In brief:
Align the accelerometer readings (G) and the magnetometer readings (B) so they follow the NED coordinate system with the x-axis pointing forward and the y and z-axis to the right and down, respectively.
Calculate the roll and pitch
// Using atan2 to restrict +/- PI
const roll = Math.atan2(Gy, Gz)
// Using atan to restrict to +/- PI/2
const pitch = Math.atan(-Gx / (Gy * Math.sin(roll) + Gz * Math.cos(roll)))
Calculate the yaw / compass heading (here Vx, Vy, Vz correspond to the Hard-Iron effects that can be calculated separately as discussed in this publication):
// Using atan2 to restring to +/- PI
let yaw = Math.atan2( (Bz-Vz)*Math.sin(roll) - (By-Vy)*Math.cos(roll),
(Bx-Vx)*Math.cos(pitch) + (By-Vy)*Math.sin(pitch)*Math.sin(roll) + (Bz-Vz)*Math.sin(pitch)*Math.cos(roll))
Correct the heading to [0, 2*PI)
if( yaw < 0 ) {
yaw += 2*Math.PI
}
Calibration:
I have calibrated the camera using this vision toolbox in Matlab. I used checkerboard images to do so. After calibration I get the following:
>> cameraParams
cameraParams =
cameraParameters with properties:
Camera Intrinsics
IntrinsicMatrix: [3x3 double]
FocalLength: [1.0446e+03 1.0428e+03]
PrincipalPoint: [604.1474 359.7477]
Skew: 3.5436
Lens Distortion
RadialDistortion: [0.0397 0.0798 -0.2034]
TangentialDistortion: [-0.0063 -0.0165]
Camera Extrinsics
RotationMatrices: [3x3x18 double]
TranslationVectors: [18x3 double]
Accuracy of Estimation
MeanReprojectionError: 0.1269
ReprojectionErrors: [48x2x18 double]
ReprojectedPoints: [48x2x18 double]
Calibration Settings
NumPatterns: 18
WorldPoints: [48x2 double]
WorldUnits: 'mm'
EstimateSkew: 1
NumRadialDistortionCoefficients: 3
EstimateTangentialDistortion: 1
I know the transformation from the camera's coordinates to the checkerboard coordinates: R1, t1. How can I figure out the transformation between the checkerboard and a perpendicular plane: R2, t2. Given that this plane is parallel to the ground and at a height 193.040 cm from it.
Note:
This question is sort of subpart of Calibration of images to obtain a top-view for points that lie on a same plane. I posted it, to ask a generalized question.
So, IIRC the view coordinate system in the toolbox are defined with the origin at the top-left corner the checkerboard, x axis toward the right and y axis downward (and of course the z axis is the cross product of x and y).This is easy to verify, just back-project points [0; 0; 0], [10; 0; 0] and [0; 10; 0] on top of one of the calibration images and see where they fall.
Let's call this the "calibration view" frame. Let's also call "floor" the second plane you are interested in.
Now let's assume (big assumption) that you carefully placed the checkerboard in that view so that it was orthogonal to the floor, and with its horizontal edge parallel to the floor. This means that the x axis of the calibration view frame is parallel to the floor, and the y axis is orthogonal to the floor.
Therefore the floor is parallel to the (x, z) plane of the calibration view frame. Therefore, if
Rc = [x y z]
is the rotation of the calibration view w.r.t the camera, then the floor has rotation
Rf = [x z y]
(assuming the normal vector of the floor goes into it. If you prefer that it goes up from it, then it would be Rf = [z x -y]).
Further, let's call H the distance (height) of the origin of the calibration view frame from the floor. Remembering that the y axis of that frame is pointing toward the floor, we see that the point F = [0; H; 0] (in view frame coordinates) is on the floor, and we can use it as the origin of the floor frame.
In camera coordinates, vector F is represented by:
Fc = Rc * F = Rc * [0; H; 0]
and if Tc is the (calibrated) translation w.r.t. the camera of the calibration view frame, then that same point on the floor is, in camera coordinates:
F = Tc + Fc
So the 3x4 coordinate transform matrix from the floor to the camera is
Q = [Rf, F]
This should give you a decent estimate, provided that your assumptions hold.
Of course, a much better way to proceed would be to take an image of the checkerboard on the floor...
Imagine a dome with its centre in the +z direction. What I want to do is to move that dome's centre to a different axis (e.g. 20 degrees x axis, 20 degrees y axis, 20 degrees z axis). How can I do that ? Any hint/tip helps.
Add more info:
I've been dabbling with rotation matrices in wiki for a while. The problem is, it is not a commutative operation. RxRyRz is not same as RzRyRx. So based on the way I multiple it I get a different final results. For example, I want my final projection to have 20 degrees from the original X axis, 20 degrees from original Y axis and 20 degrees from original Z axis. Based on the matrix, giving alpha, beta, gamma values 20 (or its corresponding radian) does NOT result the intended rotation. Am I missing something? Is there a matrix that I can just put the intended angles and get it at the end ?
Using a rotation matrix is an easy way to rotate a collection of (x,y,z) points. You can calculate a rotation matrix for your case using the equations in the general rotation section. Note that figuring out the angle values to plug into those equations can be tricky. Think of it as rotating about one axis at a time and remember that the order of your rotations (order of multiplications) does matter.
An alternative to the general rotation equations is to calculate a rotation matrix from axis and angle. It may be easier for you to define correct parameters with this method.
Update: After perusing Wikipedia, I found a simple way to calculate rotation axis and angle between two vectors. Just fill in your starting and ending vectors for a and b here:
a = [0.0 0.0 1.0];
b = [0.5 0.5 0.0];
vectorMag = #(x) sqrt(sum(x.^2));
rotAngle = acos(dot(a,b) / (vectorMag(a) * vectorMag(b)))
rotAxis = cross(a,b)
rotAxis =
-0.5 0.5 0
rotAngle =
1.5708