估计方位和高度使用IMU,磁力仪和测高计
这个例子展示了如何从硬件融合数据加速计,使用硬件陀螺仪,使用硬件磁力仪(通常被称为一个玛格磁传感器,角速率,和重力),和1-axis高度计来估算方向和高度。
仿真设置
这个模拟过程传感器数据在多个率。乌兹别克斯坦伊斯兰运动(加速度计和陀螺仪)通常以最高的速度运行。磁强计通常运行在一个更低的利率比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 (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 (“SampleRate”imuFs,…“速度”initVel,…“位置”initPos,…“定位”,initOrient);
传感器配置
模拟加速度计、陀螺仪和磁强计使用imuSensor。高度计是建模使用altimeterSensor。传感器的配置中使用的值对应于真实的MEMS传感器的值。
imu = imuSensor (“accel-gyro-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 =函数(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。分辨率= 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;
初始化状态融合滤波的协方差矩阵。地面真理是用于初始状态,所以应该有小误差估计。
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 = [1 e 1 1 e 1 1]的军医;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);为2 = 1:N%生成一组新的样本轨迹生成器accBody = rotateframe (q, [0 0 + oscMotionAcc (ii)]);omgBody = rotateframe (q, angVel);[pos, q,韦尔,acc] = traj (accBody omgBody);%给当前的位置和姿态imuSensor对象(accel、陀螺、mag) = imu (acc, omgBody q);fusionfilt。预测(accel陀螺);%引信磁强计样品磁强计的采样率如果~国防部(ii, imuSamplesPerMag) fusemag (fusionfilt,杂志,magNoise);结束%样本和保险丝高度表的测高计输出采样率如果~国防部(ii, imuSamplesPerAlt) altHeight =高度计(pos);%使用| fusealtimeter |更新融合滤波器的方法%高度计的输出。fusealtimeter (fusionfilt altHeight altimeterNoise);结束%记录实际的方向和位置[actP (ii), actQ (ii)] =姿势(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-axis传感器融合的高度和方向。
