ahrsfiltergydF4y2Ba

方向从加速度计，陀螺仪和磁力计读数gydF4y2Ba

展开全部页面gydF4y2Ba

描述gydF4y2Ba

的gydF4y2BaahrsfiltergydF4y2Ba系统对象™融合加速度计，磁力计和陀螺仪传感器数据，以估计设备方向。gydF4y2Ba

估计设备方向:gydF4y2Ba

创建gydF4y2BaahrsfiltergydF4y2Ba对象并设置其属性。gydF4y2Ba
使用参数调用对象，就像调用函数一样。gydF4y2Ba

有关系统对象如何工作的详细信息，请参见gydF4y2Ba什么是系统对象?gydF4y2Ba

创建gydF4y2Ba

语法gydF4y2Ba

保险丝= ahrsfiltergydF4y2Ba

FUSE = ahrsfilter('ReferenceFrame'，RF)gydF4y2Ba

保险丝= ahrsfilter(gydF4y2Ba___gydF4y2Ba、名称、值)gydF4y2Ba

描述gydF4y2Ba

例子gydF4y2Ba

保险丝gydF4y2Ba= ahrsfiltergydF4y2Ba返回一个间接卡尔曼滤波系统对象，gydF4y2Ba保险丝gydF4y2Ba，用于加速度计、陀螺仪和磁力计数据的传感器融合，以估计设备的方向和角速度。该滤波器使用一个12元状态向量来跟踪方向、陀螺仪偏差、线性加速度和磁扰动的估计误差。gydF4y2Ba

保险丝gydF4y2Ba= ahrsfilter (gydF4y2Ba“ReferenceFrame”gydF4y2Ba，gydF4y2Ba射频gydF4y2Ba）gydF4y2Ba返回一个ahrsfilter System对象，该对象融合加速度计、陀螺仪和磁力计数据，以估计相对于参考系的设备方向gydF4y2Ba射频gydF4y2Ba．指定gydF4y2Ba射频gydF4y2Ba作为gydF4y2BaNED的gydF4y2Ba(North-East-Down)或gydF4y2Ba“ENU表示”gydF4y2Ba(East-North-Up)。默认值为“NED”。gydF4y2Ba

例子gydF4y2Ba

保险丝gydF4y2Ba= ahrsfilter (gydF4y2Ba___gydF4y2Ba，gydF4y2Ba名称,值gydF4y2Ba）gydF4y2Ba设置每个属性gydF4y2Ba的名字gydF4y2Ba到指定的gydF4y2Ba价值gydF4y2Ba．未指定的属性有默认值。gydF4y2Ba

属性gydF4y2Ba

全部展开gydF4y2Ba

除非另有说明，属性为gydF4y2BanontunablegydF4y2Ba，这意味着在调用对象后不能更改它们的值。对象在调用时锁定，而gydF4y2Ba释放gydF4y2Ba功能解锁它们。gydF4y2Ba

如果属性为gydF4y2Ba可调gydF4y2Ba，您可以随时更改其值。gydF4y2Ba

有关更改属性值的详细信息，请参见gydF4y2Ba使用系统对象的MATLAB系统设计gydF4y2Ba．gydF4y2Ba

`SampleRategydF4y2Ba`- - - - - -gydF4y2Ba传感器数据输入采样率(Hz)gydF4y2Ba
`One hundred.gydF4y2Ba`(默认)|gydF4y2Ba积极的标量gydF4y2Ba

输入传感器数据的采样率，单位为Hz，指定为正标量。gydF4y2Ba

可调:gydF4y2Ba没有gydF4y2Ba

数据类型:gydF4y2Ba单gydF4y2Ba|gydF4y2Ba双gydF4y2Ba

`DecimationFactorgydF4y2Ba`- - - - - -gydF4y2Ba大量毁灭的因素gydF4y2Ba
`1gydF4y2Ba`(默认)|gydF4y2Ba正整数gydF4y2Ba

抽取因子通过它来降低输入传感器数据速率作为融合算法的一部分，指定为正整数。gydF4y2Ba

输入的行数gydF4y2BaaccelReadingsgydF4y2Ba，gydF4y2BagyroReadingsgydF4y2Ba,gydF4y2BamagReadingsgydF4y2Ba——必须是抽取因子的倍数。gydF4y2Ba

数据类型:gydF4y2Ba单gydF4y2Ba|gydF4y2Ba双gydF4y2Ba

`AccelerometerNoisegydF4y2Ba`- - - - - -gydF4y2Ba加速度计信号噪声方差((m/sgydF4y2Ba^2gydF4y2Ba）gydF4y2Ba^2gydF4y2Ba）gydF4y2Ba
`0.00019247gydF4y2Ba`(默认)|gydF4y2Ba正实标量gydF4y2Ba

加速度计信号噪声方差(m/s)gydF4y2Ba^2gydF4y2Ba）gydF4y2Ba^2gydF4y2Ba，指定为正实标量。gydF4y2Ba

可调:gydF4y2Ba是的gydF4y2Ba

数据类型:gydF4y2Ba单gydF4y2Ba|gydF4y2Ba双gydF4y2Ba

`MagnetometerNoisegydF4y2Ba`- - - - - -gydF4y2Ba磁强计信号噪声方差(μTgydF4y2Ba^2gydF4y2Ba）gydF4y2Ba
`0．1gydF4y2Ba`(默认)|gydF4y2Ba正实标量gydF4y2Ba

以μT为单位的磁强计信号噪声方差gydF4y2Ba^2gydF4y2Ba，指定为正实标量。gydF4y2Ba

可调:gydF4y2Ba是的gydF4y2Ba

数据类型:gydF4y2Ba单gydF4y2Ba|gydF4y2Ba双gydF4y2Ba

`GyroscopeNoisegydF4y2Ba`- - - - - -gydF4y2Ba陀螺仪信号噪声方差((rad/s)gydF4y2Ba^2gydF4y2Ba）gydF4y2Ba
`9.1385 e-5gydF4y2Ba`(默认)|gydF4y2Ba正实标量gydF4y2Ba

陀螺仪信号噪声方差(rad/s)gydF4y2Ba^2gydF4y2Ba，指定为正实标量。gydF4y2Ba

可调:gydF4y2Ba是的gydF4y2Ba

数据类型:gydF4y2Ba单gydF4y2Ba|gydF4y2Ba双gydF4y2Ba

`GyroscopeDriftNoisegydF4y2Ba`- - - - - -gydF4y2Ba陀螺仪偏移漂移方差((rad/s)gydF4y2Ba^2gydF4y2Ba）gydF4y2Ba
`3.0462 e-13gydF4y2Ba`(默认)|gydF4y2Ba正实标量gydF4y2Ba

陀螺仪偏移漂移方差(单位:rad/s)gydF4y2Ba^2gydF4y2Ba，指定为正实标量。gydF4y2Ba

可调:gydF4y2Ba是的gydF4y2Ba

数据类型:gydF4y2Ba单gydF4y2Ba|gydF4y2Ba双gydF4y2Ba

`LinearAccelerationNoisegydF4y2Ba`- - - - - -gydF4y2Ba线性加速度噪声方差(m/sgydF4y2Ba^2gydF4y2Ba）gydF4y2Ba^2gydF4y2Ba
`0.0096236gydF4y2Ba`(默认)|gydF4y2Ba正实标量gydF4y2Ba

线性加速度噪声方差(m/s)gydF4y2Ba^2gydF4y2Ba）gydF4y2Ba^2gydF4y2Ba，指定为正实标量。线性加速度被建模为一个低通滤波白噪声过程。gydF4y2Ba

可调:gydF4y2Ba是的gydF4y2Ba

数据类型:gydF4y2Ba单gydF4y2Ba|gydF4y2Ba双gydF4y2Ba

`LinearAccelerationDecayFactorgydF4y2Ba`- - - - - -gydF4y2Ba线性加速度漂移的衰减因子gydF4y2Ba
`0．5gydF4y2Ba`(默认)|gydF4y2Ba[0,1]范围内的标量gydF4y2Ba

线性加速度漂移的衰减因子，指定为范围[0,1)内的标量。如果线性加速度变化快，设置gydF4y2BaLinearAcclerationDecayFactorgydF4y2Ba到一个更低的值。如果线性加速度变化缓慢，则设置gydF4y2BaLinearAcclerationDecayFactorgydF4y2Ba到一个更高的值。线性加速度漂移被建模为低通滤波白噪声过程。gydF4y2Ba

可调:gydF4y2Ba是的gydF4y2Ba

数据类型:gydF4y2Ba单gydF4y2Ba|gydF4y2Ba双gydF4y2Ba

`MagneticDisturbanceNoisegydF4y2Ba`- - - - - -gydF4y2Ba磁干扰噪声方差(μTgydF4y2Ba^2gydF4y2Ba）gydF4y2Ba
`0．5gydF4y2Ba`(默认)|gydF4y2Ba实有限正标量gydF4y2Ba

磁干扰噪声的方差(μT)gydF4y2Ba^2gydF4y2Ba，指定为实有限正标量。gydF4y2Ba

可调:gydF4y2Ba是的gydF4y2Ba

数据类型:gydF4y2Ba单gydF4y2Ba|gydF4y2Ba双gydF4y2Ba

`MagneticDisturbanceDecayFactorgydF4y2Ba`- - - - - -gydF4y2Ba磁扰动衰减因子gydF4y2Ba
`0．5gydF4y2Ba`(默认)|gydF4y2Ba在[0,1]范围内的正标量gydF4y2Ba

磁扰动的衰减因子，指定为范围[0,1]内的正标量。磁扰动被建模为一阶马尔可夫过程。gydF4y2Ba

可调:gydF4y2Ba是的gydF4y2Ba

数据类型:gydF4y2Ba单gydF4y2Ba|gydF4y2Ba双gydF4y2Ba

`InitialProcessNoisegydF4y2Ba`- - - - - -gydF4y2Ba过程噪声的协方差矩阵gydF4y2Ba
12-by-12矩阵gydF4y2Ba

过程噪声的协方差矩阵，指定为12 × 12矩阵。默认为:gydF4y2Ba

列1到6 0 0 0 0 0 0 0.000006092348396 0.000006092348396 0.000006092348396 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.000076154354947 0.000076154354947 0.000076154354947 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0列7到12 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.009623610000000 0.009623610000000 0.009623610000000 0 0 0 0 0 0 0 0 0 0 0 0 0.600000000000000 0.600000000000000 0 0 0 0 0 0 0 0 0 0 000．600000000000000

初始过程协方差矩阵解释了过程模型中的误差。gydF4y2Ba

数据类型:gydF4y2Ba单gydF4y2Ba|gydF4y2Ba双gydF4y2Ba

`ExpectedMagneticFieldStrengthgydF4y2Ba`- - - - - -gydF4y2Ba磁场强度预期估计值(μT)gydF4y2Ba
`50gydF4y2Ba`(默认)|gydF4y2Ba实正标量gydF4y2Ba

磁场强度的预期估计值，以μT为单位，指定为实正标量。预期磁场强度是对地球在当前位置的磁场强度的估计。gydF4y2Ba

可调:gydF4y2Ba是的gydF4y2Ba

数据类型:gydF4y2Ba单gydF4y2Ba|gydF4y2Ba双gydF4y2Ba

`OrientationFormatgydF4y2Ba`- - - - - -gydF4y2Ba输出方向格式gydF4y2Ba
`“四元数”gydF4y2Ba`(默认)|gydF4y2Ba`旋转矩阵的gydF4y2Ba`

输出方向格式，指定为gydF4y2Ba“四元数”gydF4y2Ba或gydF4y2Ba旋转矩阵的gydF4y2Ba．输出的大小取决于输入的大小，gydF4y2BaNgydF4y2Ba，输出方向格式:gydF4y2Ba

“四元数”gydF4y2Ba——输出是一个gydF4y2BaNgydF4y2Ba1gydF4y2Ba四元数gydF4y2Ba．gydF4y2Ba
旋转矩阵的gydF4y2Ba——输出为3 × 3 × -gydF4y2BaNgydF4y2Ba旋转矩阵。gydF4y2Ba

数据类型:gydF4y2Ba字符gydF4y2Ba|gydF4y2Ba字符串gydF4y2Ba

使用gydF4y2Ba

语法gydF4y2Ba

[orientation, angarvelocity] = FUSE(accelerreads, gyroreads, magreads)gydF4y2Ba

描述gydF4y2Ba

例子gydF4y2Ba

［gydF4y2Ba取向gydF4y2Ba，gydF4y2BaangularVelocitygydF4y2Ba=保险丝(gydF4y2BaaccelReadingsgydF4y2Ba，gydF4y2BagyroReadingsgydF4y2Ba，gydF4y2BamagReadingsgydF4y2Ba）gydF4y2Ba融合加速度计，陀螺仪和磁力计数据来计算方向和角速度测量。该算法假设设备在第一次调用之前是静止的。gydF4y2Ba

输入参数gydF4y2Ba

全部展开gydF4y2Ba

`accelReadingsgydF4y2Ba`- - - - - -gydF4y2Ba加速度计读数在传感器本体坐标系(m/sgydF4y2Ba^2gydF4y2Ba）gydF4y2Ba
NgydF4y2Ba3矩阵gydF4y2Ba

加速度计读数在传感器本体坐标系中，单位为m/sgydF4y2Ba^2gydF4y2Ba，指定为gydF4y2BaNgydF4y2Ba3矩阵。gydF4y2BaNgydF4y2Ba是样本的数量，和三列的gydF4y2BaaccelReadingsgydF4y2Ba表示[gydF4y2BaxgydF4y2BaygydF4y2BazgydF4y2Ba)测量。方法所指定的采样速率假定加速度计读数对应gydF4y2BaSampleRategydF4y2Ba财产。gydF4y2Ba

数据类型:gydF4y2Ba单gydF4y2Ba|gydF4y2Ba双gydF4y2Ba

`gyroReadingsgydF4y2Ba`- - - - - -gydF4y2Ba陀螺仪在传感器体坐标系中的读数(rad/s)gydF4y2Ba
NgydF4y2Ba3矩阵gydF4y2Ba

陀螺仪读数在传感器体坐标系中以rad/s为单位，指定为gydF4y2BaNgydF4y2Ba3矩阵。gydF4y2BaNgydF4y2Ba是样本的数量，和三列的gydF4y2BagyroReadingsgydF4y2Ba表示[gydF4y2BaxgydF4y2BaygydF4y2BazgydF4y2Ba)测量。陀螺仪读数被假定对应的采样率规定gydF4y2BaSampleRategydF4y2Ba财产。gydF4y2Ba

数据类型:gydF4y2Ba单gydF4y2Ba|gydF4y2Ba双gydF4y2Ba

`magReadingsgydF4y2Ba`- - - - - -gydF4y2Ba传感器体坐标系中的磁力计读数(µT)gydF4y2Ba
NgydF4y2Ba3矩阵gydF4y2Ba

磁强计读数在传感器体坐标系中的µT，指定为gydF4y2BaNgydF4y2Ba3矩阵。gydF4y2BaNgydF4y2Ba是样本的数量，和三列的gydF4y2BamagReadingsgydF4y2Ba表示[gydF4y2BaxgydF4y2BaygydF4y2BazgydF4y2Ba)测量。磁强计的读数被假定对应于由gydF4y2BaSampleRategydF4y2Ba财产。gydF4y2Ba

数据类型:gydF4y2Ba单gydF4y2Ba|gydF4y2Ba双gydF4y2Ba

输出参数gydF4y2Ba

全部展开gydF4y2Ba

`取向gydF4y2Ba`-方向，将数量从本地导航坐标系旋转到传感器本体坐标系gydF4y2Ba
米gydF4y2Ba-by-1四元数数组(默认)| 3-by-3-by-gydF4y2Ba米gydF4y2Ba数组gydF4y2Ba

可以将数量从局部导航坐标系旋转到主体坐标系的方向，以四元数或数组形式返回。的大小和类型gydF4y2Ba取向gydF4y2Ba这取决于gydF4y2BaOrientationFormatgydF4y2Ba属性设置为gydF4y2Ba“四元数”gydF4y2Ba或gydF4y2Ba旋转矩阵的gydF4y2Ba：gydF4y2Ba

“四元数”gydF4y2Ba——输出是一个gydF4y2Ba米gydF4y2Ba四元数的-by-1向量，具有与输入相同的底层数据类型gydF4y2Ba
旋转矩阵的gydF4y2Ba——输出是一个3 × 3 × -gydF4y2Ba米gydF4y2Ba与输入数据类型相同的旋转矩阵数组gydF4y2Ba

输入样本的数量，gydF4y2BaNgydF4y2Ba，以及gydF4y2BaDecimationFactorgydF4y2Ba属性确定gydF4y2Ba米gydF4y2Ba．gydF4y2Ba

你可以使用gydF4y2Ba取向gydF4y2Ba在一个gydF4y2BarotateframegydF4y2Ba函数将数量从本地导航系统旋转到传感器体坐标系统。gydF4y2Ba

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

`angularVelocitygydF4y2Ba`-传感器本体坐标系中的角速度(rad/s)gydF4y2Ba
米gydF4y2Ba-by-3 array(默认)gydF4y2Ba

在传感器体坐标系中除去陀螺仪偏差后的角速度，单位为rad/s，返回为gydF4y2Ba米gydF4y2Ba3数组。输入样本的数量，gydF4y2BaNgydF4y2Ba，以及gydF4y2BaDecimationFactorgydF4y2Ba属性确定gydF4y2Ba米gydF4y2Ba．gydF4y2Ba

数据类型:gydF4y2Ba单gydF4y2Ba|gydF4y2Ba双gydF4y2Ba

对象的功能gydF4y2Ba

要使用对象函数，请将System对象指定为第一个输入参数。例如，释放system对象的系统资源gydF4y2BaobjgydF4y2Ba，使用这种语法:gydF4y2Ba

发行版(obj)gydF4y2Ba

全部展开gydF4y2Ba

特定于gydF4y2Ba`ahrsfiltergydF4y2Ba`

调优gydF4y2Ba 调优gydF4y2BaahrsfiltergydF4y2Ba参数，以减少估计误差gydF4y2Ba

所有系统对象通用gydF4y2Ba

`一步gydF4y2Ba`	运行gydF4y2Ba系统对象gydF4y2Ba算法gydF4y2Ba
`释放gydF4y2Ba`	释放资源并允许更改gydF4y2Ba系统对象gydF4y2Ba属性值和输入特征gydF4y2Ba
`重置gydF4y2Ba`	重置的内部状态gydF4y2Ba系统对象gydF4y2Ba

例子gydF4y2Ba

全部折叠gydF4y2Ba

估计方向使用gydF4y2Ba`ahrsfiltergydF4y2Ba`

打开实时脚本gydF4y2Ba

加载gydF4y2Barpy_9axisgydF4y2Ba文件，其中包含记录的加速度计、陀螺仪和磁力计传感器数据，这些数据来自一个在pitch(约gydF4y2BaygydF4y2Ba-轴)，然后偏航(绕gydF4y2BazgydF4y2Ba-轴)，然后滚(绕gydF4y2BaxgydF4y2Ba设在)。该文件还包含录音的采样率。gydF4y2Ba

负载gydF4y2Ba“rpy_9axis”gydF4y2BasensorDatagydF4y2BaFsgydF4y2Baaccelerometerreads = sensorData.Acceleration;陀螺仪读数= sensorData.AngularVelocity;磁力计读数= sensorData.MagneticField;gydF4y2Ba

创建一个gydF4y2BaahrsfiltergydF4y2Ba系统对象™gydF4y2BaSampleRategydF4y2Ba设置为传感器数据的采样率。指定抽取因子为2以减少算法的计算成本。gydF4y2Ba

Decim = 2;保险丝= ahrsfilter(gydF4y2Ba“SampleRate”gydF4y2BaFs,gydF4y2Ba“DecimationFactor”gydF4y2Ba, decim);gydF4y2Ba

将加速度计读数、陀螺仪读数和磁力计读数传递给gydF4y2BaahrsfiltergydF4y2Ba对象,gydF4y2Ba保险丝gydF4y2Ba，输出传感器体方向随时间的估计。默认情况下，方向输出为四元数向量。gydF4y2Ba

q =熔断器(加速度计读数，陀螺仪读数，磁力计读数);gydF4y2Ba

方向由将父坐标系旋转到子坐标系所需的角位移定义。用欧拉角表示方向随时间的度数。gydF4y2Ba

ahrsfiltergydF4y2Ba正确估计方向随时间的变化，包括朝南的初始方向。gydF4y2Ba

time = (0:decim:size(加速度计读数，1)-1)/Fs;情节(时间,eulerd (q,gydF4y2Ba“ZYX股票”gydF4y2Ba，gydF4y2Ba“帧”gydF4y2Ba)标题(gydF4y2Ba取向估计的gydF4y2Ba)传说(gydF4y2Baz轴的gydF4y2Ba，gydF4y2Ba“轴”gydF4y2Ba，gydF4y2Ba“轴”gydF4y2Ba) ylabel (gydF4y2Ba的旋转(度)gydF4y2Ba）gydF4y2Ba

图中包含一个轴对象。标题为Orientation Estimate的axis对象包含3个类型为line的对象。这些对象代表z轴y轴x轴。gydF4y2Ba

模拟磁干扰gydF4y2Ba`ahrsFiltergydF4y2Ba`

打开脚本gydF4y2Ba

的性能如何gydF4y2BaahrsfiltergydF4y2Ba系统对象™受磁干扰影响。gydF4y2Ba

负载gydF4y2BaStationaryIMUReadingsgydF4y2Ba，其中包含加速度计，磁力计和陀螺仪读数从一个静止的IMU。gydF4y2Ba

负载gydF4y2Ba“StationaryIMUReadings.mat”gydF4y2BaaccelReadingsgydF4y2BamagReadingsgydF4y2BagyroReadingsgydF4y2BaSampleRategydF4y2BanumSamples = size(accelreads,1);gydF4y2Ba

的gydF4y2BaahrsfiltergydF4y2Ba利用磁场强度来稳定它的方向，以对抗假定的地球恒定磁场。然而，有许多自然和人造物体会输出磁场，这可能会混淆算法。考虑到瞬态磁场的存在，您可以设置gydF4y2BaMagneticDisturbanceNoisegydF4y2Ba的属性gydF4y2BaahrsfiltergydF4y2Ba对象。gydF4y2Ba

创建一个gydF4y2BaahrsfiltergydF4y2Ba对象，并将抽取因子设置为2，并注意默认的预期磁场强度。gydF4y2Ba

Decim = 2;保险丝= ahrsfilter(gydF4y2Ba“SampleRate”gydF4y2BaSampleRate,gydF4y2Ba“DecimationFactor”gydF4y2Ba, decim);gydF4y2Ba

使用姿态和航向参考系统(AHRS)滤波器融合IMU读数，然后可视化传感器主体随时间的方向。方向在开始时波动，大约60秒后稳定。gydF4y2Ba

orientation = FUSE(accelreads, gyroreads, magreads);orientationEulerAngles = eulerd(方向，gydF4y2Ba“ZYX股票”gydF4y2Ba，gydF4y2Ba“帧”gydF4y2Ba）;time = (0:decim:(numSamples-1))'/SampleRate;图(1)图(时间,orientationEulerAngles (: 1),gydF4y2Ba.．.gydF4y2Ba时间,orientationEulerAngles (:, 2),gydF4y2Ba.．.gydF4y2Ba时间,orientationEulerAngles(:, 3))包含(gydF4y2Ba“时间(s)”gydF4y2Ba) ylabel (gydF4y2Ba的旋转(度)gydF4y2Ba)传说(gydF4y2Baz轴的gydF4y2Ba，gydF4y2Ba“轴”gydF4y2Ba，gydF4y2Ba“轴”gydF4y2Ba)标题(gydF4y2Ba“过滤IMU数据”gydF4y2Ba）gydF4y2Ba

模拟磁干扰，通过添加一个瞬态，强磁场的磁场记录在gydF4y2BamagReadingsgydF4y2Ba．想象磁场干扰。gydF4y2Ba

jamStrength = [10,5,2];startStop = (50*SampleRate):(150*SampleRate);jam = 0 (size(magreads));jam(startStop，:) = jamStrength.*ones(nummel (startStop)，3);magreads = magreads + jam;图(2)plot(time, magreads (1:decim:end，:)) xlabel(gydF4y2Ba“时间(s)”gydF4y2Ba) ylabel (gydF4y2Ba“磁场强度(\mu T)”gydF4y2Ba)标题(gydF4y2Ba“模拟干扰磁场”gydF4y2Ba)传说(gydF4y2Baz轴的gydF4y2Ba，gydF4y2Ba“轴”gydF4y2Ba，gydF4y2Ba“轴”gydF4y2Ba）gydF4y2Ba

命令再次运行模拟gydF4y2BamagReadingsgydF4y2Ba有磁干扰。绘制结果并注意方向估计性能的下降。gydF4y2Ba

重置(熔断器)方向=熔断器(accelreads, gyroreads, magreads);orientationEulerAngles = eulerd(方向，gydF4y2Ba“ZYX股票”gydF4y2Ba，gydF4y2Ba“帧”gydF4y2Ba）;图(3)图(时间,orientationEulerAngles (: 1),gydF4y2Ba.．.gydF4y2Ba时间,orientationEulerAngles (:, 2),gydF4y2Ba.．.gydF4y2Ba时间,orientationEulerAngles(:, 3))包含(gydF4y2Ba“时间(s)”gydF4y2Ba) ylabel (gydF4y2Ba的旋转(度)gydF4y2Ba)传说(gydF4y2Baz轴的gydF4y2Ba，gydF4y2Ba“轴”gydF4y2Ba，gydF4y2Ba“轴”gydF4y2Ba)标题(gydF4y2Ba“过滤IMU数据与磁扰动和默认属性”gydF4y2Ba）gydF4y2Ba

磁干扰被AHRS滤波器误读，传感器体方向被错误估计。你可以通过增加gydF4y2BaMagneticDisturbanceNoisegydF4y2Ba财产。增加了gydF4y2BaMagneticDisturbanceNoisegydF4y2Ba属性增加了磁扰动的假设噪声范围，在底层的融合算法中，整个磁强计信号的权重减小gydF4y2BaahrsfiltergydF4y2Ba．gydF4y2Ba

设置gydF4y2BaMagneticDisturbanceNoisegydF4y2Ba来gydF4y2Ba200gydF4y2Ba再运行一次模拟。gydF4y2Ba

的方向估计输出gydF4y2BaahrsfiltergydF4y2Ba精度高，受磁瞬变影响小。但是，由于底层融合算法中磁力计信号的权重较小，算法可能需要更多的时间来重新稳定。gydF4y2Ba

重置(保险丝)保险丝。米一个gnet我cD我年代turb一个nce没有我年代e = 20; orientation = FUSE(accelReadings,gyroReadings,magReadings); orientationEulerAngles = eulerd(orientation,“ZYX股票”gydF4y2Ba，gydF4y2Ba“帧”gydF4y2Ba）;图(4)图(时间,orientationEulerAngles (: 1),gydF4y2Ba.．.gydF4y2Ba时间,orientationEulerAngles (:, 2),gydF4y2Ba.．.gydF4y2Ba时间,orientationEulerAngles(:, 3))包含(gydF4y2Ba“时间(s)”gydF4y2Ba) ylabel (gydF4y2Ba的旋转(度)gydF4y2Ba)传说(gydF4y2Baz轴的gydF4y2Ba，gydF4y2Ba“轴”gydF4y2Ba，gydF4y2Ba“轴”gydF4y2Ba)标题(gydF4y2Ba“经过过滤的IMU数据与磁扰动和修正性质”gydF4y2Ba）gydF4y2Ba

轨道震动9轴IMUgydF4y2Ba

打开脚本gydF4y2Ba

本例使用gydF4y2BaahrsfiltergydF4y2Ba系统对象™融合9轴IMU数据的传感器主体是动摇的。绘制对象与其最终静息位置之间的四元数距离，以可视化性能以及滤波器收敛到正确静息位置的速度。的参数调优gydF4y2BaahrsfiltergydF4y2Ba这样滤波器就能更快地收敛到真实的静止位置。gydF4y2Ba

负载gydF4y2BaIMUReadingsShakengydF4y2Ba进入您当前的工作空间。这些数据是由一个IMU记录的，该IMU被摇动，然后放置在静止位置。将传感器记录的加速度、磁场和角速度可视化。gydF4y2Ba

负载gydF4y2Ba“IMUReadingsShaken”gydF4y2BaaccelReadingsgydF4y2BagyroReadingsgydF4y2BamagReadingsgydF4y2BaSampleRategydF4y2BanumSamples = size(accelreads,1);time = (0:(numSamples-1))'/SampleRate;图(1)subplot(3,1,1) plot(time, accelreads)标题(gydF4y2Ba加速度计读的gydF4y2Ba) ylabel (gydF4y2Ba“加速度(m / s ^ 2)”gydF4y2Ba) subplot(3,1,2) plot(时间，magreads)gydF4y2Ba“磁强计阅读”gydF4y2Ba) ylabel (gydF4y2Ba“磁场(\muT)”gydF4y2Ba) subplot(3,1,3) plot(时间，陀螺仪读数)gydF4y2Ba“陀螺仪阅读”gydF4y2Ba) ylabel (gydF4y2Ba角速度(rad/s)gydF4y2Ba)包含(gydF4y2Ba“时间(s)”gydF4y2Ba）gydF4y2Ba

创建一个gydF4y2BaahrsfiltergydF4y2Ba然后融合IMU数据来确定方向。方向作为四元数向量返回;将四元数转换为以度为单位的欧拉角。通过绘制在每个时间步骤将全局坐标系旋转到传感器体坐标系所需的欧拉角，可以可视化传感器体随时间的方向。gydF4y2Ba

保险丝= ahrsfilter(gydF4y2Ba“SampleRate”gydF4y2Ba, SampleRate);定向=熔断器(accelreads, gyroreads, magreads);orientationEulerAngles = eulerd(方向，gydF4y2Ba“ZYX股票”gydF4y2Ba，gydF4y2Ba“帧”gydF4y2Ba）;图(2)图(时间,orientationEulerAngles (: 1),gydF4y2Ba.．.gydF4y2Ba时间,orientationEulerAngles (:, 2),gydF4y2Ba.．.gydF4y2Ba时间,orientationEulerAngles(:, 3))包含(gydF4y2Ba“时间(s)”gydF4y2Ba) ylabel (gydF4y2Ba的旋转(度)gydF4y2Ba)标题(gydF4y2Ba“随时间变化的取向”gydF4y2Ba)传说(gydF4y2Ba'绕z轴旋转'gydF4y2Ba，gydF4y2Ba.．.gydF4y2Ba“绕y轴旋转”gydF4y2Ba，gydF4y2Ba.．.gydF4y2Ba“绕x轴旋转”gydF4y2Ba）gydF4y2Ba

在IMU的记录中，震动大约在6秒后停止。确定静止方向，这样你就可以描述gydF4y2BaahrsfiltergydF4y2Ba是收敛的。gydF4y2Ba

要确定静止方向，请计算最后四秒的磁场和加速度的平均值，然后使用gydF4y2BaecompassgydF4y2Ba函数来融合数据。gydF4y2Ba

可视化四元数距离静止位置随时间的变化。gydF4y2Ba

restingOrientation = ecompass(mean(accelreads (6*SampleRate:end，:))，gydF4y2Ba.．.gydF4y2Ba意思是(magReadings (6 * SampleRate:最终,:)));图(3)plot(time,rad2deg(dist(restingOrientation,orientation)) holdgydF4y2Ba在gydF4y2Ba包含(gydF4y2Ba“时间(s)”gydF4y2Ba) ylabel (gydF4y2Ba四元数距离(度)gydF4y2Ba）gydF4y2Ba

修改默认值gydF4y2BaahrsfiltergydF4y2Ba属性，以便过滤器更快地收敛到重力。增加了gydF4y2BaGyroscopeDriftNoisegydF4y2Ba来gydF4y2Ba1)依照gydF4y2Ba减少gydF4y2BaLinearAccelerationNoisegydF4y2Ba来gydF4y2Ba1的军医gydF4y2Ba．这说明gydF4y2BaahrsfiltergydF4y2Ba算法加权陀螺仪数据较少和加速度计数据更多。因为gydF4y2Ba加速度计gydF4y2Ba数据提供了稳定一致的重力矢量，得到的方向收敛更快。gydF4y2Ba

重置过滤器，融合数据，并绘制结果。gydF4y2Ba

保险丝。线性加速度噪声= 1e-4;保险丝。Gyro年代copeDr如果t没有我年代e = 1e-2; reset(fuse) orientation = fuse(accelReadings,gyroReadings,magReadings); figure(3) plot(time,rad2deg(dist(restingOrientation,orientation))) legend('默认AHRS过滤器'gydF4y2Ba，gydF4y2Ba“调谐AHRS滤波器”gydF4y2Ba）gydF4y2Ba

算法gydF4y2Ba

全部展开gydF4y2Ba

注意:以下算法只适用于NED参考系。gydF4y2Ba

的gydF4y2BaahrsfiltergydF4y2Ba中描述的九轴卡尔曼滤波器结构gydF4y2Ba［1］gydF4y2Ba．该算法尝试跟踪方位误差、陀螺仪偏移量、线性加速度和磁扰动，输出最终方位和角速度。间接卡尔曼滤波器不是直接跟踪方向，而是对误差过程进行建模，gydF4y2BaxgydF4y2Ba，递归更新:gydF4y2Ba

${xgydF4y2Ba}_{kgydF4y2Ba} ＝gydF4y2Ba ［gydF4y2Ba \begin{matrix} {θgydF4y2Ba}_{kgydF4y2Ba} \\ {bgydF4y2Ba}_{kgydF4y2Ba} \\ {一个gydF4y2Ba}_{kgydF4y2Ba} \\ {dgydF4y2Ba}_{kgydF4y2Ba} \end{matrix} ］gydF4y2Ba ＝gydF4y2Ba {FgydF4y2Ba}_{kgydF4y2Ba} ［gydF4y2Ba \begin{matrix} {θgydF4y2Ba}_{kgydF4y2Ba -gydF4y2Ba 1gydF4y2Ba} \\ {bgydF4y2Ba}_{kgydF4y2Ba -gydF4y2Ba 1gydF4y2Ba} \\ {一个gydF4y2Ba}_{kgydF4y2Ba -gydF4y2Ba 1gydF4y2Ba} \\ {dgydF4y2Ba}_{kgydF4y2Ba -gydF4y2Ba 1gydF4y2Ba} \end{matrix} ］gydF4y2Ba +gydF4y2Ba {wgydF4y2Ba}_{kgydF4y2Ba}$

在哪里gydF4y2BaxgydF4y2Ba_kgydF4y2Ba是一个12乘1的向量，由:gydF4y2Ba

θgydF4y2Ba_kgydF4y2Ba——3 × 1方向误差向量，以度为单位，在时间上gydF4y2BakgydF4y2Ba
bgydF4y2Ba_kgydF4y2Ba——3 × 1陀螺仪零角速率偏置矢量，单位deg/s，时间gydF4y2BakgydF4y2Ba
一个gydF4y2Ba_kgydF4y2Ba——在传感器帧中测量的3乘1加速度误差矢量，单位为g，在时间上gydF4y2BakgydF4y2Ba
dgydF4y2Ba_kgydF4y2Ba—在传感器框架中测量的3 × 1磁扰动误差矢量，单位为µT，时间gydF4y2BakgydF4y2Ba

在哪里gydF4y2BawgydF4y2Ba_kgydF4y2Ba是一个12乘1的加性噪声向量，和gydF4y2BaFgydF4y2Ba_kgydF4y2Ba是状态转换模型。gydF4y2Ba

因为gydF4y2BaxgydF4y2Ba_kgydF4y2Ba定义为错误过程，则gydF4y2Ba先天的gydF4y2Ba估计总是零，因此状态转移模型，gydF4y2BaFgydF4y2Ba_kgydF4y2Ba，为零。这一见解导致了以下标准卡尔曼方程的缩减:gydF4y2Ba

标准卡尔曼方程:gydF4y2Ba

$\begin{array}{l} {xgydF4y2Ba}_{kgydF4y2Ba}^{-gydF4y2Ba} ＝gydF4y2Ba {FgydF4y2Ba}_{kgydF4y2Ba} {xgydF4y2Ba}_{kgydF4y2Ba -gydF4y2Ba 1gydF4y2Ba}^{+gydF4y2Ba} \\ {PgydF4y2Ba}_{kgydF4y2Ba}^{-gydF4y2Ba} ＝gydF4y2Ba {FgydF4y2Ba}_{kgydF4y2Ba} {PgydF4y2Ba}_{kgydF4y2Ba -gydF4y2Ba 1gydF4y2Ba}^{+gydF4y2Ba} {FgydF4y2Ba}_{kgydF4y2Ba}^{TgydF4y2Ba} +gydF4y2Ba {问gydF4y2Ba}_{kgydF4y2Ba} \\ {ygydF4y2Ba}_{kgydF4y2Ba} ＝gydF4y2Ba {zgydF4y2Ba}_{kgydF4y2Ba} -gydF4y2Ba {HgydF4y2Ba}_{kgydF4y2Ba} {xgydF4y2Ba}_{kgydF4y2Ba}^{-gydF4y2Ba} \\ {年代gydF4y2Ba}_{kgydF4y2Ba} ＝gydF4y2Ba {RgydF4y2Ba}_{kgydF4y2Ba} +gydF4y2Ba {HgydF4y2Ba}_{kgydF4y2Ba} {PgydF4y2Ba}_{kgydF4y2Ba}^{-gydF4y2Ba} {HgydF4y2Ba}_{kgydF4y2Ba}^{TgydF4y2Ba} \\ {KgydF4y2Ba}_{kgydF4y2Ba} ＝gydF4y2Ba {PgydF4y2Ba}_{kgydF4y2Ba}^{-gydF4y2Ba} {HgydF4y2Ba}_{kgydF4y2Ba}^{TgydF4y2Ba} {（gydF4y2Ba {年代gydF4y2Ba}_{kgydF4y2Ba} ）gydF4y2Ba}^{-gydF4y2Ba 1gydF4y2Ba} \\ {xgydF4y2Ba}_{kgydF4y2Ba}^{+gydF4y2Ba} ＝gydF4y2Ba {xgydF4y2Ba}_{kgydF4y2Ba}^{-gydF4y2Ba} +gydF4y2Ba {KgydF4y2Ba}_{kgydF4y2Ba} {ygydF4y2Ba}_{kgydF4y2Ba} \\ {PgydF4y2Ba}_{kgydF4y2Ba}^{+gydF4y2Ba} ＝gydF4y2Ba {PgydF4y2Ba}_{kgydF4y2Ba}^{-gydF4y2Ba} -gydF4y2Ba {KgydF4y2Ba}_{kgydF4y2Ba} {HgydF4y2Ba}_{kgydF4y2Ba} {PgydF4y2Ba}_{kgydF4y2Ba}^{-gydF4y2Ba} \end{array}$

本算法使用的卡尔曼方程:gydF4y2Ba

$\begin{array}{l} {xgydF4y2Ba}_{kgydF4y2Ba}^{-gydF4y2Ba} ＝gydF4y2Ba 0gydF4y2Ba \\ {PgydF4y2Ba}_{kgydF4y2Ba}^{-gydF4y2Ba} ＝gydF4y2Ba {问gydF4y2Ba}_{kgydF4y2Ba} \\ {ygydF4y2Ba}_{kgydF4y2Ba} ＝gydF4y2Ba {zgydF4y2Ba}_{kgydF4y2Ba} \\ {年代gydF4y2Ba}_{kgydF4y2Ba} ＝gydF4y2Ba {RgydF4y2Ba}_{kgydF4y2Ba} +gydF4y2Ba {HgydF4y2Ba}_{kgydF4y2Ba} {PgydF4y2Ba}_{kgydF4y2Ba}^{-gydF4y2Ba} {HgydF4y2Ba}_{kgydF4y2Ba}^{TgydF4y2Ba} \\ {KgydF4y2Ba}_{kgydF4y2Ba} ＝gydF4y2Ba {PgydF4y2Ba}_{kgydF4y2Ba}^{-gydF4y2Ba} {HgydF4y2Ba}_{kgydF4y2Ba}^{TgydF4y2Ba} {（gydF4y2Ba {年代gydF4y2Ba}_{kgydF4y2Ba} ）gydF4y2Ba}^{-gydF4y2Ba 1gydF4y2Ba} \\ {xgydF4y2Ba}_{kgydF4y2Ba}^{+gydF4y2Ba} ＝gydF4y2Ba {KgydF4y2Ba}_{kgydF4y2Ba} {ygydF4y2Ba}_{kgydF4y2Ba} \\ {PgydF4y2Ba}_{kgydF4y2Ba}^{+gydF4y2Ba} ＝gydF4y2Ba {PgydF4y2Ba}_{kgydF4y2Ba}^{-gydF4y2Ba} -gydF4y2Ba {KgydF4y2Ba}_{kgydF4y2Ba} {HgydF4y2Ba}_{kgydF4y2Ba} {PgydF4y2Ba}_{kgydF4y2Ba}^{-gydF4y2Ba} \end{array}$

地点:gydF4y2Ba

xgydF4y2Ba_kgydF4y2Ba^−gydF4y2Ba——预测(gydF4y2Ba先天的gydF4y2Ba)状态估计;错误过程gydF4y2Ba
PgydF4y2Ba_kgydF4y2Ba^−gydF4y2Ba——预测(gydF4y2Ba先天的gydF4y2Ba估计协方差gydF4y2Ba
ygydF4y2Ba_kgydF4y2Ba——创新gydF4y2Ba
年代gydF4y2Ba_kgydF4y2Ba——创新协方差gydF4y2Ba
KgydF4y2Ba_kgydF4y2Ba——卡尔曼增益gydF4y2Ba
xgydF4y2Ba_kgydF4y2Ba^+gydF4y2Ba——已更新(gydF4y2Ba后验gydF4y2Ba)状态估计gydF4y2Ba
PgydF4y2Ba_kgydF4y2Ba^+gydF4y2Ba——已更新(gydF4y2Ba后验gydF4y2Ba估计协方差gydF4y2Ba

kgydF4y2Ba表示迭代，上标gydF4y2Ba^+gydF4y2Ba代表一个gydF4y2Ba后验gydF4y2Ba估计，和上标gydF4y2Ba^−gydF4y2Ba代表一个gydF4y2Ba先天的gydF4y2Ba估计。gydF4y2Ba

图形和以下步骤描述了通过该算法进行的单帧迭代。gydF4y2Ba

在第一次迭代之前，gydF4y2BaaccelReadingsgydF4y2Ba，gydF4y2BagyroReadingsgydF4y2Ba,gydF4y2BamagReadingsgydF4y2Ba输入被分块gydF4y2BaDecimationFactorgydF4y2Ba3帧。对于每个块，算法使用最新的加速度计和磁力计读数对应陀螺仪读数块。gydF4y2Ba

详细的概述gydF4y2Ba

对算法的各个阶段进行了详细的讲解概述。gydF4y2Ba

模型gydF4y2Ba

该算法将加速度和角度变化建模为线性过程。gydF4y2Ba

预测方向gydF4y2Ba

当前帧的方向是通过首先估计前一帧的角度变化来预测的:gydF4y2Ba

$ΔgydF4y2Ba {φgydF4y2Ba}_{NgydF4y2Ba \timesgydF4y2Ba 3.gydF4y2Ba} ＝gydF4y2Ba \frac{（gydF4y2Ba ggydF4y2Ba ygydF4y2Ba rgydF4y2Ba ogydF4y2Ba RgydF4y2Ba egydF4y2Ba 一个gydF4y2Ba dgydF4y2Ba 我gydF4y2Ba ngydF4y2Ba ggydF4y2Ba {年代gydF4y2Ba}_{NgydF4y2Ba \timesgydF4y2Ba 3.gydF4y2Ba} -gydF4y2Ba ggydF4y2Ba ygydF4y2Ba rgydF4y2Ba ogydF4y2Ba OgydF4y2Ba fgydF4y2Ba fgydF4y2Ba 年代gydF4y2Ba egydF4y2Ba {tgydF4y2Ba}_{1gydF4y2Ba \timesgydF4y2Ba 3.gydF4y2Ba} ）gydF4y2Ba}{fgydF4y2Ba 年代gydF4y2Ba}$

在哪里gydF4y2BaNgydF4y2Ba是否指定了抽取因子gydF4y2BaDecimationFactorgydF4y2Ba财产和gydF4y2BafsgydF4y2Ba抽样率是否由gydF4y2BaSampleRategydF4y2Ba财产。gydF4y2Ba

方法将角度变化转换为四元数gydF4y2BarotvecgydF4y2Ba四元数gydF4y2Ba施工的语法:gydF4y2Ba

$ΔgydF4y2Ba {问gydF4y2Ba}_{NgydF4y2Ba \timesgydF4y2Ba 1gydF4y2Ba} ＝gydF4y2Ba 四元数gydF4y2Ba （gydF4y2Ba ΔgydF4y2Ba {φgydF4y2Ba}_{NgydF4y2Ba \timesgydF4y2Ba 3.gydF4y2Ba} ，gydF4y2Ba ＇gydF4y2Ba rgydF4y2Ba ogydF4y2Ba tgydF4y2Ba vgydF4y2Ba egydF4y2Ba cgydF4y2Ba ＇gydF4y2Ba ）gydF4y2Ba$

之前的方向估计通过旋转Δ来更新gydF4y2Ba问gydF4y2Ba：gydF4y2Ba

${问gydF4y2Ba}_{1gydF4y2Ba \timesgydF4y2Ba 1gydF4y2Ba}^{-gydF4y2Ba} ＝gydF4y2Ba （gydF4y2Ba {问gydF4y2Ba}_{1gydF4y2Ba \timesgydF4y2Ba 1gydF4y2Ba}^{+gydF4y2Ba} ）gydF4y2Ba （gydF4y2Ba {\prodgydF4y2Ba}_{ngydF4y2Ba ＝gydF4y2Ba 1gydF4y2Ba}^{NgydF4y2Ba} ΔgydF4y2Ba {问gydF4y2Ba}_{ngydF4y2Ba} ）gydF4y2Ba$

在第一次迭代中，方向估计，gydF4y2Ba问gydF4y2Ba^−gydF4y2Ba初始化为gydF4y2BaecompassgydF4y2Ba．gydF4y2Ba

从方向估计重力gydF4y2Ba

重力向量被解释为四元数的第三列，gydF4y2Ba问gydF4y2Ba^−gydF4y2Ba，旋转矩阵形式为:gydF4y2Ba

${ggydF4y2Ba}_{1gydF4y2Ba \timesgydF4y2Ba 3.gydF4y2Ba} ＝gydF4y2Ba {（gydF4y2Ba rgydF4y2Ba PgydF4y2Ba rgydF4y2Ba 我gydF4y2Ba ogydF4y2Ba rgydF4y2Ba （gydF4y2Ba ：gydF4y2Ba ，gydF4y2Ba 3.gydF4y2Ba ）gydF4y2Ba ）gydF4y2Ba}^{TgydF4y2Ba}$

看到gydF4y2Ba［1］gydF4y2Ba对于第三列的解释gydF4y2BarPriorgydF4y2Ba可以解释为重力向量。gydF4y2Ba

从加速度估计重力gydF4y2Ba

第二次重力矢量估计是通过从加速度计读数中减去前一次迭代的衰减线性加速度估计得出的:gydF4y2Ba

$ggydF4y2Ba 一个gydF4y2Ba cgydF4y2Ba cgydF4y2Ba egydF4y2Ba {lgydF4y2Ba}_{1gydF4y2Ba \timesgydF4y2Ba 3.gydF4y2Ba} ＝gydF4y2Ba 一个gydF4y2Ba cgydF4y2Ba cgydF4y2Ba egydF4y2Ba lgydF4y2Ba RgydF4y2Ba egydF4y2Ba 一个gydF4y2Ba dgydF4y2Ba 我gydF4y2Ba ngydF4y2Ba ggydF4y2Ba {年代gydF4y2Ba}_{1gydF4y2Ba \timesgydF4y2Ba 3.gydF4y2Ba} -gydF4y2Ba lgydF4y2Ba 我gydF4y2Ba ngydF4y2Ba 一个gydF4y2Ba cgydF4y2Ba cgydF4y2Ba egydF4y2Ba lgydF4y2Ba pgydF4y2Ba rgydF4y2Ba 我gydF4y2Ba ogydF4y2Ba {rgydF4y2Ba}_{1gydF4y2Ba \timesgydF4y2Ba 3.gydF4y2Ba}$

估算地球的磁矢量gydF4y2Ba

地球的磁矢量是通过旋转以前迭代中估计的磁矢量来估计的gydF4y2Ba先天的gydF4y2Ba方向估计，旋转矩阵形式:gydF4y2Ba

$米gydF4y2Ba GgydF4y2Ba ygydF4y2Ba rgydF4y2Ba {ogydF4y2Ba}_{1gydF4y2Ba \timesgydF4y2Ba 3.gydF4y2Ba} ＝gydF4y2Ba {（gydF4y2Ba （gydF4y2Ba rgydF4y2Ba PgydF4y2Ba rgydF4y2Ba 我gydF4y2Ba ogydF4y2Ba rgydF4y2Ba ）gydF4y2Ba （gydF4y2Ba {米gydF4y2Ba}^{TgydF4y2Ba} ）gydF4y2Ba ）gydF4y2Ba}^{TgydF4y2Ba}$

误差模型gydF4y2Ba

错误模型结合了两个不同之处:gydF4y2Ba

加速度计读数的重力估计值和陀螺仪读数的重力估计值之间的差异:gydF4y2Ba ${zgydF4y2Ba}_{ggydF4y2Ba} ＝gydF4y2Ba ggydF4y2Ba -gydF4y2Ba ggydF4y2Ba 一个gydF4y2Ba cgydF4y2Ba cgydF4y2Ba egydF4y2Ba lgydF4y2Ba$
陀螺仪读数的磁矢量估计值与磁力计的磁矢量估计值之间的差异:gydF4y2Ba ${zgydF4y2Ba}_{米gydF4y2Ba} ＝gydF4y2Ba 米gydF4y2Ba GgydF4y2Ba ygydF4y2Ba rgydF4y2Ba ogydF4y2Ba -gydF4y2Ba 米gydF4y2Ba 一个gydF4y2Ba ggydF4y2Ba RgydF4y2Ba egydF4y2Ba 一个gydF4y2Ba dgydF4y2Ba 我gydF4y2Ba ngydF4y2Ba ggydF4y2Ba 年代gydF4y2Ba$

磁强计正确gydF4y2Ba

磁力计正确估计磁矢量估计中的误差，并检测磁干扰。gydF4y2Ba

磁力计扰动误差gydF4y2Ba

磁扰动误差由与磁矢量相关的卡尔曼增益与误差信号的矩阵乘法计算:gydF4y2Ba

$米gydF4y2Ba EgydF4y2Ba rgydF4y2Ba rgydF4y2Ba ogydF4y2Ba {rgydF4y2Ba}_{3.gydF4y2Ba \timesgydF4y2Ba 1gydF4y2Ba} ＝gydF4y2Ba {（gydF4y2Ba （gydF4y2Ba KgydF4y2Ba {（gydF4y2Ba 10:12gydF4y2Ba ，gydF4y2Ba ：gydF4y2Ba ）gydF4y2Ba}_{3.gydF4y2Ba \timesgydF4y2Ba 6gydF4y2Ba} ）gydF4y2Ba {（gydF4y2Ba {zgydF4y2Ba}_{1gydF4y2Ba \timesgydF4y2Ba 6gydF4y2Ba} ）gydF4y2Ba}^{TgydF4y2Ba} ）gydF4y2Ba}^{TgydF4y2Ba}$

卡尔曼增益，gydF4y2BaKgydF4y2Ba，为本次迭代计算的卡尔曼增益。gydF4y2Ba

磁干扰检测gydF4y2Ba

磁干扰是通过验证检测到的磁干扰的功率小于或等于预期磁场强度的四倍来确定的:gydF4y2Ba

$tgydF4y2Ba fgydF4y2Ba ＝gydF4y2Ba ｛gydF4y2Ba \begin{matrix} 真正的gydF4y2Ba \\ 假gydF4y2Ba \end{matrix} \begin{matrix} 如果gydF4y2Ba \\ 其他的gydF4y2Ba \end{matrix} \begin{matrix} {\sumgydF4y2Ba |gydF4y2Ba 米gydF4y2Ba EgydF4y2Ba rgydF4y2Ba rgydF4y2Ba ogydF4y2Ba rgydF4y2Ba |gydF4y2Ba}^{2gydF4y2Ba} >gydF4y2Ba （gydF4y2Ba 4gydF4y2Ba ）gydF4y2Ba {（gydF4y2Ba ExpectedMagneticFieldStrengthgydF4y2Ba ）gydF4y2Ba}^{2gydF4y2Ba} \end{matrix}$

ExpectedMagneticFieldStrengthgydF4y2Ba的属性。gydF4y2BaahrsfiltergydF4y2Ba．gydF4y2Ba

卡尔曼滤波方程gydF4y2Ba

卡尔曼方程使用的重力估计值来自陀螺仪读数，gydF4y2BaggydF4y2Ba，由陀螺仪读数得出的磁矢量估计值，gydF4y2BamGyrogydF4y2Ba，以及误差过程的观察，gydF4y2BazgydF4y2Ba，更新卡尔曼增益和中间协方差矩阵。将卡尔曼增益应用于误差信号，gydF4y2BazgydF4y2Ba，以输出gydF4y2Ba后验gydF4y2Ba误差估计,gydF4y2BaxgydF4y2Ba^+gydF4y2Ba．gydF4y2Ba

观察模型gydF4y2Ba

观测模型映射了1 × 3的观测状态，gydF4y2BaggydF4y2Ba而且gydF4y2BamGyrogydF4y2Ba，变成6 × 12的真态，gydF4y2BaHgydF4y2Ba．gydF4y2Ba

观测模型构造为:gydF4y2Ba

${HgydF4y2Ba}_{3.gydF4y2Ba \timesgydF4y2Ba 9gydF4y2Ba} ＝gydF4y2Ba ［gydF4y2Ba \begin{matrix} 0gydF4y2Ba & {ggydF4y2Ba}_{zgydF4y2Ba} & -gydF4y2Ba {ggydF4y2Ba}_{ygydF4y2Ba} & 0gydF4y2Ba & -gydF4y2Ba κgydF4y2Ba {ggydF4y2Ba}_{zgydF4y2Ba} & κgydF4y2Ba {ggydF4y2Ba}_{ygydF4y2Ba} & 1gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba \\ -gydF4y2Ba {ggydF4y2Ba}_{zgydF4y2Ba} & 0gydF4y2Ba & {ggydF4y2Ba}_{xgydF4y2Ba} & κgydF4y2Ba {ggydF4y2Ba}_{zgydF4y2Ba} & 0gydF4y2Ba & -gydF4y2Ba κgydF4y2Ba {ggydF4y2Ba}_{xgydF4y2Ba} & 0gydF4y2Ba & 1gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba \\ {ggydF4y2Ba}_{ygydF4y2Ba} & -gydF4y2Ba {ggydF4y2Ba}_{xgydF4y2Ba} & 0gydF4y2Ba & -gydF4y2Ba κgydF4y2Ba {ggydF4y2Ba}_{ygydF4y2Ba} & κgydF4y2Ba {ggydF4y2Ba}_{xgydF4y2Ba} & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 1gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba \\ 0gydF4y2Ba & {米gydF4y2Ba}_{zgydF4y2Ba} & -gydF4y2Ba {米gydF4y2Ba}_{ygydF4y2Ba} & 0gydF4y2Ba & -gydF4y2Ba κgydF4y2Ba {米gydF4y2Ba}_{zgydF4y2Ba} & -gydF4y2Ba κgydF4y2Ba {米gydF4y2Ba}_{ygydF4y2Ba} & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & -gydF4y2Ba 1gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba \\ -gydF4y2Ba {米gydF4y2Ba}_{zgydF4y2Ba} & 0gydF4y2Ba & {米gydF4y2Ba}_{xgydF4y2Ba} & κgydF4y2Ba {米gydF4y2Ba}_{zgydF4y2Ba} & 0gydF4y2Ba & -gydF4y2Ba κgydF4y2Ba {米gydF4y2Ba}_{xgydF4y2Ba} & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & -gydF4y2Ba 1gydF4y2Ba & 0gydF4y2Ba \\ {米gydF4y2Ba}_{ygydF4y2Ba} & -gydF4y2Ba {米gydF4y2Ba}_{xgydF4y2Ba} & 0gydF4y2Ba & -gydF4y2Ba κgydF4y2Ba {米gydF4y2Ba}_{ygydF4y2Ba} & κgydF4y2Ba {米gydF4y2Ba}_{xgydF4y2Ba} & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & -gydF4y2Ba 1gydF4y2Ba \end{matrix} ］gydF4y2Ba$

在哪里gydF4y2BaggydF4y2Ba_xgydF4y2Ba，gydF4y2BaggydF4y2Ba_ygydF4y2Ba,gydF4y2BaggydF4y2Ba_zgydF4y2Ba是gydF4y2BaxgydF4y2Ba-，gydF4y2BaygydF4y2Ba- - - - - -,gydF4y2BazgydF4y2Ba的重力矢量的元素gydF4y2Ba先天的gydF4y2Ba分别为取向。gydF4y2Ba米gydF4y2Ba_xgydF4y2Ba，gydF4y2Ba米gydF4y2Ba_ygydF4y2Ba,gydF4y2Ba米gydF4y2Ba_zgydF4y2Ba是gydF4y2BaxgydF4y2Ba-，gydF4y2BaygydF4y2Ba- - - - - -,gydF4y2BazgydF4y2Ba-磁矢量的元素，由gydF4y2Ba先天的gydF4y2Ba分别为取向。gydF4y2BaκgydF4y2Ba常数是由gydF4y2BaSampleRategydF4y2Ba而且gydF4y2BaDecimationFactorgydF4y2Ba属性:gydF4y2BaκgydF4y2Ba＝gydF4y2BaDecimationFactorgydF4y2Ba/gydF4y2BaSampleRategydF4y2Ba．gydF4y2Ba

第7.3和7.4节gydF4y2Ba［1］gydF4y2Ba对于观测模型的推导。gydF4y2Ba

创新协方差gydF4y2Ba

创新协方差是一个6乘6的矩阵，用于跟踪测量中的可变性。创新协方差矩阵计算为:gydF4y2Ba

${年代gydF4y2Ba}_{6gydF4y2Ba xgydF4y2Ba 6gydF4y2Ba} ＝gydF4y2Ba {RgydF4y2Ba}_{6gydF4y2Ba xgydF4y2Ba 6gydF4y2Ba} +gydF4y2Ba （gydF4y2Ba {HgydF4y2Ba}_{6gydF4y2Ba xgydF4y2Ba 12gydF4y2Ba} ）gydF4y2Ba （gydF4y2Ba {PgydF4y2Ba}_{_{12gydF4y2Ba xgydF4y2Ba 12gydF4y2Ba}}^{-gydF4y2Ba} ）gydF4y2Ba {（gydF4y2Ba {HgydF4y2Ba}_{6gydF4y2Ba xgydF4y2Ba 12gydF4y2Ba} ）gydF4y2Ba}^{TgydF4y2Ba}$

在哪里gydF4y2Ba

HgydF4y2Ba观察模型是矩阵吗gydF4y2Ba
PgydF4y2Ba^−gydF4y2Ba是否预测(gydF4y2Ba先天的gydF4y2Ba)对前一次迭代计算的观测模型协方差的估计gydF4y2Ba
RgydF4y2Ba为观测模型噪声的协方差，计算公式为:gydF4y2Ba

${RgydF4y2Ba}_{6gydF4y2Ba \timesgydF4y2Ba 6gydF4y2Ba} ＝gydF4y2Ba ［gydF4y2Ba \begin{matrix} 一个gydF4y2Ba cgydF4y2Ba cgydF4y2Ba egydF4y2Ba {lgydF4y2Ba}_{噪音gydF4y2Ba} & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba \\ 0gydF4y2Ba & 一个gydF4y2Ba cgydF4y2Ba cgydF4y2Ba egydF4y2Ba {lgydF4y2Ba}_{噪音gydF4y2Ba} & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba \\ 0gydF4y2Ba & 0gydF4y2Ba & 一个gydF4y2Ba cgydF4y2Ba cgydF4y2Ba egydF4y2Ba {lgydF4y2Ba}_{噪音gydF4y2Ba} & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba \\ 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 米gydF4y2Ba 一个gydF4y2Ba {ggydF4y2Ba}_{噪音gydF4y2Ba} & 0gydF4y2Ba & 0gydF4y2Ba \\ 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 米gydF4y2Ba 一个gydF4y2Ba {ggydF4y2Ba}_{噪音gydF4y2Ba} & 0gydF4y2Ba \\ 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 米gydF4y2Ba 一个gydF4y2Ba {ggydF4y2Ba}_{噪音gydF4y2Ba} \end{matrix} ］gydF4y2Ba$

在哪里gydF4y2Ba

$一个gydF4y2Ba cgydF4y2Ba cgydF4y2Ba egydF4y2Ba {lgydF4y2Ba}_{噪音gydF4y2Ba} ＝gydF4y2Ba AccelerometerNoisegydF4y2Ba +gydF4y2Ba LinearAccelerationNoisegydF4y2Ba +gydF4y2Ba {κgydF4y2Ba}^{2gydF4y2Ba} （gydF4y2Ba GyroscopeDriftNoisegydF4y2Ba +gydF4y2Ba GyroscopeNoisegydF4y2Ba ）gydF4y2Ba$

而且gydF4y2Ba

$米gydF4y2Ba 一个gydF4y2Ba {ggydF4y2Ba}_{噪音gydF4y2Ba} ＝gydF4y2Ba MagnetometerNoisegydF4y2Ba +gydF4y2Ba MagneticDisturbanceNoisegydF4y2Ba +gydF4y2Ba {κgydF4y2Ba}^{2gydF4y2Ba} （gydF4y2Ba GyroscopeDriftNoisegydF4y2Ba +gydF4y2Ba GyroscopeNoisegydF4y2Ba ）gydF4y2Ba$

以下属性定义了观测模型噪声方差:gydF4y2Ba
- κgydF4y2Ba- - -gydF4y2BaDecimationFactorgydF4y2Ba/gydF4y2BaSampleRategydF4y2Ba
- AccelerometerNoisegydF4y2Ba
- LinearAccelerationNoisegydF4y2Ba
- GyroscopeDriftNoisegydF4y2Ba
- GyroscopeNoisegydF4y2Ba
- MagneticDisturbanceNoisegydF4y2Ba
- MagnetometerNoisegydF4y2Ba

更新误差估计协方差gydF4y2Ba

误差估计协方差是一个12乘12的矩阵，用于跟踪状态的可变性。gydF4y2Ba

误差估计协方差矩阵更新为:gydF4y2Ba

${PgydF4y2Ba}_{_{12gydF4y2Ba \timesgydF4y2Ba 12gydF4y2Ba}}^{+gydF4y2Ba} ＝gydF4y2Ba {PgydF4y2Ba}_{_{12gydF4y2Ba \timesgydF4y2Ba 12gydF4y2Ba}}^{-gydF4y2Ba} -gydF4y2Ba （gydF4y2Ba {KgydF4y2Ba}_{12gydF4y2Ba \timesgydF4y2Ba 6gydF4y2Ba} ）gydF4y2Ba （gydF4y2Ba {HgydF4y2Ba}_{6gydF4y2Ba \timesgydF4y2Ba 12gydF4y2Ba} ）gydF4y2Ba （gydF4y2Ba {PgydF4y2Ba}_{_{12gydF4y2Ba \timesgydF4y2Ba 12gydF4y2Ba}}^{-gydF4y2Ba} ）gydF4y2Ba$

在哪里gydF4y2BaKgydF4y2Ba为卡尔曼增益，gydF4y2BaHgydF4y2Ba是测量矩阵，和gydF4y2BaPgydF4y2Ba^−gydF4y2Ba是在前一次迭代中计算的误差估计协方差。gydF4y2Ba

预测误差估计协方差gydF4y2Ba

误差估计协方差是一个12乘12的矩阵，用于跟踪状态的可变性。的gydF4y2Ba先天的gydF4y2Ba误差估计协方差，gydF4y2BaPgydF4y2Ba^−gydF4y2Ba，设为过程噪声协方差，gydF4y2Ba问gydF4y2Ba，在前一次迭代中确定。gydF4y2Ba问gydF4y2Ba是作为函数计算的gydF4y2Ba后验gydF4y2Ba误差估计协方差，gydF4y2BaPgydF4y2Ba^+gydF4y2Ba．在计算gydF4y2Ba问gydF4y2Ba时，假设交叉相关项与自相关项相比可以忽略不计，并设为零:gydF4y2Ba

$问gydF4y2Ba ＝gydF4y2Ba ［gydF4y2Ba \begin{matrix} {PgydF4y2Ba}^{+gydF4y2Ba} （gydF4y2Ba 1gydF4y2Ba ）gydF4y2Ba +gydF4y2Ba {κgydF4y2Ba}^{2gydF4y2Ba} （gydF4y2Ba {PgydF4y2Ba}^{+gydF4y2Ba} （gydF4y2Ba 40gydF4y2Ba ）gydF4y2Ba +gydF4y2Ba βgydF4y2Ba +gydF4y2Ba ηgydF4y2Ba ）gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & -gydF4y2Ba κgydF4y2Ba （gydF4y2Ba {PgydF4y2Ba}^{+gydF4y2Ba} （gydF4y2Ba 40gydF4y2Ba ）gydF4y2Ba +gydF4y2Ba βgydF4y2Ba ）gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba \\ 0gydF4y2Ba & {PgydF4y2Ba}^{+gydF4y2Ba} （gydF4y2Ba 14gydF4y2Ba ）gydF4y2Ba +gydF4y2Ba {κgydF4y2Ba}^{2gydF4y2Ba} （gydF4y2Ba {PgydF4y2Ba}^{+gydF4y2Ba} （gydF4y2Ba 53gydF4y2Ba ）gydF4y2Ba +gydF4y2Ba βgydF4y2Ba +gydF4y2Ba ηgydF4y2Ba ）gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & -gydF4y2Ba κgydF4y2Ba （gydF4y2Ba {PgydF4y2Ba}^{+gydF4y2Ba} （gydF4y2Ba 53gydF4y2Ba ）gydF4y2Ba +gydF4y2Ba βgydF4y2Ba ）gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba \\ 0gydF4y2Ba & 0gydF4y2Ba & {PgydF4y2Ba}^{+gydF4y2Ba} （gydF4y2Ba 27gydF4y2Ba ）gydF4y2Ba +gydF4y2Ba {κgydF4y2Ba}^{2gydF4y2Ba} （gydF4y2Ba {PgydF4y2Ba}^{+gydF4y2Ba} （gydF4y2Ba 66gydF4y2Ba ）gydF4y2Ba +gydF4y2Ba βgydF4y2Ba +gydF4y2Ba ηgydF4y2Ba ）gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & -gydF4y2Ba κgydF4y2Ba （gydF4y2Ba {PgydF4y2Ba}^{+gydF4y2Ba} （gydF4y2Ba 66gydF4y2Ba ）gydF4y2Ba +gydF4y2Ba βgydF4y2Ba ）gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba \\ -gydF4y2Ba κgydF4y2Ba （gydF4y2Ba {PgydF4y2Ba}^{+gydF4y2Ba} （gydF4y2Ba 40gydF4y2Ba ）gydF4y2Ba +gydF4y2Ba βgydF4y2Ba ）gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & {PgydF4y2Ba}^{+gydF4y2Ba} （gydF4y2Ba 40gydF4y2Ba ）gydF4y2Ba +gydF4y2Ba βgydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba \\ 0gydF4y2Ba & -gydF4y2Ba κgydF4y2Ba （gydF4y2Ba {PgydF4y2Ba}^{+gydF4y2Ba} （gydF4y2Ba 53gydF4y2Ba ）gydF4y2Ba +gydF4y2Ba βgydF4y2Ba ）gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & {PgydF4y2Ba}^{+gydF4y2Ba} （gydF4y2Ba 53gydF4y2Ba ）gydF4y2Ba +gydF4y2Ba βgydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba \\ 0gydF4y2Ba & 0gydF4y2Ba & -gydF4y2Ba κgydF4y2Ba （gydF4y2Ba {PgydF4y2Ba}^{+gydF4y2Ba} （gydF4y2Ba 66gydF4y2Ba ）gydF4y2Ba +gydF4y2Ba βgydF4y2Ba ）gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & {PgydF4y2Ba}^{+gydF4y2Ba} （gydF4y2Ba 66gydF4y2Ba ）gydF4y2Ba +gydF4y2Ba βgydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba \\ 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & {νgydF4y2Ba}^{2gydF4y2Ba} {PgydF4y2Ba}^{+gydF4y2Ba} （gydF4y2Ba 79gydF4y2Ba ）gydF4y2Ba +gydF4y2Ba ξgydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba \\ 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & {νgydF4y2Ba}^{2gydF4y2Ba} {PgydF4y2Ba}^{+gydF4y2Ba} （gydF4y2Ba 92gydF4y2Ba ）gydF4y2Ba +gydF4y2Ba ξgydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba \\ 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & {νgydF4y2Ba}^{2gydF4y2Ba} {PgydF4y2Ba}^{+gydF4y2Ba} （gydF4y2Ba 105gydF4y2Ba ）gydF4y2Ba +gydF4y2Ba ξgydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba \\ 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & {σgydF4y2Ba}^{2gydF4y2Ba} {PgydF4y2Ba}^{+gydF4y2Ba} （gydF4y2Ba 118gydF4y2Ba ）gydF4y2Ba +gydF4y2Ba γgydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba \\ 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & {σgydF4y2Ba}^{2gydF4y2Ba} {PgydF4y2Ba}^{+gydF4y2Ba} （gydF4y2Ba 131gydF4y2Ba ）gydF4y2Ba +gydF4y2Ba γgydF4y2Ba & 0gydF4y2Ba \\ 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & 0gydF4y2Ba & {σgydF4y2Ba}^{2gydF4y2Ba} {PgydF4y2Ba}^{+gydF4y2Ba} （gydF4y2Ba 144gydF4y2Ba ）gydF4y2Ba +gydF4y2Ba γgydF4y2Ba \end{matrix} ］gydF4y2Ba$

在哪里gydF4y2Ba

PgydF4y2Ba^+gydF4y2Ba——是更新后的(gydF4y2Ba后验gydF4y2Ba)误差估计协方差gydF4y2Ba
κgydF4y2Ba- - -gydF4y2BaDecimationFactorgydF4y2Ba/gydF4y2BaSampleRategydF4y2Ba
βgydF4y2Ba- - -gydF4y2BaGyroscopeDriftNoisegydF4y2Ba
ηgydF4y2Ba- - -gydF4y2BaGyroscopeNoisegydF4y2Ba
νgydF4y2Ba- - -gydF4y2BaLinearAccelerationDecayFactorgydF4y2Ba
ξgydF4y2Ba- - -gydF4y2BaLinearAccelerationNoisegydF4y2Ba
σgydF4y2Ba- - -gydF4y2BaMagneticDisturbanceDecayFactorgydF4y2Ba
γgydF4y2Ba- - -gydF4y2BaMagneticDisturbanceNoisegydF4y2Ba

见第10.1节gydF4y2Ba［1］gydF4y2Ba对于过程误差矩阵项的推导。gydF4y2Ba

卡尔曼增益gydF4y2Ba

卡尔曼增益矩阵是一个12乘6的矩阵，用于衡量创新。在这个算法中，创新被解释为误差过程，gydF4y2BazgydF4y2Ba．gydF4y2Ba

卡尔曼增益矩阵构造为:gydF4y2Ba

${KgydF4y2Ba}_{12gydF4y2Ba \timesgydF4y2Ba 6gydF4y2Ba} ＝gydF4y2Ba （gydF4y2Ba {PgydF4y2Ba}_{_{12gydF4y2Ba \timesgydF4y2Ba 12gydF4y2Ba}}^{-gydF4y2Ba} ）gydF4y2Ba {（gydF4y2Ba {HgydF4y2Ba}_{6gydF4y2Ba \timesgydF4y2Ba 12gydF4y2Ba} ）gydF4y2Ba}^{TgydF4y2Ba} {（gydF4y2Ba {（gydF4y2Ba {年代gydF4y2Ba}_{6gydF4y2Ba \timesgydF4y2Ba 6gydF4y2Ba} ）gydF4y2Ba}^{TgydF4y2Ba} ）gydF4y2Ba}^{-gydF4y2Ba 1gydF4y2Ba}$

在哪里gydF4y2Ba

PgydF4y2Ba^−gydF4y2Ba——预测误差协方差gydF4y2Ba
HgydF4y2Ba——观测模型gydF4y2Ba
年代gydF4y2Ba——创新协方差gydF4y2Ba

更新后验错误gydF4y2Ba

的gydF4y2Ba一个后gydF4y2Ba误差估计由卡尔曼增益矩阵与重力矢量和磁矢量估计误差相结合确定:gydF4y2Ba

${xgydF4y2Ba}_{12gydF4y2Ba \timesgydF4y2Ba 1gydF4y2Ba} ＝gydF4y2Ba （gydF4y2Ba {KgydF4y2Ba}_{12gydF4y2Ba \timesgydF4y2Ba 6gydF4y2Ba} ）gydF4y2Ba {（gydF4y2Ba {zgydF4y2Ba}_{1gydF4y2Ba \timesgydF4y2Ba 6gydF4y2Ba} ）gydF4y2Ba}^{TgydF4y2Ba}$

如果在当前迭代中检测到磁干扰，则忽略磁矢量误差信号gydF4y2Ba一个后gydF4y2Ba误差估计的计算方法为:gydF4y2Ba

${xgydF4y2Ba}_{9gydF4y2Ba \timesgydF4y2Ba 1gydF4y2Ba} ＝gydF4y2Ba （gydF4y2Ba KgydF4y2Ba （gydF4y2Ba 1:9gydF4y2Ba ，gydF4y2Ba 1:3gydF4y2Ba ）gydF4y2Ba {（gydF4y2Ba {zgydF4y2Ba}_{ggydF4y2Ba} ）gydF4y2Ba}^{TgydF4y2Ba}$

正确的gydF4y2Ba

估计取向gydF4y2Ba

方向估计通过将之前的估计乘以误差来更新:gydF4y2Ba

${问gydF4y2Ba}^{+gydF4y2Ba} ＝gydF4y2Ba （gydF4y2Ba {问gydF4y2Ba}^{-gydF4y2Ba} ）gydF4y2Ba （gydF4y2Ba {θgydF4y2Ba}^{+gydF4y2Ba} ）gydF4y2Ba$

估计线性加速度gydF4y2Ba

线性加速度估计通过衰减前一次迭代的线性加速度估计并减去误差来更新:gydF4y2Ba

$lgydF4y2Ba 我gydF4y2Ba ngydF4y2Ba 一个gydF4y2Ba cgydF4y2Ba cgydF4y2Ba egydF4y2Ba lgydF4y2Ba PgydF4y2Ba rgydF4y2Ba 我gydF4y2Ba ogydF4y2Ba rgydF4y2Ba ＝gydF4y2Ba （gydF4y2Ba lgydF4y2Ba 我gydF4y2Ba ngydF4y2Ba 一个gydF4y2Ba cgydF4y2Ba cgydF4y2Ba egydF4y2Ba lgydF4y2Ba PgydF4y2Ba rgydF4y2Ba 我gydF4y2Ba ogydF4y2Ba {rgydF4y2Ba}_{kgydF4y2Ba -gydF4y2Ba 1gydF4y2Ba} ）gydF4y2Ba νgydF4y2Ba -gydF4y2Ba {bgydF4y2Ba}^{+gydF4y2Ba}$

在哪里gydF4y2Ba

νgydF4y2Ba- - -gydF4y2BaLinearAccelerationDecayFactorgydF4y2Ba

陀螺仪偏移量估计gydF4y2Ba

陀螺仪偏移估计更新为用前一次迭代的陀螺仪偏移减去陀螺仪偏移误差:gydF4y2Ba

$ggydF4y2Ba ygydF4y2Ba rgydF4y2Ba ogydF4y2Ba OgydF4y2Ba fgydF4y2Ba fgydF4y2Ba 年代gydF4y2Ba egydF4y2Ba tgydF4y2Ba ＝gydF4y2Ba ggydF4y2Ba ygydF4y2Ba rgydF4y2Ba ogydF4y2Ba OgydF4y2Ba fgydF4y2Ba fgydF4y2Ba 年代gydF4y2Ba egydF4y2Ba {tgydF4y2Ba}_{kgydF4y2Ba -gydF4y2Ba 1gydF4y2Ba} -gydF4y2Ba {一个gydF4y2Ba}^{+gydF4y2Ba}$

计算角速度gydF4y2Ba

的坐标系来估计角速度gydF4y2BagyroReadingsgydF4y2Ba取平均值，并减去前一次迭代计算的陀螺仪偏移量:gydF4y2Ba

$一个gydF4y2Ba ngydF4y2Ba ggydF4y2Ba ugydF4y2Ba lgydF4y2Ba 一个gydF4y2Ba rgydF4y2Ba VgydF4y2Ba egydF4y2Ba lgydF4y2Ba ogydF4y2Ba cgydF4y2Ba 我gydF4y2Ba tgydF4y2Ba {ygydF4y2Ba}_{1gydF4y2Ba \timesgydF4y2Ba 3.gydF4y2Ba} ＝gydF4y2Ba \frac{\sumgydF4y2Ba ggydF4y2Ba ygydF4y2Ba rgydF4y2Ba ogydF4y2Ba RgydF4y2Ba egydF4y2Ba 一个gydF4y2Ba dgydF4y2Ba 我gydF4y2Ba ngydF4y2Ba ggydF4y2Ba {年代gydF4y2Ba}_{NgydF4y2Ba \timesgydF4y2Ba 3.gydF4y2Ba}}{NgydF4y2Ba} -gydF4y2Ba ggydF4y2Ba ygydF4y2Ba rgydF4y2Ba ogydF4y2Ba OgydF4y2Ba fgydF4y2Ba fgydF4y2Ba 年代gydF4y2Ba egydF4y2Ba {tgydF4y2Ba}_{1gydF4y2Ba \timesgydF4y2Ba 3.gydF4y2Ba}$

在哪里gydF4y2BaNgydF4y2Ba是否指定了抽取因子gydF4y2BaDecimationFactorgydF4y2Ba财产。gydF4y2Ba

陀螺仪偏移估计初始化为零的第一次迭代。gydF4y2Ba

更新磁矢量gydF4y2Ba

如果在当前迭代中未检测到磁干扰，则磁矢量估计，gydF4y2Ba米gydF4y2Ba的更新gydF4y2Ba后验gydF4y2Ba磁扰动误差和gydF4y2Ba后验gydF4y2Ba取向。gydF4y2Ba

磁扰动误差转换为导航框:gydF4y2Ba

$米gydF4y2Ba EgydF4y2Ba rgydF4y2Ba rgydF4y2Ba ogydF4y2Ba rgydF4y2Ba NgydF4y2Ba EgydF4y2Ba {DgydF4y2Ba}_{1gydF4y2Ba \timesgydF4y2Ba 3.gydF4y2Ba} ＝gydF4y2Ba {（gydF4y2Ba {（gydF4y2Ba rgydF4y2Ba PgydF4y2Ba ogydF4y2Ba 年代gydF4y2Ba {tgydF4y2Ba}_{3.gydF4y2Ba \timesgydF4y2Ba 3.gydF4y2Ba} ）gydF4y2Ba}^{TgydF4y2Ba} {（gydF4y2Ba 米gydF4y2Ba EgydF4y2Ba rgydF4y2Ba rgydF4y2Ba ogydF4y2Ba {rgydF4y2Ba}_{1gydF4y2Ba \timesgydF4y2Ba 3.gydF4y2Ba} ）gydF4y2Ba}^{TgydF4y2Ba} ）gydF4y2Ba}^{TgydF4y2Ba}$

从之前的磁矢量估计中减去导航框架内的磁扰动误差，解释为倾角:gydF4y2Ba

$ΜgydF4y2Ba ＝gydF4y2Ba 米gydF4y2Ba -gydF4y2Ba 米gydF4y2Ba EgydF4y2Ba rgydF4y2Ba rgydF4y2Ba ogydF4y2Ba rgydF4y2Ba NgydF4y2Ba EgydF4y2Ba DgydF4y2Ba$

$我gydF4y2Ba ngydF4y2Ba cgydF4y2Ba lgydF4y2Ba 我gydF4y2Ba ngydF4y2Ba 一个gydF4y2Ba tgydF4y2Ba 我gydF4y2Ba ogydF4y2Ba ngydF4y2Ba ＝gydF4y2Ba 量化gydF4y2Ba （gydF4y2Ba ΜgydF4y2Ba （gydF4y2Ba 3.gydF4y2Ba ）gydF4y2Ba ，gydF4y2Ba ΜgydF4y2Ba （gydF4y2Ba 1gydF4y2Ba ）gydF4y2Ba ）gydF4y2Ba$

倾角转换为约束磁矢量估计，用于下一次迭代:gydF4y2Ba

$\begin{array}{l} 米gydF4y2Ba （gydF4y2Ba 1gydF4y2Ba ）gydF4y2Ba ＝gydF4y2Ba （gydF4y2Ba ExpectedMagneticFieldStrengthgydF4y2Ba ）gydF4y2Ba （gydF4y2Ba 因为gydF4y2Ba （gydF4y2Ba 我gydF4y2Ba ngydF4y2Ba cgydF4y2Ba lgydF4y2Ba 我gydF4y2Ba ngydF4y2Ba 一个gydF4y2Ba tgydF4y2Ba 我gydF4y2Ba ogydF4y2Ba ngydF4y2Ba ）gydF4y2Ba ）gydF4y2Ba \\ 米gydF4y2Ba （gydF4y2Ba 2gydF4y2Ba ）gydF4y2Ba ＝gydF4y2Ba 0gydF4y2Ba \\ 米gydF4y2Ba （gydF4y2Ba 3.gydF4y2Ba ）gydF4y2Ba ＝gydF4y2Ba （gydF4y2Ba ExpectedMagneticFieldStrengthgydF4y2Ba ）gydF4y2Ba （gydF4y2Ba 罪gydF4y2Ba （gydF4y2Ba 我gydF4y2Ba ngydF4y2Ba cgydF4y2Ba lgydF4y2Ba 我gydF4y2Ba ngydF4y2Ba 一个gydF4y2Ba tgydF4y2Ba 我gydF4y2Ba ogydF4y2Ba ngydF4y2Ba ）gydF4y2Ba ）gydF4y2Ba \end{array}$

ExpectedMagneticFieldStrengthgydF4y2Ba的属性。gydF4y2BaahrsfiltergydF4y2Ba．gydF4y2Ba

参考文献gydF4y2Ba

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

[2] Roetenberg, D. H.J. Luinge, C.T.M. Baten和P.H. Veltink。磁干扰的补偿改善了惯性和磁感应的人体部分方向。gydF4y2BaIEEE神经系统与康复工程汇刊gydF4y2Ba．卷。13。第3期，2005，第395-405页。gydF4y2Ba

扩展功能gydF4y2Ba

C/ c++代码生成gydF4y2Ba
使用MATLAB®Coder™生成C和c++代码。gydF4y2Ba

使用注意事项和限制:gydF4y2Ba

看到gydF4y2Ba系统对象在MATLAB代码生成gydF4y2Ba(MATLAB编码器)gydF4y2Ba．gydF4y2Ba

版本历史gydF4y2Ba

在R2018b中引入gydF4y2Ba

另请参阅gydF4y2Ba

ecompassgydF4y2Ba|gydF4y2BagpsSensorgydF4y2Ba|gydF4y2BaimufiltergydF4y2Ba|gydF4y2BaimuSensorgydF4y2Ba

ahrsfiltergydF4y2Ba

描述gydF4y2Ba

创建gydF4y2Ba

语法gydF4y2Ba

描述gydF4y2Ba

属性gydF4y2Ba

SampleRategydF4y2Ba- - - - - -gydF4y2Ba传感器数据输入采样率(Hz)gydF4y2BaOne hundred.gydF4y2Ba(默认)|gydF4y2Ba积极的标量gydF4y2Ba

DecimationFactorgydF4y2Ba- - - - - -gydF4y2Ba大量毁灭的因素gydF4y2Ba1gydF4y2Ba(默认)|gydF4y2Ba正整数gydF4y2Ba

AccelerometerNoisegydF4y2Ba- - - - - -gydF4y2Ba加速度计信号噪声方差((m/sgydF4y2Ba2gydF4y2Ba）gydF4y2Ba2gydF4y2Ba）gydF4y2Ba0.00019247gydF4y2Ba(默认)|gydF4y2Ba正实标量gydF4y2Ba

MagnetometerNoisegydF4y2Ba- - - - - -gydF4y2Ba磁强计信号噪声方差(μTgydF4y2Ba2gydF4y2Ba）gydF4y2Ba0．1gydF4y2Ba(默认)|gydF4y2Ba正实标量gydF4y2Ba

GyroscopeNoisegydF4y2Ba- - - - - -gydF4y2Ba陀螺仪信号噪声方差((rad/s)gydF4y2Ba2gydF4y2Ba）gydF4y2Ba9.1385 e-5gydF4y2Ba(默认)|gydF4y2Ba正实标量gydF4y2Ba

GyroscopeDriftNoisegydF4y2Ba- - - - - -gydF4y2Ba陀螺仪偏移漂移方差((rad/s)gydF4y2Ba2gydF4y2Ba）gydF4y2Ba3.0462 e-13gydF4y2Ba(默认)|gydF4y2Ba正实标量gydF4y2Ba

LinearAccelerationNoisegydF4y2Ba- - - - - -gydF4y2Ba线性加速度噪声方差(m/sgydF4y2Ba2gydF4y2Ba）gydF4y2Ba2gydF4y2Ba0.0096236gydF4y2Ba(默认)|gydF4y2Ba正实标量gydF4y2Ba

LinearAccelerationDecayFactorgydF4y2Ba- - - - - -gydF4y2Ba线性加速度漂移的衰减因子gydF4y2Ba0．5gydF4y2Ba(默认)|gydF4y2Ba[0,1]范围内的标量gydF4y2Ba

MagneticDisturbanceNoisegydF4y2Ba- - - - - -gydF4y2Ba磁干扰噪声方差(μTgydF4y2Ba2gydF4y2Ba）gydF4y2Ba0．5gydF4y2Ba(默认)|gydF4y2Ba实有限正标量gydF4y2Ba

MagneticDisturbanceDecayFactorgydF4y2Ba- - - - - -gydF4y2Ba磁扰动衰减因子gydF4y2Ba0．5gydF4y2Ba(默认)|gydF4y2Ba在[0,1]范围内的正标量gydF4y2Ba

InitialProcessNoisegydF4y2Ba- - - - - -gydF4y2Ba过程噪声的协方差矩阵gydF4y2Ba12-by-12矩阵gydF4y2Ba

ExpectedMagneticFieldStrengthgydF4y2Ba- - - - - -gydF4y2Ba磁场强度预期估计值(μT)gydF4y2Ba50gydF4y2Ba(默认)|gydF4y2Ba实正标量gydF4y2Ba

OrientationFormatgydF4y2Ba- - - - - -gydF4y2Ba输出方向格式gydF4y2Ba“四元数”gydF4y2Ba(默认)|gydF4y2Ba旋转矩阵的gydF4y2Ba

使用gydF4y2Ba

语法gydF4y2Ba

描述gydF4y2Ba

输入参数gydF4y2Ba

accelReadingsgydF4y2Ba- - - - - -gydF4y2Ba加速度计读数在传感器本体坐标系(m/sgydF4y2Ba2gydF4y2Ba）gydF4y2BaNgydF4y2Ba3矩阵gydF4y2Ba

gyroReadingsgydF4y2Ba- - - - - -gydF4y2Ba陀螺仪在传感器体坐标系中的读数(rad/s)gydF4y2BaNgydF4y2Ba3矩阵gydF4y2Ba

magReadingsgydF4y2Ba- - - - - -gydF4y2Ba传感器体坐标系中的磁力计读数(µT)gydF4y2BaNgydF4y2Ba3矩阵gydF4y2Ba

输出参数gydF4y2Ba

取向gydF4y2Ba-方向，将数量从本地导航坐标系旋转到传感器本体坐标系gydF4y2Ba米gydF4y2Ba-by-1四元数数组(默认)| 3-by-3-by-gydF4y2Ba米gydF4y2Ba数组gydF4y2Ba

angularVelocitygydF4y2Ba-传感器本体坐标系中的角速度(rad/s)gydF4y2Ba米gydF4y2Ba-by-3 array(默认)gydF4y2Ba

对象的功能gydF4y2Ba

特定于gydF4y2BaahrsfiltergydF4y2Ba

所有系统对象通用gydF4y2Ba

例子gydF4y2Ba

估计方向使用gydF4y2BaahrsfiltergydF4y2Ba

模拟磁干扰gydF4y2BaahrsFiltergydF4y2Ba

轨道震动9轴IMUgydF4y2Ba

算法gydF4y2Ba

详细的概述gydF4y2Ba

模型gydF4y2Ba

误差模型gydF4y2Ba

磁强计正确gydF4y2Ba

卡尔曼滤波方程gydF4y2Ba

正确的gydF4y2Ba

计算角速度gydF4y2Ba

更新磁矢量gydF4y2Ba

参考文献gydF4y2Ba

扩展功能gydF4y2Ba

C/ c++代码生成gydF4y2Ba使用MATLAB®Coder™生成C和c++代码。gydF4y2Ba

版本历史gydF4y2Ba

另请参阅gydF4y2Ba

`SampleRategydF4y2Ba`- - - - - -gydF4y2Ba传感器数据输入采样率(Hz)gydF4y2Ba
`One hundred.gydF4y2Ba`(默认)|gydF4y2Ba积极的标量gydF4y2Ba

`DecimationFactorgydF4y2Ba`- - - - - -gydF4y2Ba大量毁灭的因素gydF4y2Ba
`1gydF4y2Ba`(默认)|gydF4y2Ba正整数gydF4y2Ba

`AccelerometerNoisegydF4y2Ba`- - - - - -gydF4y2Ba加速度计信号噪声方差((m/sgydF4y2Ba^2gydF4y2Ba）gydF4y2Ba^2gydF4y2Ba）gydF4y2Ba
`0.00019247gydF4y2Ba`(默认)|gydF4y2Ba正实标量gydF4y2Ba

`MagnetometerNoisegydF4y2Ba`- - - - - -gydF4y2Ba磁强计信号噪声方差(μTgydF4y2Ba^2gydF4y2Ba）gydF4y2Ba
`0．1gydF4y2Ba`(默认)|gydF4y2Ba正实标量gydF4y2Ba

`GyroscopeNoisegydF4y2Ba`- - - - - -gydF4y2Ba陀螺仪信号噪声方差((rad/s)gydF4y2Ba^2gydF4y2Ba）gydF4y2Ba
`9.1385 e-5gydF4y2Ba`(默认)|gydF4y2Ba正实标量gydF4y2Ba

`GyroscopeDriftNoisegydF4y2Ba`- - - - - -gydF4y2Ba陀螺仪偏移漂移方差((rad/s)gydF4y2Ba^2gydF4y2Ba）gydF4y2Ba
`3.0462 e-13gydF4y2Ba`(默认)|gydF4y2Ba正实标量gydF4y2Ba

`LinearAccelerationNoisegydF4y2Ba`- - - - - -gydF4y2Ba线性加速度噪声方差(m/sgydF4y2Ba^2gydF4y2Ba）gydF4y2Ba^2gydF4y2Ba
`0.0096236gydF4y2Ba`(默认)|gydF4y2Ba正实标量gydF4y2Ba

`LinearAccelerationDecayFactorgydF4y2Ba`- - - - - -gydF4y2Ba线性加速度漂移的衰减因子gydF4y2Ba
`0．5gydF4y2Ba`(默认)|gydF4y2Ba[0,1]范围内的标量gydF4y2Ba

`MagneticDisturbanceNoisegydF4y2Ba`- - - - - -gydF4y2Ba磁干扰噪声方差(μTgydF4y2Ba^2gydF4y2Ba）gydF4y2Ba
`0．5gydF4y2Ba`(默认)|gydF4y2Ba实有限正标量gydF4y2Ba

`MagneticDisturbanceDecayFactorgydF4y2Ba`- - - - - -gydF4y2Ba磁扰动衰减因子gydF4y2Ba
`0．5gydF4y2Ba`(默认)|gydF4y2Ba在[0,1]范围内的正标量gydF4y2Ba

`InitialProcessNoisegydF4y2Ba`- - - - - -gydF4y2Ba过程噪声的协方差矩阵gydF4y2Ba
12-by-12矩阵gydF4y2Ba

`ExpectedMagneticFieldStrengthgydF4y2Ba`- - - - - -gydF4y2Ba磁场强度预期估计值(μT)gydF4y2Ba
`50gydF4y2Ba`(默认)|gydF4y2Ba实正标量gydF4y2Ba

`OrientationFormatgydF4y2Ba`- - - - - -gydF4y2Ba输出方向格式gydF4y2Ba
`“四元数”gydF4y2Ba`(默认)|gydF4y2Ba`旋转矩阵的gydF4y2Ba`

`accelReadingsgydF4y2Ba`- - - - - -gydF4y2Ba加速度计读数在传感器本体坐标系(m/sgydF4y2Ba^2gydF4y2Ba）gydF4y2Ba
NgydF4y2Ba3矩阵gydF4y2Ba

`gyroReadingsgydF4y2Ba`- - - - - -gydF4y2Ba陀螺仪在传感器体坐标系中的读数(rad/s)gydF4y2Ba
NgydF4y2Ba3矩阵gydF4y2Ba

`magReadingsgydF4y2Ba`- - - - - -gydF4y2Ba传感器体坐标系中的磁力计读数(µT)gydF4y2Ba
NgydF4y2Ba3矩阵gydF4y2Ba

`取向gydF4y2Ba`-方向，将数量从本地导航坐标系旋转到传感器本体坐标系gydF4y2Ba
米gydF4y2Ba-by-1四元数数组(默认)| 3-by-3-by-gydF4y2Ba米gydF4y2Ba数组gydF4y2Ba

`angularVelocitygydF4y2Ba`-传感器本体坐标系中的角速度(rad/s)gydF4y2Ba
米gydF4y2Ba-by-3 array(默认)gydF4y2Ba

特定于gydF4y2Ba`ahrsfiltergydF4y2Ba`

估计方向使用gydF4y2Ba`ahrsfiltergydF4y2Ba`

模拟磁干扰gydF4y2Ba`ahrsFiltergydF4y2Ba`

C/ c++代码生成gydF4y2Ba
使用MATLAB®Coder™生成C和c++代码。gydF4y2Ba