坐标变换
- 手机初始状态accelerometer 与 magnetometer 读数为
(假定初始状态为水平放置,如上图所示)
G1=⎡⎣⎢ax1ay1az1⎤⎦⎥=⎡⎣⎢00g⎤⎦⎥(1)
M1=⎡⎣⎢mx1my1mz1⎤⎦⎥=⎡⎣⎢cos(δ)0sin(δ)⎤⎦⎥(2)
- 依上图坐标系,手机绕
x,y,z
轴旋转的变换矩阵为
Cx(θ)=⎡⎣⎢1000cos(θ)sin(θ)0−sin(θ)cos(θ)⎤⎦⎥(3)
Cy(ϕ)=⎡⎣⎢cos(ϕ)0−sin(ϕ)010sin(ϕ)0cos(ϕ)⎤⎦⎥(4)
Cz(ψ)=⎡⎣⎢cos(ψ)sin(ψ)0−sin(ψ)cos(ψ)0001⎤⎦⎥(5)
姿态角计算
- 下列计算均假设acclerometer读数基本稳定,即手机近似静止,可对加速度数据做低通滤波处理.
- 旋转过后accelerometer与magnetometer读数为(基于机体坐标系,旋转次序 yaw->pitch ->roll)
G2=⎡⎣⎢ax2ay2az2⎤⎦⎥=Cy(ϕ)Cx(θ)Cz(ψ)G1=Cy(ϕ)Cx(θ)Cz(ψ)⎡⎣⎢00g⎤⎦⎥(6)
M2=⎡⎣⎢mx2my2mz2⎤⎦⎥=Cy(ϕ)Cx(θ)Cz(ψ)M1=Cy(ϕ)Cx(θ)Cz(ψ)⎡⎣⎢cos(δ)0sin(δ)⎤⎦⎥(7)
计算翻滚角
ϕ
由公式(6)可得,
G2=⎡⎣⎢ax2ay2az2⎤⎦⎥=Cy(ϕ)Cx(θ)Cy(ψ)G1=Cy(ϕ)Cx(θ)Cy(ψ)⎡⎣⎢00g⎤⎦⎥=⎡⎣⎢cos(ϕ)0−sin(ϕ)010sin(ϕ)0cos(ϕ)⎤⎦⎥⎡⎣⎢1000cos(θ)sin(θ)0−sin(θ)cos(θ)⎤⎦⎥⎡⎣⎢cos(ψ)sin(ψ)0−sin(ψ)cos(ψ)0001⎤⎦⎥⎡⎣⎢00g⎤⎦⎥=⎡⎣⎢cos(ϕ)cos(ψ)+sin(ϕ)sin(θ)sin(ψ)cos(θ)sin(ψ)cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)cos(θ)cos(ψ)sin(ϕ)sin(ψ)+cos(ϕ)sin(θ)cos(ψ)sin(ϕ)cos(θ)−sin(θ)cos(ϕ)cos(θ)⎤⎦⎥⎡⎣⎢00g⎤⎦⎥=⎡⎣⎢g⋅sin(ϕ)cos(θ)−g⋅sin(θ)g⋅cos(ϕ)cos(θ)⎤⎦⎥(365)(366)(367)(368)(8)
由公式(8)可得,
ax2az2=tan(ϕ)
则
ϕ=tan(ax2az2)(9)
计算俯仰角
θ
- 方法一:由公式(8)可得,
ay2=−g⋅sin(θ)
则
θ=−arcsin(ay2g)
,此时
g=sqrt(a2x2+a2y2+a2z2)
.
- 方法二:已求得
ϕ
条件下,同由公式(8)得,
ay2ax2sin(ϕ)+az2cos(ϕ)=−tan(θ)
则
θ=−arctan(ay2ax2sin(ϕ)+az2cos(ϕ))(10)
- 方法一与方法二求得结果一致,本文采用方法一(android getOrientation()方法用的方法二)
- 计算航向角
ψ
由公式(7)可得,
M2=⎡⎣⎢mx2my2mz2⎤⎦⎥=Cy(ϕ)Cx(θ)Cz(ψ)M1=Cy(ϕ)Cx(θ)Cz(ψ)⎡⎣⎢cos(δ)0sin(δ)⎤⎦⎥=⎡⎣⎢cos(ϕ)cos(ψ)+sin(ϕ)sin(θ)sin(ψ)cos(θ)sin(ψ)cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)cos(θ)cos(ψ)sin(ϕ)sin(ψ)+cos(ϕ)sin(θ)cos(ψ)sin(ϕ)cos(θ)−sin(θ)cos(ϕ)cos(θ)⎤⎦⎥⎡⎣⎢cos(δ)0sin(δ)⎤⎦⎥=⎡⎣⎢cos(ϕ)cos(ψ)cos(δ)+sin(ϕ)sin(θ)sin(ψ)cos(δ)+sin(ϕ)cos(θ)sin(δ)cos(θ)sin(ψ)cos(δ)−sin(θ)sin(δ)cos(ϕ)sin(θ)sin(ψ)cos(δ)−sin(ϕ)cos(ψ)cos(δ)+cos(ϕ)cos(θ)sin(δ)⎤⎦⎥(369)(370)(371)(11)
已知
θ
及
ϕ
条件下,由上式可得:
mx2sin(ϕ)+mz2cos(ϕ)=sin(θ)sin(ψ)cos(δ)+cos(θ)sin(δ)
[mx2sin(ϕ)+mz2cos(ϕ)]sin(θ)+my2cos(θ)= sin(ψ)cos(δ)
mx2cos(ϕ)−mz2sin(ϕ)=cos(ψ)cos(δ)
则:
ψ=arctan(mx2sin(ϕ)sin(θ)+mz2cos(ϕ)sin(θ)+my2cos(θ)mx2cos(ϕ)−mz2sin(ϕ))
Android 应用
- 依上述公式求取航向角为与x轴夹角,通过android api getOrientation()方法获取方位角为与y轴夹角.需做角度转换
float yaw = (float) Math.toDegrees(ECompass.getYaw(accVals,magVals));
yaw -= 90;
if(yaw<-180)
yaw += 360;
public class ECompass {
private static float yaw;
private static float pitch;
private static float roll;
public static float getYaw(float[] accVals, float[] magVals) {
roll = (float)Math.atan2(accVals[0],accVals[2]);
pitch = -(float)Math.atan(accVals[1]/(accVals[0]*Math.sin(roll)+accVals[2]*Math.cos(roll)));
yaw = (float)Math.atan2(magVals[0]*Math.sin(roll)*Math.sin(pitch)+magVals[2]*Math.cos(roll)*Math.sin(pitch)+magVals[1]*Math.cos(pitch),
magVals[0]*Math.cos(roll)-magVals[2]*Math.sin(roll));
return yaw;
}
}