Logo-loam中imu消除重力影响

前言

代码中主要是将重力加速度转换到imu坐标系上，然后进行下一步的处理，也就是说将世界坐标是的重力加速度【0，0，9.8】转化到imu坐标系【x,y,z】上，所以就需要一个旋转矩阵R来完成上述工作。G=RP表示P经过R转换到G，那么P可以表示为R的转置乘以G，这里旋转矩阵和本身的转置相乘为单位矩阵I。

推导

设初始旋转角度为Φ

文章中是按照x,y,z依次旋转变换，对应的角度是roll, pitch, yaw。

tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);

所以

代入可得

再用R的转置乘以G就得到了转换后的坐标，
这很多人算出来是和代码中的符号是相反的，这是因为G=【0，0，-9.8】，而不是【0，0，9.8】，因为重力加速度和世界坐标系z轴方向相反。所以代码中加了负号

        float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;
        float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;
        float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;

如有错误，欢迎指正！
参考：
图形学随笔：三维空间中的旋转矩阵