The first IMU (called S1) is placed on the shoulder and works as a reference; the other (S2) is placed on the arm. They provide the quaternion of their rotation relative to the absolute reference (magnetic north and gravity vector). The simple idea is that I need to show the Yaw, Pitch and Roll differences between these two (e.g. an ideal abduction/adduction movement should have pitch contributions only). I started using quaternions, by calculating the rotation between the two (conj(q1) * q2) and then converting it to YPR angles by using:
# Rotation quaternion q1 = (w,x,y,z)
# unit = sum of squared elements (sqx = x^2, etc.)
yaw = math.atan2(2 * q1[2] * q1[0] - 2 * q1[1] * q1[3], sqx - sqy - sqz + sqw)
pitch = math.asin(2 * (q1[1] * q1[2] + q1[3] * q1[0]) / sqx + sqy + sqz + sqw)
roll = math.atan2(2 * q1[1] * q1[0] - 2 * q1[2] * q1[3], -sqx - sqy + sqz + sqw)
but this doesn't work in my case, since pitch and roll are not consistent in different arm positions. E.g. if relative Yaw is 90 deg, pitch and roll angles are interchanged. E.g. if I apply a pitch rotation to S2, it appears to be a roll rotation (since the rotation is on the y-axis for the reference sensor S1).
How can I avoid this?
Should I simply convert both quaternion to YPR angles and then calculate the difference of each pair (without using the difference between quaternions)? Maybe the "rotation" approach is not correct, since I don't need the inverse transformation but only the actual rotation for each axis?
Related
I had attached also the schematic to depict my question.
I need to rotate the vector V with the base point P by an angle and find the new vector V'.
The rotation axis is say for is about a local y axis at point P (which is parallel to global Y axis)
Subsequently, I need to rotate the initial vector V about x axis which is parallel to global Y axis.
The main reason for the rotation is to find the new vector V' at point P. Both the rotations are independent and each of the rotation provides a new V'. I'm programming this in VB.net and output is a double() of new vector V'.
Just apply the two rotations independently (see Wikipedia). The base point does not play any role in this because it is just a constant offset that never changes. If I got your description right, you want the following:
//rotation about y-axis
iAfterRot1 = cos(phi1) * i + sin(phi1) * k
jAfterRot1 = j
kAfterRot1 = -sin(phi1) * i + cos(phi) * k
//rotation about x-axis
iAfterRot2 = iAfterRot1
jAfterRot2 = cos(phi2) * jAfterRot1 - sin(phi2) * kAfterRot1
kAfterRot2 = sin(phi2) * jAfterRot1 + cos(phi2) * kAfterRot1
The script I'm wanting to develop uses the cartesian coordinates (XYZ) from a satellite, and in conjunction with the range, elevation and azimuth from a location, I then take a satellite’s orbital information and get the ground longitude/latitude under that satellite at a given time.
One step further from this: imagne the signal from a satellite piercing the atmosphere at exactly 300km above sea level. At this particular point when altitude is 300km, I need to calculate the ground longitude/latitude.
In the pyemph module there appears to be already a method (ephem.readtle) that can achieve this, but for TLE (two line element) data only. I'd like to use a satellite's cartesian coordinates to develop this. Is there such a method already out there? Or perhaps somebody with experience in this
domain can point me in the right direction.
A similar question already exists referring to ECEF from Azimuth, Elevation, Range and Observer Lat,Lon,Alt, but it's not the same problem.
Here's what I have developed already:
- satellite cartesian coordinates, XYZ
- azimuth, elevation and range of satellite from ground station
- ground station coordinates in lat, long, height above sea level
Here's what I need:
- ground longitude/latitude under a satellite at a specific epoch, and in particular where the piercing point in the atmosphere (the point which the signal from the satellite pierces the atmosphere) is 300km altitude.
I found what I was looking for via this:
def ionospheric_pierce_point(self, dphi, dlambda, ele, azi):
Re = 6378136.3 # Earth ellipsoid in meters
h = cs.SHELL_HEIGHT * 10**3 # Height of pierce point meters, and where maximum electron density is assumed
coeff = Re / (Re + h)
lat_rx = dphi
long_rx = dlambda
# Degrees to radians conversions
ele_rad = np.deg2rad(ele)
azi_rad = np.deg2rad(azi)
lat_rx_rad = np.deg2rad(lat_rx)
long_rx_rad = np.deg2rad(long_rx)
psi_pp = (np.pi / 2) - ele_rad - np.arcsin(coeff * np.cos(ele_rad)) # Earth central angle between user and the Eart projection of the pierce point, in radians
psi_pp_deg = np.rad2deg(psi_pp)
lat_pp = np.arcsin(np.sin(lat_rx_rad)*np.cos(psi_pp) +
np.cos(lat_rx_rad)*np.sin(psi_pp)*np.cos(azi_rad)) # in radians
if (lat_rx > 70 and ((np.tan(psi_pp)*np.cos(azi_rad)) > np.tan((np.pi/2) - lat_rx_rad))) or (lat_rx < -70 and ((np.tan(psi_pp)*np.cos(azi_rad + np.pi)) > np.tan((np.pi/2) + lat_rx_rad))):
long_pp = long_rx_rad + np.pi - np.arcsin((np.sin(psi_pp)*np.sin(azi_rad)) / np.cos(lat_pp))
else:
long_pp = long_rx_rad + np.arcsin((np.sin(psi_pp)*np.sin(azi_rad)) / np.cos(lat_pp))
lat_pp_deg = np.rad2deg(lat_pp)
long_pp_deg = np.rad2deg(long_pp)
return lat_pp_deg, long_pp_deg
I have a straight line in space with an start and end point (x,y,z) and I am attempting to get the angle between this vector and the plane defined by z=0. I am using VB.NET
Here is a picture of the line in my 3d environment (the line I'm intersted in is circled in red) :
It is set to an angle of 70 degrees right now.
You need 2 rays to define an angle.
If you want the angle between a vector and a plane, it is defined for any vector in that plane. However, there is only one minimal value for that, which is the angle between a vector and its projection onto said plane.
Therefore, that minimal value is the one we take when we speak of the angle between a vector and a plane.
This value is also π/2 - the angle between your vector and the the vector that is normal to the plane.You can read more about it all on this site.
With v your vector (thus v.x = end.x - start.x and idem for y and z), n the normal to the plane and a the angle you are looking for, we know from the definition of a scalar product that:
<v,n> = ||v|| * ||n|| * cos(π/2 - a)
We know cos(π/2 - a) = sin(a), and the normal to the z=0 plane is simply the vector n = (0, 0, 1). Thus both the scalar product, v.x * n.x + v.y * n.y + v.z * n.z, and the norm of n, ||n|| = 1, can be simplified a lot. We get the following expression:
sin(a) = v.z / ||v||
Thus finally, the formula by taking the reciprocical of the sine and expliciting the norm of v:
a = Asin(v.z / sqrt( v.x*v.x + v.y*v.y + v.z*v.z ))
According to this documentation the Asin function exists in your System.Math class. It does, however, return the value in radians:
Return Value
Type: System.Double
An angle, θ, measured in radians, such that -π/2 ≤ θ ≤ π/2
-or-
NaN if d < -1 or d > 1 or d equals NaN.
Luckily the same System.Math class contains the value of π so that you can do the conversion:
a *= 180 / Math.PI
I need to find the average Edit: total 2D velocity given multiple 2D velocities (speed and direction). A few examples:
Example 1
Velocity 1 is 90° at a speed of 10 pixels or units per second.
Velocity 2 is 270° at a speed of 5 pixels or units per second.
The average velocity is 90° at 5 pixels or units per second.
Example 2
Velocity 1 is 0° at a speed of 10 pixels or units per second
Velocity 2 is 180° at a speed of 10 pixels or units per second
Velocity 3 is 90° at a speed of 8 pixels or units per second
The average velocity is 90° at 8 pixels or units per second
Example 3
Velocity 1 is 0° at 10 pixels or units per second
Velocity 2 is 90° at 10 pixels or units per second
The average velocity is 45° at 14.142 pixels or units per second
I am using JavaScript but it's mostly a language-independent question and I can convert it to JavaScript if necessary.
If you're going to be using a bunch of angles, I would just calculate each speed,
vx = v * cos(theta),
vy = v * sin(theta)
then sum the x velocities and the y velocities separately as vector components and divide by the total number of velocities,
sum(vx) / total v, sum(vy) / total v
and then finally calculate the final speed and direction with your final vx and vy. The magnitude of the speed can be found by a simple application of pythagorean theorem, and then final angle should just be tan-1(y/x).
Per example #3
vx = 10 * cos(90) + 10 * cos(0) = 10,
vy = 10 * sin(90) + 10 * sin(0) = 10
so, tan-1(10/10) = tan-1(1) = 45
then a final magnitude of sqrt(10^2 + 10^2) = 14.142
These are vectors, and you should use vector addition to add them. So right and up are positive, while left and down are negative.
Add your left-to-right vectors (x axis).
Example 1 = -10+5 = -5
Example 2 = -8 = -8
Example 3 = 10 = 10. (90 degrees is generally 90 degrees to the right)
Add you ups and downs similarly and you get these velocities, your left-to-right on the left in the brackets, and your up-to-down on the right.
(-5, 0)
(-8,0)
(10, 10)
These vectors contain all the information you need to plot the motion of an object, you do not need to calculate angles to plot the motion of the object. If for some reason you would rather use speeds (similar to velocity, but different) and angles, then you must first calculate the vectors as above and then use the Pythagorean theorem to find the speed and simple trigonometry to get the angle. Something like this:
var speed = Math.sqrt(x * x + y * y);
var tangeant = y / x;
var angleRadians = Math.atan(tangeant);
var angleDegrees = angleRadians * (180 / Math.PI);
I'll warn you that you should probably talk to someone who know trigonometry and test this well. There is potential for misleading bugs in work like this.
From your examples it sounds like you want addition of 2-dimensional vectors, not averages.
E.g. example 2 can be represented as
(0,10) + (0,-10) + (-8, 0) = (-8,0)
The speed is then equal to the length of the vector:
sqrt(x^2+y^2)
To get average:
add each speed, and then divide by the number of speeds.
10mph + 20mph / 2 = 15
12mph + 14mph + 13mph + 16mph / 4 = 14 (13,75)
This is not so much average as it is just basic vector addition. You're finding multiple "pixel vectors" and adding them together. If you have a velocity vector of 2 pixels to the right, and 1 up, and you add it to a velocity vector of 3 pixels to the left and 2 down, you will get a velocity vector of 1 pixel left, and 1 down.
So the speed is defined by
pij = pixels going up or (-)down
pii = pixels going right or (-)left
speedi = pii1 + pii2 = 2-3 = -1 (1 pixel left)
speedj = pij1 + pij2 = 1-2 = -1 (1 pixel down)
From there, you need to decide which directions are positive, and which are negative. I recommend that left is negative, and down is negative (like a mathematical graph).
The angle of the vector, would be the arctan(speedj/speedi)
arctan(-1/-1) = 45 degrees
I want to overlay great circle arcs between airports on a map using longitudes and latitudes.
I can already get the distance and bearing from the initial and final coordinates but now I need to produce the points on the curve to plot through.
What I would like is a formula which takes the origin, destination, and a distance, and returns the latitude/longitude of the point that lies at that distance on the path between the origin and the destination.
I'm currently approximating the earth by a sphere and using radians -- eventually I'll add in spheroid corrections.
currlat = oldlat + d * sin (angle)/ (radius);
currlon = oldlon + d * cos (angle)/ (radius * cos(oldlat));
where d is distance travelled and angle is in radians. This is assuming circumference of earth at 40000km both at equator and through the poles. You can convert in radians...
Also it assumes the angle (direction) is with reference to equator line.
Obviously this needs spheroid corrections.
if you go south sin values will turn negative and north it will go positive. If you go west cos will turn negative and east it will turn positive.
d * sin(angle) and d * cos(angle) gives you the change. and you just calculate the new lat/long on that basis scaling up against circumference of earth.