Skip to content

Commit 838a277

Browse files
xingd5478-ctrlxingd5478-ctrl
committed
Fix control/filter math errors in tutorials 23-34
- EKF F matrix: use discrete I+dt*A instead of continuous Jacobian A (t31/t32/t33/t34); unobserved state estimate error roughly halved - t23 MRAC: theoretical optimal gain theta* = 1 (DC-gain match), not tau_p/tau_m - t27: State-Space B = [0;1/m]; ode45 uses step force 5*(t>=0.5) - t28: Sine Wave frequency in rad/s; Butterworth 100 Hz lowpass - t29: closed-loop step input B*K(1) for reference tracking - t30: add suspension damper c_sus (passive response previously never decayed)
1 parent 9327606 commit 838a277

9 files changed

Lines changed: 1418 additions & 17 deletions

‎t23_mrac.m‎

Lines changed: 5 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -236,11 +236,11 @@
236236
% 图2：自适应增益变化
237237
subplot(2,2,2); hold on;
238238
plot(t, theta, 'r', 'LineWidth', 1.5);
239-
yline(2/0.5, 'b--', 'LineWidth', 1); % 理论最优值 = τp/τm
240-
text(t_end*0.6, 2/0.5*1.1, sprintf('理论最优 θ*=τp/τm=%.1f', tau_real/tau_m));
239+
yline(1, 'b--', 'LineWidth', 1); % 理论最优值 θ* = 1（直流增益匹配）
240+
text(t_end*0.6, 1*1.1, '理论最优 θ*=1（直流增益匹配）');
241241
title('自适应增益 θ(t)');
242242
xlabel('时间 (s)'); ylabel('增益'); grid on;
243-
fprintf(' 理论最优增益: θ* = %.1f (真实系统/参考模型)\n', tau_real/tau_m);
243+
fprintf(' 理论最优增益: θ* = 1 (直流增益匹配，G_p(0)=G_m(0)=1)\n');
244244
fprintf(' MIT 自适应最终收敛于 θ* 附近 ✓\n\n');
245245

246246
% 图3：参数突变工况
@@ -258,10 +258,9 @@
258258
% 图4：突变后增益重新调整
259259
subplot(2,2,4); hold on;
260260
plot(t, theta2, 'r', 'LineWidth', 1.5);
261-
yline(tau_init/tau_m, 'b--', 'LineWidth', 1);
262-
yline(tau_changed/tau_m, 'g--', 'LineWidth', 1);
261+
yline(1, 'b--', 'LineWidth', 1);
263262
xline(tau_change_time, 'k:', 'LineWidth', 1.5);
264-
legend('θ(t)', '旧最优值', '新最优值', 'Location', 'best');
263+
legend('θ(t)', 'θ*=1', 'Location', 'best');
265264
title('参数突变后增益自适应重新收敛');
266265
xlabel('时间 (s)'); ylabel('增益'); grid on;
267266

‎t27_msd_modeling.m‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -85,7 +85,7 @@
8585
'Position', [280, 200, 360, 270]);
8686
set_param([mdl '/SS Model'], ...
8787
'A', sprintf('[0 1;%.4f %.4f]', -k/m, -c/m), ...
88-
'B', '[0;1]', ...
88+
'B', sprintf('[0;%.4f]', 1/m), ...
8989
'C', 'eye(2)', ... % 输出 [位移; 速度]
9090
'D', '[0;0]', ...
9191
'X0', '[0;0]');
@@ -154,7 +154,7 @@
154154
end
155155

156156
% --- ode45 (MATLAB 标准解法，参考基准) ---
157-
[t_ode, y_ode] = ode45(@(t, y) [y(2); (5 - c*y(2) - k*y(1))/m], t, [0; 0]);
157+
[t_ode, y_ode] = ode45(@(t, y) [y(2); (5*(t>=0.5) - c*y(2) - k*y(1))/m], t, [0; 0]);
158158

159159
%% ---------- 第 5 步：绘图对比 ----------
160160
figure('Name', 't27: 四种建模方法对比', 'Position', [50, 50, 1000, 500]);

‎t28_dsp_basics.m‎

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -86,12 +86,12 @@
8686
% 50Hz 主力信号
8787
add_block('simulink/Sources/Sine Wave', [mdl '/50Hz Signal'], ...
8888
'Position', [50, 60, 110, 90]);
89-
set_param([mdl '/50Hz Signal'], 'Frequency', '50', 'Amplitude', '1');
89+
set_param([mdl '/50Hz Signal'], 'Frequency', num2str(2*pi*50), 'Amplitude', '1');
9090

9191
% 120Hz 干扰信号
9292
add_block('simulink/Sources/Sine Wave', [mdl '/120Hz Noise'], ...
9393
'Position', [50, 150, 110, 180]);
94-
set_param([mdl '/120Hz Noise'], 'Frequency', '120', 'Amplitude', '0.5');
94+
set_param([mdl '/120Hz Noise'], 'Frequency', num2str(2*pi*120), 'Amplitude', '0.5');
9595

9696
% 随机噪声
9797
add_block('simulink/Sources/Band-Limited White Noise', [mdl '/Random Noise'], ...
@@ -106,12 +106,13 @@
106106
add_line(mdl, '120Hz Noise/1', 'Signal Mixer/2');
107107
add_line(mdl, 'Random Noise/1', 'Signal Mixer/3');
108108

109-
% 低通滤波器 — 截止频率约 100Hz，滤掉 120Hz 和部分噪声
109+
% 低通滤波器 — Butterworth 二阶，截止约 100 Hz（通 50 Hz、阻 120 Hz）
110+
wc = 2*pi*100; % 100 Hz → 628.3 rad/s
110111
add_block('simulink/Continuous/Transfer Fcn', [mdl '/Low Pass Filter'], ...
111112
'Position', [300, 140, 390, 190]);
112113
set_param([mdl '/Low Pass Filter'], ...
113-
'Numerator', '[10000]', ...
114-
'Denominator', '[1 200 10000]'); % ωc ≈ 100 rad/s
114+
'Numerator', sprintf('[%.0f]', wc^2), ...
115+
'Denominator', sprintf('[1 %.1f %.0f]', sqrt(2)*wc, wc^2)); % ωc = 2π·100 rad/s
115116

116117
add_line(mdl, 'Signal Mixer/1', 'Low Pass Filter/1');
117118

‎t29_msd_control.m‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -174,7 +174,7 @@
174174

175175
%% ---------- 第 4 步：阶跃响应对比 ----------
176176
[y_open, t_open] = step(tf([1], [m, c, k_s]), 5);
177-
sys_cl = ss(A - B*K_lqr, B, C, D);
177+
sys_cl = ss(A - B*K_lqr, B*K_lqr(1), C, D);
178178
[y_lqr, t_lqr] = step(sys_cl, 5);
179179

180180
figure('Name', 't29: PID vs LQR', 'Position', [50, 50, 800, 400]);

‎t30_active_suspension.m‎

Lines changed: 16 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,7 @@
3636
m_s = 250; % 车身质量 (kg)
3737
m_us = 35; % 车轮质量 (kg)
3838
k_sus = 16000; % 悬挂弹簧刚度 (N/m)
39+
c_sus = 1000; % 悬挂减震器阻尼 (N·s/m)
3940
k_t = 160000; % 轮胎刚度 (N/m) — 比弹簧硬 10 倍！
4041
F_max = 2000; % 执行器最大出力 (N)
4142
Kp = 8000; Ki = 2000; Kd = 3000; % PID 参数
@@ -44,6 +45,7 @@
4445
fprintf(' 车身 m_s = %d kg — 乘客在上面\n', m_s);
4546
fprintf(' 车轮 m_us = %d kg — 贴地而行\n', m_us);
4647
fprintf(' 弹簧 k_s = %d N/m\n', k_sus);
48+
fprintf(' 减震器 c_s = %d N·s/m\n', c_sus);
4749
fprintf(' 轮胎 k_t = %d N/m (弹簧的 10 倍硬)\n', k_t);
4850
fprintf(' 执行器限幅: ±%d N\n\n', F_max);
4951

@@ -69,7 +71,7 @@
6971
add_block('simulink/Math Operations/Gain', [mdl '/Gain_kt'], 'Position', [300,55,355,80]);
7072
set_param([mdl '/Gain_kt'], 'Gain', num2str(k_t));
7173
add_block('simulink/Math Operations/Sum', [mdl '/Sum_F_whl'], 'Position', [440,55,495,80]);
72-
set_param([mdl '/Sum_F_whl'], 'Inputs', '|+--');
74+
set_param([mdl '/Sum_F_whl'], 'Inputs', '|+---');
7375
add_block('simulink/Math Operations/Gain', [mdl '/Gain_1m_us'], 'Position', [580,55,635,80]);
7476
set_param([mdl '/Gain_1m_us'], 'Gain', num2str(1/m_us));
7577
add_block('simulink/Continuous/Integrator', [mdl '/Int_v_us'], 'Position', [720,55,775,80]);
@@ -94,7 +96,7 @@
9496

9597
% ===== 车身：SumForce → 1/m_s → ∫→∫ → x_s =====
9698
add_block('simulink/Math Operations/Sum', [mdl '/Sum_F_bdy'], 'Position', [440,200,495,225]);
97-
set_param([mdl '/Sum_F_bdy'], 'Inputs', '|++');
99+
set_param([mdl '/Sum_F_bdy'], 'Inputs', '|+++');
98100
add_block('simulink/Math Operations/Gain', [mdl '/Gain_1m_s'], 'Position', [580,200,635,225]);
99101
set_param([mdl '/Gain_1m_s'], 'Gain', num2str(1/m_s));
100102
add_block('simulink/Continuous/Integrator', [mdl '/Int_v_s'], 'Position', [720,200,775,225]);
@@ -105,6 +107,17 @@
105107
add_line(mdl, 'Int_v_s/1', 'Int_x_s/1');
106108
add_line(mdl, 'Int_x_s/1', 'Sum_xus_xs/2');
107109

110+
% ===== 减震器：F_d = c_sus*(v_us - v_s)，v 为速度 =====
111+
add_block('simulink/Math Operations/Sum', [mdl '/Sum_vus_vs'], 'Position', [440,260,495,285]);
112+
set_param([mdl '/Sum_vus_vs'], 'Inputs', '|+-');
113+
add_block('simulink/Math Operations/Gain', [mdl '/Gain_cs'], 'Position', [580,260,635,285]);
114+
set_param([mdl '/Gain_cs'], 'Gain', num2str(c_sus));
115+
add_line(mdl, 'Int_v_us/1', 'Sum_vus_vs/1');
116+
add_line(mdl, 'Int_v_s/1', 'Sum_vus_vs/2');
117+
add_line(mdl, 'Sum_vus_vs/1', 'Gain_cs/1');
118+
add_line(mdl, 'Gain_cs/1', 'Sum_F_whl/4');
119+
add_line(mdl, 'Gain_cs/1', 'Sum_F_bdy/3');
120+
108121
% ===== PID 控制器 =====
109122
add_block('simulink/Sources/Constant', [mdl '/Ref_0'], 'Position', [30,280,80,310]);
110123
set_param([mdl '/Ref_0'], 'Value', '0');
@@ -160,7 +173,7 @@
160173
fprintf(' 模型: 路面→车轮→弹簧→车身→PID→执行器, 完整闭环\n\n');
161174

162175
%% ---------- 状态空间模型 & 对比仿真 ----------
163-
A_sus = [0,1,0,0; -(k_t+k_sus)/m_us,0,k_sus/m_us,0; 0,0,0,1; k_sus/m_s,0,-k_sus/m_s,0];
176+
A_sus = [0,1,0,0; -(k_t+k_sus)/m_us,-c_sus/m_us,k_sus/m_us,c_sus/m_us; 0,0,0,1; k_sus/m_s,c_sus/m_s,-k_sus/m_s,-c_sus/m_s];
164177
B_sus = [0;-1/m_us;0;1/m_s]; B_dist = [0;k_t/m_us;0;0]; C_sus = [0,0,1,0];
165178

166179
sys_passive = ss(A_sus, B_dist, C_sus, 0);

0 commit comments

Comments
 (0)