取向

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

对于下面的算法，使用的固定的父坐标系是东北向下(NED)。NED有时被称为全局坐标系或参考系。在NED参考系中，x轴指向北，y轴指向东，z轴指向下。NED的X-Y平面被认为是地球的局部切平面。根据算法的不同，北可能是磁北或真北。本例中的算法使用磁北。

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

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

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

传感器类型

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

传感器数据

在这个示例的大部分内容中，使用了相同的传感器数据集。加速度计、陀螺仪和磁力计传感器数据被记录下来，同时设备围绕三个不同的轴旋转:首先围绕本地y轴旋转，然后围绕z轴旋转，最后围绕x轴旋转。在实验过程中，设备的x轴通常指向南方。

Ld =负载(“rpy_9axis.mat”）;acc = ld.sensorData.Acceleration;陀螺= ld.sensorData.AngularVelocity;mag = ld. sensordata .磁场;Pp = poseplot;

Accelerometer-Magnetometer融合

的ecompass功能融合加速度计和磁力计数据。这是一种不需要参数调优的无记忆算法，但该算法极易受到传感器噪声的影响。

Qe = ecompass(acc, mag);为2 = 1:尺寸(acc, 1)集(pp、“定位”， qe(ii))现在提取limitrate结束

注意ecompass算法正确找到了北方的位置。然而，由于函数是无记忆的，估计的运动是不平滑的。该算法可用于定向滤波器的初始化步骤或本文中介绍的某些技术基于四元数SLERP的低通滤波器定位可以用来平滑运动。

Accelerometer-Gyroscope融合

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

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

Ifilt = imufilter(“SampleRate”, ld.Fs);为2 = 1:尺寸(acc, 1) qimu = ifilt (acc (ii):),陀螺(ii):));集(pp、“定位”，齐木)现在画limitrate结束

禁用磁强计输入。cfilt =互补过滤器(“SampleRate”ld.Fs,“HasMagnetometer”、假);为2 = 1:尺寸(acc, 1) qimu = cfilt (acc (ii):),陀螺(ii):));集(pp、“定位”，齐木)现在画limitrate结束

虽然imufilter而且complementaryFilter算法产生的运动估计明显更平滑，与ecompass，他们不能正确地估计北方的方向。的imufilter它不处理磁力计数据，所以它简单地假设设备的x轴最初是指向北方的。给出的运动估计imufilter相对于初始估计方向。的complementaryFilter做了同样的假设HasMagnetometer属性设置为假．

Accelerometer-Gyroscope-Magnetometer融合

姿态和航向参考系统(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)现在画limitrate结束

cfilt =互补过滤器(“SampleRate”, ld.Fs);为2 = 1:尺寸(acc, 1) qahrs = cfilt (acc (ii):),陀螺(ii):),镁(ii):));集(pp、“定位”， qahrs)现在画limitrate结束

调整滤波器参数

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

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

的ahrsfilter而且imufilter系统对象™具有更多参数，可以使滤波器更紧密地匹配特定的硬件传感器。传感器的环境也是需要考虑的重要因素。的imufilter参数的子集ahrsfilter参数。的AccelerometerNoise，GyroscopeNoise，MagnetometerNoise,GyroscopeDriftNoise是测量噪声。传感器的数据表有助于确定这些值。

的LinearAccelerationNoise而且LinearAccelerationDecayFactor控制滤波器对线性(平移)加速度的响应。摇动设备是添加线性加速度的一个简单例子。

考虑一下如何imufilter与一个LinearAccelerationNoise9 e - 3 $$（米/{\mathrm{年代}}^2{）}^2$$响应一个震动轨迹，与一个与LinearAccelerationNoise9的军医 $$（米/{\mathrm{年代}}^2{）}^2$$．

Ld =负载(“shakingDevice.mat”）;加速度= ld.sensorData.Acceleration;陀螺= ld.sensorData.AngularVelocity;highVarFilt = imufilter(“SampleRate”ld.Fs,.．.“LinearAccelerationNoise”, 0.009);qHighLANoise = highVarFilt(加速，陀螺);lowVarFilt = imufilter(“SampleRate”ld.Fs,.．.“LinearAccelerationNoise”, 0.0009);qLowLANoise = lowVarFilt(加速，陀螺);

有一种方法可以看出LinearAccelerationNoise就是看输出重力矢量。重力向量就是方向旋转矩阵的第三列。

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

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

的MagneticDisturbanceNoise属性可以以大致相同的方式对磁扰动(非地磁噪声源)进行建模LinearAccelerationNoise模拟线性加速度。

两个衰减因子的特性(MagneticDisturbanceDecayFactor而且LinearAccelerationDecayFactor)建立噪声变化率的模型。对于变化缓慢的噪声源，将这些参数设置为接近1的值。对于快速变化的、不相关的噪声，将这些参数设置为接近0。一个较低的LinearAccelerationDecayFactor使方向估计能够更快地找到“向下”。一个较低的MagneticDisturbanceDecayFactor使方向估计更快地找到北方。

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

Ld =负载(“magJamming.mat”）;Hpulse = ahrsfilter(“SampleRate”, ld.Fs);Len = 1:10000;qpulse = hpulse(ld.sensorData.Acceleration(len，:))，.．.ld.sensorData.AngularVelocity (len:).．.ld.sensorData.MagneticField (len:));图;timevec = 0:ld.Fs:(ld.Fs*numel(qpulse) - 1);Plot (timevec, eulerd(qpulse，“ZYX股票”，“帧”));标题([“静止轨迹方向欧拉角”换行符.．.“磁干扰响应”]);传奇(“Z-rotation”，“Y-rotation”，“X-rotation”）;ylabel (“度”）;包含(“秒”）;

注意，滤波器几乎完全拒绝这种磁脉冲作为干扰。任何磁场强度大于四倍于ExpectedMagneticFieldStrength被认为是干扰源，磁强计信号被忽略的样本。

结论

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