只有经纬高坐标系下的飞行数据，怎么转换得到姿态角？

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Converts LLA (Latitude, Longitude, Altitude) data to an extended state
% vector including attitude (psi, theta, phi).
%
% Conversion Process:
%   1. Convert LLA to ECEF (Earth-Centered, Earth-Fixed) coordinates.
%   2. Convert ECEF to NED (North, East, Down) coordinates.
%   3. Differentiate NED coordinates to obtain NED velocities (NEDV).
%   4. Compute the attitude angles (psi, theta, phi) based on NED velocities:
%      - theta (pitch):      theta = degrees(asin(-vd/v))
%      - psi (yaw):          psi   = degrees(asin(-ve/sqrt(vn^2+ve^2)))
%      - phi (roll):         phi   = assumed zero in this implementation
%   5. Return [lat, lon, alt, psi, theta, phi].
%
% Usage:
%   llappp = lla2ppp(lla);
%   llappp = lla2ppp(lla, 'h', 0.2);
%
% Inputs:
%   - lla:   Nx3 matrix of Latitude, Longitude, and Altitude data [deg, deg, m].
%   - 'h':   Time interval for differentiation, default is 0.1 seconds.
%
% Outputs:
%   - llappp: Nx6 matrix of [lat, lon, alt, psi, theta, phi].
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function llappp = lla2ppp(lla, varargin)

    % Input Parsing
    ip = inputParser;
    ip.addRequired('lla', @(x) isnumeric(x) && size(x, 2) == 3);  % Ensure LLA is Nx3
    ip.addParameter('h', 0.1, @(x) isnumeric(x) && x > 0);        % Default time step 0.1s
    ip.parse(lla, varargin{:});
    h = ip.Results.h;

    % Convert LLA to NED coordinates relative to the first point as origin
    [nedx, nedy, nedz] = geodetic2ned(lla(:,1), lla(:,2), lla(:,3), ...
                                      lla(1,1), lla(1,2), lla(1,3), ...
                                      wgs84Ellipsoid);
    ned = [nedx, nedy, nedz];
    
    % Calculate NED velocities using numerical differentiation
    vn = gradient(nedx, h); % North velocity
    ve = gradient(nedy, h); % East velocity
    vd = gradient(nedz, h); % Down velocity

    % Calculate the total velocity magnitude
    v = sqrt(vn.^2 + ve.^2 + vd.^2);

    % Attitude Calculations
    % Avoid division by zero for velocity calculations
    theta = asind(-vd ./ max(v, eps));  % Pitch angle calculation
    psi = asind(ve ./ max(sqrt(vn.^2 + ve.^2), eps));  % Yaw angle calculation
    phi = zeros(size(theta));  % Roll angle set to zero, can be modified as needed

    % Combine LLA and attitude into the final output
    llappp = [lla, psi, theta, phi];
end

这段MATLAB程序lla2ppp的主要功能是将飞行器的LLA（Latitude, Longitude, Altitude，即纬度、经度、高度）数据转换为包含姿态信息的扩展状态向量，包括偏航角（psi）、俯仰角（theta）和滚转角（phi）。转换过程包括以下几个步骤：

1. LLA到NED转换：首先，将给定的LLA数据转换为NED（North, East, Down，即北、东、下）坐标系，该坐标系以地面为参考，通常以初始位置作为原点。这里使用MATLAB中的geodetic2ned函数进行转换，依赖于WGS84椭球体模型。

2. 计算NED速度：通过对NED坐标进行数值微分，获取对应的NED速度分量（vn, ve, vd），分别表示北向、东向和下向的速度。

3. 姿态角计算：使用NED速度计算姿态角。俯仰角（theta）是由下向速度与总速度之比计算得到的；偏航角（psi）由东向速度和水平速度（北向与东向速度的合成）之比计算。滚转角（phi）在该实现中被设为零。

4. 输出结果：最终将计算得到的姿态角与原始的LLA数据组合，生成一个包含位置和姿态的扩展状态矩阵，格式为 [lat, lon, alt, psi, theta, phi]。

程序还包含了一些输入参数的校验和错误处理，如防止除零错误（通过使用max(value, eps)）确保计算稳定。用户可以自定义微分时间步长h，默认为0.1秒。该程序适用于需要从位置数据推导出飞行器姿态的应用，如飞行仿真、导航算法验证等。