graph TD %% ================= 外部输入 ================= Ref[期望位置 p_des, yaw_des] --> PosPID

%% ================= 外环：位置与期望力 =================
subgraph Outer_Loop [1. 位置与意图层 (Outer Loop)]
    PosPID[位置 PID控制器] -- F_world_des --> RotMat[R^T 坐标变换]
    RotMat -- F_body_des --> SLSQP[SLSQP 姿态寻优]
    RotMat -- F_body_des --> MergeW((组装))
end

%% ================= 内环：姿态与角加速度 =================
subgraph Inner_Loop [2. 姿态与力矩层 (Inner Loop)]
    SLSQP -- roll_des, pitch_des --> AttP[姿态 P控制器]
    AttP -- w_des (期望角速度) --> RatePID[角速度 PID控制器]
    RatePID -- tau_des (期望力矩) --> MergeW
end

MergeW -- W_des (总期望受力) --> Sum_dW((+))

%% ================= 核心：INDI 与 NDOB =================
subgraph INDI_Allocation [3. INDI 分配与观测器层 (Control Allocation & NDOB)]
    Sum_dW -- dW (误差增量) --> INDI[INDI 雅可比伪逆计算<br>du = W_inv * G^T * (G*W_inv*G^T)^-1 * dW]
    
    INDI -- du (动作增量) --> Add_u((+))
    Delay[执行器状态延迟 z^-1<br>获取 u_real] --> Add_u
    Delay --> INDI
    Delay --> FwdModel[几何正运动学模型<br>W_model = f(u_real)]
    
    FwdModel -- W_model --> Sum_dW
    FwdModel -- W_model --> Sum_NDOB((+))
    
    Sum_NDOB -- W_dist_raw --> LPF_NDOB[低通滤波器 LPF]
    LPF_NDOB -- W_dist_est (估算干扰) --> Sum_dW
end

%% 注意 Sum_dW 的符号逻辑： dW = W_des - W_model - W_dist_est
Sum_dW -. "-" .-> FwdModel
Sum_dW -. "-" .-> LPF_NDOB

%% ================= 物理执行与反馈 =================
subgraph Physical_Plant [4. 物理世界与传感器 (Plant & Sensors)]
    Add_u -- u_target --> Actuators[电机与舵机执行器]
    Actuators --> Plant[BTAU 无人机物理动力学<br>含未建模扰动与反扭矩]
    Plant --> IMU[IMU & 里程计]
end

%% ================= 反馈回路 =================
IMU -- p_real --> PosPID
IMU -- Att_real --> RotMat
IMU -- Att_real --> AttP
IMU -- w_real --> RatePID
IMU -- W_imu (根据牛顿欧拉方程算出真实受力) --> Sum_NDOB

%% 注意 Sum_NDOB 的符号逻辑：W_dist_raw = W_imu - W_model
Sum_NDOB -. "-" .-> FwdModel

%% 执行器状态闭环
Actuators -- u_real_feedback --> Delay