主要内容

使用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;为其他helper函数构造对象。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 = hc . growamplitude (oscMotionAcc);构造轨迹对象轨迹(“SampleRate”imuFs,.．.“速度”initVel,.．.“位置”initPos,.．.“定位”, initOrient);

传感器配置

利用加速度计、陀螺仪和磁力计进行仿真imuSensor．高度计是用altimeterSensor．传感器配置中使用的值与MEMS传感器的实际值相对应。

imu = imsensor (“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);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);%高度计高度表=高度表传感器(“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.磁场;initstate(16:18) = imu.Magnetometer.ConstantBias;fusionfilt。状态=初始状态;

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

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

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

magNoise = 2*(imu. magnetometer . noisendensity (1).^2)*imuFs;altimeterNoise = 2*(altimeter.NoiseDensity)。^2 * altFs;

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

fusionfilt。加速度计噪声= [1e-1 1e-1 1e-4];fusionfilt。加速度计biasnoise = 1e-8;fusionfilt。地磁矢量噪声= 1e-12;fusionfilt。磁力计偏差噪声= 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);为imsensor对象提供当前位置和方向[accel，陀螺，mag] = imu(acc, omgBody, q);fusionfilt。预测(accel陀螺);以磁力计取样速率熔断磁力计样本如果~mod(ii,imuSamplesPerMag) fusemag(fusionfilt, mag, magNoise);结束以高度计采样率对高度计输出进行采样和熔断如果~mod(ii,imuSamplesPerAlt) altHeight =高度计(pos);%使用| fusealometer |方法更新融合过滤器%高度计输出。fusealtimeter (fusionfilt altHeight altimeterNoise);结束记录实际的方向和位置[actP(ii)， actQ(ii)] = pose(fusionfilt);记录期望的方向和位置expQ(ii) = q;expP(ii) = pos(3);如果usePoseViewer hfunct .view(actP(ii)， actQ(ii)，expP(ii)， expQ(ii));% #好< * UNRCH >结束结束

图滤波器性能

绘制过滤器的性能图。显示器使用四元数距离和高度误差显示方向误差。

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

图估计误差包含2个轴对象。标题为“方向错误-四元数距离”的坐标轴对象1包含一个类型为line的对象。标题为Z位置错误的坐标轴对象2包含一个类型为line的对象。

结论

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