多传感器融合时间软同步

// 求一个比例 double front_scale = (back_data.time - sync_time) / (back_data.time - front_data.time); double back_scale = (sync_time - front_data.time) / (back_data.time - front_data.time); synced_data.time = sync_time; synced_data.linear_acceleration.x = front_data.linear_acceleration.x * front_scale + back_data.linear_acceleration.x * back_scale; synced_data.linear_acceleration.y = front_data.linear_acceleration.y * front_scale + back_data.linear_acceleration.y * back_scale; synced_data.linear_acceleration.z = front_data.linear_acceleration.z * front_scale + back_data.linear_acceleration.z * back_scale; synced_data.angular_velocity.x = front_data.angular_velocity.x * front_scale + back_data.angular_velocity.x * back_scale; synced_data.angular_velocity.y = front_data.angular_velocity.y * front_scale + back_data.angular_velocity.y * back_scale; synced_data.angular_velocity.z = front_data.angular_velocity.z * front_scale + back_data.angular_velocity.z * back_scale; // 四元数插值有线性插值和球面插值，球面插值更准确，但是两个四元数差别不大是，二者精度相当 // 由于是对相邻两时刻姿态插值，姿态差比较小，所以可以用线性插值 synced_data.orientation.x = front_data.orientation.x * front_scale + back_data.orientation.x * back_scale; synced_data.orientation.y = front_data.orientation.y * front_scale + back_data.orientation.y * back_scale; synced_data.orientation.z = front_data.orientation.z * front_scale + back_data.orientation.z * back_scale; synced_data.orientation.w = front_data.orientation.w * front_scale + back_data.orientation.w * back_scale; // 线性插值之后要归一化 synced_data.orientation.Normlize();