airsim局部坐标系转化机体坐标系
airsim官方提供的state不包含机体坐标系的速度等信息，但包含了局部坐标系下的速度和当前机体在初始点的四元数矩阵，因此可通过四元数解算得到机体坐标系下的速度vel_body
一般来说四元矩阵的表示如下所示：


因此构造得到的旋转矩阵如下所示：

但得到的是从机体坐标系到局部坐标系的旋转矩阵，我们想要得到的是从局部坐标系到机体坐标系的旋转矩阵
由于旋转的可逆性和旋转矩阵的本身性质（旋转矩阵的逆即为矩阵的转置）
因此，局部坐标系向机体坐标系转化的旋转矩阵为：

airsim代码示意:

airsim提供了getMultirotorState作为获取无人机飞行状态信息的api函数。
该函数能够获取无人机的参数信息：是否碰撞、无人机的状态（在simple飞行模式下为ue4构建的四旋翼飞行状态，以全局坐标系为基准）、GPS信息、时间戳、是否降落、遥控器状态等信息。

	 class MultirotorState(MsgpackMixin):
     collision = CollisionInfo()                 # 碰撞信息
     kinematics_estimated = KinematicsState()    # 状态信息
     gps_location = GeoPoint()                   # GPS 信息
     timestamp = np.uint64(0)                    # 时间戳
     landed_state = LandedState.Landed           # 是否是降落状态
     rc_data = RCData()                          # 遥控器数据
     ready = False
     ready_message = ""
     can_arm = False

	因此通过该api获取无人机的全局速度与四元数，构造上述旋转矩阵即可得到无人机的局部速度（全局）向机体坐标系的转化

		fly_state = self.client.getMultirotorState()

        vel_x = fly_state.kinematics_estimated.linear_velocity.x_val
        vel_y = fly_state.kinematics_estimated.linear_velocity.y_val
        vel_z = fly_state.kinematics_estimated.linear_velocity.z_val

        x = fly_state.kinematics_estimated.orientation.x_val
        y = fly_state.kinematics_estimated.orientation.y_val
        z = fly_state.kinematics_estimated.orientation.z_val
        w = fly_state.kinematics_estimated.orientation.w_val

        vel_x_self = (1 - 2 * (y ** 2) - 2 * (z ** 2)) * vel_x + (2 * x * y - 2 * z * w) * vel_y + (
                    2 * x * z + 2 * y * w) * vel_z
        vel_y_self = (2 * x * y + 2 * z * w) * vel_x + (1 - 2 * (x ** 2) - 2 * (z ** 2)) * vel_y + (
                    2 * y * z - 2 * x * w) * vel_z
        vel_z_self = (2 * x * z - 2 * y * w) * vel_x + (2 * y * z + 2 * x * w) * vel_y + (
                    1 - 2 * (x ** 2) - 2 * (y ** 2)) * vel_z

将该旋转矩阵应用至airsim recording函数，即可记录四旋翼飞行轨迹中的机体速度。

// MultirotorPawnSimApi.cpp
//重构recording模块，记录机体坐标系飞行速度
std::string MultirotorPawnSimApi::getRecordFileLine(bool is_header_line) const
{
    std::string common_line = PawnSimApi::getRecordFileLine(is_header_line);
    if (is_header_line) {
        return common_line +
               "Body_Vel_X\tBody_Vel_Y\tBody_Vel_Z\t";
    }
    //构建从全局坐标系到机体坐标系的转换矩阵
    const auto& state = vehicle_api_->getMultirotorState();
    float vel_x = state.kinematics_estimated.twist.linear.x();
    float vel_y = state.kinematics_estimated.twist.linear.y();
    float vel_z = state.kinematics_estimated.twist.linear.z();
    float x = state.kinematics_estimated.pose.orientation.x();
    float y = state.kinematics_estimated.pose.orientation.y();
    float z = state.kinematics_estimated.pose.orientation.z();
    float w = state.kinematics_estimated.pose.orientation.w();
    float vel_x_self = (1 - 2 * (y * y) - 2 * (z*z)) * vel_x + (2 * x * y + 2 * z * w) * vel_y + (2 * x * z - 2 * y * w) * vel_z;
    float vel_y_self = (2 * x * y - 2 * z * w) * vel_x + (1 - 2 * (x *x) - 2 * (z *z)) * vel_y + (2 * y * z + 2 * x * w) * vel_z;
    float vel_z_self = (2 * x * z + 2 * y * w) * vel_x + (2 * y * z - 2 * x * w) * vel_y + (1 - 2 * (x*x) - 2 * (y*y)) * vel_z;
    std::ostringstream ss;
    ss << common_line;
    ss << vel_x_self << "\t" << vel_y_self << "\t"<< vel_z_self << "\t";

    return ss.str();
}

参考链接为：果然爱大佬的博客与官方源码

https://blog.csdn.net/weixin_44691296/article/details/119923403

导航中的几种常用坐标系

1. 地心惯性坐标系（ECI）
2. 地心地固坐标系（ECEF）
3. 当地水平坐标系（LLF）、东北天坐标系ENU
4. 地平坐标系
5. 载体/机体坐标系

机动目标跟踪/室内定位/导航/优化技术探讨：WX: ZB823618313

原创不易，路过的各位大佬请点个赞

一、 地心惯性坐标系（ECI）

         在空间保持静止或匀速直线运动(无加速度)的坐标系称为惯性坐标系。所有惯性器件在测量轴方向产生的都是惯性系下的测量结果。此外,还需要在惯性系下完成卫星绕地球的位置和速度的估计。对于近地卫星,通常采用地心惯性(ECI)坐标系。

1. 地球质心为坐标系原点;
2. $z$轴沿着地球自转轴指向协议地极;
3. $x$轴位于赤道平面内,并指向春分点;
4. $y$轴符合右手笛卡儿坐标系。

ECI 坐标系的坐标轴利用上标$i$表示为$x_i$ ,$y_i$ ,$z_i$。

二、 地心地固坐标系（ECEF）

         地心地固坐标系(ECEF)与ECI坐标系有着相同的坐标原点和$z$轴定义,但是ECEF坐标系是与地球保持同步旋转的(所以命名为地固)。定义为:
一般地球导航和跟踪中，惯性坐标系即为ECEF

1. 坐标原点与地球质心重合;
2. $z$轴指向协议地极；
3. $x$轴指向赤道与本初子午线(格林尼治子午线)的交点;
4. $y$轴在赤道平面上与$x$轴和$z$轴构成右手笛卡儿坐标系。

ECI 坐标系的坐标轴利用上标$i$表示为$x_e$ ,$y_e$ ,$z_e$。

地心地固坐标系是参与地球自转的，它相对惯性坐标系的转动角速度就等于地球自转角速度。

三、 当地水平坐标系（LLF）

又被称之为 地理坐标系

         当地水平坐标系(LLF)用来描述运载器在近地运动中的姿态与速度。LLF坐标系也称为地理或导航坐标系。通常LLF定义如下:

1. 坐标原点是惯性器件坐标系的中心(惯性器件的三轴交点);
2. $y$轴水平指真北;
3. $z$轴水平指东;
4. $z$轴与$x$轴、$y$轴构成右手笛卡儿坐标系,方向与地球椭球面垂直。即指向天或地心

LLF也称为ENU坐标系,因为它的坐标轴分别指向正北、正东,和铅直向上。下图表示了该坐标系之间的关系。另一种常用LLF坐标系与ENU坐标系的z轴定义不同,它与x轴、y轴构成左手笛卡儿坐标系,z轴方向垂直于参考椭球面向下。因此,这种坐标系称为NED(北,东,地)坐标系。本书将采用ENU坐标系。

LLF坐标系的坐标轴利用上标$g$表示为$x_g$ ,$y_g$ ,$z_g$。

四、 地平坐标系

         地平坐标系如图所示，

1. 坐标原点取在运载体重心
2. yh轴水平并指向航行方向
3. zh轴与当地地垂线重合并指向天顶
4. xh轴也是水平并与yh和zh轴构成右手直角坐标系。
5. 其中Oxhyh平面就是当地水平面，Oyhzh平面就是运载体的纵向铅垂面。

在确定运载体姿态角时，采用地平坐标系更为直接和方便。

地平坐标系的坐标轴利用上标$h$表示为$x_h$ ,$y_h$ ,$z_h$。

五、载体/机体坐标系

         在实际应用中,加速度计的测量轴是由运动平台安装固定此器件的轴向决定的。这些坐标轴就形成了载体坐标系,定义如下:

1. 坐标原点设在载体质心(这样可简化运动方程的推导);
2. y轴指向载体前方,称为横滚轴,横滚角以y轴为起始旋转轴而得到,符合右手定则;
3. x轴与y轴夹角90°,指向载体横向,称为俯仰轴,俯仰角以x轴为起始旋转轴而得到,符合右手定则;
4. z轴与x轴、y轴构成右手笛卡儿坐标系,指向载体垂直方向,称为偏航轴,偏航角以z轴为起始旋转轴而得到,符合右手定则。

即机体坐标系与载体固连，其坐标原点与飞机重心重合。

原创不易，路过的各位大佬请点个赞

大家在入门四旋翼飞行器数学模型时第一个遇到的就是坐标系的转换。这篇文章用尽量浅显的语言为大家讲解坐标系的转换的欧拉角。

机体坐标系

原点O取在飞机质心处, Xb轴指向机头, ,Yb轴指向机身右方, Zb指向机身下方。

地面坐标系

在地面上选一点Og，使Xg轴在水平面内并指向某一方向,Zg轴垂直于地面并指向地心,Yg轴在水平面内垂直于Xg轴，是一个右手坐标系。

欧拉角

机体坐标系与地面惯性坐标系之间的夹角就是飞机的姿态角，又称欧拉角。
（1）俯仰角θ\thetaθ： 机体轴与地平面（水平面）之间的夹角，飞机抬头为正。
（2）偏航角（方位角）ψ\psiψ：机体轴在水平面上的投影与地轴之间的夹角，以机头右偏为正。
（3）滚转角（倾斜角）ϕ\phiϕ：飞机对称面绕机体轴 转过的角度，右滚为正。

飞机速度分量

坐标系转换

那么，三维坐标系的转换就有

可以的到分别绕三个轴的转换矩阵为