Determine new GPS position using roll, pitch, yaw and length

Question

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°.

Shagbark

Some reference values when the rod is upright, perfectly level and the Reach device LEDs are pointing north: roll = 0, pitch = 0, yaw = 0 – Shagbark

Surely the device or its RTK library does this already? It seems like a pretty basic requirement. If you can't find that information, have you asked the manufacturer? – Sadiesadira

Unfortunately the device doesn't even provide the IMU data by default (I have to compile RTIMULib2 manually and extract the data using a TCP stream). This issue has been on the manufacturer's todo list for quite some time. – Shagbark

Actually, I think you are looking for this!. Your system is likely to suffer from gimbal lock, so you can either walk into the fourth dimension or just tell everyone not to hold the stick straight! Best luck. – Sadiesadira

The vector rotating in 3D space does not address my question on how to use the length with the matrices. – Shagbark

Sorry - what you need to do is convert the length to a vector. So, if Z is up down, it would be (0, 0, -length). If Y is up/down, it would be (0, -length, 0). You multiply that vector times the 3x3 matrices, and you get the vector which represents the offset of the end of your rod to the location of the "airplane". The tricky part is keeping your axes and angular direction (basically the pluses and minuses of the angles) straight. – Sadiesadira

I haven't tried any of these in javascript, but here is one library I googled which claims to do 3D vector and matrix multiplication if you need help with that. – Sadiesadira

Answer 1

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).

1. 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...

   • "yaw" represents a change in absolute heading (so even if you are pitched and rolled, this is the direction you are pointing with respect to earth actual.
   • "pitch" represents the angle around the wings - basically whether the nose is pointing up or down.
   • "roll" represents the banking of the airplane - so whether the wing axis is parallel to the earth surface or tilted around the fuselage.

   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.

2. Device assumptions: These are things you may need to check with your vendor. If the numbers are small relative to the expected (or permissable) GPS error, it may not matter very much.
   • The code assumes that the axes all meet right at the bottom of the device, and that the rod hangs vertically down from that point. If, for example, the rod is 2 meters long and the axes actually meet three centimeters above the connection point, you would adjust the rod length to 2.03 meters. If the rod is actually attached to a point not quite under the axes intersect, the software needs to be changed a bit to account for the end not being directly under it. Again, a few millimeters may not matter to you in the grand scheme of things.
   • The code assumes that the location the device claims to be at is actually the axes intersect. If not, you will need to adjust the location to be at that point (or change the software to allow for that).
   • You need to specify the rod length in the same units as the device location.
3. Other assumptions:
   • This does NOT deal with earth curvature - unless your rod is unusually long, this shouldn't matter, and won't matter at all if you are holding it straight up (or nearly so).

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...

function presentresult(location, length, yaw, pitch, roll) {
    console.log("Starting point");
    console.log(location);
    console.log("Rod length = " + length);
    console.log("Yaw = " + yaw + ", Pitch = " + pitch + ", Roll = " + roll);
    console.log("Result:");
    console.log(rodloc(location, length, yaw, pitch, roll));
}

presentresult([100, 100, 100], 2, 0, 0, 0); // Result:  [100, 100, 98] (3)
presentresult([100, 100, 100], 2, 30, 0, 0); // Result:  [100, 100, 98] (3)
presentresult([100, 100, 100], 2, -30, 0, 0); // Result:  [100, 100, 98] (3)
presentresult([100, 100, 100], 2, 0, 30, 0); // Result:  [99, 100, 98.26794919243112] (3)
presentresult([100, 100, 100], 2, 0, -30, 0); // Result:  [101, 100, 98.26794919243112] (3)
presentresult([100, 100, 100], 2, 0, 0, 30); // Result:  [100, 101, 98.26794919243112] (3)
presentresult([100, 100, 100], 2, 0, 0, -30); // Result:  [100, 99, 98.26794919243112] (3)
presentresult([100, 100, 100], 2, 30, 30, 30); // Result:  [98.75, 100.43301270189222, 98.5] (3)
presentresult([100, 100, 100], 2, -30, -30, -30); // Result:  [100.25, 98.70096189432334, 98.5] (3)
presentresult([100, 100, 100], 2, -30, 30, -30); // Result:  [98.75, 99.56698729810778, 98.5] (3)

/** @typedef {Array<number,number,number>} */ var Vector3D;
/** @typedef {Array<Vector3D,vector3D,Vector3D>} */ var Matrix3D;

/**
 * @param {Vector3D} location - The location (3 coordinates) of the "plane"
 * @param {number} length - The length of the rod
 * @param {number} yaw - the yaw (heading) in degrees
 * @param {number} pitch - the pitch in degrees
 * @param {number} roll - the roll in degrees
 * @returns {Vector3D} - the location of the end of the rod
 */
function rodloc(location, length, yaw, pitch, roll) {
    let ryaw = yaw * Math.PI / 180.0;       // Convert to radians
    let rpitch = pitch * Math.PI / 180.0;
    let rroll = roll * Math.PI / 180.0;

    // This is where our axes start
    let x = [1, 0, 0];
    let y = [0, 1, 0];
    let z = [0, 0, 1];

    // NOTE:  ORDER MATTERS - your data may mean different things (see
    //        assumptions in answer!
    // Rotate axes around z by yaw
    let yprime = rotatearound([0, 1, 0], [0, 0, 1], ryaw);
    let xprime = rotatearound([1, 0, 0], [0, 0, 1], ryaw);
    let zprime = z;     // rotating around itself

    // Next we need to rotate for pitch (around the Y axis...)
    let x2prime = rotatearound(xprime, yprime, rpitch); 
    let y2prime = yprime; // dont need this
    let z2prime = rotatearound(zprime, yprime, rpitch);

    // Now we need to roll around the new x axis...
    let x3prime = x2prime   // dont need this
    let y3prime = rotatearound(y2prime, x2prime, rroll); // dont need this
    let z3prime = rotatearound(z2prime, x2prime, rroll);

    // now take what started out as [0, 0, 1] and place the end of the rod
    // (at what started out as [0, 0, -length])
    let rotend = [0,1,2].map(n=>-length*z3prime[n]);

    // now take that and add it to the original location of the plane 
    // and return it as the result
    return [0,1,2].map(n=>location[n]+rotend[n]);
}

/** Multiply a vector times a matrix
 * @param {Vector3D} offset - The vector of the offset
 * @param {Matrix3D} rotate - The rotation vector
 * @returns {Vector3D} - The new offset vector
 */
function vmmult(offset, rotate) {
    return [0,1,2].map(x=>xmult(offset,rotate[x]));
}

/** dot product of two vectors
 * @param {Vector3D} col
 * @param {Vector3D} row
 * @returns {number}
 */
function xmult(col, row) {
    return [0,1,2].reduce((a,c)=>a+col[c]*row[c],0);
}

/** Rotate a point around a vector projecting from the origin
 * @param {Vector3D} point - the we want to rotate
 * @param {Vector3D} vec - the vector (from origin to here) to rotate around
 * @param {number} angle - the angle (in radians) to rotate
 * @returns {Vector3D} - the new point location
 */
function rotatearound(point, vec, angle) {
    let rotmat = setuprotationmatrix(angle, vec);
    return vmmult(point, rotmat);
}

/**
 * Adapted from C courtesy of Bibek Subedi
 * https://www.programming-techniques.com/2012/03/3d-rotation-algorithm-about-arbitrary.html
 * @param {number} angle - the angle to rotate around the vector
 * @param {Vector3D} vec - the vector around which to rotate
 * @returns {Matrix3D} - the rotation matrix
 */
function setuprotationmatrix(angle, vec) {
    // Leaving L in for reusability, but it should always be 1 in our case
    let u = vec[0], v = vec[1], w = vec[2]; 
    let L = (u*u + v * v + w * w);
    let u2 = u * u;
    let v2 = v * v;
    let w2 = w * w; 

    let rotmat = [[],[],[]];
    rotmat[0][0] = (u2 + (v2 + w2) * Math.cos(angle)) / L;
    rotmat[0][1] = (u * v * (1 - Math.cos(angle)) - w * Math.sqrt(L) * Math.sin(angle)) / L;
    rotmat[0][2] = (u * w * (1 - Math.cos(angle)) + v * Math.sqrt(L) * Math.sin(angle)) / L;

    rotmat[1][0] = (u * v * (1 - Math.cos(angle)) + w * Math.sqrt(L) * Math.sin(angle)) / L;
    rotmat[1][1] = (v2 + (u2 + w2) * Math.cos(angle)) / L;
    rotmat[1][2] = (v * w * (1 - Math.cos(angle)) - u * Math.sqrt(L) * Math.sin(angle)) / L;

    rotmat[2][0] = (u * w * (1 - Math.cos(angle)) - v * Math.sqrt(L) * Math.sin(angle)) / L;
    rotmat[2][1] = (v * w * (1 - Math.cos(angle)) + u * Math.sqrt(L) * Math.sin(angle)) / L;
    rotmat[2][2] = (w2 + (u2 + v2) * Math.cos(angle)) / L;
    return rotmat;
} 

Sadiesadira

hey, thank you so much for the effort put into this detailed answer! At the moment I'm testing your solution with my own (based on three.js - I'll post the code for that one as an answer here). – Shagbark

Great! Send me a word when the dust settles to tell me how it turned out! – Sadiesadira

Answer 2

At the moment I'm testing a solution based on three.js which works something along these lines:

function getCorrectedPosition(x, y, z, dist, roll, pitch, yaw) {
    let matrix = new THREE.Matrix4().makeRotationFromEuler(new THREE.Euler(toRadians(pitch), toRadians(roll), toRadians(yaw)));
    let moveVector = new THREE.Vector3(0, 0, -dist);
    moveVector.applyMatrix4(matrix);
    let position = new THREE.Vector3(z, y, x).add(moveVector);
    return [position.x, position.y, position.z]
}

I'll post an update with results once I have them.

Shagbark

Answer 3

I don't think your problem happens to be gimbal lock as you say X and Y are the same when rod is upright. Are you sure you have places the IMU parallel to the ground. I recommend first trying to just measure the results of the IMU and when you are certain it is giving accurate results then hook it up with the RS.

If you want to measure GPS coordinates and yaw pitch roll then I recommend using a GPS module and MPU-6050 with an Arduino mini. It is small enough to be hooked with anything and also a lot lot cheaper compared to a very expensive RS+. Also with such a gadget you will find a lot more support available rather than using RS+.

Nonproductive

The values I'm getting from the IMU are OK (I have performed some calibration steps as well). – Shagbark

I'm not looking for other hardware simply because the GPS reading needs to be more accurate than regular receivers can provide; with a less accurate receiver the roll pitch calculations wouldn't be relevant. – Shagbark