IMU惯性导航和全球定位系统(GPS)融合
这个例子展示了如何构建一个IMU + GPS融合算法适合无人机(uav)或四轴飞行器。
这个示例使用加速度计、陀螺仪、磁力计和GPS确定方向和位置的无人机。
仿真设置
设置采样率。在一个典型的系统,加速度计和陀螺仪在相对较高的采样率。处理数据的复杂性从这些传感器的融合算法相对较低。相反,全球定位系统(GPS),在某些情况下,磁强计,以相对较低的采样率运行,并处理相关的复杂性高。在这种融合算法,磁力仪和GPS处理样品在相同的低利率,加速度计和陀螺仪样品处理在高速率是相同的。
来模拟这个配置,乌兹别克斯坦伊斯兰运动(加速度计、陀螺仪和磁强计)采样在160赫兹,和GPS是采样1 Hz。每160人中只有一个样品磁强计的融合算法,所以在实际系统的磁力仪可以在更低的采样率。
imuFs = 160;gpsFs = 1;%定义在地球上这个模拟场景中使用%纬度、经度和海拔。refloc = (42.2825 -72.3430 53.0352);%验证| gpsFs | |分裂imuFs |。这使得传感器样品%的利率是模拟使用嵌套循环,没有复杂的采样率%匹配。imuSamplesPerGPS = (imuFs / gpsFs);断言(imuSamplesPerGPS = =修复(imuSamplesPerGPS),…全球定位系统(GPS)采样率必须是整数IMU采样率的因素。);
融合滤波器
创建过滤器保险丝IMU + GPS测量。融合滤波器使用扩展卡尔曼滤波跟踪定位(作为一个四元数),速度,位置,传感器偏差,地磁矢量。
这insfilterMARG有一些方法来处理传感器数据,包括预测,fusemag和fusegps。的预测方法采用加速度计和陀螺仪乌兹别克斯坦伊斯兰运动的样本作为输入。调用预测进行抽样方法每次加速度计和陀螺仪。这种方法预测美国前一个时间步基于加速度计和陀螺仪。扩展卡尔曼滤波误差协方差的更新。
的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。DecayFactor = 0.5;%随机漫步噪声参数gps。HorizontalPositionAccuracy = 1.6;gps。VerticalPositionAccuracy = 1.6;gps。VelocityAccuracy = 0.1;
IMU传感器
通常,无人机使用一个集成的玛格传感器(磁、角速率、重力)估计。玛格传感器模型,定义一个IMU传感器模型包含一个加速度计、陀螺仪和磁强计。在现实的应用程序中这三个传感器可能来自一个集成电路或单独的。属性值设置下面是典型的低成本的MEMS传感器。
imu = imuSensor (“accel-gyro-mag”,“SampleRate”,imuFs);imu。磁场= (19.5281 -5.0741 48.0067);%加速度计imu.Accelerometer。MeasurementRange = 19.6133;imu.Accelerometer。分辨率= 0.0023928;imu.Accelerometer。ConstantBias = 0.19;imu.Accelerometer。NoiseDensity = 0.0012356;%陀螺仪imu.Gyroscope。MeasurementRange =函数(250);imu.Gyroscope。解析函数= (0.0625);imu.Gyroscope。ConstantBias函数= (3.125);imu.Gyroscope。AxesMisalignment = 1.5;imu.Gyroscope。NoiseDensity函数= (0.025);%磁强计imu.Magnetometer。MeasurementRange = 1000;imu.Magnetometer。分辨率= 0.1;imu.Magnetometer。ConstantBias = 100;imu.Magnetometer。NoiseDensity = 0.3 /√(50);
初始化的状态向量insfilterMARG
的insfilterMARG跟踪构成州22-element向量。美国是:
国家单位状态向量索引定位作为一个四元数1:4位置(NED) m 5:7速度(NED) m / s 8:10δ角偏差(某某)rad 11:13三角洲速度偏差(某某)m / s十四16地磁场矢量(NED) uT十七19磁强计偏差(某某)uT 20:22
地面真理是用于初始化滤波器,滤波器收敛迅速好的答案。
%初始化滤波器的状态initstate = 0(22日1);initstate(1:4) =紧凑(meanrot (trajOrient (1:10 0)));initstate(7) =意味着(trajPos (1:10 0:), 1);initstate(8) =意味着(trajVel (1:10 0:), 1);initstate (11:13) = imu.Gyroscope.ConstantBias. / imuFs;initstate(十四16)= imu.Accelerometer.ConstantBias. / imuFs;initstate(十七19)= imu.MagneticField;initstate (20:22) = imu.Magnetometer.ConstantBias;fusionfilt。状态= initstate;
初始化的差异insfilterMARG
的insfilterMARG测量噪声描述腐蚀传感器读多少噪音。这些值的基础上imuSensor和gpsSensor参数。
过程噪声描述如何过滤方程描述进化状态。确定过程噪声经验使用参数全面,共同优化滤波器的位置和姿态估计。
%测量噪声Rmag = 0.0862;%磁强计测量噪声Rvel = 0.0051;%的GPS速度测量噪声rpo = 5.169;%的GPS位置测量噪声%过程噪音fusionfilt。AccelerometerBiasNoise = 0.010716;fusionfilt。AccelerometerNoise = 9.7785;fusionfilt。GyroscopeBiasNoise = 1.3436 e-14;fusionfilt。GyroscopeNoise = 0.00016528;fusionfilt。MagnetometerBiasNoise = 2.189 e-11; fusionfilt.GeomagneticVectorNoise = 7.67e-13;%初始误差协方差fusionfilt。StateCovariance = 1 e-9 *眼(22);
初始化范围
的HelperScrollingPlotter随着时间的推移使策划范围的变量。使用它来跟踪错误的姿势。的HelperPoseViewer允许范围的3 d可视化滤波器估计和地面真理构成。范围可以缓慢的模拟。禁用一个范围,设置相应的逻辑变量假。
useErrScope = true;%打开流错误情节usePoseView = true;%的3 d查看器如果useErrScope errscope = HelperScrollingPlotter (…“NumInputs”4…“时间间隔”10…“SampleRate”imuFs,…“YLabel”,{“度”,…“米”,…“米”,…“米”},…“标题”,{“四元数距离”,…“X位置错误”,…“Y位置错误”,…“Z位置错误”},…“YLimits”,…[3、3 2 2 2 2 2 2]);结束如果usePoseView posescope = HelperPoseViewer (…“XPositionLimits”15 [-15],…“YPositionLimits”(-15年,15),…“ZPositionLimits”,-10年[10]);结束
模拟循环
主仿真循环是一个while循环的嵌套循环。while循环执行gpsFsGPS采样率。的嵌套循环执行imuFsIMU的采样率。IMU的范围更新采样率。
%循环设置- | trajData |大约有142秒的记录数据。secondsToSimulate = 50;%模拟约50秒numsamples = secondsToSimulate * imuFs;loopBound =地板(numsamples);loopBound =地板(loopBound / imuFs) * imuFs;%确保足够的IMU样本%日志数据进行最后度量计算。pqorient = quaternion.zeros (loopBound, 1);pqpos = 0 (loopBound, 3);fcnt = 1;而(fcnt < = loopBound)% | |预测循环IMU更新频率。为ff = 1: imuSamplesPerGPS%模拟IMU数据从当前的姿势。(accel、陀螺、mag) = imu (trajAcc (fcnt:), trajAngVel (fcnt:)…trajOrient (fcnt));%使用| |预测方法来估计滤波器基于状态%的模拟加速度计和陀螺仪信号。预测(accel fusionfilt,陀螺);%获得的当前估计滤波器。[fusedPos, fusedOrient] =姿势(fusionfilt);%保存后加工的位置和姿态。pqorient (fcnt) = fusedOrient;pqpos (fcnt:) = fusedPos;%计算错误和阴谋。如果useErrScope orientErr = rad2deg (dist (fusedOrient…trajOrient (fcnt)));posErr = fusedPos - trajPos (fcnt:);errscope (orientErr posErr (1) posErr (2), posErr (3));结束%更新造成观众。如果usePoseView posescope (pqpos (fcnt:), pqorient (fcnt) trajPos (fcnt:)…trajOrient (fcnt:));结束fcnt = fcnt + 1;结束%这下一步发生在GPS采样率。%模拟GPS输出基于当前姿势。(lla gpsvel] = gps (trajPos (fcnt:), trajVel (fcnt:));%的基于GPS数据和磁过滤状态%实地测量。fusegps (lla fusionfilt, rpo, gpsvel Rvel);fusemag (fusionfilt杂志,Rmag);结束
误差指标计算
位置和姿态估计是整个模拟记录。现在计算一个端到端的位置和姿态的根均方误差。
posd = pqpos (1: loopBound,:) - trajPos (1: loopBound,:);%为取向,四元数距离是一个更好的选择%减去欧拉角,不连续。四元数的与| dist | %距离计算函数,给出了%角不同取向的弧度。转换为度在命令窗口中显示的%。quatd = rad2deg (dist (pqorient (1: loopBound) trajOrient (1: loopBound)));%显示RMS命令窗口中的错误。流(' \ n \ nEnd-to-End模拟位置均方根误差\ n”);
端到端模拟位置均方根误差
msep =√意味着(posd ^ 2));流(“\ tX: %。Y: % 2 f。Z: % 2 f。2 f(米)\ n \ n”msep (1)…msep msep (2), (3));
Y X: 0.50: 0.79, Z: 0.65(米)
流(“端到端四元数距离均方根误差(度)\ n ');
端到端均方根误差四元数距离(度)
流(' \ t %。2 f(度)\ n \ n ',sqrt(平均(quatd ^ 2)));
1.45(度)
