用互补滤波器和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;如果ushw 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轴，使传感器在静止时与NED (North-East-Down)坐标系统对齐。在这个例子中，磁强计轴改变了，而加速度计和陀螺仪轴保持固定。对于您自己的应用程序，根据需要更改以下参数。

%加速度计轴参数。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, accelYAxisIndex, accelZAxisIndex]);alignGyroAxes = @(in) [gyroXAxisSign, gyroYAxisSign, gyroZAxisSign].．..* in(:， [gyroXAxisIndex, gyroYAxisIndex, gyroZAxisIndex]);alignmagaxis = @(in) [magXAxisSign, magYAxisSign, magZAxisSign].．..* in(:， [magXAxisIndex, magYAxisIndex, magZAxisIndex]);

进行额外传感器校准

必要时，可以校准磁力仪以补偿磁失真。有关更多详情，请参阅《补偿硬铁变形》的章节使用惯性传感器融合和MPU-9250估计方向的例子。

指定互补滤波器参数

的complementaryFilter有两个可调参数。的AccelerometerGain参数决定加速度计测量相对于陀螺仪测量的可信度。的MagnetometerGain参数决定磁力计测量比陀螺仪测量可信多少。

compFilt = complementaryFilter (“SampleRate”Fs)

comfilt =互补aryfilter 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[加速度，陀螺，mag, t, overrun] = imu();accel = alignAccelAxes (accel);陀螺= alignGyroAxes(陀螺);其他的accel = allAccel (idx:);陀螺= allGyro (idx:);杂志= allMag (idx:);t = allT (idx:);泛滥= allOverrun (overrunIdx:);idx = idx + samplesPerRead;涨跌=涨跌+ 1;暂停(samplesPerRead / Fs)结束如果(isVerbose && overrun > 0)'%d samples overrun…\n'、泛滥);结束q = compFilt(加速度，陀螺);更新(调谐器,q);如果useHW如果toc > =运行时打破；结束其他的如果idx(结束)> numSamplesAccelGyro打破；结束结束结束

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

默认值为AccelerometerGain和MagnetometerGain，过滤器在短期内更相信陀螺仪的测量，但在长期内更相信加速度计和磁力计的测量。这使得过滤器能够对快速的方向变化做出更大的反应，并防止方向估计在较长时间内漂移。对于特定的IMU传感器和应用目的，您可能需要调整滤波器的参数来提高方向估计的精度。

compFilt = complementaryFilter (“SampleRate”Fs);调谐器= HelperOrientationFilterTuner (compFilt);如果useHW抽搐结束而真正的如果useHW[加速度，陀螺，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;涨跌=涨跌+ 1;暂停(samplesPerRead / Fs)结束如果(isVerbose && overrun > 0)'%d samples overrun…\n'、泛滥);结束q = compFilt(accel, gyro, mag);更新(调谐器,q);如果useHW如果toc > =运行时打破；结束其他的如果idx(结束)> numSamplesAccelGyroMag打破；结束结束结束

总结

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