[最佳控制] 離散時間 穩態 LQR 控制問題 (2) - LQR convergence

回憶離散時間穩態LQR問題： 定義 cost function
\[\begin{array}{l}
V\left( {x,{\bf{u}}} \right): = \frac{1}{2}\sum\limits_{k = 0}^\infty  {{x^T}\left( k \right)Qx\left( k \right) + {u^T}\left( k \right)Ru\left( k \right)} \\
s.t.\\
x\left( {k + 1} \right) = Ax\left( k \right) + Bu\left( k \right)\\
x\left( 0 \right) = x
\end{array}\]且 $Q, R >0$ 為正定矩陣。若 $(A,B)$ 為可控制 則 最佳化問題
\[
\min_{\bf u} V(x,{\bf u})
\]解存在 且唯一 (why?)。

現在我們將此解 (控制力序列) 記做 ${\bf u}^*(x)$；其中第一項為 $u^*(x)$。且對應的最佳控制律 $K_{\infty}(\cdot)$  故我們有
\[
K_\infty(x) = u^*(x) = {\bf u}^* (0;x)
\]

以下定理說明若可控制系統 採用 穩態 LQR 控制，則保證閉迴路穩定度：

=====================
Theorem: Steady State LQR Guarantees the Closed-Loop Stability
對 $(A,B)$ 可控制，穩態LQR問題 且 $Q,R >0$ 保證閉迴路系統
\[
x(k+1) = A x(k) + B K_\infty (x)
\]穩定。
=====================

Comment:
線性系統中，漸進穩定度 等價 漸進收斂 $x(k) \to 0$

Proof:
首先證明 穩態 cost function 有界。注意到由於  $(A,B)$ 可控制，由定義可知存在 一組控制力
\[
{\bf u} = \{u(0), u(1),...,u(n-1)\}
\] 使得 控制後的系統能在有限時間 $n$ 步之內，可由任意給定初始狀態 $x(0)$ 移動到 給定的終止狀態 $x(n) = 0$；且 在時間 $ n$ 之後的控制力 $ \{u(n+1), u(n+2),...\} = \{0,0,0,...\}$ 故原本無窮和的 cost 函數變為有限和
\[\begin{array}{l}
V\left( {x,{\bf{u}}} \right): = \frac{1}{2}\sum\limits_{k = 0}^\infty  {{x^T}\left( k \right)Qx\left( k \right) + {u^T}\left( k \right)Ru\left( k \right)} \\
 \Rightarrow V\left( {x,{\bf{u}}} \right) = \frac{1}{2}\sum\limits_{k = 0}^n {{x^T}\left( k \right)Qx\left( k \right) + {u^T}\left( k \right)Ru\left( k \right)}
\end{array}\]且由於 成本函數為 strictly convex in $\bf u$ 且 $R>0$ ( $u$ 沒有 vanish) 故 此穩態LQR最佳問題的解為唯一。

現在我們觀察 costs to go 的數列滿足
\[
V_{k+1} = V_k - 1/2 (x(k)'Qx(k) + u(k)' R u(k))
\]其中 $V_k := V^0(x(k))$ 為 在時刻 $k$ 對應 狀態為 $x(k)$ 與 對應最佳控制力 $u(k) = u^0(x(k))$ 的 cost。故此數列 $\{V_k\}$ 為非遞增 且 有下界 (為 $0$)。可推知此數列必定收斂；故
\[
|V_{k+1} - V_k| \to 0\;\;\;\; \text{as $k \to \infty$}
\]因此
\[\left\{ \begin{array}{l}
x\left( k \right)'Qx\left( k \right)\; \to 0\\
u\left( k \right)'{\rm{ }}R{\rm{ }}u\left( k \right) \to 0
\end{array} \right.\]又因為 $Q,R >0$ 故可推知
\[\left\{ \begin{array}{l}
x\left( k \right)\; \to 0\\
u\left( k \right) \to 0
\end{array} \right.\]亦即閉迴路系統狀態收斂 (閉迴路穩定!)。 $\square$

Comments 
1. $R>0$ 條件用以保證控制律唯一性。實際上若使用者不關心控制律，可將其設為參數非常小的正定矩陣
2. 上述定裡假設可從 controllability 推廣到 stabilizability。
3. $Q>0$ 的條件可被推廣到 $Q \ge 0$ 與 $(A,Q)$ detectable。

[最佳控制] Optimizing Multistage Functions - Forward/Backward Dynamic Programming

Life can only be understood going backwards, but it must be lived going forwards. --- Kierkegaard.

考慮一組變數 $w,x,y,z$ 且我們希望最佳化下列的成本函數
\[
f(w,x) + g(x,y) + h(y,z)
\]讀者可已注意到上述的成本函數中有特殊的結構，亦即每一項只有兩個變數。

Backward Dynamic Programming
若我們考慮 $w$ 固定，則上述最佳化問題
\[
\min_{x,y,z} f(w,x) + g(x,y) + h(y,z)
\]可以改寫為 分別最佳化的子問題
\[\mathop {\min }\limits_x \left[ {f(w,x) + \mathop {\min }\limits_y \left[ {g(x,y) + \mathop {\min }\limits_z h(y,z)} \right]} \right]\]故我們可以先解最內部的最佳化問題並得到對應的最佳解 $z^*$ 與 最佳解對應的成本值 $h^*$
\[\left\{ \begin{array}{l}
\mathop {\min }\limits_z h(y,z): = {h^*}\left( y \right)\\
\arg \mathop {\min }\limits_z h(y,z): = {z^*}\left( y \right)
\end{array} \right.\]接著我們求解第二部分的 最佳化的子問題
\[\mathop {\min }\limits_y \left[ {g(x,y) + {h^*}\left( y \right)} \right]\]其對應的解
\[\left\{ \begin{array}{l}
\mathop {\min }\limits_y \left[ {g(x,y) + {h^*}\left( y \right)} \right]: = {g^*}\left( x \right)\\
\arg \mathop {\min }\limits_y \left[ {g(x,y) + {h^*}\left( y \right)} \right]: = {y^*}\left( x \right)
\end{array} \right.\]最後我們求解 最後一部分的 最佳化子問題
\[\mathop {\min }\limits_x \left[ {f(w,x) + {g^*}\left( x \right)} \right]\]其對應的最佳解
\[\left\{ \begin{array}{l}
\mathop {\min }\limits_x \left[ {f(w,x) + {g^*}\left( x \right)} \right]: = {f^*}\left( w \right)\\
\mathop {\arg \min }\limits_x \left[ {f(w,x) + {g^*}\left( x \right)} \right]: = {x^*}\left( w \right)
\end{array} \right.\]

Forward Dynamic Programming
若我們改考慮 $z$ 固定，則上述最佳化問題
\[
\min_{x,y,z} f(w,x) + g(x,y) + h(y,z)
\]可以改寫為 分別最佳化的子問題，下列形式稱為 Forward Dynamic Programming
\[\mathop {\min }\limits_y \left[ {h(y,z) + \mathop {\min }\limits_x \left[ {g\left( {x,y} \right) + \mathop {\min }\limits_w f\left( {w,x} \right)} \right]} \right]\]故可得
\[\underbrace {\mathop {\min }\limits_y \left[ {h(y,z) + \underbrace {\mathop {\min }\limits_x \left[ {g\left( {x,y} \right) + \underbrace {\mathop {\min }\limits_w f\left( {w,x} \right)}_{{f^*}\left( x \right),\begin{array}{*{20}{c}}
{}
\end{array}{w^*}\left( x \right)}} \right]}_{{g^*}\left( {x,y} \right),\begin{array}{*{20}{c}}
{}
\end{array}{x^*}\left( y \right)}} \right]}_{{h^*}(y,z),\begin{array}{*{20}{c}}
{}
\end{array}{y^*}\left( z \right)}\]

[最佳控制] 離散時間 穩態 LQR 控制問題 (1)

延續前篇，這次要介紹的是 Discrete Time Linear Quadratic Regulator in Infinite Horizon 或稱 Steady State LQR。

================
LQR Problem (Infinite Horizon LQR)：
考慮離散狀態方程：
\[
x(k+1) = A x(k) + B u(k)
\]其中 $x(k) \in \mathbb{R}^n, A\in \mathbb{R}^{n \times n}, B \in \mathbb{R}^{n \times m}, u(k) \in \mathbb{R}^{m \times 1}$且 $(A,B)$ controllable。
定義 Performance index：
\[
J(u) = \displaystyle \sum_{k=0}^{\infty} x^T(k+1) Q x(k+1) + u^T(k) R u(k)
\] 其中 $Q, R$ 必須滿足 $Q^T = Q, Q \succ 0$， $R^T = R, R \succ 0$。 (亦即 $Q, R$ 必須為 對稱 + 正定 矩陣)

試求出一組最佳控制力序列 $u^*$ 使得成本函數 $J(u)$ 最小。
================

Comment:
讀者須注意到 Infinite Horizon 的 LQR問題要求計算 Performance index 為無窮級數和，此解必須保證收斂。以下定理告訴我們何時 此 Performance index 收斂

Lemma
考慮離散系統 $x(k+1) = A x(k) + B u(k)$，若 $(A,B)$ 可控制，且選 $Q, R >0$ 為正定矩陣，則上述 infinite horizon LQR 問題保證 閉迴路系統 狀態收斂到 $0$ 且 cost 為有界。

Proof: omitted. (see J. B. Rawlings and D. Q. Mayne, "Model Predictive Control: Theory and Design, p. 24", 2009)


現在我們可以開始求解 Infinite Horizon LQR問題：
Solution
回憶 Steady State Bellman Equation，為了符號簡便起見，我們寫成 functional equation 形式，
\[
I(x) = \displaystyle \min_{u \in \Omega} \{J(x,u) + I(f(x,u)) \}
\] 上式中 $J(x,u)$ 為 Branch cost，亦即 $J(x,u) = x^T Q x + u^T R u$ (並非 $\sum_{k=0}^{\infty} (\cdot)...$)

首先我們猜一組解 $I(x) = x^T P x$ 且矩陣 $P$ 為對稱正定矩陣，亦即滿足 $P^T = P, P \succ 0$。我們之後會找到此 $P$ 應該長甚麼樣子。

將猜測的解代入上述的 Steady State Bellman Equation，故現在我們得到
\[
I(x) = \min_{u \in \Omega} \{J(x,u) + I(f(x,u)) \}
\]注意到 $I(f(x,u) = f(x,u)^T P f(x,u) = (Ax+Bu)^TP(Ax+Bu)$，故我們可得
 \[
\begin{array}{l} I(x) = \mathop {\min }\limits_{u \in \Omega } \{ J(x,u) + I(f(x,u))\} \\ \Rightarrow {x^T}Px = \mathop {\min }\limits_u \left\{ {{x^T}Qx + {u^T}Ru + {{\left( {Ax + Bu} \right)}^T}P\left( {Ax + Bu} \right)} \right\}\\ \Rightarrow {x^T}Px = \mathop {\min }\limits_u \left\{ {{x^T}\left( {Q + {A^T}PA} \right)x + 2{x^T}{A^T}PBu + {u^T}Ru + {u^T}{B^T}PBu} \right\} \end{array}
\]透過一階必要條件 FONC: $ \frac{\partial }{{\partial u}} = 0$ 對上式右邊求解
 \[\begin{array}{l} 2{\left( {{x^T}{A^T}PB} \right)^T} + 2Ru + 2{B^T}PBu = 0\\ \Rightarrow {u^*} = - {\left( {R + {B^T}PB} \right)^{ - 1}}{B^T}PAx \end{array}
\]現在將 $u^*$ 代回 $(*)$  可得 \[\begin{array}{l} {x^T}Px = \mathop {\min }\limits_u \left\{ {{x^T}\left( {Q + {A^T}PA} \right)x + 2{x^T}{A^T}PBu + {u^T}Ru + {u^T}{B^T}PBu} \right\}\\ \Rightarrow {x^T}Px = \left\{ {{x^T}\left\{ {Q + {A^T}PA - {A^T}PB{{\left( {R + {B^T}PB} \right)}^{ - 1}}{B^T}PA} \right\}x} \right\} \end{array}
\]比較左右兩邊可得到 $P$ 必須滿足下式： \[P = Q + {A^T}PA - {A^T}PB{\left( {R + {B^T}PB} \right)^{ - 1}}{B^T}PA\] 此式稱為 Discrete Time Algebraic Ricatti Equation (ARE)，一般而言，可利用 MATLA 指令 dare(A,B,Q, R) 求解 P。

由於 $u^* = - {\left( {R + {B^T}PB} \right)^{ - 1}}{B^T}PAx$，其中除了 $P$ 未定之外，其餘所需要的參數都已知且皆與跌代時間無關，故此無窮時間LQR問題得到的 最佳控制力為 Time invariant。

現在我們總結如下：求解無窮時間的LQR問題只要做兩個步驟即可

STEP 1: 求解一次 Algebraic Ricatti Equation 得到 $P$ (利用 MATLAB: dare.m 或者徒手計算)

\[
P = Q + {A^T}PA - {A^T}PB{\left( {R + {B^T}PB} \right)^{ - 1}}{B^T}PA
\]STEP2 : 將 $P$ 代入 ${u^*} =  - {\left( {R + {B^T}PB} \right)^{ - 1}}{B^T}PAx$

下面我們看個例子：

Example:
考慮一個離散時間線性系統狀態方程：
\[
x_1(k+1) = x_2(k) \\
x_2(k+1) = x_1(k) + u(k)
\]且考慮 Cost function：
\[
J = \sum_{k=0}^{\infty}2x_1^2(k) + 2x_1(k)x_2(k) + x_2^2(k) + 3u^2(k)
\] 且控制力具有如下形式：
\[
u(k) = K_1 x_1(k) + K_2 x_2(k)

\] 試求 $K_1, K_2$ 使 上述 Cost function 最小：

Solution
首先定義  $x(k) := [x_1(k), x_2(k)]^T$ ，則我們有
\[x\left( {k + 1} \right) = \underbrace {\left[ {\begin{array}{*{20}{c}}
0&1\\
1&0
\end{array}} \right]}_A\left[ {\begin{array}{*{20}{c}}
{{x_1}(k)}\\
{{x_2}(k)}
\end{array}} \right] + \underbrace {\left[ {\begin{array}{*{20}{c}}
0\\
1
\end{array}} \right]}_Bu(k)
\] 與 Cost function
\[\begin{array}{l}
J = \sum\limits_{k = 0}^\infty {(2x_1^2(} k) + 2{x_1}(k){x_2}(k) + x_2^2(k) + 3{u^2}(k))\\
\begin{array}{*{20}{c}}
{}
\end{array} = x{\left( k \right)^T}\underbrace {\left[ {\begin{array}{*{20}{c}}
2&1\\
1&1
\end{array}} \right]}_Qx\left( k \right) + \underbrace 3_R{u^2}(k)
\end{array}
\] 那麼現在此問題變成 Steady-state LQR problem，故由前述討論可知我們有 Optimal feedback control 為
\[
u^*(k) = -(R+B^T P B)^{-1} B^T PA \cdot x(k)

\] 其中 $P$ 滿足 $P=P^T, P \succ 0$ 可由 ARE
\[
P= A^TPA - A^T PB (R+ B^TPB)^{-1}B^TPA+Q

\]利用 MATLAB 指令 dare(A,B,Q,R) 解得 $P = \left[ {\begin{array}{*{20}{c}}
{3.7841}&{1.6815}\\
{1.6815}&{4.4022}
\end{array}} \right]$ 現在將 $P$ 帶回 $u^*$中
\[u\left( k \right) = - \left[ {\begin{array}{*{20}{c}}
{0.5947}&{0.2272}
\end{array}} \right]x\left( k \right)

\]i.e., $K_1 = -05947, K_2 =-0.2272$. $\square$

[最佳控制] 離散時間 LQR- Finite Time Horizon

這次要介紹 控制理論中一個重要的結果：
 Discrete Time Linear Quadratic Regulator (LQR) in Finite Time Horizon，
中文翻譯為 離散時間線性二次調節器，我們這邊會針對此問題利用 Dynamic Programming 的方法來逐步求解

================
LQR Problem (Finite Horizon)：
考慮狀態方程：
\[
x(k+1) = A x(k) + B u(k)
\]其中 $x(k) \in \mathbb{R}^n, A\in \mathbb{R}^{n \times n}, B \in \mathbb{R}^{n \times m}, u(k) \in \mathbb{R}^{m \times 1}$
且 考慮 Performance index：
\[
J(u) = \displaystyle \sum_{k=0}^{N-1} x^T(k+1) Q x(k+1) + u^T(k) R u(k)
\] 其中 $Q, R$ 必須滿足 $Q^T = Q, Q \succ 0$， $R^T = R, R \succ 0$。 (亦即 $Q, R$ 必須為 對稱 + 正定 矩陣)

試求出一組最佳控制力序列 $u(N-1), u(N-2),... u(0)$ 使得成本函數 $J(u)$ 最小。
================

Comment:
1. LQR 顧名思義是其具有系統狀態方程為線性 $x(k+1) = Ax(k)+Bu(k)$ 與  Performance Index 中的項都為二次式。
\[
J(u) = \displaystyle \sum_{k=0}^{N-1} x^T(k+1) Q x(k+1) + u^T(k) R u(k)
\]
2. 上述對於矩陣 $Q, R$ 的對稱與正定假設是必須的 (之後需求在求解最佳控制力序列的時候需要求解反矩陣，故需要這些性質。)

3. 上述 LQR in Finite Horizon的問題所求得的最佳控制力序列為 Time Varying 。(此性質會在下面求解的時候再度強調。)

4. 若 Performance index 考慮 $N \rightarrow \infty$，亦即
\[
J(u) = \displaystyle \sum_{k=0}^{\infty} x^T(k+1) Q x(k+1) + u^T(k) R u(k)
\]則我們說此問題為 LQR in Infinite Horizon 或稱 Steady State LQR。這類問題會在之後再做介紹。(此類問題所得到的最佳控制序列將不再是 Time Varying，且只需求解一次 Algebraic Ricatti Equation 即可獲得最佳控制力序列)



Solution of Discrete Time LQR problem in Finite Horizon 
現在我們開始進行求解。

這邊我們使用 Dynamic Programming 方法來求解上述 LQR問題。回憶 Dynamic Programming Equation 的定義：
\[I\left( {x\left( l \right),N - l} \right) = \mathop {\min }\limits_{u\left( l \right) \in {\Omega _l}} \left\{ {J\left( {x\left( l \right),u\left( l \right)} \right) + I\left( {x\left( {l + 1} \right),N - \left( {l + 1} \right)} \right)} \right\}
\] 考慮 Optimal Cost of one-step-to-go ($l=N-1$):
\[\begin{array}{l}
 \Rightarrow I\left( {x\left( {N - 1} \right),1} \right) = \min \left\{ {J\left( {x\left( {N - 1} \right),u\left( {N - 1} \right)} \right) + I\left( {x\left( N \right),0} \right)} \right\}\\
\begin{array}{*{20}{c}}
{}&{}&{}&{}&{}&{}
\end{array} = \min \left\{ {{x^T}(N)Qx(N) + {u^T}(N - 1)Ru(N - 1)} \right\}
\end{array}
\]將系統狀態方程 $x(k+1) = A x(k) + B u(k) $ 代入：
\[\begin{array}{l}
 \Rightarrow I\left( {x\left( {N - 1} \right),1} \right) = \\
\begin{array}{*{20}{c}}
{}&{}&{}&{}
\end{array}\min \left\{ \begin{array}{l}
{\left[ {Ax\left( {N - 1} \right) + Bu\left( {N - 1} \right)} \right]^T}Q\left[ {Ax\left( {N - 1} \right) + Bu\left( {N - 1} \right)} \right]\\
 + {u^T}(N - 1)Ru(N - 1)
\end{array} \right\}\\
 \Rightarrow I\left( {x\left( {N - 1} \right),1} \right) = \min \left\{ \begin{array}{l}
{x^T}\left( {N - 1} \right){A^T}QAx\left( {N - 1} \right)\\
\begin{array}{*{20}{c}}
{}
\end{array} + 2{x^T}\left( {N - 1} \right){A^T}QBu\left( {N - 1} \right)\\
\begin{array}{*{20}{c}}
{}
\end{array} + {u^T}\left( {N - 1} \right){B^T}QBu\left( {N - 1} \right)\\
\begin{array}{*{20}{c}}
{}
\end{array} + {u^T}(N - 1)Ru(N - 1)
\end{array} \right\}
\end{array}
\] 由一階必要條件 FONC： ${\nabla _{u\left( {N - 1} \right)}}I\left( {x\left( {N - 1} \right),1} \right) = 0$ 可知
\[\begin{array}{l}
2{B^T}{Q^T}Ax\left( {N - 1} \right) + 2{B^T}QBu\left( {N - 1} \right) + 2Ru(N - 1) = 0\\
 \Rightarrow \left[ {R + {B^T}QB} \right]u(N - 1) =  - {B^T}{Q^T}Ax\left( {N - 1} \right)\\
 \Rightarrow u^*(N - 1) =  - \underbrace {{{\left[ {R + {B^T}QB} \right]}^{ - 1}}{B^T}QA}_{K\left( {N - 1} \right)}x\left( {N - 1} \right)
\end{array}
\]接著我們把上述求得的 1-step-to-go 的最佳控制力 $u^*(k)$ 代回 Optimal cost of 1-step-to-go，我們得到 (透過一些代數運算)
\[I\left( {x\left( {N - 1} \right),1} \right) = {x^T}\left( {N - 1} \right)\underbrace {{A^T}\left\{ {Q - QB{{\left[ {R + {B^T}QB} \right]}^{ - 1}}^T{B^T}Q} \right\}A}_{: = M\left( {N - 1} \right)}x\left( {N - 1} \right)
\]在做下一步跌代之前我們先給個 comments

Comments :
1.注意到上式中我們求 ${u^*}(N - 1) =  - {\left[ {R + {B^T}QB} \right]^{ - 1}}{B^T}QAx\left( {N - 1} \right)$ 需要計算反矩陣 ${\left[ {R + {B^T}QB} \right]^{ - 1}}$ ，故需檢驗反矩陣是否存在：不過這個問題可以被證明反矩陣確實存在： (因為 $R$ 為對稱正定矩陣，$B^TQB$ 為對稱半正定矩陣，由FACT: 正定矩陣+ 半正定矩陣 = 正定矩陣，且正定矩陣具有 eigenvalue 全為正，故可推知反矩陣存在。)

2. 上式所求得的最佳控制力 ${u^*}(N - 1) =  - {\left[ {R + {B^T}QB} \right]^{ - 1}}{B^T}QAx\left( {N - 1} \right) =  - K\left( {N - 1} \right)x\left( {N - 1} \right)$ 其中的 $K_{N-1}$ 稱作 Optimal Gain Matrix。此控制力具有回授控制形式 Feedback form。 (類似 $u=-Kx$ 的形式)

3. 最佳控制力儘管具有回授控制 (Feedback form)的形式，但在此問題中為時變得 Gain。此性質在下一步跌代會顯現出來。

4. 當我們有了上述第一步跌代的結果，整個 LQR問題就簡單許多，因為之後的跌代只有 $Q$ 矩陣會改變其餘結果均不變。我們直接看下一步跌代便會發現此性質：

Back to computation :
考慮 Optimal Cost of 2-steps-to-go： ($l=N-2$):
\[\begin{array}{l}
I\left( {x\left( {N - 2} \right),2} \right) = \min \left\{ {J\left( {x\left( {N - 2} \right),u\left( {N - 2} \right)} \right) + I\left( {x\left( {N - 1} \right),1} \right)} \right\}\\
 \Rightarrow I\left( {x\left( {N - 2} \right),2} \right)\\
\begin{array}{*{20}{c}}
{}&{}
\end{array} = \min \left\{ \begin{array}{l}
{x^T}(N - 1)Qx(N - 1) + {u^T}(N - 2)Ru(N - 2)\\
\begin{array}{*{20}{c}}
{}&{}&{}&{}
\end{array} + {x^T}\left( {N - 1} \right)M\left( {N - 1} \right)x\left( {N - 1} \right)
\end{array} \right\}\\
 \Rightarrow I\left( {x\left( {N - 2} \right),2} \right)\\
\begin{array}{*{20}{c}}
{}&{}
\end{array} = \min \left\{ {{x^T}(N - 1)\underbrace {\left[ {Q + M\left( {N - 1} \right)} \right]}_{: = \tilde Q}x\left( {N - 1} \right) + {u^T}(N - 2)Ru(N - 2)} \right\}
\end{array}
\]可以發現上式中只有 $Q$ 變成了 $\tilde{Q}$ 其餘參數均固定。故我們可以馬上寫下對應的最佳控制力 $u^*(N-2)$:
\[{u^*}(N - 2) =  - {\left[ {R + {B^T}\tilde QB} \right]^{ - 1}}{B^T}\tilde QAx\left( {N - 2} \right) = K (N-2) x(N-2)
\]其對應的 Optimal cost of 2 steps to go：
\[I\left( {x\left( {N - 2} \right),2} \right) = {x^T}\left( {N - 2} \right)\underbrace {{A^T}\left\{ {\tilde Q - \tilde QB{{\left[ {R + {B^T}\tilde QB} \right]}^{ - 1}}^T{B^T}\tilde Q} \right\}A}_{: = M\left( {N - 2} \right)}x\left( {N - 2} \right)
\]故我們只需要持續重複上述步驟，即可依序解得 $K(N-3), K(N-4),..., K(0)$ 與 $M(N-3), M(N-4),...,M(0)$ 與 $J^*$


Comment:
注意到 $u^*(N-2)$ 的 Gain $K(N-2)$ 與 $u^*(N-1)$ 的 Gain $K(N-1)$ 並不相同，此說明了之前所敘的時變特性(Time Varying Property)

[最佳控制] Finite Horizon LQR with Penalty on Final State

考慮 LQR 問題如下

成本函數
\[{V_N}({x_0},u): = \sum\limits_{i = 1}^{N - 1} {\underbrace {l(x\left( i \right),u\left( i \right))}_{{\rm{stage}}\begin{array}{*{20}{c}}
{}
\end{array}{\rm{cost}}}}  + \underbrace {{l_N}({x_N})}_{{\rm{terminal}}\begin{array}{*{20}{c}}
{}
\end{array}{\rm{cost}}}\]其中 \[\left\{ \begin{array}{l}
u: = \{ u(0),u(1),...,u(N - 1)\} \\
l(x,u): = \frac{1}{2}({x^T}Qx + {u^T}Ru)\\
{l_N}(x): = \frac{1}{2}{x^T}{P_f}x
\end{array} \right.\]另外假設 $Q \ge 0$  positive semi-definite 且 $R >0$ positive definite。

我們的目標：找到 $u^*=\{u^*(0),u^*(1),...,u^*(N-1)\}$ 使得
\[\begin{array}{l}
\mathop {\min }\limits_u {V_N}\left( {x\left( 0 \right),u} \right)\\
s.t.\;\;{x^ + } = Ax + Bu
\end{array}\]

現在利用 Backward Dynamic Programming，我們從最後的狀態 $x(N)$ 逐步回推最佳解，亦即觀察
\[{V_N}({x_0},u): = l({x_0},{u_0}) + l({x_1},{u_1}) + ... + \underbrace {l({x_{N - 1}},{u_{N - 1}}) + {l_N}({x_N})}_{{u_{N - 1}}\begin{array}{*{20}{c}}
{}
\end{array}{\rm{affects}}\begin{array}{*{20}{c}}
{}
\end{array}{\rm{only}}\begin{array}{*{20}{c}}
{}
\end{array}{\rm{there!}}}\]由 Optimality Principle 逐步求解的最佳解 必為整體最佳的一環，故此我們先最佳化 下列子問題
\[ \begin{array}{l}
\mathop {\min }\limits_{{u_{N - 1}}} l({x_{N - 1}},{u_{N - 1}}) + {l_N}({x_N})\\
s.t.\begin{array}{*{20}{c}}
{}
\end{array}{x_N} = A{x_{N - 1}} + B{u_{N - 1}}
\end{array}\]現在將拘束條件帶入，我們可得到以下無拘束最佳化問題：
\[\small \begin{array}{l}
\left\{ \begin{array}{l}
\mathop {\min }\limits_{{u_{N - 1}}} l({x_{N - 1}},{u_{N - 1}}) + {l_N}({x_N})\\
s.t.\begin{array}{*{20}{c}}
{}
\end{array}{x_N} = A{x_{N - 1}} + B{u_{N - 1}}
\end{array} \right.\\
 \Rightarrow \mathop {\min }\limits_{{u_{N - 1}}} \frac{1}{2}\left( {x_{N - 1}^TQx_{N - 1}^{} + u_{N - 1}^TRu_{N - 1}^{}} \right) + \frac{1}{2}{\left( {A{x_{N - 1}} + B{u_{N - 1}}} \right)^T}{P_f}\left( {A{x_{N - 1}} + B{u_{N - 1}}} \right)\\
 \Rightarrow \mathop {\min }\limits_{{u_{N - 1}}} \frac{1}{2}\left[ {x_{N - 1}^TQx_{N - 1}^{} + \underbrace {u_{N - 1}^TRu_{N - 1}^{}}_{{V_1}} + \underbrace {{{\left( {A{x_{N - 1}} + B{u_{N - 1}}} \right)}^T}{P_f}\left( {A{x_{N - 1}} + B{u_{N - 1}}} \right)}_{{V_2}}} \right] \ \ \ \ \ (*)
\end{array}\]現在我們可利用下列結果
------------
FACT: 若
\[\left\{ \begin{array}{l}
{V_1} = \frac{1}{2}{\left( {x - a} \right)^T}\Phi \left( {x - a} \right)\\
{V_2} = \frac{1}{2}{\left( {\Theta x - b} \right)^T}\Gamma \left( {\Theta x - b} \right)
\end{array} \right.\]且 $\Phi $ positive definite 與 $\Theta $ 為 positive semi-definite。則其和可表為\[V = {V_1} + {V_2} = \frac{1}{2}{\left( {x - v} \right)^T}\Omega \left( {x - v} \right) + d\]其中
\[\left\{ \begin{array}{l}
\Omega  = \Phi  + {\Theta ^T}\Gamma \Theta \\
v = {\left( {\Phi  + {\Theta ^T}\Gamma \Theta } \right)^{ - 1}}\left( {\Phi a + {\Theta ^T}\Gamma b} \right)\\
d = V\left( v \right)
\end{array} \right.\]
------------

觀察式 $(*)$，我們可利用上述 FACT 比較係數
\[\Phi  = R;\Theta  = B;\Gamma  = {P_f};v = u_{N - 1}^*;b =  - A{x_{N - 1}};a = 0
\]故在此階段的最佳解 $u_{N-1}^*$ 為

\[u_{N - 1}^* = \underbrace { - {{\left( {R + {B^T}{P_f}B} \right)}^{ - 1}}{B^T}{P_f}A}_{{K_{N - 1}}}{x_{N - 1}} = {K_{N - 1}}{x_{N - 1}}
\]且在此階段的 optimal cost (又稱 optimal cost to go) 為
\[V_{N - 1}^*\left( {{x_{N - 1}}} \right) = d + \frac{1}{2}x_{N - 1}^TQx_{N - 1}^{}
\]其中 $d$ 為前述 FACT 中的常數項，我們計算如下
\[\small \begin{array}{*{20}{l}}
{d = V\left( v \right) = {V_1}\left( v \right) + {V_2}\left( v \right)}\\
{\begin{array}{*{20}{c}}
{}&{}
\end{array} = \frac{1}{2}{{\left( {x - a} \right)}^T}\Phi \left( {x - a} \right) + \frac{1}{2}{{\left( {\Theta x - b} \right)}^T}\Gamma \left( {\Theta x - b} \right)}\\
{\begin{array}{*{20}{c}}
{}&{}
\end{array} = \frac{1}{2}{{\left( {{K_{N - 1}}{x_{N - 1}}} \right)}^T}R\left( {{K_{N - 1}}{x_{N - 1}}} \right) + \frac{1}{2}{{\left( {\left( {A + B{K_{N - 1}}} \right){x_{N - 1}}} \right)}^T}{P_f}\left( {\left( {A + B{K_{N - 1}}} \right){x_{N - 1}}} \right)}\\
{\begin{array}{*{20}{c}}
{}&{}
\end{array} = \frac{1}{2}x_{N - 1}^T\left[ {K_{N - 1}^TR{K_{N - 1}} + {{\left( {A + B{K_{N - 1}}} \right)}^T}{P_f}\left( {A + B{K_{N - 1}}} \right)} \right]{x_{N - 1}}}
\end{array}
\]故 optimal cost to go
\[\small \begin{array}{l}
V_{N - 1}^*\left( {{x_{N - 1}}} \right) = d + \frac{1}{2}x_{N - 1}^TQx_{N - 1}^{}\\
\begin{array}{*{20}{c}}
{}&{}&{}&{}
\end{array} = \frac{1}{2}x_{N - 1}^T\left[ {K_{N - 1}^TR{K_{N - 1}} + {{\left( {A + B{K_{N - 1}}} \right)}^T}{P_f}\left( {A + B{K_{N - 1}}} \right)} \right]{x_{N - 1}} + \frac{1}{2}x_{N - 1}^TQx_{N - 1}^{}\\
\begin{array}{*{20}{c}}
{}&{}&{}&{}
\end{array} = \frac{1}{2}x_{N - 1}^T\left[ {Q + K_{N - 1}^TR{K_{N - 1}} + {{\left( {A + B{K_{N - 1}}} \right)}^T}{P_f}\left( {A + B{K_{N - 1}}} \right)} \right]{x_{N - 1}}
\end{array}\]如果我們把最佳解 $u_{N - 1}^* = {K_{N - 1}}{x_{N - 1}} =  - {\left( {R + {B^T}{P_f}B} \right)^{ - 1}}{B^T}{P_f}A{x_{N - 1}}$ 帶入 optimal cost to go，可得
\[\small \begin{array}{l}
V_{N - 1}^*\left( {{x_{N - 1}}} \right) = \frac{1}{2}x_{N - 1}^T\left[ {Q + K_{N - 1}^TR{K_{N - 1}} + {{\left( {A + B{K_{N - 1}}} \right)}^T}{P_f}\left( {A + B{K_{N - 1}}} \right)} \right]{x_{N - 1}}\\
\begin{array}{*{20}{c}}
{}&{}&{}&{}
\end{array} = \frac{1}{2}x_{N - 1}^T\left[ {Q + {A^T}{P_f}A + K_{N - 1}^T\left( {R + {B^T}{P_f}B} \right){K_{N - 1}} + 2K_{N - 1}^T{B^T}{P_f}A} \right]{x_{N - 1}}\\
\begin{array}{*{20}{c}}
{}&{}&{}&{}
\end{array} = \frac{1}{2}x_{N - 1}^T\underbrace {\left[ {Q + {A^T}{P_f}A - {A^T}{P_f}B{{\left( {R + {B^T}{P_f}B} \right)}^{ - 1}}{B^T}{P_f}A} \right]}_{{M_{N - 1}}}{x_{N - 1}}
\end{array}\]但注意到我們並不曉得 $x_{N-1}$ 故我們需要繼續回頭解 $x_{N-2}$ 亦即 接著求解
\[\begin{array}{l}
\left\{ \begin{array}{l}
\mathop {\min }\limits_{{u_{N - 2}}} l\left( {{x_{N - 2}},{u_{N - 2}}} \right) + V_{N - 1}^*\left( {{x_{N - 1}}} \right)\\
s.t.\begin{array}{*{20}{c}}
{}
\end{array}{x_{N - 1}} = A{x_{N - 2}} + B{u_{N - 2}}
\end{array} \right.\\
 \Rightarrow \left\{ \begin{array}{l}
\mathop {\min }\limits_{{u_{N - 2}}} l\left( {{x_{N - 2}},{u_{N - 2}}} \right) + \frac{1}{2}x_{N - 1}^T{M_{N - 1}}{x_{N - 1}}\\
s.t.\begin{array}{*{20}{c}}
{}
\end{array}{x_{N - 1}} = A{x_{N - 2}} + B{u_{N - 2}}
\end{array} \right.
\end{array}\]但注意到先前我們已經解了
\[\left\{ \begin{array}{l}
\mathop {\min }\limits_{{u_{N - 1}}} l\left( {{x_{N - 1}},{u_{N - 1}}} \right) + \frac{1}{2}x_N^T{P_f}{x_N}\\
s.t.\begin{array}{*{20}{c}}
{}
\end{array}{x_N} = A{x_{N - 1}} + B{u_{N - 1}}
\end{array} \right.\]故讀者可直接比對上述兩者，即可得知只要將 前述我們所推出的解其中的 $P_f$ 矩陣換成 $M_{n-1}$ 即可馬上得到 $N-2$ stage的最佳解 與 optimal cost to go。亦即
\[\left\{ \begin{array}{l}
u_{N - 2}^* = {K_{N - 2}}{x_{N - 2}} =  - {\left( {R + {B^T}{M_{N - 1}}B} \right)^{ - 1}}{B^T}{M_{N - 1}}A{x_{N - 2}}\\
V_{N - 2}^*\left( {{x_{N - 2}}} \right) = \frac{1}{2}x_{N - 2}^T{M_{N - 2}}{x_{N - 2}}\\
{M_{N - 2}} = Q + {A^T}{M_{N - 1}}A - {A^T}{M_{N - 1}}B{\left( {R + {B^T}{M_{N - 1}}B} \right)^{ - 1}}{B^T}{M_{N - 1}}A
\end{array} \right.\]

Summary
透過跌代求解 Riccati equation 進而得到一組控制力序列$u(0),u(1),...u(N-1)$；亦即透過下列跌代式：
\[\begin{array}{l}
for\begin{array}{*{20}{c}}
{}
\end{array}k = N - 1,N - 2,...,0\\
\left\{ \begin{array}{l}
u_{}^*\left( k \right) = K\left( k \right)x\left( k \right),\begin{array}{*{20}{c}}
{}&{}
\end{array}\\
K\left( k \right) =  - {\left( {{B^T}M\left( {k + 1} \right)B + R} \right)^{ - 1}}{B^T}M\left( {k + 1} \right)A,
\end{array} \right.\\
\\
for\begin{array}{*{20}{c}}
{}
\end{array}k = N,N - 1,...,0\\
\left\{ \begin{array}{l}
M\left( {k - 1} \right): = Q + {A^T}M\left( k \right)A - {A^T}M\left( k \right)B{\left( {{B^T}M\left( k \right)B + R} \right)^{ - 1}}{B^T}M\left( k \right)A\\
M\left( N \right): = {P_f}\\
V_{}^*\left( k \right) = \frac{1}{2}{x^T}\left( k \right)M\left( {k + 1} \right)x\left( k \right)
\end{array} \right.
\end{array}\]

但注意到最佳解並不一定保證系統穩定 (Kalman (1960b, p.113) 指出系統的 optimality 並不保證 stability，亦即 系統採用 optimal control law 並不保證系統閉迴路穩定。)，現在我們將透過以下例子顯示 儘管 $Q >0, R>0$ 且 $N \ge 1$ 所求得的最佳控制力並不保證系統閉迴路穩定。

Example: Finite Horizon LQ control
考慮離散系統 $x(k+1) = Ax(k) + B u(k)$ 且 $y(k) = Cx(k)$；其中
\[A = \left[ {\begin{array}{*{20}{c}}
{4/3}&{ - 2/3}\\
1&0
\end{array}} \right];B = \left[ {\begin{array}{*{20}{c}}
1\\
0
\end{array}} \right];C = \left[ {\begin{array}{*{20}{c}}
{ - 2/3}&1
\end{array}} \right]\]現在考慮  $N=5$ 與 $N=7$ ，試設計 有限維度 LQ 控制器時的最佳解。
Solution
上述系統對應的 轉移函數如下
\[G\left( z \right) = \frac{{ - 2/3z + 1}}{{{z^2} - 4/3z + 2/3}}\]注意到此系統的 開迴路 零點 (zero) 落在 $z = 3/2 = 1.5$；亦即具有 不穩定 零點 ( 因為離散系統穩定範圍落在單位圓 $|z| \le 1$ 之內。) 現在我們利用 LQ 控制器，令參數矩陣 $R:=0.001$ 且 $Q := C^TC = P_f$ 且 $Q \ge 0$ 為半正定矩陣；此時若我們額外再加入一點小擾動
\[\begin{array}{l}
Q: = {C^T}C + 0.001I\\
\begin{array}{*{20}{c}}
{}&{}
\end{array} = \left[ {\begin{array}{*{20}{c}}
{ - 2/3}\\
1
\end{array}} \right]\left[ {\begin{array}{*{20}{c}}
{ - 2/3}&1
\end{array}} \right] + \left[ {\begin{array}{*{20}{c}}
{0.001}&0\\
0&{0.001}
\end{array}} \right]\\
\begin{array}{*{20}{c}}
{}&{}
\end{array} = \left[ {\begin{array}{*{20}{c}}
{4/9}&{ - 2/3}\\
{ - 2/3}&1
\end{array}} \right] + \left[ {\begin{array}{*{20}{c}}
{0.001}&0\\
0&{0.001}
\end{array}} \right]\\
\begin{array}{*{20}{c}}
{}&{}
\end{array} = \left[ {\begin{array}{*{20}{c}}
{4/9 + 0.001}&{ - 2/3}\\
{ - 2/3}&{1.001}
\end{array}} \right]
\end{array}\]接著我們逐步跌代求解最佳控制力序列 $u(4),u(3),u(2),u(1),u(0)$ 如下：
$k = N = 5$，我們有
\[\begin{array}{l}
 \Rightarrow \left\{ \begin{array}{l}
u_{}^*\left( 4 \right) = K\left( 4 \right)x\left( 4 \right),\begin{array}{*{20}{c}}
{}&{}
\end{array}\\
K\left( 4 \right) = \left[ {\begin{array}{*{20}{c}}
{0.1629}&{0.6652}
\end{array}} \right]\\
M\left( 5 \right) = Q\\
V_{}^*\left( 5 \right) = \frac{1}{2}{x^T}\left( 5 \right)Qx\left( 5 \right)
\end{array} \right. \Rightarrow \left\{ \begin{array}{l}
u_{}^*\left( 3 \right) = K\left( 3 \right)x\left( 3 \right),\begin{array}{*{20}{c}}
{}&{}
\end{array}\\
K\left( 3 \right) = \left[ {\begin{array}{*{20}{c}}
{0.1518}&{0.6652}
\end{array}} \right],\\
M\left( 4 \right): = \left[ {\begin{array}{*{20}{c}}
{0.4489}&{ - 0.666}\\
{ - 0.666}&{1.0014}
\end{array}} \right]\\
V_{}^*\left( 4 \right) = \frac{1}{2}{x^T}\left( 4 \right)M\left( 4 \right)x\left( 4 \right)
\end{array} \right.\\
 \Rightarrow \left\{ \begin{array}{l}
u_{}^*\left( 2 \right) = K\left( 2 \right)x\left( 2 \right),\begin{array}{*{20}{c}}
{}&{}
\end{array}\\
K\left( 2 \right) = \left[ {\begin{array}{*{20}{c}}
{0.1257}&{0.6652}
\end{array}} \right]\\
M\left( 3 \right): = \left[ {\begin{array}{*{20}{c}}
{0.4568}&{ - 0.666}\\
{ - 0.666}&{1.0014}
\end{array}} \right]\\
V_{}^*\left( 3 \right) = \frac{1}{2}{x^T}\left( 3 \right)M\left( 3 \right)x\left( 3 \right),
\end{array} \right. \Rightarrow \left\{ \begin{array}{l}
u_{}^*\left( 1 \right) = K\left( 1 \right)x\left( 1 \right),\begin{array}{*{20}{c}}
{}&{}
\end{array}\\
K\left( 1 \right) = \left[ {\begin{array}{*{20}{c}}
{0.0724}&{0.6653}
\end{array}} \right],\\
M\left( 2 \right): = \left[ {\begin{array}{*{20}{c}}
{0.4741}&{ - 0.666}\\
{ - 0.666}&{1.0014}
\end{array}} \right]\\
V_{}^*\left( 2 \right) = \frac{1}{2}{x^T}\left( 2 \right)M\left( 2 \right)x\left( 2 \right)
\end{array} \right.\\
 \Rightarrow \left\{ \begin{array}{l}
u_{}^*\left( 0 \right) = K\left( 0 \right)x\left( 0 \right),\begin{array}{*{20}{c}}
{}&{}
\end{array}\\
K\left( 0 \right) = \left[ {\begin{array}{*{20}{c}}
{ - 0.0256}&{0.6653}
\end{array}} \right],\\
M\left( 1 \right): = \left[ {\begin{array}{*{20}{c}}
{0.5098}&{ - 0.666}\\
{ - 0.666}&{1.0014}
\end{array}} \right]\\
V_{}^*\left( 1 \right) = \frac{1}{2}{x^T}\left( 1 \right)M\left( 1 \right)x\left( 1 \right)
\end{array} \right.
\end{array}\]注意到若我們計算閉迴路系統的極點可得
$$eig(A+ BK_{N=5}(0)) = \{1.307, 0.001\}$$ 亦即 使用此最佳控制力會導致閉迴路系統不穩定 $(z = 1.307 \ge 1)$ 但讀者可自行試驗若改考慮 $N=7$時候，閉迴路極點會變成
\[
eig(A+ BK_{N=7}(0)) = \{0.989, 0.001\}
\]亦即系統被穩定化。若我們考慮 $N=\infty$ 時可以得到
\[
eig(A+ BK_{N=\infty}(0)) = \{0.664, 0.001\}
\]此結果稱作 infinite horizon control law。會在之後再行介紹。

[動態規劃] 淺談 離散時間動態規劃 (1) - Bellman equation in Infinite Horizon

這次要介紹 無窮時間的 Bellman Equation，亦即我們的 Cost function 為 branch cost 加到無窮大的情況：
\[
J(u) := \displaystyle \sum_{k=0}^{\infty} J(x(k), u(k)) + \Phi(x(N))
\]
那麼現在問題變成 儘管我們手上有 有限時間的 Bellman Equation (請參考前篇文章)，但對於此類無窮時間的問題該如何處理??

亦即如果今天我們 cost function 的 $N \rightarrow \infty$ 該怎麼處理? 我們稱這一類問題叫做 Steady State Dynamic Programming 或稱 Dynamic Programming in Infinite Horizon。

無窮時間的動態規劃問題 (Dynamic Programming Problem in Infinite Horizon)：

考慮 Performance index (cost function)
\[
J(u) := \displaystyle \sum_{k=0}^{\infty} J(x(k), u(k)) + \Phi(x(N))
\]狀態方程( state equation)
\[
x(k+1) = f(x(k), u(k)), \ x(0) \ \text{is given} \\
\]其中 $x(k)$ 為系統在第 $k$ 時刻的 狀態，$f:\mathbb{R}^n \times \mathbb{R}^m \rightarrow \mathbb{R}^n$，
與控制力拘束條件
\[
u(k) \in \Omega
\]其中$\Omega$ 為拘束條件

此時對應的 Bellman Equation (or Dynamic Programming Equation) 寫為如下的 functional form
\[
I(x) = \min_{u \in \Omega} \{ J(x,u) + I(f(x,u)) \}
\]亦即與 $N$ 無關
上式稱為 Steady State Bellman Equation 或者 Bellman Equation in Infinite Horizon。


現在我們看個例子：

Example
考慮系統狀態方程表示如下：
\[x(k+1) = x(k) - u(k)
\]且 cost function 為
\[
J(u) = \displaystyle \sum_{k=0}^{\infty} (x(k+1)-u(k))^2 + x^2(k+1)
\]試求 Optimal $u^*$ 與其對應的 optimal cost to go

Solution
首先我們寫下 Steady State Bellman Equation:
\[
I(x) = \min_{u \in \Omega} \{J(x,u) + I(f(x,u)) \}
\]現在我們要求解上式，故我們首先猜一個解 (事實上此問題在之前文章中我們解過有限時間的 Bellman equation，當時我們解出 Optimal cost to go 具有 $I(x) = \alpha x^2$ 的形式，有興趣的讀者可前往前篇文章檢驗)，故我們現在很合理的可以猜這 $I(x) = \alpha x^2$ 其中 $\alpha$ 為代定係數 $\alpha \geq 0$。
故我們將此解代入 Steady State Bellman Equation，可得
\[\begin{array}{l}
I(x) = {\min _{u \in \Omega }}\{ J(x,u) + I(f(x,u))\} \\
 \Rightarrow \alpha {x^2} = {\min _{u \in \Omega }}\{ {(x - u)^2} + {x^2} + \alpha {\left( {x - u} \right)^2}\} \\
 \Rightarrow \alpha {x^2} = {\min _{u \in \Omega }}\{ {x^2} + \left( {1 + \alpha } \right){\left( {x - u} \right)^2}\}
\end{array}
\]由 FONC: $\frac{\partial }{{\partial u}} = 0$，我們可求解上式右方最佳控制力 $u^*$ ：
\[
u^* = x
\]故代回上式我們得到
\[\begin{array}{l}
\alpha {x^2} = {x^2}\\
 \Rightarrow \alpha  = 1
\end{array}
\] 故如果我們選 $\alpha =1$即可解得 Steady-State Bellman Equation。 $\square$

上述問題可以進一步拓展到 $\mathbb{R}^n$ 空間，這一類問題在控制理論中稱為無窮時間的線性二次調節器 (Linear Quadratic Regulator, LQR) 問題，有興趣的讀者請參考：
[最佳控制] 離散時間 穩態線性二次調節器 Discrete Time Linear Quadratic Regulator in Infinite Horizon via Dynamic Programming