姿态估计从异步传感器

开放脚本

此示例显示如何以不同速率融合传感器以估计姿势。加速度计、陀螺仪、磁强计和GPS用于确定沿圆形路径行驶的车辆的方向和位置。您可以使用“体形”窗口上的控件来改变传感器速率，并在查看对估计姿势的影响时尝试传感器丢失。

模拟装置

加载预先记录的传感器数据。传感器数据基于使用航路点轨迹班级。使用创建的传感器值GPS传感器和免疫传感器课程。这CircularTrajectorySensorData.mat此处使用的文件可以使用generateCircularTrajSensorData功能。

LD =负载（“CircularTrajectorySensorData.mat”）;FS = ld.Fs;%最高保证金率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.angularvocity；accel=ld.accel；gyro=ld.gyro；mag=ld.mag；lla=ld.lla；gpsvel=ld.gpsvel；

融合滤波器

创建InFilterAsync将IMU+GPS测量值进行融合。该融合滤波器使用连续离散扩展卡尔曼滤波器（EKF）跟踪方向（作为四元数）、角速度、位置、速度、加速度、传感器偏差和地磁矢量。

这个InFilterAsync有几种处理传感器数据的方法：fuseaccel那fusegyro那熔丝标记和fusegps。因为InFilterAsync使用连续离散EKF预测方法可以使过滤器向前移动任意时间量。

fusionfilt = insfilterAsync（'ReferenceLocation'，refloc）；

初始化的状态向量`InFilterAsync`

这InFilterAsync跟踪28个元素向量中的姿势状态。状态为：

国家单位指数方向（四元数部分）1:4角速度（XYZ）rad/s 5:7位置（NED）m8: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

地面真相是用来帮助初始化过滤器的状态，所以过滤器收敛到很好的答案很快。

NAV = 100;INITSTATE =零（28,1）;INITSTATE（1：4）=紧凑（meanrot（trajOrient（1：NAV）））;INITSTATE（5：7）=平均值（trajAngVel（10：导航，:)，1）;INITSTATE（8:10）=平均值（trajPos（1：导航，:)，1）;INITSTATE（11:13）=平均值（trajVel（1：导航，:)，1）;INITSTATE（14点16）=平均值（trajAcc（1：导航，:)，1）;INITSTATE（23点25分）= ld.magField;%Z轴的陀螺仪偏差初始值估计值较低。这是％做以说明在融合磁力的作用%模拟。INITSTATE（20时22分）= deg2rad（[3.125 3.125 3.125]）;fusionfilt.State = INITSTATE;

设置的过程噪声值`InFilterAsync`

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

fusionfilt.QuaternionNoise=1e-2；fusionfilt.AngularVelocityNoise=100；fusionfilt.AccelerationNoise=100；fusionfilt.MagnetometerBiasNoise=1e-7；fusionfilt.AccelerometerBiasNoise=1e-7；fusionfilt.陀螺仪biasNoise=1e-7；

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

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

RMAG = 0.4;Rvel = 0.01;RACC = 610;Rgyro = 0.76e-5;RPOS = 3.4;fusionfilt.StateCovariance = DIAG（1E-3 *酮（28,1））;

初始化作用域

这HelperScrollingPlotter作用域允许绘制随时间变化的变量。此处使用作用域跟踪姿势中的错误PoseViewWithSwitch范围允许过滤估计和地面实况姿势的三维可视化。该范围可以减缓模拟。要禁用范围，设置为false对应的逻辑变量。

UseerScope=true；%打开流错误绘图。usePoseView = TRUE;％开启3D姿势观众。如果usePoseView posescope = PoseViewerWithSwitches（......'XPositionLimits'[-30 30]......“YPositionLimits”, [-30, 30],......"限制", [-10 10]);结尾f=gcf；如果useErrScope errscope = HelperScrollingPlotter（......“努明普茨”4，......'时间跨度', 10,......'采样率'，FS，......“伊拉贝尔”, {“度”那......“米”那......“米”那......“米”},......“头衔”, {“四元数距离”那......“位置X错误”那......“位置Y错误”那......“位置Z错误”},......'ylimits'那......[-1，30 -2，2 -2 2 -2 2]）;结尾

模拟回路

融合算法的仿真允许你检查不同的传感器采样率的影响。另外，各个传感器的融合可以通过取消相应的复选框来防止。这可以被用来模拟传感器漏失。

一些配置产生显着的成果。例如，关闭GPS传感器使位置估计快速漂移。关闭磁力计传感器将导致姿势推测慢慢从地面真理估计旋转过快偏离。相反，如果陀螺仪被关闭并且所述磁力计被接通时，所估计的取向示出了摆动并且如果使用两个传感器缺乏本平滑度。

打开所有传感器，但将其设置为以最低速率运行，会产生明显偏离地面真实值的估计值，然后在传感器融合时迅速返回到更正确的结果。这在HelperScrollingPlotter的运行估计错误。

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

为了ii=1：尺寸（加速度，1）熔断预测（1./Fs）；%引信加速度计如果（f.UserData.Accelerometer）&&......MOD（二，修复（FS / f.UserData.AccelerometerSampleRate））== 0 fusionfilt.fuseaccel（加速度（二，:)，RACC）;结尾%引信陀螺仪如果（f.UserData.陀螺仪）&&......mod（ii，fix（Fs/f.UserData.gyloossamplerate））==0 fusionfilt.fusegyro（gyro（ii，：），Rgyro）；结尾%引信磁强计如果（f.UserData.磁强计）&&......MOD（二，修复（FS / f.UserData.MagnetometerSampleRate））== 0 fusionfilt.fusemag（MAG（二，:)，RMAG）;结尾％保险丝GPS如果（f.UserData.GPS）&& MOD（二，修复（FS / f.UserData.GPSSampleRate））== 0 fusionfilt.fusegps（LLA（二，:)，RPOS，gpsvel（二，:)，Rvel）;结尾%绘制姿态误差图[p，q]=姿势（fusionfilt）；posescope（p，q，trajPos（ii，：），trajOrient（ii））；定向器=rad2deg（距离（q，距离（ii））；posErr=p-trajPos（ii，：）；errscope（定向器、posErr（1）、posErr（2）、posErr（3））；结尾