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

打开脚本

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

连接硬件

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

Sda - a4
SCL - a5
VCC - + 3.3v
GND - GND

检查传感器连接是否完好。建议将传感器附加/连接到原型屏蔽上，以避免传感器在运动时连接松动。参考的传感器故障诊断页以调试传感器相关的问题。

创建传感器对象

创建一个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;runTime = 20;isVerbose = false;useHW = true;如果useHW a = arduino;Imu = mpu9250(a，“SampleRate”Fs,“OutputFormat”，“矩阵”，.．.“SamplesPerRead”, samplesPerRead);其他的负载(“loggedMPU9250Data.mat”，“allAccel”，“allGyro”，“allMag”，.．.“allT”，“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;用于对齐传感器数据轴的辅助函数。alignAccelAxes = @(in) [accelXAxisSign, accelYAxisSign, accelZAxisSign].．..* in(:， [accelXAxisIndex, accelYAxisIndex, 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 with properties: SampleRate: 100 AccelerometerGain: 0.0100 MagnetometerGain: 0.0100 HasMagnetometer: 1 OrientationFormat: 'quaternion'

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

设置HasMagnetometer财产假禁用磁强计测量输入。在此模式下，滤波器仅以加速度计和陀螺仪测量值作为输入。同样，过滤器假定IMU的初始方向与父导航框架对齐。如果初始时IMU未与导航帧对齐，则方向估计中会有一个常数偏移。

compFilt =互补过滤器(“HasMagnetometer”、假);tuner = helporientationfiltertuner (compFilt);如果useHW抽搐其他的idx = 1:samplesPerRead;overrunIdx = 1;结束而真正的如果useHW[加速度，陀螺，mag, t，溢出]= imu();accel = alignAccelAxes(accel);陀螺= alignGyroAxes(陀螺);其他的accel = allAccel(idx，:);陀螺= all陀螺(idx，:);mag = allMag(idx，:);t = allT(idx，:);溢出= oververrun (overrunIdx，:);idx = idx + samplesPerRead;overrunIdx = overrunIdx + 1;暂停(samplesPerRead / Fs)结束如果(isVerbose &&溢出> 0)fprintf('%d样本溢出…\n'、泛滥);结束q = compFilt(加速，陀螺);更新(调谐器,q);如果useHW如果toc >= runTime打破；结束其他的如果idx(end) > numSamplesAccelGyro . idx(end打破；结束结束结束

估计方向与加速度计，陀螺仪，和磁力计

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

compFilt =互补过滤器(“SampleRate”Fs);tuner = helporientationfiltertuner (compFilt);如果useHW抽搐结束而真正的如果useHW[加速度，陀螺，mag, t，溢出]= imu();accel = alignAccelAxes(accel);陀螺= alignGyroAxes(陀螺);mag = alignMagAxes(mag);其他的accel = allAccel(idx，:);陀螺= all陀螺(idx，:);mag = allMag(idx，:);t = allT(idx，:);溢出= oververrun (overrunIdx，:);idx = idx + samplesPerRead;overrunIdx = overrunIdx + 1;暂停(samplesPerRead / Fs)结束如果(isVerbose &&溢出> 0)fprintf('%d样本溢出…\n'、泛滥);结束q = compFilt(accel，陀螺，mag);更新(调谐器,q);如果useHW如果toc >= runTime打破；结束其他的如果idx(end) > numSamplesAccelGyroMag . idx(end打破；结束结束结束

总结

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