Matlab Sensor Fusion and Tracking Toolbox学习笔记（一）

Matlab Sensor Fusion and Tracking Toolbox学习笔记（一）

此次学习该工具箱中惯性传感器融合（Inertial Sensor Fusion）中的ahrsfilter Function。

一、ahrsfilter简介

        ahrsfilter是一个系统对象，其功能是利用加速度计，磁力计和陀螺仪的传感器数据来估计设备的姿态。

二、如何使用ahrsfilter函数

1、使用时要创建一个ahrsfilter对象fuse。此时可以设置对象属性，语法结构如下：

fuse = ahrsfilter('SampleRate',Fs,'DecimationFactor',decim);

        其中，'SampleRate’和’DecimationFactor’是对象的属性，分别代表采样率和抽取因子。而Fs和decim是这俩属性对应的值。
2、然后像使用函数一样使用这个对象fuse。

q = fuse(accelerometerReadings,gyroscopeReadings,magnetometerReadings);

        函数的输入为传感器主体坐标系中的加速度计读数（以 m/s 2 为单位），陀螺仪读数（以 rad/s 为单位），磁力计读数（以 µT 为单位），指定为N × 3 矩阵。 N是样本数，三列值分别代表各传感器在x y z 方向上的测量值。三个输入变量如下图。

        输出为1x1quaternion,即四元数

        展开为：

3、最后使用eulerd函数将四元数转换为欧拉角表示，然后画图。

三、整体代码

load 'rpy_9axis' sensorData Fs   %将文件变量加载到工作区中
accelerometerReadings = sensorData.Acceleration;
gyroscopeReadings = sensorData.AngularVelocity;
magnetometerReadings = sensorData.MagneticField;

decim = 2;       %指定抽取因子为 2 以降低算法的计算成本。
fuse = ahrsfilter('SampleRate',Fs,'DecimationFactor',decim);%创建一个ahrsfilter对象
%SampleRate设置为传感器数据的采样率。
q = fuse(accelerometerReadings,gyroscopeReadings,magnetometerReadings);
%将加速度计读数、陀螺仪读数和磁力计读数传递给ahrsfilter对象fuse，
以输出传感器身体方向随时间变化的估计值。默认情况下，方向输出为四元数向量。
time = (0:decim:size(accelerometerReadings,1)-1)/Fs;
plot(time,eulerd(q,'ZYX','point'))
title('Orientation Estimate')
legend('z-axis', 'y-axis', 'x-axis')
ylabel('Rotation (degrees)')

将上述代码在matlab中运行，可得如下图