（一次遅れ系）とMPC


## 🎯 対象モデル（一次遅れ系）

1次遅れ系の連続時間モデル：

$$
\frac{dx(t)}{dt} = -\frac{1}{T}x(t) + \frac{K}{T}u(t)
$$

これを**サンプリング周期 $dt$** で**離散化**すると、以下の差分方程式になります：

$$
x[k+1] = a \cdot x[k] + b \cdot u[k]
$$

ここで、

$$
a = e^{-dt / T}, \quad b = K(1 - e^{-dt / T})
$$

---

## 📦 MPCの制御問題定式化

予測ホライズンを $N$ ステップに設定し、各時刻 $t$ で以下を解きます：

### 🔧 状態更新式（制約条件）：

$$
x_0 = x[t]
$$

$$
x_{k+1} = a \cdot x_k + b \cdot u_k \quad \text{for } k = 0, ..., N-1
$$

### ⛓ 入力制約：

$$
u_{\min} \le u_k \le u_{\max}
$$

---

## 🧮 コスト関数

以下の形の目的関数（コスト）を最小化：

$$
J = \sum_{k=1}^{N} (x_k - r)^2 + \lambda \sum_{k=0}^{N-1} u_k^2
$$

* $r$：目標値（追従するべき定数）
* $\lambda$：操作量の重み（このコードでは `0.1`）

このコストは：

* **追従誤差**を小さくする（状態が目標に近づく）
* **操作量の抑制**（激しい入力を避ける）

---

## 📉 最適化問題（数式まとめ）

変数：

* $\mathbf{u} = [u_0, ..., u_{N-1}]^\top$
* $\mathbf{x} = [x_1, ..., x_N]^\top$

問題形式：

$$
\min_{\mathbf{u}} \quad \sum_{k=1}^N (x_k - r)^2 + \lambda \sum_{k=0}^{N-1} u_k^2
$$

$$
\text{subject to } x_{k+1} = a x_k + b u_k,\quad u_{\min} \le u_k \le u_{\max}
$$

このような\*\*凸二次計画問題（QP）\*\*は、`cvxpy` で効率的に解くことができます。

---

## 🔁 コードのMPCループの意図

各ステップで：

1. 現在の状態 $x$ を初期条件とした最適入力列 $u_0, ..., u_{N-1}$ を計算
2. 最初の入力 $u_0$ だけ適用（**Receding Horizon Control**）
3. システムを更新し、状態 $x$ を次へ
4. このループを繰り返すことで、**逐次的に最適な軌道を追従**

---

## 🧠 補足：なぜ一次遅れ系でMPCなのか？

一次遅れ系は以下のような特性を持ち、

* 遅延・応答速度の評価に最適
* 安定性を簡単に確認可能
* 現実の熱系・電気系にも対応

MPCのチューニングや構造把握の**教育的題材**として優れており、今回のようなコードと式展開で全体像を理解しやすいです。

---


```Python
# Program Name: mpc_first_order_response.py
# Creation Date: 20250622
# Overview: MPC control of a first-order lag system using cvxpy (OSQP solver)
# Usage: Run this script with Python environment where cvxpy, matplotlib, numpy are installed

# ライブラリのインストール（初回のみ必要）
# pip install cvxpy matplotlib numpy osqp

import numpy as np
import matplotlib.pyplot as plt
import cvxpy as cp

#-----------------------------
# 一元管理：パラメータ設定 / Parameter definitions
#-----------------------------
K = 1.0               # ゲイン / System gain
T = 5.0               # 時定数 / Time constant
dt = 1.0              # サンプリング周期 / Sampling time
N = 10                # 予測ホライズン / Prediction horizon
sim_time = 50         # シミュレーション時間ステップ / Simulation steps

u_min, u_max = 0, 1   # 入力制限 / Input constraints
r = 1.0               # 目標値 / Target output
rho = 0.1             # 操作量の重み / Input effort weight

#-----------------------------
# 離散モデル定数の導出 / Discrete-time model constants
#-----------------------------
a = np.exp(-dt / T)                   # 離散化係数A / Discretized state transition coefficient
b = K * (1 - np.exp(-dt / T))         # 離散化係数B / Discretized input coefficient

#-----------------------------
# 初期化 / Initialization
#-----------------------------
x = 0.0                               # 初期状態 / Initial state
x_history = []                        # 出力履歴 / Output log
u_history = []                        # 入力履歴 / Input log
r_history = []                        # 目標履歴 / Reference log

#-----------------------------
# MPC制御ループ / MPC control loop
#-----------------------------
for t in range(sim_time):
    # 変数定義 / Define optimization variables
    u = cp.Variable(N)               # 入力系列 / Input sequence
    x_pred = cp.Variable(N + 1)      # 状態予測系列 / Predicted state sequence

    # 制約初期化 / Constraints list
    constraints = [x_pred[0] == x]   # 初期状態条件 / Initial condition

    # モデル制約と入力制限 / Model and input constraints
    for k in range(N):
        constraints += [x_pred[k+1] == a * x_pred[k] + b * u[k]]  # 状態更新式 / State update
        constraints += [u_min <= u[k], u[k] <= u_max]             # 入力制限 / Input bounds

    # コスト関数定義 / Define cost function
    objective = cp.sum_squares(x_pred[1:] - r) + rho * cp.sum_squares(u)  # 追従誤差＋入力抑制

    # 最適化問題の定義と解決 / Define and solve optimization problem
    prob = cp.Problem(cp.Minimize(objective), constraints)
    prob.solve(solver=cp.OSQP, eps_abs=1e-6, eps_rel=1e-6)  # OSQP使用 / Use OSQP solver

    # 最適入力の適用 / Apply first optimal input
    u_opt = u.value[0]
    x = a * x + b * u_opt  # システムの状態更新 / Update system state

    # ログ保存 / Save logs
    x_history.append(x)
    u_history.append(u_opt)
    r_history.append(r)

#-----------------------------
# 結果の可視化 / Plotting results
#-----------------------------
plt.figure()
plt.plot(x_history, label='Output x')                      # 出力信号 / Output trajectory
plt.plot(r_history, '--', label='Reference r')             # 参照値 / Reference trajectory
plt.xlabel('Time step')
plt.ylabel('System Output')
plt.title('MPC Tracking Control of First-Order System')
plt.legend()
plt.grid(True)
plt.show()

plt.figure()
plt.step(range(sim_time), u_history, where='post')         # 入力信号（ステップ保持）/ Input sequence
plt.xlabel('Time step')
plt.ylabel('Control Input u')
plt.title('Control Input (MPC)')
plt.grid(True)
plt.show()

```