电子罗盘-航向角计算
坐标变换
- 手机初始状态accelerometer 与 magnetometer 读数为
(假定初始状态为水平放置,如上图所示)G1=⎡⎣⎢ax1ay1az1⎤⎦⎥=⎡⎣⎢00g⎤⎦⎥(1)(1)G1=[ax1ay1az1]=[00g]G_1 = \begin{bmatrix}a_{x1} \\a_{y1} \\a_{z1} \\\end{bmatrix} = \begin{bmatrix} 0 \\0 \\g\end{bmatrix} \tag{1}
M1=⎡⎣⎢mx1my1mz1⎤⎦⎥=⎡⎣⎢cos(δ)0sin(δ)⎤⎦⎥(2)(2)M1=[mx1my1mz1]=[cos(δ)0sin(δ)]M_1 = \begin{bmatrix}m_{x1} \\m_{y1} \\m_{z1}\end{bmatrix} = \begin{bmatrix} cos(\delta) \\0 \\sin(\delta)\end{bmatrix} \tag{2}
- 依上图坐标系,手机绕x,y,zx,y,zx,y,z轴旋转的变换矩阵为
Cx(θ)=⎡⎣⎢1000cos(θ)sin(θ)0−sin(θ)cos(θ)⎤⎦⎥(3)(3)Cx(θ)=[1000cos(θ)−sin(θ)0sin(θ)cos(θ)]
C_x(\theta) = \begin{bmatrix}1 & 0 & 0 \\0 & cos(\theta) & -sin(\theta) \\0 & sin(\theta) & cos(\theta) \\\end{bmatrix} \tag{3}
Cy(ϕ)=⎡⎣⎢cos(ϕ)0−sin(ϕ)010sin(ϕ)0cos(ϕ)⎤⎦⎥(4)(4)Cy(ϕ)=[cos(ϕ)0sin(ϕ)010−sin(ϕ)0cos(ϕ)]C_y(\phi) = \begin{bmatrix}cos(\phi) & 0 & sin(\phi) \\0 & 1 & 0 \\-sin(\phi) & 0 & cos(\phi) \\\end{bmatrix} \tag{4}
Cz(ψ)=⎡⎣⎢cos(ψ)sin(ψ)0−sin(ψ)cos(ψ)0001⎤⎦⎥(5)(5)Cz(ψ)=[cos(ψ)−sin(ψ)0sin(ψ)cos(ψ)0001]C_z(\psi) = \begin{bmatrix}cos(\psi) & -sin(\psi) & 0 \\sin(\psi) & cos(\psi) & 0 \\0 & 0 & 1 \\\end{bmatrix} \tag{5}
姿态角计算
- 下列计算均假设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]
G_2 = \begin{bmatrix}a_{x2} \\a_{y2} \\a_{z2} \\\end{bmatrix} =C_y(\phi) C_x(\theta) C_z(\psi)G_1= C_y(\phi) C_x(\theta) C_z(\psi) \begin{bmatrix} 0 \\0 \\g\end{bmatrix} \tag{6}
M2=⎡⎣⎢mx2my2mz2⎤⎦⎥=Cy(ϕ)Cx(θ)Cz(ψ)M1=Cy(ϕ)Cx(θ)Cz(ψ)⎡⎣⎢cos(δ)0sin(δ)⎤⎦⎥(7)(7)M2=[mx2my2mz2]=Cy(ϕ)Cx(θ)Cz(ψ)M1=Cy(ϕ)Cx(θ)Cz(ψ)[cos(δ)0sin(δ)]M_2 = \begin{bmatrix}m_{x2} \\m_{y2} \\m_{z2} \\\end{bmatrix} =C_y(\phi) C_x(\theta) C_z(\psi)M_1= C_y(\phi) C_x(\theta) C_z(\psi) \begin{bmatrix} cos(\delta) \\0 \\sin(\delta)\end{bmatrix} \tag{7}
计算翻滚角ϕϕ\phi
由公式(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)(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(θ)]\begin{align} G_2 &= \begin{bmatrix}a_{x2} \\a_{y2} \\a_{z2} \\\end{bmatrix} = C_y(\phi) C_x(\theta) C_y(\psi)G_1= C_y(\phi) C_x(\theta) C_y(\psi) \begin{bmatrix} 0 \\0 \\g\end{bmatrix} \\ &=\begin{bmatrix}cos(\phi) & 0 & sin(\phi) \\0 & 1 & 0 \\-sin(\phi) & 0 & cos(\phi) \\\end{bmatrix} \begin{bmatrix}1 & 0 & 0 \\0 & cos(\theta) & -sin(\theta) \\0 & sin(\theta) & cos(\theta) \\\end{bmatrix} \begin{bmatrix}cos(\psi) & -sin(\psi) & 0 \\sin(\psi) & cos(\psi) & 0 \\0 & 0 & 1 \\\end{bmatrix}\begin{bmatrix} 0 \\0 \\g\end{bmatrix} \\ &= \begin{bmatrix} cos(\phi)cos(\psi)+sin(\phi)sin(\theta)sin(\psi) & sin(\phi)sin(\theta)cos(\psi)-cos(\phi)sin(\psi) & sin(\phi)cos(\theta) \\cos(\theta)sin(\psi) & cos(\theta)cos(\psi) & -sin(\theta)\\cos(\phi)sin(\theta)sin(\psi)-sin(\phi)cos(\psi) & sin(\phi)sin(\psi)+cos(\phi)sin(\theta)cos(\psi) & cos(\phi)cos(\theta)\end{bmatrix}\begin{bmatrix} 0 \\0 \\g\end{bmatrix} \\&= \begin{bmatrix} g\cdot sin(\phi)cos(\theta) \\-g\cdot sin(\theta) \\g\cdot cos(\phi)cos(\theta)\end{bmatrix} \\\end{align} \tag{8}
由公式(8)可得,
ax2az2=tan(ϕ)ax2az2=tan(ϕ)\frac{a_{x2}}{a_{z2}}=tan(\phi)
则ϕ=tan(ax2az2)(9)(9)ϕ=tan(ax2az2)\phi= tan(\frac{a_{x2}}{a_{z2}}) \tag{9}
计算俯仰角θθ\theta
- 方法一:由公式(8)可得,
ay2=−g⋅sin(θ)ay2=−g⋅sin(θ)
a_{y2} =-g\cdot sin(\theta)
则θ=−arcsin(ay2g)θ=−arcsin(ay2g)\theta = -arcsin(\frac{a_{y2}}{g}),此时g=sqrt(a2x2+a2y2+a2z2)g=sqrt(ax22+ay22+az22)g=sqrt(a_{x2}^2+a_{y2}^2+a_{z2}^2).
- 方法二:已求得ϕϕ\phi条件下,同由公式(8)得,
ay2ax2sin(ϕ)+az2cos(ϕ)=−tan(θ)ay2ax2sin(ϕ)+az2cos(ϕ)=−tan(θ)
\frac{a_{y2}}{a_{x2}sin(\phi)+a_{z2}cos(\phi)}=-tan(\theta)
则θ=−arctan(ay2ax2sin(ϕ)+az2cos(ϕ))(10)(10)θ=−arctan(ay2ax2sin(ϕ)+az2cos(ϕ))\theta= -arctan(\frac{a_{y2}}{a_{x2}sin(\phi)+a_{z2}cos(\phi)}) \tag{10}
- 方法一与方法二求得结果一致,本文采用方法一(android getOrientation()方法用的方法二)
- 方法一:由公式(8)可得,
- 计算航向角ψψ\psi
由公式(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(δ)]\begin{align} M_2 &= \begin{bmatrix}m_{x2} \\m_{y2} \\m_{z2} \\\end{bmatrix} = C_y(\phi)C_x(\theta) C_z(\psi)M_1= C_y(\phi)C_x(\theta) C_z(\psi) \begin{bmatrix} cos(\delta) \\0 \\sin(\delta) \end{bmatrix} \\ &= \begin{bmatrix} cos(\phi)cos(\psi)+sin(\phi)sin(\theta)sin(\psi) & sin(\phi)sin(\theta)cos(\psi)-cos(\phi)sin(\psi) & sin(\phi)cos(\theta) \\cos(\theta)sin(\psi) & cos(\theta)cos(\psi) & -sin(\theta)\\cos(\phi)sin(\theta)sin(\psi)-sin(\phi)cos(\psi) & sin(\phi)sin(\psi)+cos(\phi)sin(\theta)cos(\psi) & cos(\phi)cos(\theta)\end{bmatrix}\begin{bmatrix} cos(\delta) \\0 \\sin(\delta)\end{bmatrix} \\&= \begin{bmatrix} cos(\phi)cos(\psi)cos(\delta)+sin(\phi)sin(\theta)sin(\psi)cos(\delta)+sin(\phi)cos(\theta)sin(\delta)\\cos(\theta)sin(\psi)cos(\delta)-sin(\theta)sin(\delta)\\cos(\phi)sin(\theta)sin(\psi)cos(\delta)-sin(\phi)cos(\psi)cos(\delta)+cos(\phi)cos(\theta)sin(\delta)\end{bmatrix} \\\end{align} \tag{11}
已知θθ\theta及ϕϕ\phi条件下,由上式可得:mx2sin(ϕ)+mz2cos(ϕ)=sin(θ)sin(ψ)cos(δ)+cos(θ)sin(δ)mx2sin(ϕ)+mz2cos(ϕ)=sin(θ)sin(ψ)cos(δ)+cos(θ)sin(δ)m_{x2}sin(\phi)+m_{z2}cos(\phi)=sin(\theta)sin(\psi)cos(\delta)+cos(\theta)sin(\delta)
[mx2sin(ϕ)+mz2cos(ϕ)]sin(θ)+my2cos(θ)= sin(ψ)cos(δ)[mx2sin(ϕ)+mz2cos(ϕ)]sin(θ)+my2cos(θ)=sin(ψ)cos(δ)[m_{x2}sin(\phi)+m_{z2}cos(\phi)]sin(\theta)+m_{y2}cos(\theta)=\sin(\psi)cos(\delta)
mx2cos(ϕ)−mz2sin(ϕ)=cos(ψ)cos(δ)mx2cos(ϕ)−mz2sin(ϕ)=cos(ψ)cos(δ)m_{x2}cos(\phi)-m_{z2}sin(\phi)=cos(\psi)cos(\delta)
则:ψ=arctan(mx2sin(ϕ)sin(θ)+mz2cos(ϕ)sin(θ)+my2cos(θ)mx2cos(ϕ)−mz2sin(ϕ))ψ=arctan(mx2sin(ϕ)sin(θ)+mz2cos(ϕ)sin(θ)+my2cos(θ)mx2cos(ϕ)−mz2sin(ϕ))\psi=arctan(\frac{m_{x2}sin(\phi)sin(\theta)+m_{z2}cos(\phi)sin(\theta)+m_{y2}cos(\theta)}{m_{x2}cos(\phi)-m_{z2}sin(\phi)})
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;}
}电子罗盘-航向角计算相关推荐
- python计算两经纬度坐标距离和角度以及给定第一个坐标、距离和航向角计算第二个坐标
总结三个坐标相关公式 因为需要用到三角函数,导入math库 import math 两个坐标计算距离的函数 def distance(Alongitude, Alatitude, Blongitude ...
- 磁力机航向角计算与补偿
地理坐标系下该点的磁场强度为(M,0,D),磁力计测得的三轴磁场强度为(mx,my,mz). 当我们认为飞机是完全水平放在地上的时候,即Z轴和Zb轴是平行的时候,完全不用考虑垂直方向了.我们只用考虑X ...
- 电子罗盘详解之地磁场和航向角
地球本身具有磁性,所以地球和近地空间之间存在着磁场,叫做地磁场.地磁场的强度为0. 3 至0. 6 高斯,其大小和方向随地点(甚至随时间) 而异.地球本身具有磁性,所以地球和近地空间之间存在着磁场,叫 ...
- 关于车路协同目标定位（含通过定位计算速度、加速度、航向角）
关于车路协同目标定位(含通过定位计算速度.加速度.航向角) 文章目录 关于车路协同目标定位(含通过定位计算速度.加速度.航向角) 1 视觉目标定位 1.1 方案原理 1.2 从算法端考虑方案测试方法 ...
- 激光感知（八）：计算目标最小凸多边形及航向角
目录 前言 一.点云凸包 格雷厄姆扫描算法流程 代码 二.车辆航向角 1. PCA算法 2. 凸多边形最小外接矩形算法 3. L-shape拟合算法 前言 自动驾驶激光感知模块,常常需要求得目标物体的 ...
- 计算航向角和经纬度之间的距离
#define PI 3.14159265358979323846 #define EARTH_RADIUS_L 6378137 #define EARTH_RADIUS_S 6356752.3142 ...
- 通过经纬度计算出航向角
通过两个经纬度点,算出航向.不墨迹,代码拿去就能用 import java.math.BigDecimal; //计算航向角 public class Bearing { /** * ...
- 作为无人机方面做嵌入式编写的飞控总结5-磁力计对航向角yaw的校准1（磁力计如何校正航向）
文档参考:上海丙寅电子有限公司 技术文档 导语:磁力计传感器取自于大地磁感应的强度hx,hy,hz: 1如何得到罗盘的方位角:怎么才能够从简单的 3 轴数据得到罗盘的方位角 1)当 3 轴磁力计工作时 ...
- 如何给定两个gps坐标 算出航向角_如何获得飞机的小扰动模型
一.飞机运动方程分析 二.线性化的基本步骤 三.纵向运动线性小扰动模型 四.横航向运动线性小扰动模型 五.参考文献 一.飞机运动方程分析 要描述一个飞机的飞行状态,我们通常需要9个状态变量,即: 即飞 ...
最新文章
- C语言单链表求环，并返回环的起始节点
- 前端学习（284）：纯css实现翻书效果
- c语言参数buf,C语言---变长参数列表---变长参数的传递
- java pg数据库事务回滚_PG 事务提交与回滚
- LOJ#3083. 「GXOI / GZOI2019」与或和（单调栈）
- BT4中文版(集成spoonwep2/spoonwpa)下载地址
- excel拆分单元格内容_Excel中最神奇的一个快捷键！牛！！
- 敬业签：备忘录在手机哪里能找到？
- Review of 2012 Goal for 2013
- Python2 简介
- 谷牛期权持仓重心上移
- 如何用java打印1-100_在java中使用10个线程打印1到100
- scanf(),printf()以及fscanf(),fprintf()的返回值
- java 刻度尺,jQuery实现腾讯信用界面(自制刻度尺)样式
- Win10 UWP应用发布流程
- python | requests库基本应用
- 顺序表和单链表的插入删除操作时间复杂度的区别
- FLUENT中文教程
- Hive函数（内置函数（字符串函数，数学函数，日期函数，集合函数，条件函数，聚合函数，表生成函数）和自定义函数（自定义函数创建流程，临时函数，永久函数）））（四）
- 计算机图形学【GAMES-101】5、几何(距离函数SDF、点云、贝塞尔曲线、曲面细分、曲面简化)