PID制御のチューニング手法にはZiegler Nicholsの限界感度法はじめ様々な手法があるが、これらの手法がどのように異なるか簡単なケースについて調べた。

前提

むだ時間あり1次遅れ系(First-order plus deadtime; FOPDT)のPI制御を対象にチューニング手法のパラメータ比較。特に

\[ \rm{P}(s) = \frac{1}{1+\tau s}e^{-Ls} \]

の $ \tau = 1, L=1 $ のケースについてチューニング手法の結果をプロットする。システムをブロック線図で表すと下図。

ベンチマーク環境

現代制御的な手法はむだ時間なしの制御対象を

\[ \rm{\tilde{P}}(s) = \frac{1}{1+\tau s} \]

と表し、モデル化誤差が含まれたプラント$ \tilde{P} $に対してチューニングした上でロバスト性に期待する前提もおいてみる。

モデル化「誤差」が含まれたシステム

パラメータ観測用おもちゃ

複素関数解析を組み合わせて安定条件を解くむだ時間を持つ1次遅れ系のPI制御パラメータの解析的な分析で求めた安定領域(下図 緑の領域)を示したおもちゃがあるので遊んでいってください。 シミュレーションのタイムステップと積分誤差で境界より少し内側で振動し、境界上で発散するけど許して。

制御応答(左)と制御パラメータ(右)

黒円をマウスでグリグリしてみてね

結果とウンチク

古典制御については過去エントリーの二番煎じ。図に書き加えた $ r, q $ は最適制御の重み付けパラメータ。1状態1入力システムの最適制御に導出経緯を掲載した。

パラメータの比較

$ H_\infty $ ノルム

モデル化誤差を含んだプラントのシステムの相補感度関数 $ \frac{\tilde{P}C}{1+\tilde{P}C} $ すなわち

\[ G(s) = \frac{K_p s + K_i}{s^2 + (K_p + 1)s + K_i} \]

をロバスト制御の考えに基づいて $ \vert G \vert_\infty < 1 $ となる $ K_p, K_i $ の領域を求めた。システムの$ L_2 $ゲイン

\[ \vert G(\omega j) \vert = \sqrt{ \frac{K_p^2 \omega^2 + K_i^2}{(K_i - \omega^2)^2 + (K_p + 1)^2 \omega^2} } \]

の最大値が$ \omega \in \mathbb{R}^+ $ によらず1未満であるのでsolve[{(a^2*x+b^2)/((b-x)^2+(a+1)^2*x)<1, a>0, b>0,x>0}] - Wolfram|Alphaから

\[ 0 < K_i \leq K_p + \frac{1}{2} \]

このままだとプラントの前提知識がうすすぎるのでむだ時間をパデ近似で埋めてノルムを求める方法もある。

強化学習のPolicy Gradient法では、制御則をパラメータ\(\theta\)で表現される方策関数\(u = \pi_{\theta}(x) \)を最適化するコスト関数\(J\)のヤコビアンを使って最急降下法にて

\[ \theta \leftarrow \theta - \alpha \nabla_{\theta} J \]

を繰り返すことにより、最適な(非線形)状態フィードバック制御のパラメータを求める。

(上記は、1.コスト関数の最小化を行う 2. 方策関数はDeterministicとした前提を置いている)

OpenAI Spinning UpのPart 3やLil'LogのPolicy Gradient AlgorithmsにてPolicy Gradientの導出方法が紹介されているが、陽に制御対象のシステムがマルコフ決定過程に従うとしており状態遷移は確率的であるため、制御則\(\pi\)のもと状態\(s_0\)から状態\(s\)まで kステップで変化する遷移確率 \(\rho(s_0 \to s, k) \)をもちいて

\[ \nabla_\theta J = \sum_{s}\sum_{k=0}^\infty \rho^\pi(s_0 \to s, k) \sum_{a \in \mathcal{A}} \nabla_\theta \pi_\theta(a \vert s)Q^\pi(s, a) \]

Deterministic Policy Gradientのケースで、

\[ \nabla_\theta J(\pi_\theta) = \int_{\mathcal{S}} \rho^\pi (s) \nabla_\theta \pi_\theta (s) \nabla_a Q^\pi (s,a) \vert_{a=\pi_\theta (s)} \text{d}s \]

と整理される。以上の特殊な状態として、システムが \(x^+ = f(x, u) \)に従う場合について考えたい。 この場合、ナイーブに遷移確率として\(\delta(x^+ − f(x, a))\)を採用し、計算すればよいのだが、 よくわからなかったので元の定義に立ち返った。

問題設定

システムの違い

制御対象がMDPで遷移が確率的なStochastic Transition Dynamic Systemと、いわゆるシステム方程式で表されるDeterministic Dynamic Systemでの違い

Deterministic	Stohacstic
\( x^+ = f(x, u) \)	\( x^+ \sim p(x^+ \mid x, u) \)
\( V^\pi(f(x, \pi(x))) \)	\(\displaystyle \int_{\mathcal{X}} p(x^+ \mid x, \pi(x)) V^\pi (x^+) \text{d} x^+\)

コスト関数等の定義

名前	数式	説明
初期コスト関数	\(C_{init}(x)\)	RLとMPC/LQRのコスト関数を一致させるために定義. 初期値のみにより最適化の文脈では定数.
ステージ・コスト関数	\(C(u, x^+)\)	強化学習では\(R=-C\)として報酬で定義されることが一般的
終端コスト関数	\(C_f(x^+)\)	強化学習では\(R=-C\)として報酬で定義されることが一般的
制御入力列	\(\mathcal{U}_N = \{u_{0}, u_{1}, \cdots, u_{N-1} \} \)	Time horizonをNとした
コスト関数	\( J_N(\mathcal{U}_N ) = \mathbb{E}_{x \sim \mathcal{D}} [C_{init}(x) + V_N^{\mathcal{U}_N}(x)] \)	Time horizonをNとした
状態価値関数	\( V^{\mathcal{U}}_N(x) = \displaystyle \sum_{i=0}^{N-1} C(u_{i}, x_{i+1}) \\ \quad + C_f(x_{N}) - C(u_{N-1}, x_{N}) \)	ただし、\(x_0=x\). \(x_{k+1} = f(x_k, u_k) \)または\(x_{k+1} \sim p(\cdot \mid x_k, u_k) \)に従う。
行動価値関数	\( Q^{\mathcal{U}}_N(x, u) = C(u, x^+) + V^{\mathcal{U}}_{N-1}(x^+) \)

補足: LQRの場合

LQRの場合、\(N \rightarrow \infty \)であって\(\pi_\theta(x)=\theta \cdot x\)の制御則に対して、正定行列 \(Q, R\)を用いたコスト関数を用いて (行動価値関数の\(Q\)と記号が被ってしまってごめんなさい)

\[ \begin{array}{ll} \displaystyle \min_{\theta} & J(\pi_\theta) = \mathbb{E}_{x_0 \sim \mathcal{D}} [C_{init}(x_0) + \displaystyle \sum_{i=0}^{\infty} C( \pi_{\theta}(x_i), x_{i+1})] \\ {\rm s.t.} & C(u, x) = \displaystyle \frac{1}{2} x^{\intercal} Q x + \displaystyle \frac{1}{2} u^{\intercal} R u \\ & x_{t+1} = A x_t + B u_t \\ \end{array} \]

を求める。こちらの方面の話では方策勾配法でLQRを解くを参照のこと。

補足: MPCの場合

MPCの場合、初期値が与えられ、オンラインで最適化問題を解く。終端コストの定数\(P\)について、LQRのリッカチ代数方程式のリッカチ行列と一致させると制約条件がない下記の問題設定ではLQRと同じ制御入力列が求まる[Mayne et al., 2000]。

\[ \begin{array}{ll} \displaystyle \min_{\mathcal{U}} & V^{\mathcal{U}}(x_0) + C_{init}(x_0) \\ {\rm s.t.} & C(u, x) = \displaystyle \frac{1}{2} x^{\intercal} Q x + \displaystyle \frac{1}{2} u^{\intercal} R u \\ & C_{init}(x) = \displaystyle \frac{1}{2} x^{\intercal} Q x \\ & C_f(x) = \displaystyle \frac{1}{2} x^{\intercal} P x \\ & x_{t+1} = A x_t + B u_t \\ \end{array} \]

方策勾配定理の導出

Step 1

以下はDeterministic Policy Gradient AlgorithmsのAppendix Bからの転記である。記号を先までのシステム方程式と揃えた。 \(\mathcal{U}^{\pi_\theta} = \{\pi_\theta(x_0), \pi_\theta(x_1), \cdots, \pi_\theta(x_{N-1})\}\)とし、 これを適用した状態価値関数を\(V^\pi \)と簡略表記して

\[ \begin{aligned} \nabla_\theta V^\pi &= \nabla_\theta Q^\pi(x, \pi(x)) \\ &= \nabla_\theta \left( \int_{\mathcal{X}} p(x^+ | x, \pi(x)) \left[C(\pi(x), x^+) +V^\pi (x^+) \right] \text{d} x^+ \right) \\ &= \int_{\mathcal{X}} \nabla_{u} p \nabla_{\theta} \pi \left[C(\cdot) + V^\pi(\cdot) \right] \text{d} x^+ + \int_{\mathcal{X}} p(\cdot) \nabla_{u} C(\cdot) \nabla_{\theta} \pi \, \text{d} x^+ + \int_{\mathcal{X}} p(\cdot) \nabla_{\theta} V^\pi \text{d} x^+ \\ &= \nabla_u \left( \int_{\mathcal{X}} p(\cdot) \left[C(\cdot) +V^\pi (\cdot) \right] \text{d} x^+ \right) \nabla_\theta \pi + \int_{\mathcal{X}} p(\cdot) \nabla_{\theta} V^\pi \text{d} x^+ \\ &= \nabla_u Q \cdot \nabla_{\theta} \pi + \int_{\mathcal{X}} p(x^+ | x, \pi(x)) \nabla_{\theta} V^\pi (x^+) \text{d} x^+ \\ \end{aligned} \]

同じことがシステムがDeterministicの場合については\(\nabla_u Q = \nabla_u C + \nabla_x C \cdot \nabla_u f + \nabla_x V \cdot \nabla_u f \)であって、

\[ \begin{aligned} \nabla_\theta V^\pi &= \nabla_\theta \left( C(\pi(x), f(x, \pi(x))) + V^\pi \left(f(x, \pi(x))\right) \right) \\ &= \nabla_u C \cdot \nabla_{\theta} \pi + \nabla_x C \cdot \nabla_{u} f \cdot \nabla_{\theta} \pi + \nabla_x V \cdot \nabla_u f \cdot \nabla_\theta \pi + \nabla_{\theta} V^\pi \left(f(x, \pi(x))\right) \\ &= \nabla_u Q \cdot \nabla_{\theta} \pi + \nabla_{\theta} V^\pi \left(f(x, \pi(x))\right) \\ \end{aligned} \]

ここまでについて一般にディラックのデルタ関数について

\[ \int_{-\infty}^\infty f(t) \delta(t-T) dt = f(T) \]

であるので[Wiki:EN]、\(p(x^+ \mid x, u) = \delta(x^+ − f(x, u)) \)とすると

\[ \int_{\mathcal{X}} p(\cdot) \nabla_{\theta} V^\pi (x^+) \text{d} x^+ = \nabla_{\theta} V^\pi \left(f(x, \pi(x))\right) \]

であって、両者は一致している。

Step 2

さて、この式を繰り返し適用して展開する。Stochasticモデルの場合は状態 \(x\)から\(x^+\)までtステップで遷移する遷移確率\(p(x \rightarrow x^+, t, \pi)\)を用いて

\[ \begin{array}{rcl} \nabla_\theta V^\pi &=& \nabla_u Q \cdot \nabla_{\theta} \pi \\ && + \displaystyle \int_{\mathcal{X}} p(x^+ \mid x, \pi(x)) \nabla_{\theta} V^\pi (x^+) \, \text{d} x^+ \\ &=& \nabla_u Q \cdot \nabla_{\theta} \pi \\ && + \displaystyle \int_{\mathcal{X}} p(x^+ \mid x, \pi(x)) \nabla_u Q \cdot \nabla_{\theta} \pi \, \text{d} x^+ \\ && + \displaystyle \int_{\mathcal{X}} p(x^+ \mid x, \pi(x)) \displaystyle \int_{\mathcal{X}} p(x^{++} \mid x^+, \pi(x^+)) \nabla_{\theta} V^\pi (x^{++}) \text{d} x^{++} \text{d} x^+ \\ &\vdots&\\ &=& \displaystyle \int_{\mathcal{X}} \displaystyle \sum_{t=0}^{N-1} p(x \rightarrow x^+, t, \pi) \nabla_u Q \cdot \nabla_{\theta} \pi \, \text{d} x^+ \\ \end{array} \]

この式の遷移確率をDeterministicなモデルに置き換えると、遷移が確定的に定まるので

\[ \begin{array}{rcl} \nabla_\theta V^\pi &=& \displaystyle \sum_{t=0}^{N-1} \nabla_u Q \vert_{x_k, u_k} \cdot \nabla_{\theta} \pi \vert_{x_k} \\ &=& \displaystyle \sum_{t=0}^{N-1} \left(\nabla_u C \vert_{u_k, x_{k+1}} + \nabla_x C \vert_{u_k, x_{k+1}} \cdot \nabla_u f \vert_{x_k, u_k} + \nabla_x V \vert_{x_{k+1}} \cdot \nabla_u f \vert_{x_k, u_k} \right) \nabla_{\theta} \pi \vert_{x_k} \\ s.t. && x_{k+1} = f(x_k, u_k) \\ && u_{k} = \pi_\theta(x_k) \\ \end{array} \]

となる。

参考

• The iterative Linear Quadratic Regulator algorithm | studywolf
• 現代制御理論
• Differential Dynamic Programming(DDP)/iterative LQR(iLQR)/Sequential LQR(SLQ)
• EE363: Lecture Slides

LQRの問題はコスト関数の最小化問題だが\(u = \theta \cdot x\) とおくと、方策勾配法の要領でコスト関数\(J\)のヤコビアンを使って最急降下法にて

\[ \theta \leftarrow \theta - \alpha \nabla_{\theta} J \]

を繰り返すことにより、最適な状態フィードバック制御のパラメータを求めることができる(できそう)。

問題設定

制御対象の離散システムとして

\[ x_{i+1} = f(x_i, u_i) \quad i \in [0, N] \]

を考える。このシステムにおいて\(u_i = \pi_{\theta}(x_i) \)の制御則を適用した上でコスト関数

\[ J^{\pi_{\theta}}(x_k, u_k; k, N) = \sum_{i=k}^{N-1} L(x_i, u_i) + L_f(x_N) \]

を考え、\(J^{\pi_{\theta}}(x_0, u_0)\)を最小化するパラメータ \(\theta^*\)を求めたい。 ここで、\(L\)はステージコスト、\(L_f\)は終端コストである。確認のため、定義から

\[ J^{\pi_{\theta}}(x_k, u_k; k, N) = L(x_k, u_k) + J^{\pi_{\theta}}(x_{k+1}, u_{k+1}; k+1, N-1) \]

である。以降、簡単のため、単に各関数の添字を省略し\(\pi\)や\(J\)と書く。

コスト関数勾配の導出

状態変数勾配の導出

コスト関数の勾配導出に先立ち、\(\nabla_{\theta} x_k \)を計算する。\(\nabla_{\theta} x_0 = 0\)と\(\nabla_{\theta} u_0 = \nabla_{\theta} \pi (x_0)\)は自明。 以下、\(k \geq 1\)について、

\[ \begin{aligned} \nabla_{\theta} x_k &= \nabla_{x} f \cdot \nabla_{\theta} x_{k-1} + \nabla_{u} f \cdot \nabla_{\theta} u_{k-1} \\ &= \nabla_{x} f \cdot \nabla_{\theta} x_{k-1} + \nabla_{u} f \cdot \nabla_{x} \pi \cdot \nabla_{\theta} x_{k-1} + \nabla_{u} f \cdot \nabla_{\theta} \pi\\ &= \nabla_{u} f \cdot \nabla_{\theta} \pi + (\nabla_{x} f + \nabla_{u} f \cdot \nabla_{x} \pi ) \cdot \nabla_{\theta} x_{k-1} \\ &= \sum_{i=0}^{k-1} \left( \prod_{j=i}^{k-2} \nabla_{x} f \big\vert_{x_{j+1}, \pi(x_{j+1})} + \nabla_{u} f \big\vert_{x_{j+1}, \pi(x_{j+1})} \cdot \nabla_{x} \pi \big\vert_{x_{j+1}} \right) \cdot \nabla_{u} f \big\vert_{x_{i}, \pi(x_{i})} \cdot \nabla_{\theta} \pi \big\vert_{x_{i}} \\ \end{aligned} \]

コスト関数勾配

先ほどと同様\(\nabla_{\theta} u_k = \nabla_{\theta} \pi \vert_{x_k} + \nabla_{x} \pi \vert_{x_k} \cdot \nabla_{\theta} x_{k} \)に注意して、

\[ \begin{aligned} \nabla_{\theta} J \vert_{x_0, \pi(x_0)} &= \nabla_{x} L \vert_{x_0, u_0} \cdot \nabla_{\theta} x_0 + \nabla_{u} L \vert_{x_0, u_0} \cdot \nabla_{\theta} u_0 + \nabla_{\theta} J \vert_{x_1, u_1} \\ &=\displaystyle \sum_{k=0}^{N-1} \Big( \nabla_{x} L \vert_{x_k, u_k} \cdot \nabla_{\theta} x_k + \nabla_{u} L \vert_{x_k, u_k} \cdot \nabla_{\theta} u_k \Big) + \nabla_{\theta} L_f \vert_{x_{N}} \\ &\begin{split} =& \displaystyle \sum_{k=0}^{N-1} \big( \nabla_{x} L \vert_{x_k, u_k} + \nabla_{u} L \vert_{x_k, u_k} \cdot \nabla_{x} \pi \vert_{x_k} \big) \cdot \nabla_{\theta} x_k \\ &+ \displaystyle \sum_{k=0}^{N-1} \nabla_{u} L \vert_{x_k, u_k} \cdot \nabla_{\theta} \pi \vert_{x_k} \\ &+ \nabla_{x} L_f \vert_{x_{N}} \cdot \nabla_{\theta} x_{N} \\ \end{split} \end{aligned} \]

線形時不変(LTI)システムでの検算

拘束条件なしの線形フィードバック問題を例にとって気休め程度に検算する。ただし、\(x \in \mathbb{R}^n, u \in \mathbb{R}^m \)とする。

\[ \begin{aligned} L(x, u) &= \frac{1}{2} x^\intercal Q x + \frac{1}{2} u^\intercal R u \\ L_f(x) &= \frac{1}{2} x^\intercal P x\\ x_{k+1} &= A x_k + B u_k \\ u_k &= - K x_k \end{aligned} \]

このとき、

\[ \begin{aligned} x_{k+1} &= (A-BK) x_{k} \\ &= (A - BK)^k x_0 \\ u_{k} &= -K(A - BK)^{k-1} x_0 \\ \end{aligned} \]

を用いて

\[ \begin{aligned} \nabla_{K} x_{i} &= (i-1)(A - BK)^{i-2} B \sum_{j=0,d=0}^{n, m} \frac{\partial K}{\partial k_{j,d}} x_0 \\ &= \sum_{l=0}^{k-1} (A-BK)^{k-l-2} B \sum_{j=0,d=0}^{n, m} \frac{\partial K}{\partial k_{j,d}} x_l \\ \nabla_{K} J &= \nabla_{K} \left( \sum_{i=0}^{N-1} \frac{1}{2} x_i^{\intercal} \left( Q + K^{\intercal} R K \right) x_i + \frac{1}{2} x_N^\intercal P x_N \right)\\ &= \sum_{i=0}^{N-1} \nabla_{K} x_i^{\intercal} \left( Q + K^{\intercal} R K \right) x_i + \sum_{i=0}^{N-1} \sum_{j=0,d=0}^{n, m} x_i^{\intercal} \frac{\partial K}{\partial k_{j,d}} R K x_i + \nabla_{K} x_N^\intercal P x_N \end{aligned} \]

であるので確かに一致している。

さらに、\(N \rightarrow \infty \)のとき、Global Convergence of Policy Gradient Methods for the Linear Quadratic Regulatorでも示されている通り、

\[ P_K = Q + K^\intercal R K + (A-BK)^\intercal P_K (A-BK) \]

と置くと、

\[ \begin{aligned} \nabla_{K} J &= \big((R+B^\intercal P_K B)K - B^\intercal P_K A \big) \sum_{i=0}^{\infty} x_i^\intercal x_i \end{aligned} \]

と求まる。上記論文ではモデルフリーの自然勾配法との収束速度比較などを行っているので参照すると面白い。

参考

• The iterative Linear Quadratic Regulator algorithm | studywolf
• 現代制御理論
• Differential Dynamic Programming(DDP)/iterative LQR(iLQR)/Sequential LQR(SLQ)

H∞制御の中心的役割を担う小ゲイン定理で求める制御安定(十分)条件はナイキストの安定判別法などから求まる必要十分な安定条件よりも保守的な条件として知られている。

一巡伝達関数

\[ P(s) = \frac{K_p}{\tau s+1}e^{-Ls} \]

で与えられる閉ループ系が安定であるための$K_p$の条件を求めよ

ナイキスト線図を用いて安定条件を求めます。PI制御チューニング : Ziegler Nicholsの限界感度法 - むだ時間ありの1次遅れ系への適用から、

\[ K_p \in \left[0, \sqrt{(\tau \omega_0)^2 + 1}\right) \qquad s.t. \arctan(\tau \omega_0) = \pi - L \omega_0 \]

小ゲイン定理では

\[ \Vert P(j\omega) \Vert_{\infty} < 1 \]

となる$ K_p $の範囲が求める範囲。線形時不変システムでは

\[ \Vert P(j\omega) \Vert_{\infty} = \sup_\omega \vert P(j\omega) \vert < 1 \]

です。これから

\[ \Vert P(j\omega) \Vert_{\infty} = \sup_\omega \left| \frac{K_p}{\sqrt{(\tau \omega)^2 + 1}} \right| = K_p \]

なので

\[ K_p \in [0, 1) \]

ここで、$\omega, \tau > 0$なので明らかに$[0,1)\subset{\left[0, \sqrt{(\tau \omega_0)^2 + 1}\right)}$であって、 保守的な条件になっていることがわかります。

先日、逐次ベイズ推定とカルマンフィルタにて、ベイズ推定とカルマンフィルタの関係を考察したが、「誤差の共分散」が何を表すのか深く考えないでいた。68–95–99.7則から「共分散すなわち $ \sigma $ の多変量版」だから、68%確率円(気象庁｜台風情報の種類と表現方法) くらいに思っていたが、甘かったのでした。

おかげで、カルマンフィルタにおける誤差楕円の計算方法 - MyEnigmaを拝見した時に、意気がったツイートをしてしまったのですが、私のほうが違っていたのでお詫び申し上げます。すみませんでした！！

正規分布と確率

1次元

確率変数Xがスカラーで、1次元の場合を考えます。

\[ X \sim \mathcal{N}(\mu, \sigma^2) \qquad \sigma \in \mathbb{R}^+ \]

このとき、$ X $ が閉区間 $ [\mu -a \sigma, \mu + a \sigma] $ (ただし、$ a \in \mathbb{R}^+ $) に収まる確率は、標準正規分布の累積密度関数 $ \Phi(x) $ を用いて

\[ \text{Pr}(|X - \mu |\leq a \sigma) = \Phi(a) - \Phi(-a) = erf(\frac{a}{\sqrt{2}}) \]

(厳密には半開区間だが、下限の等号の有無は気にしない方が多いようなので…)

あとは標準正規分布表やらerfやらを用いると「$ 3 \sigma $ の範囲に99.7%に収まる」などが求まります。「何が範囲に収まるんですか？」というところについては、 カルマンフィルタの予測ステップなりフィルタステップなりで得られた事後分布 $ \mathcal{N}(\hat{\mu}, \sigma) $ に対して、確信区間という形で「真値が収まる確率」という解釈で良いでしょう。

erfの逆関数を用いる形で任意の確率について、aを求めることができます。

多次元

さて、k次元の場合に移ります。まず、おさらいで、多変量正規分布の確率密度分布は、

\[ f(\mathbf{x}) = \mathcal{N}_k(\textbf{x}|\mathbf{\mu}, \Sigma) =\left(\frac{1}{2\pi}\right)^{k/2}|\Sigma|^{-1/2}\exp\{-\frac{1}{2}(\textbf{x}-\mathbf{\mu})'\Sigma^{-1}(\textbf{x}-\mathbf{\mu})\} \]

のように定義されます。1次元のときと同様に閉区間にあたるものを定義したいですね。 expの中の項に注目すると $ (\textbf{x}-\mathbf{\mu})'\Sigma^{-1}(\textbf{x}-\mathbf{\mu}) $ が目に付きます。 この値が等しいときは確率密度 $ f(\cdot) $ も等しくなるわけですね。 $ \sqrt{(\textbf{x}-\mathbf{\mu})'\Sigma^{-1}(\textbf{x}-\mathbf{\mu})} $ はマハラノビス距離と呼ばれていて、 マハラノビス距離が($ \mathbf{\mu}$から)等距離のベクトルはk次元の楕円の表面上にあります。 ここから、

\[ g(d) = \text{Pr}\{(\textbf{x}-\mathbf{\mu})'\Sigma^{-1}(\textbf{x}-\mathbf{\mu}) \le d\} \]

と、dと確率を結びつける関数g(1次元の例ではerf)を知りたくなります。 さて、これについては証明は参考文献[1]に讓りますが、自由度kのカイ二乗分布を用いて下記の関係があります。

\[ \text{Pr}\{(\textbf{x}-\mathbf{\mu})'\Sigma^{-1}(\textbf{x}-\mathbf{\mu}) \le \chi^2_{k,\alpha}\}=1-\alpha \]

したがって、カイ二乗分布の累積密度関数の逆関数を使えば、k次元の正規分布について確率 $ \alpha $とマハラノビス距離を結び付けられそうですね。 よって、カルマンフィルタで求めた正規分布$ p(x_n \vert Y_n) = \mathcal{N}(x_n \vert \hat{x}_{n|n}, P_{n|n}) , x_n \in \mathbb{R}^k $ などに対しては、 自由度kのカイ二乗分布を使って誤差楕円を求められそうです。 具体的な方法は、こちらの神サイトを御覧ください。カルマンフィルタにおける誤差楕円の計算方法 - MyEnigma

カルマンフィルタ誤差の共分散の意味

というわけで、68–95–99.7則の感覚で意味づけすることを考えると

\[ \text{Pr}\{(x-\hat{x})'P^{-1}(x-\hat{x}) \le n^2 \} \quad n = 1\ or\ 3 \]

などと一般化した上で、 $ P_{t|t} $ (あるいは $ P_{t|t-1} $) の次元に応じて「真値がXX%の確率で収まる確信区間」という意味でした。 XX%の下りは参考に以下を御覧ください(次元の呪いっぽいものも見えます。)

次元	1σ(?)	3σ(?)
1	68.3%	99.7%
2	39.3%	98.9%
3	19.9%	97.1%
4	9.0%	93.9%

というか、ひょっとしてモデルの次元が高まるほど真値が楕円の中心に来る確率は限りなく低くなってくるのか…？

参考

1. N-DIMENSIONAL CUMULATIVE FUNCTION, AND OTHER USEFUL FACTS ABOUT GAUSSIANS AND NORMAL DENSITIES
2. 4.3 - Exponent of Multivariate Normal Distribution | STAT 505
3. カイ２乗分布 - 高精度計算サイト
4. 高次元空間中の正規分布は超球面状に分布する - Qiita

カルマンフィルタは逐次ベイズフィルタの特殊系として言われますが、式を眺めていても結びつきがさっぱりイメージできなかったので数式いじりをしました。 もっとしっかりした入門は他でたくさん見かけますので、 参考文献として記事のリンクしておきます。

数式操作に関しては須山さんの本を参考にしました。

ベイズ推論による機械学習入門

• 作者: 須山敦志
• 出版社/メーカー: 講談社
• 発売日: 2017/10/21

逐次的ベイズ推定から見たカルマン・フィルタ

セオリーと違ってカルマンフィルタで得たい出力値 状態量 $ \hat{x}_{k|k} $ の位置付けから入ります。理解を簡単にするため、システムとしてD次元の酔歩 $ x_k \in \mathbb{R}^D $と 酔歩する粒子の観測誤差を含む観測値 $ y_{k} \in \mathbb{R}^D $ で考察します。

\[ \begin{array}{rll} x_{n+1} &= x_{n} + w_n & \qquad w_n \sim \mathcal{N}(0, Q) \quad Q \in \mathbb{R}^{D \times D} \\ y_{n} &= x_{n} + v_n & \qquad v_n \sim \mathcal{N}(0, R) \quad R \in \mathbb{R}^{D \times D} \\ \end{array} \]

観測値 $ y_n $ は真の平均値のわからない正規分布 $ \mathcal{N}(x_n, R) $ から取り出されたもので、 このn=1のデータからは、せいぜい $ \hat{x}_n = y_n $ とするのが関の山です。 しかし、うまく $ x_{n+1} - x_{n} \sim \mathcal{N}(0, Q) $ と、 観測したデータ $ Y_n = \{Y_{n-1}, y_{n}\} $ を使って、より高い精度で $ x_n $ を推定することを考えます。

カルマンフィルタの式

システムをざっくり簡略化したので、カルマンフィルタの式もザクッと簡単になります。この式は逐次ベイズ推定から求めた結果とあとから比較するために掲載しました。

予測のステップ

\[ \begin{array}{rl} \hat{x}_{k|k-1} &= \hat{x}_{k-1|k-1} \\ P_{k|k-1} &= P_{k-1|k-1} + Q \\ \end{array} \]

フィルタリングのステップ

\[ \begin{array}{rl} e_k &= y_k - \hat{x}_{k|k-1} \\ S_k &= R + P_{k|k-1} \\ K_k &= P_k S_k^{-1} = P_k (R + P_{k|k-1} )^{-1}\\ \hat{x}_{k|k} &= \hat{x}_{k|k-1} + K_k e_k\\ P_{k|k} &= (I - K_k) P_{k|k-1} \\ \end{array} \]

フィルタステップ

観測データ $ Y_n $から状態量 $ x_n $ の確率密度分布 $ p(x_n|Y_n) $ を求めます。確率密度分布にはベイズの定理から下記の性質があります。赤字では $ y_n $ と $ Y_{n-1} $ の独立性(マルコフ性)を用いました。

\[ \begin{array}{rll} p(x_n \vert Y_n) & = p(x_n | y_n, Y_{n-1}) \\ & \propto \color{red}{p(y_n | x_n, Y_{n-1})} p(x_n | Y_{n-1})\\ & = \color{red}{p(y_n | x_n)} p(x_n | Y_{n-1})\\ \end{array} \]

ここで、事前分布として $ p(x_n | Y_{n-1}) = \mathcal{N}(x_n | \hat{x}_{n \vert n-1}, P_{n \vert n-1}) $ をカルマンフィルタでは仮定する。このとき、$ p(y_n \vert x_n) = \mathcal{N}(y_n | x_n, R) $ であるので、

\[ \begin{array}{rll} \ln \left(p(y_n | x_n) p(x_n | Y_{n-1}) \right) &= \ln \mathcal{N}(y_n | x_n, k) + \ln \mathcal{N}(x_n | \hat{x}_{n|n-1}, P_{n|n-1}) \\ &= - \frac{1}{2} \left\{ x_n^T (\color{green}{R^{-1}+P_{n|n-1}^{-1}}) x_n - 2 x_n^T \color{blue}{(R^{-1} y_n + P_{n|n-1}^{-1} \hat{x}_{n|n-1}}) \right\} + \text{const.} \end{array} \]

この数式の形状により $ p(x_n \vert Y_n) $ がガウス分布であることがわかるので (須山本の手法です)、

\[ p(x_n \vert Y_n) = \mathcal{N}(x_n \vert \hat{x}_{n|n}, P_{n|n}) \\ \]

とおくと

\[ \begin{array}{rl} \ln p(x_n \vert Y_n) &= \left( -\frac{1}{2}(x_n - \hat{x}_{n|n})^T P_{n|n}^{-1} (x_n - \hat{x}_{n|n}) - \ln | P_{n|n}| \right) + \text{const}\\ & = -\frac{1}{2} \left( x_n^T \color{green}{P_{n|n}^{-1}} x_n - 2 x_n^T \color{blue}{P_{n|n}^{-1} \hat{x}_{n|n}} \right) + \text{const.} \end{array} \]

ここから各パラメータの比較をします。すると

\[ \begin{array}{crl} &P^{-1}_{n|n} &= \color{green}{R^{-1} + P_{n|n-1}^{-1}}\\ \Leftrightarrow & P_{n|n} &= \left( 1- P_{n|n-1}(P_{n|n-1}+R)^{-1} \right) P_{n|n-1} \\ && = \left( 1- K_n \right) P_{n|n-1} \\ \end{array} \]

と誤差共分散の更新式が求まります。さらに目当ての推定値が

\[ \begin{array}{crl} & P_{n|n}^{-1} \hat{x}_{n|n} &= \color{blue}{ R^{-1} y_n + P_{n|n-1}^{-1} \hat{x}_{n|n-1} }\\ \Leftrightarrow & \hat{x}_{n|n} &= P_{n|n} \left( R^{-1} y_n + P_{n|n-1}^{-1} \hat{x}_{n|n-1} \right) \\ && = \hat{x}_{n|n-1} + P_{n|n-1}(P_{n|n-1}+R)^{-1} (y_n - \hat{x}_{n|n-1}) \\ && = \hat{x}_{n|n-1} + K_n e_n \\ \text{where} & K_n &= P_{n|n-1}(P_{n|n-1}+R)^{-1} \\ & e_n &= y_n - \hat{x}_{n|n-1} \end{array} \]

と求まり、$ K $が、このシステムにおける最適カルマンゲインになっているなど、カルマンフィルタの更新ステップの式になっています。 $ \hat{x}_{n|n} $は、条件付き確率密度分布 $ p(x_n \vert Y_n) $ に従う、$ x_n $ の条件付き期待値 $ \mathbb{E}(x_n|Y_n) $ ですので、 これ自体が最小分散推定になっています(証明は省略. 参考文献[4]参照)。

予測のステップ

先ほどのフィルタリングのステップでは、事前分布として$ p(x_n | Y_{n-1}) = \mathcal{N}(x_n | \hat{x}_{n \vert n-1}, P_{n \vert n-1}) $を用いていました。この事前分布のパラメータ、$ \hat{x}_{n|n-1} $と$ P_{n|n-1} $を求めます。 $ x_{n+1} - x_{n} \sim \mathcal{N}(0, Q) $ であることから、 その1ステップ前のフィルタリングステップで求めた粒子位置に関する確率密度分布 $ p(x_{n-1}|Y_{n-1}) = \mathcal{N}(x_{n-1}|\hat{x}_{n-1|n-1}, P_{n-1|n-1}) $ に対して、 正規分布の加法性から

\[ \begin{array}{rl} \hat{x}_{n|n-1} &= \hat{x}_{n-1|n-1}\\ P_{n|n-1} &= P_{n-1|n-1} + Q\\ \end{array} \]

となります。そのままですね。

初期値

さて、同じようにしてフィルタリングステップ→予測ステップ→フィルタリングと、ステップをどんどんさかのぼっていくと、最初の位置 $ \hat{x}_{0|0} $, そしてその位置に関する共分散 $ P_{0|0} $ に行き着きます。 これはハイパーパラメータというか、えいやと決めるものになります。

システムがもう少し複雑な場合

前節では酔歩を仮定しましたが、一般の線形システムとして、

\[ \begin{array}{rl} x_{n+1} &= A x_n + B u_n + w_n \\ y_n &= C x_n + v_n \end{array} \]

について考えます。このとき、

\[ \begin{array}{rl} p(x_{n+1}|x_{n}) &= \mathcal{N}(A x_n + B u_n, Q) \\ p(y_n|x_n) &= \mathcal{N}(C x_n, R) \end{array} \]

であって、他の確率分布についても正規分布の再生性をふまえて

\[ \begin{array}{rl} x &\sim \mathcal{N}(\mu, \Sigma) \\ \Rightarrow G x &\sim \mathcal{N}(G \mu, G^T \Sigma G) \end{array} \]

に注意すれば、もとのカルマンフィルタの式になるのは自明ですね。

参考文献

1. シンプルなモデルとイラストでカルマンフィルタを直観的に理解してみる - Qiita
2. ベイズの定理からカルマンフィルタを導出する - ssk tech blog
3. データ同化｜カルマンフィルタと尤度 - ari23の研究ノート
4. 知能制御システム学 画像追跡 (3) ― ベイズ推定とパーティクルフィルタ ― 東北大学 大学院情報科学研究科 鏡 慎吾 准教授
5. カルマンフィルタの誤差共分散の意味 - 夜間飛行

ちょっと複雑な運動方程式をCやPythonコード(numba)に落とすとき、Sympyが便利です。Sympyのコード生成にちょっと癖があったのでメモ代わりに残します。 例として倒立振子をモデルにした。

モデル

欲しい式

非線形な運動方程式に対して、

\[ \dot{q} = f(q, u) \]

テーラー展開で一次線形近似した式の赤いところと青いところを計算してくれる式が欲しい。

\[ \dot{q} = \color{red}{f(q_0, u_0)} + \color{blue}{\frac{\partial f}{\partial q} \Big\vert_{q_0 u_0}} q +\color{blue}{ \frac{\partial f}{\partial u}\Big\vert_{q_0 u_0}} u \]

以降簡単のため、

\[ \begin{array}{rl} A_c(q_0, u_0) &= \frac{\partial f}{\partial q} \Big\vert_{q_0 u_0} \\ B_c(q_0, u_0) &= \frac{\partial f}{\partial u}\Big\vert_{q_0 u_0} \\ g(q_0, u_0) &= f(q_0, u_0) \end{array} \]

運動方程式

一般化座標 $ q $を使って、カートの駆動力 $ u $, カート質量 $ M $, 振り子長 $ l $, 振り子先端質量 $ m $ について運動方程式を立てます。

\[ q = [x, \theta, \dot{x}, \dot{\theta}] \]

あとの導出方法等は他所様の記事を引用して終わります

• Sympyを使った運動方程式の導出と線形化 - Qiita

(7節まで、8節はスキップ. あと回転座標のとり方が若干違う)

Sympyモデルの準備

さて、Qiitaの記事ではラグランジアンを使っていましたが、別ルートで導出したので、こちらになります。

import sympy as sy
class cart_pole():
    def gen_rhe_sympy(self):
        g = sy.symbols('g')
        l = sy.symbols('l')
        M = sy.symbols('M')
        m = sy.symbols('m')

        q  = sy.symbols('q:{0}'.format(4))
        qd = q[2:4]
        u  = sy.symbols('u')
        
        I = sy.Matrix([[1, 0, 0, 0], 
                      [0, 1, 0, 0], 
                      [0, 0, M + m, l*m*sy.cos(q[1])], 
                      [0, 0, l*m*sy.cos(q[1]), l**2*m]])
        f = sy.Matrix([
                      qd[0], 
                      qd[1],
                      l*m*sy.sin(q[1])*qd[1]**2 + u,
                      -g*l*m*sy.sin(q[1])])
        return sy.simplify(I.inv()*f)
    
    def gen_lmodel(self):
        mat = self.gen_rhe_sympy()
        q = sy.symbols('q:{0}'.format(4))
        u = sy.symbols('u')
        
        A = mat.jacobian(q)
        #B = mat.jacobian(u)
        B = mat.diff(u)
        
        return A,B

Python

Pythonの場合はlamdifyを使って

c = CartPole()
q = sy.symbols('q:{0}'.format(4))
u = sy.symbols('u')
# calc_rhe(q, u) -> np.array
calc_rhe = sy.lambdify([q,u], c.gen_rhe_sympy())

などとしたほうが自然ですが、numbaに放り込みたいなどの事情があることもあるでしょう。そこでリファレンス通りにやってみます

from sympy.printing.pycode import pycode
c = CartPole()
pycode(c.gen_rhe_sympy())

すると、あんまりありがたくない形の出力が出ます。

'ImmutableDenseMatrix([[q2], [q3], [((1/2)*g*m*math.sin(2*q1) + l*m*q3**2*math.sin(q1) + u)/(M + m*math.sin(q1)**2)], [-(g*(M + m)*math.sin(q1) + (l*m*q3**2*math.sin(q1) + u)*math.cos(q1))/(l*(M + m*math.sin(q1)**2))]])'

• ImmutableDenseMatrixというSympyの型そのままの出力
• sin/cosがmath? numpyがいいな！
• q2, q3じゃなくてq[2], q[3]と出して欲しい

さらに、リファレンスには使い方が詳しくない裏メニューのNumPyPrinterを使うとこうなります

from sympy.printing.pycode import NumPyPrinter
c = cart_pole()
NumPyPrinter().doprint(c.gen_rhe_sympy())

出力

'numpy.array([[q2], [q3], [((1/2)*g*m*numpy.sin(2*q1) + l*m*q3**2*numpy.sin(q1) + u)/(M + m*numpy.sin(q1)**2)], [-(g*(M + m)*numpy.sin(q1) + (l*m*q3**2*numpy.sin(q1) + u)*numpy.cos(q1))/(l*(M + m*numpy.sin(q1)**2))]])'

かなり良くなりましたが、

• q2, q3じゃなくてq[2], q[3]と出して欲しい

が残ります。掘ってみるとSympyの立式で

q = sy.symbols('q:{0}'.format(4))

と定義すると、q[0] が q0 というsympyシンボルとして定義されているところから来ている様子です。

Matrixやindexingを駆使するとできるようなことも書いてある気がしないでもないですが、面倒なのでこうしました。

from sympy.printing.pycode import NumPyPrinter
c = cart_pole()
class NumPyPrinterR(NumPyPrinter):
  def _print_Symbol(self, expr):
      name = super(NumPyPrinter, self)._print_Symbol(expr)

      # 変数名が他とかぶらないのでマッチングせずに頭文字だけで判断
      if name[0] == 'q':
          name = name[0] + '[' + name[1] + ']'
      elif name[0] == 'u':
          name = 'u[0]'
      return name
NumPyPrinterR().doprint(np.squeeze(c.gen_rhe_sympy()))

これでめでたし。

C++11

Pythonで検証したあとは同じようにするだけです。C++ではEigenを使いたいのとImmutableDenseMatrixの処理が定義されていないのでもう一捻りします。

別途

Eigen::Matrix<double, 4, 1> g;
Eigen::Matrix<double, 4, 4> A;
Eigen::Matrix<double, 4, 1> B;

と定義されていることとしましょう

c = cart_pole()
class CXX11CodePrinterR(CXX11CodePrinter):
    def _print_Symbol(self, expr):
        name = super(CXX11CodePrinter, self)._print_Symbol(expr)
        
        if name[0] == 'q':
            name = name[0] + '(' + name[1] + ', 0)'
        elif name[0] == 'u':
            name = 'u(0, 0)'
        return name

print('g << ')
for expr in np.squeeze(c.gen_rhe_sympy()).tolist():
    print(CXX11CodePrinterR().doprint(expr), end=',\n')

A, B = np.squeeze(c.gen_lmodel())

print('\nA << ', end='')
for expr in A:
    print(CXX11CodePrinterR().doprint(expr), end=',\n')

print('\nB << ', end='')
for expr in B:
    print(CXX11CodePrinterR().doprint(expr), end=',\n')

あとはセミコロンとか処理してお終い。

その他

Sympyには他にも

• julia
• Rust

などのプリンタも用意されているそうです！

バネマスダンパ系に対応した状態制御を考えるために、
2状態1入力システムに対する状態フィードバック+積分制御する系を考える
(1状態1入力システムと同様)

バネマスダンパ系プラントを扱ったエントリーと同じM, c, kを使い
$ A\equiv \begin{bmatrix}  0 & 1 \\ -\frac{c}{M}&-\frac{k}{M} \end{bmatrix}\equiv \begin{bmatrix}  0 & 1 \\ -a_1&-a_2 \end{bmatrix}$ ,$ B\equiv  \begin{bmatrix} 0 \\ \frac{b}{M} \end{bmatrix}\equiv  \begin{bmatrix} 0 \\ K \end{bmatrix}$
そして、$ X \equiv \begin{bmatrix} x \\ \dot{x} \end{bmatrix} $とすると、

$ X $を行列のまま扱うと少しわかりにくいので
同伴型[1]に展開してブロック線図を描くと

この時、
\[ \frac{d}{dt} \begin{bmatrix} z \\ x \\ \dot{x} \end{bmatrix} = \begin{bmatrix} 0 & 1 & 0\\ 0 & 0 & 1\\ 0 & -a_1 & -a_2 \end{bmatrix} \begin{bmatrix} z \\ x \\ \dot{x} \end{bmatrix} +\begin{bmatrix} 0 \\ 0 \\ K \end{bmatrix}u \]
この系に対して最適制御入力を求める。
\[J = \int_{0}^{\infty}\left \{ \begin{bmatrix} z & x & \dot{x} \end{bmatrix}\begin{bmatrix} 1 & 0 & 0 \\ 0 & q_1 & 0 \\ 0 & 0 & q_2 \\ \end{bmatrix} \begin{bmatrix} z \\ x \\ \dot{x} \end{bmatrix}+ru^2\right \}dt\]
この方程式を満たす対称正定行列 Pはリカッチ方程式
\[A^T P+PA+Q-PBR^{-1}B^TP=0\]
を満たす。

ただし、
\[P\equiv \begin{bmatrix} X_1 & X_2 & X_3 \\ X_2 & X_4 & X_5 \\ X_3 & X_5 & X_6 \\ \end{bmatrix}, X_1,X_2,X_3,X_4,X_5,X_6>0\]

これを解いていくと
\[\begin{bmatrix} -\frac{K^2 X_3^2}{r}+1 & -\frac{K^2 X_3 X_5}{r}-a_1 X_3 + X_1 & -\frac{K^2 X_3 X_6}{r}-a_2 X_3 + X_2 \\ -\frac{K^2 X_3 X_5}{r}-a_1 X_3 + X_1 & -\frac{K^2 X_5^2}{r}-2 a_1 X_5 + 2 X_2 + q_1 & -\frac{K^2 X_5 X_6}{r}-a_1 X_6 - a_2 X_5 + X_3 + X_4 \\ -\frac{K^2 X_3 X_6}{r}-a_2 X_3 + X_2 & -\frac{K^2 X_5 X_6}{r}-a_1 X_6 - a_2 X_5 + X_3 + X_4 & -\frac{K^2 X_6^2}{r}-2 a_2 X_6 + 2 X_5 +q_2 \\ \end{bmatrix}=0 \]

これの一般解が複雑なので、式で表すと、
\[ X_3 = \frac{\sqrt{r}}{K} \]
\[ X_5 = \frac{K^2 X_6^2}{2r}+a_2X_6-\frac{q_2}{2}\]
\[ \frac{K^6}{4r^3}X_6^4 + \frac{K^4a_2}{r^2}X_6^3 +(\frac{K^2 a_2^2}{r}+\frac{a_1 K^2}{r}+\frac{q_2 K^4}{2r^2})X_6^2\\ +(\frac{K^2a_2q_2}{r}+2a_1 a_2+\frac{2}{\sqrt{r}})X_6 +(-\frac{2\sqrt{r}a_2}{K}-q_1+a_1 q_2)=0\]
$ X_6 $は4次方程式. この解を求めるには…>> 四次方程式の解 - 高精度計算サイト 

そして、$ X_6 $は以下を満たすように選ぶ.
\[ (X_2=) \frac{X_6}{\sqrt{r}}+\frac{a_2 \sqrt{r}}{K} > 0 \]
\[ (X_1=) \frac{K X_4}{\sqrt{r}}+\frac{a_1 \sqrt{r}}{K}>0 \]
\[ (X_5=) \frac{K^2 X_4 X_6}{r}+a_1 X_6 + a_2 X_4 - \frac{\sqrt{r}}{K}>0 \]

最適制御フィードバックゲインは
\[FB=-R^{-1}B^TP=-\frac{K}{r}\begin{bmatrix} X_3 & X_5 & X_6 \end{bmatrix}  \]
この行行列の1項目からPID制御でいうところの$ K_i, K_p, K_d $に相当する．
(この場合、最適制御はPID制御チューニングの方法の一種ともいえます)

[1] : 制御工学 (JSMEテキストシリーズ) など
         

1状態1入力システムに対して定常偏差を補償できるよう
状態フィードバック + 積分制御する系を考えます[1]

\[ \frac{d}{dt}\binom{z}{x}=\begin{bmatrix} 0 & 1\\ 0 & -a \end{bmatrix}\binom{z}{x}+\binom{0}{K}u \]

この系に対して最適制御入力を求める
(※ この時プラントは $ \tau = 1/a $, ゲイン $ 1/a $ の1次遅れ系)

\[J = \int_{0}^{\infty}\left \{ (z, x)\begin{bmatrix} 1 & 0\\ 0 & q \end{bmatrix} \binom{z}{x}+ru^2\right \}dt\]

この方程式を満たす対称正定行列 Pはリカッチ方程式
\[A^T P+PA+Q-PBR^{-1}B^TP=0\]
を満たす。^[1] ($ A\equiv \begin{bmatrix} 0 & 1 \\ 0 & -a \end{bmatrix}$ ,$ B\equiv  \binom{0}{K}$)
ただし、
\[P\equiv \begin{bmatrix} X_1 & X_2\\ X_2 & X_3 \end{bmatrix}, X_1,X_2,X_3>0\]

これを解いていくと、
\[\begin{bmatrix} -\frac{K^2X_2^2}{r}+1 & \frac{K^2X_2X_3}{r}-aX_2+X_1\\ \frac{K^2X_2X_3}{r}-aX_2+X_1 & -\frac{K^2X_3^2}{r}+2X_2-2aX_3+q \end{bmatrix}=0\]
\[P = \frac{1}{K}\begin{bmatrix} \sqrt{a^2r+2r^{\frac{1}{2}}+q} & \sqrt{r} \\ \sqrt{r} & -ar+ \sqrt{a^2r^2+2r^{\frac{3}{2}}+rq} \end{bmatrix}\]
が求まる。

したがって、最適制御フィードバックゲインは
\[FB=-R^{-1}B^TP\\ =\begin{bmatrix} -\frac{1}{\sqrt{r}} & a-\sqrt{a^2+\frac{2}{\sqrt{r}}+\frac{q}{r}} \end{bmatrix}\]
このときPI制御のゲイン $ K_p, K_i $と比較すると以下のように一致する.
\[K_p = - a+\sqrt{a^2+\frac{2}{\sqrt{r}}+\frac{q}{r}}\]
\[K_i = \frac{1}{\sqrt{r}}\]

サーボ系で考える場合、目標の与え方によってI+P制御となったり、PI制御となったり違いが出ます．

1. 通常の状態フィードバック(I+P制御相当):
\[u=\begin{bmatrix} K_i & K_p \end{bmatrix}\left(\binom{r}{0}-\binom{z}{x}\right)\]

2. PI制御相当(のはず):
\[u=\begin{bmatrix} K_i & K_p \end{bmatrix}\left(\binom{r}{r}-\binom{z}{x}\right)\]

参考

[1]: Integral action in state feedback control - Prof. Alberto Bemporad
        http://cse.lab.imtlucca.it/~bemporad/teaching/ac/pdf/08-integral-action.pdf

[2] : 制御工学 (JSMEテキストシリーズ) など