使用IMU，磁强计和高度计估计方向和高度

打开实时脚本

这个例子展示了如何融合来自3轴加速度计、3轴陀螺仪、3轴磁强计(通常称为磁性、角速度和重力的MARG传感器)和1轴高度计的数据，以估计方向和高度。

仿真设置

该模拟以多种速率处理传感器数据。IMU(加速计和陀螺仪)通常以最高的速率运行。磁强计的工作速率一般比IMU低，高度计的工作速率最低。改变采样率会导致部分融合算法运行得更频繁，并可能影响性能。

设置IMU传感器、磁强计和高度计的采样率。imuFs = 100;altFs = 10;magFs = 25;imuSamplesPerAlt = fix(imuFs/altFs);imuSamplesPerMag = fix(imuFs/magFs);设置要模拟的样本数量。N = 1000;为其他助手函数构造对象。hfunc = Helper10AxisFusion;

定义轨迹

传感器体围绕所有三个轴旋转，同时在位置上垂直振荡。随着模拟的继续，振荡的震级增加。

定义传感器体的初始状态initPos = [0,0,0];初始位置% (m)initVel = [0, 0， -1];初始线速度% (m/s)initOrient = ones(1，“四元数”）;定义旋转传感器主体的恒定角速度% (rad / s)。angVel = [0.34 0.2 0.045];的简单振荡运动所需的加速度%传感器体。Fc = 0.2;t = 0:1/imuFs:(N-1)/imuFs;A = 1;oscMotionAcc = sin(2*pi*fc*t);oscMotionAcc = hfunc.growAmplitude(oscMotionAcc);构造轨迹对象traj = kinematicTrajectory(“SampleRate”imuFs,.．.“速度”initVel,.．.“位置”initPos,.．.“定位”, initOrient);

传感器配置

仿真了加速度计、陀螺仪和磁强计imuSensor．高度计的建模使用altimeterSensor．传感器配置中使用的值与MEMS传感器的实际值相对应。

imu = imussensor (“accel-gyro-mag”，“SampleRate”, imuFs);%加速度计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);陀螺仪.分辨率= deg2rad(0.0625);imu.Gyroscope.ConstantBias = deg2rad(3.125);imu.Gyroscope.AxesMisalignment = 1.5;陀螺仪. noised高密度= deg2rad(0.025);%磁强计imu.Magnetometer.MeasurementRange = 1000;imu.磁强计分辨率= 0.1;imu.Magnetometer.ConstantBias = 100;噪声密度= 0.3/根方根(50);%高度计altimeter = altimeterSensor(“UpdateRate”altFs,“NoiseDensity”2 * 0.1549);

融合滤波器

构造一个ahrs10filter和配置。

Fusionfilt = ahrs10filter;fusionfilt。IMUSampleRate = imuFs;

设置融合滤波器的初始值。

Initstate = 0 (18,1);initstate(1:4) = compact(initOrient);initstate(5) = initPos(3);initstate(6) = initVel(3);initstate(7:9) = imu.Gyroscope.ConstantBias/imuFs;initstate(10:12) = imu.Accelerometer.ConstantBias/imuFs;initstate(13:15) = imu.MagneticField;initstate(16:18) = imu.Magnetometer.ConstantBias;fusionfilt。State = initstate;

初始化融合滤波器的状态协方差矩阵。基真值用于初始状态，因此估计误差应该很小。

icv =诊断接头([1 e-8 * (1 1 1 1 1 1 1), 1 e - 3 * 1 (11)]);fusionfilt。StateCovariance = icv;

磁强计和高度计的测量噪声是与内部卡尔曼滤波器所使用的传感器相关联的观测噪声ahrs10filter．这些值通常来自传感器数据表。

magNoise = 2*(imu.Magnetometer.NoiseDensity(1).^2)*imuFs;altimeterNoise = 2*(almeter . noisedensity)。^2 * altFs;

滤波器过程噪声用于调谐滤波器以达到所需的性能。

fusionfilt。加速度计噪声= [1e-1 1e-1 1e-4];fusionfilt。AccelerometerBiasNoise = 1e-8;fusionfilt。地磁矢量噪声= 1e-12;fusionfilt。MagnetometerBiasNoise = 1e-12;fusionfilt。陀螺仪= 1e-12;

附加模拟选项:查看器

默认情况下，该模拟在模拟结束时绘制估计误差。若要在模拟运行时查看估计的位置和方向以及地面真相，请设置usePoseViewer变量来真正的．

usePoseViewer = false;

模拟循环

q = initOrient;firstTime = true;actQ = 0 (N,1，“四元数”）;expQ = 0 (N,1，“四元数”）;actP = 0 (N,1);expP = 0 (N,1);为ii = 1: N从轨迹生成器生成一组新的样本。accBody = rotateframe(q， [0 0 +oscMotionAcc(ii)]);omgBody = rotateframe(q, angVel);[pos, q, vel, acc] = traj(accBody, omgBody);将当前位置和方向提供给imussensor对象。[accel，陀螺仪，mag] = imu(acc, omgBody, q);fusionfilt。预测(accel陀螺);熔断器磁强计样品在磁强计样品率如果~mod(ii,imuSamplesPerMag) fusemag(fusionfilt, mag, magNoise);结束%按高度计采样率对高度计输出进行采样和熔断如果~mod(ii,imuSamplesPerAlt) altHeight =高度计(pos);使用| fusealmeter |方法更新融合过滤器%高度计输出。fusealtimeter (fusionfilt altHeight altimeterNoise);结束%记录实际的方向和位置[actP(ii)， actQ(ii)] = pose(fusionfilt);%记录预期的方向和位置expQ(ii) = q;expP(ii) = pos(3);如果usePoseViewer hfunc.view(actP(ii)， actQ(ii)，expP(ii)， expQ(ii));% #好< * UNRCH >结束结束

Plot滤波器性能

绘制滤波器的性能图。显示器使用四元数的距离和高度误差来显示定位误差。

hfunc。ploters (actP, actQ, expP, expQ);

图估计误差包含2个坐标轴对象。带有标题定向错误-四元数距离的axis对象1包含一个类型为line的对象。标题为Z Position Error的Axes对象2包含一个类型为line的对象。

结论

这个例子展示了如何ahrs10filter执行10轴传感器融合的高度和方向。