主要内容gydF4y2Ba

insfilterMARGgydF4y2Ba

从MARG和GPS数据估计姿态gydF4y2Ba

展开全部页面gydF4y2Ba

描述gydF4y2Ba

的gydF4y2BainsfilterMARGgydF4y2Ba对象实现了MARG和GPS数据的传感器融合，以估计NED(或ENU)参考系中的姿态。MARG(磁、角速率、重力)数据通常来自磁力计、陀螺仪和加速度计传感器。该滤波器使用22元状态向量来跟踪方向四元数、速度、位置、MARG传感器偏差和地磁矢量。的gydF4y2BainsfilterMARGgydF4y2Ba对象使用扩展卡尔曼滤波器来估计这些量。gydF4y2Ba

创建gydF4y2Ba

语法gydF4y2Ba

filter = insfilterMARGgydF4y2Ba
filter = insfilterMARG('ReferenceFrame'，RF)gydF4y2Ba
过滤器= insfilterMARG(gydF4y2Ba___gydF4y2Ba、名称、值)gydF4y2Ba

描述gydF4y2Ba

例子gydF4y2Ba

过滤器gydF4y2Ba= insfilterMARGgydF4y2Ba创建一个gydF4y2BainsfilterMARGgydF4y2Ba具有默认属性值的对象。gydF4y2Ba

过滤器gydF4y2Ba= insfilterMARG (gydF4y2Ba“ReferenceFrame”gydF4y2Ba，gydF4y2Ba射频gydF4y2Ba）gydF4y2Ba允许你指定参考系，gydF4y2Ba射频gydF4y2Ba的gydF4y2Ba过滤器gydF4y2Ba．指定gydF4y2Ba射频gydF4y2Ba作为gydF4y2BaNED的gydF4y2Ba(North-East-Down)或gydF4y2Ba“ENU表示”gydF4y2Ba(East-North-Up)。默认值为gydF4y2BaNED的gydF4y2Ba．gydF4y2Ba

过滤器gydF4y2Ba= insfilterMARG (gydF4y2Ba___gydF4y2Ba，gydF4y2Ba名称,值gydF4y2Ba）gydF4y2Ba还允许您设置已创建的属性gydF4y2Ba过滤器gydF4y2Ba使用一个或多个名称-值对。将每个属性名用单引号括起来。gydF4y2Ba

属性gydF4y2Ba

全部展开gydF4y2Ba

惯性测量单元(IMU)的采样率，单位为Hz，指定为正标量。gydF4y2Ba

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

参考位置，在大地坐标(纬度、经度和高度)中指定为3元素行向量。海拔高度是参考椭球模型WGS84之上的高度。参考位置单位是[度，度，米]。gydF4y2Ba

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

来自陀螺仪的乘法过程噪声方差(rad/s)gydF4y2Ba2gydF4y2Ba，指定为正实数的标量或3元行向量。gydF4y2Ba

  • 如果gydF4y2BaGyroscopeNoisegydF4y2Ba被指定为行向量时，元素对应于噪声中的gydF4y2BaxgydF4y2Ba，gydF4y2BaygydF4y2Ba,gydF4y2BazgydF4y2Ba陀螺仪的轴分别为。gydF4y2Ba

  • 如果gydF4y2BaGyroscopeNoisegydF4y2Ba指定为标量时，单个元素将应用于gydF4y2BaxgydF4y2Ba，gydF4y2BaygydF4y2Ba,gydF4y2BazgydF4y2Ba陀螺仪的轴。gydF4y2Ba

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

陀螺仪偏差的乘法过程噪声方差(rad/s)gydF4y2Ba2gydF4y2Ba，指定为正实数的标量或3元素行向量。gydF4y2Ba

  • 如果gydF4y2BaGyroscopeBiasNoisegydF4y2Ba被指定为行向量时，元素对应于噪声中的gydF4y2BaxgydF4y2Ba，gydF4y2BaygydF4y2Ba,gydF4y2BazgydF4y2Ba陀螺仪各轴分别偏置。gydF4y2Ba

  • 如果gydF4y2BaGyroscopeBiasNoisegydF4y2Ba指定为标量时，单个元素将应用于每个轴。gydF4y2Ba

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

乘法过程噪声方差从加速度计(m/sgydF4y2Ba2gydF4y2Ba）gydF4y2Ba2gydF4y2Ba，指定为正实数的标量或3元行向量。gydF4y2Ba

  • 如果gydF4y2BaAccelerometerNoisegydF4y2Ba被指定为行向量时，元素对应于噪声中的gydF4y2BaxgydF4y2Ba，gydF4y2BaygydF4y2Ba,gydF4y2BazgydF4y2Ba加速度计的轴，分别。gydF4y2Ba

  • 如果gydF4y2BaAccelerometerNoisegydF4y2Ba指定为标量时，单个元素将应用于每个轴。gydF4y2Ba

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

加速度计偏差的乘法过程噪声方差(m/s)gydF4y2Ba2gydF4y2Ba）gydF4y2Ba2gydF4y2Ba，指定为正实数的标量或3元素行向量。gydF4y2Ba

  • 如果gydF4y2BaAccelerometerBiasNoisegydF4y2Ba被指定为行向量时，元素对应于噪声中的gydF4y2BaxgydF4y2Ba，gydF4y2BaygydF4y2Ba,gydF4y2BazgydF4y2Ba加速度计的轴分别偏置。gydF4y2Ba

  • 如果gydF4y2BaAccelerometerBiasNoisegydF4y2Ba指定为标量时，单个元素将应用于每个轴。gydF4y2Ba

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

μ T地磁矢量的加性过程噪声gydF4y2Ba2gydF4y2Ba，指定为正实数的标量或3元素行向量。gydF4y2Ba

  • 如果gydF4y2BaGeomagneticVectorNoisegydF4y2Ba被指定为行向量时，元素对应于噪声中的gydF4y2BaxgydF4y2Ba，gydF4y2BaygydF4y2Ba,gydF4y2BazgydF4y2Ba地磁矢量的轴，分别为。gydF4y2Ba

  • 如果gydF4y2BaGeomagneticVectorNoisegydF4y2Ba指定为标量时，单个元素将应用于每个轴。gydF4y2Ba

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

μ T磁力计偏置的附加过程噪声gydF4y2Ba2gydF4y2Ba，指定为标量或3元素行向量。gydF4y2Ba

  • 如果gydF4y2BaMagnetometerBiasNoisegydF4y2Ba被指定为行向量时，元素对应于噪声中的gydF4y2BaxgydF4y2Ba，gydF4y2BaygydF4y2Ba,gydF4y2BazgydF4y2Ba磁力计的轴分别偏置。gydF4y2Ba

  • 如果gydF4y2BaMagnetometerBiasNoisegydF4y2Ba指定为标量时，单个元素将应用于每个轴。gydF4y2Ba

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

扩展卡尔曼滤波器的状态向量。状态值表示:gydF4y2Ba

状态gydF4y2Ba 单位gydF4y2Ba 指数gydF4y2Ba
定位(四元数部分)gydF4y2Ba N/AgydF4y2Ba 1:4gydF4y2Ba
职位(NED或ENU)gydF4y2Ba 米gydF4y2Ba 5:7gydF4y2Ba
速度(NED或ENU)gydF4y2Ba 米/秒gydF4y2Ba 8:10gydF4y2Ba
角偏差(XYZ)gydF4y2Ba radgydF4y2Ba 11:13gydF4y2Ba
速度偏差(XYZ)gydF4y2Ba 米/秒gydF4y2Ba 14:16gydF4y2Ba
地磁场矢量(NED或ENU)gydF4y2Ba µTgydF4y2Ba 17:19gydF4y2Ba
磁力计偏置(XYZ)gydF4y2Ba µTgydF4y2Ba 20:22gydF4y2Ba

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

扩展卡尔曼滤波器的状态误差协方差，指定为22 × 22元素矩阵或实数。gydF4y2Ba

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

对象的功能gydF4y2Ba

正确的gydF4y2Ba 使用直接状态测量的正确状态gydF4y2BainsfilterMARGgydF4y2Ba
剩余gydF4y2Ba 的直接状态测量的残差和残馀协方差gydF4y2BainsfilterMARGgydF4y2Ba
fusegpsgydF4y2Ba 正确的状态使用GPS数据gydF4y2BainsfilterMARGgydF4y2Ba
residualgpsgydF4y2Ba GPS测量的残差和残差协方差gydF4y2BainsfilterMARGgydF4y2Ba
fusemaggydF4y2Ba 正确的状态使用磁力计数据gydF4y2BainsfilterMARGgydF4y2Ba
residualmaggydF4y2Ba 的磁强计测量的残差和残差协方差gydF4y2BainsfilterMARGgydF4y2Ba
构成gydF4y2Ba 的当前方向和位置估计gydF4y2BainsfilterMARGgydF4y2Ba
预测gydF4y2Ba 使用加速计和陀螺仪数据更新状态gydF4y2BainsfilterMARGgydF4y2Ba
重置gydF4y2Ba 重置内部状态gydF4y2BainsfilterMARGgydF4y2Ba
stateinfogydF4y2Ba 显示状态向量信息gydF4y2BainsfilterMARGgydF4y2Ba
调优gydF4y2Ba 调优gydF4y2BainsfilterMARGgydF4y2Ba参数，以减少估计误差gydF4y2Ba
复制gydF4y2Ba 创建副本gydF4y2BainsfitlerMARGgydF4y2Ba

例子gydF4y2Ba

全部折叠gydF4y2Ba

打开实时脚本gydF4y2Ba

这个例子展示了如何从记录的传感器数据和地面真实姿态估计无人机(UAV)的姿态。gydF4y2Ba

加载记录的传感器数据和无人机的地面真实姿态。gydF4y2Ba

负载gydF4y2Bauavshort.matgydF4y2Ba

初始化gydF4y2BainsfilterMARGgydF4y2Ba过滤器对象。gydF4y2Ba

f = insfilterMARG;f.IMUSampleRate = imuFs;f.r efencelocation = refloc;f.AccelerometerBiasNoise = 2e-4;f.加速度计噪声= 2;f.GyroscopeBiasNoise = 1e-16;f.GyroscopeNoise = 1e-5;f.MagnetometerBiasNoise = 1e-10;f.地磁矢量噪声= 1e-12;f.StateCovariance = 1e-9*ones(22); f.State = initstate; gpsidx = 1; N = size(accel,1); p = zeros(N,3); q = zeros(N,1,“四元数”gydF4y2Ba）;gydF4y2Ba

保险丝加速度计，陀螺仪，磁力计，和GPS数据。gydF4y2Ba

为gydF4y2BaIi = 1:size(accel,1)gydF4y2Ba熔断器IMUgydF4y2Baf.predict (accel (ii:),陀螺(ii):));gydF4y2Ba如果gydF4y2Ba~国防部(ii,修复(imuFs / 2))gydF4y2Ba熔断器磁强计在1/2 IMU速率gydF4y2Baf.fusemag (mag (ii):), Rmag);gydF4y2Ba结束gydF4y2Ba如果gydF4y2Ba~国防部(ii, imuFs)gydF4y2Ba每秒一次GPS引信gydF4y2Baf.fusegps (lla (gpsidx:), rpo, gpsvel (gpsidx:), Rvel);Gpsidx = Gpsidx + 1;gydF4y2Ba结束gydF4y2Ba[p(ii，:)，q(ii)] = pose(f);gydF4y2Ba%Log估计姿态gydF4y2Ba结束gydF4y2Ba

计算和显示RMS误差。gydF4y2Ba

posErr = truePos - p;qErr = rad2deg(dist(trueOrient,q));pRMS =√(mean(posErr.^2));qRMS =√(mean(qErr.^2));流(gydF4y2Ba位置RMS错误\n\tX: %。2f, Y: %。2f, Z: %。2f（米eter年代）\n\n'人口、难民和移民事务局(1)人口、难民和移民事务局(2),人口、难民和移民事务局(3));gydF4y2Ba
位置均方根误差X: 0.57, Y: 0.53, Z: 0.68(米)gydF4y2Ba
流(gydF4y2Ba'四元数距离RMS误差\n\t%。2 f(度)\ n \ n 'gydF4y2Ba,则);gydF4y2Ba
四元数距离均方根误差0.28(度)gydF4y2Ba

算法gydF4y2Ba

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

insfilterMARGgydF4y2Ba采用22轴扩展卡尔曼滤波结构在NED参考系中估计姿态。状态定义为:gydF4y2Ba

xgydF4y2Ba ＝gydF4y2Ba ［gydF4y2Ba 问gydF4y2Ba 0gydF4y2Ba 问gydF4y2Ba 1gydF4y2Ba 问gydF4y2Ba 2gydF4y2Ba 问gydF4y2Ba 3.gydF4y2Ba pgydF4y2Ba ogydF4y2Ba 年代gydF4y2Ba 我gydF4y2Ba tgydF4y2Ba 我gydF4y2Ba ogydF4y2Ba ngydF4y2Ba NgydF4y2Ba pgydF4y2Ba ogydF4y2Ba 年代gydF4y2Ba 我gydF4y2Ba tgydF4y2Ba 我gydF4y2Ba ogydF4y2Ba ngydF4y2Ba EgydF4y2Ba pgydF4y2Ba ogydF4y2Ba 年代gydF4y2Ba 我gydF4y2Ba tgydF4y2Ba 我gydF4y2Ba ogydF4y2Ba ngydF4y2Ba DgydF4y2Ba νgydF4y2Ba NgydF4y2Ba νgydF4y2Ba EgydF4y2Ba νgydF4y2Ba DgydF4y2Ba ΔgydF4y2Ba θgydF4y2Ba bgydF4y2Ba 我gydF4y2Ba 一个gydF4y2Ba 年代gydF4y2Ba XgydF4y2Ba ΔgydF4y2Ba θgydF4y2Ba bgydF4y2Ba 我gydF4y2Ba 一个gydF4y2Ba 年代gydF4y2Ba YgydF4y2Ba ΔgydF4y2Ba θgydF4y2Ba bgydF4y2Ba 我gydF4y2Ba 一个gydF4y2Ba 年代gydF4y2Ba ZgydF4y2Ba ΔgydF4y2Ba νgydF4y2Ba bgydF4y2Ba 我gydF4y2Ba 一个gydF4y2Ba 年代gydF4y2Ba XgydF4y2Ba ΔgydF4y2Ba νgydF4y2Ba bgydF4y2Ba 我gydF4y2Ba 一个gydF4y2Ba 年代gydF4y2Ba YgydF4y2Ba ΔgydF4y2Ba νgydF4y2Ba bgydF4y2Ba 我gydF4y2Ba 一个gydF4y2Ba 年代gydF4y2Ba ZgydF4y2Ba ggydF4y2Ba egydF4y2Ba ogydF4y2Ba 米gydF4y2Ba 一个gydF4y2Ba ggydF4y2Ba ngydF4y2Ba egydF4y2Ba tgydF4y2Ba 我gydF4y2Ba cgydF4y2Ba FgydF4y2Ba 我gydF4y2Ba egydF4y2Ba lgydF4y2Ba dgydF4y2Ba VgydF4y2Ba egydF4y2Ba cgydF4y2Ba tgydF4y2Ba ogydF4y2Ba rgydF4y2Ba NgydF4y2Ba ggydF4y2Ba egydF4y2Ba ogydF4y2Ba 米gydF4y2Ba 一个gydF4y2Ba ggydF4y2Ba ngydF4y2Ba egydF4y2Ba tgydF4y2Ba 我gydF4y2Ba cgydF4y2Ba FgydF4y2Ba 我gydF4y2Ba egydF4y2Ba lgydF4y2Ba dgydF4y2Ba VgydF4y2Ba egydF4y2Ba cgydF4y2Ba tgydF4y2Ba ogydF4y2Ba rgydF4y2Ba EgydF4y2Ba ggydF4y2Ba egydF4y2Ba ogydF4y2Ba 米gydF4y2Ba 一个gydF4y2Ba ggydF4y2Ba ngydF4y2Ba egydF4y2Ba tgydF4y2Ba 我gydF4y2Ba cgydF4y2Ba FgydF4y2Ba 我gydF4y2Ba egydF4y2Ba lgydF4y2Ba dgydF4y2Ba VgydF4y2Ba egydF4y2Ba cgydF4y2Ba tgydF4y2Ba ogydF4y2Ba rgydF4y2Ba DgydF4y2Ba 米gydF4y2Ba 一个gydF4y2Ba ggydF4y2Ba bgydF4y2Ba 我gydF4y2Ba 一个gydF4y2Ba 年代gydF4y2Ba XgydF4y2Ba 米gydF4y2Ba 一个gydF4y2Ba ggydF4y2Ba bgydF4y2Ba 我gydF4y2Ba 一个gydF4y2Ba 年代gydF4y2Ba YgydF4y2Ba 米gydF4y2Ba 一个gydF4y2Ba ggydF4y2Ba bgydF4y2Ba 我gydF4y2Ba 一个gydF4y2Ba 年代gydF4y2Ba ZgydF4y2Ba ］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θ的偏见gydF4y2BaXgydF4y2Ba,ΔgydF4y2Baθ的偏见gydF4y2BaYgydF4y2Ba,ΔgydF4y2Baθ的偏见gydF4y2BaZgydF4y2Ba——集成陀螺仪读数偏差。gydF4y2Ba

  • ΔgydF4y2Baν偏见gydF4y2BaXgydF4y2Ba,ΔgydF4y2Baν偏见gydF4y2BaYgydF4y2Ba,ΔgydF4y2Baν偏见gydF4y2BaZgydF4y2Ba——集成加速度计读数偏差。gydF4y2Ba

  • geomagneticFieldVectorgydF4y2BaNgydF4y2Ba，gydF4y2BageomagneticFieldVectorgydF4y2BaEgydF4y2Ba，gydF4y2BageomagneticFieldVectorgydF4y2BaDgydF4y2Ba——参考位置地磁场矢量估计。gydF4y2Ba

  • magbiasgydF4y2BaXgydF4y2Ba，gydF4y2BamagbiasgydF4y2BaYgydF4y2Ba，gydF4y2BamagbiasgydF4y2BaZgydF4y2Ba——磁力计读数偏差。gydF4y2Ba

给定预测状态估计的常规形式，gydF4y2Ba

xgydF4y2Ba kgydF4y2Ba |gydF4y2Ba kgydF4y2Ba −gydF4y2Ba 1gydF4y2Ba ＝gydF4y2Ba fgydF4y2Ba （gydF4y2Ba xgydF4y2Ba ＾gydF4y2Ba kgydF4y2Ba −gydF4y2Ba 1gydF4y2Ba |gydF4y2Ba kgydF4y2Ba −gydF4y2Ba 1gydF4y2Ba ，gydF4y2Ba ugydF4y2Ba kgydF4y2Ba ）gydF4y2Ba

ugydF4y2BakgydF4y2Ba由加速度计和陀螺仪数据控制，通过梯形积分转换为三角速度和三角角。预测状态估计为:gydF4y2Ba

xgydF4y2Ba kgydF4y2Ba |gydF4y2Ba kgydF4y2Ba −gydF4y2Ba 1gydF4y2Ba ＝gydF4y2Ba ［gydF4y2Ba 问gydF4y2Ba 0gydF4y2Ba 问gydF4y2Ba 0gydF4y2Ba ＇gydF4y2Ba −gydF4y2Ba 问gydF4y2Ba 1gydF4y2Ba 问gydF4y2Ba 1gydF4y2Ba ＇gydF4y2Ba −gydF4y2Ba 问gydF4y2Ba 2gydF4y2Ba 问gydF4y2Ba 2gydF4y2Ba ＇gydF4y2Ba −gydF4y2Ba 问gydF4y2Ba 3.gydF4y2Ba 问gydF4y2Ba 3.gydF4y2Ba ＇gydF4y2Ba 问gydF4y2Ba 1gydF4y2Ba 问gydF4y2Ba 0gydF4y2Ba ＇gydF4y2Ba +gydF4y2Ba 问gydF4y2Ba 0gydF4y2Ba 问gydF4y2Ba 1gydF4y2Ba ＇gydF4y2Ba −gydF4y2Ba 问gydF4y2Ba 3.gydF4y2Ba 问gydF4y2Ba 2gydF4y2Ba ＇gydF4y2Ba +gydF4y2Ba 问gydF4y2Ba 2gydF4y2Ba 问gydF4y2Ba 3.gydF4y2Ba ＇gydF4y2Ba 问gydF4y2Ba 2gydF4y2Ba 问gydF4y2Ba 0gydF4y2Ba ＇gydF4y2Ba +gydF4y2Ba 问gydF4y2Ba 3.gydF4y2Ba 问gydF4y2Ba 1gydF4y2Ba ＇gydF4y2Ba +gydF4y2Ba 问gydF4y2Ba 0gydF4y2Ba 问gydF4y2Ba 2gydF4y2Ba ＇gydF4y2Ba −gydF4y2Ba 问gydF4y2Ba 1gydF4y2Ba 问gydF4y2Ba 3.gydF4y2Ba ＇gydF4y2Ba 问gydF4y2Ba 3.gydF4y2Ba 问gydF4y2Ba 0gydF4y2Ba ＇gydF4y2Ba −gydF4y2Ba 问gydF4y2Ba 2gydF4y2Ba 问gydF4y2Ba 1gydF4y2Ba ＇gydF4y2Ba +gydF4y2Ba 问gydF4y2Ba 1gydF4y2Ba 问gydF4y2Ba 2gydF4y2Ba ＇gydF4y2Ba +gydF4y2Ba 问gydF4y2Ba 0gydF4y2Ba 问gydF4y2Ba 3.gydF4y2Ba ＇gydF4y2Ba pgydF4y2Ba ogydF4y2Ba 年代gydF4y2Ba 我gydF4y2Ba tgydF4y2Ba 我gydF4y2Ba ogydF4y2Ba ngydF4y2Ba NgydF4y2Ba +gydF4y2Ba （gydF4y2Ba ΔgydF4y2Ba tgydF4y2Ba ）gydF4y2Ba （gydF4y2Ba νgydF4y2Ba NgydF4y2Ba ）gydF4y2Ba pgydF4y2Ba ogydF4y2Ba 年代gydF4y2Ba 我gydF4y2Ba tgydF4y2Ba 我gydF4y2Ba ogydF4y2Ba ngydF4y2Ba EgydF4y2Ba +gydF4y2Ba （gydF4y2Ba ΔgydF4y2Ba tgydF4y2Ba ）gydF4y2Ba （gydF4y2Ba νgydF4y2Ba EgydF4y2Ba ）gydF4y2Ba pgydF4y2Ba ogydF4y2Ba 年代gydF4y2Ba 我gydF4y2Ba tgydF4y2Ba 我gydF4y2Ba ogydF4y2Ba ngydF4y2Ba DgydF4y2Ba +gydF4y2Ba （gydF4y2Ba ΔgydF4y2Ba tgydF4y2Ba ）gydF4y2Ba （gydF4y2Ba νgydF4y2Ba DgydF4y2Ba ）gydF4y2Ba νgydF4y2Ba NgydF4y2Ba +gydF4y2Ba （gydF4y2Ba ΔgydF4y2Ba tgydF4y2Ba ）gydF4y2Ba （gydF4y2Ba ggydF4y2Ba NgydF4y2Ba ）gydF4y2Ba +gydF4y2Ba （gydF4y2Ba ΔgydF4y2Ba νgydF4y2Ba XgydF4y2Ba −gydF4y2Ba ΔgydF4y2Ba νgydF4y2Ba bgydF4y2Ba 我gydF4y2Ba 一个gydF4y2Ba 年代gydF4y2Ba XgydF4y2Ba ）gydF4y2Ba （gydF4y2Ba 问gydF4y2Ba 0gydF4y2Ba 2gydF4y2Ba +gydF4y2Ba 问gydF4y2Ba 1gydF4y2Ba 2gydF4y2Ba −gydF4y2Ba 问gydF4y2Ba 2gydF4y2Ba 2gydF4y2Ba −gydF4y2Ba 问gydF4y2Ba 3.gydF4y2Ba 2gydF4y2Ba ）gydF4y2Ba −gydF4y2Ba 2gydF4y2Ba （gydF4y2Ba ΔgydF4y2Ba νgydF4y2Ba YgydF4y2Ba −gydF4y2Ba ΔgydF4y2Ba νgydF4y2Ba bgydF4y2Ba 我gydF4y2Ba 一个gydF4y2Ba 年代gydF4y2Ba YgydF4y2Ba ）gydF4y2Ba （gydF4y2Ba 问gydF4y2Ba 0gydF4y2Ba 问gydF4y2Ba 3.gydF4y2Ba −gydF4y2Ba 问gydF4y2Ba 1gydF4y2Ba 问gydF4y2Ba 2gydF4y2Ba ）gydF4y2Ba +gydF4y2Ba 2gydF4y2Ba （gydF4y2Ba ΔgydF4y2Ba νgydF4y2Ba ZgydF4y2Ba −gydF4y2Ba ΔgydF4y2Ba νgydF4y2Ba bgydF4y2Ba 我gydF4y2Ba 一个gydF4y2Ba 年代gydF4y2Ba ZgydF4y2Ba ）gydF4y2Ba （gydF4y2Ba 问gydF4y2Ba 0gydF4y2Ba 问gydF4y2Ba 2gydF4y2Ba +gydF4y2Ba 问gydF4y2Ba 1gydF4y2Ba 问gydF4y2Ba 3.gydF4y2Ba ）gydF4y2Ba νgydF4y2Ba EgydF4y2Ba +gydF4y2Ba （gydF4y2Ba ΔgydF4y2Ba tgydF4y2Ba ）gydF4y2Ba （gydF4y2Ba ggydF4y2Ba EgydF4y2Ba ）gydF4y2Ba +gydF4y2Ba （gydF4y2Ba ΔgydF4y2Ba νgydF4y2Ba YgydF4y2Ba −gydF4y2Ba ΔgydF4y2Ba νgydF4y2Ba bgydF4y2Ba 我gydF4y2Ba 一个gydF4y2Ba 年代gydF4y2Ba YgydF4y2Ba ）gydF4y2Ba （gydF4y2Ba 问gydF4y2Ba 0gydF4y2Ba 2gydF4y2Ba −gydF4y2Ba 问gydF4y2Ba 1gydF4y2Ba 2gydF4y2Ba +gydF4y2Ba 问gydF4y2Ba 2gydF4y2Ba 2gydF4y2Ba −gydF4y2Ba 问gydF4y2Ba 3.gydF4y2Ba 2gydF4y2Ba ）gydF4y2Ba +gydF4y2Ba 2gydF4y2Ba （gydF4y2Ba ΔgydF4y2Ba νgydF4y2Ba XgydF4y2Ba −gydF4y2Ba ΔgydF4y2Ba νgydF4y2Ba bgydF4y2Ba 我gydF4y2Ba 一个gydF4y2Ba 年代gydF4y2Ba XgydF4y2Ba ）gydF4y2Ba （gydF4y2Ba 问gydF4y2Ba 0gydF4y2Ba 问gydF4y2Ba 3.gydF4y2Ba +gydF4y2Ba 问gydF4y2Ba 1gydF4y2Ba 问gydF4y2Ba 2gydF4y2Ba ）gydF4y2Ba −gydF4y2Ba 2gydF4y2Ba （gydF4y2Ba ΔgydF4y2Ba νgydF4y2Ba ZgydF4y2Ba −gydF4y2Ba ΔgydF4y2Ba νgydF4y2Ba bgydF4y2Ba 我gydF4y2Ba 一个gydF4y2Ba 年代gydF4y2Ba ZgydF4y2Ba ）gydF4y2Ba （gydF4y2Ba 问gydF4y2Ba 0gydF4y2Ba 问gydF4y2Ba 1gydF4y2Ba −gydF4y2Ba 问gydF4y2Ba 2gydF4y2Ba 问gydF4y2Ba 3.gydF4y2Ba ）gydF4y2Ba νgydF4y2Ba DgydF4y2Ba +gydF4y2Ba （gydF4y2Ba ΔgydF4y2Ba tgydF4y2Ba ）gydF4y2Ba （gydF4y2Ba ggydF4y2Ba DgydF4y2Ba ）gydF4y2Ba +gydF4y2Ba （gydF4y2Ba ΔgydF4y2Ba νgydF4y2Ba ZgydF4y2Ba −gydF4y2Ba ΔgydF4y2Ba νgydF4y2Ba bgydF4y2Ba 我gydF4y2Ba 一个gydF4y2Ba 年代gydF4y2Ba ZgydF4y2Ba ）gydF4y2Ba （gydF4y2Ba 问gydF4y2Ba 0gydF4y2Ba 2gydF4y2Ba −gydF4y2Ba 问gydF4y2Ba 1gydF4y2Ba 2gydF4y2Ba −gydF4y2Ba 问gydF4y2Ba 2gydF4y2Ba 2gydF4y2Ba +gydF4y2Ba 问gydF4y2Ba 3.gydF4y2Ba 2gydF4y2Ba ）gydF4y2Ba −gydF4y2Ba 2gydF4y2Ba （gydF4y2Ba ΔgydF4y2Ba νgydF4y2Ba XgydF4y2Ba −gydF4y2Ba ΔgydF4y2Ba νgydF4y2Ba bgydF4y2Ba 我gydF4y2Ba 一个gydF4y2Ba 年代gydF4y2Ba XgydF4y2Ba ）gydF4y2Ba （gydF4y2Ba 问gydF4y2Ba 0gydF4y2Ba 问gydF4y2Ba 2gydF4y2Ba −gydF4y2Ba 问gydF4y2Ba 1gydF4y2Ba 问gydF4y2Ba 3.gydF4y2Ba ）gydF4y2Ba +gydF4y2Ba 2gydF4y2Ba （gydF4y2Ba ΔgydF4y2Ba νgydF4y2Ba YgydF4y2Ba −gydF4y2Ba ΔgydF4y2Ba νgydF4y2Ba bgydF4y2Ba 我gydF4y2Ba 一个gydF4y2Ba 年代gydF4y2Ba YgydF4y2Ba ）gydF4y2Ba （gydF4y2Ba 问gydF4y2Ba 0gydF4y2Ba 问gydF4y2Ba 1gydF4y2Ba +gydF4y2Ba 问gydF4y2Ba 2gydF4y2Ba 问gydF4y2Ba 3.gydF4y2Ba ）gydF4y2Ba ΔgydF4y2Ba θgydF4y2Ba bgydF4y2Ba 我gydF4y2Ba 一个gydF4y2Ba 年代gydF4y2Ba XgydF4y2Ba ΔgydF4y2Ba θgydF4y2Ba bgydF4y2Ba 我gydF4y2Ba 一个gydF4y2Ba 年代gydF4y2Ba YgydF4y2Ba ΔgydF4y2Ba θgydF4y2Ba bgydF4y2Ba 我gydF4y2Ba 一个gydF4y2Ba 年代gydF4y2Ba ZgydF4y2Ba ΔgydF4y2Ba νgydF4y2Ba bgydF4y2Ba 我gydF4y2Ba 一个gydF4y2Ba 年代gydF4y2Ba XgydF4y2Ba ΔgydF4y2Ba νgydF4y2Ba bgydF4y2Ba 我gydF4y2Ba 一个gydF4y2Ba 年代gydF4y2Ba YgydF4y2Ba ΔgydF4y2Ba νgydF4y2Ba bgydF4y2Ba 我gydF4y2Ba 一个gydF4y2Ba 年代gydF4y2Ba ZgydF4y2Ba ggydF4y2Ba egydF4y2Ba ogydF4y2Ba 米gydF4y2Ba 一个gydF4y2Ba ggydF4y2Ba ngydF4y2Ba egydF4y2Ba tgydF4y2Ba 我gydF4y2Ba cgydF4y2Ba FgydF4y2Ba 我gydF4y2Ba egydF4y2Ba lgydF4y2Ba dgydF4y2Ba VgydF4y2Ba egydF4y2Ba cgydF4y2Ba tgydF4y2Ba ogydF4y2Ba rgydF4y2Ba NgydF4y2Ba ggydF4y2Ba egydF4y2Ba ogydF4y2Ba 米gydF4y2Ba 一个gydF4y2Ba ggydF4y2Ba ngydF4y2Ba egydF4y2Ba tgydF4y2Ba 我gydF4y2Ba cgydF4y2Ba FgydF4y2Ba 我gydF4y2Ba egydF4y2Ba lgydF4y2Ba dgydF4y2Ba VgydF4y2Ba egydF4y2Ba cgydF4y2Ba tgydF4y2Ba ogydF4y2Ba rgydF4y2Ba EgydF4y2Ba ggydF4y2Ba egydF4y2Ba ogydF4y2Ba 米gydF4y2Ba 一个gydF4y2Ba ggydF4y2Ba ngydF4y2Ba egydF4y2Ba tgydF4y2Ba 我gydF4y2Ba cgydF4y2Ba FgydF4y2Ba 我gydF4y2Ba egydF4y2Ba lgydF4y2Ba dgydF4y2Ba VgydF4y2Ba egydF4y2Ba cgydF4y2Ba tgydF4y2Ba ogydF4y2Ba rgydF4y2Ba DgydF4y2Ba 米gydF4y2Ba 一个gydF4y2Ba ggydF4y2Ba bgydF4y2Ba 我gydF4y2Ba 一个gydF4y2Ba 年代gydF4y2Ba XgydF4y2Ba 米gydF4y2Ba 一个gydF4y2Ba ggydF4y2Ba bgydF4y2Ba 我gydF4y2Ba 一个gydF4y2Ba 年代gydF4y2Ba YgydF4y2Ba 米gydF4y2Ba 一个gydF4y2Ba ggydF4y2Ba bgydF4y2Ba 我gydF4y2Ba 一个gydF4y2Ba 年代gydF4y2Ba ZgydF4y2Ba ］gydF4y2Ba

式中，(gydF4y2Ba问gydF4y2Ba0gydF4y2Ba＇gydF4y2Ba，gydF4y2Ba问gydF4y2Ba1gydF4y2Ba＇gydF4y2Ba，gydF4y2Ba问gydF4y2Ba2gydF4y2Ba＇gydF4y2Ba，gydF4y2Ba问gydF4y2Ba3.gydF4y2Ba＇gydF4y2Ba)是表示从一步到下一步方向变化的四元数。假设方向变化较小，则旋转向量可近似为(ΔgydF4y2BaθgydF4y2BaXgydF4y2Ba−ΔgydF4y2Baθ的偏见gydF4y2BaXgydF4y2Ba,ΔgydF4y2BaθgydF4y2BaYgydF4y2Ba−ΔgydF4y2Baθ的偏见gydF4y2BaYgydF4y2Ba,ΔgydF4y2BaθgydF4y2BaZgydF4y2Ba−ΔgydF4y2Baθ的偏见gydF4y2BaZgydF4y2Ba)，其中ΔgydF4y2BaθgydF4y2BaXgydF4y2Ba,ΔgydF4y2BaθgydF4y2BaYgydF4y2Ba,ΔgydF4y2BaθgydF4y2BaZgydF4y2Ba是集成陀螺仪读数。（gydF4y2Ba问gydF4y2Ba0gydF4y2Ba＇gydF4y2Ba，gydF4y2Ba问gydF4y2Ba1gydF4y2Ba＇gydF4y2Ba，gydF4y2Ba问gydF4y2Ba2gydF4y2Ba＇gydF4y2Ba，gydF4y2Ba问gydF4y2Ba3.gydF4y2Ba＇gydF4y2Ba)，然后通过将近似的旋转向量转换为四元数得到。在每次计算中，四元数被归一化，使得四元数的长度为1及其实部gydF4y2Ba问gydF4y2Ba0gydF4y2Ba是负的。gydF4y2Ba

此外,gydF4y2Ba

  • ΔgydF4y2BaνgydF4y2BaXgydF4y2Ba,ΔgydF4y2BaνgydF4y2BaYgydF4y2Ba,ΔgydF4y2BaνgydF4y2BaZgydF4y2Ba——集成加速度计读数。gydF4y2Ba

  • ΔgydF4y2BatgydF4y2Ba——IMU采样时间。gydF4y2Ba

  • ggydF4y2BaNgydF4y2Ba，gydF4y2BaggydF4y2BaEgydF4y2Ba，gydF4y2BaggydF4y2BaDgydF4y2Ba—恒定的重力矢量在NED框架。gydF4y2Ba

扩展功能gydF4y2Ba

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

版本历史gydF4y2Ba

在R2018b中引入gydF4y2Ba

另请参阅gydF4y2Ba

|gydF4y2Ba|gydF4y2Ba

主题gydF4y2Ba