IMU 速度估计

Question

你知道我在哪里可以找到根据 IMU（acc+陀螺仪+磁力计）数据进行速度估算的代码或示例。我根据 IMU 静止不动的数据计算偏差。我想用某种过滤器（Kalman/Complementary）实现速度估计，但我找不到。我也有相机速度估计，也许它可以帮助某种融合？ 先感谢您！ 亲切的问候

Answer 1

我没有完全适合您的案例的示例代码。但是这种方法可以帮助（根据过去的经验），

卡尔曼滤波器：

1. 确定并制定状态 X、控制输入 U、输出、预测和观察方程。
2. 实施/重用卡尔曼滤波器的一些实施。这是一个基于 Simulink 的 implementation 以供参考。
3. 设置测量噪声和预测误差方差。稍后可能需要进行一些微调。
4. 验证 KF 是否适用于某些参考。如果您有其他测量速度的方法，请对照它检查 KF 速度。

状态和控制输入：

状态可以是包含

的数组

1. 线速度 [Vx、Vy、Vz]
2. Angular 速度 [omega_x, omega_y, omega_z]
3. 陀螺仪偏差。这种偏差在很大程度上是恒定的，但会随着温度和其他因素而变化。 KF 将使用加速度计测量来校正陀螺仪偏差。
4. 加速度计偏差。这种偏差在很大程度上是恒定的，但会随着温度和其他因素而变化。 KF 将使用相机速度来校正加速度偏差。
5. 方向（欧拉角或四元数）

控制输入不必是发送到执行器的实际命令。 在这种情况下，控制输入可以是净力或净加速度，即

加速度计数据（比力）+重力加速度

预测方程：

预测方程根据当前状态和控制输入预测下一时间步长的状态。

这个MathWorks documentation对于IMU相关的预测方程有很好的参考

Observation/measurement 型号：

将测量值与状态相关联。

Accel 数据已用于预测。这里忽略。

陀螺仪数据为 [gx, gy, gz] = [omega_x + gyro_bias_x, ....] + errors

处理磁力计的一种方法是从中获取偏航角 - arctan(y/x)，然后使用 yaw_mag 作为测量值。

相机数据为[vx_cam、vy_cam、vz_cam] = [Vx, Vy, Vx] + errors

最后追加所有行并将其带入 Y=C*X + 噪声形式。

Y 表示来自不同传感器的测量值，X 表示状态。

在这种情况下，

Y 将是 [gx, gy, gz, yaw_mag, vx,cam, vy_cam, vz_cam]。

免责声明：我是 MathWorks 员工，链接来自 MathWorks 文档。