您好，很感谢您使用EKF融合IMU和运动编码器实现机器人位置和速度的估计，在仔细研读您的代码后，我有以下两个疑问：
- 在足端离地时，“足端触底速度为0”的假设不再成立，所以R矩阵对应的“速度”观测部分需要增大协方差数值，get！但是为什么要增加对应P_body-P_foot和Z_foot的协方差数值？我的理解是在Y=Hx的方程中，Pos和z的观测与足端是否离地无关，想请教一下您为何在代码中要同时增加他们的协方差数值？
for (int i = 0; i < NUM_LEG; ++i) { R.block<3, 3>(i * 3, i * 3) = (1 + (1 - estimated_contacts[i]) * 1e3) * SENSOR_NOISE_PIMU_REL_FOOT * eye3; // fk estimation R.block<3, 3>(NUM_LEG * 3 + i * 3, NUM_LEG * 3 + i * 3) = (1 + (1 - estimated_contacts[i]) * 1e3) * SENSOR_NOISE_VIMU_REL_FOOT * eye3; // vel estimation if (assume_flat_ground) { R(NUM_LEG * 6 + i, NUM_LEG * 6 + i) = (1 + (1 - estimated_contacts[i]) * 1e3) * SENSOR_NOISE_ZFOOT; // height z estimation }
您好，很感谢您使用EKF融合IMU和运动编码器实现机器人位置和速度的估计，在仔细研读您的代码后，我有以下两个疑问：
for (int i = 0; i < NUM_LEG; ++i) { R.block<3, 3>(i * 3, i * 3) = (1 + (1 - estimated_contacts[i]) * 1e3) * SENSOR_NOISE_PIMU_REL_FOOT * eye3; // fk estimation R.block<3, 3>(NUM_LEG * 3 + i * 3, NUM_LEG * 3 + i * 3) = (1 + (1 - estimated_contacts[i]) * 1e3) * SENSOR_NOISE_VIMU_REL_FOOT * eye3; // vel estimation if (assume_flat_ground) { R(NUM_LEG * 6 + i, NUM_LEG * 6 + i) = (1 + (1 - estimated_contacts[i]) * 1e3) * SENSOR_NOISE_ZFOOT; // height z estimation }