AHRS
基于加速度计、陀螺仪和磁力计读数的方向。
blockType: AHRS
|
图书馆路径: |
港口
输入
#
[医]Accel
—
传感器外壳坐标系中的加速度计读数，m/s2
实标量n乘3的矩阵
Details
加速度计读数在传感器壳体坐标系中的m/s2，设置为矩阵 上 实标量，其中 -计数的数量，并且*Accel*矩阵的三列表示测量值 分别。
| 数据类型 |
|
| 复数支持 |
非也。 |
#
陀螺仪
—
传感器外壳坐标系中的陀螺仪读数，rad/s
实标量n乘3的矩阵
Details
传感器外壳坐标系中的陀螺仪读数以rad/s为单位，设置为矩阵 上 实标量，其中 -计数的数量，并且*陀螺*矩阵的三列表示测量 分别。
| 数据类型 |
|
| 复数支持 |
非也。 |
#
马格
—
传感器外壳坐标系中的磁力计读数，MCT
实标量n乘3的矩阵
Details
磁力计读数在mkTl中的传感器外壳的坐标系中，设置为矩阵 上 实标量，其中 -计数的数量，并且*陀螺*矩阵的三列表示测量 分别。
| 数据类型 |
|
| 复数支持 |
非也。 |
输出
#
方向感
—
传感器壳体坐标系相对于导航坐标系的取向
标量M乘4的矩阵 | 3乘3乘M旋转矩阵的数组
Details
传感器壳体坐标系相对于导航坐标系的方位，作为标量矩阵返回 上 或者作为旋转矩阵的数组 上 上 . 矩阵的每一行 上 它包含四个四元数元素。 输入样本数目 和参数值 *Decimation factor*确定输出数据的大小 .
| 数据类型 |
|
| 复数支持 |
非也。 |
#
角速度
—
传感器外壳坐标系中的角速度，rad/s
实标量M乘3的矩阵
Details
角速度，以rad/s计及陀螺仪在传感器壳体坐标系中的位移，作为矩阵返回 上 实标量。 输入样本数目 和参数值 *Decimation factor*确定输出数据的大小 .
| 数据类型 |
|
| 复数支持 |
非也。 |
参数
Main
#
Reference frame —
导航坐标系
NED | ENU
Details
的导航坐标系，设为 NED (东北-下)或 ENU （东-北-上）。
| 值 |
|
| 默认值 |
|
| 程序使用名称 |
|
| 可调谐 |
无 |
| 可计算 |
无 |
#
Decimation factor —
抽取系数
Real number
Details
减小来自输入传感器的数据传输速率的抽取系数，设置为正整数。
输入数据矩阵*Accel*、*Gyro*和*Mag*中的行数必须是抽取系数的倍数。
| 默认值 |
|
| 程序使用名称 |
|
| 可调谐 |
无 |
| 可计算 |
是 |
#
Initial process noise —
程的初始噪声
Matrix of real numbers
Details
过程的初始噪声水平，定义为具有大小的实标量矩阵 上 .
| 默认值 |
|
| 程序使用名称 |
|
| 可调谐 |
无 |
| 可计算 |
是 |
#
Orientation format —
输出方向格式
quaternion | Rotation matrix
Details
输出数据方向格式，设置为 quaternion 或 Rotation matrix:
* quaternion -实标量的输出矩阵 上 . 矩阵的每一行包含四个四元数元素。
* Rotation matrix -旋转矩阵的输出阵列 茎：[3] 上 茎：[3] 上 茎：[M]
输入样本数目 茎：[N] 和参数值 *{blocklibrarypp_blocktypespp_ahrspp_basepp_paramspp_pp_decimationfactorpp_label}*确定输出数据的大小 茎：[M].
| 值 |
|
| 默认值 |
|
| 程序使用名称 |
|
| 可调谐 |
无 |
| 可计算 |
无 |
Main
#
Accelerometer noise ((m/s²)²) —
加速度计信号的噪声色散，(m/s2)2
Real number
Details
加速度计信号的噪声色散in(m/s2)2，作为正实标量给出。
| 默认值 |
|
| 程序使用名称 |
|
| 可调谐 |
是 |
| 可计算 |
是 |
#
Gyroscope noise ((rad/s)²) —
陀螺仪信号的噪声色散，(rad/s)2
Real number
Details
陀螺仪信号的噪声色散in(rad/s)2，作为正实标量给出。
| 默认值 |
|
| 程序使用名称 |
|
| 可调谐 |
是 |
| 可计算 |
是 |
#
Magnetometer noise ((μT)²) —
磁强计信号的噪声色散，MCT2
Real number
Details
磁强计信号在MCT2中的噪声色散，作为正实标量给出。
| 默认值 |
|
| 程序使用名称 |
|
| 可调谐 |
是 |
| 可计算 |
是 |
#
Gyroscope drift noise ((rad/s)²) —
陀螺位移漂移方差，(rad/s)2
Real number
Details
陀螺仪位移漂移的方差in(rad/s)2，作为正实标量给出。
| 默认值 |
|
| 程序使用名称 |
|
| 可调谐 |
是 |
| 可计算 |
是 |
Main
#
Linear acceleration noise ((m/s²)²) —
线性加速度的噪声分散，(m/s2)2
Real number
Details
线性加速度的噪声方差在(m/s2)2，作为正实标量给出。 线性加速度被建模为由低通滤波器滤除的白噪声过程。
| 默认值 |
|
| 程序使用名称 |
|
| 可调谐 |
是 |
| 可计算 |
是 |
#
Magnetic disturbance noise ((μT)²) —
磁干扰噪声的色散，mcTl2
Real number
Details
磁干扰噪声在MCT2中的色散，作为有限正实标量给出。
| 默认值 |
|
| 程序使用名称 |
|
| 可调谐 |
是 |
| 可计算 |
是 |
#
Linear acceleration decay factor —
线性加速度漂移的衰减系数
Real number
Details
线性加速度漂移的衰减系数，以范围内的标量给出 [0, 1). 如果线性加速度快速变化，请将此参数设置为较低值。 如果线性加速度变化缓慢，请将此参数设置为较高的值。 线性加速度漂移被建模为由低通滤波器滤除的白噪声过程。
| 默认值 |
|
| 程序使用名称 |
|
| 可调谐 |
是 |
| 可计算 |
是 |
#
Magnetic disturbance decay factor —
磁扰动衰减系数
Real number
Details
磁扰动的衰减系数，设定为范围内的标量 [0, 1]. 磁扰动被建模为一阶马尔可夫过程。
| 默认值 |
|
| 程序使用名称 |
|
| 可调谐 |
是 |
| 可计算 |
是 |
#
Magnetic field strength (μT) —
磁场强度，MCT
Real number
Details
MCT中的磁场强度，作为实正标量给出。 磁场强度是对地球磁场在当前位置的强度的估计。
| 默认值 |
|
| 程序使用名称 |
|
| 可调谐 |
是 |
| 可计算 |
是 |
算法
本节中给出的算法仅适用于坐标系。 NED.
|
座 *AHRS*使用[1]中描述的九轴卡尔曼滤波器的结构。 该算法试图跟踪方位误差、陀螺仪偏移、线性加速度和磁扰动，以获得最终的方位和角速度。 间接卡尔曼滤波器代替直接定向跟踪，模拟误差过程。 使用递归更新:
哪里
* -向量 上 ，含:
** -方向向量 上 ，以度为单位，一次 ;
** -陀螺仪零角速度的位移矢量 上 ，以deg/s为单位，一次 ;
** -加速度误差向量 上 ，在传感器坐标系中测量，单位为m/s2，一次 ;
** -磁扰动的误差矢量 上 ，在传感器坐标系中测量，以MCT为单位，一次 ;
* -加性噪声向量 上 ;
* -状态转换模型。
因为 它被定义为一个误差过程，先验估计总是零，因此状态转换模型。 它等于零。 这导致了标准卡尔曼方程的以下简化:
该算法中使用的卡尔曼方程:
哪里
* -条件的预测（先验）评估;错误过程;
* -协方差的预测（先验）估计;
* -差异;
* -残差的协方差;
* -卡尔曼增益因子;
* -更新（后验）病情评估;
* -更新（后验）协方差估计;
*较低指数 -迭代次数;
*上指数 -后验评估;
*上指数 -先验评估。
图形图像和以下步骤描述了基于单个帧的算法的一次迭代。
在第一次迭代之前，输入数据 , 和 它们被分成具有抽取系数的帧 3. 对于每个帧，该算法使用与陀螺仪读数的帧相对应的最新加速度计和磁力计读数。
模型
Details
该算法将加速度和角度变化建模为线性过程。
选区定位_
当前帧的方向是通过首先估计前一帧的角度变化来确定的:
哪里
- * -参数设置的抽取系数 *Decimation factor&ast
-
* -采样率。
使用四元数构造语法将角度变化转换为四元数。:
以前的方向分数通过旋转更新为 :
在第一次迭代期间，定向评估 初始化与 [医]ecompass.
按方向估计重力
重力矢量被解释为四元数的第三列 以旋转矩阵的形式:
通过加速度估计重力
重力矢量的第二估计是通过从加速度计读数中减去来自先前迭代的衰减线性加速度的估计来进行的。:
地球磁矢量的估计
通过将磁矢量估计值从前一次迭代旋转到旋转矩阵形式的先验方位估计来估计地球磁矢量:
磁力计校正
Details
磁力计校正确定磁矢量估计中的误差并检测磁干扰。
磁扰动误差
通过将与磁矢量相关的卡尔曼增益与误差信号进行矩阵乘法来计算磁扰动误差:
卡尔曼增益因子 —这是在当前迭代中计算的卡尔曼增益因子。
磁干扰检测
通过检查检测到的磁扰动的功率小于或等于预期磁场强度的功率的四倍来确定磁干扰。:
卡尔曼方程
Details
卡尔曼方程使用重力的估计 ，从陀螺仪读数获得，磁矢量估计 从陀螺仪读数中获得，并测量误差过程 来更新卡尔曼增益和中间协方差矩阵。 将卡尔曼增益应用于误差信号 以获得后验误差估计 .
测量模型
测量模型以大小显示观察到的状态 上 , 和 ，到真实状态 大小 上 .
测量模型构造如下:
哪里
* , 和 — -, -和 -重力矢量的元素，分别在先验方位的基础上估计;
* , 和 — -, -和 -磁场矢量的元素，分别基于先验取向估计;
* 是等于抽取系数与采样频率之比的常数。
残差的变异
残差的协方差是大小的矩阵 上 ，用于跟踪测量的可变性。 残差协方差矩阵计算如下:
哪里
* -测量模型的矩阵;
* -在先前迭代中计算的测量模型协方差的预测（先验）估计;
* -测量模型的噪声协方差，计算如下:
+
+ 哪里
+
+
更新误差估计协方差
误差估计的协方差是一个矩阵 上 ，用于跟踪状态的可变性。
误差估计协方差矩阵更新如下:
哪里
* -卡尔曼增益因子;
* -测量矩阵;
* -在先前迭代中计算的误差估计的协方差。
误差估计的预测协方差
误差估计的协方差是一个矩阵 上 ，用于跟踪状态的可变性。 误差估计的先验协方差 确定过程噪声的协方差相等 ，在前一次迭代中定义。 作为误差估计的a后验协方差的函数计算 . 计算时 假定互相关项与自相关项相比是可忽略的并且被设置为零。:
哪里
* -更新（后验）误差估计协方差;
* 是抽取系数除以采样频率;
* -陀螺仪漂移噪声;
* -陀螺仪噪声;
* -线性加速度的衰减系数;
* -线性加速度的噪声;
* -磁干扰的衰减系数;
* -磁干扰的噪声。
卡尔曼增益系数
卡尔曼增益矩阵是大小的矩阵 上 ，用于权衡差异。 在该算法中，差异被解释为错误过程。 .
卡尔曼增益矩阵构造如下:
哪里
* -预测误差协方差;
* -测量模型;
* -残差的协方差。
更新后验错误
通过将卡尔曼增益矩阵与重力矢量和磁场矢量的估计中的误差相结合来确定后验误差估计:
如果在当前迭代中检测到磁干扰，则忽略磁矢量误差信号，并且如下计算a后验误差估计:
文学作品
-
开源传感器融合。 https://github.com/memsindustrygroup/Open-Source-Sensor-Fusion/tree/master/docs
-
D.Roetenberg，H.J.Luinge，C.T.M.Baten，P.H.Veltink。 磁扰动的补偿改进了人体段定向的惯性和磁传感，IEEE关于神经网络和康复工程的交易，第1卷。 13,Issue3,2005,pp.395-405.