import numpy as np from scipy.linalg import solve_discrete_lyapunov import sys def generate_ground_truth_system(dx=2, du=1, dy=1, rng_seed=None): """ 生成一个 "真实" 的、稳定的、可控的、可观测的 LTI 系统。 该过程遵循论文 6.2 节中描述的方法。 参数: dx (int): 状态维度 (state dimension) du (int): 输入维度 (input dimension) dy (int): 输出维度 (output dimension) rng_seed (int, optional): 用于复现的随机种子 返回: tuple: (A, B, C, D) 矩阵 """ # 初始化随机数生成器 if rng_seed is None: rng = np.random.default_rng() else: rng = np.random.default_rng(rng_seed) # 尝试生成一个良态的系统，最多重试 100 次 max_retries = 100 for attempt in range(max_retries): try: # --- 1. 生成稳定的 A 矩阵 (dx x dx) --- # 论文图 2(b) 显示了复特征值，遵循 6.2 节的极坐标法 all_eigenvalues = [] # 确保至少一个实特征值，如果 dx 为奇数 num_real_needed = dx % 2 for _ in range(num_real_needed): all_eigenvalues.append(rng.uniform(-0.99, 0.99)) # 稳定的实数特征值 # 生成复共轭特征值对 num_complex_pairs_needed = (dx - num_real_needed) // 2 for _ in range(num_complex_pairs_needed): r_squared = rng.uniform(0, 1.0) r = np.sqrt(r_squared) * rng.uniform(0.1, 0.99) # 确保半径在稳定范围内 theta = rng.uniform(0, np.pi) # 仅在上半平面采样角度 lambda1 = r * (np.cos(theta) + 1j * np.sin(theta)) all_eigenvalues.append(lambda1) all_eigenvalues.append(np.conjugate(lambda1)) rng.shuffle(all_eigenvalues) # 随机打乱特征值顺序 # 构造一个 Real Schur Form 的块对角矩阵 (J_block) J_blocks = [] current_eigenvalues = list(all_eigenvalues) # 创建一个可变副本 while current_eigenvalues: e = current_eigenvalues.pop(0) if np.isreal(e): J_blocks.append(np.array([[e.real]])) else: # 寻找其共轭 conjugate_found = False for i, ce in enumerate(current_eigenvalues): if np.isclose(ce, np.conjugate(e)): # 使用 np.isclose 进行浮点比较 alpha = e.real beta = e.imag J_blocks.append(np.array([[alpha, beta], [-beta, alpha]])) current_eigenvalues.pop(i) # 移除已使用的共轭特征值 conjugate_found = True break if not conjugate_found: # 如果没有找到共轭（理论上不应发生，但作为健壮性处理），退化为实部 J_blocks.append(np.array([[e.real]])) # 组装 J_block if not J_blocks: # 处理 dx=0 或空列表的情况 J_block = np.zeros((0,0)) else: # 使用 np.block 构造块对角矩阵 J_block = np.block([ [J_blocks[i] if i == j else np.zeros((J_blocks[i].shape[0], J_blocks[j].shape[1])) for j in range(len(J_blocks))] for i in range(len(J_blocks)) ]) # 生成一个随机正交矩阵 V (dx x dx) Z = rng.standard_normal(size=(dx, dx)) V, _ = np.linalg.qr(Z) # 组装 A = V * J_block * V^T A = V @ J_block @ V.T # --- 2. 生成 B (dx x du) 和 C (dy x dx) 矩阵 --- # 元素从 N(0, 1) 独立采样 B = rng.standard_normal(size=(dx, du)) C = rng.standard_normal(size=(dy, dx)) # --- 3. 检查可控性和可观测性 --- # 求解离散时间李雅普诺夫方程 (Lyapunov equation) Wc = solve_discrete_lyapunov(A, B @ B.T) # Controllability Gramian Wo = solve_discrete_lyapunov(A.T, C.T @ C) # Observability Gramian # 检查 Gramian 矩阵的条件 # "拒绝主要特征值占总能量 99% 以上的系统" eig_Wc = np.linalg.eigvalsh(Wc) eig_Wo = np.linalg.eigvalsh(Wo) cond_c = np.max(eig_Wc) / np.sum(eig_Wc) cond_o = np.max(eig_Wo) / np.sum(eig_Wo) # 如果系统是良态的，则跳出循环 if cond_c < 0.99 and cond_o < 0.99: # --- 4. 定义 D 矩阵 (dy x du) --- # 算例 6.3 中 D=0 D = np.zeros((dy, du)) print(f"--- 成功生成 Ground Truth 系统 (尝试次数: {attempt + 1}) ---") # 打印部分特征值，保持输出简洁 print_eigs = [f"{e:.4f}" for e in all_eigenvalues[:min(dx, 4)]] if dx > 4: print_eigs.append("...") print(f"特征值: {', '.join(print_eigs)}") print(f"Gramian 能量占比: Wc={cond_c:.4f}, Wo={cond_o:.4f}") return A, B, C, D except np.linalg.LinAlgError: # 李雅普诺夫方程求解器可能失败（例如，A 矩阵数值上不稳定） print(f"尝试 {attempt + 1} 失败 (LinAlgError)。正在重试...", file=sys.stderr) continue # 如果循环结束仍未成功 raise RuntimeError(f"在 {max_retries} 次尝试后未能生成一个良态的系统。") def transform_to_canonical_form(A, B, C, D): """ 将给定的 LTI 系统转换为可控规范形 (controllable canonical form)。 此实现特指论文 Definition 3.1 中的 SISO 控制器规范形，并确保马尔可夫参数不变。 参数: A (np.ndarray): 状态矩阵 (dx x dx) B (np.ndarray): 输入矩阵 (dx x du) C (np.ndarray): 输出矩阵 (dy x dx) D (np.ndarray): 直通矩阵 (dy x du) 返回: tuple: 转换后的 (A_canon, B_canon, C_canon, D_canon) 抛出: ValueError: 如果系统不是 SISO 或不可控。 """ dx = A.shape[0] # 状态维度 du = B.shape[1] # 输入维度 dy = C.shape[0] # 输出维度 # 1. 验证 SISO 系统，因为论文中描述的控制器规范形是针对 SISO 的 if du != 1 or dy != 1: raise ValueError("此转换仅针对单输入单输出 (SISO) 系统实现。") # 2. D 矩阵在状态空间变换中是不变的 D_canon = D # 3. 计算 A 的特征多项式系数 # np.poly(A) 返回 [1, p_{dx-1}, p_{dx-2}, ..., p_0] # 其中特征多项式为 s^dx + p_{dx-1}s^{dx-1} + ... + p_1 s + p_0 poly_coeffs_with_leading_one = np.poly(A) # 论文 Definition 3.1 中 A_c 的最后一行是 [-a_0, -a_1, ..., -a_{dx-1}] # 这里的 a_k 与 np.poly(A) 返回的 p_k 是一致的 (a_k = p_k)。 # 需要将 np.poly(A)[1:] 的系数反序以匹配 A_c 的最后一行结构 [-p_0, -p_1, ..., -p_{dx-1}] a_coeffs_for_Ac_row = -poly_coeffs_with_leading_one[1:][::-1] # 4. 构造 A_canon (控制器规范形) A_canon = np.zeros((dx, dx)) # 填充超对角线为 1 for i in range(dx - 1): A_canon[i, i+1] = 1 # 填充最后一行 A_canon[dx-1, :] = a_coeffs_for_Ac_row # 5. 构造 B_canon (控制器规范形) B_canon = np.zeros((dx, du)) B_canon[-1, 0] = 1 # SISO 情况下，最后一个元素为 1 # 6. 计算原始系统 (A, B) 的可控性矩阵 Q # Q = [B, A@B, A^2@B, ..., A^(dx-1)@B] Q = np.zeros((dx, dx)) current_B_col = B for i in range(dx): Q[:, i] = current_B_col.flatten() # B 是 dx x 1 向量，需要展平 current_B_col = A @ current_B_col # 7. 检查可控性 if np.linalg.matrix_rank(Q) != dx: raise ValueError("系统不可控，无法转换为可控规范形。") # 8. 计算规范形系统 (A_canon, B_canon) 的可控性矩阵 Q_canon # Q_canon = [B_canon, A_canon@B_canon, ..., A_canon^(dx-1)@B_canon] Q_canon = np.zeros((dx, dx)) current_B_c_col = B_canon for i in range(dx): Q_canon[:, i] = current_B_c_col.flatten() current_B_c_col = A_canon @ current_B_c_col # 9. 计算 C_canon # 根据控制器规范形的定义，C_canon = [b_0, b_1, ..., b_{dx-1}] # 其中 b_i 是传递函数分子系数。这可以通过将原始系统的马尔可夫参数 M_k # 与规范系统的可控性矩阵 Q_canon 关联起来得到。 # 我们知道 [M_1, M_2, ..., M_dx] = C_canon @ Q_canon # 因此 C_canon = [M_1, M_2, ..., M_dx] @ Q_canon_inv # 计算原始系统的马尔可夫参数 M_k (k=1...dx) # M_k = C @ A^(k-1) @ B markov_params_row_matrix = np.zeros((1, dx)) for k in range(dx): # markov_params_row_matrix[0, k] 存储的是 M_{k+1}，对应 C A^k B markov_params_row_matrix[0, k] = (C @ np.linalg.matrix_power(A, k) @ B).item() C_canon = markov_params_row_matrix @ np.linalg.inv(Q_canon) return A_canon, B_canon, C_canon, D_canon if __name__ == '__main__': # --- 运行示例 --- # 设置一个随机种子，以便每次运行时都能得到相同的结果 A_true, B_true, C_true, D_true = generate_ground_truth_system(dx=2, du=1, dy=1, rng_seed=42) print("\n--- 原始系统 ---") print("A_true = \n", A_true) print("B_true = \n", B_true) print("C_true = \n", C_true) print("D_true = \n", D_true) # 转换为可控规范形 A_canon, B_canon, C_canon, D_canon = transform_to_canonical_form(A_true, B_true, C_true, D_true) print("\n--- 可控规范形系统 ---") print("A_canon = \n", A_canon) print("B_canon = \n", B_canon) print("C_canon = \n", C_canon) print("D_canon = \n", D_canon) # 验证转换结果 (不变的系统特性) print("\n--- 验证转换结果 ---") # 1. D矩阵应相同 print(f"D_true (原始): \n{D_true}") print(f"D_canon (规范形): \n{D_canon}") assert np.allclose(D_true, D_canon), "D 矩阵不匹配！" print("D 矩阵匹配成功！") # 2. 马尔可夫参数应相同 (输入-输出行为不变) # M_t = C A^(t-1) B (对于 t >= 1), M_0 = D def get_markov_params(A, B, C, D, steps=5): params = [D] # M_0 = D for t in range(1, steps): params.append(C @ np.linalg.matrix_power(A, t - 1) @ B) return params markov_true = get_markov_params(A_true, B_true, C_true, D_true, steps=5) markov_canon = get_markov_params(A_canon, B_canon, C_canon, D_canon, steps=5) print("\n马尔可夫参数验证 (前5步):") for i in range(len(markov_true)): print(f" Step {i}: True={markov_true[i].flatten()}, Canon={markov_canon[i].flatten()}") assert np.allclose(markov_true[i], markov_canon[i]), f"马尔可夫参数在第 {i} 步不匹配！" print("马尔可夫参数匹配成功！") # 3. 特征值应相同 eig_true = np.linalg.eigvals(A_true) eig_canon = np.linalg.eigvals(A_canon) # 对特征值进行排序以进行比较，因为它们的顺序可能不同 eig_true_sorted = np.sort(eig_true) eig_canon_sorted = np.sort(eig_canon) print(f"\n特征值: True={eig_true_sorted}, Canon={eig_canon_sorted}") assert np.allclose(eig_true_sorted, eig_canon_sorted), "特征值不匹配！" print("特征值匹配成功！") print("\n所有验证成功！") # 尝试一个更高维度的系统 print("\n--- 测试 dx=3 的系统 ---") A_true_3, B_true_3, C_true_3, D_true_3 = generate_ground_truth_system(dx=3, du=1, dy=1, rng_seed=100) A_canon_3, B_canon_3, C_canon_3, D_canon_3 = transform_to_canonical_form(A_true_3, B_true_3, C_true_3, D_true_3) print("\nA_canon (dx=3) = \n", A_canon_3) print("B_canon (dx=3) = \n", B_canon_3) print("C_canon (dx=3) = \n", C_canon_3) markov_true_3 = get_markov_params(A_true_3, B_true_3, C_true_3, D_true_3) markov_canon_3 = get_markov_params(A_canon_3, B_canon_3, C_canon_3, D_canon_3) for i in range(len(markov_true_3)): assert np.allclose(markov_true_3[i], markov_canon_3[i]), f"dx=3 马尔可夫参数在第 {i} 步不匹配！" print("dx=3 马尔可夫参数匹配成功！") eig_true_3 = np.linalg.eigvals(A_true_3) eig_canon_3 = np.linalg.eigvals(A_canon_3) assert np.allclose(np.sort(eig_true_3), np.sort(eig_canon_3)), "dx=3 特征值不匹配！" print("dx=3 特征值匹配成功！")