使用IMU、磁力仪和高度计估计方位和高度
这个例子演示了如何融合来自3轴加速度计、3轴陀螺仪、3轴磁强计(通常称为磁、角速率和重力的MARG传感器)和1轴高度计的数据来估计方向和高度。
仿真设置
该仿真以多种速率处理传感器数据。IMU(加速度计和陀螺仪)通常以最高速率运行。磁强计通常以比IMU更低的速率运行，而高度计则以最低的速率运行。改变采样率会导致部分融合算法更频繁地运行，并可能影响性能。
设置IMU传感器、磁强计和高度计的采样率。imuFs = 100;altFs = 10;magFs = 25;imuSamplesPerAlt =修复(imuFs / altFs);imuSamplesPerMag =修复(imuFs / magFs);％组样品，以模拟的数量。N = 1000;为其他辅助函数构造对象。hfunc = Helper10AxisFusion;
定义轨迹
传感器体围绕所有三个轴旋转，同时在垂直位置振荡。随着模拟的继续，振荡的幅度增加。
定义传感器体的初始状态initPos = [0,0，0];初始位置(m)initVel = [0,0，-1];初始线速度% (m/s)initOrient = 1 (1,“四元数”);％定义恒定角速度旋转所述传感器主体％（弧度/秒）。angVel = [0.34 0.2 0.045];％定义所需的简单的摆动运动的加速度%传感器的身体。FC = 0.2;t = 0时：1 / imuFs：（N-1）/ imuFs;一个= 1;oscMotionAcc = SIN（2 * PI * FC * T）;oscMotionAcc = hfunc.growAmplitude（oscMotionAcc）;％构建轨迹对象traj = kinematicTrajectory (“SampleRate”，imuFs，…“速度”，initVel，…'位置'，initPos，…'取向'，initOrient）;
传感器配置
加速度计，陀螺仪和磁强计使用模拟imuSensor.高度表使用建模altimeterSensor.在传感器配置中使用的值对应于真实的MEMS传感器的值。
imu = imuSensor ('加速 - 陀螺-MAG',“SampleRate”,imuFs);%加速度计imu.Accelerometer。MeasurementRange = 19.6133;imu.Accelerometer。分辨率= 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.NoiseDensity = deg2rad（0.025）;％磁强计imu.Magnetometer。MeasurementRange = 1000;imu.Magnetometer。分辨率= 0.1;imu.Magnetometer。ConstantBias = 100;imu.Magnetometer。NoiseDensity = 0.3 /√(50);％高度表高度计= altimeterSensor ('UpdateRate'altFs,'NoiseDensity'2 * 0.1549);
融合滤波器
构建ahrs10filter和配置。
fusionfilt = ahrs10filter;fusionfilt.IMUSampleRate = imuFs;
设置融合滤波器的初始值。
INITSTATE =零（18,1）;INITSTATE（1：4）=紧凑（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;
初始化融合滤波器的状态协方差矩阵。ground truth用于初始状态，所以估计中应该有很小的误差。
ICV = DIAG（[1E-8 * [1 1 1 1 1 1 1]，1E-3 *酮（1,11）]）;fusionfilt.StateCovariance =侧脑室;
磁力仪和高度计测量噪声是由在内部卡尔曼滤波器中使用的传感器相关联的观测噪声ahrs10filter.这些值通常将来自传感器的数据表。
magNoise = 2 *（。imu.Magnetometer.NoiseDensity（1）^ 2）* imuFs;altimeterNoise = 2 *（altimeter.NoiseDensity）^ 2 * altFs。
过滤过程的噪音是用来调整过滤器到期望的性能。
fusionfilt。加速度计噪声= [1e-1 1e-1 1e-4];fusionfilt。AccelerometerBiasNoise = 1 e-8;fusionfilt。GeomagneticVectorNoise = 1 e-12;fusionfilt。MagnetometerBiasNoise = 1 e-12;fusionfilt。GyroscopeNoise = 1 e-12;
额外的模拟选项:查看器
默认情况下，该仿真在仿真结束时绘制估计误差。要查看模拟运行时的估计位置和方向以及地面真实情况，请设置usePoseViewer变量真正.
usePoseViewer = false;
模拟循环
Q = initOrient;firstTime = TRUE;actQ =零（N，1，“四元数”);expQ =零（N，1，“四元数”);ACTP =零（N，1）;EXPP =零（N，1）;为II = 1：N从弹道发生器生成一组新的样本accBody = rotateframe(q， [0 0 +oscMotionAcc(ii)]);omgBody = rotateframe(q, angVel);[pos, q, vel, acc] = traj(accBody, omgBody);将当前位置和方向提供给imuSensor对象[加速，陀螺仪，MAG] = IMU（ACC，omgBody，Q）;fusionfilt.predict（加速度，陀螺仪）;%熔丝磁强计样品在磁强计样品率如果〜MOD（二，imuSamplesPerMag）fusemag（fusionfilt，MAG，magNoise）;结束％的样品以及在熔丝高度表采样率高度表输出如果~mod(ii,imuSamplesPerAlt) altHeight =高度计(pos);%使用|熔断高度计|方法更新融合滤波器%高度计输出。fusealtimeter (fusionfilt altHeight altimeterNoise);结束％记录实际的方向和位置[ACTP（ii）中，actQ（II）] =姿态（fusionfilt）;记录期望的方向和位置expQ（II）= Q;EXPP（II）= POS（3）;如果usePoseViewer hfunc.view（ACTP（ii）中，actQ（ii）中，EXPP（ii）中，expQ（II））;% #好< * UNRCH >结束结束
情节过滤性能
画出滤波器的性能。显示示出了使用四元数的距离和高度中的错误取向的错误。
hfunc.plotErrs（ACTP，actQ，EXPP，expQ）;
结论
这个例子展示了如何ahrs10filter对高度和方向进行10轴传感器融合。
