基于异步传感器的姿态估计

打开脚本

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

仿真设置

加载预先记录的传感器数据。传感器数据基于使用waypointTrajectory类。方法创建传感器值gpsSensor而且imuSensor类。的CircularTrajectorySensorData.mat方法生成此处使用的文件generateCircularTrajSensorData函数。

Ld =负载(“CircularTrajectorySensorData.mat”）;Fs = ld.Fs;%最大MARG速率gpsFs = ld.gpsFs;最大GPS速率%比率= 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;陀螺;Mag = ld.mag; lla = ld.lla; gpsvel = ld.gpsvel;

融合滤波器

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

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

fusionfilt = insfilterAsync(“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(travel (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。状态=初始状态;

的进程噪声值`insfilterAsync`

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

fusionfilt。四元噪声= 1e-2;fusionfilt。AngularVelocityNoise = 100;fusionfilt。加速度噪声= 100;fusionfilt。磁力计偏差噪声= 1e-7;fusionfilt。AccelerometerBiasNoise = 1e-7; fusionfilt.GyroscopeBiasNoise = 1e-7;

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

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

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

初始化范围

的HelperScrollingPlotter作用域允许绘制随时间变化的变量。这里用它来跟踪姿势中的错误。的PoseViewerWithSwitchesscope允许对滤波器估计和地面真实姿态进行3D可视化。瞄准镜会减慢模拟速度。若要禁用作用域，请将相应的逻辑变量设置为false。

usererrscope = true;打开流错误图。usePoseView = true;打开3D姿势查看器。如果usePoseView posescope = PoseViewerWithSwitches(.．.“XPositionLimits”， [-30 30]，.．.“YPositionLimits”， [- 30,30]，.．.“ZPositionLimits”， [-10 10]);结束F = gcf;如果useErrScope errscope = HelperScrollingPlotter(.．.“NumInputs”4.．.“时间间隔”10.．.“SampleRate”Fs,.．.“YLabel”, {“度”，.．.“米”，.．.“米”，.．.“米”}，.．.“标题”, {“四元数距离”，.．.“X位置错误”，.．.“位置Y错误”，.．.'位置Z错误'}，.．.“YLimits”，.．.[- 1,30 - 2,2 - 2,2 -2]);结束

模拟循环

融合算法的模拟允许您检查不同传感器采样率的影响。此外，可以通过取消选中相应的复选框来防止单个传感器的融合。这可以用来模拟传感器掉线。

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

打开所有传感器，但将它们设置为最低速率运行，会产生明显偏离地面真实值的估计，然后当传感器融合时，会恢复到更正确的结果。这最容易在HelperScrollingPlotter运行估计误差。

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

为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(陀螺(ii，:)， Rgyro);结束%保险丝磁强计如果(f.UserData.Magnetometer) & &.．.mod(ii, fix(Fs/f.UserData.MagnetometerSampleRate)) == 0 fusionfilt.fusemag(mag(ii，:)， Rmag);结束%保险丝GPS如果(f.UserData.GPS) && mod(ii, fix(Fs/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 - trjpos (ii，:);errscope(orientErr, posErr(1)， posErr(2)， posErr(3));结束