用互补滤波器和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坐标对齐

mpu9250中的加速度计、陀螺仪和磁强计的轴线没有对齐。指定每个传感器的索引和符号x、y和z轴，以便传感器在静止时与东北-下(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, accelXAxisIndex, accelZAxisIndex]);alignGyroAxes = @(in) [gyroXAxisSign, gyroYAxisSign, gyroZAxisSign]．...* in(:， [gyroXAxisIndex, gyroYAxisIndex, gyroZAxisIndex]);alignMagAxes = @(in) [magXAxisSign, magYAxisSign, magZAxisSign]．...* 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: '四元数'

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

设置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 &&溢出> 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 &&溢出> 0)'%d样本溢出…\n'、泛滥);结束q = compFilt(accel, gyro, mag);更新(调谐器,q);如果useHW如果toc > =运行时打破；结束其他的如果idx(结束)> numSamplesAccelGyroMag打破；结束结束结束

总结

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