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)
class Velocity_test(unittest.TestCase):
def test_init(self):
vel = Velocity(toVector(3, 4, -6))
self.assertEqual(3, vel.values[0])
self.assertEqual(4, vel.values[1])
self.assertEqual(-6, vel.values[2])
@patch('Velocity.G', toVector(0., 0., 10.))
def test_update(self):
vel = Velocity(toVector(3., 1., 2.))
acc = toVector(0.1, 2., -10.)
vel.update(acc, Quaternion(toVector(0., 0., 0.)), 1.)
self.assertEqual(3.1, vel.values[0])
self.assertEqual(3., vel.values[1])
self.assertEqual(2., vel.values[2])
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 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 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)
from Position import EllipsoidPosition, Position
from GeoLib import ell2xyz
from Velocity import Velocity
from numpy import deg2rad
msg = pynmea2.parse(
"$GPGGA,132525.000,5233.3292,N,01320.6101,E,2,07,1.38,25.9,M,44.7,M,0000,0000*52"
)
# msg = pynmea2.parse("GPGSA,A,3,14,12,32,29,24,25,31,,,,,,2.67,1.38,2.29*0D")
print('Time: {:} Lat: {:3.4f} Lon: {:3.4f} H: {:3.4f}'.format(
msg.timestamp, msg.latitude, msg.longitude,
float(msg.altitude) + float(msg.geo_sep)))
p0 = EllipsoidPosition(
deg2rad(
toVector(msg.latitude, msg.longitude,
float(msg.altitude) + float(msg.geo_sep))))
p0 = ell2xyz(p0) #xyz
t0 = msg.timestamp
t0 = (t0.hour * 3600 + t0.minute * 60 + t0.second - 1)
with open(
'D:\Masterarbeit\Code\Eclipse\ProjectIMU\data\\UltimateGPS\GPRMC_stream.csv',
'r') as fread:
streamreader = pynmea2.NMEAStreamReader(fread, 'ignore')
while 1:
for msg in streamreader.next():
if msg.sentence_type == 'GGA':
he = float(msg.altitude) + float(msg.geo_sep) #m
lat = deg2rad(msg.latitude)
lon = deg2rad(msg.longitude)
imu.setSlerpPower(0.02)
imu.setGyroEnable(True)
imu.setAccelEnable(True)
imu.setCompassEnable(True)
print("Recommended Poll Interval: %dmS\n" % imu.IMUGetPollInterval())
SatObs = NMEA_stream.GNSS()
thread = threading.Thread(target=SatObs.stream)
thread.daemon = True
thread.start()
print("Started GPS stream in a new thread")
print("Initializing ...\n")
s = Strapdown()
K = KalmanPVO()
rot_mean = toVector(0.018461137, -0.01460140625, 0.002765854)
accelBias = toVector(0., 0., 0.)
acc_mean = toVector(0., 0., 0.)
mag_mean = toVector(0., 0., 0.)
gyroBias = toVector(0., 0., 0.)
pos_mean = s.getPosition()
dt_mean = 0.01
t_old = 0.
i = 1
j = 1
init_time = 600 # seconds
total_field = 49.5898 # magnitude of magnetic flux
while True:
""" contains common variables used during the Strapdown-algorithm """ from MathLib import toVector from numpy import deg2rad # http://icgem.gfz-potsdam.de/calc g = 9.80623-0.42 G = toVector(0, 0, g) # m/s2 # https://www.ngdc.noaa.gov/geomag-web/?model=igrf#igrfwmm #Lat 52.52, Lon 13.40, date 2017-04-13 EARTHMAGFIELD = toVector(18636.7, 1197.4 ,45940.6)/1000 #WMM DECANGLE = deg2rad(3. + 40./60. + 34./3600.) # Declination = 3° 40' 34"
def test_init_north(self):
acc = toVector(0., 0., -10.)
b = Euler(acc, EARTHMAGFIELD)
self.assertAlmostEqual(0., b.values[0], delta=0.01)
self.assertAlmostEqual(0., b.values[1], delta=0.01)
self.assertAlmostEqual(0., rad2deg((b.values[2])), delta=0.01)
def main():
s = Strapdown()
K = Kalman()
rot_mean = toVector(0., 0., 0.)
acc_mean = toVector(0., 0., 0.)
mag_mean = toVector(0., 0., 0.)
# read sensors
filePath = "data\\adafruit10DOF\\10min_calib_360.csv"
d = CSVImporter(filePath,
columns=range(0, 10),
skip_header=7,
hasTime=True)
accelArray, rotationArray, magneticArray = convArray2IMU(d.values)
# variable sample rate test
time = d.values[:, 0]
diff = ([x - time[i - 1] for i, x in enumerate(time)][1:])
diff.insert(0, 0.0134981505504)
# realtime 3D plot
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
plt.ion()
start = datetime.now()
phi_list = []
theta_list = []
psi_list = []
for i in range(1, int(47999)): #62247
dt = diff[i]
# for 10 - 15 minutes
if not s.isInitialized:
rot_mean = runningAverage(rot_mean, rotationArray[:, i], 1 / i)
acc_mean = runningAverage(acc_mean, accelArray[:, i], 1 / i)
mag_mean = runningAverage(mag_mean, magneticArray[:, i], 1 / i)
if i >= 45500: #100*60*7.5 :
s.Initialze(acc_mean, mag_mean)
gyroBias = rot_mean #toVector(0.019686476,-0.014544547,0.002910090)
print('STRAPDOWN INITIALIZED with %i values\n' % (i, ))
print('Initial bearing\n', s.getOrientation() * 180 / pi)
print('Initial velocity\n', s.getVelocity())
print('Initial position\n', s.getPosition())
print('Initial gyro Bias\n', gyroBias * 180 / pi)
else:
if i % 10 == 0: # plot area
plt.cla()
fig.texts = []
plot3DFrame(s, ax)
plt.draw()
plt.pause(0.01)
phi, theta, psi = rad2deg(s.getOrientation())
phi_list.append(phi)
theta_list.append(theta)
psi_list.append(psi)
acceleration = accelArray[:, i]
rotationRate = rotationArray[:, i] - gyroBias
magneticField = magneticArray[:, i]
s.quaternion.update(rotationRate, dt)
s.velocity.update(acceleration, s.quaternion, dt)
s.position.update(s.velocity, dt)
K.timeUpdate(s.quaternion, dt)
# if i%10 == 0:
# K.measurementUpdate(acceleration, magneticField, s.quaternion)
#
# bearingOld = s.getOrientation()
# errorQuat = Quaternion(K.bearingError)
# s.quaternion *= errorQuat
# #s.quaternion.update(K.bearingError/DT) #angle = rate*DT
# # print(s.quaternion.values)
# bearingNew = s.getOrientation()
# # print("Differenz zwischen neuer und alter Lage \n",rad2deg(bearingNew-bearingOld))
# gyroBias = gyroBias+K.gyroBias
# K.resetState()
print("Differenz zwischen x Bias End-Start: %E" %
rad2deg(gyroBias[0] - rot_mean[0]))
print("Differenz zwischen y Bias End-Start: %E" %
rad2deg(gyroBias[1] - rot_mean[1]))
print("Differenz zwischen z Bias End-Start: %E\n" %
rad2deg(gyroBias[2] - rot_mean[2]))
print("RMS_dphi = %f deg" % rad2deg(sqrt(K.P[0, 0])))
print("RMS_dthe = %f deg" % rad2deg(sqrt(K.P[1, 1])))
print("RMS_dpsi = %f deg" % rad2deg(sqrt(K.P[2, 2])))
print("RMS_bx = %f deg/s" % rad2deg(sqrt(K.P[3, 3])))
print("RMS_by = %f deg/s" % rad2deg(sqrt(K.P[4, 4])))
print("RMS_bz = %f deg/s\n" % rad2deg(sqrt(K.P[5, 5])))
print('final orientation\n', s.getOrientation() * 180 / pi)
end = datetime.now()
diff = end - start
print('Time needed = ', diff.total_seconds())
print("RMS_phi = %f deg, max = %f, min = %f\n" %
(std(phi_list), max(phi_list), min(phi_list)))
print("RMS_theta = %f deg, max = %f, min = %f\n" %
(std(theta_list), max(theta_list), min(theta_list)))
print("RMS_psi = %f deg, max = %f, min = %f\n" %
(std(psi_list), max(psi_list), min(psi_list)))
plt.show()
def convArray2err(array):
""" if data includes errors
"""
err = toVector(array[:, 8], array[:, 9], array[:, 10])
return err.T
from MathLib import toVector, toValue
from math import pi, sin, cos
import matplotlib.pyplot as plt
from matplotlib.pyplot import title, xlabel, ylabel
g = 9.81
phi = 0 * pi / 180
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")
def test_init(self):
pos = Position(toVector(100, 200, 300))
self.assertEqual(100, pos.values[0])
self.assertEqual(200, pos.values[1])
self.assertEqual(300, pos.values[2])
def convArray2PV(array):
pos = toVector(deg2rad(array[:, 1]), deg2rad(array[:, 2]),
array[:, 3]) # rad rad m
vel = toVector(array[:, 4], array[:, 6], array[:, 6]) # ms
return pos.T, vel.T
def test_init_Null_mag(self):
acc = toVector(1., 1., 1.)
mag = toVector(0., 0., 0.)
self.assertRaises(ValueError, Euler, acc, mag)
def __init__(self, vector=toVector(0., 0., 0.)):
""" initialized by a position-vector
units are given in m
"""
self.values = vector
def test_toVector_3D(self):
vector = toVector(1, 2, 3)
self.assertEqual(vector[0].item(), 1)
self.assertEqual(vector[1].item(), 2)
self.assertEqual(vector[2].item(), 3)
self.assertEqual(shape(vector), (3, 1))
def test_init_upsidedown(self):
acc = toVector(0., 0., 9.81)
mag = toVector(1., 1., 1.)
b = Euler(acc, mag)
self.assertEqual(pi, abs(b.values[0]))
self.assertEqual(0., abs(b.values[1]))
def test_init_plane(self):
acc = toVector(0., 0., -9.81)
mag = toVector(1., 1., 1.)
b = Euler(acc, mag)
self.assertEqual(0., abs(b.values[0]))
self.assertEqual(0., abs(b.values[1]))
def test_init(self):
vel = Velocity(toVector(3, 4, -6))
self.assertEqual(3, vel.values[0])
self.assertEqual(4, vel.values[1])
self.assertEqual(-6, vel.values[2])
def __init__(self, vector=toVector(0., 0., 0.)):
""" class Velocity propagates the velocity based on acceleration
initialized by velocity-vector
units in m/s
"""
self.values = vector
def convArray2IMU(array): # adafruit 10DOF
acceleration = toVector(array[:, 4], array[:, 5], array[:, 6]) * -9.80665
rotationRate = toVector(array[:, 1], array[:, 2], array[:, 3])
magneticField = toVector(array[:, 7], array[:, 8], array[:, 9])
return acceleration.T, rotationRate.T, magneticField.T
def main():
vec = toVector(1., 2., 0.)
plotVector(1, vec)
plt.show()
def convArray2euler(array):
""" if data includes euler angles
"""
pose = toVector((array[:, 10]), (array[:, 11]), (array[:, 12]))
return pose.T
def test_update_2(self):
p = EllipsoidPosition(toVector(deg2rad(51.),deg2rad(10.),0.))
vel = Velocity(toVector(10.,20.,0.)) #equal to 1 and 2 meters
p.update(vel,0.1)
self.assertAlmostEqual(51.+8.988903268e-06,rad2deg(p.values[0].item()), delta =10e-06)
self.assertAlmostEqual(10.+1.28368253e-05,rad2deg(p.values[1]), delta = 10e-06)
def main():
""" demo function for measurements saved as CSV file
"""
s = Strapdown()
K = KalmanPVO()
# kml = simplekml.Kml()
rot_mean = toVector(0.018461137, -0.01460140625,
0.002765854) # approximate values for gyro bias
accelBias = toVector(0., 0., 0.)
acc_mean = toVector(0., 0., 0.)
mag_mean = toVector(0., 0., 0.)
pos_mean = s.getPosition()
dt_mean = 0.01
# import IMU
filePath = "data\\adafruit10DOF\\linie_dach_imu.csv"
dIMU = CSVImporter(filePath,
columns=range(0, 13),
skip_header=7,
hasTime=True)
accelArray, rotationArray, magneticArray = convArray2IMU(dIMU.values)
tIMU, deltaArray = convArray2time(dIMU.values)
# import GPS
dGPS = CSVImporter("data\\UltimateGPS\\linie_dach_gps.csv",
skip_header=1,
columns=range(7))
posArray, velArray = convArray2PV(dGPS.values)
tGPS, _ = convArray2time(dGPS.values)
# PDOP = convArray2err(dGPS.values)
j = 1 # index for GPS measurements
for i in range(100, 58164): # index for IMU measurements
dt_mean = runningAverage(dt_mean, deltaArray[i], 1)
# Initialzation
if not s.isInitialized:
rot_mean = runningAverage(rot_mean, rotationArray[:, i],
1 / (i + 1))
acc_mean = runningAverage(acc_mean, accelArray[:, i], 1 / i)
mag_mean = runningAverage(mag_mean, magneticArray[:, i], 1 / i)
if tIMU[i] >= tGPS[j]:
pos_mean = runningAverage(pos_mean, posArray[:, j], 1 / j)
j += 1
if i >= 48133: # for 10 - 15 minutes
s.Initialze(acc_mean, mag_mean, pos_mean)
gyroBias = rot_mean
print(
'STRAPDOWN INITIALIZED with %i samples and %i position measurements\n'
% (i, j))
e = Euler()
e.values = s.getOrientation()
print('Initial orientation\n', e)
print('Initial velocity\n', s.velocity)
print('Initial position\n', s.position)
e.values = gyroBias
print('Initial gyro bias\n', e)
else:
###################################### plot area
if i % 10 == 0:
plt.figure(1)
# lat, lon, h = s.getPosition()
# plt.plot(i, h, 'ro')
# plt.plot(lon,lat, 'go')
# plotVector(i,s.getVelocity())
plotVector(i, rad2deg(rad2deg(s.getOrientation())))
# kml.newpoint(name=str(i), coords=[(rad2deg(lon), rad2deg(lat), h)])
######################################
acceleration = accelArray[:, i] - accelBias
rotationRate = rotationArray[:, i] - gyroBias
magneticField = magneticArray[:, i]
s.quaternion.update(rotationRate, dt_mean)
s.velocity.update(acceleration, s.quaternion, dt_mean)
s.position.update(s.velocity, dt_mean)
K.timeUpdate(acceleration, s.quaternion, dt_mean)
try:
gpsAvailable = tIMU[i] >= tGPS[j]
except: # end of GPS file reached
gpsAvailable = False
# if 55450 <= i <= 56450: # simulated GPS outage
# gpsAvailable = False
#
if gpsAvailable or i % 10 == 0: # doing either complete update or gyro error and bias update
if gpsAvailable:
pos = posArray[:, j]
vel = velArray[:, j]
j += 1
else:
pos = toVector(0., 0., 0.)
vel = toVector(0., 0., 0.)
K.measurementUpdate(s.quaternion, s.position, s.velocity, pos,
vel, acceleration, magneticField,
gpsAvailable)
errorQuat = Quaternion(K.oriError)
s.quaternion = errorQuat * s.quaternion
s.position.correct(K.posError)
s.velocity.correct(K.velError)
gyroBias = gyroBias + K.gyrError
accelBias = accelBias - K.accError
K.resetState()
e.values = s.getOrientation()
print('\nFinal orientation\n', e)
print('Final position\n', s.position)
print("Average sampling rate: {:3.1f}".format(1. / dt_mean))
# kml.save("export\\linie_dach.kml")
plt.show()
def convArray2IMU(array): #arduino 10DOF
acceleration = toVector(array[:, 4], array[:, 5], array[:, 6]) * -9.80665
rotationRate = toVector(array[:, 1], array[:, 2], array[:, 3]) #*pi/180
magneticField = toVector(array[:, 7], array[:, 8], array[:, 9]) * 1
return acceleration.transpose(), rotationRate.transpose(
), magneticField.transpose()
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 main():
E = Euler()
E.values = toVector(pi / 2, pi / 4, pi / 6)
print('{:rad}'.format(E))
print(E)
