import numpy as np
import matplotlib.pyplot as plt
from scipy.optimize import minimize

# 定义系统参数
current_position = 0
target_position = 50
max_drive = 10
min_drive = -10
period = 0.1  # 100ms

# 定义MPC优化目标函数
def mpc_objective(drive_sequence, *args):
    current_position, target_position, period = args
    position = current_position
    total_error = 0
    for drive in drive_sequence:
        position += drive * period
        total_error += (position - target_position) ** 2
    return total_error

# 初始驱动序列
initial_drive_sequence = np.zeros(50)

# 约束条件
bounds = [(min_drive, max_drive) for _ in range(len(initial_drive_sequence))]

# 优化驱动序列
result = minimize(mpc_objective, initial_drive_sequence, args=(current_position, target_position, period), bounds=bounds)

# 获取优化后的驱动序列
optimal_drive_sequence = result.x

# 计算轨迹
trajectory = [current_position]
position = current_position
for drive in optimal_drive_sequence:
    position += drive * period
    trajectory.append(position)

# 绘制轨迹图
plt.plot(trajectory, label='Trajectory')
plt.axhline(y=target_position, color='r', linestyle='--', label='Target Position')
plt.xlabel('Time Step')
plt.ylabel('Position')
plt.legend()
plt.title('MPC Trajectory')
plt.show()