异步传感器姿态估计

打开脚本

这个例子展示了如何以不同的速率融合传感器来估计姿势。加速度计、陀螺仪、磁力计和GPS用于确定车辆沿圆周路径移动的方向和位置。您可以在图形窗口上使用控件来改变传感器速率，并在看到对估计姿态的影响时尝试传感器退出。

仿真设置

加载预先记录的传感器数据。传感器数据是基于使用创建的圆形轨迹waypointTrajectory类。方法创建传感器值gpsSensor和imuSensor类。的CircularTrajectorySensorData.mat生成这里使用的文件generateCircularTrajSensorData函数。

Ld =负载“CircularTrajectorySensorData.mat”）;Fs = ld.Fs;%最大利润率gpsFs = ld.gpsFs;最大GPS速率%ratio = Fs./gpsFs;Refloc = ld.refloc;trajOrient = ld.trajData.Orientation;travelvel = ld.trajData.Velocity;trajPos = ld.trajData.Position;trajAcc = ld.trajData.Acceleration;trajAngVel = ld.trajData.AngularVelocity;Accel = ld.accel;陀螺= ld.陀螺;Mag = ld.mag; lla = ld.lla; gpsvel = ld.gpsvel;

融合滤波器

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

这insfilterAsync有几种方法来处理传感器数据:fuseaccel，fusegyro，fusemag和fusegps。因为insfilterAsync使用连续离散EKF预测方法可以将过滤器向前步进任意时间。

fusionfilt = infilterasync (“ReferenceLocation”, refloc);

的状态向量初始化`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用于帮助初始化滤波器状态，因此滤波器很快收敛到好的答案。

Nav = 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。State = initstate;

设置进程噪声值`insfilterAsync`

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

fusionfilt。QuaternionNoise = 1e-2;fusionfilt。AngularVelocityNoise = 100;fusionfilt。AccelerationNoise = 100;fusionfilt。偏置噪声= 1e-7;fusionfilt。AccelerometerBiasNoise = 1e-7; fusionfilt.GyroscopeBiasNoise = 1e-7;

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

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

Rmag = 0.4;Rvel = 0.01;Racc = 610;Rgyro = 0.76e-5;Rpos = 3.4;fusionfilt。StateCovariance = diag(1e-3*ones(28,1));

初始化范围

的HelperScrollingPlotterScope允许绘制随时间变化的变量。它在这里用于跟踪姿势错误。的PoseViewerWithSwitches范围允许三维可视化的过滤器估计和地面的真实姿态。瞄准镜会减慢模拟速度。要禁用某个作用域，请将相应的逻辑变量设置为false。

usererrscope = true;%打开流错误图。usePoseView = true;打开3D姿势查看器。如果usePoseView posescope = poseviewwithswitches (…“XPositionLimits”， [- 3030]，…“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运行估计误差的。

主模拟运行在100hz。每次迭代检查图形窗口上的复选框，如果启用了传感器，则以适当的速率融合该传感器的数据。

为2 = 1:尺寸(accel, 1) fusionfilt.predict (1. / Fs);%熔断器加速度计如果(f.UserData.Accelerometer) & &…mod(ii, fix(Fs/f.UserData.AccelerometerSampleRate)) == 0 fusionfilt.fuseaccel(accel(ii，:)， Racc);结束%熔断器陀螺仪如果(f.UserData.Gyroscope) & &…mod(ii, fix(Fs/f.UserData.GyroscopeSampleRate)) == 0 fusionfilt.fusegyro(gyro(ii，:)， Rgyro);结束%熔断器磁强计如果(f.UserData.Magnetometer) & &…mod(ii, fix(Fs/ f.r userdata . magnetometersamplerate)) == 0 fusionfilt.fusemag(mag(ii，:)， Rmag);结束%熔断器GPS如果(f.UserData.GPS) && mod(ii, fix(f /f.UserData.GPSSampleRate)) == 0 fusionfilt.fusegps(lla(ii，:)， Rpos, gpsvel(ii，:)， Rvel);结束绘制姿态误差[p,q] = pose(fusionfilt);posescope(p, q, trajPos(ii，:)， trajOrient(ii));orientErr = rad2deg(dist(q, trajOrient(ii)));posErr = p - trajPos(ii，:);errscope(orientErr, posErr(1)， posErr(2)， posErr(3));结束

结论

的insfilterAsync允许各种不同的采样率。估计输出的质量在很大程度上取决于单个传感器的融合率。任何传感器掉线都会对输出产生深远的影响。