示例演示如何融合来自 3 轴加速度计、3 轴陀螺仪、3 轴磁力计（通常称为磁、角速率和重力的 MARG 传感器）和 1 轴高度计的数据，以估计方向和高度。

一、模拟设置

此仿真以多种速率处理传感器数据。IMU（加速度计和陀螺仪）通常以最高速率运行。磁力计通常以低于 IMU 的速率运行，高度计以最低速率运行。更改采样率会导致融合算法的某些部分运行更频繁，并可能影响性能。

% Set the sampling rate for IMU sensors, magnetometer, and altimeter.
imuFs = 100;
altFs = 10;
magFs = 25;
imuSamplesPerAlt = fix(imuFs/altFs);
imuSamplesPerMag = fix(imuFs/magFs);% Set the number of samples to simulate.
N = 1000; % Construct object for other helper functions.
hfunc = Helper10AxisFusion;

二、定义轨迹

传感器主体围绕所有三个轴旋转，同时垂直摆动位置。随着仿真的继续，振荡的幅度会增加。

% Define the initial state of the sensor body
initPos = [0, 0, 0];       % initial position (m)
initVel = [0, 0, -1];      % initial linear velocity (m/s)
initOrient = ones(1, 'quaternion'); % Define the constant angular velocity for rotating the sensor body
% (rad/s).
angVel = [0.34 0.2 0.045];  % Define the acceleration required for simple oscillating motion of the
% sensor body.
fc = 0.2;
t = 0:1/imuFs:(N-1)/imuFs;
a = 1;
oscMotionAcc = sin(2*pi*fc*t);
oscMotionAcc = hfunc.growAmplitude(oscMotionAcc);% Construct the trajectory object
traj = kinematicTrajectory('SampleRate', imuFs, ...'Velocity', initVel, ...'Position', initPos, ...'Orientation', initOrient);

三、传感器配置

加速度计、陀螺仪和磁力计使用进行模拟。高度计是使用 .传感器配置中使用的值对应于实际MEMS传感器值。

imu = imuSensor('accel-gyro-mag', 'SampleRate', imuFs);% Accelerometer
imu.Accelerometer.MeasurementRange =  19.6133;
imu.Accelerometer.Resolution = 0.0023928;
imu.Accelerometer.ConstantBias = 0.19;
imu.Accelerometer.NoiseDensity = 0.0012356;% Gyroscope
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);% Magnetometer
imu.Magnetometer.MeasurementRange = 1000;
imu.Magnetometer.Resolution = 0.1;
imu.Magnetometer.ConstantBias = 100;
imu.Magnetometer.NoiseDensity = 0.3/sqrt(50);% altimeter
altimeter = altimeterSensor('UpdateRate', altFs, 'NoiseDensity', 2*0.1549);

四、融合过滤器

fusionfilt = ahrs10filter;
fusionfilt.IMUSampleRate = imuFs;

设置融合过滤器的初始值。

initstate = zeros(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.MagneticField;
initstate(16:18) = imu.Magnetometer.ConstantBias;
fusionfilt.State = initstate;

初始化融合滤波器的状态协方差矩阵。基本事实用于初始状态，因此估计值应该几乎没有误差。

icv = diag([1e-8*[1 1 1 1 1 1 1], 1e-3*ones(1,11)]);
fusionfilt.StateCovariance = icv;

磁力计和高度计测量噪声是由内部卡尔曼滤波器中使用的与传感器相关的观测噪声。这些值通常来自传感器数据表。

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 = false;

六、模拟循环

q = initOrient;
firstTime = true;actQ = zeros(N,1, 'quaternion');
expQ = zeros(N,1, 'quaternion');
actP = zeros(N,1);
expP = zeros(N,1);for ii = 1: N% Generate a new set of samples from the trajectory generator accBody = rotateframe(q, [0 0 +oscMotionAcc(ii)]);omgBody = rotateframe(q, angVel);[pos, q, vel, acc] = traj(accBody, omgBody);% Feed the current position and orientation to the imuSensor object[accel, gyro, mag] = imu(acc, omgBody, q);fusionfilt.predict(accel, gyro);% Fuse magnetometer samples at the magnetometer sample rateif ~mod(ii,imuSamplesPerMag)fusemag(fusionfilt, mag, magNoise);end% Sample and fuse the altimeter output at the altimeter sample rateif ~mod(ii,imuSamplesPerAlt)altHeight = altimeter(pos);% Use the |fusealtimeter| method to update the fusion filter with% the altimeter output.fusealtimeter(fusionfilt,altHeight,altimeterNoise);end% Log the actual orientation and position[actP(ii), actQ(ii)] = pose(fusionfilt);% Log the expected orientation and positionexpQ(ii) = q;expP(ii) = pos(3);if usePoseViewerhfunc.view(actP(ii), actQ(ii),expP(ii), expQ(ii)); %#ok<*UNRCH>endend

七、绘图过滤器性能

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

hfunc.plotErrs(actP, actQ, expP, expQ);

此示例说明如何针对高度和方向执行 10 轴传感器融合。

八、程序

使用Matlab R2022b版本，点击打开。

打开下面的“IMUandAltimeterExample.mlx”文件，点击运行，就可以看到上述效果。

关注下面公众号，点击文章《基于Matlab使用 IMU、磁力计和高度计估计方向和高度》，获取下载链接。

基于Matlab使用 IMU、磁力计和高度计估计方向和高度（附源码）相关推荐

基于Matlab使用 System 对象模拟惯性测量单元测量仿真（附源码）
目录一.默认参数二.硬件参数调优三. 随机噪声参数调谐四.环境参数调整五.程序此示例演示如何使用 System 对象模拟惯性测量单元 (IMU) 测量.IMU 可以包括单个传感器的组合,包 ...
基于Matlab生成并可视化多架飞机轨迹仿真（附源码）
目录一.介绍二.插值器选择三.航点建设四.场景生成 4.1 轨迹可视化 4.2 轨迹 1 4.3 轨迹 2 4.4 轨迹 3 4.5 轨迹 4 4.6 轨迹 5 4.7 轨迹 6 五 ...
基于Matlab搭建单站雷达扫描天空仿真平台（附源码）
目录一.介绍二.启动跟踪方案设计器三.编辑和修改方案四.设计塔式平台五.模拟场景 5.1播放控件和时间滚动条 5.2 导出到矩阵六.运行生成的脚本 6.1 可视化目标和检测 6.2 可视化 ...
基于Matlab多平台雷达网络生成雷达检测仿真（附源码）
目录一.带旋转雷达阵列的机载平台二.带有两个雷达阵列的机载平台三.带有矩形雷达阵列的地面平台四.机载目标五.雷达探测的生成六.总结七.程序此示例演示如何从多平台雷达网络生成雷达检测.该 ...
java计算机毕业设计ssm基于Vue的校园电脑租赁系统设计与开发19xy6（附源码、数据库）
java计算机毕业设计ssm基于Vue的校园电脑租赁系统设计与开发19xy6(附源码.数据库) 项目运行环境配置: Jdk1.8 + Tomcat8.5 + Mysql + HBuilderX(We ...
毕业设计-基于SSM框架大学教务管理平台项目开发实战教程(附源码)
文章目录 1.项目简介 2.项目收获 3.项目技术栈 4.测试账号 5.项目部分截图 6.常见问题毕业设计-基于SSM框架大学教务管理平台项目实战教程-附源码课程源码下载地址:https://do ...
基于Spring+SpringMVC+Mybatis的分布式敏捷开发系统架构（附源码）
点击上方好好学java ,选择星标公众号重磅资讯,干货,第一时间送达今日推荐:推荐19个github超牛逼项目!个人原创100W +访问量博客:点击前往,查看更多作者:zheng gitee ...
基于Vision Transformer的图像去雾算法研究与实现(附源码)
基于Vision Transformer的图像去雾算法研究与实现 0. 服务器性能简单监控 \LOG_USE_CPU_MEMORY\文件夹下的use_memory.py文件可以实时输出CPU使用率以及 ...
基于Java开发一套完整的区块链系统（附源码）
来源:https://blog.csdn.net/victory_long 前言近几年区块链概念越来越火,特别是区块链技术被纳入国家基础设施建设名单后,各大企业也开始招兵买马,对区块链技术进行研究, ...

基于Matlab使用 IMU、磁力计和高度计估计方向和高度（附源码）