Fix teaching-content issues and UKF numerical stability

- t22: Lead compensator (s+0.5)/(s+5) no longer cancels plant pole at s=-2
- t23: rewrite as standard MRAC (same pole, different DC gain b_p);
       theta* = b_m/b_p = 2, parameter jump changes b_p so theta* -> 4
- t25: honest rename to force-balance modeling; drop bogus ver('simscape')
       check (physical ports can't be wired programmatically)
- t26: DH table now the correct 6-joint PUMA560, matching the computation
- t33/t34: UKF alpha 1e-3 -> 0.5, removes W0 ~ -1e6 weight degeneracy

Author: xingd5478-ctrl

t22_root_locus.m

@@ -174,18 +174,18 @@
 title('目标：ζ>0.7, ωn>3 的区域');
 % 原始根轨迹偏右 → 需要 Lead 补偿器把轨迹往左拉
 
-% 加一个零点 s=-2（把极点往左吸）
-C_lead = tf([1, 2], 1);  % 零点在 -2
+% 加一个 Lead 补偿器 C(s)=(s+0.5)/(s+5)：零点把轨迹往左吸，极点保证可实现
+C_lead = tf([1, 0.5], [1, 5]);  % 零点 -0.5，极点 -5（proper，无零极点对消）
 sys_comp = C_lead * sys;
 subplot(1,2,2);
 rlocus(sys_comp); grid on;
 sgrid(0.7, 3);
-title('加零点 s=-2 后：根轨迹被"吸"向左');
+title('加 Lead 补偿器后：根轨迹被"吸"向左');
 hold off;
 
 fprintf('【补偿器设计演示】\n');
 fprintf('  原始系统: 根轨迹偏右 → 响应慢\n');
-fprintf('  加零点 s=-2: 轨迹向左弯 → 更快更稳定\n');
+fprintf('  加 Lead 补偿器 (s+0.5)/(s+5): 零点吸轨迹向左 → 更快更稳定\n');
 fprintf('  这就是 Lead 补偿器的根轨迹解释！\n\n');
 
 fprintf('========================================\n');

t23_mrac.m

@@ -32,17 +32,18 @@
 %% ===== 第 1 步：参考模型与被控对象 =====
 
 % 参考模型：我们希望系统表现成的样子
-tau_m = 0.5;   % 参考时间常数（快！）
-sys_model = tf([1], [tau_m, 1]);
+tau = 0.5;    % 公共时间常数（对象和模型相同，这是 MRAC 能完全匹配的前提）
+b_m = 1.0;    % 参考模型直流增益
+sys_model = tf([b_m], [tau, 1]);
 
-% 真实被控对象：参数会变化（我们假装不知道）
-tau_real = 2.0;  % 真实时间常数（比参考模型慢很多）
-sys_real = tf([1], [tau_real, 1]);
+% 真实被控对象：增益未知（比模型小），时间常数与模型相同
+b_p = 0.5;    % 真实对象直流增益（控制器不知道的真实值）
+sys_real = tf([b_p], [tau, 1]);
 
 fprintf('【系统参数】\n');
-fprintf('  参考模型: G_m(s) = 1/(%.1fs+1) — 理想行为（快）\n', tau_m);
-fprintf('  真实系统: G_p(s) = 1/(%.1fs+1) — 实际行为（慢 4 倍）\n', tau_real);
-fprintf('  目标: 通过自适应调整增益，让真实系统追平参考模型\n\n');
+fprintf('  参考模型: G_m(s) = %.0f/(%.1fs+1) — 理想行为\n', b_m, tau);
+fprintf('  真实系统: G_p(s) = %.1f/(%.1fs+1) — 增益未知(真实 %.1f)\n', b_p, tau, b_p);
+fprintf('  目标: 自适应增益 θ 补偿增益差异，使 θ·b_p → b_m\n\n');
 
 %% ===== 第 2 步：Simulink 模型搭建 =====
 
@@ -59,7 +60,7 @@
 add_block('simulink/Continuous/Transfer Fcn', [mdl '/Ref Model'], ...
     'Position', [350, 50, 440, 90]);
 set_param([mdl '/Ref Model'], ...
-    'Numerator', '[1]', 'Denominator', ['[' num2str(tau_m) ' 1]']);
+    'Numerator', num2str(b_m), 'Denominator', ['[' num2str(tau) ' 1]']);
 
 % --- 自适应控制器路径 ---
 % Apply Gain 模块：计算 θ(t) * r(t)，直接驱动被控对象
@@ -70,7 +71,7 @@
 add_block('simulink/Continuous/Transfer Fcn', [mdl '/Real Plant'], ...
     'Position', [350, 160, 440, 200]);
 set_param([mdl '/Real Plant'], ...
-    'Numerator', '[1]', 'Denominator', ['[' num2str(tau_real) ' 1]']);
+    'Numerator', num2str(b_p), 'Denominator', ['[' num2str(tau) ' 1]']);
 
 % --- 固定增益对比路径 ---
 add_block('simulink/Math Operations/Gain', [mdl '/Fixed Gain'], ...
@@ -80,7 +81,7 @@
 add_block('simulink/Continuous/Transfer Fcn', [mdl '/Real Plant Fixed'], ...
     'Position', [350, 310, 440, 350]);
 set_param([mdl '/Real Plant Fixed'], ...
-    'Numerator', '[1]', 'Denominator', ['[' num2str(tau_real) ' 1]']);
+    'Numerator', num2str(b_p), 'Denominator', ['[' num2str(tau) ' 1]']);
 
 % --- MIT 自适应律：θ̇ = -γ · e · ym ---
 add_block('simulink/Math Operations/Add', [mdl '/Error Calc'], ...
@@ -167,16 +168,16 @@
 
 % 欧拉仿真
 for k = 1:N-1
-    % 参考模型: τm·ẏm + ym = r
-    ym_dot = (r(k) - ym(k)) / tau_m;
+    % 参考模型: τ·ẏm + ym = b_m·r
+    ym_dot = (b_m * r(k) - ym(k)) / tau;
     ym(k+1) = ym(k) + ym_dot * dt;
 
-    % 自适应对象: τp·ẏp + yp = θ·r
-    yp_dot = (theta(k) * r(k) - yp_adapt(k)) / tau_real;
+    % 自适应对象: τ·ẏp + yp = θ·b_p·r
+    yp_dot = (theta(k) * b_p * r(k) - yp_adapt(k)) / tau;
     yp_adapt(k+1) = yp_adapt(k) + yp_dot * dt;
 
     % 固定增益对象
-    yp_dot_f = (K_fixed * r(k) - yp_fixed(k)) / tau_real;
+    yp_dot_f = (K_fixed * b_p * r(k) - yp_fixed(k)) / tau;
     yp_fixed(k+1) = yp_fixed(k) + yp_dot_f * dt;
 
     % MIT 规则：dθ/dt = -γ · (yp - ym) · ym
@@ -187,30 +188,30 @@
 
 %% ===== 第 4 步：系统参数突变 — 自适应 vs 固定 =====
 
-% 5 秒时 τ_real 从 2.0 突变为 4.0
-tau_init = tau_real;
-tau_change_time = 5;
-tau_changed = 4.0;
+% 5 秒时对象增益 b_p 从 0.5 突变为 0.25
+b_p_init = b_p;
+b_change_time = 5;
+b_p_changed = 0.25;
 
 ym2 = zeros(1, N);  yp_a2 = zeros(1, N);
 yp_f2 = zeros(1, N);  theta2 = zeros(1, N);
 theta2(1) = 1.0;
 
 for k = 1:N-1
-    % 当前真实时间常数
-    if t(k) < tau_change_time
-        tau_now = tau_init;
+    % 当前真实对象增益
+    if t(k) < b_change_time
+        b_now = b_p_init;
     else
-        tau_now = tau_changed;
+        b_now = b_p_changed;
     end
 
-    ym_dot = (r(k) - ym2(k)) / tau_m;
+    ym_dot = (b_m * r(k) - ym2(k)) / tau;
     ym2(k+1) = ym2(k) + ym_dot * dt;
 
-    yp_dot = (theta2(k) * r(k) - yp_a2(k)) / tau_now;
+    yp_dot = (theta2(k) * b_now * r(k) - yp_a2(k)) / tau;
     yp_a2(k+1) = yp_a2(k) + yp_dot * dt;
 
-    yp_dot_f = (K_fixed * r(k) - yp_f2(k)) / tau_now;
+    yp_dot_f = (K_fixed * b_now * r(k) - yp_f2(k)) / tau;
     yp_f2(k+1) = yp_f2(k) + yp_dot_f * dt;
 
     e_adapt = yp_a2(k) - ym2(k);
@@ -236,11 +237,11 @@
 % 图2：自适应增益变化
 subplot(2,2,2); hold on;
 plot(t, theta, 'r', 'LineWidth', 1.5);
-yline(1, 'b--', 'LineWidth', 1);  % 理论最优值 θ* = 1（直流增益匹配）
-text(t_end*0.6, 1*1.1, '理论最优 θ*=1（直流增益匹配）');
+yline(b_m/b_p, 'b--', 'LineWidth', 1);  % 理论最优 θ* = b_m/b_p
+text(t_end*0.6, b_m/b_p*1.1, sprintf('理论最优 θ*=b_m/b_p=%.1f', b_m/b_p));
 title('自适应增益 θ(t)');
 xlabel('时间 (s)'); ylabel('增益'); grid on;
-fprintf('  理论最优增益: θ* = 1 (直流增益匹配，G_p(0)=G_m(0)=1)\n');
+fprintf('  理论最优增益: θ* = %.1f (b_m/b_p = %.1f/%.1f)\n', b_m/b_p, b_m, b_p);
 fprintf('  MIT 自适应最终收敛于 θ* 附近 ✓\n\n');
 
 % 图3：参数突变工况
@@ -249,18 +250,19 @@
 plot(t, ym2, 'b', 'LineWidth', 1.5);
 plot(t, yp_a2, 'r', 'LineWidth', 1.5);
 plot(t, yp_f2, 'Color', [0 0.6 0], 'LineWidth', 1);
-xline(tau_change_time, 'k:', 'LineWidth', 1.5);
-text(tau_change_time+0.2, 2.5, '参数突变!');
+xline(b_change_time, 'k:', 'LineWidth', 1.5);
+text(b_change_time+0.2, 2.5, '参数突变!');
 legend('目标', '参考模型', '自适应', '固定增益', 'Location', 'southeast');
-title(sprintf('参数突变 (τ: %.1f→%.1f) 时对比', tau_init, tau_changed));
+title(sprintf('参数突变 (b_p: %.2f→%.2f) 时对比', b_p_init, b_p_changed));
 xlabel('时间 (s)'); ylabel('输出'); grid on;
 
 % 图4：突变后增益重新调整
 subplot(2,2,4); hold on;
 plot(t, theta2, 'r', 'LineWidth', 1.5);
-yline(1, 'b--', 'LineWidth', 1);
-xline(tau_change_time, 'k:', 'LineWidth', 1.5);
-legend('θ(t)', 'θ*=1', 'Location', 'best');
+yline(b_m/b_p_init, 'b--', 'LineWidth', 1);
+yline(b_m/b_p_changed, 'g--', 'LineWidth', 1);
+xline(b_change_time, 'k:', 'LineWidth', 1.5);
+legend('θ(t)', '旧最优值', '新最优值', 'Location', 'best');
 title('参数突变后增益自适应重新收敛');
 xlabel('时间 (s)'); ylabel('增益'); grid on;
 

t25_simscape.m

@@ -1,13 +1,16 @@
 %% ============================================================
-% 教程 25：Simscape 物理建模 — 不用推公式的建模方法
+% 教程 25：物理建模 — 力平衡法 vs 传递函数
 %
-% 【Simscape vs 传统 Simulink】
-%   传统 Simulink: 你先推出传递函数 G(s)，再用 Transfer Fcn 模块
-%   Simscape: 你直接放置物理元件（电阻、弹簧、质量块），
-%             连线代表物理连接（电流、力），系统自动推导方程！
+% 【物理建模 vs 数学建模】
+%   数学建模: 先推出传递函数 G(s)，再用 Transfer Fcn 模块
+%   物理建模（力平衡法）: 按 F=ma 直接搭"加速度→速度→位移"积分链，
+%                        不写传递函数，思路更贴近物理直觉
 %
-%   Simulink 是"数学建模"：G(s) = 1/(s²+2s+5)
-%   Simscape 是"物理建模"：放一块质量 + 一个弹簧 + 一个阻尼器
+%   传递函数是"数学建模"：G(s) = 1/(s²+0.5s+4)
+%   力平衡法是"物理建模"：按 F=ma 搭积分链（本课实际演示的方法）
+%
+%   注：MathWorks 的 Simscape 工具箱可用物理元件（Mass/Spring/Damper）
+%       做更彻底的物理建模，但本课用 Simulink 信号框图演示同一思路
 %
 % 【Simscape 的优势】
 %   1. 不需要推导数学方程 → 减少人为错误
@@ -25,18 +28,15 @@
 clear; close all;
 
 fprintf('============================================\n');
-fprintf('  教程 25：Simscape 物理建模\n');
+fprintf('  教程 25：物理建模（力平衡法）\n');
 fprintf('============================================\n\n');
 
-% 检查 Simscape 是否可用
-if isempty(ver('simscape'))
-    error('需要 Simscape 工具箱。如果没有安装，请阅读本脚本了解概念。');
-end
+% 本课用 Simulink 信号框图演示力平衡法，无需 Simscape 工具箱
 
-fprintf('【Simscape 核心理念】\n');
+fprintf('【力平衡法核心理念】\n');
 fprintf('  传统方法: 物理定律 → 微分方程 → 传递函数 → Transfer Fcn 模块\n');
-fprintf('  Simscape: 物理元件 → 连线 → 自动生成方程 → 仿真\n');
-fprintf('  → 省掉了最容易出错的三步推导！\n\n');
+fprintf('  力平衡法: F=ma → 加速度 → 积分 → 速度/位移（不写 G(s)）\n');
+fprintf('  → 省掉了拉普拉斯变换这一步！\n\n');
 
 %% ===== 第 1 步：物理建模 vs 传递函数对比模型 =====
 
@@ -111,7 +111,7 @@
 add_line(mdl, 'Step Force/1', 'MSD Transfer Fcn/1');
 add_line(mdl, 'MSD Transfer Fcn/1', 'Scope/2');
 
-fprintf('【Simscape 概念模型已创建】tutorial25_simscape.slx\n');
+fprintf('【物理模型已创建】tutorial25_simscape.slx\n');
 fprintf('  左侧: MSD 物理模型 (力平衡 + 积分链)\n');
 fprintf('  右侧: 传统传递函数 G(s)=1/(s²+0.5s+4)\n');
 fprintf('  两条路径结果一致，但建模思路完全不同\n\n');
@@ -191,9 +191,9 @@
 fprintf('    3. 拉普拉斯: G(s) = 1/(ms²+cs+k)\n');
 fprintf('    4. 填入 Transfer Fcn 模块\n');
 fprintf('    → 三步推导，一步填错全错\n\n');
-fprintf('  Simscape 流程:\n');
-fprintf('    1. 拖入 Mass, Spring, Damper 元件\n');
-fprintf('    2. 连线（物理连接，不是信号流向）\n');
+fprintf('  力平衡法流程:\n');
+fprintf('    1. 画 F=ma 力平衡图（F_net = F - cv - kx）\n');
+fprintf('    2. 用 Gain/Integrator 搭"加速度→速度→位移"链\n');
 fprintf('    3. 填参数: m=1, k=4, c=0.5\n');
 fprintf('    4. 运行！\n');
 fprintf('    → 零公式推导，直接来自物理直觉\n\n');
@@ -206,8 +206,8 @@
 fprintf('  ✗ 已有精确传递函数 → 没必要重搭物理模型\n\n');
 
 fprintf('→ 打开 tutorial25_simscape.slx\n');
-fprintf('→ 双击 Mass 模块，把 mass 从 1 改成 5\n');
-fprintf('→ 重新运行 → 观察响应变慢\n');
+fprintf('→ 双击 c_damper 或 k_spring 模块改阻尼/刚度\n');
+fprintf('→ 重新运行 → 观察响应变化\n');
 fprintf('→ 这就是"物理直觉驱动建模"！\n');
 
 save_system(mdl, fullfile(fileparts(mfilename('fullpath')), 'models', [mdl '.slx']));

t26_robot_kinematics.m

@@ -44,17 +44,21 @@
 %% ---------- 第 2 步：读 PUMA560 的 DH 参数表 ----------
 % 每一行是 [a, α, d, θ]，θ 是变量（关节角）
 dh = [
-    0       0       0.672   0;       % Joint 1 — 腰部旋转
-    0.432  -pi/2   0.149   0;       % Joint 2 — 大臂俯仰
-    0.020   pi/2   0       0;       % Joint 3 — 小臂俯仰
-    0       0       0.433   0;       % Joint 4 — 腕部旋转
+    0       0       0.67183 0;       % Joint 1 — 腰部旋转
+    0.4318  0       0       0;       % Joint 2 — 大臂俯仰
+    0.0203 -pi/2    0.15005 0;       % Joint 3 — 小臂俯仰
+    0       pi/2    0.4318  0;       % Joint 4 — 腕部旋转
+    0      -pi/2    0       0;       % Joint 5 — 腕部弯曲
+    0       0       0.2     0;       % Joint 6 — 末端旋转
     ];
 
 fprintf('\n【PUMA560 DH 参数】\n');
 fprintf('  J1(腰): a=%.3f  α=%.0f°      d=%.3f\n', dh(1,1), rad2deg(dh(1,2)), dh(1,3));
 fprintf('  J2(肩): a=%.3f  α=%.0f°     d=%.3f\n', dh(2,1), rad2deg(dh(2,2)), dh(2,3));
 fprintf('  J3(肘): a=%.3f  α=%.0f°      d=%.3f\n', dh(3,1), rad2deg(dh(3,2)), dh(3,3));
-fprintf('  J4(腕): a=%.3f  α=%.0f°      d=%.3f\n\n', dh(4,1), rad2deg(dh(4,2)), dh(4,3));
+fprintf('  J4(腕): a=%.3f  α=%.0f°      d=%.3f\n', dh(4,1), rad2deg(dh(4,2)), dh(4,3));
+fprintf('  J5(腕): a=%.3f  α=%.0f°      d=%.3f\n', dh(5,1), rad2deg(dh(5,2)), dh(5,3));
+fprintf('  J6(腕): a=%.3f  α=%.0f°      d=%.3f\n\n', dh(6,1), rad2deg(dh(6,2)), dh(6,3));
 
 %% ---------- 第 3 步：Simulink 模型 — DH 正运动学 ----------
 

t33_ukf_basics.m

@@ -181,7 +181,7 @@
 
 % UKF 参数
 n = 2;                           % 状态维度
-alpha = 1e-3;                    % Sigma 点散布参数 (小→靠近均值)
+alpha = 0.5;                     % Sigma 点散布参数（避免 α 过小导致权重退化）
 beta = 2;                        % 高斯分布最优值
 kappa = 0;                       % 辅助缩放参数
 lambda = alpha^2 * (n + kappa) - n;

t34_ekf_vs_ukf.m

@@ -151,7 +151,7 @@
 
 % UKF 参数
 n_st = 2;
-alpha_ukf = 1e-3; beta_ukf = 2; kappa_ukf = 0;
+alpha_ukf = 0.5; beta_ukf = 2; kappa_ukf = 0;
 lambda_ukf = alpha_ukf^2 * (n_st + kappa_ukf) - n_st;
 Wm = zeros(2*n_st+1, 1); Wc = zeros(2*n_st+1, 1);
 Wm(1) = lambda_ukf/(n_st+lambda_ukf);