根据MARG和GPS数据估计姿态gydF4y2Ba
的gydF4y2BainsfilterMARGgydF4y2Ba
目标将MARG和GPS数据进行传感器融合,以估计在NED(或ENU)参考系中的姿态。MARG(磁性,角速度,重力)数据通常来自磁力仪,陀螺仪和加速度传感器。该滤波器使用22个元素的状态向量来跟踪方向四元数、速度、位置、MARG传感器偏差和地磁向量。的gydF4y2BainsfilterMARGgydF4y2Ba
对象使用扩展卡尔曼滤波器来估计这些量。gydF4y2Ba
创建一个gydF4y2Ba过滤器gydF4y2Ba
= insfilterMARGgydF4y2BainsfilterMARGgydF4y2Ba
对象,具有默认属性值。gydF4y2Ba
允许你指定参考帧,gydF4y2Ba过滤器gydF4y2Ba
= insfilterMARG (gydF4y2Ba“ReferenceFrame”gydF4y2Ba
,gydF4y2Ba射频gydF4y2Ba
)gydF4y2Ba射频gydF4y2Ba
的,gydF4y2Ba过滤器gydF4y2Ba
。指定gydF4y2Ba射频gydF4y2Ba
作为gydF4y2BaNED的gydF4y2Ba
(North-East-Down)或gydF4y2Ba“ENU表示”gydF4y2Ba
(East-North-Up)。默认值为gydF4y2BaNED的gydF4y2Ba
。gydF4y2Ba
还允许您设置所创建的属性gydF4y2Ba过滤器gydF4y2Ba
= insfilterMARG (gydF4y2Ba___gydF4y2Ba,gydF4y2Ba名称,值gydF4y2Ba
)gydF4y2Ba过滤器gydF4y2Ba
使用一个或多个名称-值对。将每个属性名用单引号括起来。gydF4y2Ba
正确的gydF4y2Ba |
使用直接的状态测量纠正状态gydF4y2Ba |
剩余gydF4y2Ba |
直接状态测量的残差和残差协方差gydF4y2Ba |
fusegpsgydF4y2Ba |
使用GPS数据修正状态gydF4y2Ba |
residualgpsgydF4y2Ba |
GPS测量的残差和残差协方差gydF4y2Ba |
fusemaggydF4y2Ba |
使用磁强计数据修正状态gydF4y2Ba |
residualmaggydF4y2Ba |
磁强计测量的残差和残差协方差gydF4y2Ba |
构成gydF4y2Ba |
当前方位和位置估计gydF4y2Ba |
预测gydF4y2Ba |
使用加速计和陀螺仪数据更新状态gydF4y2Ba |
重置gydF4y2Ba |
重置内部状态gydF4y2Ba |
stateinfogydF4y2Ba |
显示状态向量信息gydF4y2Ba |
注意:下面的算法只适用于NED参照系。gydF4y2Ba
insfilterMARGgydF4y2Ba
使用一个22轴扩展卡尔曼滤波结构估计姿态在NED参考帧。状态定义为:gydF4y2Ba
在哪里gydF4y2Ba
问gydF4y2Ba0gydF4y2Ba,gydF4y2Ba问gydF4y2Ba1gydF4y2Ba,gydF4y2Ba问gydF4y2Ba2gydF4y2Ba,gydF4y2Ba问gydF4y2Ba3.gydF4y2Ba——方向四元数部分。方向四元数表示从平台当前方向到本地NED坐标系的帧旋转。gydF4y2Ba
位置gydF4y2BaNgydF4y2Ba,gydF4y2Ba位置gydF4y2BaEgydF4y2Ba,gydF4y2Ba位置gydF4y2BaDgydF4y2Ba——平台在本地NED坐标系中的位置。gydF4y2Ba
νgydF4y2BaNgydF4y2Ba,gydF4y2BaνgydF4y2BaEgydF4y2Ba,gydF4y2BaνgydF4y2BaDgydF4y2Ba——平台在局部NED坐标系下的速度。gydF4y2Ba
ΔgydF4y2BaθbiasgydF4y2BaXgydF4y2Ba,ΔgydF4y2BaθbiasgydF4y2BaYgydF4y2Ba,ΔgydF4y2BaθbiasgydF4y2BaZgydF4y2Ba——陀螺仪综合读数的偏差。gydF4y2Ba
ΔgydF4y2BaνbiasgydF4y2BaXgydF4y2Ba,ΔgydF4y2BaνbiasgydF4y2BaYgydF4y2Ba,ΔgydF4y2BaνbiasgydF4y2BaZgydF4y2Ba——集成加速度计读数的偏差。gydF4y2Ba
geomagneticFieldVectorgydF4y2BaNgydF4y2Ba,gydF4y2BageomagneticFieldVectorgydF4y2BaEgydF4y2Ba,gydF4y2BageomagneticFieldVectorgydF4y2BaDgydF4y2Ba——估算参考位置的地磁场矢量。gydF4y2Ba
magbiasgydF4y2BaXgydF4y2Ba,gydF4y2BamagbiasgydF4y2BaYgydF4y2Ba,gydF4y2BamagbiasgydF4y2BaZgydF4y2Ba——磁强计读数的偏差。gydF4y2Ba
已知预测状态估计值的一般形式,gydF4y2Ba
ugydF4y2BakgydF4y2Ba由加速度计和陀螺仪数据控制,数据通过梯形积分转换为速度和角度。预测状态估计为:gydF4y2Ba
在哪里gydF4y2Ba
ΔgydF4y2BaθgydF4y2BaXgydF4y2Ba,ΔgydF4y2BaθgydF4y2BaYgydF4y2Ba,ΔgydF4y2BaθgydF4y2BaZgydF4y2Ba——综合陀螺仪读数。gydF4y2Ba
ΔgydF4y2BaνgydF4y2BaXgydF4y2Ba,ΔgydF4y2BaνgydF4y2BaYgydF4y2Ba,ΔgydF4y2BaνgydF4y2BaZgydF4y2Ba——集成加速度计读数。gydF4y2Ba
ΔgydF4y2BatgydF4y2Ba——IMU采样时间。gydF4y2Ba
ggydF4y2BaNgydF4y2Ba,gydF4y2BaggydF4y2BaEgydF4y2Ba,gydF4y2BaggydF4y2BaDgydF4y2Ba——恒定重力向量。gydF4y2Ba
insfilterAsyncgydF4y2Ba
|gydF4y2BainsfilterErrorStategydF4y2Ba
|gydF4y2BainsfilterNonholonomicgydF4y2Ba