Android:SensorManager.getRotationMatrix 和 SensorManager.getOrientation() 的算法 [英] Android: Algorithms for SensorManager.getRotationMatrix and SensorManager.getOrientation()
问题描述
要在 Android 中从欧拉角(例如俯仰、滚转、方位角)获取方向,需要执行以下操作:
To get orientation as a from of euler angles (e.g., pitch, roll, azimuth) in Android, it is required to execute followings:
- SensorManager.getRotationMatrix(float[] R, float[] I, float[] Gravity, float[] geomagnetic);
- SensorManager.getOrientation(float[] R, float[]orientation);
在第一个中,我意识到它使用了一种 TRIAD 算法;旋转矩阵(R[])由重力、地磁X重力、重力X(地磁X重力)---X为叉积组成.
见下面的代码:
In the first one, I realize that it uses a kind of TRIAD algorithms; Rotation matrix (R[]) is composed of gravity, geomagnetic X gravity, gravity X (geomagnetic X gravity) --- X is cross product.
See codes below:
float Ax = gravity[0];
float Ay = gravity[1];
float Az = gravity[2];
final float Ex = geomagnetic[0];
final float Ey = geomagnetic[1];
final float Ez = geomagnetic[2];
float Hx = Ey*Az - Ez*Ay;
float Hy = Ez*Ax - Ex*Az;
float Hz = Ex*Ay - Ey*Ax;
final float normH = (float)Math.sqrt(Hx*Hx + Hy*Hy + Hz*Hz);
if (normH < 0.1f) {
// device is close to free fall (or in space?), or close to
// magnetic north pole. Typical values are > 100.
return false;
}
final float invH = 1.0f / normH;
Hx *= invH;
Hy *= invH;
Hz *= invH;
final float invA = 1.0f / (float)Math.sqrt(Ax*Ax + Ay*Ay + Az*Az);
Ax *= invA;
Ay *= invA;
Az *= invA;
final float Mx = Ay*Hz - Az*Hy;
final float My = Az*Hx - Ax*Hz;
final float Mz = Ax*Hy - Ay*Hx;
if (R != null) {
if (R.length == 9) {
R[0] = Hx; R[1] = Hy; R[2] = Hz;
R[3] = Mx; R[4] = My; R[5] = Mz;
R[6] = Ax; R[7] = Ay; R[8] = Az;
} else if (R.length == 16) {
R[0] = Hx; R[1] = Hy; R[2] = Hz; R[3] = 0;
R[4] = Mx; R[5] = My; R[6] = Mz; R[7] = 0;
R[8] = Ax; R[9] = Ay; R[10] = Az; R[11] = 0;
R[12] = 0; R[13] = 0; R[14] = 0; R[15] = 1;
}
}
但是,我无法理解 SensorManager.getOrientation().
However, I cannot understand SensorManager.getOrientation().
azimuth = (float)Math.atan2(R[1], R[4]);
pitch = (float)Math.asin(-R[7]);
roll = (float)Math.atan2(-R[6], R[8]);
获得欧拉角的确切算法是什么?
What is the exact algorithms to get euler angles?
推荐答案
让我试着解释一下:getRotationMatrix 根据重力和磁矢量组成旋转矩阵.
Let me try to explain: getRotationMatrix compose rotation matrix on the basis of gravity and megnetic vector.
我们的主要目标是构建NED框架
Our main goal here is to construct NED frame
我们假设重力指向地心,磁体指向北极.但在实际情况下,这些向量是非垂直的,这就是为什么我们首先计算与 E 和 A 正交并属于切线平面的向量 H.H 是叉积 (E x A),与 E 和 A 正交.
We assume that gravity points toward the center of the Earth and magnet to the north Pole. But in real cases these vectors are non-perpendicular, that's why we firstly calculate vector H that is orthogonal to E and A and belong to tangential plane. H is a cross-product (E x A) and is orthogonal to E and A.
float Hx = Ey*Az - Ez*Ay;
float Hy = Ez*Ax - Ex*Az;
float Hz = Ex*Ay - Ey*Ax;
final float normH = (float)Math.sqrt(Hx*Hx + Hy*Hy + Hz*Hz);
标准化加速度和H向量(因为这些向量将构成ENU坐标系的基础)
normalize acceleration and H vector (because these vectors will compose a basis of ENU coordinate system)
final float invH = 1.0f / normH;
Hx *= invH;
Hy *= invH;
Hz *= invH;
final float invA = 1.0f / (float)Math.sqrt(Ax*Ax + Ay*Ay + Az*Az);
Ax *= invA;
Ay *= invA;
Az *= invA;
找到最后一个基向量 (M) 作为 H 和 A 的叉积:
Find last basis vector (M) as cross-product of H and A:
double Mx = Ay * Hz - Az * Hy;
double My = Az * Hx - Ax * Hz;
double Mz = Ax * Hy - Ay * Hx;
体坐标系中任意向量 (a) 的坐标通过 NED 坐标表示为 a = Ra'R - 变换矩阵矩阵,其列是旧基中新基向量的坐标
Coordinates of the arbitrary vector (a) in body frame expresses through NED coordinates as a = Ra' R - Transformation matrix matrix, whose columns are the coordinates of the new basis vectors in the old basis
但是NED框架中的坐标是计算为 a' = T^(-1) * a.对于正交变换矩阵逆等于转置矩阵.因此我们有:
But coordinates in NED frame are calculated as a' = T^(-1) * a. For orthogonal transformation matrix inverse is equal to transposed matrix. Thus we have:
R[0] = Hx; R[1] = Hy; R[2] = Hz;
R[3] = Mx; R[4] = My; R[5] = Mz;
R[6] = Ax; R[7] = Ay; R[8] = Az;
一旦我们有了旋转矩阵,我们就可以将其转换为欧拉角表示.转换公式取决于您使用的约定.你的公式
Once we have rotation matrix we can convert it to Euler angles representation. Formulas of convertion depend from convention that you use. Your formulas
azimuth = (float)Math.atan2(R[1], R[4]);
pitch = (float)Math.asin(-R[7]);
roll = (float)Math.atan2(-R[6], R[8]);
对于具有约定 Y-X-Z 的 Tiat Bryan 角是正确的.为了更好地理解从旋转矩阵到欧拉角的转换,我建议研究 Gregory G.Slabaugh - 从旋转矩阵计算欧拉角"
are correct for Tiat Bryan angles with convention Y-X-Z. In order to better understand conversion from rotation matrix to Euler angles I would suggest to study an article of Gregory G. Slabaugh - "Computing Euler angles from a rotation matrix"
这篇关于Android:SensorManager.getRotationMatrix 和 SensorManager.getOrientation() 的算法的文章就介绍到这了,希望我们推荐的答案对大家有所帮助,也希望大家多多支持IT屋!