本文主要是介绍MSF融合流程,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!
1. 添加重力约束
重力约束是在在车体系的方向是(0,0,1),添加时1)如果已知车辆在水平方向运动,添加平面约束;2)如果已知车辆静止(判断IMU加速度计和陀螺仪的norm都很小)则按照IMU的加速度计当观测,协方差用加速度计的模-重力常亮的模
double gravity_diff = fabs(imu_data.acc.norm() - GetIMUConfig().gravity_norm);
double err = std::acos(GetIMUConfig().gravity_norm /(GetIMUConfig().gravity_norm +gravity_diff));
err = (err < 0.1 / 180.0 * 3.141592653)? (0.1 / 180.0 * 3.141592653): err;
Eigen::Matrix3d cov =Eigen::Matrix3d::Identity() *(err * err + imu_data.gyro.norm() * imu_data.gyro.norm());
这篇关于MSF融合流程的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!