class Strapdown(object): """ class Strapdown generates bearing, velocity and position from 9 degree IMU """ def __init__(self): """ creates Strapdown object which includes the current bearing, velocity and position bearing (phi, theta, psi) in degrees, velocity (ax, ay, az) in m/s, position (x,y,z) in m """ self.quaternion = Quaternion() self.velocity = Velocity() self.position = EllipsoidPosition( ) # navigation system position with Position() self.isInitialized = False def Initialze(self, acceleration, magneticField, position): """ calculates attitutde and heading sets position to given position vector """ self.quaternion = Quaternion(Euler(acceleration, magneticField).values) self.position.values = position self.isInitialized = True def getOrientation(self): return self.quaternion.getEulerAngles( ) # vector of euler angles in rad def getVelocity(self): return self.velocity.values def getPosition(self): return self.position.values
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)
class Strapdown(object): def __init__(self): """ creates Strapdown object which includes the current bearing, velocity and position bearing (phi, theta, psi) in degrees, velocity (ax, ay, az) in m/s, position (x,y,z) in m """ self.quaternion = Quaternion() self.velocity = Velocity() # vector (if known) self.position = Position() # or GNSS.getPos() # toVector(52.521918/180*pi, 13.413215\180*pi, 100.) self.isInitialized = False def Initialze(self, acceleration, magneticField): self.quaternion = Quaternion(Euler(acceleration, magneticField).values) self.isInitialized = True #self.position = (toVector(n,e,d)) if GNSS measurement is icluded def getOrientation(self): return self.quaternion.getEulerAngles() def getVelocity(self): return self.velocity.values def getPosition(self): return self.position.values