謝宗翰的隨筆

考慮離散時間 LTI 系統
\[\begin{array}{l}
x(k + 1) = Ax(k) + w(k)\\
y(k) = Cx\left( k \right) + v(k)
\end{array}\]其中 $x \in \mathbb{R}^n, u \in \mathbb{R}^m, y \in \mathbb{R}^p$ 且 $x(0) = x_0$；

基本狀態估計問題：給定初始估計誤差 ( i.e., $\bar x(0) \neq x_0$) 且 不考慮 雜訊 無外部干擾的情況 ($w(k) = v(k)=0$)，我們想問 $\hat x(k) \to x(k)$ as $k \to \infty$ ?

=================
Theorem: (Convergence of Estimator Cost)
給定無 noise 量測輸出 ${\bf y}( T) = \{Cx(0), CAx(0),..., CA^T x(0)\}$ 則最佳估測器的 cost $V_T^*({\bf y}(T)) $ 在 $T \to \infty$ 時收斂 。
=================

Proof:
由於
\[V_T^{} = \frac{1}{2}\left( \begin{array}{l}
\left| {\hat x\left( 0 \right) - \bar x\left( 0 \right)} \right|_{{{\left( {{P^ - }\left( 0 \right)} \right)}^{ - 1}}}^2\\
\begin{array}{*{20}{c}}
{}&{}
\end{array} + \sum\limits_{k = 0}^{T - 1} {|\hat x\left( {k + 1} \right) - A\hat x\left( k \right)|_{{Q^{ - 1}}}^2}  + \sum\limits_{k = 0}^T {\left| {y\left( k \right) - C\hat x\left( k \right)} \right|_{{R^{ - 1}}}^2}
\end{array} \right)\]故我們分三個步驟證明 $V_T^*({\bf y}(T)) $ 在 $T …

Setpoint Tracking
考慮離散線性系統
\[\left\{ \begin{array}{l}
x\left( {k + 1} \right) = Ax\left( k \right) + Bu\left( k \right)\\
y\left( k \right) = Cx\left( k \right)
\end{array} \right.\] $x \in \mathbb{R}^n, y \in \mathbb{R}^p, u \in \mathbb{R}^m$；一般而言，控制系統中常見的 追蹤(setpoint tracking)問題 (或稱 servo problem) 如下：
給定 setpoint  $y_{sp}$ (e.g., 步階訊號 or 常數)，我們希望在系統達到穩態(steady-state) 之後，系統的輸出 $ y  = y_{sp} $。

若我們考慮給定的 setpoint $y_{sp} = 0$ 則稱此類問題為 regulation problem。

回憶在最佳控制理論中的 LQR 方法給予我們對於線性系統的 regulation 問題提供一組最佳解，故我們的想問是否能將此法應用在 一般的追蹤問題?

答案是肯定的，僅需引入 deviation variable 做基本 座標轉換 即可。

現在我們令 $y_{sp}$ 為 output setpoint，定義系統穩態時候的狀態 與 控制力為 $(x_s, u_s)$ 則線性系統方程可改寫為
\[\left\{ \begin{array}{l}
{x_s} = A{x_s} + B{u_s}\\
{y_s} = C{x_s}
\end{array} \right.\]注意到對於穩態時 我們希望 $y_s = y_{sp}$ 故 我們有 $y_{sp} = C x_s$ 現在我們將上式改寫為矩陣形式
\[\left\{ \begin{array}{l}
{x_s} = A{x_s} + B{u_s}\\
{y_{sp}} = C{x_s}
\end{array} \right. \Rightarrow \left[ {\begin{array}{*{20}{c}}
{I - A}&{ - B}\\
C&0
\end{array}} \right]\left…

以下我們介紹 線性系統理論中關於 控制性的 重要結果
==========================
Lemma: Hautus Lemma for Controllability
一個離散線性系統
\[
x(k+1) = A x(k) + Bu(k)
\]為 controllable 或稱 (A,B) controllable 若且唯若
\[
rank[\lambda I - A\;\; B] =n, \;\; \forall \lambda \in \mathbb{C}
\]其中 $\mathbb{C}$ 表示 任意 complex number 所形成的集合。
==========================
Proof: omitted.
Comment:
1. 上述 Lemma 等價為 矩陣 $[\lambda I - A\;\; B]$ 有 $n$ 個 independent rows (full row rank) 。

2. 也許讀者會好奇為何需要如此多 控制性 檢驗工具? 為什麼不使用 可控性矩陣(controllability matrix) 檢驗法就好? 事實上此 Hautus Lemma 主要是功用是大幅簡化 理論證明，但一般實際 檢驗 系統的 可控性 仍多仰賴  可控性矩陣(controllability matrix) 檢驗法。

3. 注意到上述條件需要檢驗 "任意" complex number $\lambda$ 此條件明顯過於嚴苛。不過我們可以注意到 若 $\lambda $ 並非為 $A$ 矩陣的特徵值，則 $rank[\lambda I - A\;\; B] =n$ 必然成立，故我們只需檢驗 $\lambda$ 為 $A$ 矩陣的特徵值部分即可。故

==========================
Lemma (Modified): Hautus Lemma for Controllability
一個離散線性系統
\[
x(k+1) = A x(k) + Bu(k)
\]為 controllable 或稱 (A,B) controllable 若且唯若
\[
rank[\lambda I - A\;\; B] =n, \;\; \forall \lambda \in eig(A)
\]=========…

回憶離散時間穩態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$ 之後的控制力…

此文章基於前篇 [最佳控制] 線性系統的最佳參數估計 - Kalman Filter (0)；強烈建議讀者先參閱前篇再行閱讀此文。此文將利用 計算 probability density 方式來求解 Kalman filter。


考慮離散時間動態系統
\[
x(k+1) = Ax(k) + w(k);\;\; y(k) = Cx(k) + v(k)
\]且 $w \sim N(0,Q)$ 為製程雜訊 process noise 或稱 干擾 process disturbance；$v \sim N(0,R)$ 為量測雜訊 (measurement noise) ； $x(0) \sim N(\bar x(0), Q(0))$；$x \in \mathbb{R}^n, A \in \mathbb{R}^{n \times n}, C \in \mathbb{R}^{p \times n}, y \in \mathbb{R}^p$，Kalman filter 便是在試圖回答：假設狀態未知我們只能拿到量測輸出，則最佳的狀態估計該是如何?


First Step : $k=0$
假設 初始狀態 $x(0)$ 為 mean $\bar x(0)$ 且 convariance matrix $Q(0)$ 的 normal distrbuted 隨機向量，亦即
\[
x(0) \sim N(\bar x(0), Q(0))
\]接著獲得 初始 (受雜訊污染的) 量測輸出 $y(0)$ 滿足下式
\[
y(0) = C x(0) + v(0)
\]其中 $v(0) \sim N(0, R)$ 為 雜訊 (measurement noise)。

我們的目標：獲得 conditional density $p_{x(0)|y(0)}(x(0) | y(0))$ ，則我們的狀態估計 $\hat x$ 即可透過此 conditional density 求得
$$\hat x := \arg \max_x p_{x(0)|y(0)}(x(0) | y(0))$$

Comments:
1. 若未知 $\bar x(0)$ 或者 $Q(0)$ 則我們通常選 $\bar x(0) :=0$ 且 $Q(0)$ 很大 來表示我們對初始狀態所知甚少 (noninformative prior)。
2. 若量測過…

這次要介紹 Kalman filter 或稱 Optimal Linear State Estmator，以下我們將簡單介紹一些在下一篇文章需要使用的一些結果。

Preliminary
回憶若 $x$ 為 mean $m$ 且 variance $\sigma^2$ 的 Normal 隨機變數 則 我們表示為 $x \sim N(m, \sigma^2)$ 。其 probability density function 可記做
\[
\frac{1}{\sqrt{2 \pi} \sigma} \exp(\frac{-1}{2 \sigma^2}(x-m)^2)
\]現在若我們拓展上述結果到 多個隨機變數 (又稱 random vector 隨機向量) 的情況，令 $x$ 為Normal 隨機向量 記做
\[
x \sim N(m,P); \;\;
p_x(x) := n(x,m,P)
\]上述符號 表示 $x$ 為 normal distributed 且 mean vector $m$ 與 convariance matrix $P$。另外 $n(x,m,P)$ 表示 normal probability density function
\[\;n\left( {x,m,P} \right): = \frac{1}{{{{\left( {2\pi } \right)}^{n/2}}{{\left( {\det P} \right)}^{1/2}}}}\exp \left( { - \frac{1}{2}{{\left( {x - m} \right)}^T}{P^{ - 1}}\left( {x - m} \right)} \right)\]
Comment:
若 $x \in \mathbb{R}^n$ 則 mean vector $m \in \mathbb{R}^n$ 且 convariance matrix $P \in \mathbb{R}^{n \times n}$ 且為 實數 對稱 正定 矩陣 (正定條件用以確保 $P^{-1}$ 存在，使得上述的 probability density function 可以被定義)。若 $P$ 不為對 正定 我們稱為 singular normal distribution 或稱 degenerate norma…