I'm using a Reach RS+ device to capture GPS position data as well as IMU data (roll, pitch and yaw); see the "Point Collection" image on the manufacturer's website.
I'm trying to determine the GPS coordinates of the bottom point (the empty end of the rod that the receiver is fixed on).
To be able to make calculations in meters I'm converting longitude (X) and latitude (Y) to UTM while keeping altitude (Z) unaltered.
When the rod is upright, X and Y stay the same while
Z1 = Z - ROD_LENGTH
However when the rod is tilted all coordinates are affected and I need a way to calculate the rod's end position.
I have looked at rotation matrices, triangle equations, my own sin and cos formulas based on experimental observations but I don't have a background in 3D geometry and I'm not sure which route to take (for example I don't know how to use the rod length with a rotation matrix).
Basically I need the following formulas:
X1 = X + ROD_LENGTH * func_X(roll, pitch, yaw)
Y1 = Y + ROD_LENGTH * func_Y(roll, pitch, yaw)
Z1 = Z + ROD_LENGTH * func_Z(roll, pitch, yaw)
Roll, pitch and yaw have values between -180° and 180°.
I must say, this turned out to be a lot more complex than I expected. I think I have this all straight, but let me know in comments of any corrections and I will try to fix.
Before you look at the code, below, PLEASE NOTE! The assumptions are important and you need to verify they are true in your case. There are dozens (at least!) of ways to define orientation and locations in space. The main assumpions you need to make sure are aligned with your device are the spatial frame in which we are operating. This article will give you some appreciation for why this is so important! The most obvious being how are we labelling our axes, which way is up (positive Z, as I have chosen below, but if we were talking about submarines, for instance, we might choose negative Z).
Framework assumptions: Picture an airplane (I know its not an airplane, but its easier to explain this way) with a long rod hanging straight down. We will define the Z axis as up (positive) and down (negative). The X axis points forwards (positive) and backwards (negative). The Y axis is the axis of rotation about the wings, with positive off the left wing, and negative off the right wing - this is a "right handed coordinate system". So the axes intersect is in the middle of the airplane roughly where the wings are attached. Rotation is defined as counter-clockwise around the axis for positive angles and clockwise being negative. So...
It's important to get all this right, particularly the sign (+/-) associated with your angles - try to pitch it and roll it about 30 degrees and make sure the results agree with the output - otherwise change the sign of the angle. For yaw, you will need to change both the heading and either the pitch and roll, as the heading itself will not affect the location of the end of the rod, if its straight up and down. The data you have describing the "airplane" is location (three numbers), in the same XYZ framework as described above, and the three angles (in degrees, -180 to 180), as described above.
The code:
I left in some unnecessary things (which you might need in case you need to restructure it at all) and also didn't try to make it more efficient (for example constant recalculation of the same sins and cosines) to make it a little clearer. I left in the closure compiler typing, both for a little documentation and in case you want to minify it later.
rodloc
is the function you want...