ecompass

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

所有的页面崩溃

语法

取向= ecompass (accelerometerReading magnetometerReading)

取向= ecompass (accelerometerReading magnetometerReading orientationFormat)

取向= ecompass (accelerometerReading magnetometerReading、orientationFormat ReferenceFrame, RF)

描述

例子

取向= ecompass (accelerometerReading,magnetometerReading)返回一个四元数可以从父母(NED)坐标系旋转量一个孩子(传感器)框架。

例子

取向= ecompass (accelerometerReading,magnetometerReading,orientationFormat)指定方向格式四元数或旋转矩阵。

取向= ecompass (accelerometerReading,magnetometerReading,orientationFormat,“ReferenceFrame”,RF)还允许您指定参考系射频的取向输出。指定射频作为NED的(North-East-Down)或“ENU表示”(East-North-Up)。默认值是NED的。

例子

全部折叠

确定偏差的波士顿

打开生活的脚本

使用已知的磁场强度和适当的加速设备指出真北在波士顿的磁偏角波士顿。

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

magneticFieldStrength = (19.535 -5.109 47.930);properAcceleration = [0 0 9.8];

通过磁场强度和加速度ecompass函数。的ecompass函数返回一个旋转四元数运算符。四元数转换为欧拉角的度。

q = ecompass (properAcceleration magneticFieldStrength);e = eulerd (q,“ZYX股票”,“帧”);

的角度,e,代表真北的夹角和磁北在波士顿。按照惯例,磁偏负当磁北真北以西。否定的角度来确定磁偏。

magneticDeclinationOfBoston = - e (1)

magneticDeclinationOfBoston = -14.6563

返回旋转矩阵

打开生活的脚本

的ecompass功能融合磁强计和加速度计数据返回一个四元数,在旋转四元数算子中使用时,可以旋转量从父母(NED)对一个孩子来说帧。的ecompass函数可以返回执行等效旋转的旋转矩阵的四元数运算符。

定义一个旋转,可以把一个父框架指向磁北子帧指向地理北极。定义旋转四元数和旋转矩阵。然后,四元数与旋转矩阵转化为欧拉角的度比较。

定义的磁场强度microteslas在波士顿,MA,指出真正的北方。

m = 19.535 (-5.109 - 47.930);一个= [0 0 9.8];

确定四元数和旋转矩阵的旋转框架从磁北真北。显示结果的比较。

q = ecompass (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

确定重力向量

打开生活的脚本

使用ecompass确定基于数据从一个旋转的IMU的重力向量。

负载惯性测量单元(IMU)的数据。

负载“rpy_9axis.mat”sensorDataFs

确定传感器的身体的方向相对于当地NED框架。

取向= ecompass (sensorData.Acceleration sensorData.MagneticField);

首先估计重力向量,旋转的加速度计读数传感器车身骨架NED框架使用取向四元数向量。

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

确定重力矢量的平均恢复重力向量。

gravityVectorEstimate =意味着(gravityVectors, 1)

gravityVectorEstimate =1×30.0000 -0.0000 10.2102

跟踪旋转平台

打开生活的脚本

融合建模加速度计和陀螺仪数据跟踪一个旋转平台使用理想化和真实的数据。

生成真实轨迹

随着时间的推移真实描述平台的定位。使用kinematicTrajectory系统对象™创建一个轨迹为一个平台,没有翻译和旋转的z设在。

时间= 12;fs = 100;numSamples = f *时间;accelerationBody = 0 (numSamples, 3);angularVelocityBody = 0 (numSamples, 3);zAxisAngularVelocity = [linspace(4 0 4 *π* fs), 4 *π* (1,4 * fs), linspace(4 *π,0,4 * fs)] ';angularVelocityBody (:, 3) = zAxisAngularVelocity;轨迹= kinematicTrajectory (“SampleRate”fs);[取向,~ ~,加速度,angularVelocityNED] =轨迹(accelerationBody angularVelocityBody);

接收IMU数据模型

使用一个imuSensor系统对象来模拟数据收到一个包含理想磁强计和IMU理想加速度计。

IMU = imuSensor (“accel-mag”,“SampleRate”fs);[accelerometerData, magnetometerData] = IMU(加速度,…angularVelocityNED,…取向);

IMU数据融合估计取向

通过加速度计和磁强计数据数据ecompass随着时间的推移函数来估计取向。方向转化为欧拉角度和阴谋的结果。

取向= ecompass (accelerometerData magnetometerData);orientationEuler = eulerd(取向,“ZYX股票”,“帧”);timeVector = (0: numSamples-1)。/ fs;图(1)情节(timeVector orientationEuler)传说(z轴的,“轴”,“轴”)包含(“时间(s)”)ylabel (的旋转(度))标题(“从理想IMU取向”)

图包含一个坐标轴对象。坐标轴对象与理想IMU标题定位包含3线类型的对象。这些对象代表z轴,轴,轴。

重复实验与现实的IMU传感器模型

修改参数IMU系统对象近似真实的IMU传感器数据。重置IMU然后称之为真实具有相同加速度,角速度和取向。使用ecompass保险丝IMU数据和策划的结果。

IMU。加速度计= accelparams (…“MeasurementRange”,20岁,…“决议”,0.0006,…“ConstantBias”,0.5,…“AxesMisalignment”2,…“NoiseDensity”,0.004,…“BiasInstability”,0.5);IMU。磁强计= magparams (…“MeasurementRange”,200,…“决议”,0.01);重置(IMU) [accelerometerData magnetometerData] = IMU(加速度、angularVelocityNED取向);取向= ecompass (accelerometerData magnetometerData);orientationEuler = eulerd(取向,“ZYX股票”,“帧”);图(2)情节(timeVector orientationEuler)传说(z轴的,“轴”,“轴”)包含(“时间(s)”)ylabel (的旋转(度))标题(“从现实的IMU取向”)

图包含一个坐标轴对象。坐标轴对象标题取向与现实的IMU包含3线类型的对象。这些对象代表z轴,轴,轴。

输入参数

全部折叠

`accelerometerReading`- - - - - -加速度计读数在身体传感器坐标系(m / s²)
N3矩阵

加速度计读数传感器体坐标系中m / s²指定为一个N3矩阵。矩阵的列对应x- - - - - -,y- - - - - -,z传感器的身体相互重合。矩阵的行,N,对应于个人样本。加速度计读数前规范化使用的函数。

数据类型:单|双

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

磁强计读数在µT身体传感器坐标系中,指定为一个N3矩阵。矩阵的列对应x- - - - - -,y- - - - - -,z传感器的身体相互重合。矩阵的行,N,对应于个人样本。磁强计的读数前规范化使用的函数。

数据类型:单|双

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

格式用于描述取向,指定为“四元数”或“rotmat”。

数据类型:字符|字符串

输出参数

全部折叠

`取向`——方向旋转量从全球坐标系统到传感器的身体坐标系统
N1矢量的四元数(默认)| 3-by-3-by -N数组

取向,可以旋转量从全球坐标系统于一体的坐标系统,作为向量的四元数或返回一个数组。的大小和类型取向取决于使用的格式来描述取向:

“四元数”- - -N1矢量的四元数相同的底层数据类型作为输入
“rotmat”——3-by-3-byN数组作为输入相同的数据类型

数据类型:四元数|单|双

算法

的ecompass函数返回一个四元数或旋转矩阵旋转量从父母(例如NED)框架对一个孩子(传感器)框架。对于面向输出格式,旋转操作符是由计算旋转矩阵。

首先计算旋转矩阵与一个中介:

$R = (\begin{matrix} (一个 \times 米) \times 一个 & 一个 \times 米 & 一个 \end{matrix}]$

然后列正常化。一个和米是accelerometerReading输入和magnetometerReading分别输入。

理解旋转矩阵的计算,考虑地球上任意点及其对应的本地NED框架。假设一个传感器车身骨架,x,y,z),相同的起源。

NED框架

回想一下,定位传感器的身体被定义为旋转操作符(旋转矩阵或四元数)需要从父母(NED)框架旋转量对一个孩子来说(传感器)框架:

$(\begin{matrix} R \end{matrix}] (\begin{matrix} p_{父} \end{matrix}] = (\begin{matrix} p_{孩子} \end{matrix}]$

在哪里

R3 x3的旋转矩阵,可以解释为子帧的方向。
p_父在父框架是一个3×1的向量。
p_孩子是一个3×1向量的子帧。

稳定的传感器,一个加速度计返回重力加速度。如果传感器的身体是完全符合NED坐标系统,重力加速度沿z设在,加速度计读取(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}]$

磁强计读数指向磁北的N- - - - - -D飞机。再次,考虑传感器车身骨架与NED坐标系统保持一致。

磁强计方向

根据定义,E设在垂直于N- - - - - -D平面上,因此N⨯D=E在一些振幅比例。如果传感器车身骨架与内德,加速度矢量的加速度计和磁强计的磁场矢量在撒谎N- - - - - -D飞机。因此米⨯一个=y,再用一些振幅比例。

考虑所需的旋转矩阵旋转NED孩子,(xyz]。

表达一个十字架米

$(\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}$

最后,旋转矩阵归一化列:

$R_{我 j} = \frac{R_{我 j}}{\sqrt{\sum_{我 = 1}^{3} R_{我 j}^{2}}}, \forall j$

请注意

的ecompass算法使用磁北,不是真北,NED坐标系统。

引用

[1]开源传感器融合。https://github.com/memsindustrygroup/Open-Source-Sensor-Fusion/tree/master/docs

扩展功能

C / c++代码生成
生成C和c++代码使用MATLAB®编码器™。

另请参阅

四元数|ahrsfilter|imufilter

主题

使用惯性传感器确定方向

介绍了R2018b

ecompass

语法

描述

例子

确定偏差的波士顿

返回旋转矩阵

确定重力向量

跟踪旋转平台

输入参数

`accelerometerReading`- - - - - -加速度计读数在身体传感器坐标系(m / s²)
N3矩阵

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

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

输出参数

`取向`——方向旋转量从全球坐标系统到传感器的身体坐标系统
N1矢量的四元数(默认)| 3-by-3-by -N数组

算法

引用

扩展功能

C / c++代码生成
生成C和c++代码使用MATLAB®编码器™。

另请参阅

主题

传感器融合和跟踪工具箱文档

金宝app

自治系统的传感器融合和跟踪

ecompass

语法

描述

例子

确定偏差的波士顿

返回旋转矩阵

确定重力向量

跟踪旋转平台

输入参数

accelerometerReading- - - - - -加速度计读数在身体传感器坐标系(m / s2)N3矩阵

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

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

输出参数

取向——方向旋转量从全球坐标系统到传感器的身体坐标系统N1矢量的四元数(默认)| 3-by-3-by -N数组

算法

引用

扩展功能

C / c++代码生成生成C和c++代码使用MATLAB®编码器™。

另请参阅

主题

传感器融合和跟踪工具箱文档

金宝app

自治系统的传感器融合和跟踪

`accelerometerReading`- - - - - -加速度计读数在身体传感器坐标系(m / s²)
N3矩阵

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

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

`取向`——方向旋转量从全球坐标系统到传感器的身体坐标系统
N1矢量的四元数(默认)| 3-by-3-by -N数组

C / c++代码生成
生成C和c++代码使用MATLAB®编码器™。