来自异步传感器的姿态估计

打开脚本

此示例显示了如何以不同的速率熔断传感器来估算姿势。加速度计，陀螺仪，磁力计和GPS用于确定沿圆形路径移动的车辆的取向和位置。您可以在图窗口上使用控件，以改变传感器速率，并在看到对估计姿势的影响时使用传感器丢失进行实验。

模拟设置

加载预先记录的传感器数据。传感器数据基于使用的圆形轨迹WayPointTrajectory.类。属性创建传感器值GPSSensor.和imusvesor.类。的CircularTrajectorySensordata.mat这里使用的文件可以生成generatecirculartrajsensordata.函数。

ld =负载('CircularTrajectorysensordata.mat'）;Fs = ld.Fs;％最大马格率gpsFs = ld.gpsFs;最大GPS速率%比率= fs./gpsfs;RESHOC = LD.REFLOC;trajorient = ld.trajdata.orientation;trajvel = ld.trajdata.velocity;trajpos = ld.trajdata.position;trajacc = ld.trajdata.acceleration;trajangvel = ld.trajdata.angularvelocity;Accel = ld.accel;gyro = ld.gyro;mag = ld.mag; lla = ld.lla; gpsvel = ld.gpsvel;

融合滤波器

创建一个insfilterasync.熔断IMU + GPS测量。该融合滤波器使用连续离散的扩展卡尔曼滤波器（EKF）来跟踪方向（作为四元数），角速度，位置，速度，加速度，传感器偏差和地磁矢量。

这insfilterasync.有几种处理传感器数据的方法：fuseaccel，fusegyro.，fusemag.和fusegps．因为insfilterasync.使用连续离散的EKF，预测方法可以将过滤器转发到任意时间。

fusionfilt = insfilterAsync (“ReferenceLocation”，回流）;

的状态向量`insfilterasync.`

的insfilterasync.在28元载体中追踪姿势状态。国家是：

状态单位指向方向（四元零件）1：4角速度（XYZ）RAD / S 5：7位置（NED）M 8:10速度（NED）M / S 11:13加速（NED）M / S ^ 2 14：16加速度计偏压（XYZ）M / S ^ 2 17:19陀螺偏压（XYZ）Rad / S 20:22地磁场矢量（NED）UT 23:25磁力计偏差（XYZ）UT 26:28

Ground truth被用来帮助初始化过滤器状态，所以过滤器快速收敛到好的答案。

资产净值= 100;initstate = 0(28日1);initstate(1:4) = compact(meanrot(trajOrient(1:Nav)));initstate(5:7) = mean(trajAngVel(10:Nav，:)， 1);initstate(8:10) = mean(trajPos(1:Nav，:)， 1);initstate(11:13) = mean(trajVel(1:Nav，:)， 1);initstate(14:16) = mean(trajAcc(1:Nav，:)， 1);initstate (23:25) = ld.magField;Z轴的陀螺仪偏置初始值估计值低。这是%的结果，以说明融合磁强计的影响％ 模拟。Initstate (20:22) = deg2rad([3.125 3.125 3.125]);fusionfilt。状态= initstate;

设置过程噪声值`insfilterasync.`

过程噪声方差描述了滤波器使用的运动模型的不确定性。

fusionfilt.quaternionnoise = 1e-2;fusionfilt.angularvelocitynoise = 100;Fusionfilt.accelerationNoise = 100;Fusionfilt.MagnetomerBiasnoise = 1E-7;fusionfilt.accelerometerbiasnoise = 1e-7;fusionfilt.gyroscopybiasnoise = 1e-7;

定义用于融合传感器数据的测量噪声值

每个传感器在测量中都有一些噪声。这些值通常可以在传感器的数据表中找到。

Rmag = 0.4;Rvel = 0.01;Racc = 610;Rgyro = 0.76 e-5;rpo = 3.4;fusionfilt。StateCovariance =诊断接头(1 e - 3 * 1(28日1));

初始化范围

的HelperScrollingPlotter范围使得能够随着时间的推移绘制变量。这里用于跟踪姿势的错误。的PoseViewerWithSwitches.范围允许三维可视化的滤波器估计和地面真实的姿态。示波器会减慢模拟速度。若要禁用作用域，请将相应的逻辑变量设置为false。

umererrscope = true;％打开流误差绘图。usePoseView = true;%打开3D姿势查看器。如果usePoseView posescope = PoseViewerWithSwitches(．..“XPositionLimits”, 30 [-30],．..'ypositionLimits'，[-30,30]，．..'zpositionlimits'，[-10 10]）;结束f = gcf;如果usererrscope errscope = HelperScrollingPlotter(．..'numinputs'4．..“时间间隔”10，．..“SampleRate”Fs,．..'ylabel'，{“度”，．..“米”，．..“米”，．..“米”}，．..'标题'，{'四元距离'，．..'位置x错误'，．..'位置y错误'，．..“Z位置错误”}，．..“YLimits”，．..[- 1,30 - 2,2 - 2,2 -2]);结束

仿真环路

融合算法的仿真可以让你检查传感器采样率变化的影响。此外，可以通过取消对应的复选框来防止单个传感器的融合。这可以用来模拟传感器辍学。

有些配置会产生戏剧性的结果。例如，关闭GPS传感器会导致位置估计快速漂移。关闭磁强计传感器将导致估计方向缓慢偏离地面真相，因为估计旋转得太快。相反，如果陀螺仪被关闭，而磁力计被打开，估计的方向显示一个摆动，缺乏平滑性，如果使用两个传感器。

打开所有传感器，但将它们以最低速率运行产生估计值明显地从地面真理偏离，然后在传感器融合时返回更正确的结果。这是最容易看到的HelperScrollingPlotter的运行估计误差。

主要模拟在100 Hz时运行。每次迭代都在图窗口上检查复选框，如果传感器已启用，则以适当的速率熔化该传感器的数据。

为II = 1：尺寸（Accel，1）Fusionfilt.predict（1./fs）;％保险丝加速度计如果(f.UserData.Accelerometer) & &．..mod(ii, fix(Fs/ f.s erdata . accelerometersamplerate)) == 0 fusion .fuseaccel(accel(ii，:)， Racc);结束％保险丝陀螺仪如果（f.userdata.gyroscope）&&．..mod（ii，fix（fs / f.userdata.gyroscopeSamplerge））== 0 fusionfilt.fusegyro（陀螺（ii，:)，rgyro）;结束％保险丝磁力计如果（f.UserData.Magnetometer）&&．..mod(ii, fix(Fs/ f.s erdata . magnetometersamplerate)) == 0结束%融合GPS如果(f.UserData.GPS) && mod(ii, fix(Fs/f.UserData.GPSSampleRate)) == 0 fusion .fusegps(lla(ii，:)， Rpos, gpsvel(ii，:)， Rvel);结束％绘制姿势错误[p，q] =姿势（Fusionfilt）;Posescope（p，q，trajpos（ii，:)，trajorient（ii））;Orienterr = Rad2deg（dist（q，trajorient（ii）））;poserr = p  -  trajpos（ii，:);errscope（东方，poserr（1），poserr（2），poserr（3））;结束