def test_Initialize(self):
s = Strapdown()
s.Initialze(toVector(0., 0., -10.), toVector(1., 0., 0.),
toVector(10., 5., 12.))
self.assertTrue(s.isInitialized)
q0, q1, q2, q3 = toValue(s.quaternion.values)
self.assertEqual(q0, 1.)
self.assertEqual(q1, 0.)
self.assertEqual(q2, 0.)
self.assertEqual(q3, 0.)
x, y, z = toValue(s.position.values)
self.assertEqual(x, 10.)
self.assertEqual(y, 5.)
self.assertEqual(z, 12.)
def test_init_values(self):
q = Quaternion(toVector(1., 0.5, -0.5))
q0, q1, q2, q3 = toValue(q.values)
self.assertAlmostEqual(q0, 0.795, delta=0.001)
self.assertAlmostEqual(q1, 0.504, delta=0.001)
self.assertAlmostEqual(q2, 0.095, delta=0.001)
self.assertAlmostEqual(q3, -0.325, delta=0.001)
def test_ell2xyz(self):
llh = EllipsoidPosition(deg2rad(toVector(52.52,13.40,0.)))
xyz = ell2xyz(llh)
x,y,z = toValue(xyz)
self.assertAlmostEqual(x, 3783323.72435, delta = 0.0001)
self.assertAlmostEqual(y, 901314.84651, delta = 0.0001)
self.assertAlmostEqual(z, 5038219.09955, delta = 0.0001)
def getConjugatedQuaternion(self):
""" returns the conjugated quaternion
"""
conjQuat = Quaternion()
q0, q1, q2, q3 = toValue(self.values)
conjQuat.values = toVector(q0, -q1, -q2, -q3)
return conjQuat
def test_init_empty(self):
q = Quaternion()
q0, q1, q2, q3 = toValue(q.values)
self.assertEqual(q0, 1.)
self.assertEqual(q1, 0.)
self.assertEqual(q2, 0.)
self.assertEqual(q3, 0.)
def test_getEulerAngles(self):
q = Quaternion(toVector(0.75, -0.51, pi))
vec = q.getEulerAngles()
phi, theta, psi = toValue(vec)
self.assertAlmostEqual(0.75, phi, delta=0.001)
self.assertAlmostEqual(-0.51, theta, delta=0.001)
self.assertAlmostEqual(pi, psi, delta=0.001)
def test_update_without_rotation(self):
q = Quaternion()
q.update(toVector(0.0, 0.0, 0.0), 0.01)
q0, q1, q2, q3 = toValue(q.values)
self.assertEqual(q0, 1.)
self.assertEqual(q1, 0.)
self.assertEqual(q2, 0.)
self.assertEqual(q3, 0.)
def __format__(self, f):
phi, theta, psi = toValue(self.values)
if f == 'deg':
return 'Roll: {:4.3f} deg, Pitch: {:4.3f} deg, Yaw: {:4.3f} deg'.format(
rad2deg(phi), rad2deg(theta), rad2deg(psi))
elif f == 'rad':
return 'Roll: {:4.3f} rad, Pitch: {:4.3f} rad, Yaw: {:4.3f} rad'.format(
phi, theta, psi)
def test_update(self):
q = Quaternion()
rotationRate = toVector(-100., 50., 120.)
q.update(rotationRate, 0.01)
q0, q1, q2, q3 = toValue(q.values)
self.assertAlmostEqual(q0, 0.68217, delta=0.0001)
self.assertAlmostEqual(q1, -0.44581, delta=0.0001)
self.assertAlmostEqual(q2, 0.22291, delta=0.0001)
self.assertAlmostEqual(q3, 0.53498, delta=0.0001)
def plotRGB(x, vector):
""" plots 3x1 vector in 1 axis
"""
y1, y2, y3 = toValue(vector)
handle1, = plt.plot(x, y1, 'ro')
handle2, = plt.plot(x, y2, 'go')
handle3, = plt.plot(x, y3, 'bo')
plt.grid(True)
plt.legend([handle1, handle2, handle3], ['X', 'Y', 'Z'])
def test_concatenation(self):
q = Quaternion()
q_increment = Quaternion(toVector(deg2rad(10.), 0., 0.))
#q.values = mvMultiplication(q.values,q_increment.values)
q *= q_increment
q0, q1, q2, q3 = toValue(q.values)
self.assertAlmostEqual(q0, 0.996, delta=0.001)
self.assertAlmostEqual(q1, 0.087, delta=0.001)
self.assertAlmostEqual(q2, 0., delta=0.001)
self.assertAlmostEqual(q3, 0., delta=0.001)
def test_update_180(self):
q = Quaternion()
rotationRate = toVector(0., deg2rad(5.), 0.) #rad/s
for _ in range(3600):
q.update(rotationRate, 0.01)
q0, q1, q2, q3 = toValue(q.values)
self.assertAlmostEqual(q0, 0.0, delta=0.0001)
self.assertAlmostEqual(q1, 0.0, delta=0.0001)
self.assertAlmostEqual(q2, 1., delta=0.0001)
self.assertAlmostEqual(q3, 0.0, delta=0.0001)
def correct(self, vector):
""" vector is defined as (N, E, D) in meter
"""
lat, _, h = toValue(self.values)
Rn, Re = earthCurvature(self.a, self.f, lat)
M = eye(3, 3)
M[0, 0] = 1 / (Rn - h)
M[1, 1] = 1 / ((Re - h)) * cos(lat)
M[2, 2] = 1
self.values += M * vector
def test_vecTransformation(self):
q = Quaternion(toVector(0.1, 0.5, 0.2))
vec1 = toVector(0.1, -2., 9.81)
vec2 = q.vecTransformation(vec1)
self.assertAlmostEqual(vec2[0].item(), 5.168, delta=0.001)
self.assertAlmostEqual(vec2[1].item(), -1.982, delta=0.001)
self.assertAlmostEqual(vec2[2].item(), 8.343, delta=0.001)
a, b, c = toValue(vec2)
self.assertAlmostEqual(pythagoras(0.1, -2., 9.81),
pythagoras(a, b, c),
delta=0.001)
def ell2xyz(pos_obj):
""" transformation of geographic coordinates to cartesian coordinates
input is an EllipsoidPosition-object
returns a 3x1 vector
"""
lat, lon, he = toValue(pos_obj.values)
_, N = earthCurvature(pos_obj.a, pos_obj.f, lat)
x = (N + he) * cos(lat) * cos(lon)
y = (N + he) * cos(lat) * sin(lon)
z = N * sin(lat) * (pos_obj.b**2 / pos_obj.a**2) + he * sin(lat)
return toVector(x, y, z)
def plotVector(x, vector):
""" plots 3x1 vector using subplots
"""
y1, y2, y3 = toValue(vector)
symbol = 'ro'
plt.subplot(311)
plt.plot(x, y1, symbol)
plt.subplot(312)
plt.plot(x, y2, symbol)
plt.subplot(313)
plt.plot(x, y3, symbol)
def update(self, velocity, DT):
""" updates current position based on previous position and velocity
velocity-object has attribute values in m/s
"""
lat, _, h = toValue(self.values)
Rn, Re = earthCurvature(self.a, self.f, lat)
M = eye(3, 3)
M[0, 0] = 1 / (Rn - h)
M[1, 1] = 1 / ((Re - h)) * cos(lat)
M[2, 2] = 1
self.values += (DT * M) * velocity.values
def plot3DFrame(s, ax):
""" transforms and plots 3 orthognal vectors representing the orientation
requires a strapdown-object and an 3D axis as argument
"""
q = s.quaternion.getConjugatedQuaternion()
xn = toVector(0, 1, 0)
yn = toVector(1, 0, 0)
zn = toVector(0, 0, -1)
xb = q.vecTransformation(xn)
yb = q.vecTransformation(yn)
zb = q.vecTransformation(zn)
x1, x2, x3 = toValue(xb)
y1, y2, y3 = toValue(yb)
z1, z2, z3 = toValue(zb)
xb = [x1, x2, x3]
yb = [y1, y2, y3]
zb = [z1, z2, z3]
for i in range(3):
ax.plot([0, xb[i]], [0, yb[i]], zs=[0, zb[i]])
ax.auto_scale_xyz([-1, 1], [-1, 1], [-1, 1])
ax.set_xlabel('East')
ax.set_ylabel('North')
ax.set_zlabel('Down')
phi, theta, psi = s.getOrientation()
vx, vy, vz = s.getVelocity()
px, py, pz = s.getPosition()
s1 = ' Roll: %.4f\n Pitch: %.4f\n Yaw: %.4f\n\n' % (
rad2deg(phi), rad2deg(theta), rad2deg(psi))
s2 = ' vx: %.2f\n vy: %.2f\n vz: %.2f\n\n' % (vx, vy, vz)
s3 = ' px: %.2f\n py: %.2f\n pz: %.2f\n' % (px, py, pz)
plt.figtext(0, 0, s1 + s2 + s3)
def __init__(self, acceleration=-G, magneticField=EARTHMAGFIELD):
""" calculates the bearing from raw acceleration and magnetometer values
accelaration in m/s2 and magnetic field in gauss
angles are saved in radians
calling w/o arguments creates a vector with phi, theta, psi = 0
"""
ax, ay, az = toValue(acceleration)
mx, my, mz = toValue(magneticField)
if isclose(pythagoras(ax, ay, az), 0., abs_tol=0.001):
raise ValueError("Acceleration is not significant")
if isclose(pythagoras(mx, my, mz), 0., abs_tol=0.001):
raise ValueError("MagneticFlux is not significant")
phi = -atan2(ay, -az) #phi = asin(-ay/g*cos(theta))
theta = asin(ax / g)
# transformation to horizontal coordinate system - psi = 0
q = Quaternion(toVector(phi, theta, 0.))
mHor = q.vecTransformation(magneticField)
mxh, myh, _ = toValue(mHor)
psi = atan2(myh, mxh) - DECANGLE
self.values = toVector(phi, theta, psi)
def __init__(self, euler=toVector(0., 0., 0.)):
""" Quaternion is initiated by Euler angles
the angles are given in radians using ZYX-convention
"""
phi, theta, psi = toValue(euler)
ph2 = phi / 2
th2 = theta / 2
ps2 = psi / 2
q0 = cos(ph2) * cos(th2) * cos(ps2) + sin(ph2) * sin(th2) * sin(ps2)
q1 = sin(ph2) * cos(th2) * cos(ps2) - cos(ph2) * sin(th2) * sin(ps2)
q2 = cos(ph2) * sin(th2) * cos(ps2) + sin(ph2) * cos(th2) * sin(ps2)
q3 = cos(ph2) * cos(th2) * sin(ps2) - sin(ph2) * sin(th2) * cos(ps2)
self.values = toVector(q0, q1, q2, q3)
def getEulerAngles(self):
""" calculates Euler angles from the current Quaternion
result is given in a 3x1 vector in radians
"""
q0, q1, q2, q3 = toValue(self.values)
try:
phi = atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1**2 + q2**2))
st = 2 * (q0 * q2 - q3 * q1)
st = 1 if st > 1 else st #gimbal lock
st = -1 if st < -1 else st
theta = asin(st)
psi = atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2**2 + q3**2))
except ValueError:
raise ValueError('Quaternion is invalid', q0, q1, q2, q3)
return toVector(phi, theta, psi)
def getRotationMatrix(self):
""" creates the 3x3 rotation matrix from quaternion parameters
represents the same relation between coordinate systems
return a numpy.matrix
"""
q0, q1, q2, q3 = toValue(self.values)
r11 = q0**2 + q1**2 - q2**2 - q3**2
r22 = q0**2 - q1**2 + q2**2 - q3**2
r33 = q0**2 - q1**2 - q2**2 + q3**2
r12 = 2 * (q1 * q2 - q0 * q3)
r13 = 2 * (q1 * q3 + q0 * q2)
r23 = 2 * (q2 * q3 - q0 * q1)
r21 = 2 * (q1 * q2 + q0 * q3)
r31 = 2 * (q1 * q3 - q0 * q2)
r32 = 2 * (q2 * q3 + q0 * q1)
return matrix([[r11, r12, r13], [r21, r22, r23], [r31, r32, r33]])
def data_listener():
s = Strapdown()
rot_mean = toVector(0., 0., 0.)
acc_mean = toVector(0., 0., 0.)
mag_mean = toVector(0., 0., 0.)
gyroBias = toVector(0., 0., 0.)
i = 0
init_time = 1 / 6 #min
while True:
if imu.IMURead():
data = imu.getIMUData()
gyro = data["gyro"]
gyro_v = toVector(gyro[0], gyro[1], gyro[2]) - gyroBias
accel = data["accel"]
accel_v = toVector(accel[0], accel[1], accel[2]) * -9.80665
mag = data["compass"]
mag_v = toVector(mag[0], mag[1], mag[2])
if not s.isInitialized:
rot_mean = runningAverage(rot_mean, gyro_v, 1 / i)
acc_mean = runningAverage(acc_mean, accel_v, 1 / i)
mag_mean = runningAverage(mag_mean, mag_v, 1 / i)
if (i * poll_interval / 1000) % 60 == 0:
print('Initialized in %.1f minutes\n' %
(init_time - i * poll_interval / 1000. / 60., ))
if i * poll_interval / 1000 >= init_time:
s.Initialze(acc_mean, mag_mean)
gyroBias = rot_mean
print('STRAPDOWN INITIALIZED with %i values\n' % (i, ))
print('Initial bearing\n', s.getOrientation())
print('Initial velocity\n', s.getVelocity())
print('Initial position\n', s.getPosition())
print('Initial gyro Bias\n', gyroBias)
else:
s.quaternion.update(gyro_v)
s.velocity.update(accel_v, s.quaternion)
s.position.update(s.velocity)
phi, theta, psi = toValue(rad2deg(s.getOrientation()))
phi_list.append(phi)
theta_list.append(theta)
psi_list.append(psi)
i += 1
i_list.append(i)
def update(self, rotationRate, DT):
""" updates the quaternion via the rotation of the last period
the rotation rate is a 3x1 vector - wx, wy, wz
approximated quaternion differential equation
"""
w = rotationRate * DT
wx, wy, wz = toValue(w)
norm = pythagoras(wx, wy, wz)
# exact formula
# r1 = cos(norm/2)
# factor = 1/norm * sin(norm/2)
# r234 = w*factor
# series expansion
r1 = 1 - (1 / 8) * norm**2 + (1 / 384) * norm**4 - (1 /
46080) * norm**6
factor = 0.5 - (1 / 48) * norm**2 + (1 / 3840) * norm**4 - (
1 / 645120) * norm**6
r234 = w * factor
r = insert(r234, 0, r1)
self.values = mvMultiplication(self.values, r.transpose())
def test_getVelocity(self):
s = Strapdown()
v1, v2, v3 = toValue(s.getVelocity())
self.assertEqual(v1, 0.)
self.assertEqual(v2, 0.)
self.assertEqual(v3, 0.)
def __str__(self):
q0, q1, q2, q3 = toValue(self.values)
return 'q0: {:2.3f}, q1: {:2.3f}, q2: {:2.3f}, q3: {:2.3f}'.format(
q0, q1, q2, q3)
theta = 1 * pi / 180
psi = 0 * pi / 180
# bw = toVector((theta), -(phi), 0) # 5degree/s
bw = toVector(
sin(phi) * sin(psi) + cos(phi) * sin(theta) * cos(psi),
-sin(phi) * cos(psi) + cos(phi) * sin(theta) * sin(psi),
1 - cos(phi) * cos(theta))
ba = toVector(10, 10, 10) # mg
ba_ms = ba * g / 1000
for t in range(60):
dr1 = (1 / 6) * g * (bw) * t**3
dr2 = (1 / 2) * ba_ms * t**2
x, y, z = toValue(dr1)
red_dot, = plt.plot(t, x, "ro")
blue_dot, = plt.plot(t, y, "bo")
green_dot, = plt.plot(t, z, "go")
plt.legend([red_dot, blue_dot, green_dot], ["X-Pos", "Y-Pos", "Z-Pos"])
title('Positionsfehler durch Drehratenbias')
xlabel('Zeit in [sek]')
ylabel('Positionsfehler in [m]')
plt.show()
def __str__(self):
vx, vy, vz = toValue(self.values)
return 'vx: {:4.2f} m/s, vy: {:4.3f} m/s, vz: {:4.3f} ms'.format(
vx, vy, vz)
from MathLib import toVector, toValue
from math import pi
g = 9.81
ba = toVector(10, 10, 10) # mg
ba_ms = ba * g / 1000
bx, by, bz = toValue(ba_ms)
s_phi = -bx/g
print("Lagefehler phi = ", s_phi*180/pi, "deg")
s_theta = bz/g
print("Lagefehler theta = ", s_theta*180/pi, "deg")
def test_getPosition(self):
s = Strapdown()
p1, p2, p3 = toValue(s.getPosition())
self.assertEqual(p1, 0.)
self.assertEqual(p2, 0.)
self.assertEqual(p3, 0.)
