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

打开脚本

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

仿真设置

加载预记录的传感器数据。传感器数据是基于圆形轨迹的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;trajVel = ld.trajData.Velocity;trajPos = ld.trajData.Position;trajAcc = ld.trajData.Acceleration;trajAngVel = ld.trajData.AngularVelocity;accel = ld.accel;陀螺= ld.gyro;杂志= 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被用来帮助初始化过滤器的状态，所以过滤器会很快收敛到好的答案。

资产净值= 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轴陀螺仪偏差初值估计很低。这是来说明磁力计熔合的效果%的模拟。初始状态(20:22)= deg2rad([3.125 3.125 3.125]);fusionfilt。状态= initstate;

设置进程噪声值`insfilterAsync`

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

fusionfilt。QuaternionNoise = 1飞行;fusionfilt。AngularVelocityNoise = 100;fusionfilt。AccelerationNoise = 100;fusionfilt。MagnetometerBiasNoise = 1 e;fusionfilt。AccelerometerBiasNoise = 1 e; fusionfilt.GyroscopeBiasNoise = 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。

useErrScope = true;%打开流错误图。usePoseView = true;打开3D姿势查看器。如果使用posescope = posviewerwithswitches (．..“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]);结束

模拟循环

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

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

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

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

为2 = 1:尺寸(accel, 1) fusionfilt.predict (1. / Fs);%融合加速度计如果(f.UserData.Accelerometer) & &．..mod(ii, fix(Fs/f.UserData.AccelerometerSampleRate)) == 0结束%保险丝陀螺仪如果(f.UserData.Gyroscope) & &．..mod(ii, fix(Fs/f.UserData.GyroscopeSampleRate)) == 0结束%引信磁强计如果(f.UserData.Magnetometer) & &．..mod(ii, fix(Fs/f.UserData.MagnetometerSampleRate)) == 0结束%融合GPS如果(f.UserData.GPS) && mod(ii, fix(Fs/f.UserData.GPSSampleRate)) == 0结束绘制位姿误差(p, q) =姿势(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));结束