写一个无人机地理坐标系转机体坐标系的函数

好的，以下是一个无人机地理坐标系转机体坐标系的函数的示例代码：

import math

def geo_to_body(lat, lon, alt, roll, pitch, yaw):
    # Convert degrees to radians
    lat = math.radians(lat)
    lon = math.radians(lon)
    roll = math.radians(roll)
    pitch = math.radians(pitch)
    yaw = math.radians(yaw)

    # Earth's radius in meters
    R = 6371000

    # Calculate the position vector in ECEF coordinates
    x = (R + alt) * math.cos(lat) * math.cos(lon)
    y = (R + alt) * math.cos(lat) * math.sin(lon)
    z = (R + alt) * math.sin(lat)

    # Calculate the rotation matrix
    R_roll = [[1, 0, 0],
              [0, math.cos(roll), -math.sin(roll)],
              [0, math.sin(roll), math.cos(roll)]]
    R_pitch = [[math.cos(pitch), 0, math.sin(pitch)],
               [0, 1, 0],
               [-math.sin(pitch), 0, math.cos(pitch)]]
    R_yaw = [[math.cos(yaw), -math.sin(yaw), 0],
             [math.sin(yaw), math.cos(yaw), 0],
             [0, 0, 1]]
    R_body_to_ecef = [[0, 1, 0],
                      [0, 0, 1],
                      [1, 0, 0]]
    R_ecef_to_body = [[0, 0, 1],
                      [1, 0, 0],
                      [0, 1, 0]]
    R_body = [[0, 0, 0],
              [0, 0, 0],
              [0, 0, 0]]

    # Multiply the rotation matrices to get the final rotation matrix
    for i in range(3):
        for j in range(3):
            for k in range(3):
                R_body[i][j] += R_roll[i][k] * R_pitch[k][j] * R_yaw[j][k]

    # Calculate the position vector in body coordinates
    pos_body = [[0], [0], [0]]
    for i in range(3):
        for j in range(3):
            pos_body[i][0] += R_ecef_to_body[i][j] * (R_body_to_ecef[j][0] * x + R_body_to_ecef[j][1] * y + R_body_to_ecef[j][2] * z)

    return pos_body

这个函数接受六个参数：纬度、经度、高度、横滚角、俯仰角和偏航角。它返回一个包含三个元素的列表，分别代表在机体坐标系下的 x、y、z 坐标。