imufilterGyD.F4y2Ba

加速度计和陀螺仪读数的方向GyD.F4y2Ba

全部展开页面GyD.F4y2Ba

描述GyD.F4y2Ba

这GyD.F4y2BaimufilterGyD.F4y2BaSystem Object™熔断器加速度计和陀螺仪传感器数据以估计设备方向。GyD.F4y2Ba

估计设备朝向:GyD.F4y2Ba

创建GyD.F4y2BaimufilterGyD.F4y2Ba对象，并设置其属性。GyD.F4y2Ba
使用参数调用对象，就像调用函数一样。GyD.F4y2Ba

要了解有关系统对象如何工作的更多信息，请参阅GyD.F4y2Ba什么是系统对象?GyD.F4y2Ba

创建GyD.F4y2Ba

句法GyD.F4y2Ba

保险丝= imufilterGyD.F4y2Ba

fuse = imufilter（'参考框架'，rf）GyD.F4y2Ba

保险丝= imufilter (GyD.F4y2Ba___GyD.F4y2Ba、名称、值)GyD.F4y2Ba

描述GyD.F4y2Ba

例子GyD.F4y2Ba

保险丝GyD.F4y2Ba= imufilterGyD.F4y2Ba返回间接卡尔曼滤波系统对象，GyD.F4y2Ba保险丝GyD.F4y2Ba，用于融合加速度计和陀螺仪数据来估计设备方向。该滤波器使用一个九元状态向量来跟踪方向估计、陀螺仪偏差估计和线性加速度估计中的误差。GyD.F4y2Ba

保险丝GyD.F4y2Ba= imufilter (GyD.F4y2Ba'参考范围'GyD.F4y2Ba那GyD.F4y2Ba射频GyD.F4y2Ba的）GyD.F4y2Ba返回一个GyD.F4y2BaimufilterGyD.F4y2Ba滤波融合加速度计和陀螺仪数据以估计相对于参考框架的设备方向的系统对象GyD.F4y2Ba射频GyD.F4y2Ba．指定GyD.F4y2Ba射频GyD.F4y2Ba作为GyD.F4y2BaNED的GyD.F4y2Ba(North-East-Down)或GyD.F4y2Ba“ENU表示”GyD.F4y2Ba(East-North-Up)。默认值为GyD.F4y2BaNED的GyD.F4y2Ba．GyD.F4y2Ba

例子GyD.F4y2Ba

保险丝GyD.F4y2Ba= imufilter (GyD.F4y2Ba___GyD.F4y2Ba那GyD.F4y2Ba名称,值GyD.F4y2Ba的）GyD.F4y2Ba设置每个属性GyD.F4y2Ba的名字GyD.F4y2Ba到指定的GyD.F4y2Ba价值GyD.F4y2Ba．未指定的属性具有默认值。GyD.F4y2Ba

例子:GyD.F4y2Bafuse = imufilter（'samplerate'，200，'gyroscopenoise'，1e-6）GyD.F4y2Ba创建一个System对象，GyD.F4y2Ba保险丝GyD.F4y2Ba，采样频率为200hz，陀螺仪噪声设置为1e-6弧度每秒的平方。GyD.F4y2Ba

属性GyD.F4y2Ba

全部展开GyD.F4y2Ba

除非另有说明，属性是GyD.F4y2BanontunableGyD.F4y2Ba，这意味着您不能在调用对象之后更改它们的值。对象在调用时锁定，而GyD.F4y2Ba释放GyD.F4y2Ba函数打开它们。GyD.F4y2Ba

如果一个属性是GyD.F4y2Ba可调GyD.F4y2Ba，您可以随时更改其值。GyD.F4y2Ba

有关更改属性值的更多信息，请参见GyD.F4y2Ba使用系统对象Matlab中的系统设计GyD.F4y2Ba．GyD.F4y2Ba

`SampleRateGyD.F4y2Ba`-GyD.F4y2Ba输入传感器数据采样率(Hz)GyD.F4y2Ba
`One hundred.GyD.F4y2Ba`(默认)|GyD.F4y2Ba积极的有限的标量GyD.F4y2Ba

Hz中输入传感器数据的采样率，指定为正有限标量。GyD.F4y2Ba

可调：GyD.F4y2Ba没有GyD.F4y2Ba

`DecimationFactorGyD.F4y2Ba`-GyD.F4y2Ba大量毁灭的因素GyD.F4y2Ba
`1GyD.F4y2Ba`(默认)|GyD.F4y2Ba正整数标量GyD.F4y2Ba

抽取因子，用来降低输入传感器数据的采样率，指定为一个正整数标量。GyD.F4y2Ba

输入的行数，GyD.F4y2BaaccelReadingsGyD.F4y2Ba和GyD.F4y2Ba回电GyD.F4y2Ba，必须是抽取因子的倍数。GyD.F4y2Ba

可调：GyD.F4y2Ba没有GyD.F4y2Ba

`AccelerometerNoiseGyD.F4y2Ba`-GyD.F4y2Ba加速度计信号噪声方差(m/s)GyD.F4y2Ba^2GyD.F4y2Ba的）GyD.F4y2Ba^2GyD.F4y2Ba的）GyD.F4y2Ba
`0.00019247GyD.F4y2Ba`(默认)|GyD.F4y2Ba积极的真正的标量GyD.F4y2Ba

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

可调：GyD.F4y2Ba是的GyD.F4y2Ba

`GyroscopeNoiseGyD.F4y2Ba`-GyD.F4y2Ba陀螺仪信号噪声的差异（（rad / s）GyD.F4y2Ba^2GyD.F4y2Ba的）GyD.F4y2Ba
`9.1385E-5GyD.F4y2Ba`(默认)|GyD.F4y2Ba积极的真正的标量GyD.F4y2Ba

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

可调：GyD.F4y2Ba是的GyD.F4y2Ba

`GyroscopeDriftNoiseGyD.F4y2Ba`-GyD.F4y2Ba陀螺仪偏置漂移方差(rad/s)GyD.F4y2Ba^2GyD.F4y2Ba的）GyD.F4y2Ba
`3.0462 e-13GyD.F4y2Ba`(默认)|GyD.F4y2Ba积极的真正的标量GyD.F4y2Ba

陀螺仪偏移漂移方差(rad/s)GyD.F4y2Ba^2GyD.F4y2Ba，指定为正实标量。GyD.F4y2Ba

可调：GyD.F4y2Ba是的GyD.F4y2Ba

`linearaccelerationnoise.GyD.F4y2Ba`-GyD.F4y2Ba线性加速度噪声的变化（（m / sGyD.F4y2Ba^2GyD.F4y2Ba的）GyD.F4y2Ba^2GyD.F4y2Ba的）GyD.F4y2Ba
`0.0096236GyD.F4y2Ba`(默认)|GyD.F4y2Ba积极的真正的标量GyD.F4y2Ba

线性加速度噪声的方差(m/s)GyD.F4y2Ba^2GyD.F4y2Ba的）GyD.F4y2Ba^2GyD.F4y2Ba，指定为正实标量。线性加速度模型为低通滤波白噪声过程。GyD.F4y2Ba

可调：GyD.F4y2Ba是的GyD.F4y2Ba

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

线性加速度漂移的衰减系数，指定为[0,1]范围内的标量。如果线性加速度变化很快，设置GyD.F4y2BaLinearAccelerationDecayFactorGyD.F4y2Ba到一个更低的值。如果线性加速度变化缓慢，设置GyD.F4y2BaLinearAccelerationDecayFactorGyD.F4y2Ba到更高的值。线性加速度漂移被建模为低通滤波白噪声过程。GyD.F4y2Ba

可调：GyD.F4y2Ba是的GyD.F4y2Ba

`initialProcessnoise.GyD.F4y2Ba`-GyD.F4y2Ba用于过程噪声的协方差矩阵GyD.F4y2Ba
9-by-9矩阵GyD.F4y2Ba

过程噪声的协方差矩阵，指定为9 × 9矩阵。默认的是:GyD.F4y2Ba

列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列7到9 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.009623610000000GyD.F4y2Ba

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

`OrientationFormatGyD.F4y2Ba`-GyD.F4y2Ba面向输出格式GyD.F4y2Ba
`'Quaternion'GyD.F4y2Ba`(默认)|GyD.F4y2Ba`旋转矩阵的GyD.F4y2Ba`

输出方向格式，指定为GyD.F4y2Ba'Quaternion'GyD.F4y2Ba或GyD.F4y2Ba旋转矩阵的GyD.F4y2Ba．输出的大小取决于输入大小，GyD.F4y2BaN.GyD.F4y2Ba，输出方向格式:GyD.F4y2Ba

'Quaternion'GyD.F4y2Ba——输出是一个GyD.F4y2BaN.GyD.F4y2Ba1GyD.F4y2Ba四元数GyD.F4y2Ba．GyD.F4y2Ba
旋转矩阵的GyD.F4y2Ba——输出是一个3x3x-GyD.F4y2BaN.GyD.F4y2Ba旋转矩阵。GyD.F4y2Ba

数据类型:GyD.F4y2Ba字符GyD.F4y2Ba|GyD.F4y2Ba字符串GyD.F4y2Ba

使用GyD.F4y2Ba

句法GyD.F4y2Ba

(定位、angularVelocity) =保险丝(accelReadings gyroReadings)GyD.F4y2Ba

描述GyD.F4y2Ba

例子GyD.F4y2Ba

[GyD.F4y2Ba取向GyD.F4y2Ba那GyD.F4y2BaangularVelocityGyD.F4y2Ba] =保险丝（GyD.F4y2BaaccelReadingsGyD.F4y2Ba那GyD.F4y2Ba回电GyD.F4y2Ba的）GyD.F4y2Ba引信加速度计和陀螺仪读数，以计算方向和角速度测量。算法假设设备在第一次调用之前是静止的。GyD.F4y2Ba

输入参数GyD.F4y2Ba

全部展开GyD.F4y2Ba

`accelReadingsGyD.F4y2Ba`-GyD.F4y2Ba传感器体坐标系下的加速度计读数(m/s)GyD.F4y2Ba^2GyD.F4y2Ba的）GyD.F4y2Ba
N.GyD.F4y2Ba-By-3矩阵GyD.F4y2Ba

加速度计在传感器体坐标系中的读数，单位为m/sGyD.F4y2Ba^2GyD.F4y2Ba，指定为GyD.F4y2BaN.GyD.F4y2Ba3矩阵。GyD.F4y2BaN.GyD.F4y2Ba是样本数量和三列GyD.F4y2BaaccelReadingsGyD.F4y2Ba代表[GyD.F4y2BaXGyD.F4y2BayGyD.F4y2BaZ.GyD.F4y2Ba)测量。假定加速度计读数与指定的采样率相对应GyD.F4y2BaSampleRateGyD.F4y2Ba财产。GyD.F4y2Ba

数据类型:GyD.F4y2Ba单GyD.F4y2Ba|GyD.F4y2Ba双GyD.F4y2Ba

`回电GyD.F4y2Ba`-GyD.F4y2Ba传感器体坐标系下的陀螺仪读数(rad/s)GyD.F4y2Ba
N.GyD.F4y2Ba-By-3矩阵GyD.F4y2Ba

陀螺仪在传感器体坐标系中的读数，单位为rad/s，指定为GyD.F4y2BaN.GyD.F4y2Ba3矩阵。GyD.F4y2BaN.GyD.F4y2Ba是样本数量和三列GyD.F4y2Ba回电GyD.F4y2Ba代表[GyD.F4y2BaXGyD.F4y2BayGyD.F4y2BaZ.GyD.F4y2Ba)测量。假设陀螺仪读数对应于所指定的采样率GyD.F4y2BaSampleRateGyD.F4y2Ba财产。GyD.F4y2Ba

数据类型:GyD.F4y2Ba单GyD.F4y2Ba|GyD.F4y2Ba双GyD.F4y2Ba

输出参数GyD.F4y2Ba

全部展开GyD.F4y2Ba

`取向GyD.F4y2Ba`-将量从全局坐标系旋转到传感器体坐标系的方向GyD.F4y2Ba
M.GyD.F4y2Ba-by-1四元数向量(默认GyD.F4y2BaM.GyD.F4y2Ba数组GyD.F4y2Ba

可以将量从全局坐标系旋转到物体坐标系的方向，以四元数或数组的形式返回。的大小和类型GyD.F4y2Ba取向GyD.F4y2Ba取决于GyD.F4y2BaOrientationFormatGyD.F4y2Ba属性设置为GyD.F4y2Ba'Quaternion'GyD.F4y2Ba或GyD.F4y2Ba旋转矩阵的GyD.F4y2Ba：GyD.F4y2Ba

'Quaternion'GyD.F4y2Ba——输出为anGyD.F4y2BaM.GyD.F4y2Ba-1-1个四元数向量，具有与输入相同的底层数据类型。GyD.F4y2Ba
旋转矩阵的GyD.F4y2Ba输出是一个3x3x-GyD.F4y2BaM.GyD.F4y2Ba与输入数据类型相同的旋转矩阵数组。GyD.F4y2Ba

输入样本的数量，GyD.F4y2BaN.GyD.F4y2Ba,GyD.F4y2BaDecimationFactorGyD.F4y2Ba属性确定GyD.F4y2BaM.GyD.F4y2Ba．GyD.F4y2Ba

您可以使用GyD.F4y2Ba取向GyD.F4y2Ba在一个GyD.F4y2BarotateframeGyD.F4y2Ba函数将量从全局坐标系旋转到传感器体坐标系。GyD.F4y2Ba

数据类型:GyD.F4y2Ba四元数GyD.F4y2Ba|GyD.F4y2Ba单GyD.F4y2Ba|GyD.F4y2Ba双GyD.F4y2Ba

`angularVelocityGyD.F4y2Ba`-传感器体坐标系角速度(rad/s)GyD.F4y2Ba
M.GyD.F4y2Ba-By-3阵列（默认）GyD.F4y2Ba

在传感器体坐标系中去除陀螺仪偏差的角速度(rad/s)，返回为GyD.F4y2BaM.GyD.F4y2Ba3数组。输入样本的数量，GyD.F4y2BaN.GyD.F4y2Ba,GyD.F4y2BaDecimationFactorGyD.F4y2Ba属性确定GyD.F4y2BaM.GyD.F4y2Ba．GyD.F4y2Ba

数据类型:GyD.F4y2Ba单GyD.F4y2Ba|GyD.F4y2Ba双GyD.F4y2Ba

对象功能GyD.F4y2Ba

要使用对象函数，请将System对象指定为第一个输入参数。例如，要发布命名的系统对象的系统资源GyD.F4y2Baobj.GyD.F4y2Ba，使用下面的语法:GyD.F4y2Ba

发行版(obj)GyD.F4y2Ba

全部展开GyD.F4y2Ba

特定于GyD.F4y2Ba`imufilterGyD.F4y2Ba`

调优GyD.F4y2Ba 调GyD.F4y2BaimufilterGyD.F4y2Ba参数，以减少估计误差GyD.F4y2Ba

对所有系统对象共同GyD.F4y2Ba

`一步GyD.F4y2Ba`	运行GyD.F4y2Ba系统对象GyD.F4y2Ba算法GyD.F4y2Ba
`释放GyD.F4y2Ba`	释放资源并允许更改GyD.F4y2Ba系统对象GyD.F4y2Ba属性值和输入特性GyD.F4y2Ba
`重置GyD.F4y2Ba`	使内部状态复位GyD.F4y2Ba系统对象GyD.F4y2Ba

例子GyD.F4y2Ba

全部折叠GyD.F4y2Ba

从IMU数据估计方向GyD.F4y2Ba

打开生活的脚本GyD.F4y2Ba

加载GyD.F4y2Barpy_9axisGyD.F4y2Ba文件，其中包含记录的加速度计，陀螺仪，和磁力计传感器数据从一个设备在俯仰摆动(周围GyD.F4y2BayGyD.F4y2Ba-轴)，然后偏航(绕)GyD.F4y2BaZ.GyD.F4y2Ba-轴)，然后滚动(绕)GyD.F4y2BaXGyD.F4y2Ba设在)。该文件还包含记录的采样率。GyD.F4y2Ba

加载GyD.F4y2Ba“rpy_9axis.mat”GyD.F4y2BasensorDataGyD.F4y2BaFS.GyD.F4y2BaaccelerometerReadings = sensorData.Acceleration;gyroscopeReadings = sensorData.AngularVelocity;GyD.F4y2Ba

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

decim = 2;保险丝= imufilter (GyD.F4y2Ba“SampleRate”GyD.F4y2BaFs,GyD.F4y2Ba'decimationfactor'GyD.F4y2Ba，排队）;GyD.F4y2Ba

通过加速度计读数和陀螺仪读数GyD.F4y2BaimufilterGyD.F4y2Ba对象,GyD.F4y2Ba保险丝GyD.F4y2Ba，输出随时间变化的传感器体方向的估计。默认情况下，方向输出为四元数向量。GyD.F4y2Ba

q =保险丝(accelerometerReadings gyroscopeReadings);GyD.F4y2Ba

方向是由旋转父坐标系到子坐标系所需的角位移定义的。用欧拉角绘制方向，用度数表示。GyD.F4y2Ba

imufilterGyD.F4y2Ba融合正确地估计了从假设的朝北的初始方向的方向变化。然而,设备的GyD.F4y2BaXGyD.F4y2Ba- 在记录时XIS指向南方。正确估计相对于真正初始方向或相对于NED的方向，使用GyD.F4y2BaahrsfilterGyD.F4y2Ba．GyD.F4y2Ba

时间= (0:decim:大小(accelerometerReadings, 1) 1) / Fs;情节(时间,eulerd (q,GyD.F4y2Ba'Zyx'GyD.F4y2Ba那GyD.F4y2Ba'框架'GyD.F4y2Ba)标题(GyD.F4y2Ba'定位估计'GyD.F4y2Ba)传说(GyD.F4y2Baz轴的GyD.F4y2Ba那GyD.F4y2Ba“轴”GyD.F4y2Ba那GyD.F4y2Ba'x-轴'GyD.F4y2Ba)包含(GyD.F4y2Ba“时间(s)”GyD.F4y2Ba) ylabel (GyD.F4y2Ba的旋转(度)GyD.F4y2Ba的）GyD.F4y2Ba

图中包含一个轴对象。带有标题方向估计的轴对象包含3个类型为line的对象。这些对象代表z轴，y轴，x轴。GyD.F4y2Ba

使用陀螺仪和加速度计读数的模型倾斜GyD.F4y2Ba

打开脚本GyD.F4y2Ba

模型一个倾斜的IMU，包含一个加速度计和陀螺仪使用GyD.F4y2BaimuSensorGyD.F4y2Ba系统对象™。使用理想模型和现实模型来比较使用的方向跟踪结果GyD.F4y2BaimufilterGyD.F4y2Ba系统对象。GyD.F4y2Ba

加载一个描述地面真实运动和采样率的结构体。运动结构描述顺序旋转:GyD.F4y2Ba

偏航:120度超过2秒GyD.F4y2Ba
音高:60度/秒GyD.F4y2Ba
滚:30度半秒GyD.F4y2Ba
滚:-30度超过半秒GyD.F4y2Ba
音高：-60度一秒钟GyD.F4y2Ba
偏航:-120度超过2秒GyD.F4y2Ba

在最后一个阶段，运动结构将第1、2和3个旋转组合成单轴旋转。加速度、角速度和方向在本地NED坐标系中定义。GyD.F4y2Ba

加载GyD.F4y2Bay120p60r30.matGyD.F4y2Ba运动GyD.F4y2BaFS.GyD.F4y2BaaccNED = motion.Acceleration;angVelNED = motion.AngularVelocity;取向= motion.Orientation;numSamples =大小(motion.Orientation, 1);t = (0: (numSamples-1)。/ fs;GyD.F4y2Ba

创建一个理想的IMU传感器对象和一个默认的IMU过滤器对象。GyD.F4y2Ba

IMU = imuSensor (GyD.F4y2Ba“accel-gyro”GyD.F4y2Ba那GyD.F4y2Ba“SampleRate”GyD.F4y2Bafs);aFilter = imufilter (GyD.F4y2Ba“SampleRate”GyD.F4y2Bafs);GyD.F4y2Ba

在一个循环:GyD.F4y2Ba

通过向IMU传感器对象输入地面真实运动来模拟IMU输出。GyD.F4y2Ba
使用默认的IMU过滤对象过滤IMU输出。GyD.F4y2Ba

取向= 0 (numSamples 1GyD.F4y2Ba'Quaternion'GyD.F4y2Ba）;GyD.F4y2Ba为了GyD.F4y2Bai = 1:numSamples [accelBody,gyroBody] = IMU(accNED(i，:)，angVelNED(i，:)，orientationNED(i，:));方向（i）=从事（加速体，啮道）;GyD.F4y2Ba结尾GyD.F4y2Ba发行版(aFilter)GyD.F4y2Ba

绘制随时间变化的方向。GyD.F4y2Ba

图(1)情节(t, eulerd(取向,GyD.F4y2Ba'Zyx'GyD.F4y2Ba那GyD.F4y2Ba'框架'GyD.F4y2Ba)包含(GyD.F4y2Ba“时间(s)”GyD.F4y2Ba) ylabel (GyD.F4y2Ba的旋转(度)GyD.F4y2Ba)标题(GyD.F4y2Ba方向估计——理想IMU数据，默认IMU滤波器GyD.F4y2Ba)传说(GyD.F4y2Baz轴的GyD.F4y2Ba那GyD.F4y2Ba“轴”GyD.F4y2Ba那GyD.F4y2Ba'x-轴'GyD.F4y2Ba的）GyD.F4y2Ba

修改您的属性GyD.F4y2BaimuSensorGyD.F4y2Ba模拟真实世界的传感器。再次运行这个循环，并随着时间的推移绘制方向估计。GyD.F4y2Ba

IMU。一种CCE.L.E.R.O.meter = accelparams(.．.GyD.F4y2Ba“MeasurementRange”GyD.F4y2Ba, 19.62,GyD.F4y2Ba.．.GyD.F4y2Ba“决议”GyD.F4y2Ba, 0.00059875,GyD.F4y2Ba.．.GyD.F4y2Ba'constantbias'GyD.F4y2Ba, 0.4905,GyD.F4y2Ba.．.GyD.F4y2Ba“AxesMisalignment”GyD.F4y2Ba，2，GyD.F4y2Ba.．.GyD.F4y2Ba“NoiseDensity”GyD.F4y2Ba, 0.003924,GyD.F4y2Ba.．.GyD.F4y2Ba'偏执是'GyD.F4y2Ba0,GyD.F4y2Ba.．.GyD.F4y2Ba“TemperatureBias”GyD.F4y2Ba，[0.34335 0.34335 0.5886]，GyD.F4y2Ba.．.GyD.F4y2Ba“TemperatureScaleFactor”GyD.F4y2Ba, 0.02);IMU。陀螺仪= gyroparams (GyD.F4y2Ba.．.GyD.F4y2Ba“MeasurementRange”GyD.F4y2Ba, 4.3633,GyD.F4y2Ba.．.GyD.F4y2Ba“决议”GyD.F4y2Ba, 0.00013323,GyD.F4y2Ba.．.GyD.F4y2Ba“AxesMisalignment”GyD.F4y2Ba，2，GyD.F4y2Ba.．.GyD.F4y2Ba“NoiseDensity”GyD.F4y2Ba8.7266 e-05GyD.F4y2Ba.．.GyD.F4y2Ba“TemperatureBias”GyD.F4y2Ba，0.34907，GyD.F4y2Ba.．.GyD.F4y2Ba“TemperatureScaleFactor”GyD.F4y2Ba, 0.02,GyD.F4y2Ba.．.GyD.F4y2Ba“AccelerationBias”GyD.F4y2Ba, 0.00017809,GyD.F4y2Ba.．.GyD.F4y2Ba'constantbias'GyD.F4y2Ba, 0.3491, 0.5, 0);orientationDefault = 0 (numSamples 1GyD.F4y2Ba'Quaternion'GyD.F4y2Ba）;GyD.F4y2Ba为了GyD.F4y2Bai = 1:numSamples [accelBody,gyroBody] = IMU(accNED(i，:)，angVelNED(i，:)，orientationNED(i，:));orientationDefault (i) = aFilter (accelBody gyroBody);GyD.F4y2Ba结尾GyD.F4y2Ba发行版(aFilter)图(2)情节(t, eulerd (orientationDefault,GyD.F4y2Ba'Zyx'GyD.F4y2Ba那GyD.F4y2Ba'框架'GyD.F4y2Ba)包含(GyD.F4y2Ba“时间(s)”GyD.F4y2Ba) ylabel (GyD.F4y2Ba的旋转(度)GyD.F4y2Ba)标题(GyD.F4y2Ba方向估计——真实IMU数据，默认IMU滤波器GyD.F4y2Ba)传说(GyD.F4y2Baz轴的GyD.F4y2Ba那GyD.F4y2Ba“轴”GyD.F4y2Ba那GyD.F4y2Ba'x-轴'GyD.F4y2Ba的）GyD.F4y2Ba

能力GyD.F4y2BaimufilterGyD.F4y2Ba当对真实的IMU建模时，跟踪地面真实数据的能力显著降低。要提高性能，请修改您的属性GyD.F4y2BaimufilterGyD.F4y2Ba对象。这些值是由经验决定的。再次运行这个循环，并随着时间的推移绘制方向估计。GyD.F4y2Ba

afilter.gyroscopenoise = 7.6154e-7;afilter.acceleromernoise = 0.0015398;afilter.gyroscopedriftnoise = 3.0462e-12;afilter.linearaccelerationnoise = 0.00096236;afilter.initialprocessnoise = afilter.initialprocessnoise * 10;OrientationNondefault = Zeros（NumSamples，1，GyD.F4y2Ba'Quaternion'GyD.F4y2Ba）;GyD.F4y2Ba为了GyD.F4y2Bai = 1:numSamples [accelBody,gyroBody] = IMU(accNED(i，:)，angVelNED(i，:)，orientationNED(i，:));orientationNondefault (i) = aFilter (accelBody gyroBody);GyD.F4y2Ba结尾GyD.F4y2Ba发行版(aFilter)图(3)图(t, eulerd (orientationNondefault,GyD.F4y2Ba'Zyx'GyD.F4y2Ba那GyD.F4y2Ba'框架'GyD.F4y2Ba)包含(GyD.F4y2Ba“时间(s)”GyD.F4y2Ba) ylabel (GyD.F4y2Ba的旋转(度)GyD.F4y2Ba)标题(GyD.F4y2Ba方向估计——真实IMU数据，非默认IMU滤波器GyD.F4y2Ba)传说(GyD.F4y2Baz轴的GyD.F4y2Ba那GyD.F4y2Ba“轴”GyD.F4y2Ba那GyD.F4y2Ba'x-轴'GyD.F4y2Ba的）GyD.F4y2Ba

量化改进后的性能GyD.F4y2BaimufilterGyD.F4y2Ba，绘制地面真实运动和返回方向之间的四元数距离GyD.F4y2BaimufilterGyD.F4y2Ba使用默认和非默认属性。GyD.F4y2Ba

qDistDefault = rad2deg (dist(取向,orientationDefault));qDistNondefault = rad2deg (dist(取向,orientationNondefault));图(4)图(t) [qDistDefault qDistNondefault])标题(GyD.F4y2Ba“与真实方向的四元数距离”GyD.F4y2Ba)传说(GyD.F4y2Ba'现实的IMU数据，默认IMU过滤器'GyD.F4y2Ba那GyD.F4y2Ba.．.GyD.F4y2Ba'现实的IMU数据，非默认IMU过滤器'GyD.F4y2Ba)包含(GyD.F4y2Ba“时间(s)”GyD.F4y2Ba) ylabel (GyD.F4y2Ba'四元距离（度）'GyD.F4y2Ba的）GyD.F4y2Ba

从角速度测量中移除偏置GyD.F4y2Ba

打开生活的脚本GyD.F4y2Ba

这个例子展示了如何消除陀螺仪偏差从IMU使用GyD.F4y2BaimufilterGyD.F4y2Ba．GyD.F4y2Ba

使用GyD.F4y2BakinematicTrajectoryGyD.F4y2Ba创建一个由两部分组成的轨迹。第一部分的角速度是恒定的GyD.F4y2BayGyD.F4y2Ba- - -GyD.F4y2BaZ.GyD.F4y2Ba相互重合。第二部分在所有三个轴上的角速度都是变化的。GyD.F4y2Ba

持续时间= 60 * 8;FS = 20;numsamples =持续时间* fs;RNG（GyD.F4y2Ba“默认”GyD.F4y2Ba的）GyD.F4y2Ba%给RNG种子，以再现噪声传感器的测量结果。GyD.F4y2BainitialAngVel =(0、0.5、0.25);finalAngVel = (-0.2, 0.6, 0.5);constantAngVel = repmat (initialAngVel,地板(numSamples / 2), 1);varyingAngVel = [linspace(initialAngVel(1)， finalAngVel(1)， ceil(numSamples/2)).'，GyD.F4y2Ba.．.GyD.F4y2BaLinspace（InitiNANGVEL（2），Finalangvel（2），CEIL（NUMSAMPLES / 2））。'，GyD.F4y2Ba.．.GyD.F4y2Balinspace (initialAngVel (3) finalAngVel(3),装天花板(numSamples / 2)。');angVelBody = [constantAngVel;varyingAngVel];accBody = 0 (numSamples, 3);traj = kinematicTrajectory (GyD.F4y2Ba“SampleRate”GyD.F4y2Bafs);[〜，qned，〜，加载，angvelned] = traj（Accody，Angvelbody）;GyD.F4y2Ba

创建一个GyD.F4y2BaimuSensorGyD.F4y2Ba系统对象™,GyD.F4y2BaIMUGyD.F4y2Ba，使用非理想陀螺仪。调用GyD.F4y2BaIMUGyD.F4y2Ba加速度，角速度和方向。GyD.F4y2Ba

IMU = imuSensor (GyD.F4y2Ba“accel-gyro”GyD.F4y2Ba那GyD.F4y2Ba.．.GyD.F4y2Ba“陀螺仪”GyD.F4y2Bagyroparams (GyD.F4y2Ba'randywalk'GyD.F4y2Ba, 0.003,GyD.F4y2Ba'constantbias'GyD.F4y2Ba，0.3），GyD.F4y2Ba.．.GyD.F4y2Ba“SampleRate”GyD.F4y2Bafs);[accelreads, gyroReadingsBody] = IMU(accNED,angVelNED,qNED);GyD.F4y2Ba

创建一个GyD.F4y2BaimufilterGyD.F4y2Ba系统对象,GyD.F4y2Ba保险丝GyD.F4y2Ba．调用GyD.F4y2Ba保险丝GyD.F4y2Ba用建模的加速度计读数和陀螺仪读数。GyD.F4y2Ba

保险丝= imufilter (GyD.F4y2Ba“SampleRate”GyD.F4y2Bafs,GyD.F4y2Ba“GyroscopeDriftNoise”GyD.F4y2Ba1 e-6);[~, angVelBodyRecovered] =保险丝(accelReadings gyroReadingsBody);GyD.F4y2Ba

绘制地面真实角速度，陀螺仪读数，以及每个轴的恢复角速度。GyD.F4y2Ba

角速度从GyD.F4y2BaimufilterGyD.F4y2Ba补偿陀螺仪偏置随时间并收敛到真正的角速度的影响。GyD.F4y2Ba

时间= (0:numSamples-1) / fs;图(1)图(时间,angVelBody (: 1),GyD.F4y2Ba.．.GyD.F4y2Ba时间，回电子（：1），GyD.F4y2Ba.．.GyD.F4y2Ba时间,angVelBodyRecovered(: 1)标题(GyD.F4y2Ba'x-轴'GyD.F4y2Ba)传说(GyD.F4y2Ba“真正的角速度”GyD.F4y2Ba那GyD.F4y2Ba.．.GyD.F4y2Ba“陀螺仪数据”GyD.F4y2Ba那GyD.F4y2Ba.．.GyD.F4y2Ba“恢复角速度”GyD.F4y2Ba) ylabel (GyD.F4y2Ba“角速度(rad / s)”GyD.F4y2Ba的）GyD.F4y2Ba

图中包含一个轴对象。具有标题x轴的轴对象包含3个类型的线。这些对象表示真正的角速度，陀螺仪读数，恢复的角速度。GyD.F4y2Ba

图(2)图(时间,angVelBody (:, 2),GyD.F4y2Ba.．.GyD.F4y2Ba时间,gyroReadingsBody (:, 2),GyD.F4y2Ba.．.GyD.F4y2Ba时间,angVelBodyRecovered(: 2)标题(GyD.F4y2Ba“轴”GyD.F4y2Ba) ylabel (GyD.F4y2Ba“角速度(rad / s)”GyD.F4y2Ba的）GyD.F4y2Ba

图中包含一个轴对象。标题为y轴的axis对象包含3个类型为line的对象。GyD.F4y2Ba

图(3)图(时间,angVelBody (:, 3),GyD.F4y2Ba.．.GyD.F4y2Ba时间,gyroReadingsBody (:, 3),GyD.F4y2Ba.．.GyD.F4y2Ba时间,angVelBodyRecovered(: 3)标题(GyD.F4y2Baz轴的GyD.F4y2Ba) ylabel (GyD.F4y2Ba“角速度(rad / s)”GyD.F4y2Ba)包含(GyD.F4y2Ba“时间(s)”GyD.F4y2Ba的）GyD.F4y2Ba

图中包含一个轴对象。标题为z轴的轴对象包含3个类型为line的对象。GyD.F4y2Ba

算法GyD.F4y2Ba

全部展开GyD.F4y2Ba

注:以下算法仅适用于NED参考帧。GyD.F4y2Ba

这GyD.F4y2BaimufilterGyD.F4y2Ba采用中描述的六轴卡尔曼滤波结构GyD.F4y2Ba［1］GyD.F4y2Ba．该算法试图跟踪方向、陀螺仪偏移和线性加速度的误差，以输出最终方向和角速度。与直接跟踪方向不同，间接卡尔曼滤波器模拟误差过程，GyD.F4y2BaXGyD.F4y2Ba，并进行递归更新:GyD.F4y2Ba

${XGyD.F4y2Ba}_{K.GyD.F4y2Ba} =GyD.F4y2Ba [GyD.F4y2Ba \begin{matrix} {θGyD.F4y2Ba}_{K.GyD.F4y2Ba} \\ {B.GyD.F4y2Ba}_{K.GyD.F4y2Ba} \\ {一种GyD.F4y2Ba}_{K.GyD.F4y2Ba} \end{matrix}]GyD.F4y2Ba =GyD.F4y2Ba {FGyD.F4y2Ba}_{K.GyD.F4y2Ba} [GyD.F4y2Ba \begin{matrix} {θGyD.F4y2Ba}_{K.GyD.F4y2Ba -GyD.F4y2Ba 1GyD.F4y2Ba} \\ {B.GyD.F4y2Ba}_{K.GyD.F4y2Ba -GyD.F4y2Ba 1GyD.F4y2Ba} \\ {一种GyD.F4y2Ba}_{K.GyD.F4y2Ba -GyD.F4y2Ba 1GyD.F4y2Ba} \end{matrix}]GyD.F4y2Ba +GyD.F4y2Ba {W.GyD.F4y2Ba}_{K.GyD.F4y2Ba}$

在哪里GyD.F4y2BaXGyD.F4y2Ba_K.GyD.F4y2Ba是一个9乘1的向量，包括:GyD.F4y2Ba

θGyD.F4y2Ba_K.GyD.F4y2Ba- 3×1定向误差矢量，以度为单位GyD.F4y2BaK.GyD.F4y2Ba
B.GyD.F4y2Ba_K.GyD.F4y2Ba- 3 × 1陀螺仪零角速率偏差矢量，deg/s，在时间GyD.F4y2BaK.GyD.F4y2Ba
一种GyD.F4y2Ba_K.GyD.F4y2Ba——在传感器帧中测量的3 × 1加速度误差矢量，单位为g，在时间GyD.F4y2BaK.GyD.F4y2Ba
W.GyD.F4y2Ba_K.GyD.F4y2Ba——9乘1的加性噪声向量GyD.F4y2Ba
FGyD.F4y2Ba_K.GyD.F4y2Ba- 州过渡模型GyD.F4y2Ba

因为GyD.F4y2BaXGyD.F4y2Ba_K.GyD.F4y2Ba定义为错误处理，GyD.F4y2Ba先天的GyD.F4y2Ba估计总是零，因此状态转移模型，GyD.F4y2BaFGyD.F4y2Ba_K.GyD.F4y2Ba,是零。这一见解导致了以下标准卡尔曼方程的简化:GyD.F4y2Ba

标准卡尔曼滤波方程:GyD.F4y2Ba

$\begin{array}{l} {XGyD.F4y2Ba}_{K.GyD.F4y2Ba}^{-GyD.F4y2Ba} =GyD.F4y2Ba {FGyD.F4y2Ba}_{K.GyD.F4y2Ba} {XGyD.F4y2Ba}_{K.GyD.F4y2Ba -GyD.F4y2Ba 1GyD.F4y2Ba}^{+GyD.F4y2Ba} \\ {P.GyD.F4y2Ba}_{K.GyD.F4y2Ba}^{-GyD.F4y2Ba} =GyD.F4y2Ba {FGyD.F4y2Ba}_{K.GyD.F4y2Ba} {P.GyD.F4y2Ba}_{K.GyD.F4y2Ba -GyD.F4y2Ba 1GyD.F4y2Ba}^{+GyD.F4y2Ba} {FGyD.F4y2Ba}_{K.GyD.F4y2Ba}^{T.GyD.F4y2Ba} +GyD.F4y2Ba {问：GyD.F4y2Ba}_{K.GyD.F4y2Ba} \\ {yGyD.F4y2Ba}_{K.GyD.F4y2Ba} =GyD.F4y2Ba {Z.GyD.F4y2Ba}_{K.GyD.F4y2Ba} -GyD.F4y2Ba {HGyD.F4y2Ba}_{K.GyD.F4y2Ba} {XGyD.F4y2Ba}_{K.GyD.F4y2Ba}^{-GyD.F4y2Ba} \\ {S.GyD.F4y2Ba}_{K.GyD.F4y2Ba} =GyD.F4y2Ba {R.GyD.F4y2Ba}_{K.GyD.F4y2Ba} +GyD.F4y2Ba {HGyD.F4y2Ba}_{K.GyD.F4y2Ba} {P.GyD.F4y2Ba}_{K.GyD.F4y2Ba}^{-GyD.F4y2Ba} {HGyD.F4y2Ba}_{K.GyD.F4y2Ba}^{T.GyD.F4y2Ba} \\ {K.GyD.F4y2Ba}_{K.GyD.F4y2Ba} =GyD.F4y2Ba {P.GyD.F4y2Ba}_{K.GyD.F4y2Ba}^{-GyD.F4y2Ba} {HGyD.F4y2Ba}_{K.GyD.F4y2Ba}^{T.GyD.F4y2Ba} {（GyD.F4y2Ba {S.GyD.F4y2Ba}_{K.GyD.F4y2Ba} 的）GyD.F4y2Ba}^{-GyD.F4y2Ba 1GyD.F4y2Ba} \\ {XGyD.F4y2Ba}_{K.GyD.F4y2Ba}^{+GyD.F4y2Ba} =GyD.F4y2Ba {XGyD.F4y2Ba}_{K.GyD.F4y2Ba}^{-GyD.F4y2Ba} +GyD.F4y2Ba {K.GyD.F4y2Ba}_{K.GyD.F4y2Ba} {yGyD.F4y2Ba}_{K.GyD.F4y2Ba} \\ {P.GyD.F4y2Ba}_{K.GyD.F4y2Ba}^{+GyD.F4y2Ba} =GyD.F4y2Ba {P.GyD.F4y2Ba}_{K.GyD.F4y2Ba}^{-GyD.F4y2Ba} -GyD.F4y2Ba {K.GyD.F4y2Ba}_{K.GyD.F4y2Ba} {HGyD.F4y2Ba}_{K.GyD.F4y2Ba} {P.GyD.F4y2Ba}_{K.GyD.F4y2Ba}^{-GyD.F4y2Ba} \end{array}$

算法中使用的卡尔曼方程:GyD.F4y2Ba

$\begin{array}{l} {XGyD.F4y2Ba}_{K.GyD.F4y2Ba}^{-GyD.F4y2Ba} =GyD.F4y2Ba 0.GyD.F4y2Ba \\ {P.GyD.F4y2Ba}_{K.GyD.F4y2Ba}^{-GyD.F4y2Ba} =GyD.F4y2Ba {问：GyD.F4y2Ba}_{K.GyD.F4y2Ba} \\ {yGyD.F4y2Ba}_{K.GyD.F4y2Ba} =GyD.F4y2Ba {Z.GyD.F4y2Ba}_{K.GyD.F4y2Ba} \\ {S.GyD.F4y2Ba}_{K.GyD.F4y2Ba} =GyD.F4y2Ba {R.GyD.F4y2Ba}_{K.GyD.F4y2Ba} +GyD.F4y2Ba {HGyD.F4y2Ba}_{K.GyD.F4y2Ba} {P.GyD.F4y2Ba}_{K.GyD.F4y2Ba}^{-GyD.F4y2Ba} {HGyD.F4y2Ba}_{K.GyD.F4y2Ba}^{T.GyD.F4y2Ba} \\ {K.GyD.F4y2Ba}_{K.GyD.F4y2Ba} =GyD.F4y2Ba {P.GyD.F4y2Ba}_{K.GyD.F4y2Ba}^{-GyD.F4y2Ba} {HGyD.F4y2Ba}_{K.GyD.F4y2Ba}^{T.GyD.F4y2Ba} {（GyD.F4y2Ba {S.GyD.F4y2Ba}_{K.GyD.F4y2Ba} 的）GyD.F4y2Ba}^{-GyD.F4y2Ba 1GyD.F4y2Ba} \\ {XGyD.F4y2Ba}_{K.GyD.F4y2Ba}^{+GyD.F4y2Ba} =GyD.F4y2Ba {K.GyD.F4y2Ba}_{K.GyD.F4y2Ba} {yGyD.F4y2Ba}_{K.GyD.F4y2Ba} \\ {P.GyD.F4y2Ba}_{K.GyD.F4y2Ba}^{+GyD.F4y2Ba} =GyD.F4y2Ba {P.GyD.F4y2Ba}_{K.GyD.F4y2Ba}^{-GyD.F4y2Ba} -GyD.F4y2Ba {K.GyD.F4y2Ba}_{K.GyD.F4y2Ba} {HGyD.F4y2Ba}_{K.GyD.F4y2Ba} {P.GyD.F4y2Ba}_{K.GyD.F4y2Ba}^{-GyD.F4y2Ba} \end{array}$

在哪里GyD.F4y2Ba

XGyD.F4y2Ba_K.GyD.F4y2Ba^{−GyD.F4y2Ba}——预测(GyD.F4y2Ba先天的GyD.F4y2Ba)状态估计;错误的过程GyD.F4y2Ba
P.GyD.F4y2Ba_K.GyD.F4y2Ba^{−GyD.F4y2Ba}——预测(GyD.F4y2Ba先天的GyD.F4y2Ba)估计协方差GyD.F4y2Ba
yGyD.F4y2Ba_K.GyD.F4y2Ba——创新GyD.F4y2Ba
S.GyD.F4y2Ba_K.GyD.F4y2Ba——创新协方差GyD.F4y2Ba
K.GyD.F4y2Ba_K.GyD.F4y2Ba——卡尔曼增益GyD.F4y2Ba
XGyD.F4y2Ba_K.GyD.F4y2Ba^+GyD.F4y2Ba——更新(GyD.F4y2Ba一个后念GyD.F4y2Ba）国家估计GyD.F4y2Ba
P.GyD.F4y2Ba_K.GyD.F4y2Ba^+GyD.F4y2Ba——更新(GyD.F4y2Ba一个后念GyD.F4y2Ba)估计协方差GyD.F4y2Ba

K.GyD.F4y2Ba表示迭代，上标GyD.F4y2Ba^+GyD.F4y2Ba代表A.GyD.F4y2Ba一个后念GyD.F4y2Ba估计和上标GyD.F4y2Ba^{−GyD.F4y2Ba}代表A.GyD.F4y2Ba先天的GyD.F4y2Ba估计。GyD.F4y2Ba

图和下面的步骤描述了通过算法的基于单一帧的迭代。GyD.F4y2Ba

算法流程图GyD.F4y2Ba

在第一次迭代之前GyD.F4y2BaaccelReadingsGyD.F4y2Ba和GyD.F4y2Ba回电GyD.F4y2Ba输入被分成1 × 3的帧GyD.F4y2BaDecimationFactorGyD.F4y2Ba-By-3框架。该算法使用与陀螺仪读数相对应的最电流的加速度计读数。GyD.F4y2Ba

详细的概述GyD.F4y2Ba

通过对算法的分步讲解，对每个阶段进行了详细的概述。GyD.F4y2Ba

模型GyD.F4y2Ba

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

预测方向GyD.F4y2Ba

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

$ΔGyD.F4y2Ba {φGyD.F4y2Ba}_{N.GyD.F4y2Ba \timesGyD.F4y2Ba 3.GyD.F4y2Ba} =GyD.F4y2Ba \frac{（GyD.F4y2Ba GGyD.F4y2Ba yGyD.F4y2Ba R.GyD.F4y2Ba O.GyD.F4y2Ba R.GyD.F4y2Ba E.GyD.F4y2Ba 一种GyD.F4y2Ba D.GyD.F4y2Ba 一世GyD.F4y2Ba N.GyD.F4y2Ba GGyD.F4y2Ba {S.GyD.F4y2Ba}_{N.GyD.F4y2Ba \timesGyD.F4y2Ba 3.GyD.F4y2Ba} -GyD.F4y2Ba GGyD.F4y2Ba yGyD.F4y2Ba R.GyD.F4y2Ba O.GyD.F4y2Ba O.GyD.F4y2Ba FGyD.F4y2Ba FGyD.F4y2Ba S.GyD.F4y2Ba E.GyD.F4y2Ba {T.GyD.F4y2Ba}_{1GyD.F4y2Ba \timesGyD.F4y2Ba 3.GyD.F4y2Ba} 的）GyD.F4y2Ba}{FGyD.F4y2Ba S.GyD.F4y2Ba}$

在哪里GyD.F4y2BaN.GyD.F4y2Ba是由此指定的抽取因子GyD.F4y2BaDecimationFactorGyD.F4y2Ba财产,GyD.F4y2BaFS.GyD.F4y2Ba样品率是否由GyD.F4y2BaSampleRateGyD.F4y2Ba财产。GyD.F4y2Ba

角的变化被转换成四元数GyD.F4y2BarotvecGyD.F4y2Ba四元数GyD.F4y2Ba施工语法：GyD.F4y2Ba

$ΔGyD.F4y2Ba {问：GyD.F4y2Ba}_{N.GyD.F4y2Ba \timesGyD.F4y2Ba 1GyD.F4y2Ba} =GyD.F4y2Ba 四元数GyD.F4y2Ba （GyD.F4y2Ba ΔGyD.F4y2Ba {φGyD.F4y2Ba}_{N.GyD.F4y2Ba \timesGyD.F4y2Ba 3.GyD.F4y2Ba} 那GyD.F4y2Ba'GyD.F4y2Ba R.GyD.F4y2Ba O.GyD.F4y2Ba T.GyD.F4y2Ba V.GyD.F4y2Ba E.GyD.F4y2Ba CGyD.F4y2Ba'GyD.F4y2Ba 的）GyD.F4y2Ba$

通过Δ通过旋转Δ更新以前的方向估计值GyD.F4y2Ba问：GyD.F4y2Ba：GyD.F4y2Ba

${问：GyD.F4y2Ba}_{1GyD.F4y2Ba \timesGyD.F4y2Ba 1GyD.F4y2Ba}^{-GyD.F4y2Ba} =GyD.F4y2Ba （GyD.F4y2Ba {问：GyD.F4y2Ba}_{1GyD.F4y2Ba \timesGyD.F4y2Ba 1GyD.F4y2Ba}^{+GyD.F4y2Ba} 的）GyD.F4y2Ba （GyD.F4y2Ba {\prodGyD.F4y2Ba}_{N.GyD.F4y2Ba =GyD.F4y2Ba 1GyD.F4y2Ba}^{N.GyD.F4y2Ba} ΔGyD.F4y2Ba {问：GyD.F4y2Ba}_{N.GyD.F4y2Ba} 的）GyD.F4y2Ba$

在第一次迭代中，方向估计，GyD.F4y2Ba问：GyD.F4y2Ba^{−GyD.F4y2Ba}，由GyD.F4y2BaecompassGyD.F4y2Ba假设GyD.F4y2BaXGyD.F4y2Ba设在北。GyD.F4y2Ba

从方位估计重力GyD.F4y2Ba

重力矢量被解释为四元数的第三列，GyD.F4y2Ba问：GyD.F4y2Ba^{−GyD.F4y2Ba}，以旋转矩阵形式：GyD.F4y2Ba

${GGyD.F4y2Ba}_{1GyD.F4y2Ba \timesGyD.F4y2Ba 3.GyD.F4y2Ba} =GyD.F4y2Ba {（GyD.F4y2Ba R.GyD.F4y2Ba P.GyD.F4y2Ba R.GyD.F4y2Ba 一世GyD.F4y2Ba O.GyD.F4y2Ba R.GyD.F4y2Ba （GyD.F4y2Ba ：GyD.F4y2Ba 那GyD.F4y2Ba 3.GyD.F4y2Ba 的）GyD.F4y2Ba 的）GyD.F4y2Ba}^{T.GyD.F4y2Ba}$

看GyD.F4y2BaecompassGyD.F4y2Ba来解释为什么第三列GyD.F4y2BarPriorGyD.F4y2Ba可以解释为重力矢量。GyD.F4y2Ba

从加速度估计重力GyD.F4y2Ba

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

$GGyD.F4y2Ba 一种GyD.F4y2Ba CGyD.F4y2Ba CGyD.F4y2Ba E.GyD.F4y2Ba {L.GyD.F4y2Ba}_{1GyD.F4y2Ba \timesGyD.F4y2Ba 3.GyD.F4y2Ba} =GyD.F4y2Ba 一种GyD.F4y2Ba CGyD.F4y2Ba CGyD.F4y2Ba E.GyD.F4y2Ba L.GyD.F4y2Ba R.GyD.F4y2Ba E.GyD.F4y2Ba 一种GyD.F4y2Ba D.GyD.F4y2Ba 一世GyD.F4y2Ba N.GyD.F4y2Ba GGyD.F4y2Ba {S.GyD.F4y2Ba}_{1GyD.F4y2Ba \timesGyD.F4y2Ba 3.GyD.F4y2Ba} -GyD.F4y2Ba L.GyD.F4y2Ba 一世GyD.F4y2Ba N.GyD.F4y2Ba 一种GyD.F4y2Ba CGyD.F4y2Ba CGyD.F4y2Ba E.GyD.F4y2Ba L.GyD.F4y2Ba P.GyD.F4y2Ba R.GyD.F4y2Ba 一世GyD.F4y2Ba O.GyD.F4y2Ba {R.GyD.F4y2Ba}_{1GyD.F4y2Ba \timesGyD.F4y2Ba 3.GyD.F4y2Ba}$

误差模型GyD.F4y2Ba

误差模型是加速度计读数估计的重力值与陀螺仪读数估计的重力值之间的差值:GyD.F4y2Ba $Z.GyD.F4y2Ba =GyD.F4y2Ba GGyD.F4y2Ba -GyD.F4y2Ba GGyD.F4y2Ba 一种GyD.F4y2Ba CGyD.F4y2Ba CGyD.F4y2Ba E.GyD.F4y2Ba L.GyD.F4y2Ba$ ．GyD.F4y2Ba

卡尔曼方程式GyD.F4y2Ba

卡尔曼方程使用从陀螺仪读数中衍生的重力估计，GyD.F4y2BaGGyD.F4y2Ba，观察误差过程，GyD.F4y2BaZ.GyD.F4y2Ba，更新卡尔曼增益和中间协方差矩阵。将卡尔曼增益应用于误差信号，GyD.F4y2BaZ.GyD.F4y2Ba，输出GyD.F4y2Ba一个后念GyD.F4y2Ba误差估计,GyD.F4y2BaXGyD.F4y2Ba^+GyD.F4y2Ba．GyD.F4y2Ba

观察模型GyD.F4y2Ba

观测模型绘制了1乘3的观测状态，GyD.F4y2BaGGyD.F4y2Ba，进入3乘9的真实状态，GyD.F4y2BaHGyD.F4y2Ba．GyD.F4y2Ba

观察模型构造为：GyD.F4y2Ba

${HGyD.F4y2Ba}_{3.GyD.F4y2Ba \timesGyD.F4y2Ba 9.GyD.F4y2Ba} =GyD.F4y2Ba [GyD.F4y2Ba \begin{matrix} 0.GyD.F4y2Ba & {GGyD.F4y2Ba}_{Z.GyD.F4y2Ba} & -GyD.F4y2Ba {GGyD.F4y2Ba}_{yGyD.F4y2Ba} & 0.GyD.F4y2Ba & -GyD.F4y2Ba κGyD.F4y2Ba {GGyD.F4y2Ba}_{Z.GyD.F4y2Ba} & κGyD.F4y2Ba {GGyD.F4y2Ba}_{yGyD.F4y2Ba} & 1GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba \\ -GyD.F4y2Ba {GGyD.F4y2Ba}_{Z.GyD.F4y2Ba} & 0.GyD.F4y2Ba & {GGyD.F4y2Ba}_{XGyD.F4y2Ba} & κGyD.F4y2Ba {GGyD.F4y2Ba}_{Z.GyD.F4y2Ba} & 0.GyD.F4y2Ba & -GyD.F4y2Ba κGyD.F4y2Ba {GGyD.F4y2Ba}_{XGyD.F4y2Ba} & 0.GyD.F4y2Ba & 1GyD.F4y2Ba & 0.GyD.F4y2Ba \\ {GGyD.F4y2Ba}_{yGyD.F4y2Ba} & -GyD.F4y2Ba {GGyD.F4y2Ba}_{XGyD.F4y2Ba} & 0.GyD.F4y2Ba & -GyD.F4y2Ba κGyD.F4y2Ba {GGyD.F4y2Ba}_{yGyD.F4y2Ba} & κGyD.F4y2Ba {GGyD.F4y2Ba}_{XGyD.F4y2Ba} & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & 1GyD.F4y2Ba \end{matrix}]GyD.F4y2Ba$

在哪里GyD.F4y2BaGGyD.F4y2Ba_XGyD.F4y2Ba那GyD.F4y2BaGGyD.F4y2Ba_yGyD.F4y2Ba,GyD.F4y2BaGGyD.F4y2Ba_Z.GyD.F4y2Ba是GyD.F4y2BaXGyD.F4y2Ba-，GyD.F4y2BayGyD.F4y2Ba-，和GyD.F4y2BaZ.GyD.F4y2Ba-分别由方向估计的重力矢量元素。GyD.F4y2BaκGyD.F4y2Ba常数是由GyD.F4y2BaSampleRateGyD.F4y2Ba和GyD.F4y2BaDecimationFactorGyD.F4y2Ba属性:GyD.F4y2BaκGyD.F4y2Ba=GyD.F4y2BaDecimationFactorGyD.F4y2Ba/GyD.F4y2BaSampleRateGyD.F4y2Ba．GyD.F4y2Ba

参见第7.3和7.4节GyD.F4y2Ba［1］GyD.F4y2Ba用于推导观测模型。GyD.F4y2Ba

创新协方差GyD.F4y2Ba

创新协方差是一个3 × 3矩阵，用于跟踪测量中的可变性。创新协方差矩阵计算为:GyD.F4y2Ba

${S.GyD.F4y2Ba}_{3.GyD.F4y2Ba XGyD.F4y2Ba 3.GyD.F4y2Ba} =GyD.F4y2Ba {R.GyD.F4y2Ba}_{3.GyD.F4y2Ba XGyD.F4y2Ba 3.GyD.F4y2Ba} +GyD.F4y2Ba （GyD.F4y2Ba {HGyD.F4y2Ba}_{3.GyD.F4y2Ba XGyD.F4y2Ba 9.GyD.F4y2Ba} 的）GyD.F4y2Ba （GyD.F4y2Ba {P.GyD.F4y2Ba}_{_{9.GyD.F4y2Ba XGyD.F4y2Ba 9.GyD.F4y2Ba}}^{-GyD.F4y2Ba} 的）GyD.F4y2Ba {（GyD.F4y2Ba {HGyD.F4y2Ba}_{3.GyD.F4y2Ba XGyD.F4y2Ba 9.GyD.F4y2Ba} 的）GyD.F4y2Ba}^{T.GyD.F4y2Ba}$

在哪里GyD.F4y2Ba

HGyD.F4y2Ba观察模型是矩阵吗GyD.F4y2Ba
P.GyD.F4y2Ba^{−GyD.F4y2Ba}是预测的（GyD.F4y2Ba先天的GyD.F4y2Ba)观测模型在前一次迭代中计算的协方差的估计GyD.F4y2Ba
R.GyD.F4y2Ba为观测模型噪声的协方差，计算为:GyD.F4y2Ba

${R.GyD.F4y2Ba}_{3.GyD.F4y2Ba \timesGyD.F4y2Ba 3.GyD.F4y2Ba} =GyD.F4y2Ba （GyD.F4y2Ba λGyD.F4y2Ba +GyD.F4y2Ba ξGyD.F4y2Ba +GyD.F4y2Ba κGyD.F4y2Ba （GyD.F4y2Ba βGyD.F4y2Ba +GyD.F4y2Ba ηGyD.F4y2Ba 的）GyD.F4y2Ba 的）GyD.F4y2Ba [GyD.F4y2Ba \begin{matrix} 1GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba \\ 0.GyD.F4y2Ba & 1GyD.F4y2Ba & 0.GyD.F4y2Ba \\ 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & 1GyD.F4y2Ba \end{matrix}]GyD.F4y2Ba ．GyD.F4y2Ba$

观测模型噪声方差定义如下:GyD.F4y2Ba
- κGyD.F4y2Ba——(GyD.F4y2BaDecimationFactorGyD.F4y2Ba/GyD.F4y2BaSampleRateGyD.F4y2Ba的）GyD.F4y2Ba^2GyD.F4y2Ba
- βGyD.F4y2Ba- - -GyD.F4y2BaGyroscopeDriftNoiseGyD.F4y2Ba
- ηGyD.F4y2Ba- - -GyD.F4y2BaGyroscopeNoiseGyD.F4y2Ba
- λGyD.F4y2Ba- - -GyD.F4y2BaAccelerometerNoiseGyD.F4y2Ba
- ξGyD.F4y2Ba- - -GyD.F4y2Balinearaccelerationnoise.GyD.F4y2Ba

更新误差估计协方差GyD.F4y2Ba

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

错误估计协方差矩阵已更新为：GyD.F4y2Ba

${P.GyD.F4y2Ba}_{_{9.GyD.F4y2Ba \timesGyD.F4y2Ba 9.GyD.F4y2Ba}}^{+GyD.F4y2Ba} =GyD.F4y2Ba {P.GyD.F4y2Ba}_{_{9.GyD.F4y2Ba \timesGyD.F4y2Ba 9.GyD.F4y2Ba}}^{-GyD.F4y2Ba} -GyD.F4y2Ba （GyD.F4y2Ba {K.GyD.F4y2Ba}_{9.GyD.F4y2Ba \timesGyD.F4y2Ba 3.GyD.F4y2Ba} 的）GyD.F4y2Ba （GyD.F4y2Ba {HGyD.F4y2Ba}_{3.GyD.F4y2Ba \timesGyD.F4y2Ba 9.GyD.F4y2Ba} 的）GyD.F4y2Ba （GyD.F4y2Ba {P.GyD.F4y2Ba}_{_{9.GyD.F4y2Ba \timesGyD.F4y2Ba 9.GyD.F4y2Ba}}^{-GyD.F4y2Ba} 的）GyD.F4y2Ba$

在哪里GyD.F4y2BaK.GyD.F4y2Ba是卡尔曼增益，GyD.F4y2BaHGyD.F4y2Ba是测量矩阵，和GyD.F4y2BaP.GyD.F4y2Ba^{−GyD.F4y2Ba}是在前次迭代期间计算的错误估计协方差。GyD.F4y2Ba

预测误差估计协方差GyD.F4y2Ba

误差估计协方差是一个9乘9的矩阵，用于跟踪状态的可变性。这GyD.F4y2Ba先天的GyD.F4y2Ba错误估计协方差,GyD.F4y2BaP.GyD.F4y2Ba^{−GyD.F4y2Ba}，为过程噪声协方差，GyD.F4y2Ba问：GyD.F4y2Ba，在之前的迭代中确定。GyD.F4y2Ba问：GyD.F4y2Ba的函数GyD.F4y2Ba一个后念GyD.F4y2Ba错误估计协方差,GyD.F4y2BaP.GyD.F4y2Ba^+GyD.F4y2Ba．计算时GyD.F4y2Ba问：GyD.F4y2Ba，假设互相关项与自关项相比可以忽略不计，并设为零:GyD.F4y2Ba

$问：GyD.F4y2Ba =GyD.F4y2Ba [GyD.F4y2Ba \begin{matrix} {P.GyD.F4y2Ba}^{+GyD.F4y2Ba} （GyD.F4y2Ba 1GyD.F4y2Ba 的）GyD.F4y2Ba +GyD.F4y2Ba {κGyD.F4y2Ba}^{2GyD.F4y2Ba} {P.GyD.F4y2Ba}^{+GyD.F4y2Ba} （GyD.F4y2Ba 31GyD.F4y2Ba 的）GyD.F4y2Ba +GyD.F4y2Ba βGyD.F4y2Ba +GyD.F4y2Ba ηGyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & -GyD.F4y2Ba κGyD.F4y2Ba （GyD.F4y2Ba {P.GyD.F4y2Ba}^{+GyD.F4y2Ba} （GyD.F4y2Ba 31GyD.F4y2Ba 的）GyD.F4y2Ba +GyD.F4y2Ba βGyD.F4y2Ba 的）GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba \\ 0.GyD.F4y2Ba & {P.GyD.F4y2Ba}^{+GyD.F4y2Ba} （GyD.F4y2Ba 11GyD.F4y2Ba 的）GyD.F4y2Ba +GyD.F4y2Ba {κGyD.F4y2Ba}^{2GyD.F4y2Ba} {P.GyD.F4y2Ba}^{+GyD.F4y2Ba} （GyD.F4y2Ba 41GyD.F4y2Ba 的）GyD.F4y2Ba +GyD.F4y2Ba βGyD.F4y2Ba +GyD.F4y2Ba ηGyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & -GyD.F4y2Ba κGyD.F4y2Ba （GyD.F4y2Ba {P.GyD.F4y2Ba}^{+GyD.F4y2Ba} （GyD.F4y2Ba 41GyD.F4y2Ba 的）GyD.F4y2Ba +GyD.F4y2Ba βGyD.F4y2Ba 的）GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba \\ 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & {P.GyD.F4y2Ba}^{+GyD.F4y2Ba} （GyD.F4y2Ba 21GyD.F4y2Ba 的）GyD.F4y2Ba +GyD.F4y2Ba {κGyD.F4y2Ba}^{2GyD.F4y2Ba} {P.GyD.F4y2Ba}^{+GyD.F4y2Ba} （GyD.F4y2Ba 51GyD.F4y2Ba 的）GyD.F4y2Ba +GyD.F4y2Ba βGyD.F4y2Ba +GyD.F4y2Ba ηGyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & -GyD.F4y2Ba κGyD.F4y2Ba （GyD.F4y2Ba {P.GyD.F4y2Ba}^{+GyD.F4y2Ba} （GyD.F4y2Ba 51GyD.F4y2Ba 的）GyD.F4y2Ba +GyD.F4y2Ba βGyD.F4y2Ba 的）GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba \\ -GyD.F4y2Ba κGyD.F4y2Ba （GyD.F4y2Ba {P.GyD.F4y2Ba}^{+GyD.F4y2Ba} （GyD.F4y2Ba 31GyD.F4y2Ba 的）GyD.F4y2Ba +GyD.F4y2Ba βGyD.F4y2Ba 的）GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & {P.GyD.F4y2Ba}^{+GyD.F4y2Ba} （GyD.F4y2Ba 31GyD.F4y2Ba 的）GyD.F4y2Ba +GyD.F4y2Ba βGyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba \\ 0.GyD.F4y2Ba & -GyD.F4y2Ba κGyD.F4y2Ba （GyD.F4y2Ba {P.GyD.F4y2Ba}^{+GyD.F4y2Ba} （GyD.F4y2Ba 41GyD.F4y2Ba 的）GyD.F4y2Ba +GyD.F4y2Ba βGyD.F4y2Ba 的）GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & {P.GyD.F4y2Ba}^{+GyD.F4y2Ba} （GyD.F4y2Ba 41GyD.F4y2Ba 的）GyD.F4y2Ba +GyD.F4y2Ba βGyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba \\ 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & -GyD.F4y2Ba κGyD.F4y2Ba （GyD.F4y2Ba {P.GyD.F4y2Ba}^{+GyD.F4y2Ba} （GyD.F4y2Ba 51GyD.F4y2Ba 的）GyD.F4y2Ba +GyD.F4y2Ba βGyD.F4y2Ba 的）GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & {P.GyD.F4y2Ba}^{+GyD.F4y2Ba} （GyD.F4y2Ba 51GyD.F4y2Ba 的）GyD.F4y2Ba +GyD.F4y2Ba βGyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba \\ 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & {νGyD.F4y2Ba}^{2GyD.F4y2Ba} {P.GyD.F4y2Ba}^{+GyD.F4y2Ba} （GyD.F4y2Ba 61GyD.F4y2Ba 的）GyD.F4y2Ba +GyD.F4y2Ba ξGyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba \\ 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & {νGyD.F4y2Ba}^{2GyD.F4y2Ba} {P.GyD.F4y2Ba}^{+GyD.F4y2Ba} （GyD.F4y2Ba 71GyD.F4y2Ba 的）GyD.F4y2Ba +GyD.F4y2Ba ξGyD.F4y2Ba & 0.GyD.F4y2Ba \\ 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & 0.GyD.F4y2Ba & {νGyD.F4y2Ba}^{2GyD.F4y2Ba} {P.GyD.F4y2Ba}^{+GyD.F4y2Ba} （GyD.F4y2Ba 81GyD.F4y2Ba 的）GyD.F4y2Ba +GyD.F4y2Ba ξGyD.F4y2Ba \end{matrix}]GyD.F4y2Ba$

在哪里GyD.F4y2Ba

P.GyD.F4y2Ba^+GyD.F4y2Ba——是更新的(GyD.F4y2Ba一个后念GyD.F4y2Ba)误差估计协方差GyD.F4y2Ba
κGyD.F4y2Ba- - -GyD.F4y2BaDecimationFactorGyD.F4y2Ba/GyD.F4y2BaSampleRateGyD.F4y2Ba
βGyD.F4y2Ba- - -GyD.F4y2BaGyroscopeDriftNoiseGyD.F4y2Ba
ηGyD.F4y2Ba- - -GyD.F4y2BaGyroscopeNoiseGyD.F4y2Ba
νGyD.F4y2Ba- - -GyD.F4y2BaLinearAcclerationDecayFactorGyD.F4y2Ba
ξGyD.F4y2Ba- - -GyD.F4y2Balinearaccelerationnoise.GyD.F4y2Ba

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

卡尔曼增益GyD.F4y2Ba

卡尔曼增益矩阵是一个9乘3的矩阵，用于对创新进行加权。在这个算法中，创新被解释为误差过程，GyD.F4y2BaZ.GyD.F4y2Ba．GyD.F4y2Ba

卡尔曼增益矩阵被构造为：GyD.F4y2Ba

${K.GyD.F4y2Ba}_{9.GyD.F4y2Ba \timesGyD.F4y2Ba 3.GyD.F4y2Ba} =GyD.F4y2Ba （GyD.F4y2Ba {P.GyD.F4y2Ba}_{_{9.GyD.F4y2Ba \timesGyD.F4y2Ba 9.GyD.F4y2Ba}}^{-GyD.F4y2Ba} 的）GyD.F4y2Ba {（GyD.F4y2Ba {HGyD.F4y2Ba}_{3.GyD.F4y2Ba \timesGyD.F4y2Ba 9.GyD.F4y2Ba} 的）GyD.F4y2Ba}^{T.GyD.F4y2Ba} {（GyD.F4y2Ba {（GyD.F4y2Ba {S.GyD.F4y2Ba}_{3.GyD.F4y2Ba \timesGyD.F4y2Ba 3.GyD.F4y2Ba} 的）GyD.F4y2Ba}^{T.GyD.F4y2Ba} 的）GyD.F4y2Ba}^{-GyD.F4y2Ba 1GyD.F4y2Ba}$

在哪里GyD.F4y2Ba

P.GyD.F4y2Ba^-GyD.F4y2Ba——预测误差协方差GyD.F4y2Ba
HGyD.F4y2Ba- 观察模型GyD.F4y2Ba
S.GyD.F4y2Ba——创新协方差GyD.F4y2Ba

更新Posteriori错误GyD.F4y2Ba

这GyD.F4y2Ba一个后GyD.F4y2Ba误差估计由卡尔曼增益矩阵与重力矢量估计中的误差相结合确定:GyD.F4y2Ba

${XGyD.F4y2Ba}_{9.GyD.F4y2Ba \timesGyD.F4y2Ba 1GyD.F4y2Ba} =GyD.F4y2Ba （GyD.F4y2Ba {K.GyD.F4y2Ba}_{9.GyD.F4y2Ba \timesGyD.F4y2Ba 3.GyD.F4y2Ba} 的）GyD.F4y2Ba {（GyD.F4y2Ba {Z.GyD.F4y2Ba}_{1GyD.F4y2Ba \timesGyD.F4y2Ba 3.GyD.F4y2Ba} 的）GyD.F4y2Ba}^{T.GyD.F4y2Ba}$

正确的GyD.F4y2Ba

估计取向GyD.F4y2Ba

将之前的估计乘以误差来更新方向估计:GyD.F4y2Ba

${问：GyD.F4y2Ba}^{+GyD.F4y2Ba} =GyD.F4y2Ba （GyD.F4y2Ba {问：GyD.F4y2Ba}^{-GyD.F4y2Ba} 的）GyD.F4y2Ba （GyD.F4y2Ba {θGyD.F4y2Ba}^{+GyD.F4y2Ba} 的）GyD.F4y2Ba$

估计线性加速度GyD.F4y2Ba

线性加速度估计的更新方法是将线性加速度估计从之前的迭代中衰减并减去误差:GyD.F4y2Ba

$L.GyD.F4y2Ba 一世GyD.F4y2Ba N.GyD.F4y2Ba 一种GyD.F4y2Ba CGyD.F4y2Ba CGyD.F4y2Ba E.GyD.F4y2Ba L.GyD.F4y2Ba P.GyD.F4y2Ba R.GyD.F4y2Ba 一世GyD.F4y2Ba O.GyD.F4y2Ba R.GyD.F4y2Ba =GyD.F4y2Ba （GyD.F4y2Ba L.GyD.F4y2Ba 一世GyD.F4y2Ba N.GyD.F4y2Ba 一种GyD.F4y2Ba CGyD.F4y2Ba CGyD.F4y2Ba E.GyD.F4y2Ba L.GyD.F4y2Ba P.GyD.F4y2Ba R.GyD.F4y2Ba 一世GyD.F4y2Ba O.GyD.F4y2Ba {R.GyD.F4y2Ba}_{K.GyD.F4y2Ba -GyD.F4y2Ba 1GyD.F4y2Ba} 的）GyD.F4y2Ba νGyD.F4y2Ba -GyD.F4y2Ba {B.GyD.F4y2Ba}^{+GyD.F4y2Ba}$

在哪里GyD.F4y2Ba

νGyD.F4y2Ba- - -GyD.F4y2BaLinearAcclerationDecayFactorGyD.F4y2Ba

估计陀螺仪抵消GyD.F4y2Ba

通过从先前的迭代中减去陀螺仪偏移的陀螺仪偏移误差来更新陀螺仪偏移估计：GyD.F4y2Ba

$GGyD.F4y2Ba yGyD.F4y2Ba R.GyD.F4y2Ba O.GyD.F4y2Ba O.GyD.F4y2Ba FGyD.F4y2Ba FGyD.F4y2Ba S.GyD.F4y2Ba E.GyD.F4y2Ba T.GyD.F4y2Ba =GyD.F4y2Ba GGyD.F4y2Ba yGyD.F4y2Ba R.GyD.F4y2Ba O.GyD.F4y2Ba O.GyD.F4y2Ba FGyD.F4y2Ba FGyD.F4y2Ba S.GyD.F4y2Ba E.GyD.F4y2Ba {T.GyD.F4y2Ba}_{K.GyD.F4y2Ba -GyD.F4y2Ba 1GyD.F4y2Ba} -GyD.F4y2Ba {一种GyD.F4y2Ba}^{+GyD.F4y2Ba}$

计算角速度GyD.F4y2Ba

估计角速度，框架GyD.F4y2Ba回电GyD.F4y2Ba取平均值，并减去前一次迭代中计算的陀螺仪偏移量:GyD.F4y2Ba

$一种GyD.F4y2Ba N.GyD.F4y2Ba GGyD.F4y2Ba 你GyD.F4y2Ba L.GyD.F4y2Ba 一种GyD.F4y2Ba R.GyD.F4y2Ba V.GyD.F4y2Ba E.GyD.F4y2Ba L.GyD.F4y2Ba O.GyD.F4y2Ba CGyD.F4y2Ba 一世GyD.F4y2Ba T.GyD.F4y2Ba {yGyD.F4y2Ba}_{1GyD.F4y2Ba \timesGyD.F4y2Ba 3.GyD.F4y2Ba} =GyD.F4y2Ba \frac{\sumGyD.F4y2Ba GGyD.F4y2Ba yGyD.F4y2Ba R.GyD.F4y2Ba O.GyD.F4y2Ba R.GyD.F4y2Ba E.GyD.F4y2Ba 一种GyD.F4y2Ba D.GyD.F4y2Ba 一世GyD.F4y2Ba N.GyD.F4y2Ba GGyD.F4y2Ba {S.GyD.F4y2Ba}_{N.GyD.F4y2Ba \timesGyD.F4y2Ba 3.GyD.F4y2Ba}}{N.GyD.F4y2Ba} -GyD.F4y2Ba GGyD.F4y2Ba yGyD.F4y2Ba R.GyD.F4y2Ba O.GyD.F4y2Ba O.GyD.F4y2Ba FGyD.F4y2Ba FGyD.F4y2Ba S.GyD.F4y2Ba E.GyD.F4y2Ba {T.GyD.F4y2Ba}_{1GyD.F4y2Ba \timesGyD.F4y2Ba 3.GyD.F4y2Ba}$

在哪里GyD.F4y2BaN.GyD.F4y2Ba是由此指定的抽取因子GyD.F4y2BaDecimationFactorGyD.F4y2Ba财产。GyD.F4y2Ba

在第一次迭代时，将陀螺仪偏移估计初始化为零。GyD.F4y2Ba

参考文献GyD.F4y2Ba

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

Roetenberg, D.， H.J. Luinge, C.T.M. Baten和P.H. Veltink。磁扰动补偿改善了人体节段定位的惯性和磁传感。GyD.F4y2Ba神经系统与康复工程学报GyD.F4y2Ba．卷。13。2005年第3期，第395-405页。GyD.F4y2Ba

扩展功能GyD.F4y2Ba

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

使用注意事项及限制:GyD.F4y2Ba

看GyD.F4y2Ba系统对象在MATLAB代码生成GyD.F4y2Ba(MATLAB编码器)GyD.F4y2Ba．GyD.F4y2Ba

也可以看看GyD.F4y2Ba

ecompassGyD.F4y2Ba|GyD.F4y2BaahrsfilterGyD.F4y2Ba|GyD.F4y2BaimuSensorGyD.F4y2Ba|GyD.F4y2BagpsSensorGyD.F4y2Ba|GyD.F4y2Ba四元数GyD.F4y2Ba

话题GyD.F4y2Ba

使用惯性传感器确定方向GyD.F4y2Ba

在R2018B中介绍GyD.F4y2Ba

imufilterGyD.F4y2Ba

描述GyD.F4y2Ba

创建GyD.F4y2Ba

句法GyD.F4y2Ba

描述GyD.F4y2Ba

属性GyD.F4y2Ba

SampleRateGyD.F4y2Ba-GyD.F4y2Ba输入传感器数据采样率(Hz)GyD.F4y2BaOne hundred.GyD.F4y2Ba(默认)|GyD.F4y2Ba积极的有限的标量GyD.F4y2Ba

DecimationFactorGyD.F4y2Ba-GyD.F4y2Ba大量毁灭的因素GyD.F4y2Ba1GyD.F4y2Ba(默认)|GyD.F4y2Ba正整数标量GyD.F4y2Ba

AccelerometerNoiseGyD.F4y2Ba-GyD.F4y2Ba加速度计信号噪声方差(m/s)GyD.F4y2Ba2GyD.F4y2Ba的）GyD.F4y2Ba2GyD.F4y2Ba的）GyD.F4y2Ba0.00019247GyD.F4y2Ba(默认)|GyD.F4y2Ba积极的真正的标量GyD.F4y2Ba

GyroscopeNoiseGyD.F4y2Ba-GyD.F4y2Ba陀螺仪信号噪声的差异（（rad / s）GyD.F4y2Ba2GyD.F4y2Ba的）GyD.F4y2Ba9.1385E-5GyD.F4y2Ba(默认)|GyD.F4y2Ba积极的真正的标量GyD.F4y2Ba

GyroscopeDriftNoiseGyD.F4y2Ba-GyD.F4y2Ba陀螺仪偏置漂移方差(rad/s)GyD.F4y2Ba2GyD.F4y2Ba的）GyD.F4y2Ba3.0462 e-13GyD.F4y2Ba(默认)|GyD.F4y2Ba积极的真正的标量GyD.F4y2Ba

linearaccelerationnoise.GyD.F4y2Ba-GyD.F4y2Ba线性加速度噪声的变化（（m / sGyD.F4y2Ba2GyD.F4y2Ba的）GyD.F4y2Ba2GyD.F4y2Ba的）GyD.F4y2Ba0.0096236GyD.F4y2Ba(默认)|GyD.F4y2Ba积极的真正的标量GyD.F4y2Ba

LinearAcclerationDecayFactorGyD.F4y2Ba-GyD.F4y2Ba线性加速度漂移的衰减因子GyD.F4y2Ba0．5GyD.F4y2Ba(默认)|GyD.F4y2Ba标量在范围内[0,1]GyD.F4y2Ba

initialProcessnoise.GyD.F4y2Ba-GyD.F4y2Ba用于过程噪声的协方差矩阵GyD.F4y2Ba9-by-9矩阵GyD.F4y2Ba

OrientationFormatGyD.F4y2Ba-GyD.F4y2Ba面向输出格式GyD.F4y2Ba'Quaternion'GyD.F4y2Ba(默认)|GyD.F4y2Ba旋转矩阵的GyD.F4y2Ba

使用GyD.F4y2Ba

句法GyD.F4y2Ba

描述GyD.F4y2Ba

输入参数GyD.F4y2Ba

accelReadingsGyD.F4y2Ba-GyD.F4y2Ba传感器体坐标系下的加速度计读数(m/s)GyD.F4y2Ba2GyD.F4y2Ba的）GyD.F4y2BaN.GyD.F4y2Ba-By-3矩阵GyD.F4y2Ba

回电GyD.F4y2Ba-GyD.F4y2Ba传感器体坐标系下的陀螺仪读数(rad/s)GyD.F4y2BaN.GyD.F4y2Ba-By-3矩阵GyD.F4y2Ba

输出参数GyD.F4y2Ba

取向GyD.F4y2Ba-将量从全局坐标系旋转到传感器体坐标系的方向GyD.F4y2BaM.GyD.F4y2Ba-by-1四元数向量(默认GyD.F4y2BaM.GyD.F4y2Ba数组GyD.F4y2Ba

angularVelocityGyD.F4y2Ba-传感器体坐标系角速度(rad/s)GyD.F4y2BaM.GyD.F4y2Ba-By-3阵列（默认）GyD.F4y2Ba

对象功能GyD.F4y2Ba

特定于GyD.F4y2BaimufilterGyD.F4y2Ba

对所有系统对象共同GyD.F4y2Ba

例子GyD.F4y2Ba

从IMU数据估计方向GyD.F4y2Ba

使用陀螺仪和加速度计读数的模型倾斜GyD.F4y2Ba

从角速度测量中移除偏置GyD.F4y2Ba

算法GyD.F4y2Ba

详细的概述GyD.F4y2Ba

模型GyD.F4y2Ba

误差模型GyD.F4y2Ba

卡尔曼方程式GyD.F4y2Ba

正确的GyD.F4y2Ba

计算角速度GyD.F4y2Ba

参考文献GyD.F4y2Ba

扩展功能GyD.F4y2Ba

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

也可以看看GyD.F4y2Ba

话题GyD.F4y2Ba

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

金宝app

自主系统的传感器融合与跟踪GyD.F4y2Ba

`SampleRateGyD.F4y2Ba`-GyD.F4y2Ba输入传感器数据采样率(Hz)GyD.F4y2Ba
`One hundred.GyD.F4y2Ba`(默认)|GyD.F4y2Ba积极的有限的标量GyD.F4y2Ba

`DecimationFactorGyD.F4y2Ba`-GyD.F4y2Ba大量毁灭的因素GyD.F4y2Ba
`1GyD.F4y2Ba`(默认)|GyD.F4y2Ba正整数标量GyD.F4y2Ba

`AccelerometerNoiseGyD.F4y2Ba`-GyD.F4y2Ba加速度计信号噪声方差(m/s)GyD.F4y2Ba^2GyD.F4y2Ba的）GyD.F4y2Ba^2GyD.F4y2Ba的）GyD.F4y2Ba
`0.00019247GyD.F4y2Ba`(默认)|GyD.F4y2Ba积极的真正的标量GyD.F4y2Ba

`GyroscopeNoiseGyD.F4y2Ba`-GyD.F4y2Ba陀螺仪信号噪声的差异（（rad / s）GyD.F4y2Ba^2GyD.F4y2Ba的）GyD.F4y2Ba
`9.1385E-5GyD.F4y2Ba`(默认)|GyD.F4y2Ba积极的真正的标量GyD.F4y2Ba

`GyroscopeDriftNoiseGyD.F4y2Ba`-GyD.F4y2Ba陀螺仪偏置漂移方差(rad/s)GyD.F4y2Ba^2GyD.F4y2Ba的）GyD.F4y2Ba
`3.0462 e-13GyD.F4y2Ba`(默认)|GyD.F4y2Ba积极的真正的标量GyD.F4y2Ba

`linearaccelerationnoise.GyD.F4y2Ba`-GyD.F4y2Ba线性加速度噪声的变化（（m / sGyD.F4y2Ba^2GyD.F4y2Ba的）GyD.F4y2Ba^2GyD.F4y2Ba的）GyD.F4y2Ba
`0.0096236GyD.F4y2Ba`(默认)|GyD.F4y2Ba积极的真正的标量GyD.F4y2Ba

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

`initialProcessnoise.GyD.F4y2Ba`-GyD.F4y2Ba用于过程噪声的协方差矩阵GyD.F4y2Ba
9-by-9矩阵GyD.F4y2Ba

`OrientationFormatGyD.F4y2Ba`-GyD.F4y2Ba面向输出格式GyD.F4y2Ba
`'Quaternion'GyD.F4y2Ba`(默认)|GyD.F4y2Ba`旋转矩阵的GyD.F4y2Ba`

`accelReadingsGyD.F4y2Ba`-GyD.F4y2Ba传感器体坐标系下的加速度计读数(m/s)GyD.F4y2Ba^2GyD.F4y2Ba的）GyD.F4y2Ba
N.GyD.F4y2Ba-By-3矩阵GyD.F4y2Ba

`回电GyD.F4y2Ba`-GyD.F4y2Ba传感器体坐标系下的陀螺仪读数(rad/s)GyD.F4y2Ba
N.GyD.F4y2Ba-By-3矩阵GyD.F4y2Ba

`取向GyD.F4y2Ba`-将量从全局坐标系旋转到传感器体坐标系的方向GyD.F4y2Ba
M.GyD.F4y2Ba-by-1四元数向量(默认GyD.F4y2BaM.GyD.F4y2Ba数组GyD.F4y2Ba

`angularVelocityGyD.F4y2Ba`-传感器体坐标系角速度(rad/s)GyD.F4y2Ba
M.GyD.F4y2Ba-By-3阵列（默认）GyD.F4y2Ba

特定于GyD.F4y2Ba`imufilterGyD.F4y2Ba`

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