├── .DS_Store ├── .gitattributes ├── LICENSE ├── Matlab ├── .DS_Store ├── LQR - 线性二次型调节器 │ ├── Inverted_Pendulum_Dynamics.mlx │ └── LQR_Inverted_Pendulum.mlx ├── MPC - 模型预测控制 │ ├── F4_MPC_Matrices_PM.m │ ├── F5_MPC_Controller_noConstraints.m │ └── MPC_1D.mlx ├── QP - 二次规划问题 │ ├── QP_EQconstraint.m │ ├── QP_free.m │ └── QPnonEQconstraint.m ├── 控制之美-卡尔曼滤波 │ ├── F8_LinearKalmanFilter.m │ ├── KalmanFilter_UAV_ConstantInput.m │ └── NoiseData.csv └── 连续系统离散化 │ └── System_discretization.mlx ├── Octave ├── DP_Numerical_Test.m ├── Extended_KalmanFilter.m ├── F1_LQR_Gain.m ├── F2_InputAugmentMatrix_SS_U.m ├── F3_InputAugmentMatrix_Delta_U.m ├── F4_MPC_Matrices_PM.m ├── F5_MPC_Controller_noConstraints.m ├── F6_MPC_Matrices_Constraints.m ├── F7_MPC_Controller_withConstriants.m ├── F8_LinearKalmanFilter.m ├── KalmanFilter_MPC_UAV.m ├── KalmanFilter_UAV_ConstantInput.m ├── LQR_InvertedPed_Pendulum.m ├── LQR_Test_1D.m ├── LQR_Test_tracking_Delta_U_AD_MSD.m ├── LQR_Test_tracking_Delta_U_MSD.m ├── LQR_Test_tracking_E_offset_MSD.m ├── LQR_Test_tracking_SS_U_MSD.m ├── LQR_UAV_tracking_SS_U.m ├── LQR_UAV_tracking_SS_U_InputConstraints.m ├── Linear_Regression.m ├── Linear_regression_gradient_descent.m ├── MPC_1D.m ├── MPC_2D.m ├── MPC_MSD_Delta_AD.m ├── MPC_MSD_Delta_U.m ├── MPC_MSD_SS_U.m ├── MPC_MSD_SS_U_withConstraints.m ├── MPC_UAV _ST_Analysis.m ├── MPC_UAV.m ├── NoiseData.csv ├── Octave程序代码说明.mp4 ├── QP_EQconstraint.m ├── QP_Free.m ├── QP_nonEQconstraint.m └── System_discretization.m ├── README.md └── 控制之美（卷2）勘误表_1017.pdf /.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Teddy-Liao/Beauty-of-Control-using-Matlab/0648680a97cb68c23d47956df382d081848805ee/.DS_Store -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 Liao Dengting 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Matlab/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Teddy-Liao/Beauty-of-Control-using-Matlab/0648680a97cb68c23d47956df382d081848805ee/Matlab/.DS_Store -------------------------------------------------------------------------------- /Matlab/LQR - 线性二次型调节器/Inverted_Pendulum_Dynamics.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Teddy-Liao/Beauty-of-Control-using-Matlab/0648680a97cb68c23d47956df382d081848805ee/Matlab/LQR - 线性二次型调节器/Inverted_Pendulum_Dynamics.mlx -------------------------------------------------------------------------------- /Matlab/LQR - 线性二次型调节器/LQR_Inverted_Pendulum.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Teddy-Liao/Beauty-of-Control-using-Matlab/0648680a97cb68c23d47956df382d081848805ee/Matlab/LQR - 线性二次型调节器/LQR_Inverted_Pendulum.mlx -------------------------------------------------------------------------------- /Matlab/MPC - 模型预测控制/F4_MPC_Matrices_PM.m: -------------------------------------------------------------------------------- 1 | 2 | % 《控制之美-卷二》 P100--103 3 | % 求解模型预测控制中二次规划所需矩阵F，H 4 | % 以及模型预测控制一系列中间过程矩阵Phi，Gamma，Omega，Psi 5 | 6 | % 输入：系统矩阵 A，B； 权重矩阵 Q，R，S； 预测区间：N_P 7 | % 输出：二次规划矩阵F，H 中间过程矩阵Phi，Gamma，Omega，Psi 8 | function [Phi,Gamma,Omega,Psi,F,H]= F4_MPC_Matrices_PM(A,B,Q,R,S,N_P) 9 | 10 | % 计算系统矩阵维度，n 11 | n = size(A,1); 12 | % 计算输入矩阵维度，p 13 | p = size(B,2); 14 | 15 | % 初始化Phi矩阵并定义维度 16 | Phi = zeros(N_P*n,n); 17 | % 初始化Gamma矩阵并定义维度 18 | Gamma = zeros(N_P*n,N_P*p); 19 | % 定义临时对角单位矩阵 20 | tmp = eye(n); % Create a n x n "I" matrix 21 | % 定义for循环行向量 22 | rows = 1:n; 23 | % for循环，用于构建Phi和Gamma矩阵 24 | for i = 1:N_P 25 | % 构建Phi矩阵，参考式（5.3.5b） 26 | Phi((i-1)*n+1:i*n,:) = A^i; 27 | % 构建Gamma矩阵，参考式（5.3.5b） 28 | Gamma(rows,:) = [tmp*B,Gamma(max(1,rows-n), 1:end-p)]; 29 | % Gamma矩阵行数更新，由于Gamma矩阵是由一系列“小”矩阵组成的，因此下一个循环要更新n行 30 | rows =i*n+(1:n); 31 | % 构建临时矩阵用于矩阵A的幂计算 32 | tmp= A*tmp; 33 | end 34 | 35 | % 构建Omega矩阵，包含Q矩阵的部分 36 | Omega = kron(eye(N_P-1),Q); 37 | % 构建最终Omega矩阵，包含S矩阵 38 | Omega = blkdiag(Omega,S); 39 | % 构建Psi矩阵，其为R矩阵组成的对角阵 40 | Psi = kron(eye(N_P),R); 41 | 42 | % 计算二次规划矩阵F 43 | F = Gamma'*Omega*Phi; 44 | % 计算二次规划矩阵H 45 | H = Psi+Gamma'*Omega*Gamma; 46 | 47 | end -------------------------------------------------------------------------------- /Matlab/MPC - 模型预测控制/F5_MPC_Controller_noConstraints.m: -------------------------------------------------------------------------------- 1 | 2 | 3 | % 输入：二次规划矩阵 F，H； 系统控制量维度 p； 系统状态：x 4 | % 输出：系统控制（输入） U，u 5 | % 其中：U为完整的控制序列，u为控制序列的第一项 6 | function [U,u]= F5_MPC_Controller_noConstraints(x,F,H,p) 7 | % 选取最优化求解模式 8 | options = optimset('MaxIter', 200); 9 | % 利用二次规划求解系统控制（输入） 10 | U = quadprog(H,F*x,[],[],[],[],[],[],[],options); 11 | % [U, FVAL, EXITFLAG, OUTPUT, LAMBDA] = quadprog(H,F*x,[],[],[],[],[],[],[],options); 12 | % 根据模型预测控制的策略 13 | % 仅选取所得控制输入序列的第一项 14 | u = U(1:p,1); 15 | end -------------------------------------------------------------------------------- /Matlab/MPC - 模型预测控制/MPC_1D.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Teddy-Liao/Beauty-of-Control-using-Matlab/0648680a97cb68c23d47956df382d081848805ee/Matlab/MPC - 模型预测控制/MPC_1D.mlx -------------------------------------------------------------------------------- /Matlab/QP - 二次规划问题/QP_EQconstraint.m: -------------------------------------------------------------------------------- 1 | % 《控制之美-卷二》 P97-98 2 | % 求解令 J = 0.5*(U1.^2 + U2.^2)+U1+U2 最小时的u1、u2 3 | % 需同时满足等式约束： 4 | % u1 - u2 = 1; 5 | 6 | clear all; 7 | close all; 8 | clc; 9 | 10 | % 定义二次规划问题的H,f,Meq 11 | H = [1 0; 0 1]; 12 | f = [1; 1]; 13 | n = size(H,1); 14 | % 定义等式约束的Meq和beq 15 | Meq = [1 -1]; 16 | beq =1; 17 | m = size(Meq,1); 18 | % 定义二次规划问题的u和lamda 19 | u = zeros(n,1); 20 | lamda = zeros(m,1); 21 | % 求解二次规划问题 22 | u_lamda = inv([H,Meq';Meq,zeros(m,m)])*[-f;beq]; 23 | u = u_lamda(1:n,:); 24 | 25 | 26 | %% 绘图 27 | % 绘制二次规划问题的可行域和最优解点（3D图） 28 | [U1,U2] = meshgrid(-2:0.1:0); 29 | J = 0.5*(U1.^2 + U2.^2)+U1+U2; 30 | subplot(1,2,1); 31 | surf(U1,U2,J,'FaceAlpha', 0.1); 32 | hold on; 33 | u1_proj = -2:0.1:0; 34 | u2_proj = u1_proj - 1; 35 | J_proj = 0.5 * (u1_proj .^ 2 + u2_proj .^ 2) + u1_proj + u2_proj; 36 | plot3(u1_proj, u2_proj, J_proj, 'b', 'LineWidth', 5); 37 | plot3(u(1), u(2), 0.5*(u(1)^2 + u(2)^2)+u(1)+u(2), 'r^', 'MarkerSize', 20,'MarkerFaceColor', 'red'); 38 | [J_proj, U1_proj] = meshgrid(-1:0.1:0, u1_proj); 39 | U2_proj = U1_proj - 1; 40 | surf(U1_proj, U2_proj, J_proj, 'FaceColor','blue','FaceAlpha', 0.2,'EdgeColor', 'none'); 41 | xlabel('u1'); 42 | ylabel('u2'); 43 | zlabel('J(u1,u2)'); 44 | xlim([-2 0]); 45 | ylim([-2 0]); 46 | zlim([-1.05 0]); 47 | set(gca,'FontSize',20); 48 | % 绘制等高线图 49 | subplot(1,2,2); 50 | contour(U1,U2,J,30); 51 | hold on; 52 | plot(u(1), u(2), 'r*', 'MarkerSize', 10); 53 | u1_con = -2:0.1:2; 54 | u2_con = u1_con - 1; 55 | plot(u1_con, u2_con, 'k', 'LineWidth', 2); 56 | xlabel('u1'); 57 | ylabel('u2'); 58 | set(gca,'FontSize',20); 59 | sgtitle('二次规划问题'); -------------------------------------------------------------------------------- /Matlab/QP - 二次规划问题/QP_free.m: -------------------------------------------------------------------------------- 1 | % 《控制之美-卷二》 P95-96 2 | % 求解令 J = 0.5*(U1.^2 + U2.^2)+U1+U2 最小时的u1、u2 3 | clear; 4 | close all; 5 | clc; 6 | 7 | %% 转化为标准二次规划问题 8 | % J = (1/2)(u^T)Hu + (uT)f 9 | % 定义二次规划问题的H和f 10 | H = [1 0; 0 1]; 11 | f = [1; 1]; 12 | 13 | % 求解二次规划问题 14 | % 可以轻易求出无约束情况下的解析解： 15 | u = -inv(H)*f; 16 | 17 | 18 | 19 | %% 绘图 20 | % 绘制二次规划问题的可行域和最优解点（3D图） 21 | [U1,U2] = meshgrid(-2:0.1:0); 22 | J = 0.5*(U1.^2 + U2.^2)+U1+U2; 23 | subplot(1,2,1); 24 | surf(U1,U2,J,'FaceAlpha', 0.1); 25 | hold on; 26 | plot3(u(1), u(2), 0.5*(u(1)^2 + u(2)^2)+u(1)+u(2), 'r^', 'MarkerSize', 20,'MarkerFaceColor', 'red'); 27 | xlabel('u1'); 28 | ylabel('u2'); 29 | zlabel('J(u1,u2)'); 30 | xlim([-2 0]); 31 | ylim([-2 0]); 32 | zlim([-1.05 0]); 33 | set(gca,'FontSize',20); 34 | % 绘制等高线图 35 | subplot(1,2,2); 36 | contour(U1,U2,J,30); 37 | hold on; 38 | plot(u(1), u(2), 'r*', 'MarkerSize', 10); 39 | xlabel('u1'); 40 | ylabel('u2'); 41 | set(gca,'FontSize',20); 42 | sgtitle('二次规划问题'); 43 | -------------------------------------------------------------------------------- /Matlab/QP - 二次规划问题/QPnonEQconstraint.m: -------------------------------------------------------------------------------- 1 | % 《控制之美-卷二》 P97-98 2 | % 求解令 J = 0.5*(U1.^2 + U2.^2)+U1+U2 最小时的u1、u2 3 | % 同时满足不等式约束 4 | % u1 + u2 <= 2 5 | % -u1 + u2 <= 1 6 | % 0 =< u1 <= 1 7 | % 0 =< u2 <= 2 8 | 9 | clear all; 10 | close all; 11 | clc; 12 | 13 | % 定义二次规划问题的H和f 14 | H = [1 0; 0 1]; 15 | f = [1; 1]; 16 | 17 | % 定义不等式约束的A和b 18 | A = [-1 1; 1 1]; 19 | b = [1; 2]; 20 | % 定义变量的边界条件 21 | lb = [0; 0]; 22 | ub = [1; 2]; 23 | 24 | % 使用 quadprog求解器求解含不等式约束的二次规划问题 25 | % 使用格式 [x,fval] = quadprog(H,f,A,b,Aeq,beq,lb,ub,x0,options) 26 | % fval 是标准的代价函数 27 | % fval = 0.5*x'*H*x + f'*x 28 | [u, J] = quadprog(H, f, A, b, [], [], lb, ub, []); 29 | 30 | %% 绘图 31 | % 绘制等高线图 32 | subplot(1,1,1); 33 | [U1, U2] = meshgrid(-1:0.1:2); 34 | J = 0.5*(U1.^2 + U2.^2) + U1 + U2; 35 | contour(U1, U2, J, 60); 36 | hold on; 37 | % 绘制可行域 38 | plot([-0.5, 1], [0.5, 2], 'k', 'LineWidth', 1.5); 39 | hold on; 40 | plot([0, 1.5], [2, 0.5], 'k', 'LineWidth', 1.5); 41 | plot([0, 0], [-1, 2], 'k', 'LineWidth', 1.5); 42 | plot([-0.5, 1.5], [0, 0], 'k', 'LineWidth', 1.5); 43 | plot([1, 1], [-1, 2], 'k', 'LineWidth', 1.5); 44 | fill([0, 0.5, 1], [1, 1.5, 1], 'green', 'FaceAlpha', 0.2, 'EdgeColor', 'none'); 45 | fill([0, 0, 1, 1], [0, 1, 1, 0], 'green', 'FaceAlpha', 0.2, 'EdgeColor', 'none'); 46 | % 绘制最优解点 47 | plot(u(1), u(2), 'r^', 'MarkerSize', 20,'MarkerFaceColor', 'red'); 48 | 49 | % 添加坐标轴标签和图标题 50 | xlabel('u1'); 51 | ylabel('u2'); 52 | xlim([-0.5 1.5]); 53 | ylim([-0.5 2]); 54 | set(gca, 'FontSize', 20); -------------------------------------------------------------------------------- /Matlab/控制之美-卡尔曼滤波/F8_LinearKalmanFilter.m: -------------------------------------------------------------------------------- 1 | % 搭建离散卡尔曼滤波器 2 | % 理论模型如下 - 五大经典卡尔曼滤波方程 3 | % /************** 先验状态估计 ****************/ 4 | % xk(-) = A * x(k-1)(+) + B * u(k-1) 5 | % /************** 先验状态估计误差协方差矩阵 ****************/ 6 | % P(-) = A * P(+) * A^T + Qc 7 | % /************** 卡尔曼增益系数 ****************/ 8 | % K = P(-) * H^T * (Rc + H * P(-) * H^T)^-1 9 | % /************** 后验估计 ****************/ 10 | % x(+) = x(-) + K * (z - H * x(-)) 11 | % /************** 后验状态估计误差协方差矩阵 ****************/ 12 | % P(+) = (I - K * H) * P(-) 13 | 14 | 15 | % A B —— 状态空间方程中的矩阵 16 | % Q_c —— 过程噪声w的协方差矩阵 17 | % R_c —— 测量噪声v的协方差矩阵 18 | % H —— 测量矩阵 19 | % z —— 测量值 20 | % x_hat —— x的后验估计 21 | % x_hat_minus —— x的先验估计 22 | % P —— 先验状态估计误差协方差矩阵 23 | % u —— 输入 24 | function [x_hat, x_hat_minus, P] = F8_LinearKalmanFilter(A,B,Q_c,R_c,H,z,x_hat,P,u) 25 | 26 | % 先验状态估计 27 | x_hat_minus = A * x_hat + B * u; 28 | % 先验状态估计误差协方差矩阵 29 | P_minus = A * P * transpose(A) + Q_c; 30 | % 卡尔曼增益系数 31 | K = P_minus * transpose(H) * pinv (H * P_minus * transpose(H) + R_c); 32 | % 这里用到了伪逆的概念 33 | % 分母矩阵的行列式为0， 这意味着这个矩阵是奇异的，因此不能被逆。这是导致您在MATLAB中遇到Nan问题的原因。 34 | 35 | % 后验估计 36 | x_hat = x_hat_minus + K * (z - H *x_hat_minus); 37 | % 后验状态估计误差协方差矩阵 38 | P = (eye(size(A,1)) - K * H) * P_minus; 39 | 40 | end 41 | -------------------------------------------------------------------------------- /Matlab/控制之美-卡尔曼滤波/KalmanFilter_UAV_ConstantInput.m: -------------------------------------------------------------------------------- 1 | clear all 2 | close all 3 | clc 4 | 5 | %% 定义连续系统：无人机高度控制 6 | % 《控制之美 卷2》P41 7 | % 构建系统矩阵A 8 | A = [0 1 0; 9 | 0 0 1; 10 | 0 0 0]; 11 | % 计算A矩阵维度：取A的行数 12 | n = size (A,1); 13 | % 构建输入矩阵B 14 | B = [0; 15 | 1; 16 | 0]; 17 | % y = C*x + D*u 18 | C = [1 0 0; 19 | 0 1 0]; 20 | D = 0; 21 | % 计算输入矩阵维度：取B的列数 22 | p = size(B,2); 23 | % 定义测量矩阵H_m 24 | H_m = [1 0 0; 25 | 0 1 0; 26 | 0 0 1]; 27 | % 重力加速度常数 28 | g = 10; 29 | 30 | %% 连续系统离散化 31 | % 离散时间步长 32 | Ts = 0.1; 33 | % 连续系统转离散系统 - continuous to discrete 34 | sys_d = c2d(ss(A,B,C,D),Ts); 35 | % 提取离散系统A矩阵 36 | A = sys_d.a; 37 | % 提取离散系统B矩阵 38 | B = sys_d.b; 39 | 40 | %% 标定卡尔曼滤波器参数 41 | % 定义过程噪声协方差矩阵 42 | Q_c = [0.01 0 0; 43 | 0 0.01 0; 44 | 0 0 0]; 45 | % 定义测量噪声协方差矩阵 46 | R_c = [1 0 0; 47 | 0 1 0; 48 | 0 0 0]; 49 | 50 | %% 给定系统初始状态 51 | % [初始高度 初始速度 g] 52 | x0 = [0; 1 ; -10]; 53 | % 初始化状态赋值 54 | x = x0; 55 | % 系统输入初始化 56 | u0 = g; 57 | % 初始输入赋值 58 | u = u0; 59 | % 初始化后验估计 60 | x_hat0 = [0; 1; -10]; 61 | % 初始化后验估计赋值 62 | x_hat = x_hat0; 63 | % 初始化后验估计误差协方差矩阵 64 | P0 = [1 0 0; 65 | 0 1 0; 66 | 0 0 0]; 67 | % 初始后验估计误差协方差矩阵赋值 68 | P = P0; 69 | 70 | %% 初始化一些结果矩阵用于存储结果 71 | % 定义系统运行步数 72 | k_steps = 100; 73 | % 定义x_history零矩阵，用于储存系统状态结果，维度n x k_steps 74 | x_history = zeros(n,k_steps); 75 | % 定义u_history零矩阵，用于储存系统输入结果，维度p x k_steps 76 | u_history = zeros(p,k_steps); 77 | % 定义x_hat_history零矩阵，用于储存后验估计结果，维度n x k_steps 78 | x_hat_history = zeros(n,k_steps); 79 | % 定义x_hat_minus_history零矩阵，用于储存先验估计结果，维度n x k_steps 80 | x_hat_minus_history = zeros(n,k_steps); 81 | % 定义z_historyy零矩阵，用于测量结果，维度n x k_steps 82 | z_history = zeros(n,k_steps); 83 | 84 | %% 定义噪声（直接读取噪声文件） 85 | % 定义过程噪声矩阵w，维度n x k_steps 86 | w = zeros (n,k_steps); 87 | % 定义测量噪声矩阵V，维度n x k_steps 88 | v = zeros (n,k_steps); 89 | % 从文件NoiseData.csv中读取数据 90 | % 数据来自于系统随机生成，保存为文件可以方便进行多组实验之间的对比 91 | noise_data = readmatrix('NoiseData.csv'); 92 | w = noise_data(2:4, :); 93 | v = noise_data(6:8, :); 94 | % %%%%%%%%%%%%%生成过程与测量噪声%%%%%%%%%%%%%%%%%% 95 | % % 使用以下代码生成随机噪声，之后保存在NoiseData.csv中，方便下次读取。%%%%%% 96 | 97 | %% 定义真实噪声 98 | % % 定义真实的过程噪声协方差矩阵 99 | % Q_ca = [0.05 0; 0 0.05]; 100 | % % 定义真实的测量噪声协方差矩阵 101 | % R_ca = [1 0; 0 1]; 102 | % % 随机生成过程噪声 103 | % w(1:2,:) = chol(Q_ca)* randn(2,k_steps); 104 | % % 随机生成测量噪声 105 | % v(1:2,:) = chol(R_ca)* randn(2,k_steps); 106 | 107 | %% 仿真开始，建立for循环 108 | for k = 1:k_steps 109 | % 系统状态空间方程，计算实际状态变量 110 | x = A * x + B * u + w(:,k); 111 | % 计算实际测量值，添加了噪声（在实际应用中，这一项来自传感器测量） 112 | z = H_m * x + v(:,k); 113 | % 使用卡尔曼滤波器 114 | [x_hat,x_hat_minus, P] = F8_LinearKalmanFilter(A,B,Q_c,R_c,H_m,z,x_hat,P,u); 115 | % 保存系统状态到预先定义矩阵的相应位置 116 | x_history (:,k+1) = x; 117 | % 保存测量值到预先定义矩阵的相应位置 118 | z_history (:,k+1) = z; 119 | % 保存先验估计到预先定义矩阵的相应位置 120 | x_hat_minus_history (:,k+1) = x_hat_minus; 121 | % 保存后验估计到预先定义矩阵的相应位置 122 | x_hat_history (:,k+1) = x_hat; 123 | end 124 | 125 | %% 绘图 126 | % figure(1) 127 | % x1真实结果 128 | plot (0:length(x_history)-1,x_history(1,:),'--','LineWidth',2); 129 | hold on 130 | % x1测量值 131 | plot (0:length(z_history)-1,z_history(1,:),'*','MarkerSize',8) 132 | hold on 133 | % x1先验估计值 134 | plot (0:length(x_hat_minus_history)-1,x_hat_minus_history(1,:),'o','MarkerSize',8); 135 | hold on 136 | % x1后验估计值 137 | plot ( 0:length(x_hat_history)-1,x_hat_history(1,:),'LineWidth',2); 138 | legend(' 真实值 ',' 测量值 ',' 先验估计值 ',' 后验估计值 ') 139 | set(legend, 'Location', 'southeast','FontSize', 20); 140 | ylim([-2 12]); 141 | hold off; 142 | grid on 143 | 144 | 145 | 146 | -------------------------------------------------------------------------------- /Matlab/控制之美-卡尔曼滤波/NoiseData.csv: -------------------------------------------------------------------------------- 1 | W,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, 2 | 0.16777,-0.23863,-0.30754,-0.16607,-0.098293,-0.10772,0.21584,0.25693,0.0034506,0.2365,-0.19913,0.17759,-0.37375,0.09489,-0.045805,-0.19073,0.061853,-0.21437,0.02735,0.030791,0.23099,-0.058602,-0.16159,-0.14029,-0.04496,0.19538,0.24663,0.2676,0.28402,0.42296,0.050613,0.036664,0.12478,0.3166,-0.03748,0.49567,0.047892,0.20908,-0.43451,-0.094871,0.17524,0.2393,-0.19013,-0.19752,0.085521,-0.24881,-0.26308,0.28483,-0.095117,2.20E-01,-0.1192,-0.00923,-0.12522,-0.042058,0.0085322,-0.025384,-0.24313,-0.025312,-0.2733,-0.036242,-0.22746,0.25653,0.11745,0.08376,-0.022888,0.020046,-0.18759,0.17802,0.16367,0.027173,0.17639,-0.087117,-0.070067,0.19823,0.17591,-0.21935,0.35102,-0.17019,-0.040652,0.088605,0.43391,-0.34293,-0.14951,0.32249,0.49867,0.030386,-0.45321,-0.13402,-0.15158,-0.049666,0.0104,-0.33852,-0.37582,0.16483,0.31131,0.10924,0.089983,0.20679,-0.44931,0.21389 3 | -0.11137,0.18288,0.282,0.45686,0.012511,0.082863,0.045152,-0.4198,0.1489,-0.10782,-0.16333,0.19509,-0.080096,0.49343,-0.33671,-0.11597,-0.065428,-0.18417,-0.66045,0.16749,-0.015864,-0.2158,0.091402,-0.24196,0.22902,-0.095408,0.021871,-0.52084,0.39901,0.077507,-0.14835,0.041585,0.15046,-0.031936,0.2809,-0.21423,-0.070098,0.11142,-0.17589,0.010092,-0.11278,-0.057325,-0.078722,-0.0008646,0.2805,0.070395,0.01105,0.1452,0.058264,-0.010069,-0.16314,0.058182,-0.24563,0.17948,0.11011,0.15955,0.038317,0.15714,0.26802,0.21713,0.20863,0.19772,0.26882,-0.063072,-0.036993,-0.65869,0.025766,0.036889,-0.5379,0.14888,0.16621,-0.13005,0.24508,-0.2282,-0.1788,0.21827,-0.36234,-0.075307,-0.29145,0.092035,-0.026155,0.32007,0.48516,-0.14264,0.015359,-0.075981,-0.11289,-0.36782,0.0091562,0.28847,-0.25652,0.14886,-0.41084,0.10827,-0.039769,-0.24822,-0.36823,-0.061889,-0.48947,-0.25573 4 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 5 | V,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, 6 | -1.4074,-0.59492,0.2163,-0.25072,0.25474,0.16127,0.10038,0.57475,-1.2121,-0.72364,-1.0775,1.5151,2.348,0.13198,0.95729,0.056651,0.35449,-0.419,0.63198,0.98887,-1.1665,0.49643,1.1759,-0.041543,-0.15773,0.12893,-0.56709,2.0682,-1.8037,0.84331,0.4549,-0.45044,0.089828,-1.2474,0.49553,-0.38666,1.179,0.080986,0.83898,1.2541,0.28983,-2.2077,1.1374,-0.090443,-1.4051,0.96262,0.67947,0.64459,-1.5585,0.56929,1.0605,1.7294,-0.54772,0.15612,-0.99507,-0.47439,-0.085259,-2.8441,-0.097192,0.44774,0.80802,-1.22,-0.06701,0.18064,0.6438,1.2213,0.9951,-1.3629,-0.17415,0.070147,0.072511,0.024813,0.97374,-0.83002,0.79455,-0.87716,-1.5169,1.3863,0.57285,-1.2997,-1.2896,-0.43287,0.89785,0.078842,0.82559,0.29353,0.36765,-2.0845,-1.2306,1.0493,-0.71329,1.4933,-1.5148,0.41076,-0.47319,0.41696,-1.6239,-0.98357,0.37025,1.5854 7 | -0.80584,-0.48031,-0.18977,-0.35396,-0.29403,-1.2032,-0.96578,0.32351,0.55081,1.4534,-0.24546,-0.34567,0.71498,-0.14205,-1.1803,-0.40903,0.060082,0.4101,0.61289,-0.25632,-2.2262,0.69363,-1.2295,-0.54831,-0.86918,-0.614,-0.99089,-1.0948,1.5206,2.4036,-0.76575,-0.26564,-0.43201,-1.1398,-0.63463,0.92373,-0.18749,1.873,-1.3382,-0.72414,0.63597,-2.0758,0.15186,1.0797,-0.17095,-1.0046,1.2438,0.66241,-0.64482,-1.0755,0.11092,0.59234,0.19683,0.063897,0.37786,1.5433,-0.47429,0.93115,-0.56141,0.20077,-0.71567,-0.24863,-2.4206,0.98249,0.57278,-0.038913,0.42607,0.014999,0.98717,-0.61145,1.057,-1.1149,-1.9866,0.24645,-1.4231,-0.40029,0.57846,-1.0546,1.8024,-1.1117,-0.78439,-0.94797,-1.3545,-1.1281,0.95894,-0.23108,-1.7893,-0.25282,-0.95407,-1.2209,0.10542,1.5044,-0.62599,1.342,1.119,1.0547,0.58491,1.6814,-0.042993,0.337 8 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 9 | -------------------------------------------------------------------------------- /Matlab/连续系统离散化/System_discretization.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Teddy-Liao/Beauty-of-Control-using-Matlab/0648680a97cb68c23d47956df382d081848805ee/Matlab/连续系统离散化/System_discretization.mlx -------------------------------------------------------------------------------- /Octave/DP_Numerical_Test.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：LQR_UAV_tracking_SS_U 6 | %% 程序功能：无人机上升目标高度最短用时控制-动态规划数值方法 （4.2节案例） 7 | %% 理论基础：贝尔曼最优化理论 8 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 9 | 10 | %% 程序初始化，清空工作空间，缓存 11 | clear all; 12 | close all; 13 | clc; 14 | 15 | %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% 16 | % 无人机高度初始化 17 | h_init = 0 ; 18 | % 无人机速度初始化 19 | v_init = 0 ; 20 | 21 | %%%%%%%%%%%%%%%%%系统终值定义%%%%%%%%%%%%%%%%%% 22 | % 无人机终点高度 23 | h_final = 10; 24 | % 无人机终点速度 25 | v_final = 0; 26 | 27 | %%%%%%%%%%%%%%%%%边界条件定义%%%%%%%%%%%%%%%%%% 28 | % 高度下限 29 | h_min = 0; 30 | % 高度上限 31 | h_max = 10; 32 | % 高度离散数 33 | N_h = 100; 34 | % 速度下限 35 | v_min = 0; 36 | % 速度上限 37 | v_max = 3; 38 | % 速度离散数 39 | N_v = 500; 40 | 41 | %%%%%%%%%%%%%%%%%创建离散向量%%%%%%%%%%%%%%%%%% 42 | % 高度向量 43 | Hd = h_min : (h_max - h_min)/N_h: h_max; 44 | % 速度向量 45 | Vd = v_min: (v_max - v_min)/N_v : v_max; 46 | 47 | % 无人机加速度上下限设置 48 | u_min = -3; u_max = 2; 49 | % 定义初始剩余代价矩阵 50 | J_costtogo = zeros(N_h + 1, N_v + 1); 51 | % 定义系统输入矩阵 52 | Input_acc = zeros(N_h + 1, N_v + 1); 53 | 54 | %%%%%%%%%%%%%%%计算最后一级的情况%%%%%%%%%%%%%%% 55 | % 计算最后一级至上一级间平均速度矩阵 56 | v_avg = 0.5 * (v_final + Vd); 57 | % 计算最后一级至上一级间用时矩阵 58 | T_delta = (h_max - h_min)./(N_h * v_avg); 59 | % 计算最后一级至上一级间加速度矩阵 60 | acc = (v_final - Vd)./T_delta; 61 | % 将用时存入代价矩阵 62 | J_temp = T_delta; 63 | % 筛选超限的系统输入（加速度需满足上下限） 64 | [acc_x,acc_y] = find(acc < u_min | acc > u_max); 65 | % 通过线性检索命令找到加速度超限的位置 66 | Ind_lin_acc = sub2ind (size(acc),acc_x,acc_y); 67 | % 将加速度超限位置的系统代价人为赋值为无穷大 68 | J_temp (Ind_lin_acc) = inf; 69 | % 将更新的代价存入相应的剩余代价矩阵的对应元素的位置 70 | J_costtogo(2,:) = J_temp; 71 | % 将对应的加速度存入系统输入矩阵的相应位置 72 | Input_acc (2,:) = acc; 73 | 74 | %%%%%%%%%%%%%%%倒数第二级至第二级的情况%%%%%%%%%%%%%%% 75 | % 设计for循环进行逆向级间的更新 （参考图4.2.7） 76 | for k = 3 : 1 : N_h 77 | % 构建速度方阵，代表相邻两级间速度的所有组合 78 | [Vd_x, Vd_y] = meshgrid(Vd , Vd); 79 | % 计算级间平均速度矩阵 80 | v_avg = 0.5 * (Vd_x + Vd_y); 81 | % 计算级间用时矩阵，这也是级间代价矩阵 82 | T_delta = (h_max - h_min)./(N_h * v_avg); 83 | % 计算级间加速度矩阵 84 | acc = (Vd_y - Vd_x)./T_delta; 85 | % 将级间用时赋值代价矩阵 86 | J_temp = T_delta; 87 | % 筛选超限的系统输入（加速度需满足上下限） 88 | [acc_x, acc_y] = find(acc < u_min | acc > u_max); 89 | % 通过线性检索命令找到加速度超限的位置 90 | Ind_lin_acc = sub2ind (size(acc),acc_x,acc_y); 91 | % 将加速度超限位置的系统代价人为赋值为无穷大 92 | J_temp (Ind_lin_acc) = inf; 93 | %%%%%%%%%%%%%%%%%% 重要的步骤! %%%%%%%%%%%%%%%%%%% 94 | % 生成代价矩阵，注意需要将临时代价与上一步的剩余代价结合 95 | J_temp = J_temp + meshgrid(J_costtogo(k-1,:))'; 96 | % 提取剩余代价矩阵的最小值 97 | [J_costtogo(k,:), l] = min(J_temp) ; 98 | % 线性索引找到最小值的位置 99 | Ind_lin_acc = sub2ind (size(J_temp), l, 1:length(l)); 100 | % 保存相应的系统输入 101 | Input_acc (k,:) = acc(Ind_lin_acc) ; 102 | end 103 | 104 | %%%%%%%%%%%%%%%%第二级至第一级的情况%%%%%%%%%%%%%%%%% 105 | % 计算级间平均速度矩阵 106 | v_avg = 0.5 * (Vd + v_init); 107 | % 计算级间用时矩阵 108 | T_delta = (h_max - h_min)./(N_h * v_avg); 109 | % 计算级间加速度矩阵 110 | acc = (Vd - v_init)./T_delta; 111 | % 级间用时存入临时代价矩阵 112 | J_temp = T_delta; 113 | % 筛选超限的系统输入（加速度需满足上下限） 114 | [acc_x, acc_y] = find(acc < u_min | acc > u_max); 115 | % 通过线性检索命令找到加速度超限的位置 116 | Ind_lin_acc = sub2ind (size(acc),acc_x,acc_y); 117 | % 将加速度超限位置的系统代价人为赋值为无穷大 118 | J_temp (Ind_lin_acc) = inf; % Let certain elements to infitiy 119 | %%%%%%%%%%%%%%%%%% 重要的步骤! %%%%%%%%%%%%%%%%%%% 120 | % 生成代价矩阵，注意需要将临时代价与上一步的剩余代价结合 121 | J_temp = J_temp + J_costtogo(N_h,:); 122 | % 提取剩余代价矩阵的最小值 123 | [J_costtogo(N_h+1,1), l] = min(J_temp); 124 | % 线性索引找到最小值的位置 125 | Ind_lin_acc = sub2ind (size(J_temp), l); 126 | % 保存相应的系统输入 127 | Input_acc (N_h+1,1) = acc(Ind_lin_acc); 128 | 129 | %%%%%%%%%%%%%%%%%结果（画图）%%%%%%%%%%%%%%%%%%%%% 130 | % 初始化高度 131 | h_plot_init = 0; 132 | % 初始化速度 133 | v_plot_init = 0; 134 | % 初始化时间 135 | t_plot_init = 0; 136 | 137 | % 定义加速度结果维度 138 | acc_plot = zeros(length(Hd),1); 139 | % 定义速度结果维度 140 | v_plot = zeros(length(Hd),1); 141 | % 定义高度结果维度 142 | h_plot = zeros(length(Hd),1); 143 | % 定义时间维度 144 | t_plot = zeros(length(Hd),1); 145 | 146 | % 定义高度初值 147 | h_plot (1) = h_plot_init; 148 | % 定义速度初值 149 | v_plot (1) = v_plot_init; 150 | % 定义时间初值 151 | t_plot (1) = t_plot_init; 152 | 153 | %% 查表确定最优路线 154 | for k = 1 : 1 : N_h 155 | % 确认高度最优索引 156 | [min_h,h_plot_index] = min(abs(h_plot(k) - Hd)); 157 | % 群人速度最优索引 158 | [min_v,v_plot_index] = min(abs(v_plot(k) - Vd)); 159 | % 查表确定最优系统输入位置 160 | acc_index = sub2ind(size(Input_acc), N_h+2-h_plot_index, v_plot_index); 161 | % 将最优系统输入存入表格 162 | acc_plot (k) = Input_acc(acc_index); 163 | % 计算无人机速度 164 | v_plot (k + 1) = sqrt((2 * (h_max - h_min)/N_h * acc_plot(k))+ v_plot (k)^2); % Calculate speed and height 165 | % 计算无人机高度 166 | h_plot (k + 1) = h_plot(k) + (h_max - h_min)/N_h; 167 | % 计算系统相应时刻 168 | t_plot (k + 1) = t_plot (k)+2*(h_plot (k + 1) - h_plot(k))/(v_plot (k + 1) + v_plot (k)); 169 | end 170 | 171 | %%绘制视图%% 172 | % 绘制速度vs.高度视图 173 | subplot(3,2,1); 174 | plot(v_plot,h_plot,'--o'),grid on; 175 | ylabel('h(m)'); 176 | xlabel('v(m/s)'); 177 | 178 | % 绘制加速度vs.高度视图 179 | subplot(3,2,2); 180 | plot(acc_plot,h_plot,'--o'),grid on; 181 | ylabel('h(m)'); 182 | xlabel('a(m/s^2)'); 183 | 184 | % 绘制速度vs.时间视图 185 | subplot(3,2,3); 186 | plot(t_plot,v_plot,'--o'),grid on; 187 | ylabel('v(m/s)'); 188 | xlabel('t(s)'); 189 | 190 | % 绘制高度vs.时间视图 191 | subplot(3,2,4); 192 | plot(t_plot,h_plot,'--o'),grid on; 193 | ylabel('h(m)'); 194 | xlabel('t'); 195 | 196 | % 绘制加速度vs.时间视图 197 | subplot(3,2,5); 198 | plot(t_plot,acc_plot,'--o'),grid on; 199 | ylabel('a(m/s^2)'); 200 | xlabel('t'); 201 | -------------------------------------------------------------------------------- /Octave/Extended_KalmanFilter.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：Extended_KalmanFilter.m 6 | %% 程序功能：扩展卡尔曼滤波器案例（6.5.2节案例） 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | 9 | %% 程序初始化，清空工作空间，缓存， 10 | clear all; 11 | close all; 12 | clc; 13 | % 离散时间步长 14 | Ts = 0.01; 15 | 16 | %%%%%%%%%%%%%%%%%参数设计%%%%%%%%%%%%%%%%%%%%% 17 | % 定义过程噪声协方差矩阵 18 | Q = [0.01 0; 0 0.01]; 19 | % 定义测量噪声协方差矩阵 20 | R_c = [0.1 0; 0 0.1]; 21 | % 定义重力加速度 22 | g=10; 23 | % 定义连杆长度 24 | l=0.5; 25 | 26 | %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% 27 | % 初始化系统状态 28 | x0 = [pi/4; 0]; 29 | % 初始化状态赋值 30 | x = x0; 31 | % 初始化测量值 32 | z0 = [0; 0]; 33 | % 初始化测量值赋值 34 | z = z0; 35 | % 初始化先验估计 36 | x_hat_minus0 = [0; 0]; 37 | % 初始化先验估计赋值 38 | x_hat_minus = x_hat_minus0; 39 | % 初始化后验估计 40 | x_hat0 = [pi/4; 0]; 41 | % 初始化后验估计赋值 42 | x_hat = x_hat0; 43 | % 初始化后验估计误差协方差矩阵 44 | P0 = [1 0;0 1]; 45 | % 初始后验估计误差协方差矩阵赋值 46 | P = P0; 47 | % 计算变量维度 48 | n = size (x,1); 49 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 50 | % 定义系统运行步数 51 | k_steps = 200; 52 | % 定义x_history零矩阵，用于储存系统状态结果，维度n x k_steps 53 | x_history = zeros(n,k_steps); 54 | % 定义x_hat_history零矩阵，用于储存后验估计结果，维度n x k_steps 55 | x_hat_history = zeros(n,k_steps); 56 | % 定义x_hat_minus_history零矩阵，用于储存先验估计结果，维度n x k_steps 57 | x_hat_minus_history = zeros(n,k_steps); 58 | % 定义z_history零矩阵，用于测量结果，维度n x k_steps 59 | z_history = zeros(n,k_steps); 60 | 61 | %%%%%%%%%%%%%生成过程与测量噪声%%%%%%%%%%%%%%%%%% 62 | Q_a = [0.01 0; 0 0.01]; 63 | % 定义真实的测量噪声协方差矩阵 64 | R_a = [0.1 0; 0 0.1]; 65 | % 随机生成过程噪声 66 | w = chol(Q_a)* randn(2,k_steps); 67 | % 随机生成测量噪声 68 | v = sqrt(R_a)* randn(2,k_steps); 69 | 70 | %% Kalman Filter 71 | 72 | for k = 1:k_steps 73 | % 系统状态空间方程，计算实际状态变量x1 74 | x(1) = x(1) + x(2)*Ts + w(1,k); 75 | % 系统状态空间方程，计算实际状态变量x2 76 | x(2) = x(2) - g/l*sin(x(1))*Ts + w(2,k); 77 | % 计算实际测量值，在实际应用中，这一项来自传感器测量 78 | z(1) = x(1) + v(1,k); 79 | z(2) = x(2) + v(2,k); 80 | 81 | %%%%%%%%%%%%%%%%%%扩展卡尔曼滤波器%%%%%%%%%%%%%%%%% 82 | % 计算先验状态估计 83 | x_hat_minus(1) = x_hat(1) + x_hat(2)*Ts; 84 | x_hat_minus(2) = x_hat(2) -g/l*sin(x_hat(1))*Ts; 85 | % 计算更新雅可比矩阵 86 | A = [1 Ts ; -g/l*cos(x_hat(1))*Ts,1]; 87 | W = [1 0; 0 1]; 88 | H_m = [1 0; 0 1]; 89 | V = [1 0; 0 1]; 90 | %% 计算先验估计误差协方差矩阵 91 | P_minus = A*P*transpose(A) + transpose(W)*Q*W; 92 | %% 计算卡尔曼增益 93 | K = P_minus*transpose(H_m)/(H_m*P_minus*transpose(H_m)+ V *R_c*transpose(V) ); 94 | %% 更新后验估计 95 | x_hat = x_hat_minus + K*(z-x_hat_minus); 96 | %% 后验估计误差协方差矩阵 97 | P = (eye(n)- K*H_m)*P_minus; 98 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 99 | 100 | % 保存系统状态到预先定义矩阵的相应位置 101 | x_history (:,k+1) = x; 102 | % 保存测量值到预先定义矩阵的相应位置 103 | z_history (:,k+1) = z; 104 | % 保存先验估计到预先定义矩阵的相应位置 105 | x_hat_minus_history (:,k+1) = x_hat_minus; 106 | % 保存后验估计到预先定义矩阵的相应位置 107 | x_hat_history (:,k+1) = x_hat; 108 | end 109 | 110 | 111 | %%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%% 112 | figure(1, 'position',[150 150 1500 500]); 113 | % x1真实结果 114 | plot (0:length(x_history)-1,x_history(1,:),'--','LineWidth',2); 115 | hold on 116 | % x1测量值 117 | plot ( 0:length(z_history)-1,z_history(1,:),'*','MarkerSize',8) 118 | ylim([-3 3]); 119 | % x1先验估计值 120 | plot (0:length(x_hat_minus_history)-1,x_hat_minus_history(1,:),'o','MarkerSize',8); 121 | % x1后验估计值 122 | plot ( 0:length(x_hat_history)-1,x_hat_history(1,:),'LineWidth',2); 123 | legend(' 真实值 ',' 测量值 ',' 先验估计值 ',' 后验估计值 ') 124 | set(legend, 'Location', 'southeast','FontSize', 20); 125 | hold off; 126 | grid on 127 | % 计算Ems 均方误差 128 | Ems= (x_hat_history(1,:)-x_history(1,:))*(x_hat_history(1,:)-x_history(1,:))'/k_steps 129 | 130 | 131 | -------------------------------------------------------------------------------- /Octave/F1_LQR_Gain.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：F1_LQR_Gain %% [F1]反馈矩阵求解模块 6 | %% 模块功能：求解LQR反馈增益F 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | 9 | 10 | %% 输入：系统矩阵 A，B； 权重矩阵 Q，R，S 11 | %% 输出：反馈增益矩阵F 12 | function [F] = F1_LQR_Gain(A,B,Q,R,S); 13 | 14 | % 计算系统矩阵维度，n 15 | n=size(A,1); 16 | % 计算输入矩阵维度，p 17 | p=size(B,2); 18 | % 系统终值代价权重矩阵，定义为P0 19 | P0 = S; 20 | % 定义最大迭代次数，用于限制程序运行时间 21 | max_iter = 200; 22 | % 初始化矩阵P为0矩阵，后续用于存放计算得到的一系列矩阵P[k] 23 | P = zeros(n,n*max_iter); 24 | % 初始化矩阵P的第一个位置为P0 25 | P(:,1:n) = P0; 26 | % 定义P[k-1]的初值为P0，即当k=1时，参考式（4.4.23）与（4.4.24） 27 | P_k_min_1 = P0; 28 | % 定义系统稳态误差阈值，用于判断系统是否到达稳态 29 | tol = 1e-3; 30 | % 初始化系统误差为无穷 31 | diff = Inf; 32 | % 初始化系统反馈增益为无穷 33 | F_N_min_k = Inf; 34 | % 初始化系统迭代步 35 | k = 1; 36 | 37 | % 判断系统是否达到稳态，即相邻步的增益差是否小于预设阈值，如达到稳态跳出while循环 38 | while diff > tol 39 | % 将系统增益F[N-k]赋值给Fpre[N-k]，此步骤用于判断系统是否达到稳态 40 | F_N_min_k_pre = F_N_min_k; 41 | % 计算F[N-k],参考式（4.4.23b） 42 | F_N_min_k = inv(R+B'*P_k_min_1*B)*B'*P_k_min_1*A; 43 | % 计算P[k]，参考式（4.4.24b） 44 | P_k = (A-B*F_N_min_k)'*P_k_min_1*(A-B*F_N_min_k)+(F_N_min_k)'*R*(F_N_min_k)+Q; 45 | % 将P[k]矩阵存入P矩阵的相应位置 46 | P(:,n*k-n+1:n*k) = P_k; 47 | % 更新P[k-1],用于下一次迭代 48 | P_k_min_1 = P_k; 49 | % 计算系统相邻步增益差值 50 | diff = abs(max(F_N_min_k - F_N_min_k_pre)); 51 | % 迭代步加 1 52 | k = k + 1; 53 | % 如程序超过预设最大迭代步，则报错 54 | if k > max_iter 55 | error('Maximum Number of Iterations Exceeded'); 56 | end 57 | end 58 | % 输出系统迭代步 59 | fprintf('No. of Interation is %d \n', k); 60 | % 模块输出：系统增益F 61 | F=F_N_min_k; 62 | end 63 | -------------------------------------------------------------------------------- /Octave/F2_InputAugmentMatrix_SS_U.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：F2_InputAugmentMatrix_SS_U %% [F2]稳态非零控制矩阵转化模块 6 | %% 模块功能：计算系统增广矩阵Aa，Ba，Qa，Sa，R以及稳态控制输入ud 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | 9 | 10 | %% 输入：系统矩阵 A，B； 权重矩阵： Q，R，S； 稳态目标状态： xd 11 | %% 输出：增广矩阵Aa，Ba，Sa，Qa，权重矩阵R，稳态控制输入ud 12 | function [Aa,Ba,Qa,Sa,R,ud] = F2_InputAugmentMatrix_SS_U(A,B,Q,R,S,xd); 13 | 14 | % 计算系统矩阵维度，n 15 | n=size(A,1); 16 | % 计算输入矩阵维度，p 17 | p=size(B,2); 18 | % 构建增广矩阵Ca，参考式（4.5.17） 19 | Ca =[eye(n) -eye(n)]; 20 | % 构建增广矩阵Aa，参考式（4.5.16b） 21 | Aa = [A eye(n)-A;zeros(n) eye(n)]; 22 | % 构建增广矩阵Ba，参考式（4.5.16b） 23 | Ba = [B;zeros(n,p)]; 24 | % 构建增广矩阵Qa，参考式（4.5.18） 25 | Qa = transpose(Ca)*Q*Ca; 26 | % 构建增广矩阵Sa，参考式（4.5.18） 27 | Sa = transpose(Ca)*S*Ca; 28 | % 设计权重矩阵R（这里R不变） 29 | R = R; 30 | % 计算稳态控制输入ud 31 | ud = mldivide(B,(eye(n)-A)*xd); 32 | end 33 | -------------------------------------------------------------------------------- /Octave/F3_InputAugmentMatrix_Delta_U.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：F3_InputAugmentMatrix_Delta_U %% [F3]输入增量控制矩阵转换模块 6 | %% 模块功能：计算系统增广矩阵Aa，Ba，Qa，Sa，R 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | 9 | 10 | %% 输入：系统矩阵 A，B； 权重矩阵： Q，R，S； 目标转移矩阵： AD 11 | %% 输出：增广矩阵Aa，Ba，Sa，Qa，权重矩阵R 12 | function [Aa,Ba,Qa,Sa,R] = F3_InputAugmentMatrix_Delta_U(A,B,Q,R,S,AD); 13 | 14 | % 计算系统矩阵维度，n 15 | n=size(A,1); 16 | % 计算输入矩阵维度，p 17 | p=size(B,2); 18 | % 构建增广矩阵Ca，参考式（4.5.25） 19 | Ca =[eye(n) -eye(n) zeros(n,p)]; 20 | % 构建增广矩阵Aa，参考式（4.5.24b） 21 | Aa = [A zeros(n) B;zeros(n) AD zeros(n,p);zeros(p,n) zeros(p,n) eye(p,p)]; 22 | % 构建增广矩阵Ba，参考式（4.5.24b） 23 | Ba = [B;zeros(n,p);eye(p)]; 24 | % 构建增广矩阵Qa，参考式（4.5.26） 25 | Qa = transpose(Ca)*Q*Ca; 26 | % 构建增广矩阵Sa，参考式（4.5.26） 27 | Sa = transpose(Ca)*S*Ca; 28 | % 设计权重矩阵R（这里R不变） 29 | R = R; 30 | end 31 | -------------------------------------------------------------------------------- /Octave/F4_MPC_Matrices_PM.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：F4_MPC_Matrices_PM %% [F4]性能指标矩阵转换模块 6 | %% 模块功能： 7 | %% 求解模型预测控制中二次规划所需矩阵F，H 8 | %% 求解模型预测控制一系列中间过程矩阵Phi，Gamma，Omega，Psi 9 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 10 | 11 | %% 输入：系统矩阵 A，B； 权重矩阵 Q，R，S； 预测区间：N_P 12 | %% 输出：二次规划矩阵F，H 中间过程矩阵Phi，Gamma，Omega，Psi 13 | function [Phi,Gamma,Omega,Psi,F,H]= F4_MPC_Matrices_PM(A,B,Q,R,S,N_P) 14 | 15 | % 计算系统矩阵维度，n 16 | n = size(A,1); 17 | % 计算输入矩阵维度，p 18 | p = size(B,2); 19 | 20 | % 初始化Phi矩阵并定义维度 21 | Phi = zeros(N_P*n,n); 22 | % 初始化Gamma矩阵并定义维度 23 | Gamma = zeros(N_P*n,N_P*p); 24 | % 定义临时对角单位矩阵 25 | tmp = eye(n); % Create a n x n "I" matrix 26 | % 定义for循环行向量 27 | rows = 1:n; 28 | % for循环，用于构建Phi和Gamma矩阵 29 | for i = 1:N_P 30 | % 构建Phi矩阵，参考式（5.3.5b） 31 | Phi((i-1)*n+1:i*n,:) = A^i; 32 | % 构建Gamma矩阵，参考式（5.3.5b） 33 | Gamma(rows,:) = [tmp*B,Gamma(max(1,rows-n), 1:end-p)]; 34 | % Gamma矩阵行数更新，由于Gamma矩阵是由一系列“小”矩阵组成的，因此下一个循环要更新n行 35 | rows =i*n+(1:n); 36 | % 构建临时矩阵用于矩阵A的幂计算 37 | tmp= A*tmp; 38 | end 39 | 40 | % 构建Omega矩阵，包含Q矩阵的部分 41 | Omega = kron(eye(N_P-1),Q); 42 | % 构建最终Omega矩阵，包含S矩阵 43 | Omega = blkdiag(Omega,S); 44 | % 构建Psi矩阵，其为R矩阵组成的对角阵 45 | Psi = kron(eye(N_P),R); 46 | 47 | % 计算二次规划矩阵F 48 | F = Gamma'*Omega*Phi; 49 | % 计算二次规划矩阵H 50 | H = Psi+Gamma'*Omega*Gamma; 51 | 52 | end 53 | -------------------------------------------------------------------------------- /Octave/F5_MPC_Controller_noConstraints.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：F5_MPC_Controller_noConstraints %% [F5]无约束二次规划求解模块 6 | %% 模块功能：利用二次规划求解模型预测控制中的系统控制量 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | 9 | %% 输入：二次规划矩阵 F，H； 系统控制量维度 p； 系统状态：x 10 | %% 输出：系统控制（输入） U，u 11 | 12 | function [U,u]= F5_MPC_Controller_noConstraints(x,F,H,p) 13 | 14 | % 选取最优化求解模式 15 | options = optimset('MaxIter', 200); 16 | % 利用二次规划求解系统控制（输入） 17 | [U, FVAL, EXITFLAG, OUTPUT, LAMBDA] = quadprog(H,F*x,[],[],[],[],[],[],[],options); 18 | % 根据模型预测控制的策略，仅选取所得输入的第一项， 参考（5.3.18） 19 | u = U(1:p,1); 20 | end 21 | -------------------------------------------------------------------------------- /Octave/F6_MPC_Matrices_Constraints.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：F6_MPC_Matrices_Constraints %% [F6]约束条件矩阵转换模块 6 | %% 模块功能：生成MPC控制器所需的约束矩阵 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | 9 | %% 输入：系统状态上下限x_low,x_high； 输入上下限u_low,u_high； 矩阵Phi，Gamma； 预测区间N_P 10 | %% 输出：含约束二次规划矩阵 M,Beta_bar,b 11 | 12 | function [M,Beta_bar,b]= F6_MPC_Matrices_Constraints(x_low,x_high,u_low,u_high,N_P,Phi,Gamma) 13 | % 计算系统状态维度 14 | n = size (x_low,1); 15 | % 计算系统输入维度 16 | p = size (u_low,1); 17 | % 构建M矩阵，参考式（5.5.14c） 18 | M = [zeros(p,n);zeros(p,n);-eye(n);eye(n)]; 19 | % 构建F矩阵，参考式（5.5.14c） 20 | F = [-eye(p);eye(p);zeros(n,p);zeros(n,p)]; 21 | % 构建Beta矩阵，参考式（5.5.14c） 22 | Beta = [-u_low; u_high; -x_low; x_high]; 23 | % 构建M_N矩阵，参考式（5.5.14c） 24 | M_Np = [-eye(n);eye(n)]; 25 | % 构建Beta_N矩阵，参考式（5.5.14c） 26 | Beta_N = [-x_low; x_high]; 27 | 28 | % 构建M_bar矩阵，参考式（5.5.15b） 29 | M_bar = zeros((2*n+2*p)*N_P+2*n,n); 30 | M_bar (1: (2*n+2*p) , :) = M; 31 | % 构建Beta_bar矩阵，参考式（5.5.15e），模块输出 32 | Beta_bar = [repmat(Beta,N_P,1); Beta_N]; 33 | 34 | % 初始化M_2bar矩阵 35 | M_2bar = M; 36 | % 初始化F_2bar矩阵 37 | F_2bar = F; 38 | 39 | % for循环创建M_2bar和F_2bar矩阵 40 | for i=1:N_P-2 41 | M_2bar = blkdiag(M_2bar, M); 42 | F_2bar = blkdiag(F_2bar, F); 43 | end 44 | M_2bar = blkdiag(M_2bar,M_Np); 45 | % 构建M_2bar矩阵最终形式（加入顶部一行的零矩阵） 46 | M_2bar = [zeros(2*n+2*p,n*N_P); M_2bar]; 47 | F_2bar = blkdiag(F_2bar, F); 48 | % 构建F_2bar矩阵最终形式（加入底部一行的零矩阵） 49 | F_2bar = [F_2bar; zeros(2*n,p*N_P)]; 50 | % 构建b矩阵，参考式（5.5.9） 51 | b = -(M_bar + M_2bar*Phi); 52 | % 构建M矩阵，参考式（5.5.9） 53 | M = M_2bar*Gamma + F_2bar; 54 | end 55 | 56 | -------------------------------------------------------------------------------- /Octave/F7_MPC_Controller_withConstriants.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：F7_MPC_Controller_withConstraints %% [F7]含约束二次规划求解模块 6 | %% 模块功能：利用二次规划求解模型预测控制中的系统控制量-含约束 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | 9 | %% 输入：二次规划矩阵 F，H； 系统控制量维度 p； 系统状态：x 约束条件矩阵 M,Beta_bar,b 10 | %% 输出：系统控制（输入） U，u 11 | function [U, u]= F7_MPC_Controller_withConstriants(x,F,H,M,Beta_bar,b,p) 12 | % 利用二次规划求解系统控制（输入） 13 | U = quadprog(H,F*x,M,Beta_bar+b*x,[],[],[],[]); 14 | % 根据模型预测控制的策略，仅选取所得输入的第一项， 参考（5.3.18） 15 | u = U(1:p,1); 16 | end 17 | -------------------------------------------------------------------------------- /Octave/F8_LinearKalmanFilter.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：F8_KalmanFilter %% [F8]线性卡尔曼滤波器 6 | %% 模块功能：求解卡尔曼滤波最优估计值 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | 9 | %% 输入：系统矩阵 A，B； 协方差矩阵 Q_c，R_c； 测量矩阵 H； 10 | %% 第k-1次的： 测量值 z； 后验估计误差协方差矩阵 P； 后验估计值 x_hat； 输入 u； 11 | %% 输出：第k次的： 后验估计值: x_hat； 后验估计误差协方差矩阵 P; 12 | function [x_hat, x_hat_minus, P]= F8_LinearKalmanFilter(A,B,Q_c,R_c,H,z,x_hat,P,u) 13 | %% 计算先验状态估计 14 | x_hat_minus = A*x_hat + B*u; 15 | %% 计算先验估计误差协方差矩阵 16 | P_minus = A*P*transpose(A) + Q_c; 17 | %% 计算卡尔曼增益 18 | K = P_minus*transpose(H)/(H*P_minus*transpose(H)+R_c); 19 | %% 更新后验估计 20 | x_hat = x_hat_minus + K*(z-H*x_hat_minus); 21 | %% 后验估计误差协方差矩阵 22 | P = (eye(size(A,1)) - K*H)*P_minus; 23 | end 24 | -------------------------------------------------------------------------------- /Octave/KalmanFilter_MPC_UAV.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：MPC_Kalman_UAV 6 | %% 程序功能：无人机高度速度模型预测控制与卡尔曼滤波器结合示例(6.4.3节案例) 7 | %% 所用模块： 8 | %% [F2]稳态非零控制矩阵转化模块 9 | %% [F4]性能指标矩阵转换模块 10 | %% [F6]约束条件矩阵转换模块 11 | %% [F7]含约束二次规划求解模块 12 | %% [F8]含约束二次规划求解模块 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | 15 | %% 程序初始化，清空工作空间，缓存， 16 | clear all; 17 | close all; 18 | clc; 19 | % 读取Octave控制数据库（注：如使用Matlab，可删除或注释掉本行代码） 20 | pkg load control; 21 | % 读取Octave优化求解器数据库（注：如使用Matlab，可删除或注释掉本行代码） 22 | pkg load optim; 23 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 24 | 25 | %% 定义系统参数 26 | % 定义无人机质量 27 | m = 1; 28 | % 定义重力加速度常数 29 | g = 10; 30 | 31 | %%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%% 32 | % 构建系统矩阵A，n x n 33 | A = [0 1 0; 0 0 1 ;0 0 0]; 34 | % 计算A矩阵维度 35 | n= size (A,1); 36 | % 构建输入矩阵B，n x p 37 | B = [0; 1/m; 0]; 38 | % 计算输入矩阵维度 39 | p = size(B,2); 40 | % 构建测量矩阵H_m，n x n 41 | H_m = [1 0 0; 0 1 0; 0 0 1]; 42 | 43 | %%%%%%%%%%%%%%%%%%系统离散%%%%%%%%%%%%%%%%%%%% 44 | % 离散时间步长 45 | Ts = 0.1; 46 | % 连续系统转离散系统 47 | sys_d = c2d(ss(A,B),Ts); 48 | % 提取离散系统A矩阵 49 | A = sys_d.a; 50 | % 提取离散系统B矩阵 51 | B = sys_d.b; 52 | 53 | %%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%%%%%%%%% 54 | % 设计状态权重系数矩阵, n x n 55 | Q=[1 0 0; 0 1 0; 0 0 0]; 56 | % 设计终值权重系数矩阵, n x n 57 | S=[1 0 0; 0 1 0; 0 0 0]; 58 | % 设计输入权重系数矩阵, p x p 59 | R=0.1; 60 | 61 | %%%%%%%%%%%%%%%%%系统参考值%%%%%%%%%%%%%%%%%%%% 62 | % 系统状态参考值 63 | xd = [10 ; 0 ; -g]; 64 | % 构建目标转移矩阵 65 | AD = eye(n); 66 | % 计算目标输入 67 | ud = mldivide(B,(eye(n)-A)*xd); 68 | 69 | %%%%%%%%%%%%卡尔曼滤波器参数设计%%%%%%%%%%%%%%%%%% 70 | % 定义过程噪声协方差矩阵 71 | Q_c = [0.01 0 0; 0 0.01 0; 0 0 0]; 72 | % 定义测量噪声协方差矩阵 73 | R_c = [1 0 0; 0 1 0; 0 0 0]; 74 | 75 | %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% 76 | % 初始化系统状态 77 | x0 = [0; 0; -g]; x = x0; 78 | % 初始化增广状态矩阵 79 | xa = [x; xd]; 80 | % 初始化系统输入 81 | u0 = 0; u = 0; 82 | % 初始化后验估计 83 | x_hat0 = [0; 1; -g]; 84 | % 初始化后验估计赋值 85 | x_hat = x_hat0; 86 | % 初始化后验估计误差协方差矩阵 87 | P0 = [1 0 0;0 1 0; 0 0 0]; 88 | % 初始后验估计误差协方差矩阵赋值 89 | P = P0; 90 | 91 | 92 | %%%%%%%%%%%%%%系统约束定义%%%%%%%%%%%%%%%%%%%%% 93 | % 输入下限 94 | u_low = -3; 95 | % 输入上限 96 | u_high = 2; 97 | % 状态下限 98 | x_low = [0;-4;-10]; 99 | % 状态上限 100 | x_high = [12;5;-10]; 101 | % 增广状态下限 102 | xa_low = [x_low;-inf;-inf;-inf]; 103 | % 增广状态上限 104 | xa_high = [x_high;inf;inf;inf]; 105 | 106 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 107 | % 定义系统运行步数 108 | k_steps = 100; 109 | % 定义x_history零矩阵，用于储存系统状态结果，维度n x k_step 110 | x_history = zeros(n,k_steps); 111 | % 定义u_history零矩阵，用于储存系统输入结果，维度p x k_step 112 | u_history = zeros(p,k_steps); 113 | % 定义x_hat_history零矩阵，用于储存后验估计结果，维度n x k_steps 114 | x_hat_history = zeros(n,k_steps); 115 | % 定义x_hat_minus_history零矩阵，用于储存先验估计结果，维度n x k_steps 116 | x_hat_minus_history = zeros(n,k_steps); 117 | % 定义z_historyy零矩阵，用于测量结果，维度n x k_steps 118 | z_history = zeros(n,k_steps); 119 | 120 | %%%%%%%%%%%%%%%定义仿真环境%%%%%%%%%%%%%%%%%%%% 121 | % 定义过程噪声矩阵W，维度n x k_steps 122 | w = zeros (n,k_steps); 123 | % 定义测量噪声矩阵V，维度n x k_steps 124 | v = zeros (n,k_steps); 125 | % 从文件NoiseData.csv中读取数据 126 | % 数据来自于系统随机生成，保存为文件可以方便进行多组实验之间的对比 127 | w = csvread('NoiseData.csv')(2:4,:); 128 | v = csvread('NoiseData.csv')(6:8,:); 129 | 130 | 131 | % 定义预测区间 132 | N_P = 20; 133 | 134 | % 调用模块[F2]，计算系统增广矩阵Aa，Ba，Qa，Sa，R以及目标输入ud 135 | [Aa,Ba,Qa,Sa,R,ud] = F2_InputAugmentMatrix_SS_U(A,B,Q,R,S,xd); 136 | 137 | % 调用模块[F4]计算二次规划需用到的矩阵 138 | [Phi,Gamma,Omega,Psi,F,H]= F4_MPC_Matrices_PM(Aa,Ba,Qa,R,Sa,N_P); 139 | 140 | % 调用模块[F6]计算含约束二次规划需用到的矩阵 141 | [M,Beta_bar,b]= F6_MPC_Matrices_Constraints(xa_low,xa_high,u_low,u_high,N_P,Phi,Gamma); 142 | 143 | 144 | % for循环开始仿真 145 | for k = 1 : k_steps 146 | % 调用模块[F7]计算系统系统控制（输入增量） 147 | [delta_U, delta_u]= F7_MPC_Controller_withConstriants(xa,F,H,M,Beta_bar,b,p); 148 | % 根据输入增量计算系统输入 149 | u = delta_u+ud; 150 | % 系统输入代入系统方程，计算系统响应 151 | x = A * x + B*u + w(:,k); 152 | % 计算实际测量值，在实际应用中，这一项来自传感器测量 153 | z = H_m * x + v(:,k); 154 | % 使用卡尔曼滤波器 155 | [x_hat,x_hat_minus, P]= F8_LinearKalmanFilter(A,B,Q_c,R_c,H_m,z,x_hat,P,u); 156 | % 更新增广矩阵xa,使用后验估计x_hat，因为真实值x在实际情况中不可知 157 | xa = [x_hat; xd]; 158 | % 保存系统状态到预先定义矩阵的相应位置 159 | x_history (:,k+1) = x; 160 | % 保存系统输入到预先定义矩阵的相应位置 161 | u_history (:,k) = u ; 162 | % 保存测量值到预先定义矩阵的相应位置 163 | z_history (:,k+1) = z; 164 | % 保存先验估计到预先定义矩阵的相应位置 165 | x_hat_minus_history (:,k+1) = x_hat_minus; 166 | % 保存后验估计到预先定义矩阵的相应位置 167 | x_hat_history (:,k+1) = x_hat; 168 | end 169 | 170 | %%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%% 171 | figure(1, 'position',[150 100 1550 900]); 172 | subplot (2, 1, 1); 173 | % x1真实结果 174 | plot (0:length(x_history)-1,x_history(1,:),'--','LineWidth',2); 175 | hold on 176 | % x1测量值 177 | plot ( 0:length(x_history)-1,z_history(1,:),'*','MarkerSize',8) 178 | ylim([-2 14]); 179 | % x1先验估计值 180 | plot (0:length(x_hat_minus_history)-1,x_hat_minus_history(1,:),'o','MarkerSize',8); 181 | % x1后验估计值 182 | plot ( 0:length(x_hat_history)-1,x_hat_history(1,:),'LineWidth',2); 183 | grid on; 184 | legend(' 真实值 ',' 测量值 ',' 先验估计值 ',' 后验估计值 ') 185 | set(legend, 'Location', 'southeast','FontSize', 20); 186 | 187 | % 系统输入 188 | subplot (2, 1, 2); 189 | hold; 190 | stairs (0:length(u_history)-1,u_history(1,:)); 191 | legend("u") 192 | grid on 193 | xlim([0 k_steps]); 194 | % 计算Ems 均方误差 195 | Ems= (x_hat_history(1,:)-x_history(1,:))*(x_hat_history(1,:)-x_history(1,:))'/k_steps 196 | 197 | -------------------------------------------------------------------------------- /Octave/KalmanFilter_UAV_ConstantInput.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：KalmanFilter_UAV_ConstantInput 6 | %% 程序功能：线性卡尔曼滤波器案例（6.4.2节，无人机高度预测） 7 | %% 所用模块： 8 | %% [F8]线性卡尔曼滤波器 9 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 10 | 11 | 12 | %% 程序初始化，清空工作空间，缓存， 13 | clear all; 14 | close all; 15 | clc; 16 | % 读取Octave控制数据库（注：如使用Matlab，可删除或注释掉本行代码） 17 | pkg load control; 18 | %%%%%%%%%%%%%%%%%%%%%%%%%% 19 | 20 | %%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%% 21 | % 构建系统矩阵A，n x n 22 | A = [0 1 0; 0 0 1 ;0 0 0]; 23 | % 计算A矩阵维度 24 | n = size (A,1); 25 | % 构建输入矩阵B，n x p 26 | B = [0; 1; 0]; 27 | % 计算输入矩阵维度 28 | p = size(B,2); 29 | % 定义测量矩阵H_m, n x n 30 | H_m = [1 0 0; 0 1 0; 0 0 1]; 31 | % 重力加速度常数 32 | g = 10; 33 | 34 | %%%%%%%%%%%%%%%%%%系统离散%%%%%%%%%%%%%%%%%%%% 35 | % 离散时间步长 36 | Ts = 0.1; 37 | % 连续系统转离散系统 38 | sys_d = c2d(ss(A,B),Ts); 39 | % 提取离散系统A矩阵 40 | A = sys_d.a; 41 | % 提取离散系统B矩阵 42 | B = sys_d.b; 43 | 44 | %%%%%%%%%%%%%%%%%参数设计%%%%%%%%%%%%%%%%%%%%% 45 | % 定义过程噪声协方差矩阵 46 | Q_c = [0.01 0 0; 0 0.01 0; 0 0 0]; 47 | % 定义测量噪声协方差矩阵 48 | R_c = [1 0 0; 0 1 0; 0 0 0]; 49 | 50 | %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% 51 | % 初始化系统状态 52 | x0 = [0; 1 ; -10]; 53 | % 初始化状态赋值 54 | x = x0; 55 | % 系统输入初始化 56 | u0 = g; 57 | % 初始输入赋值 58 | u = u0; 59 | % 初始化后验估计 60 | x_hat0 = [0; 1; -10]; 61 | % 初始化后验估计赋值 62 | x_hat = x_hat0; 63 | % 初始化后验估计误差协方差矩阵 64 | P0 = [1 0 0;0 1 0; 0 0 0]; 65 | % 初始后验估计误差协方差矩阵赋值 66 | P = P0; 67 | 68 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 69 | % 定义系统运行步数 70 | k_steps = 100; 71 | % 定义x_history零矩阵，用于储存系统状态结果，维度n x k_steps 72 | x_history = zeros(n,k_steps); 73 | % 定义u_history零矩阵，用于储存系统输入结果，维度p x k_steps 74 | u_history = zeros(p,k_steps); 75 | % 定义x_hat_history零矩阵，用于储存后验估计结果，维度n x k_steps 76 | x_hat_history = zeros(n,k_steps); 77 | % 定义x_hat_minus_history零矩阵，用于储存先验估计结果，维度n x k_steps 78 | x_hat_minus_history = zeros(n,k_steps); 79 | % 定义z_historyy零矩阵，用于测量结果，维度n x k_steps 80 | z_history = zeros(n,k_steps); 81 | 82 | 83 | %%%%%%%%%%%%%%%定义仿真环境%%%%%%%%%%%%%%%%%%%% 84 | % 定义过程噪声矩阵W，维度n x k_steps 85 | w = zeros (n,k_steps); 86 | % 定义测量噪声矩阵V，维度n x k_steps 87 | v = zeros (n,k_steps); 88 | % 从文件NoiseData.csv中读取数据 89 | % 数据来自于系统随机生成，保存为文件可以方便进行多组实验之间的对比 90 | w = csvread('NoiseData.csv')(2:4,:); 91 | v = csvread('NoiseData.csv')(6:8,:); 92 | %%%%%%%%%%%%%生成过程与测量噪声%%%%%%%%%%%%%%%%%% 93 | % 使用以下代码生成随机噪声，之后保存在NoiseData.csv中，方便下次读取。%%%%%% 94 | 95 | % 定义真实的过程噪声协方差矩阵 96 | %Q_ca = [0.05 0; 0 0.05]; 97 | % 定义真实的测量噪声协方差矩阵 98 | %R_ca = [1 0; 0 1]; 99 | % 随机生成过程噪声 100 | %w(1:2,:) = chol(Q_ca)* randn(2,k_steps); 101 | % 随机生成测量噪声 102 | %v(1:2,:) = chol(R_ca)* randn(2,k_steps); 103 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 104 | 105 | 106 | % 仿真开始，建立for循环 107 | for k = 1:k_steps 108 | % 系统状态空间方程，计算实际状态变量 109 | x = A * x + B*u + w(:,k); 110 | % 计算实际测量值，在实际应用中，这一项来自传感器测量 111 | z = H_m * x + v(:,k); 112 | % 使用卡尔曼滤波器 113 | [x_hat,x_hat_minus, P]= F8_LinearKalmanFilter(A,B,Q_c,R_c,H_m,z,x_hat,P,u); 114 | % 保存系统状态到预先定义矩阵的相应位置 115 | x_history (:,k+1) = x; 116 | % 保存测量值到预先定义矩阵的相应位置 117 | z_history (:,k+1) = z; 118 | % 保存先验估计到预先定义矩阵的相应位置 119 | x_hat_minus_history (:,k+1) = x_hat_minus; 120 | % 保存后验估计到预先定义矩阵的相应位置 121 | x_hat_history (:,k+1) = x_hat; 122 | end 123 | 124 | %%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%% 125 | figure(1, 'position',[150 150 1500 500]); 126 | % x1真实结果 127 | plot (0:length(x_history)-1,x_history(1,:),'--','LineWidth',2); 128 | hold on 129 | % x1测量值 130 | plot ( 0:length(z_history)-1,z_history(1,:),'*','MarkerSize',8) 131 | ylim([-2 12]); 132 | % x1先验估计值 133 | plot (0:length(x_hat_minus_history)-1,x_hat_minus_history(1,:),'o','MarkerSize',8); 134 | % x1后验估计值 135 | plot ( 0:length(x_hat_history)-1,x_hat_history(1,:),'LineWidth',2); 136 | legend(' 真实值 ',' 测量值 ',' 先验估计值 ',' 后验估计值 ') 137 | set(legend, 'Location', 'southeast','FontSize', 20); 138 | hold off; 139 | grid on 140 | % 计算Ems 均方误差 141 | Ems= (x_hat_history(1,:)-x_history(1,:))*(x_hat_history(1,:)-x_history(1,:))'/k_steps 142 | -------------------------------------------------------------------------------- /Octave/LQR_InvertedPed_Pendulum.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：LQR_InvertedPed_Pendulum 6 | %% 程序功能：平衡车控制 - 连续系统案例分析 （4.4.5节案例） 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | 9 | %% 程序初始化，清空工作空间，缓存， 10 | clear all; 11 | close all; 12 | clc; 13 | % 读取Octave控制数据库（注：如使用Matlab，可删除或注释掉本行代码） 14 | pkg load control; 15 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 16 | %% 定义系统参数 17 | % 定义重力加速度 18 | g = 10; 19 | % 定义平衡车连杆长度 20 | d = 1; 21 | %%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%% 22 | % 定义系统矩阵A 23 | A = [0 1; g/d 0]; 24 | % 计算系统矩阵A的维度 25 | n= size (A,1); 26 | % 定义输入矩阵B 27 | B = [0; 1]; 28 | % 计算输入矩阵B的维度 29 | p = size(B,2); 30 | % 定义矩阵C 31 | C = [1, 0]; 32 | % 定义矩阵D 33 | D = 0; 34 | %%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%%%%%%%%% 35 | % 设计系统状态权重矩阵q1，维度n x n,案例1 36 | q1 = [100 0;0 1]; 37 | % 设计系统状态权重矩阵q2，维度n x n,案例2 38 | q2 = [1 0;0 100]; 39 | % 设计系统状态权重矩阵q3，维度n x n,案例3 40 | q3 = [1 0;0 1]; 41 | % 设计系统输入权重矩阵r1，维度p x p，案例1 42 | r1 = 1; 43 | % 设计系统输入权重矩阵r2，维度p x p，案例2 44 | r2 = 1; 45 | % 设计系统输入权重矩阵r3，维度p x p，案例3 46 | r3 = 1; 47 | %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% 48 | % 状态初始化 49 | x0 = [pi/20;0]; x = x0; 50 | 51 | %%%%%%%%%%%%%代数Riccati方程求解%%%%%%%%%%%%%%%% 52 | % Riccati方程求解，案例1 53 | [P1,L1,G1] = care(A,B,q1,r1); 54 | % 计算反馈增益，参考式（4.4.52） 55 | K1=inverse(r1)*B'*P1; 56 | 57 | % Riccati方程求解，案例2 58 | [P2,L2,G2] = care(A,B,q2,r2); 59 | % 计算反馈增益，参考式（4.4.52） 60 | K2=inverse(r2)*B'*P2; 61 | 62 | % Riccati方程求解，案例3 63 | [P3,L3,G3] = care(A,B,q3,r3); 64 | % 计算反馈增益，参考式（4.4.52） 65 | K3=inverse(r3)*B'*P3; 66 | 67 | %%%%%%%%%%%%%%%构建闭环系统%%%%%%%%%%%%%%%%%%%%% 68 | % 构建闭环状态空间方程，案例1 69 | sys_cl1=ss(A-B*K1,[0;0],C,D); 70 | % 构建闭环状态空间方程，案例2 71 | sys_cl2=ss(A-B*K2,[0;0],C,D); 72 | % 构建闭环状态空间方程，案例3 73 | sys_cl3=ss(A-B*K3,[0;0],C,D); 74 | 75 | %%%%%%%%%%%%%%%系统仿真开始%%%%%%%%%%%%%%%%%%%%% 76 | t_span = 0.01; 77 | % 系统时间离散 78 | t=0:t_span:5; 79 | % 闭环系统初值响应，案例1 80 | [y1,t,x1]=initial(sys_cl1,x,t); 81 | % 闭环系统初值响应，案例2 82 | [y2,t,x2]=initial(sys_cl2,x,t); 83 | % 闭环系统初值响应，案例3 84 | [y3,t,x3]=initial(sys_cl3,x,t); 85 | 86 | %%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%% 87 | figure(1, 'position',[150 50 800 950]); 88 | %% 系统响应视图，状态量x1（角度） 89 | subplot(3,1,1); 90 | % 测试1结果 91 | plot(t,x1(:,1),"linewidth",2); 92 | % 近似计算x1的积分 93 | x1_1_cost = x1(:,1)'*x1(:,1)*t_span; 94 | hold on; 95 | % 测试2结果 96 | plot(t,x2(:,1),'--',"linewidth",2); 97 | % 近似计算x1的积分 98 | x2_1_cost = x2(:,1)'*x2(:,1)*t_span; 99 | hold on; 100 | % 测试3结果 101 | plot(t,x3(:,1),'-.',"linewidth",2); 102 | % 近似计算x1的积分 103 | x3_1_cost = x3(:,1)'*x3(:,1)*t_span; 104 | legend('测试一','测试二','测试三',"FontSize",14); 105 | xlim([0 4]); 106 | grid on; 107 | hold off; 108 | 109 | %% 系统响应结果视图，状态量x2（角速度） 110 | subplot(3,1,2); 111 | % 测试1结果 112 | plot(t,x1(:,2),"linewidth",2); 113 | % 近似计算x2的积分 114 | x1_2_cost = x1(:,2)'*x1(:,2)*t_span; 115 | hold on; 116 | % 测试2结果 117 | plot(t,x2(:,2),'--',"linewidth",2); 118 | % 近似计算x2的积分 119 | x2_2_cost = x2(:,2)'*x2(:,2)*t_span; 120 | hold on; 121 | % 测试3结果 122 | plot(t,x3(:,2),'-.',"linewidth",2); 123 | % 近似计算x2的积分 124 | x3_2_cost = x3(:,2)'*x3(:,2)*t_span; 125 | legend('测试一','测试二','测试三',"FontSize",14); 126 | xlim([0 4]); 127 | grid on; 128 | hold off; 129 | 130 | 131 | %% 系统响应结果视图，输入（加速度） 132 | subplot(3,1,3); 133 | % 测试1结果 134 | plot(t,-K1*x1',"linewidth",2); 135 | % 近似计算u的总代价 136 | u1_cost = (-K1*x1')*(-K1*x1')'*t_span'; 137 | hold on; 138 | % 测试2结果 139 | plot(t,-K2*x2','--',"linewidth",2); 140 | % 近似计算u的总代价 141 | u2_cost = (-K2*x2')*(-K2*x2')'*t_span; 142 | hold on; 143 | % 测试3结果 144 | plot(t,-K3*x3','-.',"linewidth",2); 145 | % 近似计算u的总代价 146 | u3_cost = (-K3*x3')*(-K3*x3')'*t_span; 147 | legend('测试一','测试二','测试三',"FontSize",14); 148 | xlim([0 4]); 149 | grid on; 150 | hold off; 151 | -------------------------------------------------------------------------------- /Octave/LQR_Test_1D.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：LQR_Test_1D 6 | %% 程序功能：离散型一维案例分析 - LQR方法 （4.4.2节案例） 7 | %% 所用模块：[F1]反馈矩阵求解模块 8 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 9 | 10 | %% 程序初始化，清空工作空间，缓存， 11 | clear all; 12 | close all; 13 | clc; 14 | % 读取Octave优化求解器数据库（注：如使用Matlab，可删除或注释掉本行代码） 15 | pkg load optim; 16 | 17 | %%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%% 18 | % 定义系统矩阵A 19 | A = 1; 20 | % 计算系统矩阵A的维度 21 | n = size (A,1); 22 | % 定义输入矩阵B 23 | B = 1; 24 | % 计算输入矩阵B的维度 25 | p = size(B,2); 26 | 27 | %%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%%%%%%%%% 28 | % 设计系统状态权重矩阵Q，维度n x n 29 | Q = 1; 30 | % 设计系统终值权重矩阵S，维度n x n 31 | S = 1; 32 | % 设计系统输入权重矩阵R，维度p x p 33 | R = 1; 34 | 35 | %%%%%%%%%%%%%%%%%系统参考值%%%%%%%%%%%%%%%%%%%% 36 | % 定义系统参考值xd 37 | xd = 0; 38 | 39 | %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% 40 | % 状态初始化 41 | x0 = 1; x = x0; 42 | % 输入初始化 43 | u0 = 0; u = u0; 44 | 45 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 46 | % 定义系统运行步数 47 | k_steps = 20; 48 | % 定义x_history零矩阵，用于储存系统状态结果，维度n x k_step 49 | x_history = zeros(n,k_steps); 50 | % 将系统状态初始值赋值到x_history矩阵第一个位置 51 | x_history (:,1) = x; 52 | % 定义u_history零矩阵，用于储存系统输入结果，维度p x k_step 53 | u_history = zeros(p,k_steps); 54 | % 将系统输入初始值赋值到u_history矩阵第一个位置 55 | u_history (:,1) = u; 56 | % 定义控制区间，对LQR控制而言，控制区间与运行步数，k_steps，一致 57 | N = k_steps; 58 | % 读取模块[F1]，计算系统反馈增益，F 59 | [F] = F1_LQR_Gain(A,B,Q,R,S); 60 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 61 | 62 | % 仿真开始，建立for循环 63 | for k = 1 : k_steps 64 | % 计算系统输入 65 | u = -F * x; 66 | % 系统输入代入系统方程，计算系统响应 67 | x = A * x + B * u; 68 | % 保存系统状态到预先定义矩阵的相应位置 69 | x_history (:,k+1) = x; 70 | % 保存系统输入到预先定义矩阵的相应位置 71 | u_history (:,k) = u; 72 | end 73 | 74 | %%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%% 75 | %% 结果视图：系统状态vs.运行步数 76 | subplot (2, 1, 1); 77 | plot (x_history(1,:)); 78 | legend("x1") 79 | grid on 80 | %% 结果视图：系统输入vs.运行步数 81 | subplot (2, 1, 2); 82 | hold; 83 | plot (u_history(1,:)); 84 | legend("u1") 85 | grid on 86 | -------------------------------------------------------------------------------- /Octave/LQR_Test_tracking_Delta_U_AD_MSD.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：LQR_Test_tracking_Delta_U_MSD 6 | %% 程序功能：弹簧质量阻尼系统 -输入增量非常数目标（4.5.4节案例） 7 | %% 所用模块： 8 | %% [F1]反馈矩阵求解模块 9 | %% [F3]输入增量控制矩阵转换模块 10 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 11 | 12 | %% 程序初始化，清空工作空间，缓存， 13 | clear all; 14 | close all; 15 | clc; 16 | % 读取Octave控制数据库（注：如使用Matlab，可删除或注释掉本行代码） 17 | pkg load control; 18 | % 读取Octave优化求解器数据库（注：如使用Matlab，可删除或注释掉本行代码） 19 | pkg load optim; 20 | %%%%%%%%%%%%%%%%%%%%%%%%%% 21 | 22 | %% 定义系统参数 23 | % 定义质量块质量 24 | m_sys = 1; 25 | % 定义阻尼系数 26 | b_sys = 0.5; 27 | % 定义弹簧弹性系数 28 | k_sys = 1; 29 | 30 | %%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%% 31 | % 构建系统矩阵A，n x n 32 | A = [0 1 ; -k_sys/m_sys -b_sys/m_sys]; 33 | % 计算A矩阵维度 34 | n = size (A,1); 35 | % 构建输入矩阵B，n x p 36 | B = [0; 1/m_sys]; 37 | % 计算输入矩阵维度 38 | p = size(B,2); 39 | 40 | %%%%%%%%%%%%%%%%%%系统离散%%%%%%%%%%%%%%%%%%%% 41 | % 离散时间步长 42 | Ts = 0.1; 43 | % 连续系统转离散系统 44 | sys_d = c2d(ss(A,B),Ts); 45 | % 提取离散系统A矩阵 46 | A = sys_d.a; 47 | % 提取离散系统B矩阵 48 | B = sys_d.b; 49 | 50 | %%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%%%%%%%%% 51 | % 设计状态权重系数矩阵, n x n 52 | Q = [1 0 ; 0 1]; 53 | % 设计终值权重系数矩阵, n x n 54 | S = [1 0; 0 1]; 55 | % 设计输入权重系数矩阵, p x p 56 | R = 1; 57 | 58 | %%%%%%%%%%%%%%%%%系统参考值%%%%%%%%%%%%%%%%%%%% 59 | % 系统状态参考值 60 | xd = [0 ; 0.2]; 61 | % 构建目标转移矩阵 62 | AD = c2d(ss( [0 1; 0 0],[0;0]),0.1).A; 63 | 64 | %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% 65 | % 初始化系统状态 66 | x0 = [0; 0]; 67 | % 初始化状态赋值 68 | x = x0; 69 | % 系统输入初始化 70 | u0 = 0; 71 | % 初始输入赋值 72 | u = u0; 73 | % 构建初始化增广状态矩阵，参考式（4.5.23） 74 | xa = [x; xd; u]; 75 | 76 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 77 | % 定义系统运行步数 78 | k_steps = 200; 79 | % 定义x_history零矩阵，用于储存系统状态结果，维度n x k_steps 80 | x_history = zeros(n,k_steps); 81 | % 定义u_history零矩阵，用于储存系统输入结果，维度p x k_steps 82 | u_history = zeros(p,k_steps); 83 | % 定义xd_history零矩阵，用于储存系统目标状态，维度n x k_steps 84 | xd_history = zeros(n,k_steps); 85 | 86 | % 调用模块[F3]，计算Aa，Ba，Qa，Sa，和R 87 | [Aa,Ba,Qa,Sa,R] = F3_InputAugmentMatrix_Delta_U(A,B,Q,R,S,AD); 88 | % 调用模块[F1]，计算系统反馈增益，F 89 | [F] = F1_LQR_Gain(Aa,Ba,Qa,R,Sa); 90 | 91 | % 仿真开始，建立for循环 92 | for k = 1 : k_steps 93 | % 系统目标状态不再是常数，if else语句定义不同阶段的系统目标状态 94 | if (k==50) 95 | xd = [xd(1) ; -0.2]; 96 | elseif (k==100) 97 | xd = [xd(1) ; 0.2]; 98 | elseif (k==150) 99 | xd = [xd(1) ; -0.2]; 100 | elseif (k==200) 101 | xd = [xd(1) ; 0.2]; 102 | end 103 | % 计算系统输入增量 104 | Delta_u = -F * xa; 105 | % 计算系统输入 106 | u = Delta_u + u; 107 | % 系统输入代入系统方程，计算系统响应 108 | x = A * x + B * u; 109 | % 更新系统增广状态，参考式（4.5.23） 110 | xd = AD * xd; 111 | xa = [x; xd; u]; 112 | % 保存系统状态到预先定义矩阵的相应位置 113 | x_history (:,k+1) = x; 114 | % 保存系统输入到预先定义矩阵的相应位置 115 | u_history (:,k) = u ; 116 | % 保存系统目标状态到预先定义的矩阵的相应位置 117 | xd_history (:,k+1) = xd; 118 | end 119 | 120 | %%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%% 121 | figure(1, 'position',[150 150 500 675]); 122 | % 系统状态x1结果图，质量块位移 123 | subplot (3, 1, 1); 124 | hold; 125 | plot (0:length(x_history)-1,x_history(1,:),"linewidth",1); 126 | plot (0:length(xd_history)-1,xd_history(1,:),'--',"linewidth",2); 127 | grid on 128 | legend("x1","x1d") 129 | hold off; 130 | xlim([0 k_steps]); 131 | ylim([-0.2 1.2]); 132 | 133 | % 系统状态x2结果图，质量块速度 134 | subplot (3, 1, 2); 135 | hold; 136 | plot (0:length(x_history)-1,x_history(2,:),"linewidth",1); 137 | plot (0:length(xd_history)-1,xd_history(2,:),'--',"linewidth",2); 138 | grid on 139 | legend("x2","x2d") 140 | hold off; 141 | xlim([0 k_steps]); 142 | ylim([-0.5 0.5]); 143 | 144 | % 系统输入结果图 145 | subplot (3, 1, 3); 146 | stairs (0:length(u_history)-1,u_history(1,:)); 147 | legend("u") 148 | grid on 149 | xlim([0 k_steps]); 150 | ylim([-0.5 1.4]); 151 | -------------------------------------------------------------------------------- /Octave/LQR_Test_tracking_Delta_U_MSD.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：LQR_Test_tracking_Delta_U_MSD 6 | %% 程序功能：弹簧质量阻尼系统 - 输入增量控制 （4.5.4节） 7 | %% 所用模块： 8 | %% [F1]反馈矩阵求解模块 9 | %% [F3]输入增量控制矩阵转换模块 10 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 11 | 12 | %% 程序初始化，清空工作空间，缓存， 13 | clear all; 14 | close all; 15 | clc; 16 | % 读取Octave控制数据库（注：如使用Matlab，可删除或注释掉本行代码） 17 | pkg load control; 18 | % 读取Octave优化求解器数据库（注：如使用Matlab，可删除或注释掉本行代码） 19 | pkg load optim; 20 | %%%%%%%%%%%%%%%%%%%%%%%%%% 21 | 22 | %% 定义系统参数 23 | % 定义质量块质量 24 | m_sys = 1; 25 | % 定义阻尼系数 26 | b_sys = 0.5; 27 | % 定义弹簧弹性系数 28 | k_sys = 1; 29 | 30 | %%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%% 31 | % 构建系统矩阵A，n x n 32 | A = [0 1 ; -k_sys/m_sys -b_sys/m_sys]; 33 | % 计算A矩阵维度 34 | n = size (A,1); 35 | % 构建输入矩阵B，n x p 36 | B = [0; 1/m_sys]; 37 | % 计算输入矩阵维度 38 | p = size(B,2); 39 | 40 | %%%%%%%%%%%%%%%%%%系统离散%%%%%%%%%%%%%%%%%%%% 41 | % 离散时间步长 42 | Ts = 0.1; 43 | % 连续系统转离散系统 44 | sys_d = c2d(ss(A,B),Ts); 45 | % 提取离散系统A矩阵 46 | A = sys_d.a; 47 | % 提取离散系统B矩阵 48 | B = sys_d.b; 49 | 50 | %%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%%%%%%%%% 51 | % 设计状态权重系数矩阵, n x n 52 | Q = [1 0 ; 0 1]; 53 | % 设计终值权重系数矩阵, n x n 54 | S = [1 0; 0 1]; 55 | % 设计输入权重系数矩阵, p x p 56 | R = 0.1; 57 | 58 | %%%%%%%%%%%%%%%%%系统参考值%%%%%%%%%%%%%%%%%%%% 59 | % 系统状态参考值 60 | xd = [1 ; 0]; 61 | % 构建目标转移矩阵，参考式（4.5.4） 62 | AD = eye(n); 63 | 64 | %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% 65 | % 初始化系统状态 66 | x0 = [0; 0]; 67 | % 初始化状态赋值 68 | x = x0; 69 | % 系统输入初始化 70 | u0 = 0; 71 | % 初始输入赋值 72 | u = u0; 73 | % 构建初始化增广状态矩阵，参考式（4.5.23） 74 | xa = [x; xd; u]; 75 | 76 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 77 | % 定义系统运行步数 78 | k_steps = 100; 79 | % 定义x_history零矩阵，用于储存系统状态结果，维度n x k_step 80 | x_history = zeros(n,k_steps); 81 | % 定义u_history零矩阵，用于储存系统输入结果，维度p x k_step 82 | u_history = zeros(p,k_steps); 83 | 84 | % 调用模块[F3]，计算Aa，Ba，Qa，Sa，和R 85 | [Aa,Ba,Qa,Sa,R] = F3_InputAugmentMatrix_Delta_U(A,B,Q,R,S,AD); 86 | % 调用模块[F1]，计算系统反馈增益，F 87 | [F] = F1_LQR_Gain(Aa,Ba,Qa,R,Sa); 88 | 89 | % 仿真开始，建立for循环 90 | for k = 1 : k_steps 91 | % 计算系统输入增量 92 | Delta_u = -F * xa; 93 | % 计算系统输入，参考式（4.5.20） 94 | u = Delta_u + u; 95 | % 系统输入代入系统方程，计算系统响应 96 | x = A * x + B * u; 97 | % 更新系统增广状态，参考式（4.5.23） 98 | xd = AD * xd; 99 | xa = [x; xd; u]; 100 | % 保存系统状态到预先定义矩阵的相应位置 101 | x_history (:,k+1) = x; 102 | % 保存系统输入到预先定义矩阵的相应位置 103 | u_history (:,k) = u ; 104 | end 105 | 106 | %%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%% 107 | figure(1, 'position',[150 150 500 500]); 108 | % 状态变量结果图 109 | subplot (2, 1, 1); 110 | hold; 111 | % 系统状态x1结果图，质量块位移 112 | plot (0:length(x_history)-1,x_history(1,:)); 113 | % 系统状态x2结果图，质量块速度 114 | plot (0:length(x_history)-1,x_history(2,:),'--'); 115 | grid on 116 | legend("x1","x2") 117 | hold off; 118 | xlim([0 k_steps]); 119 | ylim([-0.2 1.2]); 120 | 121 | % 系统输入结果图 122 | subplot (2, 1, 2); 123 | % 系统输入结果图，施加在质量块上的力，f 124 | stairs (0:length(u_history)-1,u_history(1,:)); 125 | legend("u") 126 | grid on 127 | xlim([0 k_steps]); 128 | ylim([0 3]); 129 | -------------------------------------------------------------------------------- /Octave/LQR_Test_tracking_E_offset_MSD.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：LQR_Test_tracking_E_offset_MSD 6 | %% 程序功能：弹簧质量阻尼系统非零参考点分析-引入控制目标误差 （4.5.2节） 7 | %% 所用模块：[F1]反馈矩阵求解模块 8 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 9 | 10 | %% 程序初始化，清空工作空间，缓存， 11 | clear all; 12 | close all; 13 | clc; 14 | % 读取Octave控制数据库（注：如使用Matlab，可删除或注释掉本行代码） 15 | pkg load control; 16 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 17 | %% 定义系统参数 18 | % 定义质量块质量 19 | m_sys = 1; 20 | % 定义阻尼系数 21 | b_sys = 0.5; 22 | % 定义弹簧弹性系数 23 | k_sys = 1; 24 | %%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%% 25 | % 构建系统矩阵A，n x n 26 | A = [0 1 ; -k_sys/m_sys -b_sys/m_sys]; 27 | % 计算A矩阵维度 28 | n = size (A,1); 29 | % 构建输入矩阵B，n x p 30 | B = [0; 1/m_sys]; 31 | % 计算输入矩阵维度 32 | p = size(B,2); 33 | 34 | %%%%%%%%%%%%%%%%%%系统离散%%%%%%%%%%%%%%%%%%%% 35 | % 离散时间步长 36 | Ts = 0.1; 37 | % 连续系统转离散系统 38 | sys_d = c2d(ss(A,B),Ts); 39 | % 提取离散系统A矩阵 40 | A = sys_d.a; 41 | % 提取离散系统B矩阵 42 | B = sys_d.b; 43 | 44 | %%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%%%%%%%%% 45 | % 设计状态权重系数矩阵, n x n 46 | Q = [1 0 ; 0 1]; 47 | % 设计终值权重系数矩阵, n x n 48 | S = [1 0; 0 1]; 49 | % 设计输入权重系数矩阵, p x p 50 | R = 1; 51 | 52 | %%%%%%%%%%%%%%%%%系统参考值%%%%%%%%%%%%%%%%%%%% 53 | % 系统状态参考值 54 | xd = [1 ; 0]; 55 | % 构建目标转移矩阵，参考式（4.5.4） 56 | AD = eye(n); 57 | 58 | %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% 59 | % 初始化系统状态 60 | x0 = [0; 0]; 61 | % 初始化状态赋值 62 | x = x0; 63 | % 构建初始化增广矩阵，参考式（4.5.5b） 64 | xa = [x; xd]; 65 | % 系统输入初始化 66 | u = 0; 67 | 68 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 69 | % 定义系统运行步数 70 | k_steps = 100; 71 | % 定义x_history零矩阵，用于储存系统状态结果，维度n x k_step 72 | x_history = zeros(n,k_steps); 73 | % 定义u_history零矩阵，用于储存系统输入结果，维度p x k_step 74 | u_history = zeros(p,k_steps); 75 | 76 | % 构建增广矩阵Ca，参考式（4.5.6b） 77 | Ca =[eye(n) -eye(n)]; 78 | % 构建增广矩阵Aa，参考式（4.5.5b） 79 | Aa = [A zeros(n);zeros(n) AD]; 80 | % 构建增广矩阵Ba，参考式（4.5.5b） 81 | Ba = [B;zeros(n,1)]; 82 | % 构建增广矩阵Sa，参考式（4.5.8b） 83 | Sa = transpose(Ca)*S*Ca; 84 | % 构建增广矩阵Qa，参考式（4.5.8b） 85 | Qa = transpose(Ca)*Q*Ca; 86 | 87 | % 读取模块[F1]，计算系统反馈增益，F 88 | [F] = F1_LQR_Gain(Aa,Ba,Qa,R,Sa); 89 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 90 | 91 | % 仿真开始，建立for循环 92 | for k = 1 : k_steps 93 | % 计算系统输入 94 | u = -F * (xa); 95 | % 系统输入代入系统方程，计算系统响应 96 | x = A * x + B * u; 97 | % 更新增广状态xa 98 | xa = [x; xd]; 99 | % 保存系统状态到预先定义矩阵的相应位置 100 | x_history (:,k+1) = x; 101 | % 保存系统输入到预先定义矩阵的相应位置 102 | u_history (:,k) = u ; 103 | end 104 | 105 | %%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%% 106 | figure(1, 'position',[150 150 500 500]); 107 | % 状态变量结果图 108 | subplot (2, 1, 1); 109 | hold; 110 | % 系统状态x1结果图，质量块位移 111 | plot (0:length(x_history)-1,x_history(1,:)); 112 | % 系统状态x2结果图，质量块速度 113 | plot (0:length(x_history)-1,x_history(2,:),'--'); 114 | grid on 115 | legend("x1","x2") 116 | hold off; 117 | xlim([0 k_steps]); 118 | ylim([-0.2 1.2]); 119 | % 系统输入结果图 120 | subplot (2, 1, 2); 121 | % 系统输入结果图，施加在质量块上的力，f 122 | stairs (0:length(u_history)-1,u_history(1,:)); 123 | legend("u") 124 | grid on 125 | xlim([0 k_steps]); 126 | ylim([0 3]); 127 | -------------------------------------------------------------------------------- /Octave/LQR_Test_tracking_SS_U_MSD.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：LQR_Test_tracking_SS_U_MSD 6 | %% 程序功能：弹簧质量阻尼系统 - 稳态非零参考值控制 （4.5.3节案例） 7 | %% 所用模块： 8 | %% [F1]反馈矩阵求解模块 9 | %% [F2]稳态非零控制矩阵转化模块 10 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 11 | 12 | %% 程序初始化，清空工作空间，缓存， 13 | clear all; 14 | close all; 15 | clc; 16 | % 读取Octave控制数据库（注：如使用Matlab，可删除或注释掉本行代码） 17 | pkg load control; 18 | % 读取Octave优化求解器数据库（注：如使用Matlab，可删除或注释掉本行代码） 19 | pkg load optim; 20 | %%%%%%%%%%%%%%%%%%%%%%%%%% 21 | 22 | %% 定义系统参数 23 | % 定义质量块质量 24 | m_sys = 1; 25 | % 定义阻尼系数 26 | b_sys = 0.5; 27 | % 定义弹簧弹性系数 28 | k_sys = 1; 29 | %%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%% 30 | % 构建系统矩阵A，n x n 31 | A = [0 1 ; -k_sys/m_sys -b_sys/m_sys]; 32 | % 计算A矩阵维度 33 | n = size (A,1); 34 | % 构建输入矩阵B，n x p 35 | B = [0; 1/m_sys]; 36 | % 计算输入矩阵维度 37 | p = size(B,2); 38 | 39 | %%%%%%%%%%%%%%%%%%系统离散%%%%%%%%%%%%%%%%%%%% 40 | % 离散时间步长 41 | Ts = 0.1; 42 | % 连续系统转离散系统 43 | sys_d = c2d(ss(A,B),Ts); 44 | % 提取离散系统A矩阵 45 | A = sys_d.a; 46 | % 提取离散系统B矩阵 47 | B = sys_d.b; 48 | 49 | %%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%%%%%%%%% 50 | % 设计状态权重系数矩阵, n x n 51 | Q = [1 0 ; 0 1]; 52 | % 设计终值权重系数矩阵, n x n 53 | S = [1 0; 0 1]; 54 | % 设计输入权重系数矩阵, p x p 55 | R = 0.1; 56 | 57 | %%%%%%%%%%%%%%%%%系统参考值%%%%%%%%%%%%%%%%%%%% 58 | % 系统状态参考值 59 | xd = [1 ; 0]; 60 | % 构建目标转移矩阵，参考式（4.5.4） 61 | AD = eye(n); 62 | 63 | %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% 64 | % 初始化系统状态 65 | x0 = [0; 0]; 66 | % 初始化状态赋值 67 | x = x0; 68 | % 构建初始化增广矩阵，参考式（4.5.5b） 69 | xa = [x; xd]; 70 | % 系统输入初始化 71 | u = 0; 72 | 73 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 74 | % 定义系统运行步数 75 | k_steps = 100; 76 | % 定义x_history零矩阵，用于储存系统状态结果，维度n x k_step 77 | x_history = zeros(n,k_steps); 78 | % 定义u_history零矩阵，用于储存系统输入结果，维度p x k_step 79 | u_history = zeros(p,k_steps); 80 | 81 | % 调用模块[F2]，计算系统增广矩阵Aa，Ba，Qa，Sa，R以及目标输入ud 82 | [Aa,Ba,Qa,Sa,R,ud] = F2_InputAugmentMatrix_SS_U(A,B,Q,R,S,xd); 83 | 84 | % 调用模块[F1]，计算系统反馈增益，F 85 | [F] = F1_LQR_Gain(Aa,Ba,Qa,R,Sa); 86 | 87 | % 仿真开始，建立for循环 88 | for k = 1 : k_steps 89 | % 计算系统输入 90 | u = -F * (xa) + ud; 91 | % 系统输入代入系统方程，计算系统响应 92 | x = A * x + B * u; 93 | % 更新增广状态，xa 94 | xa = [x; xd]; 95 | % 保存系统状态到预先定义矩阵的相应位置 96 | x_history (:,k+1) = x; 97 | % 保存系统输入到预先定义矩阵的相应位置 98 | u_history (:,k) = u ; 99 | end 100 | 101 | %%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%% 102 | figure(1, 'position',[150 150 500 500]); 103 | % 状态变量结果图 104 | subplot (2, 1, 1); 105 | hold; 106 | % 系统状态x1结果图，质量块位移 107 | plot (0:length(x_history)-1,x_history(1,:)); 108 | % 系统状态x2结果图，质量块速度 109 | plot (0:length(x_history)-1,x_history(2,:),'--'); 110 | grid on 111 | legend("x1","x2") 112 | hold off; 113 | xlim([0 k_steps]); 114 | ylim([-0.2 1.2]); 115 | 116 | % 系统输入结果图 117 | subplot (2, 1, 2); 118 | % 系统输入结果图，施加在质量块上的力，f 119 | stairs (0:length(u_history)-1,u_history(1,:)); 120 | legend("u") 121 | grid on 122 | xlim([0 k_steps]); 123 | ylim([0 3]); 124 | -------------------------------------------------------------------------------- /Octave/LQR_UAV_tracking_SS_U.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：LQR_UAV_tracking_SS_U 6 | %% 程序功能：无人机高度追踪控制 （4.6节案例） 7 | %% 所用模块： 8 | %% [F1]反馈矩阵求解模块 9 | %% [F2]稳态非零控制矩阵转化模块 10 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 11 | 12 | %% 程序初始化，清空工作空间，缓存 13 | clear all; 14 | close all; 15 | clc; 16 | % 读取Octave控制数据库（注：如使用Matlab，可删除或注释掉本行代码） 17 | pkg load control; 18 | % 读取Octave优化求解器数据库（注：如使用Matlab，可删除或注释掉本行代码） 19 | pkg load optim; 20 | %%%%%%%%%%%%%%%%%%%%%%%%%% 21 | 22 | %% 定义系统参数 23 | % 定义无人机质量 24 | m = 1; 25 | % 定义重力加速度常数 26 | g = 10; 27 | 28 | %%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%% 29 | % 构建系统矩阵A，n x n 30 | A = [0 1 0; 0 0 1 ;0 0 0]; 31 | % 计算A矩阵维度 32 | n= size (A,1); 33 | % 构建输入矩阵B，n x p 34 | B = [0; 1/m; 0]; 35 | % 计算输入矩阵维度 36 | p = size(B,2); 37 | 38 | %%%%%%%%%%%%%%%%%%系统离散%%%%%%%%%%%%%%%%%%%% 39 | % 离散时间步长 40 | Ts = 0.1; 41 | % 连续系统转离散系统 42 | sys_d = c2d(ss(A,B),Ts); 43 | % 提取离散系统A矩阵 44 | A = sys_d.a; 45 | % 提取离散系统B矩阵 46 | B = sys_d.b; 47 | 48 | %%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%%%%%%%%% 49 | % 设计状态权重系数矩阵, n x n 50 | Q=[1 0 0; 0 1 0; 0 0 0]; 51 | % 设计终值权重系数矩阵, n x n 52 | S=[1 0 0; 0 1 0; 0 0 0]; 53 | % 设计输入权重系数矩阵, p x p 54 | R=1; 55 | 56 | %%%%%%%%%%%%%%%%%系统参考值%%%%%%%%%%%%%%%%%%%% 57 | % 系统状态参考值 58 | xd = [10 ; 0 ; -10]; 59 | % 构建目标转移矩阵 60 | AD = eye(n); 61 | 62 | %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% 63 | % 初始化系统状态 64 | x0 = [0; 0; -10 ]; x = x0; 65 | % 初始化增广状态矩阵 66 | xa = [x; xd]; 67 | % 初始化系统输入 68 | u0 = 0; u = 0; 69 | 70 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 71 | % 定义系统运行步数 72 | k_steps = 100; 73 | % 定义x_history零矩阵，用于储存系统状态结果，维度n x k_step 74 | x_history = zeros(n,k_steps); 75 | % 定义u_history零矩阵，用于储存系统输入结果，维度p x k_step 76 | u_history = zeros(p,k_steps); 77 | 78 | % 调用模块[F2]，计算系统增广矩阵Aa，Ba，Qa，Sa，R以及目标输入ud 79 | [Aa,Ba,Qa,Sa,R,ud] = F2_InputAugmentMatrix_SS_U(A,B,Q,R,S,xd); 80 | 81 | % 调用模块[F1]，计算系统反馈增益，F 82 | [F] = F1_LQR_Gain(Aa,Ba,Qa,R,Sa); 83 | 84 | % 仿真开始，建立for循环 85 | for k = 1 : k_steps 86 | % 计算系统输入 87 | u = -F * (xa) + ud; 88 | % 系统输入代入系统方程，计算系统响应 89 | x = A * x + B * u; 90 | % 更新增广矩阵xa 91 | xa = [x; xd]; 92 | % 保存系统状态到预先定义矩阵的相应位置 93 | x_history (:,k+1) = x; 94 | % 保存系统输入到预先定义矩阵的相应位置 95 | u_history (:,k) = u ; 96 | end 97 | 98 | %%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%% 99 | figure(1, 'position',[150 150 500 500]); 100 | % 状态变量结果图 101 | subplot (2, 1, 1); 102 | hold; 103 | % 无人机高度 104 | plot (0:length(x_history)-1,x_history(1,:)); 105 | % 无人机速度 106 | plot (0:length(x_history)-1,x_history(2,:),'--'); 107 | grid on 108 | legend("x1","x2") 109 | hold off; 110 | xlim([0 k_steps]); 111 | ylim([0 10.2]); 112 | subplot (2, 1, 2); 113 | hold; 114 | % 系统输入 115 | stairs (0:length(u_history)-1,u_history(1,:)); 116 | legend("u") 117 | grid on 118 | xlim([0 k_steps]); 119 | ylim([0 35]); 120 | -------------------------------------------------------------------------------- /Octave/LQR_UAV_tracking_SS_U_InputConstraints.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：LQR_UAV_tracking_SS_U 6 | %% 程序功能：无人机高度追踪控制-包含饱和函数 （4.6节案例） 7 | %% 所用模块： 8 | %% [F1]反馈矩阵求解模块 9 | %% [F2]稳态非零控制矩阵转化模块 10 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 11 | 12 | %% 程序初始化，清空工作空间，缓存 13 | clear all; 14 | close all; 15 | clc; 16 | % 读取Octave控制数据库（注：如使用Matlab，可删除或注释掉本行代码） 17 | pkg load control; 18 | % 读取Octave优化求解器数据库（注：如使用Matlab，可删除或注释掉本行代码） 19 | pkg load optim; 20 | %%%%%%%%%%%%%%%%%%%%%%%%%% 21 | 22 | %% 定义系统参数 23 | % 定义无人机质量 24 | m = 1; 25 | % 定义重力加速度常数 26 | g = 10; 27 | 28 | %%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%% 29 | % 构建系统矩阵A，n x n 30 | A = [0 1 0; 0 0 1 ;0 0 0]; 31 | % 计算A矩阵维度 32 | n= size (A,1); 33 | % 构建输入矩阵B，n x p 34 | B = [0; 1/m; 0]; 35 | % 计算输入矩阵维度 36 | p = size(B,2); 37 | 38 | %%%%%%%%%%%%%%%%%%系统离散%%%%%%%%%%%%%%%%%%%% 39 | % 离散时间步长 40 | Ts = 0.1; 41 | % 连续系统转离散系统 42 | sys_d = c2d(ss(A,B),Ts); 43 | % 提取离散系统A矩阵 44 | A = sys_d.a; 45 | % 提取离散系统B矩阵 46 | B = sys_d.b; 47 | 48 | %%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%%%%%%%%% 49 | % 设计状态权重系数矩阵, n x n 50 | Q=[1 0 0; 0 1 0; 0 0 0]; 51 | % 设计终值权重系数矩阵, n x n 52 | S=[1 0 0; 0 1 0; 0 0 0]; 53 | % 设计输入权重系数矩阵, p x p 54 | R=0.1; 55 | 56 | %%%%%%%%%%%%%%%%%系统参考值%%%%%%%%%%%%%%%%%%%% 57 | % 系统状态参考值 58 | xd = [10 ; 0 ; -10]; 59 | % 构建目标转移矩阵 60 | AD = eye(n); 61 | 62 | %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% 63 | % 初始化系统状态 64 | x0 = [0; 0; -10 ]; x = x0; 65 | % 初始化增广状态矩阵 66 | xa = [x; xd]; 67 | % 初始化系统输入 68 | u0 = 0; u = 0; 69 | 70 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 71 | % 定义系统运行步数 72 | k_steps = 100; 73 | % 定义x_history零矩阵，用于储存系统状态结果，维度n x k_step 74 | x_history = zeros(n,k_steps); 75 | % 定义u_history零矩阵，用于储存系统输入结果，维度p x k_step 76 | u_history = zeros(p,k_steps); 77 | 78 | % 调用模块[F2]，计算系统增广矩阵Aa，Ba，Qa，Sa，R以及目标输入ud 79 | [Aa,Ba,Qa,Sa,R,ud] = F2_InputAugmentMatrix_SS_U(A,B,Q,R,S,xd); 80 | 81 | % 调用模块[F1]，计算系统反馈增益，F 82 | [F] = F1_LQR_Gain(Aa,Ba,Qa,R,Sa); 83 | 84 | % 系统输入限制 85 | u_max = 12; %上限 86 | u_min = 7; %下限 87 | 88 | % 仿真开始，建立for循环 89 | for k = 1 : k_steps 90 | % 计算系统输入 91 | u = -F * (xa) + ud; 92 | % 施加系统输入限制（基于饱和函数的硬限制） 93 | u = min(max(u,u_min),u_max); 94 | % 系统输入代入系统方程，计算系统响应 95 | x = A * x + B * u; 96 | % 更新增广矩阵xa 97 | xa = [x; xd]; 98 | % 保存系统状态到预先定义矩阵的相应位置 99 | x_history (:,k+1) = x; 100 | % 保存系统输入到预先定义矩阵的相应位置 101 | u_history (:,k) = u ; 102 | end 103 | 104 | %%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%% 105 | figure(1, 'position',[150 150 500 500]); 106 | % 状态变量结果图 107 | subplot (2, 1, 1); 108 | hold; 109 | % 无人机高度 110 | plot (0:length(x_history)-1,x_history(1,:)); 111 | % 无人机速度 112 | plot (0:length(x_history)-1,x_history(2,:),'--'); 113 | grid on 114 | legend("x1","x2") 115 | hold off; 116 | xlim([0 k_steps]); 117 | ylim([0 10.2]); 118 | subplot (2, 1, 2); 119 | hold; 120 | % 系统输入 121 | stairs (0:length(u_history)-1,u_history(1,:)); 122 | legend("u") 123 | grid on 124 | xlim([0 k_steps]); 125 | ylim([7 12]); 126 | -------------------------------------------------------------------------------- /Octave/Linear_Regression.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：Linear_Regression.m 6 | %% 程序功能：简单线性回归案例，解析解 （2.4节案例） 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | %% 程序初始化，清空工作空间，缓存， 9 | clear all; 10 | close all; 11 | clc; 12 | %%%%%%%%%%%%%%%%%%%%%%%%%% 13 | % 定义z向量 14 | z = [183; 175; 187; 185; 176; 176; 185; 191; 195; 185; 174; 180; 178; 170; 184]; 15 | % 定义x向量 16 | x = [75; 71; 83; 74; 73; 67; 79; 73; 88; 80; 81; 78; 73; 68; 71]; 17 | % 扩展x向量 18 | x = [ones(length(x),1) x]; 19 | % 定义y向量 20 | y = zeros(2,1); 21 | 22 | %%%%%%%%%%%%%%%%%%%%%%%%%% 23 | % 求解最优y 24 | y = inverse(transpose (x)*x)*transpose (x)*z; 25 | %%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%% 26 | figure(1, 'position',[150 150 1000 400]); 27 | % 定义横坐标 28 | x_draw = 65:0.1:90; 29 | % 散点图 30 | scatter(x(:,2),z, 80,"r"); 31 | hold on; 32 | % 线型图 33 | plot (x_draw, y(2)*x_draw+ y(1)); 34 | grid on; 35 | 36 | -------------------------------------------------------------------------------- /Octave/Linear_regression_gradient_descent.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：Linear_Regression_gradient_descent.m 6 | %% 程序功能：简单线性回归案例，梯度下降法 （2.4节案例） 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | %% 程序初始化，清空工作空间，缓存， 9 | clear all; 10 | close all; 11 | clc; 12 | %%%%%%%%%%%%%%%%%%%%%%%%%% 13 | % 定义z向量 14 | z = [183; 175; 187; 185; 176; 176; 185; 191; 195; 185; 174; 180; 178; 170; 184]; 15 | % 定义x向量 16 | x = [75; 71; 83; 74; 73; 67; 79; 73; 88; 80; 81; 78; 73; 68; 71]; 17 | % 扩展x向量 18 | x = [ones(length(x),1) x]; 19 | % 定义y向量 20 | y = zeros(2,1); 21 | 22 | %%%%%%%%%%%%%%%%%%%%%%%%%% 23 | %%% 梯度下降法%%%%%%%%%%%%%% 24 | % 定义初始值 25 | y = [120;1]; 26 | % 定义学习率 27 | alpha = [0.001 0; 0 0.00001]; 28 | % 定义停止条件阈值，用于判断系统是否到达稳态 29 | tol = 1e-4; 30 | % 初始化函数变化 31 | diff = Inf; 32 | % 定义迭代次数 33 | i = 0; 34 | % 定义最大迭代次数，用于限制程序运行时间 35 | max_iter = 100000; 36 | 37 | while diff > tol 38 | % 保存上一次f_y 39 | f_y_pre = transpose (z)*z -2*transpose (z)*x*y + transpose (y)*transpose (x)*x*y; 40 | % 更新y 41 | y = y - alpha* transpose (x) * ( - z + x * y); 42 | % 计算当前f_y 43 | f_y = transpose (z)*z -2*transpose (z)*x*y + transpose (y)*transpose (x)*x*y; 44 | % 计算两次迭代后y的变化 45 | diff = abs(f_y-f_y_pre); 46 | % 更新迭代次数 47 | i = i+1; 48 | % 如程序超过预设最大迭代步，则报错 49 | if i > max_iter 50 | error('Maximum Number of Iterations Exceeded'); 51 | end 52 | 53 | end 54 | %%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%% 55 | figure(1, 'position',[150 150 1000 400]); 56 | % 定义横坐标 57 | x_draw = 65:0.1:90; 58 | % 散点图 59 | scatter(x(:,2),z, 80,"r"); 60 | hold on; 61 | % 线型图 62 | plot (x_draw, y(2)*x_draw+ y(1)); 63 | grid on; 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /Octave/MPC_1D.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：MPC_1D 6 | %% 程序功能：模型预测控制一维示例（5.3.5节案例） 7 | %% 所用模块： 8 | %% [F4]性能指标矩阵转换模块 9 | %% [F5]无约束二次规划求解模块 10 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 11 | 12 | %% 程序初始化，清空工作空间，缓存， 13 | clear all; 14 | close all; 15 | clc; 16 | % 读取Octave控制数据库（注：如使用Matlab，可删除或注释掉本行代码） 17 | pkg load control; 18 | % 读取Octave优化数据库（注：如使用Matlab，可删除或注释掉本行代码） 19 | pkg load optim; 20 | %%%%%%%%%%%%%%%%%%%%%%%%%% 21 | % 系统定义 22 | A = 1; 23 | B = 1; 24 | % 系统状态维度计算 25 | n = size (A,1); 26 | % 27 | p = size (B,2); 28 | % 状态代价权重系数 29 | Q = 1; 30 | % 终值代价权重系数 31 | S = 1; 32 | % 系统输入代价权重系数 33 | R = 1; 34 | % 定义系统运行步数 35 | k_steps = 5; 36 | 37 | %%%%%%%%%%%%%%%%初始化系统%%%%%%%%%%%%%%%%%%% 38 | % 初始化系统状态矩阵 - 在线 39 | x_history = zeros(n,k_steps); 40 | % 初始化系统状态矩阵 - 离线 41 | x_history_2 = zeros(n,k_steps); 42 | % 初始化系统输入矩阵 - 在线 43 | u_history = zeros(p,k_steps); 44 | % 初始化系统输入矩阵 - 离线 45 | u_history_2 = zeros(p,k_steps); 46 | % 系统状态初始化，第一步 47 | x_0 = 1; 48 | % 系统初始状态赋值 - 在线 49 | x = x_0; 50 | % 系统初始状态赋值 - 离线 51 | x2 = x_0; 52 | % 系统初始状态存入状态矩阵的第一个位置 - 在线 53 | x_history (:,1) = x; 54 | % 系统初始状态存入状态矩阵的第一个位置 - 离线 55 | x_history_2(:,1) = x2; 56 | 57 | % 定义预测区间，预测区间要小于系统运行步数 58 | N_P = 5; %Predict horizon 59 | 60 | % 调用模块[F4]计算二次规划需用到的矩阵 61 | [Phi,Gamma,Omega,Psi,F,H] = F4_MPC_Matrices_PM(A,B,Q,R,S,N_P); 62 | 63 | % 调用模块[F5]利用二次规划计算无约束系统的控制 - 离线 64 | [U_offline, u_offline]= F5_MPC_Controller_noConstraints(x,F,H,p); 65 | 66 | for k = 1 : k_steps 67 | % 调用模块[F5]利用二次规划计算无约束系统的控制 - 在线 68 | [U,u] = F5_MPC_Controller_noConstraints(x,F,H,p); 69 | % 计算系统给响应 - 在线 70 | x = A * x + B * u ; 71 | if k == 2 72 | x = x +0.2; 73 | end 74 | %% 将在线、离线系统状态以及输入保存进相应的矩阵 75 | % 保存系统状态 - 在线 76 | x_history (:,k+1) = x; 77 | % 保存系统输入 - 在线 78 | u_history (:,k) = u; 79 | % 系统输入赋值 - 离线 80 | u_offline = U_offline(k); 81 | % 计算系统给响应 - 离线 82 | x2 = A * x2 + B * u_offline ; 83 | if k == 2 84 | x2 = x2 +0.2 ; 85 | end 86 | % 保存系统状态 - 离线 87 | x_history_2 (:,k+1) = x2; 88 | % 保存系统输入 - 离线 89 | u_history_2 (:,k) = u_offline; 90 | end 91 | 92 | %%%%%%%%%%%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 93 | % 系统状态结果视图 在线vs.离线 94 | subplot (2, 1, 1); 95 | hold; 96 | plot (0:length(x_history)-1,x_history(1,:)); 97 | plot (0:length(x_history_2)-1,x_history_2(1,:),'--'); 98 | grid on 99 | legend("在线 ","离线 ") 100 | hold off; 101 | xlim([0 k_steps-1]); 102 | % 系统输入结果视图 在线vs.离线 103 | subplot (2, 1, 2); 104 | hold; 105 | stairs (0:length(u_history)-1,u_history(1,:)); 106 | stairs (0:length(u_history_2)-1,u_history_2(1,:),'--'); 107 | legend("在线 ","离线 ") 108 | legend('location', 'southeast'); 109 | grid on 110 | xlim([0 k_steps-1]); 111 | 112 | 113 | -------------------------------------------------------------------------------- /Octave/MPC_2D.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：MPC_2D 6 | %% 程序功能：模型预测控制二维系统示例 7 | %% 所用模块： 8 | %% [F4]性能指标矩阵转换模块 9 | %% [F6]约束条件矩阵转换模块 10 | %% [F7]含约束二次规划求解模块 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 12 | 13 | %% 程序初始化，清空工作空间，缓存， 14 | clear all; 15 | close all; 16 | clc; 17 | % 读取Octave控制数据库（注：如使用Matlab，可删除或注释掉本行代码） 18 | pkg load control; 19 | % 读取Octave优化求解器数据库（注：如使用Matlab，可删除或注释掉本行代码） 20 | pkg load optim; 21 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 22 | 23 | %%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%% 24 | % 构建系统矩阵A，n x n 25 | A = [1 0.1; 0 -2]; 26 | % 计算A矩阵维度 27 | n = size (A,1); 28 | % 构建输入矩阵B，n x p 29 | B = [0 0.2; -0.1 0.5]; 30 | % 计算输入矩阵维度 31 | p = size (B,2); 32 | 33 | %%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%%%%%%%%% 34 | % 设计状态权重系数矩阵, n x n 35 | Q=[1 0 ; 0 1]; 36 | % 设计终值权重系数矩阵, n x n 37 | S=[1 0; 0 1]; 38 | % 设计输入权重系数矩阵, p x p 39 | R=[0.1 0; 0 0.1]; 40 | 41 | %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% 42 | % 状态初值 43 | x_0 = [1; -1]; 44 | x = x_0; 45 | 46 | %%%%%%%%%%%%%%系统约束定义%%%%%%%%%%%%%%%%%%%%% 47 | % 定义系统状态下限 48 | x_low = [-inf;-inf]; 49 | % 定义系统约束上限 50 | x_high = [inf; 0]; 51 | % 定义输入下限 52 | u_low = [-inf;-3]; 53 | % 定义输入上限 54 | u_high = [inf;inf]; 55 | 56 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 57 | % 定义系统运行步数 58 | k_steps = 10; 59 | % 定义x_history零矩阵，用于储存系统状态结果，维度n x k_steps 60 | x_history = zeros(n,k_steps); 61 | % 初始状态存入状态向量第一个位置 62 | x_history (:,1) = x; 63 | % 定义u_history零矩阵，用于储存系统输入结果，维度p x k_steps 64 | u_history = zeros(p,k_steps); 65 | 66 | 67 | % 定义预测区间，预测区间要小于系统运行步数 68 | N_P = 2; 69 | 70 | % 调用模块[F4]计算二次规划需用到的矩阵 71 | [Phi,Gamma,Omega,Psi,F,H]= F4_MPC_Matrices_PM(A,B,Q,R,S,N_P); 72 | 73 | % 调用模块[F6]计算含约束二次规划需用到的矩阵 74 | [M,Beta_bar,b]= F6_MPC_Matrices_Constraints(x_low,x_high,u_low,u_high,N_P,Phi,Gamma); 75 | 76 | % for循环，仿真开始 77 | for k = 1 : k_steps 78 | % 调用模块[F7]计算系统系统控制（输入） 79 | [U, u]= F7_MPC_Controller_withConstriants(x,F,H,M,Beta_bar,b,p); 80 | % 系统输入代入系统方程，计算系统响应 81 | x = A * x + B * u ; 82 | % 保存系统状态到预先定义矩阵的相应位置 83 | x_history (:,k+1) = x; 84 | % 保存系统输入到预先定义矩阵的相应位置 85 | u_history (:,k) = u ; 86 | end 87 | 88 | %%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%% 89 | figure(1, 'position',[150 150 500 500]); 90 | % 系统状态结果视图 91 | subplot (2, 1, 1); 92 | hold; 93 | % 系统状态x1 94 | plot (0:length(x_history)-1,x_history(1,:)); 95 | % 系统状态x2 96 | plot (0:length(x_history)-1,x_history(2,:),'--'); 97 | legend('x1', 'x2'); 98 | grid on 99 | hold off; 100 | xlim([0 k_steps-1]); 101 | ylim([-1 1]); 102 | % 系统输入结果视图 103 | subplot (2, 1, 2); 104 | hold; 105 | % 系统输入u1 106 | stairs (0:length(u_history)-1,u_history(1,:)); 107 | % 系统输入u2 108 | stairs (0:length(u_history)-1,u_history(2,:),'--'); 109 | legend('u1', 'u2'); 110 | grid on 111 | xlim([0 k_steps-1]); 112 | ylim([-4 6]); 113 | -------------------------------------------------------------------------------- /Octave/MPC_MSD_Delta_AD.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：MPC_MSD_Delta_AD 6 | %% 程序功能：弹簧质量阻尼系统模型预测控制-输入增量非常数目标 （5.4.2节案例） 7 | %% 所用模块： 8 | %% [F3]输入增量控制矩阵转换模块 9 | %% [F4]性能指标矩阵转换模块 10 | %% [F5]无约束二次规划求解模块 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 12 | 13 | %% 程序初始化，清空工作空间，缓存， 14 | clear all; 15 | close all; 16 | clc; 17 | % 读取Octave控制数据库（注：如使用Matlab，可删除或注释掉本行代码） 18 | pkg load control; 19 | % 读取Octave优化求解器数据库（注：如使用Matlab，可删除或注释掉本行代码） 20 | pkg load optim; 21 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 22 | 23 | %% 定义系统参数 24 | % 定义质量块质量 25 | m_sys = 1; 26 | % 定义阻尼系数 27 | b_sys = 0.5; 28 | % 定义弹簧弹性系数 29 | k_sys = 1; 30 | 31 | %%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%% 32 | % 构建系统矩阵A，n x n 33 | A = [0 1 ; -k_sys/m_sys -b_sys/m_sys]; 34 | % 计算A矩阵维度 35 | n = size (A,1); 36 | % 构建输入矩阵B，n x p 37 | B = [0; 1/m_sys]; 38 | % 计算输入矩阵维度 39 | p = size(B,2); 40 | 41 | %%%%%%%%%%%%%%%%%%系统离散%%%%%%%%%%%%%%%%%%%% 42 | % 离散时间步长 43 | Ts = 0.1; 44 | % 连续系统转离散系统 45 | sys_d = c2d(ss(A,B),Ts); 46 | % 提取离散系统A矩阵 47 | A = sys_d.a; 48 | % 提取离散系统B矩阵 49 | B = sys_d.b; 50 | 51 | %%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%%%%%%%%% 52 | % 设计状态权重系数矩阵, n x n 53 | Q = [1 0 ; 0 1]; 54 | % 设计终值权重系数矩阵, n x n 55 | S = [1 0; 0 1]; 56 | % 设计输入权重系数矩阵, p x p 57 | R = 0.1; 58 | 59 | %%%%%%%%%%%%%%%%%系统参考值%%%%%%%%%%%%%%%%%%%% 60 | % 系统状态参考值 61 | xd = [0 ; 0.2]; 62 | % 构建目标转移矩阵 63 | AD = c2d(ss( [0 1; 0 0],[0;0]),0.1).A; 64 | 65 | %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% 66 | % 初始化系统状态 67 | x0 = [0; 0]; 68 | % 初始化状态赋值 69 | x = x0; 70 | % 系统输入初始化 71 | u0 = 0; 72 | % 初始输入赋值 73 | u = u0; 74 | % 构建初始化增广状态矩阵，参考式（4.5.23） 75 | xa = [x; xd; u]; 76 | 77 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 78 | % 定义系统运行步数 79 | k_steps = 200; 80 | % 定义x_history零矩阵，用于储存系统状态结果，维度n x k_steps 81 | x_history = zeros(n,k_steps); 82 | % 定义u_history零矩阵，用于储存系统输入结果，维度p x k_steps 83 | u_history = zeros(p,k_steps); 84 | % 定义xd_history零矩阵，用于储存系统目标状态，维度n x k_steps 85 | xd_history = zeros(n,k_steps); 86 | 87 | % 定义预测区间，预测区间要小于系统运行步数 88 | N_P = 20; 89 | 90 | % 调用模块[F3]，计算Aa，Ba，Qa，Sa，和R 91 | [Aa,Ba,Qa,Sa,R] = F3_InputAugmentMatrix_Delta_U(A,B,Q,R,S,AD); 92 | 93 | % 调用模块[F4]计算二次规划需用到的矩阵 94 | [Phi,Gamma,Omega,Psi,F,H] = F4_MPC_Matrices_PM(Aa,Ba,Qa,R,Sa,N_P); 95 | 96 | % for循环，建立可变参考值 97 | for k = 1 : k_steps 98 | % 系统目标状态不再是常熟，if else语句定义不同阶段的系统目标状态 99 | if (k==50) 100 | xd = [xd(1) ; -0.2]; 101 | elseif (k==100) 102 | xd = [xd(1) ; 0.2]; 103 | elseif (k==150) 104 | xd = [xd(1) ; -0.2]; 105 | elseif (k==200) 106 | xd = [xd(1) ; 0.2]; 107 | end 108 | 109 | % 调用模块[F5]利用二次规划计算无约束系统的控制 （输入增量） 110 | [Delta_U, Delta_u] = F5_MPC_Controller_noConstraints(xa,F,H,p); 111 | % 计算系统输入 112 | u = Delta_u + u; 113 | % 系统输入代入系统方程，计算系统响应 114 | x = A * x + B * u; 115 | % 更新系统增广状态，参考式（4.5.23） 116 | xd = AD * xd; 117 | xa = [x; xd; u]; 118 | % 保存系统状态到预先定义矩阵的相应位置 119 | x_history (:,k+1) = x; 120 | % 保存系统输入到预先定义矩阵的相应位置 121 | u_history (:,k) = u ; 122 | % 保存系统目标状态到预先定义的矩阵的相应位置 123 | xd_history (:,k+1) = xd; 124 | end 125 | 126 | %%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%% 127 | figure(1, 'position',[150 150 500 675]); 128 | % 系统状态x1结果图，质量块位移 129 | subplot (3, 1, 1); 130 | hold; 131 | plot (0:length(x_history)-1,x_history(1,:),"linewidth",1); 132 | % 目标值 133 | plot (0:length(xd_history)-1,xd_history(1,:),'--',"linewidth",2); 134 | grid on 135 | legend("x1","x1d") 136 | hold off; 137 | xlim([0 k_steps]); 138 | ylim([-0.2 1.2]); 139 | 140 | % 系统状态x2结果图，质量块速度 141 | subplot (3, 1, 2); 142 | hold; 143 | plot (0:length(x_history)-1,x_history(2,:),"linewidth",1); 144 | % 目标值 145 | plot (0:length(xd_history)-1,xd_history(2,:),'--',"linewidth",2); 146 | grid on 147 | legend("x2","x2d") 148 | hold off; 149 | xlim([0 k_steps]); 150 | ylim([-0.5 0.5]); 151 | 152 | % 系统输入结果图 153 | subplot (3, 1, 3); 154 | stairs (0:length(u_history)-1,u_history(1,:)); 155 | legend("u") 156 | grid on 157 | xlim([0 k_steps]); 158 | ylim([-0.5 1.4]); 159 | -------------------------------------------------------------------------------- /Octave/MPC_MSD_Delta_U.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：MPC_MSD_Delta_U 6 | %% 程序功能：弹簧质量阻尼系统模型预测控制-输入增量 （5.4.2节案例） 7 | %% 所用模块： 8 | %% [F3]输入增量控制矩阵转换模块 9 | %% [F4]性能指标矩阵转换模块 10 | %% [F5]无约束二次规划求解模块 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 12 | 13 | %% 程序初始化，清空工作空间，缓存， 14 | clear all; 15 | close all; 16 | clc; 17 | 18 | % 读取Octave控制数据库（注：如使用Matlab，可删除或注释掉本行代码） 19 | pkg load control; 20 | % 读取Octave优化求解器数据库（注：如使用Matlab，可删除或注释掉本行代码） 21 | pkg load optim; 22 | %%%%%%%%%%%%%%%%%%%%%%%%%% 23 | 24 | %% 定义系统参数 25 | % 定义质量块质量 26 | m_sys = 1; 27 | % 定义阻尼系数 28 | b_sys = 0.5; 29 | % 定义弹簧弹性系数 30 | k_sys = 1; 31 | 32 | %%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%% 33 | % 构建系统矩阵A，n x n 34 | A = [0 1 ; -k_sys/m_sys -b_sys/m_sys]; 35 | % 计算A矩阵维度 36 | n = size (A,1); 37 | % 构建输入矩阵B，n x p 38 | B = [0; 1/m_sys]; 39 | % 计算输入矩阵维度 40 | p = size(B,2); 41 | 42 | %%%%%%%%%%%%%%%%%%系统离散%%%%%%%%%%%%%%%%%%%% 43 | % 离散时间步长 44 | Ts = 0.1; 45 | % 连续系统转离散系统 46 | sys_d = c2d(ss(A,B),Ts); 47 | % 提取离散系统A矩阵 48 | A = sys_d.a; 49 | % 提取离散系统B矩阵 50 | B = sys_d.b; 51 | 52 | %%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%%%%%%%%% 53 | % 设计状态权重系数矩阵, n x n 54 | Q = [1 0 ; 0 1]; 55 | % 设计终值权重系数矩阵, n x n 56 | S = [1 0; 0 1]; 57 | % 设计输入权重系数矩阵, p x p 58 | R = 0.1; 59 | 60 | %%%%%%%%%%%%%%%%%系统参考值%%%%%%%%%%%%%%%%%%%% 61 | % 系统状态参考值 62 | xd = [1 ; 0]; 63 | % 构建目标转移矩阵，参考式（4.5.4） 64 | AD = eye(n); 65 | 66 | %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% 67 | % 初始化系统状态 68 | x0 = [0; 0]; 69 | % 初始化状态赋值 70 | x = x0; 71 | % 系统输入初始化 72 | u0 = 0; 73 | % 初始输入赋值 74 | u = u0; 75 | % 构建初始化增广状态矩阵，参考式（4.5.23） 76 | xa = [x; xd; u]; 77 | 78 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 79 | % 定义系统运行步数 80 | k_steps = 100; 81 | % 定义x_history零矩阵，用于储存系统状态结果，维度n x k_step 82 | x_history = zeros(n,k_steps); 83 | % 定义u_history零矩阵，用于储存系统输入结果，维度p x k_step 84 | u_history = zeros(p,k_steps); 85 | 86 | % 定义预测区间，预测区间要小于系统运行步数 87 | N_P = 20; 88 | 89 | % 调用模块[F3]，计算Aa，Ba，Qa，Sa，和R 90 | [Aa,Ba,Qa,Sa,R] = F3_InputAugmentMatrix_Delta_U(A,B,Q,R,S,AD); 91 | 92 | % 调用模块[F4]计算二次规划需用到的矩阵 93 | [Phi,Gamma,Omega,Psi,F,H]=F4_MPC_Matrices_PM(Aa,Ba,Qa,R,Sa,N_P); 94 | 95 | % for循环 96 | for k = 1 : k_steps 97 | % 调用模块[F5]利用二次规划计算无约束系统的控制 98 | [Delta_U, Delta_u] = F5_MPC_Controller_noConstraints(xa,F,H,p) 99 | % 结合输入增量计算系统输入 100 | u = Delta_u + u; 101 | % 系统输入代入系统方程，计算系统响应 102 | x = A * x + B * u; 103 | % 更新系统增广状态 104 | xa = [x; xd; u]; 105 | % 保存系统状态到预先定义矩阵的相应位置 106 | x_history (:,k+1) = x; 107 | % 保存系统输入到预先定义矩阵的相应位置 108 | u_history (:,k) = u ; 109 | end 110 | 111 | %%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%% 112 | figure(1, 'position',[150 150 500 500]); 113 | % 状态变量结果图 114 | subplot (2, 1, 1); 115 | hold; 116 | % 系统状态x1结果图，质量块位移 117 | plot (0:length(x_history)-1,x_history(1,:)); 118 | % 系统状态x2结果图，质量块速度 119 | plot (0:length(x_history)-1,x_history(2,:),'--'); 120 | grid on 121 | legend("x1","x2") 122 | hold off; 123 | xlim([0 k_steps]); 124 | ylim([-0.2 1.2]); 125 | 126 | % 系统输入结果图 127 | subplot (2, 1, 2); 128 | % 系统输入结果图，施加在质量块上的力，f 129 | stairs (0:length(u_history)-1,u_history(1,:)); 130 | legend("u") 131 | grid on 132 | xlim([0 k_steps]); 133 | ylim([0 3]); 134 | 135 | -------------------------------------------------------------------------------- /Octave/MPC_MSD_SS_U.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：MPC_MSD_SS_U 6 | %% 程序功能：弹簧质量阻尼系统模型预测控制-稳态输入 （5.4.1节案例） 7 | %% 所用模块： 8 | %% [F2]稳态非零控制矩阵转化模块 9 | %% [F4]性能指标矩阵转换模块 10 | %% [F5]无约束二次规划求解模块 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 12 | 13 | %% 程序初始化，清空工作空间，缓存， 14 | clear all; 15 | close all; 16 | clc; 17 | % 读取Octave控制数据库（注：如使用Matlab，可删除或注释掉本行代码） 18 | pkg load control; 19 | % 读取Octave优化求解器数据库（注：如使用Matlab，可删除或注释掉本行代码） 20 | pkg load optim; 21 | %%%%%%%%%%%%%%%%%%%%%%%%%% 22 | 23 | %% 定义系统参数 24 | % 定义质量块质量 25 | m_sys = 1; 26 | % 定义阻尼系数 27 | b_sys = 0.5; 28 | % 定义弹簧弹性系数 29 | k_sys = 1; 30 | 31 | %%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%% 32 | % 构建系统矩阵A，n x n 33 | A = [0 1 ; -k_sys/m_sys -b_sys/m_sys]; 34 | % 计算A矩阵维度 35 | n = size (A,1); 36 | % 构建输入矩阵B，n x p 37 | B = [0; 1/m_sys]; 38 | % 计算输入矩阵维度 39 | p = size(B,2); 40 | 41 | %%%%%%%%%%%%%%%%%%系统离散%%%%%%%%%%%%%%%%%%%% 42 | % 离散时间步长 43 | Ts = 0.1; 44 | % 连续系统转离散系统 45 | sys_d = c2d(ss(A,B),Ts); 46 | % 提取离散系统A矩阵 47 | A = sys_d.a; 48 | % 提取离散系统B矩阵 49 | B = sys_d.b; 50 | 51 | %%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%%%%%%%%% 52 | % 设计状态权重系数矩阵, n x n 53 | Q = [1 0 ; 0 1]; 54 | % 设计终值权重系数矩阵, n x n 55 | S = [1 0; 0 1]; 56 | % 设计输入权重系数矩阵, p x p 57 | R = 1; 58 | 59 | %%%%%%%%%%%%%%%%%系统参考值%%%%%%%%%%%%%%%%%%%% 60 | % 系统状态参考值 61 | xd = [1 ; 0]; 62 | % 构建目标转移矩阵，参考式（4.5.4） 63 | AD = eye(n); 64 | ud = mldivide(B,(eye(n)-A)*xd); 65 | 66 | %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% 67 | % 初始化系统状态 68 | x0 = [0; 0]; 69 | % 初始化状态赋值 70 | x = x0; 71 | % 构建初始化增广矩阵，参考式（4.5.5b） 72 | xa = [x; xd]; 73 | % 系统输入初始化 74 | u = 0; 75 | 76 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 77 | % 定义系统运行步数 78 | k_steps = 100; 79 | % 定义x_history零矩阵，用于储存系统状态结果，维度n x k_step 80 | x_history = zeros(n,k_steps); 81 | % 定义u_history零矩阵，用于储存系统输入结果，维度p x k_step 82 | u_history = zeros(p,k_steps); 83 | delta_u = 0; 84 | 85 | % 定义预测区间，预测区间要小于系统运行步数 86 | N_P = 20; 87 | 88 | % 调用模块[F2]，计算增广矩阵以及ud 89 | [Aa,Ba,Qa,Sa,R,ud] = F2_InputAugmentMatrix_SS_U(A,B,Q,R,S,xd); 90 | 91 | % 调用模块[F4]计算二次规划需用到的矩阵 92 | [Phi,Gamma,Omega,Psi,F,H]=F4_MPC_Matrices_PM(Aa,Ba,Qa,R,Sa,N_P); 93 | 94 | for k = 1 : k_steps 95 | % 调用模块[F5]利用二次规划计算无约束系统的控制 96 | [delta_U,delta_u]= F5_MPC_Controller_noConstraints(xa,F,H,p); 97 | % 计算系统给响应 98 | x = A * x + B * (delta_u + ud); 99 | % 构建增广状态xa 100 | xa = [x; xd]; 101 | % 保存系统状态 102 | x_history (:,k+1) = x; 103 | % 保存系统输入 104 | u_history (:,k) = delta_u + ud; 105 | end 106 | 107 | %%%%%%%%%%%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 108 | 109 | figure(1, 'position',[150 150 500 500]); 110 | % 状态变量结果图 111 | subplot (2, 1, 1); 112 | hold; 113 | % 系统状态x1结果图，质量块位移 114 | plot (0:length(x_history)-1,x_history(1,:)); 115 | % 系统状态x2结果图，质量块速度 116 | plot (0:length(x_history)-1,x_history(2,:),'--'); 117 | grid on 118 | legend("x1","x2") 119 | hold off; 120 | xlim([0 k_steps]); 121 | ylim([-0.2 1.2]); 122 | 123 | % 系统输入结果图 124 | subplot (2, 1, 2); 125 | % 系统输入结果图，施加在质量块上的力，f 126 | stairs (0:length(u_history)-1,u_history(1,:)); 127 | legend("u") 128 | grid on 129 | xlim([0 k_steps]); 130 | ylim([0 3]); 131 | -------------------------------------------------------------------------------- /Octave/MPC_MSD_SS_U_withConstraints.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：MPC_MSD_SS_U_withConstraints 6 | %% 程序功能：弹簧质量阻尼系统模型预测控制示例-用约束限制超调量 7 | %% 所用模块： 8 | %% [F2]稳态非零控制矩阵转化模块 9 | %% [F4]性能指标矩阵转换模块 10 | %% [F5]无约束二次规划求解模块 11 | %% [F6]约束条件矩阵转换模块 12 | %% [F7]含约束二次规划求解模块 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | 15 | %% 程序初始化，清空工作空间，缓存， 16 | clear all; 17 | close all; 18 | clc; 19 | % 读取Octave控制数据库（注：如使用Matlab，可删除或注释掉本行代码） 20 | pkg load control; 21 | % 读取Octave优化求解器数据库（注：如使用Matlab，可删除或注释掉本行代码） 22 | pkg load optim; 23 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 24 | 25 | %% 定义系统参数 26 | % 定义质量块质量 27 | m_sys = 1; 28 | % 定义阻尼系数 29 | b_sys = 0.5; 30 | % 定义弹簧弹性系数 31 | k_sys = 1; 32 | 33 | %%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%% 34 | % 构建系统矩阵A，n x n 35 | A = [0 1 ; -k_sys/m_sys -b_sys/m_sys]; 36 | % 计算A矩阵维度 37 | n = size (A,1); 38 | % 构建输入矩阵B，n x p 39 | B = [0; 1/m_sys]; 40 | % 计算输入矩阵维度 41 | p = size(B,2); 42 | 43 | %%%%%%%%%%%%%%%%%%系统离散%%%%%%%%%%%%%%%%%%%% 44 | % 离散时间步长 45 | Ts = 0.1; 46 | % 连续系统转离散系统 47 | sys_d = c2d(ss(A,B),Ts); 48 | % 提取离散系统A矩阵 49 | A = sys_d.a; 50 | % 提取离散系统B矩阵 51 | B = sys_d.b; 52 | 53 | %%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%%%%%%%%% 54 | % 设计状态权重系数矩阵, n x n 55 | Q = [1 0 ; 0 1]; 56 | % 设计终值权重系数矩阵, n x n 57 | S = [1 0; 0 1]; 58 | % 设计输入权重系数矩阵, p x p 59 | R = 1; 60 | 61 | %%%%%%%%%%%%%%%%%系统参考值%%%%%%%%%%%%%%%%%%%% 62 | % 系统状态参考值 63 | xd = [1 ; 0]; 64 | % 构建目标转移矩阵 65 | AD = eye(n); 66 | % 计算目标输入 67 | ud = mldivide(B,(eye(n)-A)*xd); 68 | 69 | %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% 70 | % 初始化系统状态 71 | x0 = [0; 0]; x = x0; 72 | % 初始化增广状态矩阵 73 | xa = [x; xd]; 74 | % 初始化系统输入 75 | u0 = 0; u = 0; 76 | 77 | %%%%%%%%%%%%%%系统约束定义%%%%%%%%%%%%%%%%%%%%% 78 | % 输入下限 79 | u_low = -inf; 80 | % 输入上限 81 | u_high = inf; 82 | % 状态下限 83 | x_low = [0;-inf]; 84 | % 状态上限 85 | x_high = [1;inf]; 86 | % 增广状态下限 87 | xa_low = [x_low;-inf;-inf]; 88 | % 增广状态上限 89 | xa_high = [x_high;inf;inf]; 90 | 91 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 92 | % 定义系统运行步数 93 | k_steps = 100; 94 | % 定义x_history零矩阵，用于储存系统状态结果，维度n x k_step 95 | x_history = zeros(n,k_steps); 96 | % 定义u_history零矩阵，用于储存系统输入结果，维度p x k_step 97 | u_history = zeros(p,k_steps); 98 | % 定义x_history_noconstraint零矩阵，用于储存系统状态结果，维度n x k_step 99 | x_history_noconstraint = zeros(n,k_steps); 100 | % 定义u_history_noconstraint零矩阵，用于储存系统输入结果，维度p x k_step 101 | u_history_noconstraint = zeros(p,k_steps); 102 | % 定义预测区间 103 | N_P = 20; 104 | 105 | % 调用模块[F2]，计算系统增广矩阵Aa，Ba，Qa，Sa，R以及目标输入ud 106 | [Aa,Ba,Qa,Sa,R,ud] = F2_InputAugmentMatrix_SS_U(A,B,Q,R,S,xd); 107 | 108 | % 调用模块[F4]计算二次规划需用到的矩阵 109 | [Phi,Gamma,Omega,Psi,F,H]= F4_MPC_Matrices_PM(Aa,Ba,Qa,R,Sa,N_P); 110 | 111 | % 调用模块[F6]计算含约束二次规划需用到的矩阵 112 | [M,Beta_bar,b]= F6_MPC_Matrices_Constraints(xa_low,xa_high,u_low,u_high,N_P,Phi,Gamma); 113 | 114 | % for循环开始仿真 有约束 115 | for k = 1 : k_steps 116 | % 调用模块[F7]计算系统系统控制（输入增量） 117 | [delta_U, delta_u]= F7_MPC_Controller_withConstriants(xa,F,H,M,Beta_bar,b,p); 118 | % 根据输入增量计算系统输入 119 | u = delta_u+ud; 120 | % 系统输入代入系统方程，计算系统响应 121 | x = A * x + B * u; 122 | % 更新增广矩阵xa 123 | xa = [x; xd]; 124 | % 保存系统状态到预先定义矩阵的相应位置 125 | x_history (:,k+1) = x; 126 | % 保存系统输入到预先定义矩阵的相应位置 127 | u_history (:,k) = u ; 128 | end 129 | 130 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 131 | %%%%计算无约束情况作对比%%%%%%%%%%%%%%%%%%%%%%%% 132 | %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% 133 | % 初始化系统状态 134 | x0 = [0; 0]; x = x0; 135 | % 初始化增广状态矩阵 136 | xa = [x; xd]; 137 | % 初始化系统输入 138 | u0 = 0; u = 0; 139 | 140 | % for循环开始仿真 无约束 141 | for k = 1 : k_steps 142 | % 调用模块[F5]利用二次规划计算无约束系统的控制 143 | [delta_U,delta_u]= F5_MPC_Controller_noConstraints(xa,F,H,p); 144 | % 计算系统给响应 145 | x = A * x + B * (delta_u + ud); 146 | % 构建增广状态xa 147 | xa = [x; xd]; 148 | % 保存系统状态 149 | x_history_noconstraint (:,k+1) = x; 150 | % 保存系统输入 151 | u_history_noconstraint (:,k) = delta_u + ud; 152 | end 153 | 154 | %%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%% 155 | 156 | figure(1, 'position',[150 150 1150 500]); 157 | % 状态变量结果图 158 | subplot (2, 2, 1); 159 | hold; 160 | % 系统状态x1结果图，质量块位移 161 | plot (0:length(x_history)-1,x_history(1,:)); 162 | % 系统状态x2结果图，质量块速度 163 | plot (0:length(x_history)-1,x_history(2,:),'--'); 164 | grid on 165 | legend("x1","x2") 166 | hold off; 167 | xlim([0 k_steps]); 168 | ylim([-0.2 1.2]); 169 | 170 | % 系统输入结果图 171 | subplot (2, 2, 3); 172 | % 系统输入结果图，施加在质量块上的力，f 173 | stairs (0:length(u_history)-1,u_history(1,:)); 174 | legend("u") 175 | grid on 176 | xlim([0 k_steps]); 177 | ylim([0 3]); 178 | 179 | % 状态X1对比图 180 | subplot (2, 2, 2); 181 | hold; 182 | % 系统状态x1结果图，质量块位移，有约束 183 | plot (0:length(x_history)-1,x_history(1,:)); 184 | % 系统状态x1结果图，质量块速度，无约束 185 | plot (0:length(x_history_noconstraint)-1,x_history_noconstraint(1,:),'--'); 186 | grid on 187 | legend("有约束","无约束") 188 | hold off; 189 | xlim([0 k_steps]); 190 | ylim([-0.2 1.2]); 191 | 192 | % 系统输入对比图 193 | subplot (2, 2, 4); 194 | hold; 195 | % 系统输入结果图，施加在质量块上的力，f，有约束 196 | stairs (0:length(u_history)-1,u_history(1,:)); 197 | % 系统输入结果图，施加在质量块上的力，f，无约束 198 | stairs (0:length(u_history_noconstraint)-1,u_history_noconstraint(1,:),'--'); 199 | legend("有约束","无约束") 200 | grid on 201 | xlim([0 k_steps]); 202 | ylim([0 3]); 203 | 204 | 205 | -------------------------------------------------------------------------------- /Octave/MPC_UAV _ST_Analysis.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：MPC_UAV_ST_Analysis.m 6 | %% 程序功能：无人机高度速度模型预测控制 （采样时间、预测区间分析，5.6.2节案例） 7 | %% 所用模块： 8 | %% [F2]稳态非零控制矩阵转化模块 9 | %% [F4]性能指标矩阵转换模块 10 | %% [F6]约束条件矩阵转换模块 11 | %% [F7]含约束二次规划求解模块 12 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 13 | 14 | %% 程序初始化，清空工作空间，缓存， 15 | clear all; 16 | close all; 17 | clc; 18 | % 读取Octave控制数据库（注：如使用Matlab，可删除或注释掉本行代码） 19 | pkg load control; 20 | % 读取Octave优化求解器数据库（注：如使用Matlab，可删除或注释掉本行代码） 21 | pkg load optim; 22 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 23 | 24 | %% 定义系统参数 25 | % 定义无人机质量 26 | m = 1; 27 | % 定义重力加速度常数 28 | g = 10; 29 | 30 | %%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%% 31 | % 构建系统矩阵A，n x n 32 | A = [0 1 0; 0 0 1 ;0 0 0]; 33 | % 计算A矩阵维度 34 | n= size (A,1); 35 | % 构建输入矩阵B，n x p 36 | B = [0; 1/m; 0]; 37 | % 计算输入矩阵维度 38 | p = size(B,2); 39 | 40 | %%%%%%%%%%%%%%%%%%系统离散%%%%%%%%%%%%%%%%%%%% 41 | % 离散时间步长（采样时间影响计算速度） 42 | Ts = 0.1; 43 | % 连续系统转离散系统 44 | sys_d = c2d(ss(A,B),Ts); 45 | % 提取离散系统A矩阵 46 | A = sys_d.a; 47 | % 提取离散系统B矩阵 48 | B = sys_d.b; 49 | 50 | %%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%%%%%%%%% 51 | % 设计状态权重系数矩阵, n x n 52 | Q=[1 0 0; 0 1 0; 0 0 0]; 53 | % 设计终值权重系数矩阵, n x n 54 | S=[1 0 0; 0 1 0; 0 0 0]; 55 | % 设计输入权重系数矩阵, p x p 56 | R=0.1; 57 | 58 | %%%%%%%%%%%%%%%%%系统参考值%%%%%%%%%%%%%%%%%%%% 59 | % 系统状态参考值 60 | xd = [10 ; 0 ; -g]; 61 | % 构建目标转移矩阵 62 | AD = eye(n); 63 | % 计算目标输入 64 | ud = mldivide(B,(eye(n)-A)*xd); 65 | 66 | %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% 67 | % 初始化系统状态 68 | x0 = [0; 0; -g ]; x = x0; 69 | % 初始化增广状态矩阵 70 | xa = [x; xd]; 71 | % 初始化系统输入 72 | u0 = 0; u = 0; 73 | 74 | %%%%%%%%%%%%%%系统约束定义%%%%%%%%%%%%%%%%%%%%% 75 | % 输入下限 76 | u_low = -3; 77 | % 输入上限 78 | u_high = 2; 79 | % 状态下限 80 | x_low = [0;0;-10]; 81 | % 状态上限 82 | x_high = [10;3;-10]; 83 | % 增广状态下限 84 | xa_low = [x_low;-inf;-inf;-inf]; 85 | % 增广状态上限 86 | xa_high = [x_high;inf;inf;inf]; 87 | 88 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 89 | % 定义系统运行步数（计算步数需要根据采样时间调整，这里由于采样时间较大，不用运行100步） 90 | k_steps = 100; 91 | % 定义x_history零矩阵，用于储存系统状态结果，维度n x k_step 92 | x_history = zeros(n,k_steps); 93 | % 定义u_history零矩阵，用于储存系统输入结果，维度p x k_step 94 | u_history = zeros(p,k_steps); 95 | 96 | % 定义预测区间，请根据采样时间调整 97 | N_P = 100; 98 | 99 | % 定义程序运行时间向量，用于存储计算时间，考量实时控制可行性 100 | elapsed_time_history = zeros(1,k_steps); 101 | 102 | % 调用模块[F2]，计算系统增广矩阵Aa，Ba，Qa，Sa，R以及目标输入ud 103 | [Aa,Ba,Qa,Sa,R,ud] = F2_InputAugmentMatrix_SS_U(A,B,Q,R,S,xd); 104 | 105 | % 调用模块[F4]计算二次规划需用到的矩阵 106 | [Phi,Gamma,Omega,Psi,F,H]= F4_MPC_Matrices_PM(Aa,Ba,Qa,R,Sa,N_P); 107 | 108 | % 调用模块[F6]计算含约束二次规划需用到的矩阵 109 | [M,Beta_bar,b]= F6_MPC_Matrices_Constraints(xa_low,xa_high,u_low,u_high,N_P,Phi,Gamma); 110 | 111 | % for循环开始仿真 112 | for k = 1 : k_steps 113 | % 开始计时 114 | tic; 115 | % 调用模块[F7]计算系统系统控制（输入增量） 116 | [delta_U, delta_u]= F7_MPC_Controller_withConstriants(xa,F,H,M,Beta_bar,b,p); 117 | % 根据输入增量计算系统输入 118 | u = delta_u+ud; 119 | % 系统输入代入系统方程，计算系统响应 120 | x = A * x + B * u; 121 | % 更新增广矩阵xa 122 | xa = [x; xd]; 123 | % 保存系统状态到预先定义矩阵的相应位置 124 | x_history (:,k+1) = x; 125 | % 保存系统输入到预先定义矩阵的相应位置 126 | u_history (:,k) = u ; 127 | % 记录运行时间 128 | elapsed_time= toc; 129 | % 运行时间存入向量相应位置 130 | elapsed_time_history (:,k) = elapsed_time ; 131 | end 132 | 133 | %%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%% 134 | figure(1, 'position',[150 50 500 800]); 135 | % 状态变量结果图 136 | subplot (3, 1, 1); 137 | hold; 138 | % 无人机高度 139 | plot (0:length(x_history)-1,x_history(1,:)); 140 | % 无人机速度 141 | plot (0:length(x_history)-1,x_history(2,:),'--'); 142 | grid on 143 | legend("x1","x2") 144 | hold off; 145 | xlim([0 k_steps]); 146 | ylim([-2 11]); 147 | % 系统输入 148 | subplot (3, 1, 2); 149 | hold; 150 | stairs (0:length(u_history)-1,u_history(1,:)); 151 | legend("u") 152 | grid on 153 | xlim([0 k_steps]); 154 | ylim([6 12]); 155 | % 系统每个迭代步计算时间 156 | subplot (3, 1, 3); 157 | hold; 158 | plot (0:length(elapsed_time_history)-1,elapsed_time_history(1,:),"linewidth", 2); 159 | grid on 160 | xlim([0 k_steps]); 161 | 162 | 163 | 164 | 165 | -------------------------------------------------------------------------------- /Octave/MPC_UAV.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：MPC_UAV 6 | %% 程序功能：无人机高度速度模型预测控制（5.6.1节案例） 7 | %% 所用模块： 8 | %% [F2]稳态非零控制矩阵转化模块 9 | %% [F4]性能指标矩阵转换模块 10 | %% [F6]约束条件矩阵转换模块 11 | %% [F7]含约束二次规划求解模块 12 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 13 | 14 | %% 程序初始化，清空工作空间，缓存， 15 | clear all; 16 | close all; 17 | clc; 18 | % 读取Octave控制数据库（注：如使用Matlab，可删除或注释掉本行代码） 19 | pkg load control; 20 | % 读取Octave优化求解器数据库（注：如使用Matlab，可删除或注释掉本行代码） 21 | pkg load optim; 22 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 23 | 24 | %% 定义系统参数 25 | % 定义无人机质量 26 | m = 1; 27 | % 定义重力加速度常数 28 | g = 10; 29 | 30 | %%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%% 31 | % 构建系统矩阵A，n x n 32 | A = [0 1 0; 0 0 1 ;0 0 0]; 33 | % 计算A矩阵维度 34 | n= size (A,1); 35 | % 构建输入矩阵B，n x p 36 | B = [0; 1/m; 0]; 37 | % 计算输入矩阵维度 38 | p = size(B,2); 39 | 40 | %%%%%%%%%%%%%%%%%%系统离散%%%%%%%%%%%%%%%%%%%% 41 | % 离散时间步长 42 | Ts = 0.1; 43 | % 连续系统转离散系统 44 | sys_d = c2d(ss(A,B),Ts); 45 | % 提取离散系统A矩阵 46 | A = sys_d.a; 47 | % 提取离散系统B矩阵 48 | B = sys_d.b; 49 | 50 | %%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%%%%%%%%% 51 | % 设计状态权重系数矩阵, n x n 52 | Q=[1 0 0; 0 1 0; 0 0 0]; 53 | % 设计终值权重系数矩阵, n x n 54 | S=[1 0 0; 0 1 0; 0 0 0]; 55 | % 设计输入权重系数矩阵, p x p 56 | R= 0.1; 57 | 58 | %%%%%%%%%%%%%%%%%系统参考值%%%%%%%%%%%%%%%%%%%% 59 | % 系统状态参考值 60 | xd = [10 ; 0 ; -g]; 61 | % 构建目标转移矩阵 62 | AD = eye(n); 63 | % 计算目标输入 64 | ud = mldivide(B,(eye(n)-A)*xd); 65 | 66 | %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% 67 | % 初始化系统状态 68 | x0 = [0; 0; -g ]; x = x0; 69 | % 初始化增广状态矩阵 70 | xa = [x; xd]; 71 | % 初始化系统输入 72 | u0 = 0; u = u0; 73 | 74 | %%%%%%%%%%%%%%系统约束定义%%%%%%%%%%%%%%%%%%%%% 75 | % 输入下限 76 | u_low = -3; 77 | % 输入上限 78 | u_high = 2; 79 | % 状态下限 80 | x_low = [0;0;-g]; 81 | % 状态上限 82 | x_high = [10;3;-g]; 83 | % 增广状态下限 84 | xa_low = [x_low;-inf;-inf;-inf]; 85 | % 增广状态上限 86 | xa_high = [x_high;inf;inf;inf]; 87 | 88 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 89 | % 定义系统运行步数 90 | k_steps = 100; 91 | % 定义x_history零矩阵，用于储存系统状态结果，维度n x k_step 92 | x_history = zeros(n,k_steps); 93 | % 定义u_history零矩阵，用于储存系统输入结果，维度p x k_step 94 | u_history = zeros(p,k_steps); 95 | 96 | % 定义预测区间 97 | N_P = 20; 98 | 99 | % 调用模块[F2]，计算系统增广矩阵Aa，Ba，Qa，Sa，R以及目标输入ud 100 | [Aa,Ba,Qa,Sa,R,ud] = F2_InputAugmentMatrix_SS_U(A,B,Q,R,S,xd); 101 | 102 | % 调用模块[F4]计算二次规划需用到的矩阵 103 | [Phi,Gamma,Omega,Psi,F,H]= F4_MPC_Matrices_PM(Aa,Ba,Qa,R,Sa,N_P); 104 | 105 | % 调用模块[F6]计算含约束二次规划需用到的矩阵 106 | [M,Beta_bar,b]= F6_MPC_Matrices_Constraints(xa_low,xa_high,u_low,u_high,N_P,Phi,Gamma); 107 | 108 | % for循环开始仿真 109 | for k = 1 : k_steps 110 | % 调用模块[F7]计算系统系统控制（输入增量） 111 | [delta_U, delta_u]= F7_MPC_Controller_withConstriants(xa,F,H,M,Beta_bar,b,p); 112 | % 根据输入增量计算系统输入 113 | u = delta_u+ud; 114 | % 系统输入代入系统方程，计算系统响应 115 | x = A * x + B * u; 116 | % 更新增广矩阵xa 117 | xa = [x; xd]; 118 | % 保存系统状态到预先定义矩阵的相应位置 119 | x_history (:,k+1) = x; 120 | % 保存系统输入到预先定义矩阵的相应位置 121 | u_history (:,k) = u ; 122 | end 123 | 124 | %%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%% 125 | figure(1, 'position',[150 150 500 500]); 126 | % 状态变量结果图 127 | subplot (2, 1, 1); 128 | hold; 129 | % 无人机高度 130 | plot (0:length(x_history)-1,x_history(1,:)); 131 | % 无人机速度 132 | plot (0:length(x_history)-1,x_history(2,:),'--'); 133 | grid on 134 | legend("x1","x2") 135 | hold off; 136 | xlim([0 k_steps]); 137 | ylim([0 10.2]); 138 | % 系统输入 139 | subplot (2, 1, 2); 140 | hold; 141 | stairs (0:length(u_history)-1,u_history(1,:)); 142 | legend("u") 143 | grid on 144 | xlim([0 k_steps]); 145 | ylim([7 12]); 146 | 147 | -------------------------------------------------------------------------------- /Octave/NoiseData.csv: -------------------------------------------------------------------------------- 1 | W,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, 2 | 0.16777,-0.23863,-0.30754,-0.16607,-0.098293,-0.10772,0.21584,0.25693,0.0034506,0.2365,-0.19913,0.17759,-0.37375,0.09489,-0.045805,-0.19073,0.061853,-0.21437,0.02735,0.030791,0.23099,-0.058602,-0.16159,-0.14029,-0.04496,0.19538,0.24663,0.2676,0.28402,0.42296,0.050613,0.036664,0.12478,0.3166,-0.03748,0.49567,0.047892,0.20908,-0.43451,-0.094871,0.17524,0.2393,-0.19013,-0.19752,0.085521,-0.24881,-0.26308,0.28483,-0.095117,2.20E-01,-0.1192,-0.00923,-0.12522,-0.042058,0.0085322,-0.025384,-0.24313,-0.025312,-0.2733,-0.036242,-0.22746,0.25653,0.11745,0.08376,-0.022888,0.020046,-0.18759,0.17802,0.16367,0.027173,0.17639,-0.087117,-0.070067,0.19823,0.17591,-0.21935,0.35102,-0.17019,-0.040652,0.088605,0.43391,-0.34293,-0.14951,0.32249,0.49867,0.030386,-0.45321,-0.13402,-0.15158,-0.049666,0.0104,-0.33852,-0.37582,0.16483,0.31131,0.10924,0.089983,0.20679,-0.44931,0.21389 3 | -0.11137,0.18288,0.282,0.45686,0.012511,0.082863,0.045152,-0.4198,0.1489,-0.10782,-0.16333,0.19509,-0.080096,0.49343,-0.33671,-0.11597,-0.065428,-0.18417,-0.66045,0.16749,-0.015864,-0.2158,0.091402,-0.24196,0.22902,-0.095408,0.021871,-0.52084,0.39901,0.077507,-0.14835,0.041585,0.15046,-0.031936,0.2809,-0.21423,-0.070098,0.11142,-0.17589,0.010092,-0.11278,-0.057325,-0.078722,-0.0008646,0.2805,0.070395,0.01105,0.1452,0.058264,-0.010069,-0.16314,0.058182,-0.24563,0.17948,0.11011,0.15955,0.038317,0.15714,0.26802,0.21713,0.20863,0.19772,0.26882,-0.063072,-0.036993,-0.65869,0.025766,0.036889,-0.5379,0.14888,0.16621,-0.13005,0.24508,-0.2282,-0.1788,0.21827,-0.36234,-0.075307,-0.29145,0.092035,-0.026155,0.32007,0.48516,-0.14264,0.015359,-0.075981,-0.11289,-0.36782,0.0091562,0.28847,-0.25652,0.14886,-0.41084,0.10827,-0.039769,-0.24822,-0.36823,-0.061889,-0.48947,-0.25573 4 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 5 | V,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, 6 | -1.4074,-0.59492,0.2163,-0.25072,0.25474,0.16127,0.10038,0.57475,-1.2121,-0.72364,-1.0775,1.5151,2.348,0.13198,0.95729,0.056651,0.35449,-0.419,0.63198,0.98887,-1.1665,0.49643,1.1759,-0.041543,-0.15773,0.12893,-0.56709,2.0682,-1.8037,0.84331,0.4549,-0.45044,0.089828,-1.2474,0.49553,-0.38666,1.179,0.080986,0.83898,1.2541,0.28983,-2.2077,1.1374,-0.090443,-1.4051,0.96262,0.67947,0.64459,-1.5585,0.56929,1.0605,1.7294,-0.54772,0.15612,-0.99507,-0.47439,-0.085259,-2.8441,-0.097192,0.44774,0.80802,-1.22,-0.06701,0.18064,0.6438,1.2213,0.9951,-1.3629,-0.17415,0.070147,0.072511,0.024813,0.97374,-0.83002,0.79455,-0.87716,-1.5169,1.3863,0.57285,-1.2997,-1.2896,-0.43287,0.89785,0.078842,0.82559,0.29353,0.36765,-2.0845,-1.2306,1.0493,-0.71329,1.4933,-1.5148,0.41076,-0.47319,0.41696,-1.6239,-0.98357,0.37025,1.5854 7 | -0.80584,-0.48031,-0.18977,-0.35396,-0.29403,-1.2032,-0.96578,0.32351,0.55081,1.4534,-0.24546,-0.34567,0.71498,-0.14205,-1.1803,-0.40903,0.060082,0.4101,0.61289,-0.25632,-2.2262,0.69363,-1.2295,-0.54831,-0.86918,-0.614,-0.99089,-1.0948,1.5206,2.4036,-0.76575,-0.26564,-0.43201,-1.1398,-0.63463,0.92373,-0.18749,1.873,-1.3382,-0.72414,0.63597,-2.0758,0.15186,1.0797,-0.17095,-1.0046,1.2438,0.66241,-0.64482,-1.0755,0.11092,0.59234,0.19683,0.063897,0.37786,1.5433,-0.47429,0.93115,-0.56141,0.20077,-0.71567,-0.24863,-2.4206,0.98249,0.57278,-0.038913,0.42607,0.014999,0.98717,-0.61145,1.057,-1.1149,-1.9866,0.24645,-1.4231,-0.40029,0.57846,-1.0546,1.8024,-1.1117,-0.78439,-0.94797,-1.3545,-1.1281,0.95894,-0.23108,-1.7893,-0.25282,-0.95407,-1.2209,0.10542,1.5044,-0.62599,1.342,1.119,1.0547,0.58491,1.6814,-0.042993,0.337 8 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 9 | -------------------------------------------------------------------------------- /Octave/Octave程序代码说明.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Teddy-Liao/Beauty-of-Control-using-Matlab/0648680a97cb68c23d47956df382d081848805ee/Octave/Octave程序代码说明.mp4 -------------------------------------------------------------------------------- /Octave/QP_EQconstraint.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：QP_EQconstraint 6 | %% 程序功能：等式约束二次规划示例 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | 9 | %% 程序初始化，清空工作空间，缓存， 10 | clear all; 11 | close all; 12 | clc; 13 | 14 | % 定义二次规划问题的H,f,Meq 15 | H = [1 0; 0 1]; 16 | f = [1; 1]; 17 | n = size(H,1); 18 | % 定义等式约束的Meq和beq 19 | Meq = [1 -1]; 20 | beq =1; 21 | m = size(Meq,1); 22 | % 定义二次规划问题的u和lamda 23 | u = zeros(n,1); 24 | lamda = zeros(m,1); 25 | % 求解二次规划问题 26 | u_lamda = inverse([H,Meq';Meq,zeros(m,m)])*[-f;beq]; 27 | u = u_lamda(1:n,:); 28 | figure(1, 'position',[150 150 1500 500]); 29 | % 绘制二次规划问题的可行域和最优解点（3D图） 30 | [U1,U2] = meshgrid(-2:0.1:0); 31 | J = 0.5*(U1.^2 + U2.^2)+U1+U2; 32 | subplot(1,2,1); 33 | surf(U1,U2,J,'FaceAlpha', 0.1); 34 | hold on; 35 | u1_proj = -2:0.1:0; 36 | u2_proj = u1_proj - 1; 37 | J_proj = 0.5 * (u1_proj .^ 2 + u2_proj .^ 2) + u1_proj + u2_proj; 38 | plot3(u1_proj, u2_proj, J_proj, 'b', 'LineWidth', 5); 39 | plot3(u(1), u(2), 0.5*(u(1)^2 + u(2)^2)+u(1)+u(2), 'r^', 'MarkerSize', 20,'MarkerFaceColor', 'red'); 40 | [J_proj, U1_proj] = meshgrid(-1:0.1:0, u1_proj); 41 | U2_proj = U1_proj - 1; 42 | surf(U1_proj, U2_proj, J_proj, 'FaceColor','blue','FaceAlpha', 0.2,'EdgeColor', 'none'); 43 | 44 | xlabel('u1'); 45 | ylabel('u2'); 46 | zlabel('J(u1,u2)'); 47 | xlim([-2 0]); 48 | ylim([-2 0]); 49 | zlim([-1.05 0]); 50 | 51 | set(gca,'FontSize',20); 52 | 53 | % 绘制等高线图 54 | subplot(1,2,2); 55 | contour(U1,U2,J,30); 56 | hold on; 57 | plot(u(1), u(2), 'r*', 'MarkerSize', 10); 58 | u1_con = -2:0.1:2; 59 | u2_con = u1_con - 1; 60 | plot(u1_con, u2_con, 'k', 'LineWidth', 2); 61 | xlabel('u1'); 62 | ylabel('u2'); 63 | set(gca,'FontSize',20); 64 | sgtitle('二次规划问题'); 65 | -------------------------------------------------------------------------------- /Octave/QP_Free.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：QP_Free 6 | %% 程序功能：无约束二次规划示例 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | 9 | %% 程序初始化，清空工作空间，缓存， 10 | clear all; 11 | close all; 12 | clc; 13 | 14 | % 定义二次规划问题的H和f 15 | H = [1 0; 0 1]; 16 | f = [1; 1]; 17 | 18 | % 求解二次规划问题 19 | u = -inverse(H)*f; 20 | 21 | figure(1, 'position',[150 150 1500 500]); 22 | 23 | % 绘制二次规划问题的可行域和最优解点（3D图） 24 | [U1,U2] = meshgrid(-2:0.1:0); 25 | J = 0.5*(U1.^2 + U2.^2)+U1+U2; 26 | subplot(1,2,1); 27 | surf(U1,U2,J,'FaceAlpha', 0.1); 28 | hold on; 29 | plot3(u(1), u(2), 0.5*(u(1)^2 + u(2)^2)+u(1)+u(2), 'r^', 'MarkerSize', 20,'MarkerFaceColor', 'red'); 30 | xlabel('u1'); 31 | ylabel('u2'); 32 | zlabel('J(u1,u2)'); 33 | xlim([-2 0]); 34 | ylim([-2 0]); 35 | zlim([-1.05 0]); 36 | set(gca,'FontSize',20); 37 | 38 | % 绘制等高线图 39 | subplot(1,2,2); 40 | contour(U1,U2,J,30); 41 | hold on; 42 | plot(u(1), u(2), 'r*', 'MarkerSize', 10); 43 | xlabel('u1'); 44 | ylabel('u2'); 45 | set(gca,'FontSize',20); 46 | sgtitle('二次规划问题'); 47 | 48 | -------------------------------------------------------------------------------- /Octave/QP_nonEQconstraint.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：QP_nonEQconstraint 6 | %% 程序功能：不等式约束二次规划示例 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | 9 | %% 程序初始化，清空工作空间，缓存， 10 | clear all; 11 | close all; 12 | clc; 13 | % 读取Octave优化数据库（注：如使用Matlab，可删除或注释掉本行代码） 14 | pkg load optim; 15 | 16 | % 定义二次规划问题的H和f 17 | H = [1 0; 0 1]; 18 | f = [1; 1]; 19 | 20 | % 定义不等式约束的A和b 21 | A = [-1 1; 1 1]; 22 | b = [1; 2]; 23 | 24 | % 定义变量的边界条件 25 | lb = [0; 0]; 26 | ub = [1; 2]; 27 | 28 | % 求解二次规划问题 29 | options = optimset('Display','off'); 30 | [u, J] = quadprog(H, f, A, b, [], [], lb, ub, [], options); 31 | 32 | % 绘制等高线图和可行域 33 | figure('position',[150 150 800 600]); 34 | 35 | % 绘制等高线图 36 | subplot(1,1,1); 37 | [U1, U2] = meshgrid(-1:0.1:2); 38 | J = 0.5*(U1.^2 + U2.^2) + U1 + U2; 39 | contour(U1, U2, J, 60); 40 | hold on; 41 | 42 | % 绘制可行域 43 | plot([-0.5, 1], [0.5, 2], 'k', 'LineWidth', 1.5); 44 | hold on; 45 | plot([0, 1.5], [2, 0.5], 'k', 'LineWidth', 1.5); 46 | plot([0, 0], [-1, 2], 'k', 'LineWidth', 1.5); 47 | plot([-0.5, 1.5], [0, 0], 'k', 'LineWidth', 1.5); 48 | plot([1, 1], [-1, 2], 'k', 'LineWidth', 1.5); 49 | fill([0, 0.5, 1], [1, 1.5, 1], 'green', 'FaceAlpha', 0.2, 'EdgeColor', 'none'); 50 | fill([0, 0, 1, 1], [0, 1, 1, 0], 'green', 'FaceAlpha', 0.2, 'EdgeColor', 'none'); 51 | % 绘制最优解点 52 | plot(u(1), u(2), 'r^', 'MarkerSize', 20,'MarkerFaceColor', 'red'); 53 | 54 | % 添加坐标轴标签和图标题 55 | xlabel('u1'); 56 | ylabel('u2'); 57 | xlim([-0.5 1.5]); 58 | ylim([-0.5 2]); 59 | 60 | 61 | set(gca, 'FontSize', 20); 62 | 63 | -------------------------------------------------------------------------------- /Octave/System_discretization.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %% 《控制之美-卷二》 代码 3 | %% 作者：王天威，黄军魁 4 | %% 清华大学出版社 5 | %% 程序名称：System_discretization 6 | %% 程序功能：系统离散化与比较 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | %% 程序初始化，清空工作空间，缓存， 9 | clear all; 10 | close all; 11 | clc; 12 | % 读取Octave控制数据库（注：如使用Matlab，可删除或注释掉本行代码） 13 | pkg load control; 14 | %%%%%%%%%%%%%%%%%%%%%%%%%% 15 | % 构建系统矩阵A 16 | A = [0 1 ; -2 -3]; 17 | % 构建输入矩阵B 18 | B = [0 ; 1]; 19 | %定义两组采样时间 20 | Ts_1 = 0.2; 21 | Ts_2 = 1; 22 | %根据公式计算； 23 | Fd_1 = expm(A*Ts_1); 24 | Gd_1 = inverse(A)*(Fd_1-eye(size(A,1)))*B; 25 | Fd_2 = expm(A*Ts_2); 26 | Gd_2 = inverse(A)*(Fd_2-eye(size(A,1)))*B; 27 | % 连续系统转离散系统 28 | sys_d_1 = c2d(ss(A,B), Ts_1); 29 | sys_d_2 = c2d(ss(A,B), Ts_2); 30 | % 连续系统的单位阶跃响应 31 | step (ss(A,B),'r'); 32 | hold on; 33 | % 两组不同采样时间的离散系统的单位阶跃响应 34 | step (sys_d_1,'b'); 35 | step (sys_d_2); 36 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Beauty of Control using Matlab 2 | 3 | 这是在Github上正式上传的第一份代码仓库，也当作学习使用Git工具的过程了。 4 | 5 | 《控制之美：卷2》是王天威UP主出的一本专门讲最优化控制和卡尔曼滤波的书籍，通俗易懂，并且配套了完整的Octave代码，在Octave中可以顺利运行。 6 | 7 | 阅读过程中，我也跟着配套的Octave代码写了一部分Matlab代码，一方面是为了对书籍理解得更透彻，另一方面也是因为我对Matlab更熟悉。 8 | 9 | 在本项目的Matlab文件夹中，相关的重要函数命名基本与Octave源代码一致，有些采用.m文件，有些则采用交互体验感更好的.mlx文件。 -------------------------------------------------------------------------------- /控制之美（卷2）勘误表_1017.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Teddy-Liao/Beauty-of-Control-using-Matlab/0648680a97cb68c23d47956df382d081848805ee/控制之美（卷2）勘误表_1017.pdf --------------------------------------------------------------------------------