方向从加速度计和陀螺仪读数gydF4y2Ba
的gydF4y2BaimufiltergydF4y2Ba
系统对象™融合加速度计和陀螺仪传感器数据,以估计设备方向。gydF4y2Ba
估计设备方向:gydF4y2Ba
创建gydF4y2BaimufiltergydF4y2Ba
对象并设置其属性。gydF4y2Ba
使用参数调用对象,就像调用函数一样。gydF4y2Ba
有关系统对象如何工作的详细信息,请参见gydF4y2Ba什么是系统对象?gydF4y2Ba
返回一个间接卡尔曼滤波系统对象,gydF4y2Ba保险丝gydF4y2Ba
= imufiltergydF4y2Ba保险丝gydF4y2Ba
,用于融合加速度计和陀螺仪数据,以估计设备方向。该滤波器使用一个九元状态矢量来跟踪方向估计、陀螺仪偏差估计和线性加速度估计中的误差。gydF4y2Ba
返回一个gydF4y2Ba保险丝gydF4y2Ba
= imufilter (gydF4y2Ba“ReferenceFrame”gydF4y2Ba
,gydF4y2Ba射频gydF4y2Ba
)gydF4y2BaimufiltergydF4y2Ba
融合加速度计和陀螺仪数据以估计相对于参考系的设备方向的系统对象gydF4y2Ba射频gydF4y2Ba
.指定gydF4y2Ba射频gydF4y2Ba
作为gydF4y2BaNED的gydF4y2Ba
(North-East-Down)或gydF4y2Ba“ENU表示”gydF4y2Ba
(East-North-Up)。默认值为gydF4y2BaNED的gydF4y2Ba
.gydF4y2Ba
设置每个属性gydF4y2Ba保险丝gydF4y2Ba
= imufilter (gydF4y2Ba___gydF4y2Ba,gydF4y2Ba名称,值gydF4y2Ba
)gydF4y2Ba的名字gydF4y2Ba
到指定的gydF4y2Ba价值gydF4y2Ba
.未指定的属性有默认值。gydF4y2Ba
FUSE = imufilter('SampleRate',200,'GyroscopeNoise',1e-6)gydF4y2Ba
创建一个System对象,gydF4y2Ba保险丝gydF4y2Ba
,采样率为200hz,陀螺仪噪声设置为1e-6弧度每秒平方。gydF4y2Ba
除非另有说明,属性为gydF4y2BanontunablegydF4y2Ba,这意味着在调用对象后不能更改它们的值。对象在调用时锁定,而gydF4y2Ba释放gydF4y2Ba
功能解锁它们。gydF4y2Ba
如果属性为gydF4y2Ba可调gydF4y2Ba,您可以随时更改其值。gydF4y2Ba
有关更改属性值的详细信息,请参见gydF4y2Ba使用系统对象的MATLAB系统设计gydF4y2Ba.gydF4y2Ba
SampleRategydF4y2Ba
- - - - - -gydF4y2Ba输入传感器数据采样率(Hz)gydF4y2BaOne hundred.gydF4y2Ba
(默认)|gydF4y2Ba正有限标量gydF4y2Ba输入传感器数据的采样率,以Hz为单位,指定为正的有限标量。gydF4y2Ba
可调:gydF4y2Ba没有gydF4y2Ba
数据类型:gydF4y2Ba单gydF4y2Ba
|gydF4y2Ba双gydF4y2Ba
|gydF4y2Bauint8gydF4y2Ba
|gydF4y2Bauint16gydF4y2Ba
|gydF4y2Bauint32gydF4y2Ba
|gydF4y2Bauint64gydF4y2Ba
|gydF4y2Baint8gydF4y2Ba
|gydF4y2Baint16gydF4y2Ba
|gydF4y2Baint32gydF4y2Ba
|gydF4y2Baint64gydF4y2Ba
DecimationFactorgydF4y2Ba
- - - - - -gydF4y2Ba大量毁灭的因素gydF4y2Ba1gydF4y2Ba
(默认)|gydF4y2Ba正整数标量gydF4y2Ba抽取因子,用来降低输入传感器数据的采样率,指定为正整数标量。gydF4y2Ba
输入的行数,gydF4y2BaaccelReadingsgydF4y2Ba
而且gydF4y2BagyroReadingsgydF4y2Ba
,必须是抽取因子的倍数。gydF4y2Ba
可调:gydF4y2Ba没有gydF4y2Ba
数据类型:gydF4y2Ba单gydF4y2Ba
|gydF4y2Ba双gydF4y2Ba
|gydF4y2Bauint8gydF4y2Ba
|gydF4y2Bauint16gydF4y2Ba
|gydF4y2Bauint32gydF4y2Ba
|gydF4y2Bauint64gydF4y2Ba
|gydF4y2Baint8gydF4y2Ba
|gydF4y2Baint16gydF4y2Ba
|gydF4y2Baint32gydF4y2Ba
|gydF4y2Baint64gydF4y2Ba
AccelerometerNoisegydF4y2Ba
- - - - - -gydF4y2Ba加速度计信号噪声方差((m/sgydF4y2Ba2gydF4y2Ba)gydF4y2Ba2gydF4y2Ba)gydF4y2Ba0.00019247gydF4y2Ba
(默认)|gydF4y2Ba正实标量gydF4y2Ba加速度计信号噪声方差(m/s)gydF4y2Ba2gydF4y2Ba)gydF4y2Ba2gydF4y2Ba,指定为正实标量。gydF4y2Ba
可调:gydF4y2Ba是的gydF4y2Ba
数据类型:gydF4y2Ba单gydF4y2Ba
|gydF4y2Ba双gydF4y2Ba
|gydF4y2Bauint8gydF4y2Ba
|gydF4y2Bauint16gydF4y2Ba
|gydF4y2Bauint32gydF4y2Ba
|gydF4y2Bauint64gydF4y2Ba
|gydF4y2Baint8gydF4y2Ba
|gydF4y2Baint16gydF4y2Ba
|gydF4y2Baint32gydF4y2Ba
|gydF4y2Baint64gydF4y2Ba
GyroscopeNoisegydF4y2Ba
- - - - - -gydF4y2Ba陀螺仪信号噪声方差((rad/s)gydF4y2Ba2gydF4y2Ba)gydF4y2Ba9.1385 e-5gydF4y2Ba
(默认)|gydF4y2Ba正实标量gydF4y2Ba陀螺仪信号噪声方差(rad/s)gydF4y2Ba2gydF4y2Ba,指定为正实标量。gydF4y2Ba
可调:gydF4y2Ba是的gydF4y2Ba
数据类型:gydF4y2Ba单gydF4y2Ba
|gydF4y2Ba双gydF4y2Ba
|gydF4y2Bauint8gydF4y2Ba
|gydF4y2Bauint16gydF4y2Ba
|gydF4y2Bauint32gydF4y2Ba
|gydF4y2Bauint64gydF4y2Ba
|gydF4y2Baint8gydF4y2Ba
|gydF4y2Baint16gydF4y2Ba
|gydF4y2Baint32gydF4y2Ba
|gydF4y2Baint64gydF4y2Ba
GyroscopeDriftNoisegydF4y2Ba
- - - - - -gydF4y2Ba陀螺仪偏移漂移方差((rad/s)gydF4y2Ba2gydF4y2Ba)gydF4y2Ba3.0462 e-13gydF4y2Ba
(默认)|gydF4y2Ba正实标量gydF4y2Ba陀螺仪偏移漂移方差(单位:rad/s)gydF4y2Ba2gydF4y2Ba,指定为正实标量。gydF4y2Ba
可调:gydF4y2Ba是的gydF4y2Ba
数据类型:gydF4y2Ba单gydF4y2Ba
|gydF4y2Ba双gydF4y2Ba
|gydF4y2Bauint8gydF4y2Ba
|gydF4y2Bauint16gydF4y2Ba
|gydF4y2Bauint32gydF4y2Ba
|gydF4y2Bauint64gydF4y2Ba
|gydF4y2Baint8gydF4y2Ba
|gydF4y2Baint16gydF4y2Ba
|gydF4y2Baint32gydF4y2Ba
|gydF4y2Baint64gydF4y2Ba
LinearAccelerationNoisegydF4y2Ba
- - - - - -gydF4y2Ba线性加速度噪声方差((m/sgydF4y2Ba2gydF4y2Ba)gydF4y2Ba2gydF4y2Ba)gydF4y2Ba0.0096236gydF4y2Ba
(默认)|gydF4y2Ba正实标量gydF4y2Ba线性加速度噪声方差(m/s)gydF4y2Ba2gydF4y2Ba)gydF4y2Ba2gydF4y2Ba,指定为正实标量。线性加速度建模为低通滤波白噪声过程。gydF4y2Ba
可调:gydF4y2Ba是的gydF4y2Ba
数据类型:gydF4y2Ba单gydF4y2Ba
|gydF4y2Ba双gydF4y2Ba
|gydF4y2Bauint8gydF4y2Ba
|gydF4y2Bauint16gydF4y2Ba
|gydF4y2Bauint32gydF4y2Ba
|gydF4y2Bauint64gydF4y2Ba
|gydF4y2Baint8gydF4y2Ba
|gydF4y2Baint16gydF4y2Ba
|gydF4y2Baint32gydF4y2Ba
|gydF4y2Baint64gydF4y2Ba
LinearAcclerationDecayFactorgydF4y2Ba
- - - - - -gydF4y2Ba线性加速度漂移的衰减因子gydF4y2Ba0.5gydF4y2Ba
(默认)|gydF4y2Ba范围[0,1]中的标量gydF4y2Ba线性加速度漂移的衰减因子,指定为范围[0,1]中的标量。如果线性加速度变化快,设置gydF4y2BaLinearAccelerationDecayFactorgydF4y2Ba
到一个更低的值。如果线性加速度变化缓慢,则设置gydF4y2BaLinearAccelerationDecayFactorgydF4y2Ba
到一个更高的值。线性加速度漂移被建模为低通滤波白噪声过程。gydF4y2Ba
可调:gydF4y2Ba是的gydF4y2Ba
数据类型:gydF4y2Ba单gydF4y2Ba
|gydF4y2Ba双gydF4y2Ba
|gydF4y2Bauint8gydF4y2Ba
|gydF4y2Bauint16gydF4y2Ba
|gydF4y2Bauint32gydF4y2Ba
|gydF4y2Bauint64gydF4y2Ba
|gydF4y2Baint8gydF4y2Ba
|gydF4y2Baint16gydF4y2Ba
|gydF4y2Baint32gydF4y2Ba
|gydF4y2Baint64gydF4y2Ba
InitialProcessNoisegydF4y2Ba
- - - - - -gydF4y2Ba过程噪声的协方差矩阵gydF4y2Ba过程噪声的协方差矩阵,指定为9 × 9矩阵。默认为: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列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.009623610000000gydF4y2Ba
初始过程协方差矩阵解释了过程模型中的误差。gydF4y2Ba
数据类型:gydF4y2Ba单gydF4y2Ba
|gydF4y2Ba双gydF4y2Ba
|gydF4y2Bauint8gydF4y2Ba
|gydF4y2Bauint16gydF4y2Ba
|gydF4y2Bauint32gydF4y2Ba
|gydF4y2Bauint64gydF4y2Ba
|gydF4y2Baint8gydF4y2Ba
|gydF4y2Baint16gydF4y2Ba
|gydF4y2Baint32gydF4y2Ba
|gydF4y2Baint64gydF4y2Ba
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取向gydF4y2Ba
,gydF4y2BaangularVelocitygydF4y2Ba
=保险丝(gydF4y2BaaccelReadingsgydF4y2Ba
,gydF4y2BagyroReadingsgydF4y2Ba
)gydF4y2Ba
accelReadingsgydF4y2Ba
- - - - - -gydF4y2Ba加速度计读数在传感器本体坐标系(m/sgydF4y2Ba2gydF4y2Ba)gydF4y2Ba加速度计读数在传感器本体坐标系中,单位为m/sgydF4y2Ba2gydF4y2Ba,指定为gydF4y2BaNgydF4y2Ba3矩阵。gydF4y2BaNgydF4y2Ba是样本的数量,和三列的gydF4y2BaaccelReadingsgydF4y2Ba
表示[gydF4y2BaxgydF4y2BaygydF4y2BazgydF4y2Ba)测量。方法所指定的采样速率假定加速度计读数对应gydF4y2BaSampleRategydF4y2Ba财产。gydF4y2Ba
数据类型:gydF4y2Ba单gydF4y2Ba
|gydF4y2Ba双gydF4y2Ba
gyroReadingsgydF4y2Ba
- - - - - -gydF4y2Ba陀螺仪在传感器体坐标系中的读数(rad/s)gydF4y2Ba陀螺仪读数在传感器体坐标系中以rad/s为单位,指定为gydF4y2BaNgydF4y2Ba3矩阵。gydF4y2BaNgydF4y2Ba是样本的数量,和三列的gydF4y2BagyroReadingsgydF4y2Ba
表示[gydF4y2BaxgydF4y2BaygydF4y2BazgydF4y2Ba)测量。陀螺仪读数被假定对应的采样率规定gydF4y2BaSampleRategydF4y2Ba财产。gydF4y2Ba
数据类型:gydF4y2Ba单gydF4y2Ba
|gydF4y2Ba双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在传感器体坐标系中除去陀螺仪偏差后的角速度,单位为rad/s,返回为gydF4y2Ba米gydF4y2Ba3数组。输入样本的数量,gydF4y2BaNgydF4y2Ba,以及gydF4y2BaDecimationFactorgydF4y2Ba属性确定gydF4y2Ba米gydF4y2Ba.gydF4y2Ba
数据类型:gydF4y2Ba单gydF4y2Ba
|gydF4y2Ba双gydF4y2Ba
要使用对象函数,请将System对象指定为第一个输入参数。例如,释放system对象的系统资源gydF4y2BaobjgydF4y2Ba
,使用这种语法:gydF4y2Ba
发行版(obj)gydF4y2Ba
imufiltergydF4y2Ba
调优gydF4y2Ba |
调优gydF4y2BaimufiltergydF4y2Ba 参数,以减少估计误差gydF4y2Ba |
一步gydF4y2Ba |
运行gydF4y2Ba系统对象gydF4y2Ba算法gydF4y2Ba |
释放gydF4y2Ba |
释放资源并允许更改gydF4y2Ba系统对象gydF4y2Ba属性值和输入特征gydF4y2Ba |
重置gydF4y2Ba |
重置的内部状态gydF4y2Ba系统对象gydF4y2Ba |
加载gydF4y2Barpy_9axisgydF4y2Ba
文件,其中包含记录的加速度计、陀螺仪和磁力计传感器数据,这些数据来自一个在pitch(约gydF4y2BaygydF4y2Ba-轴),然后偏航(绕gydF4y2BazgydF4y2Ba-轴),然后滚(绕gydF4y2BaxgydF4y2Ba设在)。该文件还包含录音的采样率。gydF4y2Ba
负载gydF4y2Ba“rpy_9axis.mat”gydF4y2BasensorDatagydF4y2BaFsgydF4y2Baaccelerometerreads = sensorData.Acceleration;陀螺仪读数= sensorData.AngularVelocity;gydF4y2Ba
创建一个gydF4y2BaimufiltergydF4y2Ba
将采样率设置为传感器数据的采样率的系统对象™。指定抽取因子为2以减少算法的计算成本。gydF4y2Ba
Decim = 2;保险丝= imufiltergydF4y2Ba“SampleRate”gydF4y2BaFs,gydF4y2Ba“DecimationFactor”gydF4y2Ba, decim);gydF4y2Ba
将加速度计读数和陀螺仪读数传递给gydF4y2BaimufiltergydF4y2Ba
对象,gydF4y2Ba保险丝gydF4y2Ba
,输出传感器体方向随时间的估计。默认情况下,方向输出为四元数向量。gydF4y2Ba
q =熔断器(加速度计读数,陀螺仪读数);gydF4y2Ba
方向由将父坐标系旋转到子坐标系所需的角位移定义。用欧拉角表示方向随时间的度数。gydF4y2Ba
imufiltergydF4y2Ba
融合正确地估计了从假定的朝北的初始方向的方向变化。然而,该设备的gydF4y2BaxgydF4y2Ba-axis在记录时指向南方。要正确估计相对于真实初始方向或相对于NED的方向,请使用gydF4y2BaahrsfiltergydF4y2Ba
.gydF4y2Ba
time = (0:decim:size(加速度计读数,1)-1)/Fs;情节(时间,eulerd (q,gydF4y2Ba“ZYX股票”gydF4y2Ba,gydF4y2Ba“帧”gydF4y2Ba)标题(gydF4y2Ba取向估计的gydF4y2Ba)传说(gydF4y2Baz轴的gydF4y2Ba,gydF4y2Ba“轴”gydF4y2Ba,gydF4y2Ba“轴”gydF4y2Ba)包含(gydF4y2Ba“时间(s)”gydF4y2Ba) ylabel (gydF4y2Ba的旋转(度)gydF4y2Ba)gydF4y2Ba
模型一个倾斜IMU,包含加速度计和陀螺仪使用gydF4y2BaimuSensorgydF4y2Ba
系统对象™。用理想模型和现实模型比较了应用该方法的方向跟踪结果gydF4y2BaimufiltergydF4y2Ba
系统对象。gydF4y2Ba
加载一个描述真实运动和采样率的结构。运动结构体描述了连续的旋转:gydF4y2Ba
偏航:两秒内120度gydF4y2Ba
俯仰角:一秒60度gydF4y2Ba
翻滚:半秒30度gydF4y2Ba
翻滚:半秒内-30度gydF4y2Ba
俯仰:-60度/秒gydF4y2Ba
偏航:两秒内-120度gydF4y2Ba
在最后一个阶段,运动结构将第1、2和3个旋转组合成一个单轴旋转。加速度、角速度和方向在局部NED坐标系中定义。gydF4y2Ba
负载gydF4y2Bay120p60r30.matgydF4y2Ba运动gydF4y2BafsgydF4y2BaaccNED =运动。加速度;angVelNED =运动。角速度;orientationNED = motion.Orientation;numSamples = size(motion.Orientation,1);t = (0:(numSamples-1)).'/fs;gydF4y2Ba
创建一个理想的IMU传感器对象和默认的IMU过滤器对象。gydF4y2Ba
IMU =传感器(gydF4y2Ba“accel-gyro”gydF4y2Ba,gydF4y2Ba“SampleRate”gydF4y2Bafs);过滤器= imufilter(gydF4y2Ba“SampleRate”gydF4y2Bafs);gydF4y2Ba
在循环中:gydF4y2Ba
通过向IMU传感器对象输入地面真实运动来模拟IMU输出。gydF4y2Ba
使用默认的IMU过滤器对象过滤IMU输出。gydF4y2Ba
方向= 0 (numSamples,1,gydF4y2Ba“四元数”gydF4y2Ba);gydF4y2Ba为gydF4y2Bai = 1:numSamples [accelBody,gyroBody] = IMU(accNED(i,:),angVelNED(i,:),orientationNED(i,:));定向(i) = aFilter(accelBody,gyroBody);gydF4y2Ba结束gydF4y2Ba发行版(aFilter)gydF4y2Ba
画出随时间变化的方向。gydF4y2Ba
图(1)情节(t, eulerd(取向,gydF4y2Ba“ZYX股票”gydF4y2Ba,gydF4y2Ba“帧”gydF4y2Ba)包含(gydF4y2Ba“时间(s)”gydF4y2Ba) ylabel (gydF4y2Ba的旋转(度)gydF4y2Ba)标题(gydF4y2Ba“方向估计—理想IMU数据,默认IMU过滤器”gydF4y2Ba)传说(gydF4y2Baz轴的gydF4y2Ba,gydF4y2Ba“轴”gydF4y2Ba,gydF4y2Ba“轴”gydF4y2Ba)gydF4y2Ba
修改您的gydF4y2BaimuSensorgydF4y2Ba
模拟真实世界的传感器。再次运行循环并绘制随时间变化的方向估计。gydF4y2Ba
IMU。一个ccelerometer = accelparams(...gydF4y2Ba“MeasurementRange”gydF4y2Ba, 19.62,gydF4y2Ba...gydF4y2Ba“决议”gydF4y2Ba, 0.00059875,gydF4y2Ba...gydF4y2Ba“ConstantBias”gydF4y2Ba, 0.4905,gydF4y2Ba...gydF4y2Ba“AxesMisalignment”gydF4y2Ba,2,gydF4y2Ba...gydF4y2Ba“NoiseDensity”gydF4y2Ba, 0.003924,gydF4y2Ba...gydF4y2Ba“BiasInstability”gydF4y2Ba0,gydF4y2Ba...gydF4y2Ba“TemperatureBias”gydF4y2Ba, [0.34335 0.34335 0.5886],gydF4y2Ba...gydF4y2Ba“TemperatureScaleFactor”gydF4y2Ba, 0.02);IMU。陀螺仪=陀螺参数(gydF4y2Ba...gydF4y2Ba“MeasurementRange”gydF4y2Ba, 4.3633,gydF4y2Ba...gydF4y2Ba“决议”gydF4y2Ba, 0.00013323,gydF4y2Ba...gydF4y2Ba“AxesMisalignment”gydF4y2Ba,2,gydF4y2Ba...gydF4y2Ba“NoiseDensity”gydF4y2Ba8.7266 e-05gydF4y2Ba...gydF4y2Ba“TemperatureBias”gydF4y2Ba, 0.34907,gydF4y2Ba...gydF4y2Ba“TemperatureScaleFactor”gydF4y2Ba, 0.02,gydF4y2Ba...gydF4y2Ba“AccelerationBias”gydF4y2Ba, 0.00017809,gydF4y2Ba...gydF4y2Ba“ConstantBias”gydF4y2Ba, 0.3491, 0.5, 0);orientationDefault = 0 (numSamples,1,gydF4y2Ba“四元数”gydF4y2Ba);gydF4y2Ba为gydF4y2Bai = 1:numSamples [accelBody,gyroBody] = IMU(accNED(i,:),angVelNED(i,:),orientationNED(i,:));orientationDefault(i) = aFilter(accelBody,gyroBody);gydF4y2Ba结束gydF4y2Ba(2) plot(t,eulerd(orientationDefault,gydF4y2Ba“ZYX股票”gydF4y2Ba,gydF4y2Ba“帧”gydF4y2Ba)包含(gydF4y2Ba“时间(s)”gydF4y2Ba) ylabel (gydF4y2Ba的旋转(度)gydF4y2Ba)标题(gydF4y2Ba“方向估计—真实的IMU数据,默认IMU过滤器”gydF4y2Ba)传说(gydF4y2Baz轴的gydF4y2Ba,gydF4y2Ba“轴”gydF4y2Ba,gydF4y2Ba“轴”gydF4y2Ba)gydF4y2Ba
的能力gydF4y2BaimufiltergydF4y2Ba
在建模真实的IMU时,跟踪地面真实数据的能力显著降低。要提高性能,请修改gydF4y2BaimufiltergydF4y2Ba
对象。这些数值是凭经验确定的。再次运行循环并绘制随时间变化的方向估计。gydF4y2Ba
aFilter。陀螺仪= 7.6154e-7;aFilter。一个ccelerometerNoise = 0.0015398; aFilter.GyroscopeDriftNoise = 3.0462e-12; aFilter.LinearAccelerationNoise = 0.00096236; aFilter.InitialProcessNoise = aFilter.InitialProcessNoise*10; orientationNondefault = zeros(numSamples,1,“四元数”gydF4y2Ba);gydF4y2Ba为gydF4y2Bai = 1:numSamples [accelBody,gyroBody] = IMU(accNED(i,:),angVelNED(i,:),orientationNED(i,:));orientationNondefault(i) = aFilter(accelBody,gyroBody);gydF4y2Ba结束gydF4y2Ba(3) plot(t,eulerd(orientationNondefault,gydF4y2Ba“ZYX股票”gydF4y2Ba,gydF4y2Ba“帧”gydF4y2Ba)包含(gydF4y2Ba“时间(s)”gydF4y2Ba) ylabel (gydF4y2Ba的旋转(度)gydF4y2Ba)标题(gydF4y2Ba方向估计——真实的IMU数据,非默认IMU过滤器gydF4y2Ba)传说(gydF4y2Baz轴的gydF4y2Ba,gydF4y2Ba“轴”gydF4y2Ba,gydF4y2Ba“轴”gydF4y2Ba)gydF4y2Ba
对改进后的性能进行量化改进gydF4y2BaimufiltergydF4y2Ba
方法返回的方向与地面真实运动之间的四元数距离gydF4y2BaimufiltergydF4y2Ba
默认和非默认属性。gydF4y2Ba
qDistDefault = rad2deg(dist(orientationNED,orientationDefault));qDistNondefault = rad2deg(dist(orientationNED,orientationNondefault));图(4)plot(t,[qDistDefault,qDistNondefault])gydF4y2Ba“与真方向的四元数距离”gydF4y2Ba)传说(gydF4y2Ba“真实的IMU数据,默认IMU过滤器”gydF4y2Ba,gydF4y2Ba...gydF4y2Ba真实的IMU数据,非默认IMU过滤器gydF4y2Ba)包含(gydF4y2Ba“时间(s)”gydF4y2Ba) ylabel (gydF4y2Ba四元数距离(度)gydF4y2Ba)gydF4y2Ba
这个例子展示了如何删除陀螺仪偏差从IMU使用gydF4y2BaimufiltergydF4y2Ba
.gydF4y2Ba
使用gydF4y2BakinematicTrajectorygydF4y2Ba
创造一个由两部分组成的轨迹。第一部分的角速度是恒定的gydF4y2BaygydF4y2Ba- - -gydF4y2BazgydF4y2Ba相互重合。第二部分在三个轴上都有不同的角速度。gydF4y2Ba
持续时间= 60*8;Fs = 20;numSamples = duration * fs;rng (gydF4y2Ba“默认”gydF4y2Ba)gydF4y2Ba种子RNG以重现噪声传感器测量。gydF4y2BainitialAngVel = [0,0.5,0.25];finalAngVel = [-0.2,0.6,0.5];constantAngVel = repmat(initialAngVel,floor(numSamples/2),1);varyingAngVel = [linspace(initialAngVel(1), finalAngVel(1), ceil(numSamples/2)).',gydF4y2Ba...gydF4y2Balinspace(initialAngVel(2), finalAngVel(2), ceil(numSamples/2)).',gydF4y2Ba...gydF4y2Balinspace(initialAngVel(3), finalAngVel(3), ceil(numSamples/2)).'];angVelBody = [constantAngVel;varyingAngVel];accBody = 0 (numSamples,3);轨迹(gydF4y2Ba“SampleRate”gydF4y2Bafs);[~,qNED,~,accNED,angVelNED] = traj(accBody,angVelBody);gydF4y2Ba
创建一个gydF4y2BaimuSensorgydF4y2Ba
系统对象™,gydF4y2BaIMUgydF4y2Ba
,带有非理想陀螺仪。调用gydF4y2BaIMUgydF4y2Ba
有真实的加速度,角速度和方向。gydF4y2Ba
IMU =传感器(gydF4y2Ba“accel-gyro”gydF4y2Ba,gydF4y2Ba...gydF4y2Ba“陀螺”gydF4y2Bagyroparams (gydF4y2Ba“随机散步”gydF4y2Ba, 0.003,gydF4y2Ba“ConstantBias”gydF4y2Ba0.3),gydF4y2Ba...gydF4y2Ba“SampleRate”gydF4y2Bafs);[accelreads, gyroReadingsBody] = IMU(accNED,angVelNED,qNED);gydF4y2Ba
创建一个gydF4y2BaimufiltergydF4y2Ba
系统对象,gydF4y2Ba保险丝gydF4y2Ba
.调用gydF4y2Ba保险丝gydF4y2Ba
与模拟加速度计读数和陀螺仪读数。gydF4y2Ba
保险丝= imufiltergydF4y2Ba“SampleRate”gydF4y2Bafs,gydF4y2Ba“GyroscopeDriftNoise”gydF4y2Ba1 e-6);[~,angVelBodyRecovered] = fuse(accelreads,gyroReadingsBody);gydF4y2Ba
绘制真实的角速度,陀螺仪读数,以及每个轴的恢复角速度。gydF4y2Ba
角速度从gydF4y2BaimufiltergydF4y2Ba
补偿陀螺仪偏差随时间的影响,收敛到真实角速度。gydF4y2Ba
time = (0:numSamples-1)'/fs;图(1)图(时间,angVelBody (: 1),gydF4y2Ba...gydF4y2Ba时间,gyroReadingsBody (: 1),gydF4y2Ba...gydF4y2Ba时间,angVelBodyRecovered(: 1)标题(gydF4y2Ba“轴”gydF4y2Ba)传说(gydF4y2Ba“真实角速度”gydF4y2Ba,gydF4y2Ba...gydF4y2Ba“陀螺仪数据”gydF4y2Ba,gydF4y2Ba...gydF4y2Ba“恢复角速度”gydF4y2Ba) ylabel (gydF4y2Ba角速度(rad/s)gydF4y2Ba)gydF4y2Ba
图(2)图(时间,angVelBody (:, 2),gydF4y2Ba...gydF4y2Ba时间,gyroReadingsBody (:, 2),gydF4y2Ba...gydF4y2Ba时间,angVelBodyRecovered(: 2)标题(gydF4y2Ba“轴”gydF4y2Ba) ylabel (gydF4y2Ba角速度(rad/s)gydF4y2Ba)gydF4y2Ba
图(3)图(时间,angVelBody (:, 3),gydF4y2Ba...gydF4y2Ba时间,gyroReadingsBody (:, 3),gydF4y2Ba...gydF4y2Ba时间,angVelBodyRecovered(: 3)标题(gydF4y2Baz轴的gydF4y2Ba) ylabel (gydF4y2Ba角速度(rad/s)gydF4y2Ba)包含(gydF4y2Ba“时间(s)”gydF4y2Ba)gydF4y2Ba
注意:以下算法只适用于NED参考系。gydF4y2Ba
的gydF4y2BaimufiltergydF4y2Ba
中描述的六轴卡尔曼滤波器结构gydF4y2Ba[1]gydF4y2Ba.该算法尝试跟踪方向误差、陀螺仪偏移量误差和线性加速度误差,以输出最终的方向和角速度。间接卡尔曼滤波器不是直接跟踪方向,而是对误差过程进行建模,gydF4y2BaxgydF4y2Ba,递归更新:gydF4y2Ba
在哪里gydF4y2BaxgydF4y2BakgydF4y2Ba是一个9乘1的向量,由:gydF4y2Ba
θgydF4y2BakgydF4y2Ba——3 × 1方向误差向量,以度为单位,在时间上gydF4y2BakgydF4y2Ba
bgydF4y2BakgydF4y2Ba——3 × 1陀螺仪零角速率偏置矢量,单位deg/s,时间gydF4y2BakgydF4y2Ba
一个gydF4y2BakgydF4y2Ba——在传感器帧中测量的3乘1加速度误差矢量,单位为g,在时间上gydF4y2BakgydF4y2Ba
wgydF4y2BakgydF4y2Ba——9乘1加性噪声矢量gydF4y2Ba
FgydF4y2BakgydF4y2Ba——状态转移模型gydF4y2Ba
因为gydF4y2BaxgydF4y2BakgydF4y2Ba定义为错误过程,则gydF4y2Ba先天的gydF4y2Ba估计总是零,因此状态转移模型,gydF4y2BaFgydF4y2BakgydF4y2Ba,为零。这一见解导致了以下标准卡尔曼方程的缩减:gydF4y2Ba
标准卡尔曼方程:gydF4y2Ba
本算法使用的卡尔曼方程:gydF4y2Ba
在哪里gydF4y2Ba
xgydF4y2BakgydF4y2Ba−gydF4y2Ba——预测(gydF4y2Ba先天的gydF4y2Ba)状态估计;错误过程gydF4y2Ba
PgydF4y2BakgydF4y2Ba−gydF4y2Ba——预测(gydF4y2Ba先天的gydF4y2Ba估计协方差gydF4y2Ba
ygydF4y2BakgydF4y2Ba——创新gydF4y2Ba
年代gydF4y2BakgydF4y2Ba——创新协方差gydF4y2Ba
KgydF4y2BakgydF4y2Ba——卡尔曼增益gydF4y2Ba
xgydF4y2BakgydF4y2Ba+gydF4y2Ba——已更新(gydF4y2Ba后验gydF4y2Ba)状态估计gydF4y2Ba
PgydF4y2BakgydF4y2Ba+gydF4y2Ba——已更新(gydF4y2Ba后验gydF4y2Ba估计协方差gydF4y2Ba
kgydF4y2Ba表示迭代,上标gydF4y2Ba+gydF4y2Ba代表一个gydF4y2Ba后验gydF4y2Ba估计,和上标gydF4y2Ba−gydF4y2Ba代表一个gydF4y2Ba先天的gydF4y2Ba估计。gydF4y2Ba
图形和以下步骤描述了通过该算法进行的单帧迭代。gydF4y2Ba
在第一次迭代之前,gydF4y2BaaccelReadingsgydF4y2Ba
而且gydF4y2BagyroReadingsgydF4y2Ba
输入被分成1乘3帧gydF4y2BaDecimationFactorgydF4y2Ba
-by-3帧。该算法使用最新的加速度计读数对应陀螺仪读数块。gydF4y2Ba
通过对算法的逐步解释,对每个阶段进行了详细的概述。gydF4y2Ba
该算法将加速度和角度变化建模为线性过程。gydF4y2Ba
当前帧的方向是通过首先估计前一帧的角度变化来预测的:gydF4y2Ba
在哪里gydF4y2BaNgydF4y2Ba是否指定了抽取因子gydF4y2BaDecimationFactorgydF4y2Ba
财产,gydF4y2BafsgydF4y2Ba抽样率是否由gydF4y2BaSampleRategydF4y2Ba
财产。gydF4y2Ba
方法将角度变化转换为四元数gydF4y2BarotvecgydF4y2Ba
四元数gydF4y2Ba
施工的语法:gydF4y2Ba
之前的方向估计通过旋转Δ来更新gydF4y2Ba问gydF4y2Ba:gydF4y2Ba
在第一次迭代中,方向估计,gydF4y2Ba问gydF4y2Ba−gydF4y2Ba初始化为gydF4y2BaecompassgydF4y2Ba
假设gydF4y2BaxgydF4y2Ba-轴指向北方。gydF4y2Ba
重力向量被解释为四元数的第三列,gydF4y2Ba问gydF4y2Ba−gydF4y2Ba,旋转矩阵形式为:gydF4y2Ba
看到gydF4y2BaecompassgydF4y2Ba
对于第三列的解释gydF4y2BarPriorgydF4y2Ba可以解释为重力向量。gydF4y2Ba
第二次重力矢量估计是通过从加速度计读数中减去前一次迭代的衰减线性加速度估计得出的:gydF4y2Ba
误差模型是加速度计读数的重力估计和陀螺仪读数的重力估计之间的差异:gydF4y2Ba .gydF4y2Ba
卡尔曼方程使用的重力估计值来自陀螺仪读数,gydF4y2BaggydF4y2Ba,以及误差过程的观察,gydF4y2BazgydF4y2Ba,更新卡尔曼增益和中间协方差矩阵。将卡尔曼增益应用于误差信号,gydF4y2BazgydF4y2Ba,以输出gydF4y2Ba后验gydF4y2Ba误差估计,gydF4y2BaxgydF4y2Ba+gydF4y2Ba.gydF4y2Ba
观测模型映射了1 × 3的观测状态,gydF4y2BaggydF4y2Ba,变成3 × 9的真态,gydF4y2BaHgydF4y2Ba.gydF4y2Ba
观测模型构造为:gydF4y2Ba
在哪里gydF4y2BaggydF4y2BaxgydF4y2Ba,gydF4y2BaggydF4y2BaygydF4y2Ba,gydF4y2BaggydF4y2BazgydF4y2Ba是gydF4y2BaxgydF4y2Ba- - - - - -,gydF4y2BaygydF4y2Ba- - - - - -,gydF4y2BazgydF4y2Ba-由方向估计的重力矢量的元素,分别为。gydF4y2BaκgydF4y2Ba常数是由gydF4y2BaSampleRategydF4y2Ba而且gydF4y2BaDecimationFactorgydF4y2Ba属性:gydF4y2BaκgydF4y2Ba=gydF4y2BaDecimationFactorgydF4y2Ba
/gydF4y2BaSampleRategydF4y2Ba
.gydF4y2Ba
第7.3和7.4节gydF4y2Ba[1]gydF4y2Ba对于观测模型的推导。gydF4y2Ba
创新协方差是一个3乘3的矩阵,用于跟踪测量中的可变性。创新协方差矩阵计算为:gydF4y2Ba
在哪里gydF4y2Ba
HgydF4y2Ba观察模型是矩阵吗gydF4y2Ba
PgydF4y2Ba−gydF4y2Ba是否预测(gydF4y2Ba先天的gydF4y2Ba)对前一次迭代计算的观测模型协方差的估计gydF4y2Ba
RgydF4y2Ba为观测模型噪声的协方差,计算公式为:gydF4y2Ba
以下属性定义了观测模型噪声方差:gydF4y2Ba
κgydF4y2Ba——(gydF4y2BaDecimationFactorgydF4y2Ba/gydF4y2BaSampleRategydF4y2Ba)gydF4y2Ba2gydF4y2Ba
βgydF4y2Ba- - -gydF4y2BaGyroscopeDriftNoisegydF4y2Ba
ηgydF4y2Ba- - -gydF4y2BaGyroscopeNoisegydF4y2Ba
λgydF4y2Ba- - -gydF4y2BaAccelerometerNoisegydF4y2Ba
ξgydF4y2Ba- - -gydF4y2BaLinearAccelerationNoisegydF4y2Ba
误差估计协方差是一个9乘9的矩阵,用于跟踪状态的可变性。gydF4y2Ba
误差估计协方差矩阵更新为:gydF4y2Ba
在哪里gydF4y2BaKgydF4y2Ba为卡尔曼增益,gydF4y2BaHgydF4y2Ba是测量矩阵,和gydF4y2BaPgydF4y2Ba−gydF4y2Ba是在前一次迭代中计算的误差估计协方差。gydF4y2Ba
误差估计协方差是一个9乘9的矩阵,用于跟踪状态的可变性。的gydF4y2Ba先天的gydF4y2Ba误差估计协方差,gydF4y2BaPgydF4y2Ba−gydF4y2Ba,设为过程噪声协方差,gydF4y2Ba问gydF4y2Ba,在前一次迭代中确定。gydF4y2Ba问gydF4y2Ba是作为函数计算的gydF4y2Ba后验gydF4y2Ba误差估计协方差,gydF4y2BaPgydF4y2Ba+gydF4y2Ba.在计算gydF4y2Ba问gydF4y2Ba时,假设交叉相关项与自相关项相比可以忽略不计,并设为零:gydF4y2Ba
在哪里gydF4y2Ba
PgydF4y2Ba+gydF4y2Ba——是更新后的(gydF4y2Ba后验gydF4y2Ba)误差估计协方差gydF4y2Ba
κgydF4y2Ba- - -gydF4y2BaDecimationFactorgydF4y2Ba/gydF4y2BaSampleRategydF4y2Ba
βgydF4y2Ba- - -gydF4y2BaGyroscopeDriftNoisegydF4y2Ba
ηgydF4y2Ba- - -gydF4y2BaGyroscopeNoisegydF4y2Ba
νgydF4y2Ba- - -gydF4y2BaLinearAcclerationDecayFactorgydF4y2Ba
ξgydF4y2Ba- - -gydF4y2BaLinearAccelerationNoisegydF4y2Ba
见第10.1节gydF4y2Ba[1]gydF4y2Ba对于过程误差矩阵项的推导。gydF4y2Ba
卡尔曼增益矩阵是一个9乘3的矩阵,用于衡量创新。在这个算法中,创新被解释为误差过程,gydF4y2BazgydF4y2Ba.gydF4y2Ba
卡尔曼增益矩阵构造为:gydF4y2Ba
在哪里gydF4y2Ba
PgydF4y2Ba-gydF4y2Ba——预测误差协方差gydF4y2Ba
HgydF4y2Ba——观测模型gydF4y2Ba
年代gydF4y2Ba——创新协方差gydF4y2Ba
的gydF4y2Ba一个后gydF4y2Ba误差估计由卡尔曼增益矩阵与重力矢量估计误差相结合确定:gydF4y2Ba
方向估计通过将之前的估计乘以误差来更新:gydF4y2Ba
线性加速度估计通过衰减前一次迭代的线性加速度估计并减去误差来更新:gydF4y2Ba
在哪里gydF4y2Ba
νgydF4y2Ba- - -gydF4y2BaLinearAcclerationDecayFactorgydF4y2Ba
陀螺仪偏移估计更新为用前一次迭代的陀螺仪偏移减去陀螺仪偏移误差:gydF4y2Ba
的坐标系来估计角速度gydF4y2BagyroReadingsgydF4y2Ba
取平均值,并减去前一次迭代计算的陀螺仪偏移量:gydF4y2Ba
在哪里gydF4y2BaNgydF4y2Ba是否指定了抽取因子gydF4y2BaDecimationFactorgydF4y2Ba
财产。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
看到gydF4y2Ba系统对象在MATLAB代码生成gydF4y2Ba(MATLAB编码器)gydF4y2Ba.gydF4y2Ba
ecompassgydF4y2Ba
|gydF4y2BaahrsfiltergydF4y2Ba
|gydF4y2BaimuSensorgydF4y2Ba
|gydF4y2BagpsSensorgydF4y2Ba
你点击了一个对应于这个MATLAB命令的链接:gydF4y2Ba
在MATLAB命令窗口中输入该命令来运行该命令。Web浏览器不支持MATLAB命令。万博1manbetxgydF4y2Ba
选择一个网站,在可用的地方获得翻译的内容,并查看当地的活动和优惠。根据您所在的位置,我们建议您选择:gydF4y2Ba.gydF4y2Ba
选择gydF4y2Ba网站gydF4y2Ba您也可以从以下列表中选择一个网站:gydF4y2Ba
选择中国站点(中文或英文)以获得最佳站点性能。其他MathWorks国家站点没有针对您所在位置的访问进行优化。gydF4y2Ba