电子罗盘

磁强计和加速度计读数的方向

全页折叠

句法

取向= ecompass (accelerometerReading magnetometerReading)

取向=电子罗盘（accelerometerReading，magnetometerReading，orientationFormat）

取向=电子罗盘（accelerometerReading，magnetometerReading，orientationFormat， 'ReferenceFrame'，RF）

描述

例

取向= ecompass (accelerometerReading，magnetometerReading）回报四元可以将数量从父(NED)帧旋转到子(sensor)帧。

例

取向= ecompass (accelerometerReading，magnetometerReading，orientationFormat）将方向格式指定为四元或旋转矩阵。

取向= ecompass (accelerometerReading，magnetometerReading，orientationFormat，'参考范围'，RF）还允许您指定参考帧射频的取向输出。指定射频如'NED'（北 - 东 - 下）或'ENU'(East-North-Up)。默认值为'NED'。

例子

全部收缩

确定波士顿的赤纬

打开生活的脚本

使用已知的磁场强度，并在波士顿设备尖真北的固有加速度，以确定波士顿的磁偏角。

定义波士顿已知的加速度和磁场强度。

磁场强度= [19.535 -5.109 47.930];属性加速度= [0 0 9.8];

通过磁场强度和加速到电子罗盘功能。该电子罗盘函数返回一个四元数旋转操作。转换的四元数欧拉角度。

Q =电子罗盘（properAcceleration，magneticFieldStrength）;E = eulerd（Q，'ZYX'，'帧');

角度，Ë代表在波士顿真北和磁北之间的角度。按照惯例，磁偏角为负时，磁北以西正北。否定角度来确定磁偏角。

magneticDeclinationOfBoston = -e（1）

magneticDeclinationOfBoston = -14.6563

返回旋转矩阵

打开生活的脚本

该电子罗盘函数融合磁力仪和加速度计的数据，以返回一个四元数，当在一个四元数旋转操作符中使用时，可以将数量从父(NED)帧旋转到子帧。该电子罗盘函数还可以返回执行与四元数操作符相同的旋转的旋转矩阵。

定义一个旋转，可以将指向磁北的父帧旋转到指向地理北的子帧。将旋转定义为四元数和旋转矩阵。然后，将四元数和旋转矩阵转换为欧拉角(以度为单位)进行比较。

定义在马萨诸塞州的波士顿，当指着正北微特斯拉的磁场强度。

m = [19.535 -5.109 47.930];a = [0 0 9.8];

确定四元数和旋转矩阵能够从磁北框架旋转到正北。显示用于比较的结果。

Q =电子罗盘（A，M）;quaterionEulerAngles = eulerd（Q，'ZYX'，'帧'）

quaterionEulerAngles =1×314.6563 0 0

r = ecompass (a, m,“rotmat”);θ=最佳(r (1、3));ψ=量化(r (2、3) / cos(θ)、r (3,3) / cos(θ));ρ=量化(r (1、2) / cos(θ)、r (1,1) / cos(θ));rotmatEulerAngles = rad2deg((ρ,θ,psi))

rotmatEulerAngles =1×314.6563 0 0

确定引力向量

打开生活的脚本

使用电子罗盘基于来自旋转IMU数据，以确定重力矢量。

加载惯性测量单元（IMU）的数据。

加载“rpy_9axis.mat”sensorDataFS

随着时间的推移，确定传感器体相对于本地NED帧的方向。

取向=电子罗盘（sensorData.Acceleration，sensorData.MagneticField）;

要估计重力矢量，首先使用取向四元数向量。

gravityVectors = rotatepoint(定位、sensorData.Acceleration);

确定随时间的重力矢量作为平均回收的重力向量。

gravityVectorEstimate =平均（gravityVectors，1）

gravityVectorEstimate =1×30.0000 -0.0000 10.2102

跟踪纺纱平台

打开生活的脚本

保险丝模拟加速度计和陀螺仪数据跟踪同时使用理想化和现实数据的旋转平台。

生成地面实况轨迹

描述平台随时间变化的基本真实方向。使用kinematicTrajectory系统对象™为一个没有平移和旋转的平台创建一个轨迹ž设在。

持续时间= 12;FS = 100;NUMSAMPLES = FS *持续时间;accelerationBody =零（NUMSAMPLES，3）;angularVelocityBody =零（NUMSAMPLES，3）;zAxisAngularVelocity = [linspace（0,4 * PI，4个* FS），4 * PI *酮（1,4 * FS），linspace（4 * PI，0,4 * FS）]';angularVelocityBody（：，3）= zAxisAngularVelocity;轨迹= kinematicTrajectory（“SampleRate”fs);[取向,~ ~,加速度,angularVelocityNED] =轨迹(accelerationBody angularVelocityBody);

模型接受的IMU数据

使用一个imuSensor系统对象，用于模拟从包含理想磁强计和理想加速度计的IMU接收的数据。

IMU = imuSensor (“加速 -  MAG”，“SampleRate”fs);[accelerometerData, magnetometerData] = IMU(加速度,…angularVelocityNED,…orientationNED）;

保险丝IMU数据来估计方向

将加速度计数据和磁强计数据传递给电子罗盘函数估计方向随时间的变化。将方向转换为欧拉角度并绘制结果。

取向= ecompass (accelerometerData magnetometerData);orientationEuler = eulerd（取向，'ZYX'，'帧');timeVector =（0：NUMSAMPLES-1）'/ FS。图（之一）图（timeVector，orientationEuler）图例（“z轴”，“轴”，“x轴”)包含('时间（s）')ylabel (“旋转（度）”）标题（“从理想IMU方向”）

用真实的IMU传感器模型重复实验

的修改参数IMU系统对象到近似现实IMU传感器数据。重置IMU然后用相同的地面真实加速度，角速度和方向来调用它。使用电子罗盘融合IMU数据并绘制结果。

IMU.Accelerometer = accelparams（…'测量范围',20岁,…'解析度'，0.0006，…'ConstantBias',0.5,…'AxesMisalignment'，2，…“NoiseDensity”,0.004,…'BiasInstability'，0.5）;IMU.Magnetometer = magparams（…'测量范围'，200，…'解析度'，0.01）;复位（IMU）[accelerometerData，magnetometerData] = IMU（accelerationNED，angularVelocityNED，orientationNED）;取向= ecompass (accelerometerData magnetometerData);orientationEuler = eulerd（取向，'ZYX'，'帧');图(2)情节(timeVector orientationEuler)传说(“z轴”，“轴”，“x轴”)包含('时间（s）')ylabel (“旋转（度）”）标题（“现实IMU的定位”）

输入参数

全部收缩

`accelerometerReading`-传感器体坐标系统中的加速度计读数(m/s²）
ñ-by-3矩阵

加速度计传感器主体读数坐标系以m / s²，指定为ñ-by-3矩阵。矩阵的列对应于X- - - - - -,ÿ- 和ž-axes传感器主体的。在矩阵的行，ñ，对应于各个样本。加速度计读数功能使用前归。

数据类型:单|双

`magnetometerReading`-磁强计读数在身体传感器坐标系(µT)
ñ-by-3矩阵

在传感器主体磁力计读数坐标系中μT，指定为ñ-by-3矩阵。矩阵的列对应于X- - - - - -,ÿ- 和ž-axes传感器主体的。在矩阵的行，ñ，对应于各个样本。磁力计读数在功能使用前归。

数据类型:单|双

`orientationFormat`-格式用来描述取向
`“四元”`(默认)|`“rotmat”`

用于描述方向的格式，指定为“四元”或“rotmat”。

数据类型:烧焦|字符串

输出参数

全部收缩

`取向`-将数量从全局坐标系旋转到传感器体坐标系的定位
ñ×1四元数（默认值）的向量|3×3逐ñ排列

取向可以从全局坐标系旋转量到主体坐标系中，返回作为四元或阵列的矢量。的尺寸和类型取向取决于用来描述方位的格式为：

“四元”- - -ñ用相同的基础数据类型作为输入四元数×1矢量
“rotmat”- 3×3逐ñ数组的数据类型与输入相同

数据类型:四元|单|双

算法

该电子罗盘函数返回一个四元数或旋转矩阵，该矩阵可以将数量从父(例如NED)帧旋转到子(传感器)帧。对于这两种输出方向格式，旋转操作符是通过计算旋转矩阵来确定的。

旋转矩阵首先通过中介计算:

$[R = [\begin{matrix} （一种 \times 米） \times 一种 & 一种 \times 米 & 一种 \end{matrix}]$

然后按列标准化。一种和米是accelerometerReading输入和magnetometerReading分别输入。

为了理解旋转矩阵的计算，考虑地球上任意一点及其对应的局部NED坐标系。设传感器体框架为，[X，ÿ，ž]，具有相同的起源。

回想一下，传感器主体的方位被定义为旋转算（旋转矩阵或四元数），以从父（NED）帧的量旋转到子（传感器本体）帧必需的：

$[\begin{matrix} [R \end{matrix}] [\begin{matrix} p_{亲} \end{matrix}] = [\begin{matrix} p_{儿童} \end{matrix}]$

在哪里

[R是一个3×3的旋转矩阵，可以理解为子帧的方向。
p_亲是一个3×1矢量在父帧。
p_儿童是一个3×1向量在子帧。

对于一个稳定的传感器主体，加速计返回由于重力的加速度。如果传感器主体被完美地与NED坐标系对齐，所有重力加速度是沿ž轴，和加速计读取[0 0 1]。考虑到旋转从NED的量的坐标系的数量由所述加速度计所指示所需的旋转矩阵。

$[\begin{matrix} {[R}_{11} & {[R}_{21} & {[R}_{31} \\ {[R}_{12} & {[R}_{22} & {[R}_{32} \\ {[R}_{13} & {[R}_{23} & {[R}_{33} \end{matrix}] [\begin{matrix} 0 \\ 0 \\ 1 \end{matrix}] = [\begin{matrix} {一种}_{1} \\ {一种}_{2} \\ {一种}_{3} \end{matrix}]$

旋转矩阵对应于加速计读数的第三列：

$[\begin{matrix} {[R}_{31} \\ {[R}_{32} \\ {[R}_{33} \end{matrix}] = [\begin{matrix} {一种}_{1} \\ {一种}_{2} \\ {一种}_{3} \end{matrix}]$

磁力计读数朝磁北极点，是在ñ-d飞机。同样，考虑一个与NED坐标系对齐的传感器体框架。

根据定义，Ë轴垂直于所述ñ-d平面上,因此ñ⨯d=Ë，一些幅度缩放范围内。如果传感器主体框架与NED，既来自加速度计的加速度矢量和从磁力计横亘在磁场矢量对准ñ-d飞机。因此米⨯一种=ÿ，再次与一些幅度缩放。

考虑需要旋转NED给孩子框架旋转矩阵，Xÿž]。

$[\begin{matrix} {[R}_{11} & {[R}_{21} & {[R}_{31} \\ {[R}_{12} & {[R}_{22} & {[R}_{32} \\ {[R}_{13} & {[R}_{23} & {[R}_{33} \end{matrix}] [\begin{matrix} 0 \\ 1 \\ 0 \end{matrix}] = [\begin{matrix} {一种}_{1} \\ {一种}_{2} \\ {一种}_{3} \end{matrix}] \times [\begin{matrix} 米_{1} \\ 米_{2} \\ 米_{3} \end{matrix}]$

旋转矩阵对应于加速度计读数和所述磁力计读数的交叉乘积的第二列：

$[\begin{matrix} {[R}_{21} \\ {[R}_{22} \\ {[R}_{23} \end{matrix}] = [\begin{matrix} {一种}_{1} \\ {一种}_{2} \\ {一种}_{3} \end{matrix}] \times [\begin{matrix} 米_{1} \\ 米_{2} \\ 米_{3} \end{matrix}]$

通过旋转矩阵的定义中，第1列是2列3的交叉乘积：

$\begin{array}{l} [\begin{matrix} {[R}_{11} \\ {[R}_{12} \\ {[R}_{13} \end{matrix}] = [\begin{matrix} {[R}_{21} \\ {[R}_{22} \\ {[R}_{23} \end{matrix}] \times [\begin{matrix} {[R}_{31} \\ {[R}_{32} \\ {[R}_{33} \end{matrix}] \\ = （一种 \times 米） \times 一种 \end{array}$