I have an IMU, and I am trying to obtain the orientation of it using just the accelerometer and magnetometer. I am using the gravity vector returned from the accelerometer and the vector returned from the magnetometer and finding their cross products to get the east and north vectors. I have the following code:
import time
import math
from pyquaternion import Quaternion
import numpy as np
import vectormath as vmath
# Read acceleration, magnetometer, and gyroscope
accel_x, accel_y, accel_z = sensor.acceleration
mag_x, mag_y, mag_z = sensor.magnetic
gyro_x, gyro_y, gyro_z = sensor.gyro
# Define vectors for each sensor reading
accelerometer = vmath.Vector3(accel_x, accel_y, accel_z)
gyroscope = vmath.Vector3(math.radians(gyro_x), math.radians(gyro_y), math.radians(gyro_z))
magnetometer = vmath.Vector3(mag_x, mag_y, mag_z)
# Normalize the magnetometer and accelerometer vectors
magnetometer = magnetometer.normalize()
accelerometer = accelerometer.normalize()
down = -accelerometer # Get down vector, which is opposite the accelerometer reading
east = down.cross(magnetometer) # Get vector pointing east (perpendicular to down vector and magnetometer vector)
east = east.normalize() # Normalize east vector
north = east.cross(down) # Get vector pointing north (perpendicular to east vector and down vector)
north = north.normalize() # Normalize north vector
# Define vectors for axes in world view
eastworld = vmath.Vector3(0, 1, 0)
northworld = vmath.Vector3(-1, 0, 0)
downworld = vmath.Vector3(0, 0, -1)
# Find the angle between each vector and its respective world axis vector
anglediffX = round(east.angle(eastworld, unit='deg'))
anglediffY = round(north.angle(northworld, unit='deg'))
anglediffZ = round(down.angle(downworld, unit='deg'))
print(anglediffX, anglediffY, anglediffZ)
When I run the script, I seem to get accurate results, but depending on how I rotate the IMU, two of the axes may lock up and have their rotation values be the same. Am I missing something?
