你知道我在哪里可以找到根据 IMU(acc+陀螺仪+磁力计)数据估计速度的代码或示例吗?我根据 IMU 静止的数据计算了偏差。我想用某种滤波器(卡尔曼/互补)来实现速度估计,但我找不到任何滤波器。我还有相机速度估计,也许它可以作为某种融合提供帮助? 先感谢您! 亲切的问候
最佳答案
我没有完全适合您的情况的示例代码。但这种方法可以提供帮助(根据过去的经验),
卡尔曼滤波器:
- 决定并制定状态 X、控制输入 U、输出、预测和观测方程。
- 实现/重用卡尔曼滤波器的某些实现。这是一个基于 Simulink 的 implementation供引用。
- 设置测量噪声和预测误差方差。稍后可能需要一些微调。
- 验证 KF 是否针对某些引用起作用。如果您有其他方法来测量速度,请对照它检查 KF 速度。
状态和控制输入:
状态可以是一个包含以下内容的数组
- 线速度 [Vx、Vy、Vz]
- 角速度 [omega_x、omega_y、omega_z]
- 陀螺仪的偏差。这种偏差基本上是恒定的,但会随着温度和其他因素而变化。 KF 将使用加速度计测量值来校正陀螺仪偏差。
- 加速度计的偏差。这种偏差基本上是恒定的,但会随着温度和其他因素而变化。 KF 将使用相机速度来校正加速度偏差。
- 方向(欧拉角或四元数)
控制输入不必是发送到执行器的实际命令。 在这种情况下,控制输入可以是净力或净加速度,即,
加速度计数据(即比力)+重力加速度
预测方程:
预测方程根据当前状态和控制输入预测下一个时间步的状态。
这个MathWorks documentation对IMU相关的预测方程有很好的引用。
观察/测量模型:
将测量值与状态关联起来。
Accel 数据已用于预测。这里忽略它。
陀螺仪数据为 [gx, gy, gz] = [omega_x + gyro_bias_x, ....] + 误差
处理磁力计的一种方法是从中获取偏航角 - arctan(y/x),然后使用 yaw_mag 作为测量值。
相机数据为 [vx_cam, vy_cam, vz_cam] = [Vx, Vy, Vx] + 错误
最后追加所有行并将其转换为 Y=C*X + 噪声形式。
Y 表示不同传感器的测量值,X 表示状态。
在这种情况下,Y 将为 [gx, gy, gz, yaw_mag, vx,cam, vy_cam, vz_cam]。
免责声明:我是 MathWorks 员工,链接来自 MathWorks 文档。
关于accelerometer - IMU速度估计,我们在Stack Overflow上找到一个类似的问题: https://stackoverflow.com/questions/71270530/