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

开放脚本

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

连接硬件

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

SDA-A4
症状自评量表-A5
VCC-+3.3V
GND-GND

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

创建传感器对象

创建一个阿杜伊诺对象和一个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；样本数=10；运行时间=20；isVerbose=false；useHW=真；如果useHW a=arduino；imu=mpu9250（a，“SampleRate”Fs,“OutputFormat”，“矩阵”，.．.“SamplesPerRead”，samplesPerRead，“流媒体”,真正的);其他的负载(“loggedMPU9250Data.mat”，“阿拉塞尔”，“allGyro”，“allMag”，.．.“好的”，“allOverrun”，.．.“numSamplesAccelGyro”，“numSamplesAccelGyroMag”）结束

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

MPU-9250中的加速计、陀螺仪和磁强计的轴彼此不对齐。请指定每个传感器的索引和符号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;%辅助功能用于对齐传感器数据轴。AlignAccelAxis=@（in）[accelXAxisSign，accelYAxisSign，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=互补过滤器(“SampleRate”Fs)

compFilt=complementaryFilter，属性为：SampleRate:100加速度计组织：0.0100磁强计组织：0.0100 HasMagnetometer:1方向格式：“四元数”

利用加速度计和陀螺仪估计方位

设置HasMagnetometer财产假禁用磁强计测量输入。在此模式下，过滤器仅将加速计和陀螺仪测量值作为输入。此外，过滤器假设IMU的初始方向与主导航框架对齐。如果IMU最初未与导航框架对齐，则方向中会有恒定的偏移估计误差。

compFilt=互补过滤器(“哈斯磁强计”，假）；调谐器=HelperOrientationFilterTuner（compFilt）；如果useHW tic其他的idx = 1: samplesPerRead;overrunIdx = 1;结束虽然符合事实的如果useHW[加速度、陀螺仪、磁力、t、超速]=imu（）；加速度=对准加速度轴（加速度）；陀螺=对准陀螺轴（陀螺）；其他的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=互补过滤器(“SampleRate”Fs);调谐器= HelperOrientationFilterTuner (compFilt);如果useHW tic结束虽然符合事实的如果使用HW[accel，gyro，mag，t，Overward]=imu（）；accel=alignAccelAxes（accel）；gyro=alignGyroAxes（gyro）；mag=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，并讨论了调整互补滤波器参数的影响。