惯性导航的IMU和GPS融合
这个例子展示了如何构建适合于无人机或四轴飞行器的IMU + GPS融合算法。
本例使用加速度计、陀螺仪、磁力计和GPS来确定无人机的方向和位置。
仿真设置
设置采样率。在一个典型的系统中，加速度计和陀螺仪以相对较高的采样率运行。融合算法处理这些传感器数据的复杂度相对较低。相反，GPS，在某些情况下，磁力计以相对较低的采样率运行，与处理它们相关的复杂性很高。在该融合算法中，磁力计和GPS样本以相同的低速率一起处理，加速度计和陀螺仪样本以相同的高速率一起处理。
为了模拟这种配置，IMU(加速度计、陀螺仪和磁强计)以160 Hz采样，GPS以1 Hz采样。每160个磁力计样本中只有一个被提供给融合算法，因此在实际系统中，磁力计的采样率可以低得多。
imuFs = 160;gpsFs = 1;定义模拟场景发生在地球上的哪个位置%纬度，经度和高度。Refloc = [42.2825 -72.3430 53.0352];%验证|gpsFs|除|imuFs|。这允许传感器采样%的速率，使用嵌套的for循环模拟，没有复杂的采样率%匹配。imuSamplesPerGPS = (imuFs/gpsFs);assert(imuSamplesPerGPS == fix(imuSamplesPerGPS)，.．.GPS采样率必须是IMU采样率的整数因子。）;
融合滤波器
创建过滤器来融合IMU + GPS测量。融合滤波器使用扩展卡尔曼滤波器来跟踪方向(作为四元数)、速度、位置、传感器偏差和地磁矢量。
这insfilterMARG有一些方法来处理传感器数据，包括预测，fusemag而且fusegps．的预测方法以IMU上的加速度计和陀螺仪样本作为输入。调用预测方法每次对加速度计和陀螺仪进行采样。该方法利用加速度计和陀螺仪提前一个时间步预测状态。这里更新了扩展卡尔曼滤波器的误差协方差。
的fusegps方法以GPS样本作为输入。该方法通过计算卡尔曼增益来更新基于GPS样本的滤波器状态，该卡尔曼增益根据各种传感器输入的不确定性进行加权。这里还更新了误差协方差，这次也使用了卡尔曼增益。
的fusemag方法是类似的，但更新的状态，卡尔曼增益，和误差协方差基于磁力计样本。
虽然insfilterMARG以加速度计和陀螺仪样本作为输入，将它们集成起来分别计算三角速度和三角角。滤波器跟踪磁强计的偏置和这些集成信号。
fusionfilt = insfilterMARG;fusionfilt。IMUSampleRate = imuFs;fusionfilt。ReferenceLocation = refloc;
无人机轨迹
本例使用从无人机记录的保存的轨迹作为地面真相。该轨迹被馈送到几个传感器模拟器，以计算模拟加速度计、陀螺仪、磁力计和GPS数据流。
加载“地面真相”无人机轨迹。负载LoggedQuadcopter.mattrajData；trajOrient = trajData.Orientation;trajVel = trajData.Velocity;trajPos = trajData.Position;trajAcc = trajData.Acceleration;trajAngVel = trajData.AngularVelocity;初始化用于模拟传感器的随机数发生器%的噪音。rng (1)
GPS传感器
在指定的采样率和参考位置设置GPS。其他参数控制输出信号中噪声的性质。
gps = gpsSensor(“UpdateRate”, gpsFs);gps。ReferenceLocation = refloc;gps。衰减因子= 0.5;%随机游走噪声参数gps。HorizontalPositionAccuracy = 1.6;gps。VerticalPositionAccuracy = 1.6;gps。速度精度= 0.1;
IMU传感器
通常，无人机使用集成MARG传感器(磁，角速率，重力)进行姿态估计。为了对MARG传感器建模，定义一个包含加速度计、陀螺仪和磁力计的IMU传感器模型。在实际应用中，这三个传感器可以来自一个集成电路，也可以来自不同的集成电路。此处设置的属性值是低成本MEMS传感器的典型值。
imu = imsensor (“accel-gyro-mag”，“SampleRate”, imuFs);imu。磁场= [19.5281 -5.0741 48.0067];%加速度计imu.Accelerometer.MeasurementRange = 19.6133;imu.Accelerometer.Resolution = 0.0023928;imu.Accelerometer.ConstantBias = 0.19;imu.Accelerometer.NoiseDensity = 0.0012356;%陀螺仪imu.Gyroscope.MeasurementRange = deg2rad(250);imu.Gyroscope.Resolution = deg2rad(0.0625);imu.Gyroscope.ConstantBias = deg2rad(3.125);imu.Gyroscope.AxesMisalignment = 1.5;imu. gyroscope . noisendensity = deg2rad(0.025);%磁强计imu.Magnetometer.MeasurementRange = 1000;imu.磁强计。分辨率= 0.1;imu.Magnetometer.ConstantBias = 100;imu. magnetometer . noisendensity = 0.3/√(50);
对象的状态向量初始化insfilterMARG
的insfilterMARG跟踪22元素矢量中的姿态状态。这些状态是:
状态单位状态矢量指数方向为四元数1:4位置(NED) m 5:7速度(NED) m/s 8:10三角角偏置(XYZ) rad 11:13三角速度偏置(XYZ) m/s 14:16地磁场矢量(NED) uT 17:19磁力计偏置(XYZ) uT 20:22
Ground truth用于帮助初始化过滤器状态，因此过滤器可以快速收敛到好的答案。
初始化筛选器的状态Initstate = 0 (22,1);initstate(1:4) = compact(mean (trajOrient(1:100)));initstate(5:7) = mean(trajPos(1:100，:)， 1);initstate(8:10) = mean(travel (1:100，:)， 1);initstate(11:13) = imuFs . gyroscope . constantbias ./imuFs;initstate(14:16) = imuFs . accelerometer . constantbias ./imuFs;initstate(17:19) = imu.磁场;initstate(20:22) = imu.Magnetometer.ConstantBias;fusionfilt。状态=初始状态;
的方差初始化insfilterMARG
的insfilterMARG测量噪声描述了有多少噪声破坏了传感器读数。这些值是基于imuSensor而且gpsSensor参数。
过程噪声描述了滤波方程对状态演化的描述程度。过程噪声通过参数扫描来确定，从而联合优化来自滤波器的位置和方向估计。
%测量噪声Rmag = 0.09;%磁强计测量噪声狂欢= 0.01;% GPS速度测量噪声Rpos = 2.56;% GPS位置测量噪声%进程噪声fusionfilt。加速度计biasnoise = 2e-4;fusionfilt。加速度计噪声= 2;fusionfilt。GyroscopeBiasNoise = 1e-16;fusionfilt。陀螺仪= 1e-5;fusionfilt。磁力计偏差噪声= 1e-10; fusionfilt.GeomagneticVectorNoise = 1e-12;初始误差协方差fusionfilt。StateCovariance = 1e-9*ones(22);
初始化范围
的HelperScrollingPlotter作用域允许绘制随时间变化的变量。这里用它来跟踪姿势中的错误。的HelperPoseViewerscope允许三维可视化的滤波器估计和地面真实姿态。瞄准镜会减慢模拟速度。若要禁用作用域，请将相应的逻辑变量设置为假．
usererrscope = true;打开流错误图usePoseView = true;打开3d姿势查看器。如果useErrScope errscope = HelperScrollingPlotter(.．.“NumInputs”4.．.“时间间隔”10.．.“SampleRate”imuFs,.．.“YLabel”, {“度”，.．.“米”，.．.“米”，.．.“米”}，.．.“标题”, {“四元数距离”，.．.“X位置错误”，.．.“位置Y错误”，.．.'位置Z错误'}，.．.“YLimits”，.．.[- 1,1 - 2,2 - 2,2]);结束如果usePoseView posescope = helpposeviewer (.．.“XPositionLimits”， [-15 15]，.．.“YPositionLimits”， [- 15,15]，.．.“ZPositionLimits”， [-10 10]);结束
模拟循环
主要模拟循环是一个带有嵌套for循环的while循环。while循环执行于gpsFs，为GPS采样率。嵌套的for循环执行于imuFs，为IMU采样率。作用域以IMU采样率更新。
|trajData|有大约142秒的记录数据。secondstosimulation = 50;模拟大约50秒numsamples = secondsToSimulate*imuFs;loopBound = floor(numsamples);loopBound = floor(loopBound/imuFs)*imuFs;确保足够的IMU样本用于最终度量计算的日志数据。pqorient = quaterion .zero (loopBound,1);pqpos = 0 (loopBound,3);FCNT = 1;而(fcnt < = loopBound)% |预测|循环在IMU更新频率。为ff = 1: imuSamplesPerGPS从当前姿态模拟IMU数据。[accel，陀螺，mag] = imu(trajAcc(fcnt，:))， trajAngVel(fcnt，:)，.．.trajOrient (fcnt));使用|predict|方法估计滤波器状态%的模拟加速度计和陀螺仪信号。预测(fusionfilt, accel，陀螺);获取滤波器状态的当前估计。[fusedPos, fusedOrient] = pose(fusionfilt);保存位置和方向以便后期处理。pqorient(fcnt) = fusedOrient;pqpos(fcnt，:) = fusedPos;计算误差并绘制。如果usererrscope orientErr = rad2deg(dist(fusedOrient，.．.trajOrient (fcnt)));posErr = fusedPos - trjpos (fcnt，:);errscope(orientErr, posErr(1)， posErr(2)， posErr(3));结束更新姿态查看器。如果usePoseView posescope(pqpos(fcnt，:)， pqorient(fcnt)， trjpos (fcnt，:)，.．.trajOrient (fcnt:));结束FCNT = FCNT + 1;结束下一步发生在GPS采样率。根据当前姿态模拟GPS输出。[lla, gpsvel] = gps(trjpos (fcnt，:))， trjpos (fcnt，:));根据GPS数据和磁场修正滤波器状态%现场测量。fusegps(fusionfilt, lla, Rpos, gpsvel, Rvel);fusemag(fusionfilt, mag, Rmag);结束
误差度量计算
位置和方向估计被记录在整个模拟过程中。现在计算位置和方向的端到端的均方根误差。
posd = pqpos(1:loopBound，:) - trappos (1:loopBound，:);对于方向，四元数距离是一个更好的选择%减去欧拉角，欧拉角有不连续。四元数的%距离可以用|dist|函数计算，该函数给出%以弧度为单位的方向角差。换算成度数%用于在命令窗口中显示。quatd = rad2deg(dist(pqorient(1:loopBound)， trajOrient(1:loopBound)));在命令窗口显示RMS错误。流(端到端仿真位置RMS误差\n'）;
端到端仿真位置RMS误差
Msep =√(mean(posd.^2));流(“\ tX: %。2f, Y: %。2f, Z: %。2 f(米)\ n \ n”msep (1).．.msep msep (2), (3));
X: 0.57, Y: 0.53, Z: 0.68(米)
流('端到端四元数距离RMS误差(度)\n'）;
端到端四元数距离RMS误差(度)
流(' \ t %。2 f(度)\ n \ n ', sqrt(平均(quatd ^ 2)));
0.32(度)
您也可以从以下列表中选择一个网站: