Calculate GPS position offset using roll, pitch, yaw and length

iticus
Posts: 10
Joined: Thu Sep 20, 2018 8:42 pm
Location: Romania
Contact: Website

Calculate GPS position offset using roll, pitch, yaw and length

Hello,

I'm using a Reach RS+ device connected via Wifi to a Raspberry Pi3.
The Pi reads from Reach the GPS position and IMU data (in the form of roll, pitch and yaw - the IMU data processing is done on the Reach device using RTIMULib2).
The Reach device is mounted on a rod as you can see on the manufacturer's website in the "Point Collection" section.
This all works OK but I'm having a difficult time calculating the final GPS position of the other end of the rod that the Reach device is mounted 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 and facing north (in terms of orientation: roll=0, pitch=0, yaw=0), X and Y stay the same while

Code: Select all

``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.
Basically I need the following formulas:

Code: Select all

``````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)``````
I found some information in this post but I need a way to incorporate the rod length as well.
I've also looked at rotation matrices, "point on a sphere", triangle equations but I can't find the right solution.

Can someone point me in the right direction? I'm interested in Linux, Python, Raspberry Pi, PostgreSQL, Tornado, GIS
Relevant projects:
- https://github.com/iticus/alfred
- https://github.com/iticus/picamweb

iticus
Posts: 10
Joined: Thu Sep 20, 2018 8:42 pm
Location: Romania
Contact: Website

Re: Calculate GPS position offset using roll, pitch, yaw and length

The IMU axes are the same as AstroPi's from what I can tell from the image seen here.
Considering I need the calculate deltas (dx, dy and dz) to add to the initial position I tried to think of all possible positions and it defines a sphere having the GPS/IMU device as center.
To determine the position on a sphere I would need radius (which is rod (stick) length), polar angle (theta - which could be either roll or pitch) and azimuthal angle (phi = yaw).
If I consider theta = pitch I could use spherical coordinates to cartesian coordinates transformation to find dx, dy and dz but that means that I would ignore roll so perhaps I need to rotate using roll and then apply spherical to cartesian?
I'm interested in Linux, Python, Raspberry Pi, PostgreSQL, Tornado, GIS
Relevant projects:
- https://github.com/iticus/alfred
- https://github.com/iticus/picamweb

Idahowalker
Posts: 445
Joined: Wed Jan 03, 2018 5:43 pm

Re: Calculate GPS position offset using roll, pitch, yaw and length

Yes, the points around the rod make a circle but the point, also make triangles.

If you have a X/Y system around the rod based upon North, South, East, West, and rod height you can use trig to find your positional offset distances around the rod from Lat and Lon.
Without knowing why you are deleting my postings, I will not know how...

iticus
Posts: 10
Joined: Thu Sep 20, 2018 8:42 pm
Location: Romania
Contact: Website

Re: Calculate GPS position offset using roll, pitch, yaw and length

The direction (heading) is given by yaw; the raw values I'm getting from the IMU are in the [-180,180] interval but this can be easily translated to [0,360].
Here are values for yaw:

Code: Select all

``````yaw = 0 //North
yaw = 90 //East
yaw = 180 //South
yaw = 270 //West
yaw = 360 //North``````
I'm interested in Linux, Python, Raspberry Pi, PostgreSQL, Tornado, GIS
Relevant projects:
- https://github.com/iticus/alfred
- https://github.com/iticus/picamweb