%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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）。转换过程包括以下几个步骤：
-
LLA到NED转换：首先，将给定的LLA数据转换为NED（North, East, Down，即北、东、下）坐标系，该坐标系以地面为参考，通常以初始位置作为原点。这里使用MATLAB中的
geodetic2ned函数进行转换，依赖于WGS84椭球体模型。 -
计算NED速度：通过对NED坐标进行数值微分，获取对应的NED速度分量（vn, ve, vd），分别表示北向、东向和下向的速度。
-
姿态角计算：使用NED速度计算姿态角。俯仰角（theta）是由下向速度与总速度之比计算得到的；偏航角（psi）由东向速度和水平速度（北向与东向速度的合成）之比计算。滚转角（phi）在该实现中被设为零。
-
输出结果：最终将计算得到的姿态角与原始的LLA数据组合，生成一个包含位置和姿态的扩展状态矩阵，格式为 [lat, lon, alt, psi, theta, phi]。
程序还包含了一些输入参数的校验和错误处理，如防止除零错误（通过使用max(value, eps)）确保计算稳定。用户可以自定义微分时间步长h，默认为0.1秒。该程序适用于需要从位置数据推导出飞行器姿态的应用，如飞行仿真、导航算法验证等。