I am trying to subtract gravity from acceleration data. I have the quaternion values in w, x, y, z form. This is my code:
# acc - accelerometer data
# g - gravity vector g = (0, 0, 9.81)
# q - orientation quaternion
# The approach is to rotate gravity vector to the current orientation and subtract it from acc.
def subtract_gravity(acc, g, q):
# rotate gravity to the current orientation q, by performing q * g * q_conjugate
g_rotated = rotate(g, q)
# subtract rotated g from acc
return np.subtract(acc, g_rotated)
# For the rotation, we need to perform qgq_conjuagte
def rotate(g, q):
# Convert gravity vector into quaternion
q_g = (0.0,) + g
# rotate gravity to the current orientation q, by performing q * g * q_conjugate
return q_mult(q_mult(q, q_g), q_conjugate(q))[1:]
# Normalize quaternion - the norm should be 1
def normalize(q, tolerance=0.00001):
mag2 = sum(n * n for n in q)
if abs(mag2 - 1.0) > tolerance:
mag = sqrt(mag2)
q = tuple(n / mag for n in q)
return q
def q_mult(q1, q2):
w1, x1, y1, z1 = q1
w2, x2, y2, z2 = q2
w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2
z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2
return w, x, y, z
def q_conjugate(q):
w, x, y, z = q
return (w, -x, -y, -z)
a = [9.1, -3.7, -0.03]
b = [0.12, 0.79, 0.56, 0.21]
acc = np.array(a)
q = np.array(b)
g = (0, 0, 9.81)
output = subtract_gravity(acc, g, q)
However, the output I am getting seems incorrect. I am keeping the IMU such that the gravity is predominantly affecting the X-axis and so gravity subtraction should just result in near zero x-axis, but the above code outputs [4.48 -4.25 8.61]. Any ideas on why this is happening - are there bugs in my logic?
Related
I am trying to multiply a quaternion with a vector following the formula q * v * q_conjugate(q) I found in this thread. When I run the following code in Python, I get an output that looks correct. However, when I run the code again I get another output. How can that be?
Hope you can help!
def q_conjugate(q):
w, x, y, z = q
return (w, -x, -y, -z)
def q_mult(q1, q2):
w1, x1, y1, z1 = q1
w2, x2, y2, z2 = q2
w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2
z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2
return w, x, y, z
def qv_mult(q1, v1):
return q_mult(q_mult(q1, v1), q_conjugate(q1))[1:]
v = np.array([0.0, -0.118443, -0.412128, 0.006673])
q = np.array([0.921369, 0.027301, 0.049432, -0.384565])
res = qv_mult(q, v)
Output 1: (0.20736419033763884, -0.37378682006270764, 0.03473104416868859)
Output 2: (-0.37553122668322314, -0.2065883731602419, 0.01484188589166425)
Easy answere. Use pyquaternion to rotate the vector. The correct answere should be output 1.
from pyquaternion import Quaternion
Quaternion([0.921369, 0.027301, 0.049432, -0.384565]).conjugate.rotate([-0.122545--0.004102, -0.214168-0.19796, -0.010722--0.017395])
I am attempting to use the 4th order Yoshida integration technique to model the orbit of satellites in circular orbits around the Earth.
However, the orbits I achieve spiral away quite quickly. The code for a Moon like satellite is below. Interestingly, the particles behaved when I use Euler method, however, I wanted to try a more accurate method. The issue could then be within how I have implemented the algorithm itself.
I have tried using the gravitational parameter rather then computing G*M, but this did not help. I also reduced the time-step, messed around with units, printed and checked values for various integration steps etc., but could not find anything.
Is this the correct use of this algorithm?
G = 6.674e-20 # km^3 kg^-1 s^-2
day = 60.0 * 60.0 * 24.0 # length of day
dt = day / 10.0
M = 5.972e24 # kg
N = 1
delta = np.random.random(1) * 2.0 * np.pi / N
angles = np.linspace(0.0, 2.0 * np.pi, N) + delta
rad = np.random.uniform(low = 384e3, high = 384e3, size = (N))
x, y = rad * np.cos(angles), ringrad * np.sin(angles)
vx, vy = np.sqrt(G*M / rad) * -np.sin(angles), np.sqrt(G*M / rad) * np.cos(angles)
def update(frame):
global x, y, vx, vy, dt, day
positions.set_data(x, y)
# coefficients
q = 2**(1/3)
w1 = 1 / (2 - q)
w0 = -q * w1
d1 = w1
d3 = w1
d2 = w0
c1 = w1 / 2
c2 = (w0 + w1) / 2
c3 = c2
c4 = c1
# Step 1
x1 = x + c1*vx*dt
y1 = y + c1*vy*dt
dist1 = np.hypot(x1, y1)
acc1 = -(G*M) / (dist1**2.0)
dx1 = x1 - x
dy1 = y1 - y
accx1 = (acc1*dx1)/(x1)
accy1 = (acc1*dy1)/(y1)
vx1 = vx + d1*accx1*dt
vy1 = vy + d1*accy1*dt
# Step 2
x2 = x1 + c2*vx1*dt
y2 = y1 + c2*vy1*dt
dist2 = np.hypot(x2, y2)
acc2 = -(G*M) / (dist2**2.0)
dx2 = x2 - x1
dy2 = y2 - y1
accx2 = (acc2*dx2)/(x2)
accy2 = (acc2*dy2)/(y2)
vx2 = vx1 + d2*accx2*dt
vy2 = vy1 + d2*accy2*dt
# Step 3
x3 = x2 + c3*vx2*dt
y3 = y2 + c3*vy2*dt
dist3 = np.hypot(x3, y3)
acc3 = -(G*M) / (dist3**2.0)
dx3 = x3 - x2
dy3 = y3 - y2
accx3 = (acc3*dx3)/(x3)
accy3 = (acc3*dy3)/(y3)
vx3 = vx2 + d3*accx3*dt
vy3 = vy2 + d3*accy3*dt
# Full step
x = x3 + c4*vx3*dt
y = y3 + c4*vy3*dt
vx = vx3
vy = vy3
return positions
I would like to reset/remove offset quaternion data from real-time IMU sensor recordings. E.g. I get [-0.754, -0.0256, 0.0321, 0.324] (XYZW) when I start recordings. I would like to reset this to be [0, 0, 0, 1] when I start recording, as I am using it to rotate a 3D object which initial quaternion values are [X: 0, Y: 0, Z: 0, W: 1].
I tried subtracting all quaternion data from the first as so:
counter = 0
quat = getting_sensor_data_from_somewhere()
while True:
if counter == 1:
offset = quat
calib_data = [x1 -x2 for x1, x2 in zip(quat, offset)
However, it doesn't seem to be the right solution. I am a bit confused of why W should be 1. Can you help me?
The 3D model are from ThreeJS libraries.
Try this out:
def quaternion_multiply(quaternion1, quaternion0):
import numpy as np
w0, x0, y0, z0 = quaternion0
w1, x1, y1, z1 = quaternion1
return np.array([-x1 * x0 - y1 * y0 - z1 * z0 + w1 * w0,
x1 * w0 + y1 * z0 - z1 * y0 + w1 * x0,
-x1 * z0 + y1 * w0 + z1 * x0 + w1 * y0,
x1 * y0 - y1 * x0 + z1 * w0 + w1 * z0], dtype=np.float64)
glob_quat = [1,0,0,0]
counter = 0
quat = getting_sensor_data_from_somewhere() # W,X,Y,Z order
while True:
if counter == 0:
quat_first = [quat[0], -quat[1], -quat[2], -quat[3]]
offset = quaternion_multiply(glob_quat, quat_first)
quat_calib = quaternion_multiply(offset, quat)
# Now first quat will always be [1, 0, 0, 0]
# Shift places if W is at the end instead of in the start
Nice description of usage of equation rotation about axis set as quaterion can be found here:
http://wiki.alioth.net/index.php/Quaternion
quote
A quaternion is a set of four values (W X Y Z) that are used in Oolite to specify a rotation in 3D space. To specify a particular rotation you need to think about the axis about which the rotation is made and the angle or amount by which the model is to be rotated.
For a given axis (x y z) and angle (α), the quaternion representing a rotation of a degrees around the axis from the origin (0,0,0) to (x,y,z) is:
W = cos (0.5 × α)
X = x × sin (0.5 × α)
Y = y × sin (0.5 × α)
Z = z × sin (0.5 × α)
So a rotation of 90 degrees about the z axis (0 0 1) would be:
W = cos 45 ° = 0.707…
X = 0 × sin 45 ° = 0
Y = 0 × sin 45 ° = 0
Z = 1 × sin 45 ° = 0.707…
That mean You need to select different axis to achieve the setting to [0,0,0,1] as desired.
I'd like to use an implementation of RK4 I found online for something, but I'm having a bit of difficulty wrapping my head around the implementations I have found online.
For example:
def rk4(f, x0, y0, x1, n):
vx = [0] * (n + 1)
vy = [0] * (n + 1)
h = (x1 - x0) / float(n)
vx[0] = x = x0
vy[0] = y = y0
for i in range(1, n + 1):
k1 = h * f(x, y)
k2 = h * f(x + 0.5 * h, y + 0.5 * k1)
k3 = h * f(x + 0.5 * h, y + 0.5 * k2)
k4 = h * f(x + h, y + k3)
vx[i] = x = x0 + i * h
vy[i] = y = y + (k1 + k2 + k2 + k3 + k3 + k4) / 6
return vx, vy
Could someone please help me understand what exactly the parameters are? If possible, I'd like a more general explanation, but, if being more specific makes it easier to explain, I'm going to be using it specifically for an ideal spring system.
You are asking for the parameters here:
def rk4(f, x0, y0, x1, n):
...
return vx, vy
f is the ODE function, declared as def f(x,y) for the differential equation y'(x)=f(x,y(x)),
(x0,y0) is the initial point and value,
x1 is the end of the integration interval [x0,x1]
n is the number of sub-intervals or integration steps
vx,vx are the computed sample points, vy[k] approximates y(vx[k]).
You can not use this for the spring system, as that code only works for a scalar v. You would need to change it to work with numpy for vector operations.
def rk4(func, x0, y0, x1, n):
y0 = np.array(y0)
f = lambda x,y: np.array(func(x,y))
vx = [0] * (n + 1)
vy = np.zeros( (n + 1,)+y0.shape)
h = (x1 - x0) / float(n)
vx[0] = x = x0
vy[0] = y = y0[:]
for i in range(1, n + 1):
k1 = h * f(x, y)
k2 = h * f(x + 0.5 * h, y + 0.5 * k1)
k3 = h * f(x + 0.5 * h, y + 0.5 * k2)
k4 = h * f(x + h, y + k3)
vx[i] = x = x0 + i * h
vy[i] = y = y + (k1 + 2*(k2 + k3) + k4) / 6
return vx, vy
I have a travel time map, I want to get the integer points along the shortest path from source to receiver.
My present solution is that I make a runge-kutta integration from the receiver location and get a series of float points. Then I sample every 5 or some number of points and assume it a straight line between in order to use the Bresenham's line algorithm. With this approach, I will get the integer points.
However, it's not enough fast. Because I need to calculate a lot of receivers' shortest path, the sum of time will be very large.
I used line_profiler to analysis the time-consuming, which shows the major part of time is for function ruge-kutta and its calling function get_velocity
codes are below
def optimal_path_2d(gradx_interp,
grady_interp,
starting_point,
dx,
N=100):
"""
Find the optimal path from starting_point to the zero contour
of travel_time. dx is the grid spacing
Solve the equation x_t = - grad t / | grad t |
"""
def get_velocity(position):
""" return normalized velocity at pos """
x, y = position
vel = np.array([gradx_interp(y, x)[0][0], grady_interp(y, x)[0][0]])
return vel / np.linalg.norm(vel)
def runge_kutta(pos, ds):
""" Fourth order Runge Kutta point update """
k1 = ds * get_velocity(pos)
k2 = ds * get_velocity(pos - k1 / 2.0)
k3 = ds * get_velocity(pos - k2 / 2.0)
k4 = ds * get_velocity(pos - k3)
return pos - (k1 + 2 * k2 + 2 * k3 + k4) / 6.0
x = runge_kutta(starting_point, dx)
xl, yl = [], []
for i in range(N):
xl.append(x[0])
yl.append(x[1])
x = runge_kutta(x, dx)
distance = ((x[0] - xl[-1])**2 +
(x[1] - yl[-1])**2)**0.5
if distance < dx*0.9:
break
return yl, xl
def get_curve(x_curve, y_curve, num_interval):
"""Curve Algorithm based on Bresenham's Line Algorithm
Produces a list of tuples
"""
num = len(x_curve)
if num < num_interval:
print("num_interval is too large.")
ret_set = set()
x0 = x_curve[0]
y0 = y_curve[0]
for i in range(num_interval, num, num_interval):
x1 = x_curve[i]
y1 = y_curve[i]
points_on_line = get_line((x0, y0), (x1, y1))
ret_set.update(points_on_line)
x0 = x1
y0 = y1
if num % num_interval != 0:
n = int(num/num_interval)*num_interval
x0 = x_curve[n]
y0 = y_curve[n]
x1 = x_curve[-1]
y1 = y_curve[-1]
points_on_line = get_line((x0, y0), (x1, y1))
ret_set.update(points_on_line)
return list(ret_set)
def get_line(start, end):
"""modifed version of Bresenham's Line Algorithm
Produces a list of tuples from start and end
>>> points1 = get_line((0, 0), (3, 4))
>>> points2 = get_line((3, 4), (0, 0))
>>> assert(set(points1) == set(points2))
>>> print points1
[(0, 0), (1, 1), (1, 2), (2, 3), (3, 4)]
>>> print points2
[(3, 4), (2, 3), (1, 2), (1, 1), (0, 0)]
"""
# Setup initial conditions
x1, y1 = (int(x) for x in start)
x2, y2 = (int(x) for x in end)
dx = x2 - x1
dy = y2 - y1
# Determine how steep the line is
is_steep = abs(dy) > abs(dx)
# Rotate line
if is_steep:
x1, y1 = y1, x1
x2, y2 = y2, x2
# Swap start and end points if necessary and store swap state
swapped = False
if x1 > x2:
x1, x2 = x2, x1
y1, y2 = y2, y1
swapped = True
# Recalculate differentials
dx = x2 - x1
dy = y2 - y1
# Calculate error
error = int(dx / 2.0)
ystep = 1 if y1 < y2 else -1
# Iterate over bounding box generating points between start and end
y = y1
points = []
for x in range(x1, x2 + 1):
coord = (y, x) if is_steep else (x, y)
points.append(coord)
error -= abs(dy)
if error < 0:
y += ystep
error += dx
# Reverse the list if the coordinates were swapped
if swapped:
points.reverse()
return points
nx = 100
ny = 100
num_interval = 5
loc_src = (10, 10)
loc_rec = (70, 90)
coordx = np.arange(nx)
coordy = np.arange(ny)
X, Y = np.meshgrid(coordx, coords)
travel_time = (X-loc_src[0])**2/5 + (Y-loc_src[1])**2/10 # for simplicity
grad_t_y, grad_t_x = np.gradient(travel_time, dx)
if isinstance(travel_time, np.ma.MaskedArray):
grad_t_y[grad_t_y.mask] = 0.0
grad_t_y = grad_t_y.data
grad_t_x[grad_t_x.mask] = 0.0
grad_t_x = grad_t_x.data
gradx_interp = RectBivariateSpline(coordy, coordx, grad_t_x)
grady_interp = RectBivariateSpline(coordy, coordx, grad_t_y)
yl, xl = optimal_path(gradx_interp, grady_interp, loc_rec, dx)
grid_indx = get_curve(xl, yl, num_interval)
I hear that Cython will be faster, then I learn a little recently and try it. the result is only 2 faster than codes above because I'm really new to Cython. The code below is incomplete, and I just wrote it for testing.
import numpy as np
from numpy.core.umath_tests import inner1d
def func(X_interp, Y_interp):
def get_velocity(double x, double y ):
""" return normalized velocity at pos """
cdef double vel[2], norm
a = X_interp(y, x)
vel[0] = a[0][0]
b = Y_interp(y, x)
vel[1] = b[0][0]
# norm = (vel[0]**2 + vel[1]**2)**0.5
# vel[0] = vel[0]/norm
# vel[1] = vel[1]/norm
return vel
def runge_kutta(double x, double y, double ds):
""" Fourth order Runge Kutta point update """
cdef double k1[2], k2[2], k3[2], k4[2], r[2], pos[2]
pos[0] = x; pos[1] = y
k1 = get_velocity(pos[0], pos[1])
k2 = get_velocity(pos[0] - k1[0]/2.0*ds,pos[1] - k1[1]/2.0*ds)
k3 = get_velocity(pos[0] - k2[0]/2.0*ds,pos[1] - k2[1]/2.0*ds)
k4 = get_velocity(pos[0] - k3[0]/2.0*ds,pos[1] - k3[1]/2.0*ds)
cdef size_t i
for i in range(2):
r[i] = pos[i] - ds * (k1[i] + 2*k2[i] + 2*k3[i] + k4[i])/6.0
return r
for i in range(50):
runge_kutta(0, 0, 1.)
# print(runge_kutta(0, 0, 1.))