自动驾驶坐标系与旋转矩阵的确定（位置补偿）

最近新接受的模块需要做位置补偿，按说是比较简单的。但是因为坐标系的问题导致了诸多麻烦出现，事后复盘如下：

一个未知的坐标系如何确定方向：
此处出现了转换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 <
                    -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;
    };