免费seo网站诊断免费,html网页开发工具,网站开发售后服务承诺,选择做华为网站的目的和意义1. 添加重力约束
重力约束是在在车体系的方向是#xff08;0#xff0c;0#xff0c;1#xff09;#xff0c;添加时1#xff09;如果已知车辆在水平方向运动#xff0c;添加平面约束#xff1b;2#xff09;如果已知车辆静止#xff08;判断IMU加速度计和陀螺仪的n…1. 添加重力约束
重力约束是在在车体系的方向是001添加时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());
