估计方向和高度使用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];％的初始线速度（米/秒）initOrient =一（1，“四元”);定义旋转传感器体的恒定角速度% (rad / s)。angVel = [0.34 0.2 0.045];定义的简单振荡运动所需的加速度％传感器主体。fc = 0.2;t = 0:1 / imuFs: (n - 1) / imuFs;= 1;oscMotionAcc =罪(2 *π* fc * t);oscMotionAcc = hfunc.growAmplitude (oscMotionAcc);构造弹道目标TRAJ = kinematicTrajectory（'采样率'imuFs,...'速度'initVel,...“位置”initPos,...“定位”,initOrient);
传感器配置
对加速度计、陀螺仪和磁强计进行了仿真imuSensor。方法对高度计进行建模altimeterSensor。传感器配置中使用的值与实际MEMS传感器值相对应。
IMU = imuSensor（“accel-gyro-mag”，'采样率'，imuFs）;加速度计％imu.Accelerometer.MeasurementRange = 19.6133;imu.Accelerometer.Resolution = 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.Resolution = 0.1;imu.Magnetometer.ConstantBias = 100;imu.Magnetometer.NoiseDensity = 0.3 / SQRT（50）;%高度计高度表= altimeterSensor（“UpdateRate”，altFs，“NoiseDensity”，2 * 0.1549）;
融合滤波器
构造一个ahrs10filter和配置。
fusionfilt = ahrs10filter;fusionfilt。IMUSampleRate = imuFs;
聚变滤波器中设定的初始值。
initstate = 0(18日1);initstate(1:4) =紧凑(initOrient);initstate (5) = initPos (3);initstate (6) = initVel (3);initstate(七)= imu.Gyroscope.ConstantBias / imuFs;initstate(十12)= imu.Accelerometer.ConstantBias / imuFs;initstate(行)= imu.MagneticField;initstate (16:18) = imu.Magnetometer.ConstantBias;fusionfilt。状态= 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 * (altimeter.NoiseDensity)。^ 2 * altFs;
滤波处理噪声被用于调谐滤波器，以期望的性能。
fusionfilt.AccelerometerNoise = [1E-1 1E-1 1E-4];fusionfilt.AccelerometerBiasNoise = 1E-8;fusionfilt.GeomagneticVectorNoise = 1E-12;fusionfilt.MagnetometerBiasNoise = 1E-12;fusionfilt.GyroscopeNoise = 1E-12;
其他模拟选项：浏览器
默认情况下，这个模拟在模拟结束绘制的估计误差。与地面真理模拟运行以及查看估计位置和方向两者设置usePoseViewer变量来真正的。
usePoseViewer = FALSE;
仿真循环
q = initOrient;首次= true;actQ = 0 (N, 1,“四元”);expQ = 0 (N, 1,“四元”);actP = 0 (N, 1);expP = 0 (N, 1);对于i = 1: N％生成一组新的样品从轨迹发生器accBody = rotateframe（Q，[0 0 + oscMotionAcc（II）]）;omgBody = rotateframe（Q，angVel）;[POS，Q，VEL，ACC] = TRAJ（accBody，omgBody）;％饲料当前位置和方位的imuSensor对象[accel, gyro, mag] = imu(acc, omgBody, q);fusionfilt。预测(accel陀螺);在磁力采样率％保险丝磁力计样本如果~mod(ii,imuSamplesPerMag) fusemag(fusionfilt, mag, magNoise);结束%采样，并以测高计采样率熔接测高计输出如果〜MOD（二，imuSamplesPerAlt）altHeight =高度表（POS）;％使用| fusealtimeter |方法与更新融合滤波器％高度表输出。fusealtimeter（fusionfilt，altHeight，altimeterNoise）;结束记录实际的方向和位置[actP(ii)， actQ(ii)] = pose(fusionfilt);％登录预期的方向和位置expQ (ii) =问;expP (ii) = pos (3);如果usePoseViewer hfunc.view(actP(ii)， actQ(ii)，expP(ii)， expQ(ii));％＃确定<* UNRCH>结束结束
情节滤波器性能
绘制过滤器的性能。该显示器利用四元数距离和高度误差显示方向误差。
hfunc。plotErrs(actP, actQ, expP, expQ);
结论
这个例子显示了如何将ahrs10filter执行10轴传感器融合的高度和方向。
你也可以从以下列表中选择一个网站:
