视觉惯性测程使用合成数据
这个例子演示了如何使用惯性测量单元(IMU)和单目摄像机估计地面车辆的姿态(位置和方向)。在这个例子中，你:
创建一个包含车辆的地面真实轨迹的驾驶场景。
使用IMU和可视化里程测量模型来生成测量结果。
融合这些测量来估计车辆的姿态，然后显示结果。
视觉-惯性测程法将单目相机的视觉测程法与IMU的视觉测程法结合起来进行姿态估计。IMU在小的时间间隔内返回一个精确的位姿估计，但是由于集成了惯性传感器的测量结果而导致较大的漂移。单目相机在较大的时间间隔内返回准确的姿态估计，但存在尺度模糊的问题。鉴于这些互补的优势和弱点，这些传感器的融合使用视觉惯性测程是一个合适的选择。这种方法可用于无法获得GPS读数的情况，例如在城市峡谷中。
创建一个带有轨迹的驾驶场景
创建一个drivingScenario对象，它包含：
在车辆行驶的道路上
道路两旁的建筑物
地面真实的姿态的车辆
估计的车辆姿态
地面真实姿态的车辆显示为一个固体蓝色长方体。估计的姿态显示为一个透明的蓝色长方体。注意，估计的姿态不会出现在最初的可视化中，因为ground truth和估计的姿态有重叠。
生成使用地面车辆的基准轨迹waypointTrajectory系统对象™。注意，waypointTrajectory是用来代替drivingScenario /轨迹因为车辆需要加速。轨迹是使用一组路径点、到达时间和速度以指定的采样率生成的。
％既创建驾驶情形的地面实况和估计%车辆构成。现场= drivingScenario;groundTruthVehicle =车辆(场景,“PlotColor”，[0 0.4470 0.7410]）;estVehicle =车辆（场景，“PlotColor”，[0 0.4470 0.7410]）;％生成基准轨迹。采样率= 100;航点= [0 0 0;200 0 0;200 50 0;200 230 0;215 245 0;260 245 0;290 240 0;310 258 0;290 275 0; 260 260 0; -20 260 0]; t = [0 20 25 44 46 50 54 56 59 63 90].'; speed = 10; velocities = [ speed 0 0; speed 0 0; 0 speed 0; 0 speed 0; speed 0 0; speed 0 0; speed 0 0; 0 speed 0; -speed 0 0; -speed 0 0; -speed 0 0]; traj = waypointTrajectory(wayPoints,'到达时间't...“速度对”，速度，“SampleRate”,sampleRate);在场景中添加道路和建筑物。helperPopulateScene(场景,groundTruthVehicle);
创建一个融合过滤器
创建过滤器，以融合IMU和视觉里程测量。本例使用松散耦合的方法来融合测量结果。虽然结果不像紧密耦合方法那样精确，但所需的处理量要少得多，结果也足够了。融合滤波器使用误差状态卡尔曼滤波器来跟踪方向(作为四元数)、位置、速度和传感器偏差。
该insfilterErrorState对象具有以下处理传感器数据的功能:预测和fusemvo。
该预测函数采用加速度计和陀螺仪测量从IMU作为输入。调用预测每个时间函数的加速度计和陀螺仪进行采样。此功能预测基于加速度计和陀螺仪的测量通过一次一步的状态，并更新过滤器的错误状态协方差。
该fusemvo函数以视觉测程姿态估计值为输入。该函数通过计算卡尔曼增益来更新基于视觉测程姿态估计的误差状态，卡尔曼增益根据输入的不确定性对输入进行加权。与预测函数，该函数也更新错误状态协方差，这次考虑了卡尔曼增益。然后使用新的错误状态更新状态，并重置错误状态。
FILT = insfilterErrorState（“IMUSampleRate”sampleRate,...'参考范围'，“ENU表示”）设置初始状态和错误状态协方差。helperInitialize (filt traj);
filt = insfilterErrorState属性:IMUSampleRate: 100 Hz ReferenceLocation:[0 0 0][度度m]状态:[17 x1双]StateCovariance: [16 x16双]过程噪声方差GyroscopeNoise: [1 e-06 e-06 1 e-06] (rad / s)²AccelerometerNoise: [0.0001 0.0001 0.0001] (m / s²)²GyroscopeBiasNoise: [1 e-09 e-09 1 e-09] (rad / s)²AccelerometerBiasNoise: [0.0001 0.0001 0.0001] (m / s²)²
指定可视里程测量模型
定义视觉里程模型参数。这些参数使用单眼相机模型基于跟踪特征匹配和视觉里程计系统。该规模参数占单眼相机的后续视觉帧的未知规模。其它参数建模在视觉测距读数为白噪声和一阶高斯 - 马尔可夫过程的组合的漂移。
％标志useVO确定如果使用视觉里程计：% useVO = false;只使用IMU。useVO = true;使用IMU和目视测程法。paramsVO.scale = 2;paramsVO.sigmaN = 0.139;paramsVO.tau = 232;paramsVO.sigmaB = SQRT（1.34）;paramsVO.driftBias = [0 0 0];
指定IMU传感器
定义包含加速度计的IMU传感器模型以及使用陀螺仪imuSensor系统对象。该传感器模型包含对确定性和随机性噪声源进行建模的特性。这里设置的属性值是低成本MEMS传感器的典型值。
％设置RNG种子默认为获得后续相同的结果％运行。rng (“默认”) imu = imuSensor(“SampleRate”sampleRate,'参考范围'，“ENU表示”);%加速度计imu.Accelerometer。MeasurementRange = 19.6;% m / s ^ 2imu.Accelerometer。分辨率= 0.0024;％米/秒^ 2 / LSBimu.Accelerometer.NoiseDensity = 0.01;% (m / s ^ 2) /√(赫兹)%陀螺仪imu.Gyroscope.MeasurementRange = deg2rad（250）;% rad /秒imu.Gyroscope。解析函数= (0.0625);% rad / s / LSBimu.Gyroscope。NoiseDensity函数= (0.0573);% (rad / s) /√(赫兹)imu.Gyroscope.ConstantBias = deg2rad（2）;% rad /秒
设置仿真
指定的时间运行模拟和初始化是在模拟循环过程中记录变量的数量。
%运行模拟60秒。numSecondsToSimulate = 60;numIMUSamples = numSecondsToSimulate *采样;％定义视觉里程计的采样率。imuSamplesPerCamera = 4;numCameraSamples =小区（numIMUSamples / imuSamplesPerCamera）;为绘制结果预先分配数据数组。[pos, orient, vel, acc, angvel，...posVO orientVO,...姿势,东方,或者]...= helperPreallocateData (numIMUSamples numCameraSamples);设置视觉测程融合的测量噪声参数。RposVO = 0.1;RorientVO = 0.1;
运行模拟循环
运行在IMU采样率的模拟。每个样品IMU是用来通过一个时间步前向预测滤波器的状态。一旦有新的视觉里程读数是可用的，它是用来纠正当前过滤器的状态。
滤波器估计值中存在一些漂移，可以通过附加的传感器(如GPS)或附加的约束(如道路边界地图)来进一步修正。
cameraIdx = 1;为我= 1:numIMUSamples%生成地面真值轨迹值。(pos(我:),东方(我:),或者(我,:),acc(我,:),angvel(我,:)]= traj ();产生加速度计和陀螺仪测量从地面真理%值轨迹。[accelMeas, gyroMeas] = imu (acc(我,:),angvel(我:),东方(我));的基础上，预测滤波状态向前一个时间步长加速度计和陀螺仪测量。预测(filt accelMeas gyroMeas);如果（1个== MOD（I，imuSamplesPerCamera））&& useVO％生成从地面真理视觉里程姿态估计%值和视觉测程模型。[posVO（cameraIdx，:)，orientVO（cameraIdx，:)，paramsVO] =...helperVisualOdometryModel (pos(我:),东方(我,:),paramsVO);基于视觉里程计数据正确％滤波器状态。fusemvo (filt posVO (cameraIdx:), RposVO,...orientVO (cameraIdx) RorientVO);cameraIdx = cameraIdx + 1;结束(pos(我:),东方(我:),或者(我,:)]=姿势(filt);更新估计的车辆姿态。helperUpdatePose（estVehicle，posEst（I，:)，velEst（I，:)，orientEst（I））;％更新地面实况车辆姿态。helperUpdatePose（groundTruthVehicle，POS（i，:)，VEL（I，:)，东方（I））;%更新驾驶场景可视化。updatePlots(现场);drawnowlimitrate;结束
阴谋的结果
绘制地面真值车辆轨迹，视觉测程估计，融合滤波估计。
数字如果useVO plot3（POS（：，1），POS（：，2），POS（：，3），“-”。，...posVO（：，1），posVO（：，2），posVO（：，3），...pos (: 1), pos (:, 2), pos (:, 3),...“线宽”，3）图例（“地面实况”，'视觉测程（VO）'，...“Visual-Inertial测程法(VIO) '，“位置”，'东北'）其他的plot3 (pos (: 1), pos (:, 2), pos (:, 3),“-”。，...pos (: 1), pos (:, 2), pos (:, 3),...“线宽”，3）图例（“地面实况”，IMU姿态估计的）结束视图(-90、90)标题(车辆位置的)包含(“X (m)”)ylabel (“Y (m)”网格)在
从图中可以看出，视觉测程法对轨迹形状的估计是比较准确的。IMU和目视测程测量的融合消除了目视测程测量的标度因子不确定性和IMU测量的漂移。
万博1manbetx支持功能
helperVisualOdometryModel
根据地面真值输入和参数结构计算视觉测程。为了模拟单目相机后续帧之间缩放的不确定性，我们对地面真值位置应用了一个恒定的缩放因子和一个随机漂移。
函数[posVO，orientVO，paramsVO]...= helperVisualOdometryModel(pos, orient, paramsVO)％提取模型参数。scaleVO = paramsVO.scale;西格曼= paramsVO.sigmaN;tau蛋白= paramsVO.tau;sigmaB = paramsVO.sigmaB;sigmaA = SQRT（（2 / TAU）+ 1 /（TAU * tau蛋白））* sigmaB;B = paramsVO.driftBias;%计算漂移。b = (1 - 1/)* b + randn (1、3) * sigmaA;漂移= randn(1,3)*sigmaN + b;paramsVO。driftBias = b;计算目视里程测量值。posVO = scaleVO * POS +漂移;orientVO =东方;结束
helperInitialize
设置融合滤波器的初始状态和协方差值。
函数helperInitialize（FILT，TRAJ）属性获取初始位置、方向和速度％轨迹对象和复位的内部状态。[pos, orient, vel] = traj();重置(traj);％设定的初始状态的值。filt.State(1:4) =紧凑(东方(1))。”;filt.State (7) = pos (1:)。”;filt.State(8) =韦尔(1:)。”;%设置陀螺仪偏差和视觉测程因子协方差较大的值对应于较低的置信度。filt.StateCovariance（10：12,10：12）= 1E6;filt.StateCovariance（结束）= 2E2;结束
helperPreallocateData
预分配数据记录的模拟结果。
函数[pos, orient, vel, acc, angvel，...posVO orientVO,...姿势,东方,或者]...= helperPreallocateData (numIMUSamples numCameraSamples)指定地面真相。POS =零（numIMUSamples，3）;定向= quaternion.zeros（numIMUSamples，1）;VEL =零（numIMUSamples，3）;ACC =零（numIMUSamples，3）;angvel =零（numIMUSamples，3）;%视觉里程测量输出。posVO =零（numCameraSamples，3）;orientVO = quaternion.zeros（numCameraSamples，1）;％滤波器的输出。posEst = 0 (numIMUSamples, 3);东方=四元数。0 (numIMUSamples, 1);velEst = 0 (numIMUSamples, 3);结束
helperUpdatePose
更新车辆的姿态。
函数帮助更新(veh, pos, vel, orient)位置= pos;阿明费。速度=韦尔;rpy = eulerd(东方,“ZYX股票”，“帧”);阿明费。偏航= rpy (1);阿明费。距= rpy (2);阿明费。滚= rpy (3);结束
参考文献
索拉，J。“四元数运动学的错误状态卡尔曼滤波器。”的arXiv电子印花图案，的arXiv：1711.02508v1 [cs.RO] 2017年11月3日。
R. Jiang, R.， R. Klette, S. Wang。无界长程漂移的视觉测程建模。2010 Fourth Pacific-Rim Symposium on Image and Video Technology. Nov. 2010, pp. 121-126.
你也可以从以下列表中选择一个网站:
