取向

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

对于下面的算法，所用的固定的，父坐标系统是北 - 东 - 向下（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.MagneticField;观察者= HelperOrientationViewer;

加速度计，磁力计融合

的电子罗盘功能融合加速度计和磁强计数据。这是一个无记忆算法，不需要参数调整，但该算法很容易受到传感器噪声的影响。

qe = ecompass(acc, mag);为2 = 1:尺寸(acc, 1)查看器(量化宽松(ii));暂停(0.01);结束

请注意,电子罗盘算法正确找到北方的位置。然而，由于函数是无记忆的，估计的运动不是平滑的。该算法可以作为方向滤波器的初始化步骤，也可以作为本文中介绍的某些技术的初始化步骤低通滤波器定向使用四元数SLERP(Sensor Fusion and Tracking Toolbox)可用于平滑运动。

Accelerometer-Gyroscope融合

以下对象估计取向使用任一个误差状态卡尔曼滤波器或互补滤波器。误差状态卡尔曼滤波器是标准估计滤波器，并允许该系统的许多不同方面使用相应的噪声参数进行调谐。互补滤波器可以用作与存储器约束系统的替代品，并具有最小的可调参数，这允许更容易地配置在更精细的调谐的成本。

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

ifilt = imufilter (“SampleRate”，ld.Fs）;为II = 1：尺寸（ACC，1）=奇木IFILT（ACC（二，:)，陀螺仪（二，:)）;查看器(qimu);暂停(0.01);结束

％禁用磁力输入。cfilt = complementaryFilter (“SampleRate”ld.Fs,“HasMagnetometer”，假）;为2 = 1:尺寸(acc, 1) qimu = cfilt (acc (ii):),陀螺(ii):));查看器(qimu);暂停(0.01);结束

虽然imufilter和complementaryFilter算法产生的运动的平滑显著估计，相对于电子罗盘他们没有正确地估计北方的方向。的imufilter不处理磁强计的数据，所以它只是简单地假设设备的x轴最初指向北方。运动估计由imufilter相对于初始估计方向。的complementaryFilter做同样的假设HasMagnetometer属性设置为假.

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

的姿态航向基准系统（AHRS）由一个使用加速度计，陀螺仪和磁强计来计算方位的9轴的系统。的ahrsfilter和complementaryFilterSystem objects™结合了之前的算法的优点，可以平滑地估计设备的方向，同时正确地估计设备的北方方向。的complementaryFilter使用与之前相同的补充滤波算法，增加了一个步骤来包括磁强计和改进方向估计。就像imufilter,ahrsfilter算法还使用了误差状态卡尔曼滤波器。除陀螺仪的偏置消除外，该ahrsfilter具有一定的检测和排除轻微电磁干扰的能力。

ifilt = ahrsfilter (“SampleRate”，ld.Fs）;为2 = 1:尺寸(acc, 1) qahrs = ifilt (acc (ii):),陀螺(ii):),镁(ii):));查看器(qahrs);暂停(0.01);结束

cfilt = complementaryFilter (“SampleRate”，ld.Fs）;为2 = 1:尺寸(acc, 1) qahrs = cfilt (acc (ii):),陀螺(ii):),镁(ii):));查看器(qahrs);暂停(0.01);结束

优化滤波器参数

的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与一个LinearAccelerationNoise的9E-3响应颤抖的轨迹，与一个有LinearAccelerationNoise的9E-4.

ld =负载(“shakingDevice.mat”);accel = ld.sensorData.Acceleration;陀螺= ld.sensorData.AngularVelocity;观众= HelperOrientationViewer;highVarFilt = imufilter (“SampleRate”ld.Fs,...'LinearAccelerationNoise',0.009);qHighLANoise = highVarFilt(accel, gyro);lowVarFilt = imufilter (“SampleRate”ld.Fs,...'LinearAccelerationNoise',0.0009);qLowLANoise = lowVarFilt(accel, gyro);

看到的效果的一种方式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”,...'LinearAccelerationNoise = 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);情节(timevec eulerd (qpulse“ZYX股票”,'帧'））;标题（[静止轨道定向欧拉角换行符...磁干扰响应的]）;传说（“Z-rotation”,“Y旋转”,“X-rotation”);ylabel (“度”);包含(“秒”);

需要注意的是过滤器几乎完全反对这种电磁脉冲的干扰。任何磁场强度大于四倍ExpectedMagneticFieldStrength被认为是一种干扰源和所述磁力计信号被忽略对那些样品。

结论

在这里，当适当调整提出的算法，使定位的估计和对抗环境噪声源的鲁棒性。考虑其中传感器的使用和调整相应的过滤器的情况下，它是很重要的。