激光导引头蒙特卡洛仿真系统

1. 系统概述

本仿真系统是一个基于蒙特卡洛方法的激光半主动制导系统模拟器。它通过追踪大量离散的光线（光子包），精确模拟了从激光指示器发射激光，经目标漫反射后，被激光导引头接收的完整物理过程。

该系统的主要目标是：评估激光导引头接收到的总光强；可视化激光光斑在导引头探测器焦平面上的分布情况；分析激光束发散、大气衰减、目标反射特性以及导引头视场角等因素对制导性能的影响。

2. 核心组件与参数

2.1 激光指示器 (Laser Designator)

功能：向目标发射编码激光束，为导引头提供照射源。

关键参数：

DESIGNATOR_POS

：指示器在三维空间中的位置

[x, y, z]

（单位: 米）。

DESIGNATOR_POWER

：激光发射总功率（单位: 瓦特, W）。

DESIGNATOR_BEAM_ANGLE

：激光束的发散角（半角）（单位: 弧度, rad）。此参数决定了光束的宽度和能量集中度。

2.2 目标 (Target)

功能：被激光照射，并将入射激光进行漫反射（遵循朗伯余弦定律）。

关键参数：

TARGET_POS

：目标在三维空间中的位置

[x, y, z]

（单位: 米）。

反射模型：采用朗伯体 (Lambertian) 模型。这意味着目标向所有方向均匀散射光线，其反射强度与入射角的余弦成正比。

2.3 激光导引头 (Laser Seeker)

功能：安装在导弹或飞行器上，用于探测并跟踪来自目标的反射激光信号。

关键参数：

SEEKER_POS

：导引头在三维空间中的初始位置

[x, y, z]

（单位: 米）。

SEEKER_FOV

：导引头的视场角 (Field of View, FOV)（单位: 弧度, rad）。只有在此角度范围内的光线才能被有效接收。

SEEKER_APERTURE_AREA

：光学系统的孔径面积（单位: 平方米, m）。决定了收集光能的能力。

SEEKER_SENSOR_SIZE

：探测器焦平面的物理尺寸（单位: 米, m）。用于计算成像落点。

2.4 环境与传播模型

大气衰减：使用比尔-朗伯定律 (Beer-Lambert Law) 模拟激光在大气中传播时的能量损耗。

ATM_LOSS_COEFF

：大气衰减系数（单位: /米）。值越大，衰减越严重。

光束噪声：通过高斯随机分布模拟激光束固有的发散和抖动。

3. 仿真流程详解

仿真过程采用蒙特卡洛方法，对

RAY_TOTAL_N

（例如1000）条独立的光线进行追踪。每条光线的处理流程如下：

步骤 1: 初始化与准备

计算视线方向：系统首先计算出三个核心方向：指示器指向目标的方向 (

DESIGNATOR_DIRECTION

)；目标指向导引头的方向 (

TARGET_DIRECTION

)；导引头指向目标的方向 (

SEEKER_DIRECTION

)。

生成光束噪声：根据

DESIGNATOR_BEAM_ANGLE

生成

RAY_TOTAL_N

组随机的偏航角 (yaw) 和俯仰角 (pitch) 噪声，用于模拟每条光线的实际发射方向偏差。

步骤 2: 光线追踪循环 (对每条光线执行)

分配能量：将激光总功率

DESIGNATOR_POWER

平均分配给

RAY_TOTAL_N

条光线，得到每条光线的初始能量。

添加方向噪声：将预先生成的噪声应用到指示器的理想指向方向上，得到该光线的实际发射方向。

计算与目标的交点：将光线建模为一条从

DESIGNATOR_POS

出发的射线；将目标建模为一个以其位置为中心、法向量为

TARGET_DIRECTION

的无限大平面；通过求解射线与平面的交点，确定光线是否击中目标以及击中点的位置

ray_hit_pos

。

计算第一次大气衰减：计算从指示器到击中点的距离；应用比尔-朗伯定律，计算光线到达目标时的剩余能量。

模拟目标反射：计算光线入射到目标的角度；根据朗伯余弦定律，计算从目标向导引头方向反射的光强。

检查导引头视场：计算反射光线相对于导引头主轴的夹角 (

angle_from_seeker

)；只有当该夹角小于导引头的

SEEKER_FOV

并且入射角小于90度（确保是正面照射）时，该光线才被认为能被导引头接收。否则，该光线被丢弃。

计算第二次大气衰减与接收光强：计算从目标击中点到导引头的距离；再次应用大气衰减模型；计算导引头光学系统收集到的立体角；最终计算出该光线贡献给探测器的光强，并累加到

sensor_total_intensity

中。

计算探测器落点：在导引头内部建立一个焦平面坐标系 (U, V)，该平面垂直于导引头的主视轴 (

SEEKER_DIRECTION

)；计算反射光线与该焦平面的交点；将交点坐标投影到 (U, V) 坐标系中，得到

(u, v)

像素坐标，并存储起来用于后续成像。

步骤 3: 结果输出与可视化

输出总光强：打印导引头接收到的累计总光强。

2D 成像图：绘制所有有效光线在探测器焦平面上的

(u, v)

落点分布图，直观展示光斑形状和大小。

3D 空间图：在三维空间中绘制指示器、目标、导引头的位置，并用线条示意部分光线的完整路径（从指示器到目标，再到导引头）。

4. 关键技术与模型

蒙特卡洛方法

通过广泛的随机取样来模拟复杂的物理现象，尤其适用于处理具有不确定性的光学问题。

朗伯反射模型

: 精确地再现了大多数非镜面物体对激光的散射行为。

比尔-朗伯定律

: 清晰地阐述了光线在介质（例如大气）中传播时的指数衰减特性。

射线-平面交点算法

: 有效地计算光线与物体的相互作用。

坐标系转换

: 在导引头局部坐标系中准确确定光斑的成像位置。

5. 应用场景

制导系统性能评估

: 在设计初期预测各种参数设置下的导引头信号强度。

作战效能分析

: 考察不同作战距离和大气条件对制导成功概率的影响。

算法验证

: 为导引头信号处理和追踪算法提供真实的输入数据。

教学与演示

: 生动地展示激光半主动制导的基本原理。

import matplotlib.pyplot as plt
import numpy as np
from math import atan2

# FUNCTIONS MODULE
def normal_line(direction):
    """将方向角转换为单位向量 (辅助函数，未在主程序中使用)"""
    az = direction[0]
    al = direction[1]
    R = np.array([np.cos(al) * np.cos(az), np.sin(al) * np.cos(az), np.sin(az)])
    return R

def gen_noise_profile(N, angle):
    """
    生成光束发散角的噪声分布。
    模拟激光束在传播过程中的角度抖动。
    """
    sigma = angle / 4  # 假设发散角为高斯分布的标准差
    R = []
    pitch = []
    yaw = []
    roll = []
    r = sigma * np.random.randn(N)  # 生成N个随机偏转角
    theta = np.random.randint(0, 180, N) # 随机方向
    for i in range(N):
        pitch.append(float(r[i] * np.cos(np.radians(theta[i]))))
        yaw.append(float(r[i] * np.sin(np.radians(theta[i]))))
        roll.append(float(0.0)) # 滚转角设为0
    R.insert(0, roll) # 滚转角
    R.insert(0, pitch) # 俯仰角
    R.insert(0, yaw) # 偏航角
    return R

def vector_2_direction(source, target):
    """
    计算从源点到目标点的方向角。
    返回 [方位角, 仰角, 滚转角(0)]
    """
    v = target - source
    r = np.sqrt(np.power(v[0], 2) + np.power(v[1], 2) + np.power(v[2], 2))
    if r == 0:
        return np.array([0, 0, 0]) # 避免除零
    theta = np.arcsin(v[2] / r)  # 仰角
    phi = atan2(v[1], v[0])      # 方位角
    phi = np.mod(phi, np.pi * 2) # 确保方位角在 [0, 2π)
    R = np.array([phi, theta, 0])
    return R

def vector_2_range(source, target):
    """计算两点之间的距离"""
    v = target - source
    R = np.sqrt(np.power(v[0], 2) + np.power(v[1], 2) + np.power(v[2], 2))
    return R

def direction_2_vector(dir):
    """
    将方向角 [方位角, 仰角, 滚转角] 转换为单位向量。
    滚转角在此简化模型中被忽略。
    """
    r = 1
    phi = dir[0]  # 方位角
    theta = dir[1] # 仰角
    x = r * np.cos(theta) * np.cos(phi)
    y = r * np.cos(theta) * np.sin(phi)
    z = r * np.sin(theta)
    R = np.array([x, y, z])
    return R

def vector_2_magnitude(v):
    """计算向量的模长"""
    R = np.sqrt(np.power(v[0], 2) + np.power(v[1], 2) + np.power(v[2], 2))
    return R

def angle_direction_2_direction(d1, d2):
    """
    计算两个方向角之间的夹角。
    使用向量点积计算夹角。
    """
    v1 = direction_2_vector(d1)
    v2 = direction_2_vector(d2)
    cos_angle = np.clip(np.dot(v1, v2), -1.0, 1.0) # 防止数值误差导致arccos越界
    R = np.arccos(cos_angle)
    return R

# SIMULATOR PARAMETER
MAP_SIZE = 200  # [m] 地图大小

# ANALYSIS PARAMETER
RAY_TOTAL_N = 1000 # 光线总数，用于蒙特卡洛仿真
ATM_LOSS_COEFF = 1.22e-4 # [/m] 大气损耗系数

# DESIGNATOR PARAMETER (激光指示器)
DESIGNATOR_POS = np.array([0, 0, 0])  # [m] x, y, z
DESIGNATOR_DIRECTION = np.array([0, 0, 0])  # [rad] az, al, r (指向目标的初始方向角)
DESIGNATOR_POWER = 100  # [W] 激光器功率
DESIGNATOR_BEAM_ANGLE = 0.002  # [rad] 激光发散角

# SEEKER PARAMETER (激光导引头)
SEEKER_POS = np.array([0, 0, 100])  # [m] x, y, z (初始位置，假设在高空)
SEEKER_DIRECTION = np.array([0, 0, 0])  # [rad] az, al, roll (初始指向)
SEEKER_SENSOR_SIZE = 0.005  # [m] (5mm) 探测器尺寸
SEEKER_APERTURE_AREA = 0.0016  # [m^2] (4cm * 4cm) 光学孔径面积
SEEKER_FOV = 0.174533 # [rad] (10deg) 导引头视场角

# TARGET PARAMETER (目标)
TARGET_POS = np.array([300, 1, 0])  # [m] x, y, z (目标位置，有微小偏移)

# ETC (临时变量)
TMP = 0
ERR = 0
sensor_x = []
sensor_y = []
sensor_total_intensity = 0.0 # 累计接收光强，初始化为浮点数

# --- 仿真主循环 ---

# 生成光束噪声分布
noise_arr = gen_noise_profile(RAY_TOTAL_N, DESIGNATOR_BEAM_ANGLE)

# 计算各组件之间的初始视线方向
DESIGNATOR_DIRECTION = vector_2_direction(DESIGNATOR_POS, TARGET_POS)
TARGET_DIRECTION = vector_2_direction(TARGET_POS, SEEKER_POS) # 目标指向导引头
SEEKER_DIRECTION = vector_2_direction(SEEKER_POS, TARGET_POS) # 导引头指向目标

for i in range(RAY_TOTAL_N):
    # 每条光线携带的能量
    intensity = DESIGNATOR_POWER / RAY_TOTAL_N
    
    # 为光线添加发散角噪声
    ray_direction = np.array([
        (DESIGNATOR_DIRECTION[0] + noise_arr[0][i]), # 偏航角
        (DESIGNATOR_DIRECTION[1] + noise_arr[1][i]), # 俯仰角
        TMP # 滚转角
    ])
    ray_vector = direction_2_vector(ray_direction)

    # 计算光线是否打中目标 (光线与目标平面的交点)
    # 目标平面法向量为 TARGET_DIRECTION 对应的单位向量
    t = np.dot(direction_2_vector(TARGET_DIRECTION), (TARGET_POS - DESIGNATOR_POS)) / np.dot(direction_2_vector(TARGET_DIRECTION), ray_vector)
    ray_hit_pos = DESIGNATOR_POS + (ray_vector * t)
    
    # 计算从指示器到目标的传播距离
    ray_length_designator_2_target = np.abs(vector_2_range(ray_hit_pos, DESIGNATOR_POS))

    # 计算大气损耗
    atm_loss_designator_2_target = np.exp(-ATM_LOSS_COEFF * ray_length_designator_2_target)
    intensity_designator_2_target = atm_loss_designator_2_target * intensity

    # --- 计算目标反射到导引头 ---
    direction_target_2_seeker = vector_2_direction(TARGET_POS, SEEKER_POS)
    angle_target_2_seeker = angle_direction_2_direction(TARGET_DIRECTION, direction_target_2_seeker) # 入射角
    direction_seeker_2_target = vector_2_direction(SEEKER_POS, TARGET_POS)
    angle_seeker_2_target = angle_direction_2_direction(SEEKER_DIRECTION, direction_seeker_2_target) # 接收角

    # 判断光线是否在导引头视场内
    if angle_target_2_seeker < (np.pi/2) and angle_seeker_2_target <= SEEKER_FOV:
        # 模拟目标漫反射 (Lambertian)
        intensity_target_2_seeker = intensity_designator_2_target / np.pi * np.cos(angle_target_2_seeker)
        
        # 计算从目标到导引头的距离
        length_target_2_seeker = np.abs(vector_2_range(ray_hit_pos, SEEKER_POS))
        
        # 计算大气损耗
        atmosphere_loss_target_2_seeker = np.exp(-ATM_LOSS_COEFF * length_target_2_seeker)
        
        # 计算立体角 (导引头孔径面积/距离平方 * cos接收角)
        solid_angle_seeker = SEEKER_APERTURE_AREA / np.power(length_target_2_seeker, 2) * np.cos(angle_seeker_2_target)

        # 计算最终到达探测器的光强
        intensity_seeker = intensity_target_2_seeker * solid_angle_seeker * atmosphere_loss_target_2_seeker
        sensor_total_intensity += intensity_seeker

        # --- 计算光线在探测器上的落点 ---
        direction_seeker_2_ray = vector_2_direction(SEEKER_POS, ray_hit_pos)
        vector_seeker_2_ray = ray_hit_pos - SEEKER_POS
        
        # 定义探测器焦平面
        plane_bias = SEEKER_SENSOR_SIZE / (2 * np.tan(SEEKER_FOV / 2)) # 焦距估算
        plane_pos = SEEKER_POS + direction_2_vector(SEEKER_DIRECTION) * plane_bias

        # 计算光线与焦平面的交点
        t = np.dot(direction_2_vector(SEEKER_DIRECTION), (plane_pos - SEEKER_POS)) / np.dot(direction_2_vector(SEEKER_DIRECTION), vector_seeker_2_ray)
        plane_hit_pos = SEEKER_POS + (vector_seeker_2_ray * t)
        
        # 建立焦平面坐标系 U, V
        vector_plane_U = np.cross(np.array([0, 0, 1]), direction_2_vector(SEEKER_DIRECTION))
        if np.linalg.norm(vector_plane_U) < 1e-6: # 如果视线平行于Z轴
            vector_plane_U = np.cross(np.array([1, 0, 0]), direction_2_vector(SEEKER_DIRECTION))
        vector_plane_V = np.cross(direction_2_vector(SEEKER_DIRECTION), vector_plane_U)
        vector_plane_dot = plane_hit_pos - plane_pos
        
        # 计算在焦平面上的U, V坐标
        u = np.dot(vector_plane_dot, vector_plane_U) / np.dot(vector_plane_U, vector_plane_U)
        v = np.dot(vector_plane_dot, vector_plane_V) / np.dot(vector_plane_V, vector_plane_V)
        
        # 记录坐标
        sensor_x.append(u)
        sensor_y.append(v)

print(f"Total Intensity Received by Seeker: {sensor_total_intensity:.6f} W")

# --- 可视化 ---
# 2D 探测器成像图
fig2d, ax2 = plt.subplots()
ax2.plot(sensor_x, sensor_y, 'bo', markersize=1, alpha=0.6)
ax2.set_xlim(-SEEKER_SENSOR_SIZE/2, SEEKER_SENSOR_SIZE/2)
ax2.set_ylim(-SEEKER_SENSOR_SIZE/2, SEEKER_SENSOR_SIZE/2)
ax2.set_xlabel('Detector X [m]')
ax2.set_ylabel('Detector Y [m]')
ax2.set_title('Laser Spot Distribution on Seeker Detector')
ax2.grid(True)

# 3D 空间图
fig3d = plt.figure(figsize=(10, 8))
ax3d = fig3d.add_subplot(projection='3d')
ax3d.scatter(DESIGNATOR_POS[0], DESIGNATOR_POS[1], DESIGNATOR_POS[2], label='Laser Designator', color='red', s=100)
ax3d.scatter(TARGET_POS[0], TARGET_POS[1], TARGET_POS[2], label='Target', color='green', s=100)
ax3d.scatter(SEEKER_POS[0], SEEKER_POS[1], SEEKER_POS[2], label='Laser Seeker', color='blue', s=100)

# 绘制光线路径 (仅绘制部分光线以避免过于密集)
for i in range(0, RAY_TOTAL_N, 100): # 每100条光线画一条
    ray_direction_noisy = np.array([
        (DESIGNATOR_DIRECTION[0] + noise_arr[0][i]),
        (DESIGNATOR_DIRECTION[1] + noise_arr[1][i]),
        TMP
    ])
    ray_vector = direction_2_vector(ray_direction_noisy)
    t = np.dot(direction_2_vector(TARGET_DIRECTION), (TARGET_POS - DESIGNATOR_POS)) / np.dot(direction_2_vector(TARGET_DIRECTION), ray_vector)
    ray_hit_pos = DESIGNATOR_POS + (ray_vector * t)
    ax3d.plot([DESIGNATOR_POS[0], ray_hit_pos[0]], [DESIGNATOR_POS[1], ray_hit_pos[1]], [DESIGNATOR_POS[2], ray_hit_pos[2]], 'k-', alpha=0.1)
    if angle_direction_2_direction(TARGET_DIRECTION, vector_2_direction(TARGET_POS, SEEKER_POS)) < (np.pi/2) and angle_direction_2_direction(SEEKER_DIRECTION, vector_2_direction(SEEKER_POS, TARGET_POS)) <= SEEKER_FOV:
        ax3d.plot([ray_hit_pos[0], SEEKER_POS[0]], [ray_hit_pos[1], SEEKER_POS[1]], [ray_hit_pos[2], SEEKER_POS[2]], 'g--', alpha=0.1)

ax3d.legend()
ax3d.set_xlim(0, MAP_SIZE)
ax3d.set_ylim(0, MAP_SIZE)
ax3d.set_zlim(0, MAP_SIZE)
ax3d.set_xlabel('X [m]')
ax3d.set_ylabel('Y [m]')
ax3d.set_zlabel('Z [m]')
ax3d.set_title('3D Visualization of Laser Path (Designator -> Target -> Seeker)')

plt.show()

每行代码加上注释，并解析代码，同时基于此代码编写一份MATLAB代码

Qwen3-Max

激光导引头仿真系统 - Python代码逐行注释与MATLAB实现

编辑

# 导入必要的库
import matplotlib.pyplot as plt  # 用于绘图
import numpy as np              # 用于数值计算，如数组、向量运算
from math import atan2          # 用于计算方位角，处理象限问题

# FUNCTIONS MODULE - 定义用于仿真计算的各种辅助函数
def normal_line(direction):
    """
    函数: normal_line
    功能: 将方向角（方位角、仰角）转换为单位向量。
    参数: direction - 包含方位角(az)和仰角(al)的数组。
    返回: 表示该方向的单位向量 [x, y, z]。
    注意: 此函数在当前主程序中未被直接调用。
    """
    az = direction[0]  # 方位角 (azimuth)
    al = direction[1]  # 仰角 (elevation)
    # 使用球坐标到笛卡尔坐标的转换公式
    R = np.array([np.cos(al) * np.cos(az), np.sin(al) * np.cos(az), np.sin(az)])
    return R

def gen_noise_profile(N, angle):
    """
    函数: gen_noise_profile
    功能: 生成模拟激光束发散角的随机噪声。
    参数: N - 需要生成的噪声样本数。
          angle - 激光束的发散角 (半角)。
    返回: 一个包含三个列表的列表 [roll, pitch, yaw]，每个列表包含N个随机偏转角。
    """
    sigma = angle / 4  # 假设发散角的标准差为发散角的1/4 (这是一个模型参数)
    R = []             # 最终返回的列表
    pitch = []         # 俯仰角 (pitch) 噪声列表
    yaw = []           # 偏航角 (yaw) 噪声列表
    roll = []          # 滚转角 (roll) 噪声列表
    r = sigma * np.random.randn(N)  # 生成N个服从高斯分布的随机数，标准差为sigma
    theta = np.random.randint(0, 180, N) # 生成N个0到180度的随机整数，用于决定偏转方向
    for i in range(N):
        # 将径向的随机偏转 r[i] 分解到 pitch 和 yaw 方向
        pitch.append(float(r[i] * np.cos(np.radians(theta[i])))) # pitch 方向的偏转
        yaw.append(float(r[i] * np.sin(np.radians(theta[i]))))   # yaw 方向的偏转
        roll.append(float(0.0)) # 滚转角通常假设为0
    # 按照 [pitch, yaw, roll] 的顺序插入到返回列表 R
    R.insert(0, roll)   # 先插入roll
    R.insert(0, pitch)  # 再插入pitch
    R.insert(0, yaw)    # 最后插入yaw
    return R

def vector_2_direction(source, target):
    """
    函数: vector_2_direction
    功能: 根据源点和目标点坐标，计算从源点指向目标点的方向角。
    参数: source - 源点坐标 [x, y, z]。
          target - 目标点坐标 [x, y, z]。
    返回: 方向角数组 [方位角, 仰角, 滚转角(0)]。
    """
    v = target - source  # 计算从源到目标的向量
    r = np.sqrt(np.power(v[0], 2) + np.power(v[1], 2) + np.power(v[2], 2)) # 计算向量模长
    if r == 0:
        return np.array([0, 0, 0]) # 如果源点和目标点重合，返回零方向
    theta = np.arcsin(v[2] / r)  # 仰角 (elevation angle)
    phi = atan2(v[1], v[0])      # 方位角 (azimuth angle)，atan2能正确处理象限
    phi = np.mod(phi, np.pi * 2) # 将方位角限制在 [0, 2π) 范围内
    R = np.array([phi, theta, 0]) # 滚转角设为0
    return R

def vector_2_range(source, target):
    """
    函数: vector_2_range
    功能: 计算两点之间的欧几里得距离。
    参数: source - 源点坐标 [x, y, z]。
          target - 目标点坐标 [x, y, z]。
    返回: 两点之间的距离。
    """
    v = target - source
    R = np.sqrt(np.power(v[0], 2) + np.power(v[1], 2) + np.power(v[2], 2))
    return R

def direction_2_vector(dir):
    """
    函数: direction_2_vector
    功能: 将方向角 [方位角, 仰角, 滚转角] 转换为单位向量。
    参数: dir - 包含方向角的数组 [az, el, roll]。
    返回: 表示该方向的单位向量 [x, y, z]。
    注意: 滚转角在此简化模型中被忽略。
    """
    r = 1  # 设为单位向量
    phi = dir[0]   # 方位角
    theta = dir[1] # 仰角
    # 使用球坐标到笛卡尔坐标的转换公式
    x = r * np.cos(theta) * np.cos(phi)
    y = r * np.cos(theta) * np.sin(phi)
    z = r * np.sin(theta)
    R = np.array([x, y, z])
    return R

def vector_2_magnitude(v):
    """
    函数: vector_2_magnitude
    功能: 计算向量的模长。
    参数: v - 输入向量 [x, y, z]。
    返回: 向量的模长。
    """
    R = np.sqrt(np.power(v[0], 2) + np.power(v[1], 2) + np.power(v[2], 2))
    return R

def angle_direction_2_direction(d1, d2):
    """
    函数: angle_direction_2_direction
    功能: 计算两个方向角向量之间的夹角。
    参数: d1 - 第一个方向角 [az, el, roll]。
          d2 - 第二个方向角 [az, el, roll]。
    返回: 两个方向之间的夹角 (弧度)。
    """
    v1 = direction_2_vector(d1)  # 将方向角转换为单位向量
    v2 = direction_2_vector(d2)
    # 使用向量点积公式 cos(theta) = (a.b) / (|a||b|) 计算夹角
    # np.clip 确保点积结果在 [-1, 1] 范围内，防止 arccos 计算出错
    cos_angle = np.clip(np.dot(v1, v2), -1.0, 1.0)
    R = np.arccos(cos_angle)
    return R

# SIMULATOR PARAMETER - 仿真器参数
MAP_SIZE = 200  # [m] 三维地图的大小

# ANALYSIS PARAMETER - 分析参数
RAY_TOTAL_N = 1000 # 光线总数，用于蒙特卡洛仿真
ATM_LOSS_COEFF = 1.22e-4 # [/m] 大气损耗系数 (CO2激光在特定条件下的近似值)

# DESIGNATOR PARAMETER - 激光指示器参数
DESIGNATOR_POS = np.array([0, 0, 0])  # [m] x, y, z - 激光指示器的位置
DESIGNATOR_DIRECTION = np.array([0, 0, 0])  # [rad] az, al, r - 指示器初始指向 (指向目标)
DESIGNATOR_POWER = 100  # [W] 激光器发射功率
DESIGNATOR_BEAM_ANGLE = 0.002  # [rad] 激光发散角 (半角)

# SEEKER PARAMETER - 激光导引头参数
SEEKER_POS = np.array([0, 0, 100])  # [m] x, y, z - 导引头位置 (假设在高空)
SEEKER_DIRECTION = np.array([0, 0, 0])  # [rad] az, al, roll - 导引头初始指向 (指向目标)
SEEKER_SENSOR_SIZE = 0.005  # [m] (5mm) 探测器物理尺寸
SEEKER_APERTURE_AREA = 0.0016  # [m^2] (4cm * 4cm) 光学孔径面积
SEEKER_FOV = 0.174533 # [rad] (约10度) 导引头视场角

# TARGET PARAMETER - 目标参数
TARGET_POS = np.array([300, 1, 0])  # [m] x, y, z - 目标位置 (有微小偏移)

# ETC (临时变量)
TMP = 0 # 临时变量，用于滚转角
ERR = 0 # 错误计数器 (在此代码中未使用)
sensor_x = [] # 存储光线在探测器X轴上的落点坐标
sensor_y = [] # 存储光线在探测器Y轴上的落点坐标
sensor_total_intensity = 0.0 # 累计接收到的总光强，初始化为浮点数

# --- 仿真主循环 ---

# 1. 生成光束噪声分布
noise_arr = gen_noise_profile(RAY_TOTAL_N, DESIGNATOR_BEAM_ANGLE)

# 2. 计算各组件之间的初始视线方向
DESIGNATOR_DIRECTION = vector_2_direction(DESIGNATOR_POS, TARGET_POS) # 指示器指向目标
TARGET_DIRECTION = vector_2_direction(TARGET_POS, SEEKER_POS) # 目标指向导引头 (反射方向)
SEEKER_DIRECTION = vector_2_direction(SEEKER_POS, TARGET_POS) # 导引头指向目标

# 3. 开始蒙特卡洛仿真，追踪每一条光线
for i in range(RAY_TOTAL_N):
    # 3.1 每条光线携带的能量
    intensity = DESIGNATOR_POWER / RAY_TOTAL_N
    
    # 3.2 为光线添加发散角噪声
    ray_direction = np.array([
        (DESIGNATOR_DIRECTION[0] + noise_arr[0][i]), # 偏航角 (yaw)
        (DESIGNATOR_DIRECTION[1] + noise_arr[1][i]), # 俯仰角 (pitch)
        TMP # 滚转角
    ])
    ray_vector = direction_2_vector(ray_direction) # 将方向角转换为单位向量

    # 3.3 计算光线是否打中目标 (光线与目标平面的交点)
    # 假设目标是一个平面，其法向量为从目标指向导引头的方向
    # 使用参数方程求解光线与平面的交点: P = P0 + t * D
    # 平面方程: (P - P_plane) . n = 0
    # 代入光线方程: (P0 + t * D - P_plane) . n = 0
    # 解得: t = (P_plane - P0) . n / (D . n)
    t = np.dot(direction_2_vector(TARGET_DIRECTION), (TARGET_POS - DESIGNATOR_POS)) / np.dot(direction_2_vector(TARGET_DIRECTION), ray_vector)
    ray_hit_pos = DESIGNATOR_POS + (ray_vector * t) # 光线与目标的交点
    
    # 3.4 计算从指示器到目标的传播距离
    ray_length_designator_2_target = np.abs(vector_2_range(ray_hit_pos, DESIGNATOR_POS))

    # 3.5 计算大气损耗 (Beer-Lambert law)
    atm_loss_designator_2_target = np.exp(-ATM_LOSS_COEFF * ray_length_designator_2_target)
    intensity_designator_2_target = atm_loss_designator_2_target * intensity

    # --- 3.6 计算目标反射到导引头 ---
    direction_target_2_seeker = vector_2_direction(TARGET_POS, SEEKER_POS) # 目标指向导引头
    angle_target_2_seeker = angle_direction_2_direction(TARGET_DIRECTION, direction_target_2_seeker) # 入射角
    direction_seeker_2_target = vector_2_direction(SEEKER_POS, TARGET_POS) # 导引头指向目标
    angle_seeker_2_target = angle_direction_2_direction(SEEKER_DIRECTION, direction_seeker_2_target) # 接收角

    # 3.7 判断光线是否在导引头视场内 (同时满足入射和接收角度条件)
    if angle_target_2_seeker < (np.pi/2) and angle_seeker_2_target <= SEEKER_FOV:
        # 3.8 模拟目标漫反射 (Lambertian散射模型)
        # 反射光强与入射角余弦成正比
        intensity_target_2_seeker = intensity_designator_2_target / np.pi * np.cos(angle_target_2_seeker)
        
        # 3.9 计算从目标到导引头的距离
        length_target_2_seeker = np.abs(vector_2_range(ray_hit_pos, SEEKER_POS))
        
        # 3.10 再次计算大气损耗
        atmosphere_loss_target_2_seeker = np.exp(-ATM_LOSS_COEFF * length_target_2_seeker)
        
        # 3.11 计算立体角 (导引头孔径面积/距离平方 * cos接收角)
        solid_angle_seeker = SEEKER_APERTURE_AREA / np.power(length_target_2_seeker, 2) * np.cos(angle_seeker_2_target)

        # 3.12 计算最终到达探测器的光强
        intensity_seeker = intensity_target_2_seeker * solid_angle_seeker * atmosphere_loss_target_2_seeker
        sensor_total_intensity += intensity_seeker # 累加光强

        # --- 3.13 计算光线在探测器上的落点 (成像) ---
        direction_seeker_2_ray = vector_2_direction(SEEKER_POS, ray_hit_pos) # 导引头指向光线交点
        vector_seeker_2_ray = ray_hit_pos - SEEKER_POS # 从导引头到光线交点的向量
        
        # 3.14 定义探测器焦平面
        # 焦距估算 (根据视场角和传感器尺寸)
        plane_bias = SEEKER_SENSOR_SIZE / (2 * np.tan(SEEKER_FOV / 2))
        # 焦平面中心位置
        plane_pos = SEEKER_POS + direction_2_vector(SEEKER_DIRECTION) * plane_bias

        # 3.15 计算光线与焦平面的交点
        # 同样使用参数方程求解
        t = np.dot(direction_2_vector(SEEKER_DIRECTION), (plane_pos - SEEKER_POS)) / np.dot(direction_2_vector(SEEKER_DIRECTION), vector_seeker_2_ray)
        plane_hit_pos = SEEKER_POS + (vector_seeker_2_ray * t)
        
        # 3.16 建立焦平面坐标系 U, V (正交于视线方向)
        # U轴: 与Z轴和视线方向的叉积 (如果视线平行Z轴，则用X轴)
        vector_plane_U = np.cross(np.array([0, 0, 1]), direction_2_vector(SEEKER_DIRECTION))
        if np.linalg.norm(vector_plane_U) < 1e-6: # 如果视线平行于Z轴
            vector_plane_U = np.cross(np.array([1, 0, 0]), direction_2_vector(SEEKER_DIRECTION))
        # V轴: 视线方向与U轴的叉积
        vector_plane_V = np.cross(direction_2_vector(SEEKER_DIRECTION), vector_plane_U)
        # 计算交点相对于焦平面中心的向量
        vector_plane_dot = plane_hit_pos - plane_pos
        
        # 3.17 计算在焦平面上的U, V坐标 (投影)
        u = np.dot(vector_plane_dot, vector_plane_U) / np.dot(vector_plane_U, vector_plane_U) # U坐标
        v = np.dot(vector_plane_dot, vector_plane_V) / np.dot(vector_plane_V, vector_plane_V) # V坐标
        
        # 3.18 记录坐标
        sensor_x.append(u)
        sensor_y.append(v)

# 输出仿真结果
print(f"Total Intensity Received by Seeker: {sensor_total_intensity:.6f} W")

# --- 可视化 ---
# 2D 探测器成像图
fig2d, ax2 = plt.subplots()
ax2.plot(sensor_x, sensor_y, 'bo', markersize=1, alpha=0.6) # 绘制落点
ax2.set_xlim(-SEEKER_SENSOR_SIZE/2, SEEKER_SENSOR_SIZE/2) # 设置X轴范围
ax2.set_ylim(-SEEKER_SENSOR_SIZE/2, SEEKER_SENSOR_SIZE/2) # 设置Y轴范围
ax2.set_xlabel('Detector X [m]') # X轴标签
ax2.set_ylabel('Detector Y [m]') # Y轴标签
ax2.set_title('Laser Spot Distribution on Seeker Detector') # 图标题
ax2.grid(True) # 显示网格

# 3D 空间图
fig3d = plt.figure(figsize=(10, 8))
ax3d = fig3d.add_subplot(projection='3d')
ax3d.scatter(DESIGNATOR_POS[0], DESIGNATOR_POS[1], DESIGNATOR_POS[2], label='Laser Designator', color='red', s=100) # 指示器
ax3d.scatter(TARGET_POS[0], TARGET_POS[1], TARGET_POS[2], label='Target', color='green', s=100) # 目标
ax3d.scatter(SEEKER_POS[0], SEEKER_POS[1], SEEKER_POS[2], label='Laser Seeker', color='blue', s=100) # 导引头

# 绘制光线路径 (仅绘制部分光线以避免过于密集)
for i in range(0, RAY_TOTAL_N, 100): # 每100条光线画一条
    ray_direction_noisy = np.array([
        (DESIGNATOR_DIRECTION[0] + noise_arr[0][i]),
        (DESIGNATOR_DIRECTION[1] + noise_arr[1][i]),
        TMP
    ])
    ray_vector = direction_2_vector(ray_direction_noisy)
    t = np.dot(direction_2_vector(TARGET_DIRECTION), (TARGET_POS - DESIGNATOR_POS)) / np.dot(direction_2_vector(TARGET_DIRECTION), ray_vector)
    ray_hit_pos = DESIGNATOR_POS + (ray_vector * t)
    ax3d.plot([DESIGNATOR_POS[0], ray_hit_pos[0]], [DESIGNATOR_POS[1], ray_hit_pos[1]], [DESIGNATOR_POS[2], ray_hit_pos[2]], 'k-', alpha=0.1) # 指示器到目标
    # 检查该光线是否被导引头接收
    if angle_direction_2_direction(TARGET_DIRECTION, vector_2_direction(TARGET_POS, SEEKER_POS)) < (np.pi/2) and angle_direction_2_direction(SEEKER_DIRECTION, vector_2_direction(SEEKER_POS, TARGET_POS)) <= SEEKER_FOV:
        ax3d.plot([ray_hit_pos[0], SEEKER_POS[0]], [ray_hit_pos[1], SEEKER_POS[1]], [ray_hit_pos[2], SEEKER_POS[2]], 'g--', alpha=0.1) # 目标到导引头

ax3d.legend()
ax3d.set_xlim(0, MAP_SIZE)
ax3d.set_ylim(0, MAP_SIZE)
ax3d.set_zlim(0, MAP_SIZE)
ax3d.set_xlabel('X [m]')
ax3d.set_ylabel('Y [m]')
ax3d.set_zlabel('Z [m]')
ax3d.set_title('3D Visualization of Laser Path (Designator -> Target -> Seeker)')

plt.show()

matlab

编辑

% MATLAB 实现激光导引头仿真系统
% 此代码与Python版本功能完全相同，用于仿真激光指示器、目标、导引头之间的光束传播和探测器成像

% FUNCTIONS MODULE - 定义用于仿真计算的各种辅助函数
function R = normal_line(direction)
    % 将方向角转换为单位向量 (辅助函数，未在主程序中使用)
    az = direction(1);
    al = direction(2);
    R = [cos(al) * cos(az); sin(al) * cos(az); sin(az)];
end

function R = gen_noise_profile(N, angle)
    % 生成光束发散角的噪声分布。
    % 模拟激光束在传播过程中的角度抖动。
    sigma = angle / 4; % 假设发散角为高斯分布的标准差
    R = cell(1, 3); % 创建元胞数组存储 [roll, pitch, yaw]
    pitch = zeros(1, N);
    yaw = zeros(1, N);
    roll = zeros(1, N);
    r = sigma * randn(1, N); % 生成N个随机偏转角
    theta = rand(1, N) * 180; % 随机方向 (0-180度)
    for i = 1:N
        pitch(i) = r(i) * cosd(theta(i)); % 偏转分解到pitch
        yaw(i) = r(i) * sind(theta(i));   % 偏转分解到yaw
        roll(i) = 0.0; % 滚转角设为0
    end
    R{1} = yaw;   % 偏航角
    R{2} = pitch; % 俯仰角
    R{3} = roll;  % 滚转角
end

function R = vector_2_direction(source, target)
    % 计算从源点到目标点的方向角。
    % 返回 [方位角, 仰角, 滚转角(0)]
    v = target - source;
    r = norm(v);
    if r == 0
        R = [0, 0, 0]; % 避免除零
        return;
    end
    theta = asin(v(3) / r); % 仰角
    phi = atan2(v(2), v(1)); % 方位角
    phi = mod(phi, 2*pi); % 确保方位角在 [0, 2π)
    R = [phi, theta, 0];
end

function R = vector_2_range(source, target)
    % 计算两点之间的距离
    v = target - source;
    R = norm(v);
end

function R = direction_2_vector(dir)
    % 将方向角 [方位角, 仰角, 滚转角] 转换为单位向量。
    % 滚转角在此简化模型中被忽略。
    r = 1;
    phi = dir(1); % 方位角
    theta = dir(2); % 仰角
    x = r * cos(theta) * cos(phi);
    y = r * cos(theta) * sin(phi);
    z = r * sin(theta);
    R = [x; y; z];
end

function R = vector_2_magnitude(v)
    % 计算向量的模长
    R = norm(v);
end

function R = angle_direction_2_direction(d1, d2)
    % 计算两个方向角之间的夹角。
    % 使用向量点积计算夹角。
    v1 = direction_2_vector(d1);
    v2 = direction_2_vector(d2);
    cos_angle = dot(v1, v2) / (norm(v1) * norm(v2)); % 点积公式
    cos_angle = max(-1, min(1, cos_angle)); % 防止数值误差导致acos越界
    R = acos(cos_angle);
end

% SIMULATOR PARAMETER
MAP_SIZE = 200;  % [m] 地图大小

% ANALYSIS PARAMETER
RAY_TOTAL_N = 1000; % 光线总数，用于蒙特卡洛仿真
ATM_LOSS_COEFF = 1.22e-4; % [/m] 大气损耗系数

% DESIGNATOR PARAMETER (激光指示器)
DESIGNATOR_POS = [0, 0, 0];  % [m] x, y, z
DESIGNATOR_DIRECTION = [0, 0, 0];  % [rad] az, al, r (指向目标的初始方向角)
DESIGNATOR_POWER = 100;  % [W] 激光器功率
DESIGNATOR_BEAM_ANGLE = 0.002;  % [rad] 激光发散角

% SEEKER PARAMETER (激光导引头)
SEEKER_POS = [0, 0, 100];  % [m] x, y, z (初始位置，假设在高空)
SEEKER_DIRECTION = [0, 0, 0];  % [rad] az, al, roll (初始指向)
SEEKER_SENSOR_SIZE = 0.005;  % [m] (5mm) 探测器尺寸
SEEKER_APERTURE_AREA = 0.0016;  % [m^2] (4cm * 4cm) 光学孔径面积
SEEKER_FOV = 0.174533; % [rad] (10deg) 导引头视场角

% TARGET PARAMETER (目标)
TARGET_POS = [300, 1, 0];  % [m] x, y, z (目标位置，有微小偏移)

% ETC (临时变量)
TMP = 0;
ERR = 0;
sensor_x = [];
sensor_y = [];
sensor_total_intensity = 0.0; % 累计接收光强，初始化为浮点数

% --- 仿真主循环 ---

% 1. 生成光束噪声分布
noise_cell = gen_noise_profile(RAY_TOTAL_N, DESIGNATOR_BEAM_ANGLE);
% 从元胞数组中提取噪声数组
noise_yaw = noise_cell{1};
noise_pitch = noise_cell{2};
% noise_roll = noise_cell{3}; % 滚转角为0，未使用

% 2. 计算各组件之间的初始视线方向
DESIGNATOR_DIRECTION = vector_2_direction(DESIGNATOR_POS, TARGET_POS);
TARGET_DIRECTION = vector_2_direction(TARGET_POS, SEEKER_POS); % 目标指向导引头
SEEKER_DIRECTION = vector_2_direction(SEEKER_POS, TARGET_POS); % 导引头指向目标

% 3. 开始蒙特卡洛仿真，追踪每一条光线
for i = 1:RAY_TOTAL_N
    % 3.1 每条光线携带的能量
    intensity = DESIGNATOR_POWER / RAY_TOTAL_N;
    
    % 3.2 为光线添加发散角噪声
    ray_direction = [
        (DESIGNATOR_DIRECTION(1) + noise_yaw(i)), % 偏航角
        (DESIGNATOR_DIRECTION(2) + noise_pitch(i)), % 俯仰角
        TMP % 滚转角
    ];
    ray_vector = direction_2_vector(ray_direction);

    % 3.3 计算光线是否打中目标 (光线与目标平面的交点)
    t = dot(direction_2_vector(TARGET_DIRECTION), (TARGET_POS - DESIGNATOR_POS)) / dot(direction_2_vector(TARGET_DIRECTION), ray_vector);
    ray_hit_pos = DESIGNATOR_POS + (ray_vector.' * t); % 注意MATLAB中向量是列向量，需要转置
    
    % 3.4 计算从指示器到目标的传播距离
    ray_length_designator_2_target = abs(vector_2_range(ray_hit_pos, DESIGNATOR_POS));

    % 3.5 计算大气损耗
    atm_loss_designator_2_target = exp(-ATM_LOSS_COEFF * ray_length_designator_2_target);
    intensity_designator_2_target = atm_loss_designator_2_target * intensity;

    % --- 3.6 计算目标反射到导引头 ---
    direction_target_2_seeker = vector_2_direction(TARGET_POS, SEEKER_POS);
    angle_target_2_seeker = angle_direction_2_direction(TARGET_DIRECTION, direction_target_2_seeker); % 入射角
    direction_seeker_2_target = vector_2_direction(SEEKER_POS, TARGET_POS);
    angle_seeker_2_target = angle_direction_2_direction(SEEKER_DIRECTION, direction_seeker_2_target); % 接收角

    % 3.7 判断光线是否在导引头视场内
    if angle_target_2_seeker < (pi/2) && angle_seeker_2_target <= SEEKER_FOV
        % 3.8 模拟目标漫反射 (Lambertian)
        intensity_target_2_seeker = intensity_designator_2_target / pi * cos(angle_target_2_seeker);
        
        % 3.9 计算从目标到导引头的距离
        length_target_2_seeker = abs(vector_2_range(ray_hit_pos, SEEKER_POS));
        
        % 3.10 计算大气损耗
        atmosphere_loss_target_2_seeker = exp(-ATM_LOSS_COEFF * length_target_2_seeker);
        
        % 3.11 计算立体角 (导引头孔径面积/距离平方 * cos接收角)
        solid_angle_seeker = SEEKER_APERTURE_AREA / (length_target_2_seeker^2) * cos(angle_seeker_2_target);

        % 3.12 计算最终到达探测器的光强
        intensity_seeker = intensity_target_2_seeker * solid_angle_seeker * atmosphere_loss_target_2_seeker;
        sensor_total_intensity = sensor_total_intensity + intensity_seeker; % 累加光强

        % --- 3.13 计算光线在探测器上的落点 ---
        direction_seeker_2_ray = vector_2_direction(SEEKER_POS, ray_hit_pos);
        vector_seeker_2_ray = ray_hit_pos - SEEKER_POS;
        
        % 3.14 定义探测器焦平面
        plane_bias = SEEKER_SENSOR_SIZE / (2 * tan(SEEKER_FOV / 2)); % 焦距估算
        plane_pos = SEEKER_POS + direction_2_vector(SEEKER_DIRECTION).' * plane_bias; % 转置

        % 3.15 计算光线与焦平面的交点
        t = dot(direction_2_vector(SEEKER_DIRECTION), (plane_pos - SEEKER_POS)) / dot(direction_2_vector(SEEKER_DIRECTION), vector_seeker_2_ray);
        plane_hit_pos = SEEKER_POS + (vector_seeker_2_ray * t);
        
        % 3.16 建立焦平面坐标系 U, V
        vector_plane_U = cross([0, 0, 1], direction_2_vector(SEEKER_DIRECTION).'); % 转置
        if norm(vector_plane_U) < 1e-6 % 如果视线平行于Z轴
            vector_plane_U = cross([1, 0, 0], direction_2_vector(SEEKER_DIRECTION).'); % 转置
        end
        vector_plane_V = cross(direction_2_vector(SEEKER_DIRECTION).', vector_plane_U); % 转置
        vector_plane_dot = plane_hit_pos - plane_pos;
        
        % 3.17 计算在焦平面上的U, V坐标
        u = dot(vector_plane_dot, vector_plane_U) / dot(vector_plane_U, vector_plane_U);
        v = dot(vector_plane_dot, vector_plane_V) / dot(vector_plane_V, vector_plane_V);
        
        % 3.18 记录坐标
        sensor_x(end+1) = u; % MATLAB中数组追加元素
        sensor_y(end+1) = v;
    end
end

% 输出仿真结果
fprintf('Total Intensity Received by Seeker: %.6f W\n', sensor_total_intensity);

% --- 可视化 ---
% 2D 探测器成像图
figure;
plot(sensor_x, sensor_y, 'bo', 'MarkerSize', 1, 'MarkerFaceAlpha', 0.6);
xlim([-SEEKER_SENSOR_SIZE/2, SEEKER_SENSOR_SIZE/2]);
ylim([-SEEKER_SENSOR_SIZE/2, SEEKER_SENSOR_SIZE/2]);
xlabel('Detector X [m]');
ylabel('Detector Y [m]');
title('Laser Spot Distribution on Seeker Detector');
grid on;

% 3D 空间图
figure;
ax = axes('projection', '3d');
scatter3(DESIGNATOR_POS(1), DESIGNATOR_POS(2), DESIGNATOR_POS(3), 100, 'red', 'filled', 'DisplayName', 'Laser Designator');
hold on;
scatter3(TARGET_POS(1), TARGET_POS(2), TARGET_POS(3), 100, 'green', 'filled', 'DisplayName', 'Target');
scatter3(SEEKER_POS(1), SEEKER_POS(2), SEEKER_POS(3), 100, 'blue', 'filled', 'DisplayName', 'Laser Seeker');

% 绘制光线路径 (仅绘制部分光线以避免过于密集)
for i = 1:100:RAY_TOTAL_N % 每100条光线画一条
    ray_direction_noisy = [
        (DESIGNATOR_DIRECTION(1) + noise_yaw(i)),
        (DESIGNATOR_DIRECTION(2) + noise_pitch(i)),
        TMP
    ];
    ray_vector = direction_2_vector(ray_direction_noisy);
    t = dot(direction_2_vector(TARGET_DIRECTION), (TARGET_POS - DESIGNATOR_POS)) / dot(direction_2_vector(TARGET_DIRECTION), ray_vector);
    ray_hit_pos = DESIGNATOR_POS + (ray_vector.' * t);
    plot3([DESIGNATOR_POS(1), ray_hit_pos(1)], [DESIGNATOR_POS(2), ray_hit_pos(2)], [DESIGNATOR_POS(3), ray_hit_pos(3)], 'k-', 'Alpha', 0.1);
    
    % 检查该光线是否被导引头接收
    if angle_direction_2_direction(TARGET_DIRECTION, vector_2_direction(TARGET_POS, SEEKER_POS)) < (pi/2) && ...
       angle_direction_2_direction(SEEKER_DIRECTION, vector_2_direction(SEEKER_POS, TARGET_POS)) <= SEEKER_FOV
        plot3([ray_hit_pos(1), SEEKER_POS(1)], [ray_hit_pos(2), SEEKER_POS(2)], [ray_hit_pos(3), SEEKER_POS(3)], 'g--', 'Alpha', 0.1);
    end
end

legend show;
xlim([0, MAP_SIZE]);
ylim([0, MAP_SIZE]);
zlim([0, MAP_SIZE]);
xlabel('X [m]');
ylabel('Y [m]');
zlabel('Z [m]');
title('3D Visualization of Laser Path (Designator -> Target -> Seeker)');

% 激光导引头仿真系统 - 完全重构版
% 此代码模拟了激光指示器、目标和激光导引头之间的光束传播过程。
% 它追踪大量光线，计算它们在探测器上的落点和接收光强。
% 该版本严格遵循MATLAB向量处理规范。

%% 定义辅助函数 (必须在脚本末尾或单独的.m文件中定义)
function R = gen_noise_profile(N, angle)
    % 生成光束发散角的噪声分布
    sigma = angle / 4;
    r = sigma * randn(1, N);
    theta = rand(1, N) * 2 * pi; % 使用弧度
    pitch = r .* cos(theta);
    yaw = r .* sin(theta);
    roll = zeros(1, N);
    % 返回一个包含三个向量的结构体，更清晰
    R.pitch = pitch;
    R.yaw = yaw;
    R.roll = roll;
end

function R = vector_2_direction(source, target)
    % 计算从源点到目标点的方向角
    % 输入: 1x3 行向量
    v = target - source;
    r = norm(v);
    if r == 0
        R = [0, 0, 0];
        return;
    end
    theta = asin(v(3) / r); % 仰角
    phi = atan2(v(2), v(1)); % 方位角
    phi = mod(phi, 2*pi);
    R = [phi, theta, 0];
end

function R = vector_2_range(source, target)
    % 计算两点之间的距离
    % 输入: 1x3 行向量
    v = target - source;
    R = norm(v);
end

function R = direction_2_vector(dir)
    % 将方向角转换为单位向量
    % 输入: 1x3 行向量 [az, el, roll]
    % 输出: 3x1 列向量
    r = 1;
    phi = dir(1); % 方位角
    theta = dir(2); % 仰角
    x = r * cos(theta) * cos(phi);
    y = r * cos(theta) * sin(phi);
    z = r * sin(theta);
    R = [x; y; z]; % 返回列向量
end

function R = angle_direction_2_direction(d1, d2)
    % 计算两个方向角之间的夹角
    % 输入: 1x3 行向量
    v1 = direction_2_vector(d1); % 得到 3x1 列向量
    v2 = direction_2_vector(d2); % 得到 3x1 列向量
    cos_angle = dot(v1, v2) / (norm(v1) * norm(v2));
    % 限制范围以避免数值误差
    cos_angle = max(-1, min(1, cos_angle));
    R = acos(cos_angle);
end

%% 主仿真参数
MAP_SIZE = 200;  % [m]
RAY_TOTAL_N = 1000;
ATM_LOSS_COEFF = 1.22e-4; % [/m]

% 激光指示器参数
DESIGNATOR_POS = [0, 0, 0];  % [m] - 1x3 行向量
DESIGNATOR_POWER = 100;  % [W]
DESIGNATOR_BEAM_ANGLE = 0.002;  % [rad]

% 激光导引头参数
SEEKER_POS = [0, 0, 100];  % [m] - 1x3 行向量
SEEKER_SENSOR_SIZE = 0.005;  % [m]
SEEKER_APERTURE_AREA = 0.0016;  % [m^2]
SEEKER_FOV = 0.174533; % [rad] (约10度)

% 目标参数
TARGET_POS = [300, 1, 0];  % [m] - 1x3 行向量

%% 初始化变量
sensor_x = [];
sensor_y = [];
sensor_total_intensity = 0.0;

%% 计算初始视线方向
DESIGNATOR_DIRECTION = vector_2_direction(DESIGNATOR_POS, TARGET_POS);
TARGET_DIRECTION = vector_2_direction(TARGET_POS, SEEKER_POS);
SEEKER_DIRECTION = vector_2_direction(SEEKER_POS, TARGET_POS);

%% 生成光线噪声
noise_data = gen_noise_profile(RAY_TOTAL_N, DESIGNATOR_BEAM_ANGLE);

%% 仿真主循环
for i = 1:RAY_TOTAL_N
    % 每条光线的能量
    intensity_per_ray = DESIGNATOR_POWER / RAY_TOTAL_N;
    
    % 添加发散角噪声
    noisy_direction = [
        DESIGNATOR_DIRECTION(1) + noise_data.yaw(i),
        DESIGNATOR_DIRECTION(2) + noise_data.pitch(i),
        0 % 滚转角
    ];
    ray_vector = direction_2_vector(noisy_direction); % 得到 3x1 列向量
    
    % 计算光线与目标平面的交点
    % 目标平面法向量 (3x1 列向量)
    target_normal = direction_2_vector(TARGET_DIRECTION);
    % 参数 t 的计算 (光线: P = P0 + t*D, 平面: (P-P_plane).n = 0)
    % 代入得: (P0+t*D-P_plane).n = 0 -> t = (P_plane-P0).n / (D.n)
    numerator = dot(target_normal, (TARGET_POS' - DESIGNATOR_POS')); % P_plane - P0
    denominator = dot(target_normal, ray_vector); % D.n
    if abs(denominator) < 1e-10 % 避免除零或近似平行
        continue; % 该光线未打中目标
    end
    t = numerator / denominator;
    % 计算交点 (3x1 列向量)
    ray_hit_pos = DESIGNATOR_POS' + (ray_vector * t); % P0 + t*D
    
    % 计算传播距离和大气损耗
    dist_to_target = vector_2_range(DESIGNATOR_POS, ray_hit_pos'); % 需要转置回 1x3
    atm_loss_to_target = exp(-ATM_LOSS_COEFF * dist_to_target);
    intensity_at_target = atm_loss_to_target * intensity_per_ray;
    
    % 计算反射到导引头的光
    angle_to_seeker = angle_direction_2_direction(TARGET_DIRECTION, vector_2_direction(TARGET_POS, SEEKER_POS));
    angle_from_seeker = angle_direction_2_direction(SEEKER_DIRECTION, vector_2_direction(SEEKER_POS, TARGET_POS));
    
    % 检查是否在视场内
    if angle_to_seeker < (pi/2) && angle_from_seeker <= SEEKER_FOV
        % Lambertian反射模型
        intensity_reflected = intensity_at_target / pi * cos(angle_to_seeker);
        
        % 计算从目标到导引头的距离和损耗
        dist_to_seeker = vector_2_range(ray_hit_pos', SEEKER_POS); % 需要转置回 1x3
        atm_loss_to_seeker = exp(-ATM_LOSS_COEFF * dist_to_seeker);
        
        % 计算立体角
        solid_angle = SEEKER_APERTURE_AREA / (dist_to_seeker^2) * cos(angle_from_seeker);
        
        % 最终到达探测器的光强
        intensity_at_seeker = intensity_reflected * solid_angle * atm_loss_to_seeker;
        sensor_total_intensity = sensor_total_intensity + intensity_at_seeker;
        
        % 计算光线在探测器上的落点
        % 焦平面定义
        focal_length = SEEKER_SENSOR_SIZE / (2 * tan(SEEKER_FOV / 2));
        focal_plane_center = SEEKER_POS' + direction_2_vector(SEEKER_DIRECTION) * focal_length; % 3x1
        
        % 光线向量 (从导引头到光线交点)
        ray_to_seeker = ray_hit_pos - SEEKER_POS'; % 3x1
        
        % 计算光线与焦平面交点
        % t2 = dot(n, (P_plane_center - P_seeker)) / dot(n, ray_dir)
        t2 = dot(direction_2_vector(SEEKER_DIRECTION), (focal_plane_center - SEEKER_POS')) / ...
             dot(direction_2_vector(SEEKER_DIRECTION), ray_to_seeker);
        hit_on_focal_plane = SEEKER_POS' + (ray_to_seeker * t2); % 3x1
        
        % 定义焦平面坐标系 (U, V)
        % U轴：垂直于视线和Z轴（如果视线平行Z轴，则用X轴）
        temp_cross = cross([0; 0; 1], direction_2_vector(SEEKER_DIRECTION)); % 3x1
        if norm(temp_cross) < 1e-6
            vector_U = cross([1; 0; 0], direction_2_vector(SEEKER_DIRECTION)); % 3x1
        else
            vector_U = temp_cross; % 3x1
        end
        vector_U = vector_U / norm(vector_U); % 归一化
        vector_V = cross(direction_2_vector(SEEKER_DIRECTION), vector_U); % 3x1
        vector_V = vector_V / norm(vector_V); % 归一化
        
        % 投影到U, V坐标 (相对于焦平面中心)
        delta_pos = hit_on_focal_plane - focal_plane_center; % 3x1
        u = dot(delta_pos, vector_U); % 标量
        v = dot(delta_pos, vector_V); % 标量
        
        % 记录坐标
        sensor_x(end+1) = u;
        sensor_y(end+1) = v;
    end
end

%% 输出结果
fprintf('Total Intensity Received by Seeker: %.6f W\n', sensor_total_intensity);

%% 可视化
% 2D 探测器成像图
figure;
plot(sensor_x, sensor_y, 'bo', 'MarkerSize', 1, 'MarkerFaceAlpha', 0.6);
xlim([-SEEKER_SENSOR_SIZE/2, SEEKER_SENSOR_SIZE/2]);
ylim([-SEEKER_SENSOR_SIZE/2, SEEKER_SENSOR_SIZE/2]);
xlabel('Detector X [m]');
ylabel('Detector Y [m]');
title('Laser Spot Distribution on Seeker Detector');
grid on;

% 3D 空间图
figure;
ax = axes('Projection', '3d');
scatter3(DESIGNATOR_POS(1), DESIGNATOR_POS(2), DESIGNATOR_POS(3), 100, 'red', 'filled', 'DisplayName', 'Laser Designator');
hold on;
scatter3(TARGET_POS(1), TARGET_POS(2), TARGET_POS(3), 100, 'green', 'filled', 'DisplayName', 'Target');
scatter3(SEEKER_POS(1), SEEKER_POS(2), SEEKER_POS(3), 100, 'blue', 'filled', 'DisplayName', 'Laser Seeker');

% 绘制部分光线路径 (为避免卡顿，只绘制少量)
for i = 1:floor(RAY_TOTAL_N/10):RAY_TOTAL_N % 每100条光线画一条
    noisy_direction = [
        DESIGNATOR_DIRECTION(1) + noise_data.yaw(i),
        DESIGNATOR_DIRECTION(2) + noise_data.pitch(i),
        0
    ];
    ray_vector = direction_2_vector(noisy_direction); % 3x1
    numerator = dot(direction_2_vector(TARGET_DIRECTION), (TARGET_POS' - DESIGNATOR_POS'));
    denominator = dot(direction_2_vector(TARGET_DIRECTION), ray_vector);
    if abs(denominator) > 1e-10
        t = numerator / denominator;
        ray_hit_pos = DESIGNATOR_POS' + (ray_vector * t); % 3x1
        plot3([DESIGNATOR_POS(1), ray_hit_pos(1)], [DESIGNATOR_POS(2), ray_hit_pos(2)], [DESIGNATOR_POS(3), ray_hit_pos(3)], 'k-', 'Alpha', 0.1);
        
        % 检查是否被接收
        angle_to_seeker = angle_direction_2_direction(TARGET_DIRECTION, vector_2_direction(TARGET_POS, SEEKER_POS));
        angle_from_seeker = angle_direction_2_direction(SEEKER_DIRECTION, vector_2_direction(SEEKER_POS, TARGET_POS));
        if angle_to_seeker < (pi/2) && angle_from_seeker <= SEEKER_FOV
            plot3([ray_hit_pos(1), SEEKER_POS(1)], [ray_hit_pos(2), SEEKER_POS(2)], [ray_hit_pos(3), SEEKER_POS(3)], 'g--', 'Alpha', 0.1);
        end
    end
end

legend;
xlim([0, MAP_SIZE]);
ylim([0, MAP_SIZE]);
zlim([0, MAP_SIZE]);
xlabel('X [m]');
ylabel('Y [m]');
zlabel('Z [m]');
title('3D Visualization of Laser Path (Designator -> Target -> Seeker)');