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])
Related
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?
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
Can somebody please point me in the right direction...
I need to find the parameters a,b,c,d of two functions:
Y1 = ( (a * X1 + b) * p0 + (c * X2 + d) * p1 ) / (a * X1 + b + c * X2 + d)
Y2 = ( (a * X2 + b) * p2 + (c * X2 + d) * p3 ) / (a * X1 + b + c * X2 + d)
X1, X2 (independent variables) and Y1, Y2 (dependent variables) are observations, i.e. one-dimensional arrays with thousands of entries each.
p0, p1, p2, p3 are known constants (scalars).
I successfully solved the problem with the first function only with a curve-fit (see below), but how do i solve the problem for Y1 and Y2 ?
Thank you.
import numpy as np
import matplotlib.pyplot as plt
from scipy.optimize import curve_fit
X = [X1,X2]
def fitFunc(X, a,b,c,d):
X1, X2 = X
return ((a * X1 + b) * p0 + (c * X2 + d) * p1) / (a * X1 + b + c * X2 + d)
fitPar, fitCov = curve_fit(fitFunc, X, Y1)
print(fitPar)
One way would be to minimize both your functions together using scipy.optimize.minimze. In the example below, a function residual is passed a, b, c, and d as initial guesses. Using these guesses, Y1 and Y2 are evaluated, then the mean squared error is taken using the data and predicted values of respective functions. The error is returned as the mean error of the two functions. The optimized set of parameters is stored in res as res.x.
import numpy as np
from scipy.optimize import minimize
#p0 = ... known
#p1 = ... known
#p2 = ... known
#p3 = ... known
def Y1(X, a,b,c,d):
X1, X2 = X
return ((a * X1 + b) * p0 + (c * X2 + d) * p1) / (a * X1 + b + c * X2 + d)
def Y2(X, a,b,c,d):
X1, X2 = X
return ((a * X1 + b) * p2 + (c * X2 + d) * p3) / (a * X1 + b + c * X2 + d)
X1 = np.array([X1]) # your X1 array
X2 = np.array([X2]) # your X2 array
X = np.array([X1, X2])
y1_data = np.array([y1_data]) # your y1 data
y2_data = np.array([y2_data]) # your y2 data
def residual(x):
a = x[0]
b = x[1]
c = x[2]
d = x[3]
y1_pred = Y1(X,a,b,c,d)
y2_pred = Y2(X,a,b,c,d)
err1 = np.mean((y1_data - y1_pred)**2)
err2 = np.mean((y2_data - y2_pred)**2)
error = (err1 + err2) / 2
return error
x0 = [1, 1, 1, 1] # Initial guess for a, b, c, and d respectively
res = minimize(residual, x0, method="Nelder-Mead")
print(res.x)
I am trying to find the gradient of the function
f(x) = w1 * x1^2 + w2 * x2
where x is a vector coordinate (x1,x2).
def gradient(w1, w2, x):
x= (x1,x2)
gradx1=2*w1*x1 + w2 * x2
gradx2= w2 + w1 * x1^2
return (gradx1, gradx2)
My code is coming up with a nameError, saying x1 is not defined when calling the function:
gradient(5, 6, (10,10))
First things first:
x1, x2 = x # unpack your coord tuple
And secondly:
gradx2= w2 + w1 * x1 ** 2 # or gradx2= w2 + w1 * x1 * x1
in python ^ is bitwise XOR. Exponentiation is **.
x is a tuple which you need to unpack like so:
x1, x2 = x
Rather than:
x = (x1, x2)
This question already has answers here:
Creating uniform random quaternion and multiplication of two quaternions
(5 answers)
Closed 6 years ago.
I have two quaternions: Q1= w0, x0, y0, z0 and Q2 = w1, x1, y1, z1. I would like to multiply them by using NumPy or Python function which can return 2-d array. I found some pseudocodes on the internet which is written by Christoph Gohlke to do this kind of multiplication. I tried a lot but failed to apply it. Can anyone help me please to do this kind of multiplication? The pseudocodes are here: `
def quaternion_multiply(quaternion1, quaternion0):
w0, x0, y0, z0 = quaternion0
w1, x1, y1, z1 = quaternion1
return 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=float64)`
Here's a little example using your function:
import numpy as np
import random
def quaternion_multiply(quaternion1, quaternion0):
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)
N = 4
for i in range(N):
q1 = np.random.rand(4)
q2 = np.random.rand(4)
q = quaternion_multiply(q1, q2)
print("{0} x {1} = {2}".format(q1, q2, q))