使用合成数据的视觉惯性内径测量
这个例子展示了如何使用惯性测量单元(IMU)和单目摄像机估计地面车辆的姿态(位置和方向)。在这个例子中，你:
创建包含车辆的地面真理轨迹的驾驶场景。
使用IMU和可视化里程测量模型来生成测量值。
融合这些测量来估计车辆的姿态，然后显示结果。
视觉惯性里程计通过融合来自单目摄像机的视觉里程计姿势估计和来自IMU的姿势估计来估计姿势。IMU在较小的时间间隔内返回准确的姿势估计，但由于集成惯性传感器测量值，会出现较大的漂移。单目相机在较大的时间间隔内返回准确的姿势估计，但存在比例模糊。鉴于这些互补的优势和劣势，使用视觉惯性里程计融合这些传感器是一个合适的选择。该方法可用于GPS读数不可用的场景，例如在城市峡谷中。
创建具有轨迹的驾驶场景
创建一个驾驶场景（自动驾驶工具箱）包含：
车辆的道路旅行
路两边的建筑物
车辆的地面姿态
车辆的估计姿态
车辆的地面真实姿态显示为实心蓝色长方体。估计的姿势显示为透明的蓝色长方体。请注意，估计的姿势不会出现在初始可视化中，因为地面真实值和估计的姿势重叠。
生成用于地面车辆的基线轨迹使用waypointTrajectorySystem Object™。请注意waypointTrajectory是用来代替驾驶场景/轨迹因为车辆需要加速。使用一组航路点、到达时间和速度，以指定的采样率生成轨迹。
％与地面真相创建了驾驶场景并估计%车辆构成。现场= drivingScenario;groundTruthVehicle =车辆(场景,“绘图颜色”，[0 0.4470 0.7410]）;Estvehicle =车辆（场景，“绘图颜色”，[0 0.4470 0.7410]）;％生成基线轨迹。Samplere = 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);添加一条路和建筑到场景和可视化。helperPopulateScene(场景,groundTruthVehicle);
创建融合过滤器
创建过滤器以融合IMU和视觉里程测量。本例使用松耦合方法融合测量值。虽然结果不如紧密耦合方法准确，但所需的处理量明显较少，结果足够。融合滤波器使用误差状态卡尔曼滤波器跟踪方向（作为四元数）、位置、速度和传感器偏差。
这insfiltererrorstate.对象具有以下处理传感器数据的功能：预测和福塞姆沃。
这预测该功能将来自IMU的加速计和陀螺仪测量值作为输入。打电话给预测每次采样加速度计和陀螺仪时功能。该功能通过基于加速度计和陀螺仪测量的一次时间步骤预测状态，并更新过滤器的误差状态协方差。
这福塞姆沃函数将视觉里程计姿势估计值作为输入。该函数通过计算卡尔曼增益，根据各种输入的不确定性对其进行加权，从而根据视觉里程计姿势估计更新误差状态。就像预测函数，该函数还更新了误差状态协方差，这次考虑了卡尔曼增益。然后使用新的错误状态更新状态，并重置错误状态。
filt = insfiltererrorstate（“IMUSampleRate”，采样器，......'参考范围'那“ENU表示”）设置初始状态和错误状态的协方差。帮助初始化（过滤器、traj）；
filt=INSFILTERRORSTATE with properties:IMU采样率：100 Hz参考位置：[0 0][deg deg m]状态：[17x1双]状态协方差：[16x16双]过程噪声方差陀螺仪噪声：[1e-06 1e-06 1e-06]（rad/s）²加速度计噪声：[0.0001 0.0001 0.0001]（m/s²）陀螺偏差：[1e 1e-09 1e-09]（rad/s）²加速度计噪音：[0.0001 0.0001 0.0001]（m/s²）²
指定可视里程计模型
定义可视化内径型号参数。这些参数使用单眼相机模拟了基于特征匹配和基于跟踪的视觉径管系统。这规模参数占单眼相机的后续视觉框架的未知比例。另一个参数模拟视觉径管读数的漂移作为白噪声的组合和一阶高斯-Markov过程。
％标志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 = imussensor (“采样器”，采样器，'参考范围'那“ENU表示”）;%加速计imu.Accelerometer.MeasurementRange = 19.6;%m/s^2imu.Accelerometer.Resolution = 0.0024;％m / s ^ 2 / lsbIMU.Accelerometer。不生系= 0.01;% (m / s ^ 2) /√(赫兹)%陀螺仪imu.gyroscope.measurementrange = deg2rad（250）;% rad /秒imu.Gyroscope.Resolution函数= (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 = ceil（numimusamples / imusamplespercamera）;%为打印结果预先分配数据数组。方位，方向，方向，角度，......posVO，orientVO，......posEst，orientEst，velEst]......= helperPreallocateData (numIMUSamples numCameraSamples);%设置视觉里程计融合的测量噪声参数。rposvo = 0.1;rorientvo = 0.1;
运行模拟循环
以IMU采样率运行模拟。每个IMU样本用于通过一次执行滤波器的状态。一旦提供了新的视觉内径读数，它用于校正当前的滤波器状态。
滤波器估计中存在一些偏差，可以通过GPS等附加传感器或道路边界图等附加约束进一步修正。
cameraIdx=1；对于i=1:numu样本%生成地面真实轨迹值。[pos（i，：）、orient（i，：）、vel（i，：）、acc（i，：）、angvel（i，：）]=traj（）；生成加速度计和陀螺仪测量从地面真实%值轨迹。[accelMeas，gyroMeas]=imu（acc（i，：）、angvel（i，：）、orient（i））；预测过滤器状态转发一个时间步%加速度计和陀螺仪测量。预测(filt accelMeas gyroMeas);如果（1 == mod（i，imusamplespercamera））&& duredvo％从地面真理生成视觉径管姿势估计%值和可视化里程计模型。[POSVO（Cameraidx，:)，Orientvo（Cameraidx，:)，paramsvo] =......helperVisualOdometryModel (pos(我:),东方(我,:),paramsVO);基于视觉测量数据的％正确的滤波器状态。fusemvo (filt posVO (cameraIdx:), RposVO,......orientVO（cameraIdx），RorientVO；cameraIdx=cameraIdx+1；终止[posEst（i，：）、定向测试（i，：）、velEst（i，：）]=pose（filt）；更新估计的车辆姿态。HelperupdatePose（Estvehicle，Posest（我，:)，Velest（I，:)，Sizeest（I））;％更新地面真相车辆姿势。HelperupdatePose（Tounttruthvehicle，POS（我，:)，Vel（I，:)，东方（I））;%更新驾驶场景可视化。更新图（场景）；刷新屏幕限制;终止
绘制结果
绘制地面真实车辆轨迹，视觉测程估计，融合滤波估计。
图形如果USEVO PLOT3（POS（：，1），POS（:,2），POS（:,3），'-.'那......POSVO（：，1），POSVO（:,2），POSVO（：3），......pos (: 1), pos (:, 2), pos (:, 3),......“线宽”，3）传奇（“地面实况”那'视觉odometry（vo）'那......“视觉惯性里程计（VIO）”那“位置”那'东北'）其他的图3（位置（：，1），位置（：，2），位置（：，3），'-.'那......pos (: 1), pos (:, 2), pos (:, 3),......“线宽”，3）传奇（“地面实况”那“IMU姿态估计”）终止视图(-90、90)标题(车辆位置的)包含(“X (m)”) ylabel (“Y (m)”网格)在
从图中可以看出，目视测程法估计弹道的形状是比较准确的。IMU和视觉里程计测量的融合消除了视觉里程计测量的尺度不确定性和IMU测量的漂移。
金宝app支持功能
HelperVisuaLodMethodModel
从地面真实输入和参数结构计算视觉里程测量。为了对单目相机后续帧间缩放的不确定性进行建模，将一个常数缩放因子结合随机漂移应用于地面真值位置。
函数[Posvo，Orientvo，Paramsvo]......= helpervisualdometrymodel (pos, orient, paramsVO)％提取模型参数。scalevo = paramsvo.scale;sigman = 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;(1) = rman (1, 2);paramsVO。driftBias = b;%计算视觉里程计测量值。posvo = scalevo * pos +漂移;Orientvo =东方;终止
帮助初始化
设置融合滤波器的初始状态和协方差值。
函数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（END）= 2E2;终止
helperPreallocateData
预先利用数据以日志模拟结果。
函数方位，方向，方向，角度，......posVO，orientVO，......posEst，orientEst，velEst]......= helperPreallocateData (numIMUSamples numCameraSamples)%指定地面真理。POS =零（Numimusamples，3）;东方= Quaternion.zeros（Numimusamples，1）;vel = zeros（Numimusamples，3）;ACC =零（Numimusamples，3）;Angvel =零（Numimusamples，3）;%目视里程输出。posvo =零（numcamerasamples，3）;Orientvo = Quaternion.zeros（NumCamerasamples，1）;％过滤器输出。posEst = 0 (numimussamples, 3);东方=四元数。0 (numIMUSamples, 1);velEst = 0 (numimussamples, 3);终止
恳求
更新车辆的姿势。
函数helperUpdatePose(veh, pos, vel, orient)位置= pos;阿明费。速度=韦尔;rpy = eulerd(东方,“ZYX”那“框架”）;阿明费。偏航= rpy (1);阿明费。距= rpy (2);阿明费。滚= rpy (3);终止
参考文献
SOLA，J.“误差状态卡尔曼滤波器的”四元管运动学“。ARXIV E-PRINTS，ARXIV：1711.02508V1 [CS.RO] 2017年11月3日。
蒋瑞玲，R. Klette，和S. Wang。“视觉测程中无边界远程漂移的建模”。2010年第四届环太平洋地区图像和视频技术研讨会。2010年11月，页121-126。
你也可以从以下列表中选择一个网站:
