使用互补滤波器和IMU数据估计方向

打开脚本

这个例子展示了如何从Arduino流IMU数据，并使用互补滤波器估计方向。

连接硬件

将MPU-9250传感器的SDA、SCL、GND、VCC引脚连接到Arduino®硬件相应的引脚。本例使用Arduino®Uno板，连接如下:

SDA - A4
sci - A5
VCC - + 3.3 v
接地,接地

确保传感器连接完好。建议将传感器连接到原型屏蔽上，以避免传感器运动时连接松动。参考的传感器故障诊断页面调试传感器相关问题。

创建传感器对象

创建一个arduino对象和一个mpu9250对象。指定传感器采样率Fs以及运行循环所需的时间。可选地,使isVerbose标记检查是否有样品溢位。通过禁用的useHW标记后，还可以使用保存在mat文件中的传感器数据运行示例loggedMPU9250Data.mat。

中的数据loggedMPU9250Data.mat在IMU通常面向正南时记录，然后旋转:

绕z轴+90度
绕z轴-180度
绕z轴+90度
绕y轴+90度
绕y轴-180度
绕y轴+90度
绕x轴+90度
绕x轴-270度
绕x轴+180度

注意，绕x轴的最后两个旋转是一个额外的90度。这是为了将设备上下颠倒。IMU的最终方向与初始方向相同，正南。

Fs = 100;samplesPerRead = 10;运行时= 20;isVerbose = false;useHW = true;如果useHW a = arduino;imu = mpu9250 (,“SampleRate”Fs,“OutputFormat”,“矩阵”,…“SamplesPerRead”samplesPerRead,“流”,真正的);其他的负载(“loggedMPU9250Data.mat”,“allAccel”,“allGyro”,“allMag”,…“allT”,“allOverrun”,…“numSamplesAccelGyro”,“numSamplesAccelGyroMag”)结束

将MPU-9250传感器轴线与NED坐标对齐

MPU-9250的加速度计、陀螺仪和磁力计轴线没有对齐。指定每个传感器的x轴、y轴和z轴的索引和符号，以便传感器在静止时与North-East-Down (NED)坐标系对齐。在这个例子中，磁强计的轴被改变，而加速度计和陀螺仪的轴保持固定。对于您自己的应用程序，请根据需要更改以下参数。

%加速度计轴参数。accelXAxisIndex = 1;accelXAxisSign = 1;accelYAxisIndex = 2;accelYAxisSign = 1;accelZAxisIndex = 3;accelZAxisSign = 1;%陀螺仪轴参数。gyroXAxisIndex = 1;gyroXAxisSign = 1;gyroYAxisIndex = 2;gyroYAxisSign = 1;gyroZAxisIndex = 3;gyroZAxisSign = 1;%磁强计轴参数。magXAxisIndex = 2;magXAxisSign = 1;magYAxisIndex = 1;magYAxisSign = 1;magZAxisIndex = 3;magZAxisSign = 1;帮助器函数用于对齐传感器数据轴。alignAccelAxes = @(in) [accelXAxisSign, accelZAxisSign, accelZAxisSign]….* in(:， [accelXAxisIndex, accelYAxisIndex, accelZAxisIndex]);alignGyroAxes = @(in) [gyroXAxisSign, gyroYAxisSign, gyroZAxisSign]….* in(:， [gyroXAxisIndex, gyroYAxisIndex, gyroZAxisIndex]);= @(in) [/ i] [/ i] [/ i] [/ i] [/ i]….* in(:， [magXAxisIndex, magYAxisIndex, magZAxisIndex]);

进行额外的传感器校准

如有必要，可校正磁强计以补偿磁畸变。更多详细信息，请参见补偿硬铁扭曲部分基于惯性传感器融合和MPU-9250的定位估计的例子。

指定互补过滤器参数

的complementaryFilter有两个可调参数。的AccelerometerGain参数决定了加速度计测量比陀螺仪测量更可靠。的MagnetometerGain参数决定了磁强计测量在陀螺仪测量上的可信度。

compFilt = complementaryFilter (“SampleRate”Fs)

compFilt = complementaryFilter with properties: SampleRate: 100 AccelerometerGain: 0.0100 MagnetometerGain: 0.0100 HasMagnetometer: 1 OrientationFormat: 'quaternion'

用加速度计和陀螺仪估计方向

设置HasMagnetometer财产假禁用磁力计测量输入。在这种模式下，滤波器只将加速度计和陀螺仪的测量值作为输入。此外，该过滤器假定IMU的初始方向与父导航框架对齐。如果IMU最初没有与导航帧对齐，则在方向估计中会有恒定的偏移量。

compFilt = complementaryFilter (“HasMagnetometer”、假);调谐器= HelperOrientationFilterTuner (compFilt);如果useHW抽搐其他的idx = 1: samplesPerRead;overrunIdx = 1;结束而真正的如果useHW [accel, gyro, mag, t, overrun] = imu();accel = alignAccelAxes (accel);陀螺= alignGyroAxes(陀螺);其他的accel = allAccel (idx:);陀螺= allGyro (idx:);杂志= allMag (idx:);t = allT (idx:);泛滥= allOverrun (overrunIdx:);idx = idx + samplesPerRead;overrunIdx = overrunIdx + 1;暂停(samplesPerRead / Fs)结束如果(isVerbose && overrun > 0)'%d样本溢出…\n'、泛滥);结束q = compFilt(accel，陀螺);更新(调谐器,q);如果useHW如果toc > =运行时打破;结束其他的如果idx(结束)> numSamplesAccelGyro打破;结束结束结束

用加速度计、陀螺仪和磁力计估计方向

使用默认值AccelerometerGain和MagnetometerGain，滤波器在短期内更依赖陀螺仪测量，但在长期内更依赖加速度计和磁力计测量。这允许过滤器对快速方向变化更有反应性，并防止方向估计在更长时间内漂移。对于特定的IMU传感器和应用目的，您可能希望调整滤波器的参数以提高方向估计精度。

compFilt = complementaryFilter (“SampleRate”Fs);调谐器= HelperOrientationFilterTuner (compFilt);如果useHW抽搐结束而真正的如果useHW [accel, gyro, mag, t, overrun] = imu();accel = alignAccelAxes (accel);陀螺= alignGyroAxes(陀螺);杂志= alignMagAxes (mag);其他的accel = allAccel (idx:);陀螺= allGyro (idx:);杂志= allMag (idx:);t = allT (idx:);泛滥= allOverrun (overrunIdx:);idx = idx + samplesPerRead;overrunIdx = overrunIdx + 1;暂停(samplesPerRead / Fs)结束如果(isVerbose && overrun > 0)'%d样本溢出…\n'、泛滥);结束q = compFilt(accel, gyro, mag);更新(调谐器,q);如果useHW如果toc > =运行时打破;结束其他的如果idx(结束)> numSamplesAccelGyroMag打破;结束结束结束

总结

这个例子展示了如何使用Arduino和互补滤波器的数据估计IMU的方向。这个例子还展示了如何配置IMU，并讨论了调整互补滤波器参数的效果。