主要内容

IMU和GPS融合用于惯性导航

打开实时脚本

This example shows how you might build an IMU + GPS fusion algorithm suitable for unmanned aerial vehicles (UAVs) or quadcopters.

This example uses accelerometers, gyroscopes, magnetometers, and GPS to determine orientation and position of a UAV.

Simulation Setup

Set the sampling rates. In a typical system, the accelerometer and gyroscope run at relatively high sample rates. The complexity of processing data from those sensors in the fusion algorithm is relatively low. Conversely, the GPS, and in some cases the magnetometer, run at relatively low sample rates, and the complexity associated with processing them is high. In this fusion algorithm, the magnetometer and GPS samples are processed together at the same low rate, and the accelerometer and gyroscope samples are processed together at the same high rate.

To simulate this configuration, the IMU (accelerometer, gyroscope, and magnetometer) are sampled at 160 Hz, and the GPS is sampled at 1 Hz. Only one out of every 160 samples of the magnetometer is given to the fusion algorithm, so in a real system the magnetometer could be sampled at a much lower rate.

imufs= 160; gpsFs = 1;％定义在地球上的位置，这种模拟场景是使用% latitude, longitude and altitude.refloc = [42.2825 -72.3430 53.0352];％验证| gpsfs |Divides | Imufs |。这允许传感器样品使用嵌套循环的嵌套不复杂的采样速率来模拟的％率% matching.imuSamplesPerGPS = (imuFs/gpsFs); assert(imuSamplesPerGPS == fix(imuSamplesPerGPS),。。。“ GPS采样率必须是IMU采样率的整数因素。”);

Fusion Filter

Create the filter to fuse IMU + GPS measurements. The fusion filter uses an extended Kalman filter to track orientation (as a quaternion), velocity, position, sensor biases, and the geomagnetic vector.

ThisinsfilterMARGhas a few methods to process sensor data, including预测,Fusemag和fusegps。这预测method takes the accelerometer and gyroscope samples from the IMU as inputs. Call the预测method each time the accelerometer and gyroscope are sampled. This method predicts the states one time step ahead based on the accelerometer and gyroscope. The error covariance of the extended Kalman filter is updated here.

这fusegpsmethod takes GPS samples as input. This method updates the filter states based on GPS samples by computing a Kalman gain that weights the various sensor inputs according to their uncertainty. An error covariance is also updated here, this time using the Kalman gain as well.

这Fusemagmethod is similar but updates the states, Kalman gain, and error covariance based on the magnetometer samples.

虽然insfilterMARGtakes accelerometer and gyroscope samples as inputs, these are integrated to compute delta velocities and delta angles, respectively. The filter tracks the bias of the magnetometer and these integrated signals.

fusionfilt = insfilterMARG; fusionfilt.IMUSampleRate = imuFs; fusionfilt.ReferenceLocation = refloc;

UAV Trajectory

该示例使用从无人机记录的保存轨迹作为地面真相。该轨迹被馈送到几个传感器模拟器中，以计算模拟加速度计，陀螺仪，磁力计和GPS数据流。

％加载“地面真相”无人机轨迹。loadLoggedQuadcopter.matTrajdata;trajorient = trajdata.orientation;trajvel = trajdata.velocity;trajpos = trajdata.position;trajacc = trajdata.Acceleration;trajangvel = trajdata.angularvelocity;% Initialize the random number generator used in the simulation of sensor% noise.rng(1)

GPS传感器

建立了GPS在指定的sample rate and reference location. The other parameters control the nature of the noise in the output signal.

gps = gpsSensor(“更新”, gpsFs); gps.ReferenceLocation = refloc; gps.DecayFactor = 0.5;% Random walk noise parametergps.horizo​​ntalpositionAccuracy = 1.6;gps.verticalpositionAccuracy = 1.6;gps.velocityAccuracy = 0.1;

IMU Sensors

Typically, a UAV uses an integrated MARG sensor (Magnetic, Angular Rate, Gravity) for pose estimation. To model a MARG sensor, define an IMU sensor model containing an accelerometer, gyroscope, and magnetometer. In a real-world application the three sensors could come from a single integrated circuit or separate ones. The property values set here are typical for low-cost MEMS sensors.

imu = imuSensor('accel-gyro-mag','SampleRate'，imufs）;imu.magneticfield = [19.5281 -5.0741 48.0067];% Accelerometerimu.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.Sousolution = Deg2rad（0.0625）;imu.gyroscope.constantbias = deg2rad（3.125）;imu.gyRoscope.AxesMisalignment = 1.5;imu.gyRoscope.noisedense = deg2rad（0.025）;％磁力计imu.magnetometer.measurementrange = 1000;imu.magnetometer.somutions = 0.1;imu.magnetometer.constantbias = 100;imu.magnetometer.noisterense = 0.3/ sqrt（50）;

Initialize the State Vector of theinsfilterMARG

这insfilterMARGtracks the pose states in a 22-element vector. The states are:

国家单位状态矢量索引方向作为四元基因1：4位置（NED）m 5：7速度（NED）m/s 8:10 delta角偏置（xyz）rad 11:13 delta速度偏置（xyz）m/s 14：16地磁场矢量（NED）UT 17:19磁力计偏置（XYZ）UT 20:22

Ground truth is used to help initialize the filter states, so the filter converges to good answers quickly.

％初始化过滤器的状态initstate =零（22,1）;initstate（1：4）= compact（seanrot（trajorient（1：100）））;initstate（5：7）=平均值（trajpos（1：100，:)，1）;initstate（8:10）=平均值（1：100，:)，1）;initstate（11:13）= imu.gyroscope.constantbias./imufs;initstate（14:16）= imu.accelerometer.constantbias./imufs;initstate（17:19）= imu.magneticfield;initstate（20:22）= imu.magnetometer.constantbias;fusionfilt.state = initstate;

Initialize the Variances of theinsfilterMARG

这insfilterMARGmeasurement noises describe how much noise is corrupting the sensor reading. These values are based on theimusensor和GPSSENSORparameters.

这process noises describe how well the filter equations describe the state evolution. Process noises are determined empirically using parameter sweeping to jointly optimize position and orientation estimates from the filter.

％测量噪声Rmag = 0.09;％磁力计measurement noiseRvel = 0.01;％GPS速度测量噪声Rpos = 2.56;% GPS Position measurement noise% Process noisesfusionfilt。AccelerometerBiasNoise = 2的军医;fusionfilt。AccelerometerNoise = 2; fusionfilt.GyroscopeBiasNoise = 1e-16; fusionfilt.GyroscopeNoise = 1e-5; fusionfilt.MagnetometerBiasNoise = 1e-10; fusionfilt.GeomagneticVectorNoise = 1e-12;% Initial error covariancefusionfilt。StateCovariance = 1e-9*ones(22);

Initialize Scopes

这HelperScrollingPlotter范围可以随着时间的推移绘制变量。它在这里用于跟踪姿势错误。这HelperPoseViewerscope allows 3-D visualization of the filter estimate and ground truth pose. The scopes can slow the simulation. To disable a scope, set the corresponding logical variable tofalse。

useerrscope = true;% Turn on the streaming error plotusePoseView = true;％打开3D姿势查看器ifuseErrScope errscope = HelperScrollingPlotter(。。。'numInputs', 4,。。。'TimeSpan'，10，。。。'SampleRate', imuFs,。。。'ylabel'，{'degrees',。。。'meters',。。。'meters',。。。'meters'},。。。'标题'，{'Quaternion Distance',。。。'位置x错误',。。。“位置y错误”,。。。'Position Z Error'},。。。'YLimits',。。。[ -1, 1 -2, 2 -2 2 -2 2]);endifusePoseView posescope = HelperPoseViewer(。。。'XPositionLimits'，[-15 15]，。。。'ypositionlimits', [-15, 15],。。。“ zpositionlimits”，[-10 10]）;end

Simulation Loop

主仿真循环是一个while循环，带有嵌套以进行循环。while循环执行gpsFs，这是GPS样本率。嵌套以进行循环执行imufs，这是IMU样本率。范围以IMU采样速率更新。

% Loop setup - |trajData| has about 142 seconds of recorded data.secondsToSimulate = 50;％模拟约50秒numsamples = secondsToSimulate*imuFs; loopBound = floor(numsamples); loopBound = floor(loopBound/imuFs)*imuFs;% ensure enough IMU Samples% Log data for final metric computation.pqorient = quaternion.zeros(loopBound,1); pqpos = zeros(loopBound,3); fcnt = 1;尽管(fcnt <=loopBound)％|预测|IMU更新频率的循环。forff=1:imuSamplesPerGPS% Simulate the IMU data from the current pose.[accel, gyro, mag] = imu(trajAcc(fcnt,:), trajAngVel(fcnt, :),。。。trajorient（fcnt））;％使用|预测|估计基于过滤状态的方法模拟加速度计和陀螺仪信号上的％。预测（Fusionfilt，Accel，Gyro）;% Acquire the current estimate of the filter states.[fusedPos, fusedOrient] = pose(fusionfilt);% Save the position and orientation for post processing.pqorient(fcnt) = fusedOrient; pqpos(fcnt,:) = fusedPos;% Compute the errors and plot.ifuseerrscope orienterr = rad2deg（fusedorient，。。。trajorient（fcnt）））;poserr = fusedpos -trajpos（fcnt，:);Errscope（Orienterr，Poserr（1），Poserr（2），Poserr（3））;end% Update the pose viewer.ifusePoseView posescope(pqpos(fcnt,:), pqorient(fcnt), trajPos(fcnt,:),。。。trajorient（fcnt，:)）;endfcnt = fcnt + 1;end％此下一步以GPS样本率发生。% Simulate the GPS output based on the current pose.[lla, gpsvel] = gps( trajPos(fcnt,:), trajVel(fcnt,:) );% Correct the filter states based on the GPS data and magnetic% field measurements.Fusegps（FusionFilt，LLA，RPOS，GPSVEL，RVEL）;fusemag（融合风格，mag，rmag）;end

Figure Scrolling Plotter contains 4 axes objects. Axes object 1 with title Position Z Error contains an object of type line. Axes object 2 with title Position Y Error contains an object of type line. Axes object 3 with title Position X Error contains an object of type line. Axes object 4 with title Quaternion Distance contains an object of type line.

图姿势查看器包含3个轴对象。轴对象1具有标题位置（仪表）包含​​4个类型线的对象。轴对象2具有标题方向 - 估计为空。轴对象3具有标题的定向 - 地面真相是空的。

错误度量计算

Position and orientation estimates were logged throughout the simulation. Now compute an end-to-end root mean squared error for both position and orientation.

posd = pqpos(1:loopBound,:) - trajPos( 1:loopBound, :);% For orientation, quaternion distance is a much better alternative to% subtracting Euler angles, which have discontinuities. The quaternion% distance can be computed with the |dist| function, which gives the% angular difference in orientation in radians. Convert to degrees％用于在命令窗口中显示。quatd = rad2deg(dist(pqorient(1:loopBound), trajOrient(1:loopBound)) );％在命令窗口中显示RMS错误。fprintf('\ n \ nend-to-end仿真位置rms erry误差\ n');
End-to-End Simulation Position RMS Error
msep = sqrt(mean(posd.^2)); fprintf('\tX: %.2f , Y: %.2f, Z: %.2f (meters)\n\n',msep(1),。。。msep(2), msep(3));
X：0.57，Y：0.53，Z：0.68（米）
fprintf('End-to-End Quaternion Distance RMS Error (degrees) \n');
End-to-End Quaternion Distance RMS Error (degrees)
fprintf('\t%.2f (degrees)\n\n'，sqrt（平均值（quatd。^2）））;
0.32（度）

传感器融合和跟踪工具箱Documentation

金宝app

Sensor Fusion and Tracking for Autonomous Systems

Sensor Fusion and Tracking for Autonomous Systems

下载白皮书