某矢量与某平面的交点

闪闪发亮的小星星2026-03-12 8:36
matlab 复制代码
clc
clear
close all

%% =====================
% 输入数据
%% =====================

% 激光基向量
P_laser = [1;0;0];

% 安装矩阵
C_lct_laser = eye(3);      % 根据实际填写
C_sat_lct   = eye(3);      % 标定结果

N = size(timeAlign_4S_data,1);

%% =====================
% 存储变量
%% =====================

P_intersect = zeros(N,3);
miss_angle  = zeros(N,1);

%% =====================
% 计算循环
%% =====================

for i = 1:N

    %% 卫星位置
    r_sat = timeAlign_4S_data(i,2:4)';   % XYZ

    %% 姿态矩阵 (根据你的数据调整)
    Cib = quat2dcm(timeAlign_4S_data(i,17:20)); 

    %% 激光方向 (惯性系)
    v_laser = Cib * C_sat_lct * C_lct_laser * P_laser;
    v_laser = v_laser / norm(v_laser);

    %% 太阳矢量
    sun_vec = SUN(i,2:4)';
    sun_vec = sun_vec / norm(sun_vec);

    %% 太阳平面
    n = sun_vec;

    %% 求交点
    lambda = -(n'*r_sat) / (n'*v_laser);

    P = r_sat + lambda * v_laser;

    P_intersect(i,:) = P';

    %% 脱靶角
    miss_angle(i) = acosd(dot(v_laser,sun_vec));

end

%% =====================
% 3D交点轨迹
%% =====================

figure
plot3(P_intersect(:,1),P_intersect(:,2),P_intersect(:,3),'r','LineWidth',2)

grid on
axis equal

xlabel('X')
ylabel('Y')
zlabel('Z')

title('激光与太阳平面交点轨迹')

%% =====================
% 脱靶角
%% =====================

figure
plot(miss_angle,'LineWidth',2)

grid on

xlabel('time index')
ylabel('miss angle (deg)')

title('对日指向误差')