这个函数perifocal2ECI的matlab代码

function [r_ECI, v_ECI] = perifocal2ECI(r_peri, v_peri, omega, i, OMEGA)

% Inputs:
% r_peri - position vector in perifocal coordinates [km]
% v_peri - velocity vector in perifocal coordinates [km/s]
% omega - argument of periapsis [rad]
% i - inclination [rad]
% OMEGA - right ascension of the ascending node [rad]

% Outputs:
% r_ECI - position vector in ECI coordinates [km]
% v_ECI - velocity vector in ECI coordinates [km/s]

% Earth constants
mu = 3.986004418e5; % gravitational parameter [km^3/s^2]

% Transformation matrix from perifocal to ECI coordinates
Qx = [1 0 0; 0 cos(OMEGA) sin(OMEGA); 0 -sin(OMEGA) cos(OMEGA)];
Qz = [cos(i) sin(i) 0; -sin(i) cos(i) 0; 0 0 1];
Q1 = [cos(omega) sin(omega) 0; -sin(omega) cos(omega) 0; 0 0 1];
Q = Qx*Qz*Q1;

% Cartesian position and velocity vectors in ECI coordinates
r_ECI = Q*r_peri';
v_ECI = Q*v_peri';

% Cartesian acceleration vector in ECI coordinates
a_ECI = -mu/norm(r_ECI)^3*r_ECI;

% Transforming acceleration from ECI coordinates to perifocal coordinates
A = inv(Q)*a_ECI;

% Angular velocity of the orbit in perifocal coordinates
n = sqrt(mu/norm(r_peri)^3);

% Transformation matrix from perifocal to ECI coordinates
P11 = cos(n*1); P12 = sin(n*1); P13 = 0;
P21 = -sin(n*1); P22 = cos(n*1); P23 = 0;
P31 = 0; P32 = 0; P33 = 1;
P = [P11 P12 P13; P21 P22 P23; P31 P32 P33];

% Transformation matrix from ECI to perifocal coordinates
Q_T = inv(Q);

% Cartesian position and velocity vectors in perifocal coordinates
r_peri_new = P*Q_T*r_ECI;
v_peri_new = P*Q_T*v_ECI;

% Checking the accuracy of the transformations
r_err = max(abs(r_peri - r_peri_new))
v_err = max(abs(v_peri - v_peri_new))