IMU速度估计

Question

你知道我在哪里可以找到从IMU (acc+gyro+magnetometer)数据中估计速度的代码或例子吗？我根据IMU静止的数据计算偏差。我想用某种滤波器(卡尔曼/互补)来实现速度估计，但我找不到。我也有相机速度估计，也许它可以作为某种融合？提前谢谢你！亲切的问候

Answer 1

我没有适合你情况的示例代码。但这种方法会有所帮助(根据过去的经验)，

卡尔曼滤波器：

1. 确定和制定状态X，控制输入U，输出，预测和观测方程。
2. 实现/重用卡尔曼滤波的一些实现。这是一个基于Simulink的实现，以供参考。
3. 设置测量噪声和预测误差方差。以后可能需要进行一些微调。
4. 验证KF对某些引用是否有效。如果你有另一种测量速度的方法，检查KF的速度。

国家和控制投入：

国家可以是一个数组

1. 线性速度Vx，Vy，Vz
2. 角速度omega_x，omega_y，omega_z
3. 陀螺中的偏置这种偏置在很大程度上是恒定的，但会随温度和其他因素而变化。KF将使用加速度计测量来校正陀螺的偏置。
4. 加速度计的偏置。这种偏置在很大程度上是恒定的，但会随温度和其他因素而变化。相机速度将被KF用于校正accel偏置。
5. 方位(欧拉角或四元数)

控制输入不一定是发送给执行机构的实际命令。在这种情况下，控制输入可以是净力或净加速度，

加速度计数据(即比力)+重力加速度

预测方程：

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

该MathWorks文档对与IMU相关的预测方程有很好的参考价值。

观测/测量模型：

把测量和状态联系起来。

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

陀螺数据为gx，gy，gz = omega_x + gyro_bias_x，.+错误

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

相机数据为vx_cam，vy_cam，vz_cam = Vx，Vy，Vx +误差

最后，将所有行附加到Y=C*X + noise表单中。

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

在这种情况下，y将是gx，gy，gz，yaw_mag，vx，cam，vy_cam，vz_cam。

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