Compass Heading from Magnetometer on other axis - axis

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
}

Related

Calculate acceleration from data points

I have a servo motor, and this servo motor I would like to make it follow a "motion pattern" as closely as possible, and use the same value for acceleration and deceleration.
The attach picture illustrates the "motion pattern" (Y = velocity, X = Time)
motion pattern:
accelerates 0m/s to 0.100m/s.
constant velocity 0.100m/s for 4 sec.
decelerates to negative ?m/s.
accelerates to 0m/s, and motor position = 0.
How can i calculate the acceleration and deceleration?
What i have tried so far is:
Time = (total time - constant velocity time) 10 - 4 = 6sec.
Distances = (total distances - constant velocity distances ) 1 - 0.4 = 0.6meter.
acceleration = (2 * distances / (time^2) 2 * 0.6 / sqr(6) = 0.0333m/s.
But with this acceleration it over shoots in the negative direction by 500mm.
Take a look at the PLC Open motion function blocks, for example the MC_MoveRelative and the MC_MoveContinuesRelative block:
(Beckhoff documentation)
As Sergey already stated you can use those blocks to create a motion profile by entering all the parameters you need and integrating the blocks in a step chain.

bug with wrapping angle by using wrapToPi()

I found a bug in my code which affects the subsequent calculations. I have two state vectors, one for the actual robot's movement and the second one for the estimated state vector. For the robot
robot = [101.3203; % x
170.6334; % y
2.1103]; % theta in radian
From this position, the robot makes an observation to a beacon located at <200,0> (i.e. <x,y>). Now, the range and angle between the robot and beacon is computed as following
sigma_phi = (degtorad(1))/2; % noise of the angle
sigma_r = (0.001)/2; % noise of the range
% range with some noise from robot to beacon
qr = sqrt((200 - robot(1))^2 + (0 - robot(2))^2) + sigma_r*randn();
% angle with some noise in radian from the robot to beacon
phi = wrapToPi(atan2(0 - robot(2), 200 - robot(1)) - robot(3) + sigma_phi*randn());
the angle is now phi = 3.1285 in radian. For the estimated state vector, I have
Mu = [101.2143; % x
171.0308; % y
2.094]; % theta in radian
From this estimated position, the estimated state vector makes an observation to a beacon at <200,0> (i.e. <x,y>). Now, the range and angle between the estimated state vector and beacon is computed with no noise as following
q = sqrt((200 - Mu(1))^2 + (0 - Mu(2))^2);
theta = wrapToPi(atan2(0 - Mu(2), 200 - Mu(1)) - Mu(3));
the angle is now theta = -3.1410 in radian. My question is why phi is in the opposite direction of theta even though the robot and the estimated state vector have close values??
You say theta = -3.1410, measured in radians. That's very close to -pi radians. Add a little noise to your estimation, and you can easily obtain an angle that is slightly less
than (more negative than) -pi radians.
If you give wrapToPi an angle slightly less than -pi radians, it will return an angle
that is slightly less than pi radians instead.
That is, as far as wrapToPi is concerned, there is not much difference between
your angles phi and theta. If you compute wrapToPi(phi - theta), which is what
you should do if you really want to know how different two directional angles are,
the result will be a relatively small number (something less than 0.02, I think).

point projection into yx rotated plane

I want to simulate depth in a 2D space, If I have a point P1 I suppose that I need to project that given point P1 into a plane x axis rotated "theta" rads clockwise, to get P1'
It seems that P1'.x coord has to be the same as the P1.x and the P1'.y has to b shorter than P1.y. In a 3D world:
cosa = cos(theta)
sina = sin(theta)
P1'.x = P1.x
P1'.y = P1.y * cosa - P1.z * sina
P1'.z = P1.y * sina + P1.z * cosa
Is my P1.z = 0? I tried it and P1'.y = P1.y * cosa doesn't result as expected
Any response would be appreciated, Thanks!
EDIT: What I want, now I rotate camera and translate matrix
EDIT 2: an example of a single line with a start1 point and a end1 point (it's an horizontal line, result expected is a falling line to the "floor" as long as tilt angle increases)
I think it's a sign error or an offset needed (java canvas drawing (0,0) is at top-left), because my new line with a tilt of 0 is the one below of all and with a value of 90ยบ the new line and the original one match
The calculation you are performing is correct if you would like to perform a rotation around the x axis clockwise. If you think of your line as a sheet of paper, a rotation of 0 degrees is you looking directly at the line.
For the example you have given the line is horizontal to the x axis. This will not change on rotation around the x axis (the line and the axis around which it is rotating are parallel to one another). As you rotate between 0 and 90 degrees the y co-ordinates of the line will decrease with P1.y*cos(theta) down to 0 at 90 degrees (think about the piece of paper we have been rotating around it's bottom edge, the x axis, at 90 degrees the paper is flat, and the y axis is perpendicular to the page, thus both edges of the page have the same y co-ordinate, both the side that is the "x-axis" and the opposite parallel side will have y=0).
Thus as you can see for your example this has worked correctly.
EDIT: The reason that multiplying by 90 degrees does not give an exactly zero answer is simply floating point rounding

Dome rotation on arbitrary axis?

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

Cant Understand Angle of Inclination Calculation using Accelerometer on iPhone

double = rollingZ = acceleration.x;
double = rollingX = acceleration.y;
if (rollingZ > 0.0) {
self.centerCoordinate.inclination = atan(rollingX / rollingZ) + M_PI / 2.0; //LINE 1
}
else if (rollingZ < 0.0) {
self.centerCoordinate.inclination = atan(rollingX / rollingZ) - M_PI / 2.0; // LINE 2
}
else if (rollingX < 0) {
self.centerCoordinate.inclination = M_PI/2.0; //atan returns a radian
}
else if (rollingX >= 0) {
self.centerCoordinate.inclination = 3 * M_PI/2.0;
Im just trying to fully understand this piece of code. I'm looking to build AR apps on the iphone and this code has the function of calculating the angle of inclination of the device using the accelerometer readings.
My understanding is this:
Assuming a portrait orientation if i roll the device forward the x axis of the accelerometer increases towards a negative number of -1.0 (i.e. the device is laid flat with the screen facing up). If i tilt the device towards me the x axis value increases towards a value of 1.0 (until the device is flat facing the ground).
The y axis changes up and down its axis between -1.0 and 0.0 (0 implies the device is horizontal).
If we take some example readings say x = 0.5 (a -45 degree angle, tilting the device towards me) and y = 0.8. If i plotted this on a cartesian coordinate graph with y (rollingX as the vertical axis) and x (rollingZ as the horizontal) and draw a line between them i understand that i can use the reverse tangent function (atan) to calculate the angle. My confusion comes on line 1. I dont understand why that line adds 90 degrees (in radians) to the calculated angle given by the atan function?
I just cant seem to visualise on a graph whats going on. If someone could shed some light on this - that would be much appreciated.
I suppose that these +90 degrees or -90 degrees (in case of negative rollingZ) are added to bring inclination value to widely used Polar coordinate system with angle between -180 and 180 degrees.
Assuming that you have Z line projecting upward when you look at the screen of the device and Z line looking at you from the screen, the result of calculations above vill give you an angle between screen plane and horizontal plane.
Let us assume that acceleration value is positive when it is goes "inside" the device:
1) Device is in vertical position, we have rollingZ = 1, rollingX = 0. The code returns 90 degrees.
2) Device is tilted towards user. Let rollingZ be 0.7 and rollingX be -0.7. This will give us 45 degree angle.
3) Device is in upside-down position, now we have rollingZ = -1 and rollingX = 0, and it is -90 degrees.