电子罗盘-航向角计算

这里写图片描述

坐标变换

  • 手机初始状态accelerometer 与 magnetometer 读数为
    (假定初始状态为水平放置,如上图所示)
    G1=ax1ay1az1=00g(1)(1)G1=[ax1ay1az1]=[00g]

    M1=mx1my1mz1=cos(δ)0sin(δ)(2)(2)M1=[mx1my1mz1]=[cos(δ)0sin(δ)]
  • 依上图坐标系,手机绕x,y,zx,y,z轴旋转的变换矩阵为
    Cx(θ)=1000cos(θ)sin(θ)0sin(θ)cos(θ)(3)(3)Cx(θ)=[1000cos(θ)−sin(θ)0sin(θ)cos(θ)]

    Cy(ϕ)=cos(ϕ)0sin(ϕ)010sin(ϕ)0cos(ϕ)(4)(4)Cy(ϕ)=[cos(ϕ)0sin(ϕ)010−sin(ϕ)0cos(ϕ)]

    Cz(ψ)=cos(ψ)sin(ψ)0sin(ψ)cos(ψ)0001(5)(5)Cz(ψ)=[cos(ψ)−sin(ψ)0sin(ψ)cos(ψ)0001]

姿态角计算

  • 下列计算均假设acclerometer读数基本稳定,即手机近似静止,可对加速度数据做低通滤波处理.
  • 旋转过后accelerometer与magnetometer读数为(基于机体坐标系,旋转次序 yaw->pitch ->roll)
    G2=ax2ay2az2=Cy(ϕ)Cx(θ)Cz(ψ)G1=Cy(ϕ)Cx(θ)Cz(ψ)00g(6)(6)G2=[ax2ay2az2]=Cy(ϕ)Cx(θ)Cz(ψ)G1=Cy(ϕ)Cx(θ)Cz(ψ)[00g]

    M2=mx2my2mz2=Cy(ϕ)Cx(θ)Cz(ψ)M1=Cy(ϕ)Cx(θ)Cz(ψ)cos(δ)0sin(δ)(7)(7)M2=[mx2my2mz2]=Cy(ϕ)Cx(θ)Cz(ψ)M1=Cy(ϕ)Cx(θ)Cz(ψ)[cos(δ)0sin(δ)]
  • 计算翻滚角ϕϕ
    由公式(6)可得,

    G2=ax2ay2az2=Cy(ϕ)Cx(θ)Cy(ψ)G1=Cy(ϕ)Cx(θ)Cy(ψ)00g=cos(ϕ)0sin(ϕ)010sin(ϕ)0cos(ϕ)1000cos(θ)sin(θ)0sin(θ)cos(θ)cos(ψ)sin(ψ)0sin(ψ)cos(ψ)000100g=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=gsin(ϕ)cos(θ)gsin(θ)gcos(ϕ)cos(θ)(365)(366)(367)(368)(8)(8)(365)G2=[ax2ay2az2]=Cy(ϕ)Cx(θ)Cy(ψ)G1=Cy(ϕ)Cx(θ)Cy(ψ)[00g](366)=[cos(ϕ)0sin(ϕ)010−sin(ϕ)0cos(ϕ)][1000cos(θ)−sin(θ)0sin(θ)cos(θ)][cos(ψ)−sin(ψ)0sin(ψ)cos(ψ)0001][00g](367)=[cos(ϕ)cos(ψ)+sin(ϕ)sin(θ)sin(ψ)sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)sin(ϕ)cos(θ)cos(θ)sin(ψ)cos(θ)cos(ψ)−sin(θ)cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)sin(ϕ)sin(ψ)+cos(ϕ)sin(θ)cos(ψ)cos(ϕ)cos(θ)][00g](368)=[g⋅sin(ϕ)cos(θ)−g⋅sin(θ)g⋅cos(ϕ)cos(θ)]


    由公式(8)可得,

    ax2az2=tan(ϕ)ax2az2=tan(ϕ)


    ϕ=tan(ax2az2)(9)(9)ϕ=tan(ax2az2)
  • 计算俯仰角θθ

    • 方法一:由公式(8)可得,
      ay2=gsin(θ)ay2=−g⋅sin(θ)


      θ=arcsin(ay2g)θ=−arcsin(ay2g)
      ,此时g=sqrt(a2x2+a2y2+a2z2)g=sqrt(ax22+ay22+az22).
    • 方法二:已求得ϕϕ条件下,同由公式(8)得,
      ay2ax2sin(ϕ)+az2cos(ϕ)=tan(θ)ay2ax2sin(ϕ)+az2cos(ϕ)=−tan(θ)


      θ=arctan(ay2ax2sin(ϕ)+az2cos(ϕ))(10)(10)θ=−arctan(ay2ax2sin(ϕ)+az2cos(ϕ))
    • 方法一与方法二求得结果一致,本文采用方法一(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)(11)(369)M2=[mx2my2mz2]=Cy(ϕ)Cx(θ)Cz(ψ)M1=Cy(ϕ)Cx(θ)Cz(ψ)[cos(δ)0sin(δ)](370)=[cos(ϕ)cos(ψ)+sin(ϕ)sin(θ)sin(ψ)sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)sin(ϕ)cos(θ)cos(θ)sin(ψ)cos(θ)cos(ψ)−sin(θ)cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)sin(ϕ)sin(ψ)+cos(ϕ)sin(θ)cos(ψ)cos(ϕ)cos(θ)][cos(δ)0sin(δ)](371)=[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(δ)]

    已知θθϕϕ条件下,由上式可得:
    mx2sin(ϕ)+mz2cos(ϕ)=sin(θ)sin(ψ)cos(δ)+cos(θ)sin(δ)mx2sin(ϕ)+mz2cos(ϕ)=sin(θ)sin(ψ)cos(δ)+cos(θ)sin(δ)

    [mx2sin(ϕ)+mz2cos(ϕ)]sin(θ)+my2cos(θ)= sin(ψ)cos(δ)[mx2sin(ϕ)+mz2cos(ϕ)]sin(θ)+my2cos(θ)= sin(ψ)cos(δ)

    mx2cos(ϕ)mz2sin(ϕ)=cos(ψ)cos(δ)mx2cos(ϕ)−mz2sin(ϕ)=cos(ψ)cos(δ)

    则:
    ψ=arctan(mx2sin(ϕ)sin(θ)+mz2cos(ϕ)sin(θ)+my2cos(θ)mx2cos(ϕ)mz2sin(ϕ))ψ=arctan(mx2sin(ϕ)sin(θ)+mz2cos(ϕ)sin(θ)+my2cos(θ)mx2cos(ϕ)−mz2sin(ϕ))

Android 应用

  • 依上述公式求取航向角为与x轴夹角,通过android api getOrientation()方法获取方位角为与y轴夹角.需做角度转换
  float yaw = (float) Math.toDegrees(ECompass.getYaw(accVals,magVals));
                //getOrientation获取方位角为与y轴夹角,本文计算方法求取方位角为与x轴夹角,角度需转换
                yaw -= 90;
                if(yaw<-180)
                    yaw += 360;
public class ECompass {

    private static float yaw;
    private static float pitch;
    private static float roll;
    //accVal,magVal为android phone 获取传感器原始数据
    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;
    }
}
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值