insfilterAsyncGÿdF4y2Ba

利用异步MARG和GPS数据估计姿态GÿdF4y2Ba

描述GÿdF4y2Ba

该GÿdF4y2BainsfilterAsyncGÿdF4y2Ba目标将MARG和GPS数据进行传感器融合,以估计在NED(或ENU)参考系中的姿态。MARG(磁性,角速度,重力)数据通常分别来自磁强计,陀螺仪和加速度计数据。过滤器使用一个28个元素的状态向量来跟踪方向GÿdF4y2Ba四元GÿdF4y2Ba,速度,位置,玛格传感器偏差,和地磁向量。该GÿdF4y2BainsfilterAsyncGÿdF4y2Ba对象使用连续离散扩展卡尔曼滤波器来估计这些量。GÿdF4y2Ba

创建GÿdF4y2Ba

描述GÿdF4y2Ba

例GÿdF4y2Ba

过滤器GÿdF4y2Ba= insfilterAsyncGÿdF4y2Ba创建GÿdF4y2BainsiflterAsyncGÿdF4y2Ba与默认属性值对象到保险丝异步玛格及GPS数据。GÿdF4y2Ba

过滤器GÿdF4y2Ba= insfilterAsync (GÿdF4y2Ba'参考范围'GÿdF4y2Ba,GÿdF4y2Ba射频GÿdF4y2Ba)GÿdF4y2Ba允许您指定参考帧,GÿdF4y2Ba射频GÿdF4y2Ba的,GÿdF4y2Ba过滤器GÿdF4y2Ba。指定GÿdF4y2Ba射频GÿdF4y2Ba如GÿdF4y2BaNED的GÿdF4y2Ba(北 - 东 - 下)或GÿdF4y2Ba“ENU表示”GÿdF4y2Ba(East-North-Up)。默认值为GÿdF4y2BaNED的GÿdF4y2Ba。GÿdF4y2Ba

过滤器GÿdF4y2Ba= insfilterAsync (GÿdF4y2Ba___GÿdF4y2Ba,GÿdF4y2Ba名称,值GÿdF4y2Ba)GÿdF4y2Ba还允许您设置所创建的属性GÿdF4y2Ba过滤器GÿdF4y2Ba使用一个或多个名称值对。单引号括起来每个属性的名称。GÿdF4y2Ba

属性GÿdF4y2Ba

全部展开GÿdF4y2Ba

参考位置,指定为在大地坐标的三元件行向量(纬度,经度和高度)。高度为基准椭球模型,WGS84以上的高度。参考位置单位是[度度米]。GÿdF4y2Ba

数据类型:GÿdF4y2Ba单GÿdF4y2Ba|GÿdF4y2Ba双GÿdF4y2Ba

加性四元数处理噪声方差,指定为四元数部分的标量或四元向量。GÿdF4y2Ba

数据类型:GÿdF4y2Ba单GÿdF4y2Ba|GÿdF4y2Ba双GÿdF4y2Ba

局部导航坐标系中附加的角速度过程噪声(rad/s)GÿdF4y2Ba2GÿdF4y2Ba指定为正实有限数的标量或三元素行向量。GÿdF4y2Ba

  • 如果GÿdF4y2BaAngularVelocityNoiseGÿdF4y2Ba是一个行向量,该元素对应于在该噪声GÿdF4y2BaXGÿdF4y2Ba,GÿdF4y2BaÿGÿdF4y2Ba和GÿdF4y2BažGÿdF4y2Ba分别为局部导航坐标系的坐标轴。GÿdF4y2Ba

  • 如果GÿdF4y2BaAngularVelocityNoiseGÿdF4y2Ba是一个标量,单一元件被施加到每个轴。GÿdF4y2Ba

数据类型:GÿdF4y2Ba单GÿdF4y2Ba|GÿdF4y2Ba双GÿdF4y2Ba

在m的局部导航坐标系中加入位置过程噪声GÿdF4y2Ba2GÿdF4y2Ba指定为正实有限数的标量或三元素行向量。GÿdF4y2Ba

  • 如果GÿdF4y2BaPositionNoiseGÿdF4y2Ba是一个行向量,该元素对应于在该噪声GÿdF4y2BaXGÿdF4y2Ba,GÿdF4y2BaÿGÿdF4y2Ba和GÿdF4y2BažGÿdF4y2Ba分别为局部导航坐标系的坐标轴。GÿdF4y2Ba

  • 如果GÿdF4y2BaPositionNoiseGÿdF4y2Ba是一个标量,单一元件被施加到每个轴。GÿdF4y2Ba

数据类型:GÿdF4y2Ba单GÿdF4y2Ba|GÿdF4y2Ba双GÿdF4y2Ba

局部导航坐标系中速度加性过程噪声(m/s)GÿdF4y2Ba2GÿdF4y2Ba指定为正实有限数的标量或三元素行向量。GÿdF4y2Ba

  • 如果GÿdF4y2BaVelocityNoiseGÿdF4y2Ba是一个行向量,该元素对应于在该噪声GÿdF4y2BaXGÿdF4y2Ba,GÿdF4y2BaÿGÿdF4y2Ba和GÿdF4y2BažGÿdF4y2Ba分别为局部导航坐标系的坐标轴。GÿdF4y2Ba

  • 如果GÿdF4y2BaVelocityNoiseGÿdF4y2Ba是一个标量,单一元件被施加到每个轴。GÿdF4y2Ba

数据类型:GÿdF4y2Ba单GÿdF4y2Ba|GÿdF4y2Ba双GÿdF4y2Ba

在添加剂加速过程噪声(米/秒GÿdF4y2Ba2GÿdF4y2Ba)GÿdF4y2Ba2GÿdF4y2Ba指定为正实有限数的标量或三元素行向量。GÿdF4y2Ba

  • 如果GÿdF4y2BaAccelerationNoiseGÿdF4y2Ba是一个行向量,该元素对应于在该噪声GÿdF4y2BaXGÿdF4y2Ba,GÿdF4y2BaÿGÿdF4y2Ba和GÿdF4y2BažGÿdF4y2Ba分别为局部导航坐标系的坐标轴。GÿdF4y2Ba

  • 如果GÿdF4y2BaAccelerationNoiseGÿdF4y2Ba是一个标量,单一元件被施加到每个轴。GÿdF4y2Ba

数据类型:GÿdF4y2Ba单GÿdF4y2Ba|GÿdF4y2Ba双GÿdF4y2Ba

(rad/s)中陀螺仪偏差的加性过程噪声方差GÿdF4y2Ba2GÿdF4y2Ba指定为正实有限数的标量或三元素行向量。GÿdF4y2Ba

  • 如果GÿdF4y2BaGyroscopeBiasNoiseGÿdF4y2Ba是一个行向量,该元素对应于在该噪声GÿdF4y2BaXGÿdF4y2Ba,GÿdF4y2BaÿGÿdF4y2Ba和GÿdF4y2BažGÿdF4y2Ba陀螺仪的轴。GÿdF4y2Ba

  • 如果GÿdF4y2BaGyroscopeBiasNoiseGÿdF4y2Ba是一个标量,单一元件被施加到每个轴。GÿdF4y2Ba

数据类型:GÿdF4y2Ba单GÿdF4y2Ba|GÿdF4y2Ba双GÿdF4y2Ba

加速度计偏差引起的附加过程噪声方差(m/s)GÿdF4y2Ba2GÿdF4y2Ba)GÿdF4y2Ba2GÿdF4y2Ba,指定为具有正实数的标量或三元素行向量。GÿdF4y2Ba

  • 如果GÿdF4y2BaAccelerometerBiasNoiseGÿdF4y2Ba是一个行向量,该元素对应于在该噪声GÿdF4y2BaXGÿdF4y2Ba,GÿdF4y2BaÿGÿdF4y2Ba和GÿdF4y2BažGÿdF4y2Ba加速度计的轴。GÿdF4y2Ba

  • 如果GÿdF4y2BaAccelerometerBiasNoiseGÿdF4y2Ba是一个标量,单一元件被施加到每个轴。GÿdF4y2Ba

在μT地磁向量的加成法噪声方差GÿdF4y2Ba2GÿdF4y2Ba,指定为具有正实数的标量或三元素行向量。GÿdF4y2Ba

  • 如果GÿdF4y2BaGeomagneticVectorNoiseGÿdF4y2Ba是一个行向量,该元素对应于在该噪声GÿdF4y2BaXGÿdF4y2Ba,GÿdF4y2BaÿGÿdF4y2Ba和GÿdF4y2BažGÿdF4y2Ba分别为局部导航坐标系的坐标轴。GÿdF4y2Ba

  • 如果GÿdF4y2BaGeomagneticVectorNoiseGÿdF4y2Ba是一个标量,单一元件被施加到每个轴。GÿdF4y2Ba

添加剂过程噪声方差在μT磁强计偏差GÿdF4y2Ba2GÿdF4y2Ba,指定为具有正实数的标量或三元素行向量。GÿdF4y2Ba

  • 如果GÿdF4y2BaMagnetometerBiasNoiseGÿdF4y2Ba是一个行向量,该元素对应于在该噪声GÿdF4y2BaXGÿdF4y2Ba,GÿdF4y2BaÿGÿdF4y2Ba和GÿdF4y2BažGÿdF4y2Ba轴磁强计,分别。GÿdF4y2Ba

  • 如果GÿdF4y2BaMagnetometerBiasNoiseGÿdF4y2Ba是一个标量,单一元件被施加到每个轴。GÿdF4y2Ba

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

州GÿdF4y2Ba 单位GÿdF4y2Ba 指数GÿdF4y2Ba
取向(四元数份)GÿdF4y2Ba N/AGÿdF4y2Ba 1:4GÿdF4y2Ba
角速度(XYZ)GÿdF4y2Ba rad /秒GÿdF4y2Ba 7GÿdF4y2Ba
职位(NED或ENU)GÿdF4y2Ba 米GÿdF4y2Ba 8:10GÿdF4y2Ba
速度(NED或ENU)GÿdF4y2Ba 米/秒GÿdF4y2Ba 11:13GÿdF4y2Ba
加速度(NED或ENU)GÿdF4y2Ba 米/秒GÿdF4y2Ba2GÿdF4y2Ba 14:16GÿdF4y2Ba
加速度计的偏见(某某)GÿdF4y2Ba 米/秒GÿdF4y2Ba2GÿdF4y2Ba 17:19GÿdF4y2Ba
陀螺仪偏置(XYZ)GÿdF4y2Ba rad /秒GÿdF4y2Ba 20:22GÿdF4y2Ba
地磁场矢量(NED或ENU)GÿdF4y2Ba μTGÿdF4y2Ba 23:25GÿdF4y2Ba
磁强计偏差(某某)GÿdF4y2Ba μTGÿdF4y2Ba 26:28GÿdF4y2Ba

默认的初始状态对应于位于rest的对象GÿdF4y2Ba(0 0 0)GÿdF4y2Ba在大地测量坐标LLA。GÿdF4y2Ba

数据类型:GÿdF4y2Ba单GÿdF4y2Ba|GÿdF4y2Ba双GÿdF4y2Ba

扩展卡尔曼滤波器的状态误差协方差,指定为一个28乘28元素的实数矩阵。GÿdF4y2Ba

数据类型:GÿdF4y2Ba单GÿdF4y2Ba|GÿdF4y2Ba双GÿdF4y2Ba

对象函数GÿdF4y2Ba

预测GÿdF4y2Ba 基于运动模型更新状态GÿdF4y2Ba
fuseaccelGÿdF4y2Ba 使用加速度计数据纠正状态GÿdF4y2Ba
fusegyroGÿdF4y2Ba 使用陀螺仪数据修正状态GÿdF4y2Ba
fusemagGÿdF4y2Ba 使用磁强计数据修正状态GÿdF4y2Ba
fusegpsGÿdF4y2Ba 使用GPS数据正确的状态GÿdF4y2Ba
正确GÿdF4y2Ba 使用直接的状态测量纠正状态GÿdF4y2Ba
剩余的GÿdF4y2Ba 直接状态测量的残差和残差协方差GÿdF4y2Ba
residualaccelGÿdF4y2Ba 残差和由加速度计测量的残余协方差GÿdF4y2Ba
residualgpsGÿdF4y2Ba 残差和GPS测量剩余协方差GÿdF4y2Ba
residualmagGÿdF4y2Ba 残差和来自磁力计测量残余协方差GÿdF4y2Ba
residualgyroGÿdF4y2Ba 残差和来自陀螺仪的测量协方差残留GÿdF4y2Ba
姿势GÿdF4y2Ba 当前位置、方向和速度估计GÿdF4y2Ba
重置GÿdF4y2Ba 复位内部状态GÿdF4y2Ba
stateinfoGÿdF4y2Ba 显示状态向量信息GÿdF4y2Ba

例子GÿdF4y2Ba

全部收缩GÿdF4y2Ba

加载传感器数据和地面真实姿态。GÿdF4y2Ba

负载(GÿdF4y2Ba“uavshort.mat”GÿdF4y2Ba,GÿdF4y2Ba“refloc”GÿdF4y2Ba,GÿdF4y2Ba“initstate”GÿdF4y2Ba,GÿdF4y2Ba“imuFs”GÿdF4y2Ba,GÿdF4y2Ba...GÿdF4y2Ba“加速度”GÿdF4y2Ba,GÿdF4y2Ba“陀螺”GÿdF4y2Ba,GÿdF4y2Ba“杂志”GÿdF4y2Ba,GÿdF4y2Ba'LLA'GÿdF4y2Ba,GÿdF4y2Ba'gpsvel'GÿdF4y2Ba,GÿdF4y2Ba...GÿdF4y2Ba“trueOrient”GÿdF4y2Ba,GÿdF4y2Ba'truePos'GÿdF4y2Ba)GÿdF4y2Ba

创建一个INS过滤器来融合异步MARG和GPS数据来估计姿态。GÿdF4y2Ba

filt = insfilterAsync;filt。ReferenceLocation = refloc;filt。状态= [initstate (1:4); 0, 0, 0; initstate (5:10); 0, 0, 0; initstate(11:结束)];GÿdF4y2Ba

定义传感器测量噪声。通过数据表和实验对噪声进行了测定。GÿdF4y2Ba

Rmag = 80;Rvel = 0.0464;Racc = 800;Rgyro = 1的军医;rpo = 34;GÿdF4y2Ba

为位置和方向预先分配变量。为GPS数据分配一个索引变量。GÿdF4y2Ba

N =大小(accel, 1);p = 0 (N, 3);q = 0 (N, 1GÿdF4y2Ba“四元”GÿdF4y2Ba);gpsIdx = 1;GÿdF4y2Ba

保险丝加速度计,陀螺仪,磁力计和GPS数据。外环向前预测滤波器,以IMU采样率一个时间步长和保险丝的加速度计和陀螺仪数据。GÿdF4y2Ba

对于GÿdF4y2Ba2 = 1:NGÿdF4y2Ba%预测滤波器向前一个时间步长GÿdF4y2Ba预测(FILT,1 / imuFs。);GÿdF4y2Ba熔丝加速度计和陀螺仪读数GÿdF4y2Bafuseaccel (filt accel (ii):), Racc);fusegyro (filt陀螺(ii):), Rgyro);GÿdF4y2Ba%熔断器磁强计在1/2的IMU的速度GÿdF4y2Ba如果GÿdF4y2Ba〜MOD(二,修复(imuFs / 2))fusemag(FILT,MAG(二,:),RMAG);GÿdF4y2Ba结束GÿdF4y2Ba每秒融合GPS一次GÿdF4y2Ba如果GÿdF4y2Ba〜MOD(二,imuFs)fusegps(FILT,LLA(gpsIdx,:),RPOS,gpsvel(gpsIdx,:),Rvel);gpsIdx = gpsIdx + 1;GÿdF4y2Ba结束GÿdF4y2Ba记录当前位姿估计值GÿdF4y2Ba[p (ii):), (ii)] =姿势(filt);GÿdF4y2Ba结束GÿdF4y2Ba

计算已知真实位置和方向与异步IMU滤波器输出之间的RMS误差。GÿdF4y2Ba

posErr = truePos - p;qErr = rad2deg (dist (trueOrient q));人口、难民和移民事务局(12(平均(posErr ^ 2));则=√意味着(qErr ^ 2));fprintf中(GÿdF4y2Ba'位置RMS错误。\ n'GÿdF4y2Ba);GÿdF4y2Ba
位置RMS错误GÿdF4y2Ba
fprintf中(GÿdF4y2Ba“\ tX: %。2F,ÿ: %.2f, Z: %.2f (meters)\n\n',PRMS(1),PRMS(2),PRMS(3));GÿdF4y2Ba
X:0.55,Y:0.71,Z:0.74(米)GÿdF4y2Ba
fprintf中(GÿdF4y2Ba四元数距离均方根误差GÿdF4y2Ba);GÿdF4y2Ba
四元数距离均方根误差GÿdF4y2Ba
fprintf中(GÿdF4y2Ba' \ t %。2F(dËG[RËË小号)\n\n',则);GÿdF4y2Ba
4.72(度)GÿdF4y2Ba

可视化的真实位置和估计位置。GÿdF4y2Ba

plot3(truePos(:,1),truePos(:,2),truePos(:,3),GÿdF4y2Ba“线宽”GÿdF4y2Ba2)保持GÿdF4y2Ba在GÿdF4y2Baplot3 (p (: 1), p (:, 2), p (:, 3),GÿdF4y2Ba'R:'GÿdF4y2Ba,GÿdF4y2Ba“线宽”GÿdF4y2Ba,2)网格GÿdF4y2Ba在GÿdF4y2Ba包含(GÿdF4y2Ba“N (m)”GÿdF4y2Ba)ylabel (GÿdF4y2Ba'E(M)'GÿdF4y2Ba)zlabel(GÿdF4y2Ba“D (m)”GÿdF4y2Ba)GÿdF4y2Ba

算法GÿdF4y2Ba

全部展开GÿdF4y2Ba

扩展功能GÿdF4y2Ba

C / c++代码生成GÿdF4y2Ba
生成使用MATLAB®编码器™C和C ++代码。GÿdF4y2Ba

介绍了在R2019aGÿdF4y2Ba