主要内容
使用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,“四元数”）;定义旋转传感器体的恒定角速度% (rad / s)。角度= [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 (“SampleRate”imuFs,．..“速度”initVel,．..“位置”initPos,．..“定位”, initOrient);
传感器配置
对加速度计、陀螺仪和磁强计进行了仿真imuSensor．高度表是用altimeterSensor．传感器配置中使用的值与实际MEMS传感器值对应。
imu = imuSensor (“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 =函数(250);imu.Gyroscope.Resolution函数= (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 /√(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;
初始化融合滤波器的状态协方差矩阵。ground truth用于初始状态，所以估计误差应该很小。
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 = 1 e-8;fusionfilt。GeomagneticVectorNoise = 1 e-12;fusionfilt。MagnetometerBiasNoise = 1 e-12;fusionfilt。GyroscopeNoise = 1 e-12;
附加模拟选项:查看器
默认情况下，该仿真在仿真结束时绘制估计误差图。为了在模拟运行时同时查看估计的位置和方向以及地面真相，设置usePoseViewer变量来真正的．
usePoseViewer = false;
模拟循环
q = initOrient;首次= 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, angle vel);[pos, q, vel, acc] = traj(accBody, omgBody);%输入当前的位置和方向到imussensor对象[accel, gyro, mag] = imu(acc, omgBody, q);fusionfilt。预测(accel陀螺);熔丝磁力计样品在磁力计取样率如果~mod(ii,imuSamplesPerMag) fusemag(fusionfilt, mag, magNoise);结束%按照高度计采样率采样和熔断高度计输出如果~mod(ii,imuSamplesPerAlt) altHeight =高度计(pos);%使用|熔丝测高仪|方法更新融合滤波器%高度计输出。fusealtimeter (fusionfilt altHeight altimeterNoise);结束%记录实际方向和位置[actP(ii)， actQ(ii)] = pose(fusionfilt);记录预期的入职情况和职位expQ (ii) =问;expP (ii) = pos (3);如果使用poseviewer hfunct .view(actP(ii)， actQ(ii)，expP(ii)， expQ(ii));% #好< * UNRCH >结束结束
情节过滤性能
绘制过滤器的性能。显示器使用四元数距离和高度误差显示方向误差。
hfunc。plterrs (actP, actQ, expP, expQ);
结论
这个例子展示了如何ahrs10filter进行高度和方向的10轴传感器融合。
你也可以从以下列表中选择一个网站:
