取向

一个物体的方向描述了它在三维空间中相对于某个坐标系(有时称为父坐标系)的旋转。

对于以下算法，使用的固定父坐标系为东北向下（NED）.NED有时被称为全球坐标系或参考系。在NED参考系中，X轴指向北方，Y轴指向东方，Z轴指向下方。NED的X-Y平面被视为地球的局部切面。根据算法，北方可能是磁北或真北。本例中的算法使用磁北。

如果指定，以下算法可以估计相对于East-North-Up (ENU)父坐标系的方向，而不是NED。

一个对象可以被认为有它自己的坐标系统，通常称为局部或子坐标系统。这个子坐标系与对象相对于父坐标系旋转。如果没有平移，两个坐标系的原点就会重叠。

计算的方向量是一个从父参照系到子参照系的旋转量。旋转用四元数或旋转矩阵表示。

传感器类型

为了估计方位，通常使用三种类型的传感器:加速度计、陀螺仪和磁力计。加速度计测量适当的加速度。陀螺仪测量角速度。磁力计测量局部磁场。不同的算法用于融合不同的传感器组合来估计方向。

传感器数据

在本例的大部分情况下，使用了相同的传感器数据集。当设备围绕三个不同的轴旋转时，记录了加速计、陀螺仪和磁强计传感器数据：首先围绕其局部Y轴，然后围绕其Z轴，最后围绕其X轴。设备的X轴通常在旋转期间指向南方实验

ld =负载(“rpy_9axis.mat”）;acc = ld.sensorData.Acceleration;陀螺= ld.sensorData.AngularVelocity;杂志= ld.sensorData.MagneticField;页= poseplot;

加速度计磁强计融合

的生态公司功能熔断加速度计和磁力计数据。这是一个不需要参数调整的无记忆算法，但该算法非常容易受到传感器噪声的影响。

Qe =罗盘(acc, mag);为2 = 1:尺寸(acc, 1)集(pp、“定位”，qe（ii））现时限制终止

请注意,生态公司算法正确地找到了北方的位置。然而，由于函数是无记忆的，估计的运动不是平滑的。该算法可以作为一个初始化步骤的方向滤波器或一些技术中提出的使用四元数SLERP的低通滤波器定位可以用来平滑运动。

Accelerometer-Gyroscope融合

下列物体使用误差状态卡尔曼滤波器或互补滤波器估计方向。误差状态卡尔曼滤波器是标准估计滤波器，允许使用相应的噪声参数对系统的许多不同方面进行调整。互补滤波器可替代th内存限制，并且具有最小的可调参数，这允许以更精细的调整为代价进行更轻松的配置。

的imufilter和complementaryFilter系统对象™ 融合加速度计和陀螺仪数据imufilter使用内部错误状态卡尔曼滤波器和complementaryFilter使用互补滤波器。滤波器能够消除陀螺仪随时间漂移的偏置噪声。

ifilt = imufilter (“SampleRate”, ld.Fs);为2 = 1:尺寸(acc, 1) qimu = ifilt (acc (ii):),陀螺(ii):));集(pp、“定位”qimu) drawnow限制终止

%禁用磁强计输入。cfilt = complementaryFilter (“SampleRate”ld.Fs,“哈斯磁强计”，假）；为2 = 1:尺寸(acc, 1) qimu = cfilt (acc (ii):),陀螺(ii):));集(pp、“定位”qimu) drawnow限制终止

虽然imufilter和complementaryFilter与传统算法相比，算法生成的运动估计要平滑得多生态公司，他们不能正确地估计北方的方向。的imufilter不处理磁力计数据，所以它只是假设设备的x轴最初指向北方。给出的运动估计imufilter相对于初始估计方向。的complementaryFilter做同样的假设HasMagnetometer属性设置为错误的．

加速度计陀螺仪磁强计融合

姿态和航向参考系统（AHRS）由一个9轴系统组成，该系统使用加速计、陀螺仪和磁强计来计算方位ahrsfilter和complementaryFilter系统对象™结合了最好的以前的算法，以产生一个平滑变化的估计设备的方向，同时正确估计北方的方向。的complementaryFilter使用与之前相同的互补滤波算法，增加一个步骤，包括磁强计和改进方向估计。就像imufilter，ahrsfilter算法还使用了错误状态卡尔曼滤波器。除了陀螺仪偏压消除，ahrsfilter具有一定的抗干扰能力。

ifilt = ahrsfilter (“SampleRate”, ld.Fs);为2 = 1:尺寸(acc, 1) qahrs = ifilt (acc (ii):),陀螺(ii):),镁(ii):));集(pp、“定位”，qahrs）现在开始限制终止

cfilt = complementaryFilter (“SampleRate”, ld.Fs);为2 = 1:尺寸(acc, 1) qahrs = cfilt (acc (ii):),陀螺(ii):),镁(ii):));集(pp、“定位”，qahrs）现在开始限制终止

优化滤波器参数

的complementaryFilter，imufilter,ahrsfilter系统对象™都有可调参数。根据所使用的指定传感器调整参数可以提高性能。

的complementaryFilter参数AccelerometerGain和MagnetometerGain可以调整，以改变每个传感器的测量影响方向估计的量。当AccelerometerGain被设置为0，只有陀螺仪用于x轴和y轴方向。当AccelerometerGain被设置为1，只有加速计用于x轴和y轴方向。当MagnetometerGain被设置为0，只有陀螺仪用于z轴定向。当MagnetometerGain被设置为1，只有磁力计用于z轴定位。

的ahrsfilter和imufilter系统对象™有更多的参数，可以让过滤器更紧密地匹配特定的硬件传感器。传感器的环境也是需要考虑的重要因素。的imufilter参数是ahrsfilter参数。的加速度计噪声，陀螺噪声，磁强计噪声,GyroscopeDriftNoise是测量噪声。传感器的数据表有助于确定这些值。

的LinearAccelerationNoise和LinearAccelerationDecayFactor控制滤波器对线性（平移）加速度的响应。晃动设备是添加线性加速度的一个简单示例。

考虑一下如何imufilter与一个LinearAccelerationNoise第9e-3章 $$（米/{\mathrm{年代}}^2{）}^2$$对震动轨迹作出响应，而不是LinearAccelerationNoise第9e-4节 $$（米/{\mathrm{年代}}^2{）}^2$$．

ld =负载(“shakingDevice.mat”); 加速度=ld.sensorData.Acceleration；陀螺=ld.sensorData.AngularVelocity；highVarFilt=imufilter(“SampleRate”ld.Fs,．..“线性加速度噪声”, 0.009);qHighLANoise = highVarFilt(accel，陀螺仪);lowVarFilt = imufilter (“SampleRate”ld.Fs,．..“线性加速度噪声”, 0.0009);qLowLANoise = lowVarFilt(accel，陀螺仪);

一种查看LinearAccelerationNoise就是看输出的重力向量，重力向量就是方向旋转矩阵的第三列。

rmatHigh=rotmat（qHighLANoise，“框架”); rmatLow=rotmat（qLowLANoise，“框架”）;gravDistHigh =√sum (rmatHigh(:, 3:)[0, 0, 1])。^ 2,1));gravDistLow =√sum (rmatLow(:, 3:)[0, 0, 1])。^ 2,1));图;情节([挤压(gravDistHigh),挤压(gravDistLow)]);标题(“欧几里德引力距离”）;传奇(“LinearAccelerationNoise = 0.009”，．..“线性加速度噪声=0.0009”）;

的lowVarFilt有一个低的LinearAccelerationNoise，所以它期望处于一个低线性加速度的环境中。因此，它更容易受到线性加速度的影响，正如图中前面的大变化所示。然而，由于它预期处于一个低线性加速度的环境中，加速度计信号的可信度较高。因此，一旦震动结束，方位估计会迅速收敛到垂直方向。反之亦然highVarFilt。滤波器受震动的影响较小，但当震动停止时，方向估计需要更长时间才能收敛到垂直方向。

的磁干扰噪声属性可以以几乎相同的方式对磁扰动（非地磁噪声源）进行建模LinearAccelerationNoise模型线性加速度。

两个衰减因子的性质(磁扰动衰变因子和LinearAccelerationDecayFactor)模拟噪音的变化率。对于缓慢变化的噪声源，将这些参数设置为接近1的值。对于快速变化的、不相关的噪声，将这些参数设置为接近0。一个较低的LinearAccelerationDecayFactor使方向估计更快速地找到“下”。一个较低的磁扰动衰变因子使方向估计更快地找到北。

非常大的、短的磁干扰几乎完全被ahrsfilter．考虑当从一个固定的传感器记录时施加一个[0 250]uT的脉冲。理想情况下，方向估计应该没有变化。

ld =负载(“magJamming.mat”）;hpulse = ahrsfilter (“SampleRate”，ld.Fs）；len=1:10000；qpulse=HPpulse（ld.传感器数据.加速度（len，：），．..ld.sensorData.AngularVelocity (len:)．..ld.sensorData.MagneticField (len:));图;timevec = 0:ld.Fs:(ld.Fs*numel(qpulse) - 1);情节(timevec eulerd (qpulse“ZYX股票”，“框架”) ); 头衔([静止轨迹方向欧拉角换行符．..磁干扰响应的]); 传奇(“Z-rotation”，“Y旋转”，“X-rotation”）;ylabel (“度”）;包含(“秒”）;

请注意，滤波器几乎完全排除这种磁脉冲的干扰。任何磁场强度大于4倍的ExpectedMagneticFieldStrength被视为干扰源，这些样本的磁强计信号被忽略。

结论

这里提出的算法，当被适当调谐时，能够估计方向，并且对环境噪声源具有鲁棒性。考虑传感器使用的情况并相应地调整滤波器是很重要的。