307 lines
13 KiB
Python
307 lines
13 KiB
Python
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 特征值匹配成功！")
|
|
|