Airsim无人机状态获取getMultirotorState

getMultirotorState

此 API 一次调用即可返回车辆的状态。状态包括碰撞、估计运动学（即通过融合传感器计算的运动学）和时间戳（自纪元以来的纳秒）。这里的运动学意味着 6 个量：位置、方向、线速度和角速度、线速度和角加速度。请注意，simple_slight 目前不支持状态估计器，这意味着 simple_flight 的估计和地面实况运动学值将相同。但是，除了角加速度外，还可以为 PX4 提供估计的运动学。所有量都在 NED 坐标系中，世界坐标系中的 SI 单位除了角速度和加速度在体坐标系中。

这个状态是由传感器估计的状态，并不是无人机状态的真值。
AirSim默认的无人机底层飞控 simple_flight 并不支持状态估计，所以如果是simple_flight 飞控，此函数得到的状态与真值相同。
使用PX4 飞控可以获取估计的状态

fly_state = client.getMultirotorState()

# query vehicle state
def getMultirotorState(self, vehicle_name = ''):
    """
    Args:
        vehicle_name (str, optional): Vehicle to get the state of

    Returns:
        MultirotorState:
    """
    return MultirotorState.from_msgpack(self.client.call('getMultirotorState', vehicle_name))
getMultirotorState.__annotations__ = {'return': MultirotorState}

 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

KinematicsState

此API获得的是相对于起始位置的无人机状态，其中速度为起始位置xyz方向的速度。

 class KinematicsState(MsgpackMixin):
     position = Vector3r()               # 位置
     orientation = Quaternionr()         # 姿态角
     linear_velocity = Vector3r()        # 速度
     angular_velocity = Vector3r()       # 机体角速率
     linear_acceleration = Vector3r()    # 加速度
     angular_acceleration = Vector3r()   # 机体角加速度

相对于全局的速度转换为无人机自身的速度

    fly_state = 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
    
    return vel_x_self,vel_y_self,vel_z_self