SLAM 基础学习记录（1） 载体速度解算载体位姿 ECEF解算ENU坐标系位置

载体速度解算载体位姿

载体速度对时间进行积分，传感器为NAV982惯性导航，其坐标系：X轴向右，Y轴向前，Z轴向下，符合右手坐标系原则。

在对载体速度进行时间积分前

1. 一定要用姿态角进行在各个方向的载体速度分解

2. 要将载体坐标系转换到世界坐标系下。（以载体初始位置为原点，东北天（ENU）坐标系的定义方式为世界坐标系）

相关代码如下：

  MatrixXf Z_RotMat(3,3);
  MatrixXf Y_RotMat(3,3);
  MatrixXf X_RotMat(3,3);
  MatrixXf CurrentVelocit(3,1);
  MatrixXf TransformToENU(3,3);

  TransformToENU << 0, 1, 0, 1, 0, 0, 0, 0, -1;  //载体坐标系转换到ENU坐标系
  Z_RotMat<< cos(yaw),  -sin(yaw),  0,  sin(yaw),  cos(yaw),  0,  0,  0,  1;
  Y_RotMat<< cos(pitch),   0,  sin(pitch),  0,  1,  0,  -sin(pitch), cos(pitch),  0;
  X_RotMat<< 1,  0,   0,  0,  cos(roll),  -sin(roll),  0,  sin(roll),  cos(roll);

  CurrentVelocit<< imuIn->BodyVelocity_[0], imuIn->BodyVelocity_[1], imuIn->BodyVelocity_[2];
  CurrentVelocit = TransformToENU * X_RotMat * Y_RotMat * Z_RotMat * CurrentVelocit;

ECEF解算位姿

利用前面关于几个坐标系的转换方法，将获取到的ECEF数据解算为ENU坐标系位姿。

①输入：

经度：Lon；纬度：Lat；第一组接受到的ECEF数据：x0,y0,z0；后面接收到的ECEF数据：x,y,z。

②计算：

ee = [-sin(Lon),cos(lon),0];

en = [-cos(Lon)*sin(Lat),-sin(Lon)*sin(L