从异步传感器姿态估计

打开脚本

这个例子展示了如何融合传感器以不同的速率估计姿势。加速度计、陀螺仪、磁力仪和GPS用于确定方向和位置的车沿着一个环形的道路。您可以使用控制图窗口改变传感器利率和试验传感器辍学而影响估计的姿势。

仿真设置

加载预先录制的传感器数据。传感器数据是基于使用创建的圆形轨迹waypointTrajectory类。创建使用的传感器值gpsSensor和imuSensor类。的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.AngularVelocity;accel = ld.accel;陀螺= ld.gyro;杂志= ld.mag; lla = ld.lla; gpsvel = ld.gpsvel;

融合滤波器

创建一个insfilterAsync保险丝IMU + GPS测量。这种融合滤波器使用continuous-discrete扩展卡尔曼滤波器(EKF)跟踪定位(作为一个四元数),角速度,位置,速度,加速度,传感器偏差,地磁矢量。

这insfilterAsync有几种方法来处理传感器数据:fuseaccel,fusegyro,fusemag和fusegps。因为insfilterAsync使用一个continuous-discrete卡尔曼滤波器,预测方法可以过滤向前一个任意的时间步。

fusionfilt = insfilterAsync (“ReferenceLocation”,refloc);

初始化的状态向量`insfilterAsync`

的insfilterAsync跟踪构成州28-element向量。美国是:

国家单位指数取向(四元数部分)1:4角速度(某某)rad / s 5:7位置(NED) m 8:10分速度(NED) m / s 11:13加速度(NED) m / s ^ 2十四16加速度计偏差(某某)m / s ^ 2十七19陀螺仪偏见(某某)rad / s 20:22地磁场矢量(NED) uT 23:25磁强计偏差(某某)圣经uT 26:28

地面真理是用于初始化滤波器,滤波器收敛迅速好的答案。

资产净值= 100;initstate = 0(28日1);initstate(1:4) =紧凑(meanrot (trajOrient (1: Nav)));initstate(7) =意味着(trajAngVel(10:导航:),1);initstate(8) =意味着(trajPos(1:导航,:),1);initstate(11:13) =意味着(trajVel(1:导航,:),1);initstate(十四16)=意味着(trajAcc(1:导航,:),1);initstate (23:25) = ld.magField;%的陀螺仪偏见初始值估计低z轴。这是%完成说明融合的磁力仪的影响%的模拟。initstate函数(20:22)= (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允许范围的3 d可视化滤波器估计和地面真理构成。范围可以缓慢的模拟。禁用一个范围,相应的逻辑变量设置为false。

useErrScope = true;%打开流错误情节。usePoseView = true;%的3 d查看器。如果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 2]);结束

模拟循环

融合算法的仿真允许您检查不同传感器采样率的影响。此外,融合个人传感器可以阻止取消勾选对应的复选框。这可以用来模拟传感器辍学。

某些配置产生戏剧性的结果。例如,关闭GPS传感器使位置估计迅速漂移。关掉磁力计传感器将导致方位估计慢慢偏离地面真理作为估计旋转太快。相反,如果陀螺仪是关闭和磁强计,估计定位显示摆动和缺乏平滑礼物如果两个传感器使用。

将所有传感器但设置运行产生最低的速度估计明显偏离地面实况,然后重新摆向当传感器融合更多的正确的结果。这是最容易看到的HelperScrollingPlotter正在运行的估计错误。

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

为2 = 1:尺寸(accel, 1) fusionfilt.predict (1. / Fs);%融合加速度计如果(f.UserData.Accelerometer) & &…国防部(ii,修复(Fs / f.UserData.AccelerometerSampleRate)) = = 0 fusionfilt.fuseaccel (accel (ii):), Racc);结束%保险丝陀螺仪如果(f.UserData.Gyroscope) & &…国防部(ii,修复(Fs / f.UserData.GyroscopeSampleRate)) = = 0 fusionfilt.fusegyro(陀螺(ii):), Rgyro);结束%引信磁强计如果(f.UserData.Magnetometer) & &…国防部(ii,修复(Fs / f.UserData.MagnetometerSampleRate)) = = 0 fusionfilt.fusemag (mag (ii):), Rmag);结束%融合GPS如果(f.UserData.GPS) & &国防部(ii,修复(Fs / f.UserData.GPSSampleRate)) = = 0 fusionfilt.fusegps (lla (ii):), rpo, gpsvel (ii):), Rvel);结束%的阴谋造成的错误(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));结束

结论

的insfilterAsync允许不同和不同的采样率。估计输出的质量在很大程度上依赖于单个传感器融合率。任何传感器辍学将在输出产生深远的影响。