trigonometry - 如何使用陀螺仪和磁力计计算航向

我假设您正在使用手机收集gyroscope+magnetometer数据。

除非您知道设备的初始状态，否则无法使用Gyro+Magnetometer计算标题。你必须尝试Accelerometer+Magnetometer。您可以从Android / IOS设备的Motion传感器API中获取rotation-matrix' or yaw`组件的标题值。

如果您想实施自己的尝试倾斜补偿算法。您所要做的就是使用accelerometer和magnetometer实现倾斜补偿的ecompass。您在设备框架中获得accelerometer和magnetometer个读数。 Accelerometer和Magnetometer将读数作为设备旋转的函数。此旋转包含设备的Roll，Pitch和Heading信息。以下是使用accelerometer+magnetometer进行航向估算的解释和推导。

Tilt compensation algorithm

让accelerometer和magnetometer给你

$\begin{bmatrix} G_x\\ G_y\\ G_z \end{bmatrix} and \begin{bmatrix} H_x\\ H_y\\ H_z \end{bmatrix}$

分别是读数。它们位于设备框架中，设备通过

旋转

$\begin{bmatrix} \theta (roll) & \phi (pitch) & \psi (yaw) \end{bmatrix}$

$\psi (yaw)$ 正在寻找的地方。所以设备的旋转矩阵将是

$R = R_y(\theta)*R_x(\phi)*R_z(\psi)$

$\begin{bmatrix} G_x\\ G_y\\ G_z \end{bmatrix} = R_y(\theta)*R_x(\phi)*R_z(\psi)*\begin{bmatrix} 0\\ 0\\ g \end{bmatrix}$

$\begin{bmatrix} H_x\\ H_y\\ H_z \end{bmatrix} = R_y(\theta)*R_x(\phi)*R_z(\psi)*\begin{bmatrix} Bcos\delta\\ 0\\ Bsin\delta \end{bmatrix}$

我们必须解决 $\psi$ 。所以一步一步解决 $\psi$ 。

$R_x(-\phi)*R_y(-\theta)*\begin{bmatrix} G_x\\ G_y\\ G_z \end{bmatrix} = R_z(\psi)*\begin{bmatrix} 0\\ 0\\ g \end{bmatrix}$

从上面，等于零，我们得到

$tan\theta = (-G_x/G_z)\\ tan\phi = G_y/(-G_x*sin\theta + G_z*cos\theta)$

上面有 $\theta, \phi$

$resB = R_x(-\phi)*R_y(-\theta)*\begin{bmatrix} B_x\\ B_y\\ B_z \end{bmatrix}$

$tan\psi = ResB_x/ResB_y$

这是从accelerometer+magentomete获取标题值的基本方法。偏航/航向值由移动电话中的运动传感器API提供。

在实施sensor-fusion算法时，您必须查看一些内容。 Accelerometers并不总是重力方向，它给出了传感器所经历的超长加速度的方向，例如线性加速度下的设备。由于附近的铁磁性物质，Magnetometer读数会受到失真。准确度也取决于gyroscope传感器中的系统偏差分量。

有许多算法可用于估算准确的航向值。其中一些是

Madgwick AHRS
补充过滤器
卡尔曼滤波器
Compensation of Magnetic Disturbances，此算法在所有方面都取得了最佳效果。

如何使用陀螺仪和磁力计计算航向

1 个答案: