最近新接受的模块需要做位置补偿，按说是比较简单的。但是因为坐标系的问题导致了诸多麻烦出现，事后复盘如下：
一个未知的坐标系如何确定方向：
此处出现了转换WGS坐标系转东北天，且东北天第一帧初始化局部坐标系。
此时我不清楚到底东北天和所取XYZ有什么关系。选择某个已知的heading角度，最好是正东方向，然后观察坐标变化，我发现正东方向前进时，Y增加，因此Y轴正方向确定，同理可找正北向，发现X增加。此时可以确定程序坐标系为
/* N(y)^||W <------O------> E(x)||S*/这时候你还需要去确定你的heading变化方向是顺时针还是逆时针，选择一个中间角度，观察后发现heading角为与正北方夹角得顺时针变化，此时可以得到转换关系。
车辆坐标系下的位置关系转到东北天局部坐标系可以如下计算：
已知局部坐标系下，GPS坐标（x,y）且已知车辆坐标系下GPS与IMU位置差距（-0.4，-1）求IMU（x',y'）.
则可得如下方程:将局部坐标系转车辆坐标系即可有位置关系等式：
R逆（（x,y）-(x',y')）=(-0.4,-1)
(x',y')=(x,y)-R顺（-0.4，-1）
其中R逆转置后变成R顺
转换函数轮子如下:
//true 顺时针 false 逆时针static Eigen::Matrix2d rotationMatrix(double angle,bool direction){double cosTheta = cos(angle);double sinTheta = sin(angle);Eigen::Matrix2d rotMat;if (direction){rotMat <<cosTheta,sinTheta,-sinTheta,cosTheta;}else{rotMat << cosTheta, -sinTheta,sinTheta, cosTheta;}return rotMat;};//坐标系转换+位置补偿static Eigen::Vector2d positionCompensation(const Eigen::Vector2d Compensation,double angle,bool direction){return rotationMatrix(angle,direction)*Compensation;}; 
)
