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 __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 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_pythagoras_2D(self):
c = pythagoras(3., 4.)
self.assertEqual(c, 5.)
def test_pythagoras_5D(self):
c = pythagoras(3., 4., 5., 6., 7.)
self.assertAlmostEqual(c, 11.6190, delta=0.0001)
def test_pythagoras_3D(self):
c = pythagoras(3., 4., 5.)
self.assertAlmostEqual(c, 7.0711, delta=0.0001)
from MathLib import pythagoras
from numpy import deg2rad, rad2deg
from GeoLib import earthCurvature
from math import tan
ve = 100 #km/h
vn = 100 #km/h^
v = pythagoras(ve, vn)
print("Mittlere Geschwindigkeit = ", v, "km/h")
ve *= 1000 / 3600
vn *= 1000 / 3600
Lat = 52.5 #deg
Lat_rad = deg2rad(Lat)
a = 6378137.0 #WGS84
f = 1. / 298.257223563
Rn, Re = earthCurvature(a, f, Lat_rad)
h = -30 #m
wtx = ve / (Re - h)
wty = -vn / (Rn - h)
wtz = (ve * tan(Lat_rad)) / (Re - h)
print("Einfluss der Transportrate = ", wtx, wty, wtz, "rad/s")
print("Einfluss der Transportrate = ",
rad2deg(wtx) * 3600,
rad2deg(wty) * 3600,
rad2deg(wtz) * 3600, "deg/h")