У меня есть IMU, и я пытаюсь определить его ориентацию, используя только акселерометр и магнитометр. Я использую вектор гравитации, возвращенный из акселерометра, и вектор, возвращенный из магнитометра, и нахожу их перекрестные произведения, чтобы получить восточный и северный векторы. У меня есть следующий код:
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)
Когда я запускаю сценарий, я, кажется, получаю точные результаты, но в зависимости от того, как я вращаю IMU, две оси могут заблокироваться и их значения вращения будут то же. Я что-то упустил?