LQRが代数的Riccati方程式の解を用いて表されることの証明及び，安定性について

はじめに

現代制御を勉強していると，LQRが代数的Riccati方程式の正定解を用いて表されることを勉強すると思います(私も学部生の時に勉強しました)．なぜ代数的Riccati方程式の解で表されるのかと考え，証明を見ると，代数的Riccati方程式ありきで，証明されているものが多い印象を受けました(私個人の感想で恐縮ですが...)．私としては，結果的に代数的Riccati方程式で表されるという説明の方がしっくりきます．今回は，自分向けのメモを兼ねて，ここに記述させて頂きます．間違いがあれば，教えて頂けると幸いです．

前提条件

対象とするシステムの状態・観測方程式は，式(1)，(2)で記述されるとします．

\begin{align}
    \dot{\boldsymbol{x}} &=  \boldsymbol{A}\boldsymbol{x} + \boldsymbol{B}\boldsymbol{u} \tag{1}\\ 
   \boldsymbol{y} &=  \boldsymbol{C}\boldsymbol{x}\tag{2}\\
\end{align}

ここで，以下二つを仮定します．

$\textbf{仮定}$1：$(\boldsymbol{A},\boldsymbol{B})$は可制御である．
$\textbf{仮定}$2：$(\boldsymbol{C},\boldsymbol{A})$は完全可観測である．つまり$\boldsymbol{C} = \boldsymbol{I}$である．

問題設定

次に，制御器の定式化を行います．ここでは，線形出力フィードバック(FB)制御を考えます．

\begin{align}
   \boldsymbol{u} &=  -\boldsymbol{K}\boldsymbol{y}\tag{3}\\
                  &=  -\boldsymbol{K}\boldsymbol{x}\tag{4}
\end{align}

したがって，自律系の式(5)となります．

\begin{align}
   \dot{\boldsymbol{x}} &= (\boldsymbol{A}-\boldsymbol{B}\boldsymbol{K})\boldsymbol{x}  \tag{5}
\end{align}

式(5)の自律系が安定となる条件の下で，最適となる制御器を見つけることが目標です．
式(5)の自律系が安定となる条件は，リアプノフの安定性定理より，$\forall \boldsymbol{Q'}= \boldsymbol{Q'}^T\succ 0$に対して，式(6)の線形リアプノフ方程式が唯一の正定値対称解$\boldsymbol{P} = \boldsymbol{P}^T\succ 0$を有することとして記述できます．

\begin{align}
   \boldsymbol{P}(\boldsymbol{A}-\boldsymbol{B}\boldsymbol{K})+(\boldsymbol{A}-\boldsymbol{B}\boldsymbol{K})^T\boldsymbol{P} + \boldsymbol{Q'} = \boldsymbol{0}  \tag{6}
\end{align}

また，何をもって最適であるのかを定めるために，評価関数を式(7)で定義します．

\begin{align} 
        J = \int_{0}^{\infty} 
(\boldsymbol{y}^{T}\boldsymbol{Q}\boldsymbol{y} +\boldsymbol{u}^{T}\boldsymbol{R}\boldsymbol{u})dt\tag{7}
\end{align}

ただし，$\boldsymbol{Q}$,$\boldsymbol{R}$は正定値対称行列です．以上から，上記のFB制御器を次の最適化問題を解くことで制御器を設計します．

\begin{align}
\tag{8}
\begin{split}
   \mathrm{min~~~} &  J \\
   \mathrm{s.t.~~~} &  \boldsymbol{P}(\boldsymbol{A}-\boldsymbol{B}\boldsymbol{K})+(\boldsymbol{A}-\boldsymbol{B}\boldsymbol{K})^T\boldsymbol{P} + \boldsymbol{Q'} = \boldsymbol{0} 
\end{split}
\end{align}

最適化問題の置き換え

最適化問題(8)を解きやすい形に置き換えをすることを考えます．観測方程式(2)とFB制御器(4)を式(7)に代入し整理すると，次式を得ます．

\begin{align} 
        J = \int_{0}^{\infty} 
\boldsymbol{x}^{T}
(\boldsymbol{Q}+\boldsymbol{K}^T\boldsymbol{R}\boldsymbol{K})
\boldsymbol{x} dt\tag{9}
\end{align}

ここで，$\boldsymbol{x}^{T}
(\boldsymbol{Q}+\boldsymbol{K}^T\boldsymbol{R}\boldsymbol{K})
\boldsymbol{x}=\boldsymbol{y}^{T}\boldsymbol{Q}\boldsymbol{y} +\boldsymbol{u}^{T}\boldsymbol{R}\boldsymbol{u}\succ 0$であることから，$
\boldsymbol{Q}+\boldsymbol{K}^T\boldsymbol{R}\boldsymbol{K}$は正定値対称行列です．そこで，$\boldsymbol{Q'}=\boldsymbol{Q}+\boldsymbol{K}^T\boldsymbol{R}\boldsymbol{K}$として，評価関数を整理します．

\begin{align} 
        J 
&= \int_{0}^{\infty} 
\boldsymbol{x}^{T}
(\boldsymbol{Q}+\boldsymbol{K}^T\boldsymbol{R}\boldsymbol{K})
\boldsymbol{x} dt\\
& = \int_{0}^{\infty} \boldsymbol{x}^{T}\boldsymbol{Q}'\boldsymbol{x} dt\\
& = -\int_{0}^{\infty} \boldsymbol{x}^{T}
\left\{
 \boldsymbol{P}(\boldsymbol{A}-\boldsymbol{B}\boldsymbol{K})+(\boldsymbol{A}-\boldsymbol{B}\boldsymbol{K})^T\boldsymbol{P} 
\right\}
\boldsymbol{x} dt~~~~~\because (6)\\
& =  -\int_{0}^{\infty} 
\left\{
 ((\boldsymbol{A}-\boldsymbol{B}\boldsymbol{K})\boldsymbol{x})^{T}\boldsymbol{P}\boldsymbol{x}
+
\boldsymbol{x}^{T}
\boldsymbol{P}(\boldsymbol{A}-\boldsymbol{B}\boldsymbol{K})\boldsymbol{x})
\right\}
dt\\
& =  -\int_{0}^{\infty} 
\left(
\dot{\boldsymbol{x}}^{T}\boldsymbol{P}\boldsymbol{x}
+
\boldsymbol{x}^{T}\boldsymbol{P}\dot{\boldsymbol{x}}
\right)
dt~~~~~\because (5)\\
&=  -\int_{0}^{\infty}
\frac{d} {dt}
\left(
{\boldsymbol{x}}^{T}\boldsymbol{P}\boldsymbol{x}
\right)
dt \\
&=  {\boldsymbol{x}}(0)^{T}\boldsymbol{P}\boldsymbol{x}(0)~~~~~\because \boldsymbol{A}-\boldsymbol{B}\boldsymbol{K}:\text{stable}\Rightarrow
\lim_{t\to\infty}\boldsymbol{x}(t) = \boldsymbol{0}\\
\end{align}

したがって，$J = {\boldsymbol{x}}(0)^{T}\boldsymbol{P}\boldsymbol{x}(0)$として，表されます．ここで，各初期値に対して評価関数を最適化するのではなく，初期値が確率ベクトルとして考え，$E[J]$を最適化することを考えます．ここでは，初期値$\boldsymbol{x}(0)$が正規分布に従う確率ベクトル，つまり$E[\boldsymbol{x}(0)] = \boldsymbol{0}$かつ，$E[\boldsymbol{x}(0)\boldsymbol{x}(0)^T] = \boldsymbol{I}$であるとします．
これより，

\begin{align}
    E[J] &= E[{\boldsymbol{x}}(0)^{T}\boldsymbol{P}\boldsymbol{x}(0)]\\
         &= E[\mathrm{trace}(\boldsymbol{P}\boldsymbol{x}(0){\boldsymbol{x}}(0)^{T})]\\
         &= \mathrm{trace}(E[\boldsymbol{P}\boldsymbol{x}(0){\boldsymbol{x}}(0)^{T})])\\
         &= \mathrm{trace}(E[\boldsymbol{P}]E[\boldsymbol{x}(0){\boldsymbol{x}}(0)^{T})])\\
         &= \mathrm{trace}\boldsymbol{P}\tag{10}
\end{align}

次に拘束条件を次式で変換します．

\begin{align}
\tag{11}
\begin{split}
\mathrm{trace}({\boldsymbol{P}(\boldsymbol{A}-\boldsymbol{B}\boldsymbol{K})+(\boldsymbol{A}-\boldsymbol{B}\boldsymbol{K})^T\boldsymbol{P} }+\boldsymbol{Q}+\boldsymbol{K}^T\boldsymbol{R}\boldsymbol{K} ) = 0
\end{split}
\end{align}

$\mathrm{trace}$で表したのは，後で$\boldsymbol{K}$で評価関数と拘束条件式を，微分するからです．以上より，最適化問題を式(12)で置き換えます．

\begin{align}
\tag{12}
\begin{split}
   \mathrm{min~~~} &  \mathrm{trace}\boldsymbol{P} \\
   \mathrm{s.t.~~~} &  \mathrm{trace}(
{\boldsymbol{P}(\boldsymbol{A}-\boldsymbol{B}\boldsymbol{K})+
(\boldsymbol{A}-\boldsymbol{B}\boldsymbol{K})^T\boldsymbol{P} }+\boldsymbol{Q}+\boldsymbol{K}^T\boldsymbol{R}\boldsymbol{K} ) = 0
\end{split}
\end{align}

最適化問題(12)を解く

最適化問題(8)は等式制約付き最適化問題です．これをラグランジュの未定乗数法を用いて解きます．未定乗数を$\lambda$とおき，$f(\boldsymbol{K},\lambda)$を次式で定めます．

\begin{align}
\tag{13}
\begin{split}
f(\boldsymbol{K},\lambda)=&  \mathrm{trace}\boldsymbol{P} 
                  &+  \lambda\mathrm{trace}({\boldsymbol{P}
(\boldsymbol{A}-\boldsymbol{B}\boldsymbol{K})+
(\boldsymbol{A}-\boldsymbol{B}\boldsymbol{K})^T\boldsymbol{P} }+\boldsymbol{Q}+\boldsymbol{K}^T\boldsymbol{R}\boldsymbol{K} ) 
\end{split}
\end{align}

$\frac{\partial f}{\partial \lambda}$は次式で計算されます．

\begin{align}
\frac{\partial f}{\partial \lambda}&=0\\
\Leftrightarrow \mathrm{trace}({\boldsymbol{P}
(\boldsymbol{A}-\boldsymbol{B}\boldsymbol{K})
+(\boldsymbol{A}-\boldsymbol{B}\boldsymbol{K})^T\boldsymbol{P} }+\boldsymbol{Q}+\boldsymbol{K}^T\boldsymbol{R}\boldsymbol{K} )&=0
\end{align}

次に，$\frac{\partial f}{\partial \boldsymbol{K}}$を計算します．

\begin{align}
\frac{\partial f}{\partial \boldsymbol{K}}
&=\boldsymbol{B}^T\boldsymbol{P}+\boldsymbol{B}^T\boldsymbol{P}^T
  -\boldsymbol{K}(\boldsymbol{R}+\boldsymbol{R}^T)\\
&=2(\boldsymbol{B}^T\boldsymbol{P}-\boldsymbol{K}\boldsymbol{R})
~~~\because \boldsymbol{P} =\boldsymbol{P}^T,~\boldsymbol{R} =\boldsymbol{R}^T\\
&=\boldsymbol{0}\\
\Leftrightarrow
\boldsymbol{K}&=\boldsymbol{R}^{-1}\boldsymbol{B}^T\boldsymbol{P}\tag{13}
\end{align}

これにより，カルマンゲインが得られました．式(13)をリアプノフ方程式(6)に代入します．

\begin{align}
&{\boldsymbol{P}(\boldsymbol{A}
-\boldsymbol{B}\boldsymbol{K})
+(\boldsymbol{A}-\boldsymbol{B}\boldsymbol{K})^T\boldsymbol{P} }+\boldsymbol{Q}+\boldsymbol{K}^T\boldsymbol{R}\boldsymbol{K} =\boldsymbol{0}\\
%%
\Leftrightarrow&
{\boldsymbol{P}
(\boldsymbol{A}-\boldsymbol{B}\boldsymbol{R}^{-1}\boldsymbol{B}^T\boldsymbol{P})+
(\boldsymbol{A}-\boldsymbol{B}\boldsymbol{R}^{-1}\boldsymbol{B}^T\boldsymbol{P})^T\boldsymbol{P} }+\boldsymbol{Q}+
(\boldsymbol{R}^{-1}\boldsymbol{B}^T\boldsymbol{P})^T
\boldsymbol{R}\
(\boldsymbol{R}^{-1}\boldsymbol{B}^T\boldsymbol{P})  = \boldsymbol{0}\\
%%
\Leftrightarrow&
{\boldsymbol{P}\boldsymbol{A}+\boldsymbol{A}^T\boldsymbol{P} }
+\boldsymbol{Q}
-\boldsymbol{P}\boldsymbol{B}\boldsymbol{R}^{-1}\boldsymbol{B}^T\boldsymbol{P}\tag{14}
 = \boldsymbol{0}
\end{align}

よって，代数的Riccati方程式(14)を得ました．代数的Riccati方程式の解法は主に，以下の二つです．
・Riccatiの微分方程式を逆時間で数値的に解く(ただし$\boldsymbol{P}(\infty)=\boldsymbol{0}$)
・有本ポッターの方法(ハミルトン行列の固有ベクトルの特性を利用する方法)
上記の解法の詳細は「@taka_horibe」さんの記事，もしくは解説論文を参照してください．(リンクは下に貼っておきます．)
https://qiita.com/taka_horibe/items/5d053cdaaf4c886d27b5
https://www.jstage.jst.go.jp/article/sicejl1962/35/5/35_5_386/_pdf

LQRの安定性

代数的Riccati方程式から，ナイキスト安定性について考察します．まず，式(14)を下記式(15)に変形します．

\begin{align}
\boldsymbol{P}(j\omega\boldsymbol{I}-\boldsymbol{A})+
(-j\omega\boldsymbol{I}-\boldsymbol{A}^T)\boldsymbol{P} 
-\boldsymbol{Q}
+\boldsymbol{P}\boldsymbol{B}\boldsymbol{R}^{-1}\boldsymbol{B}^T\boldsymbol{P}
 = \boldsymbol{0}\tag{15}
\end{align}

左から，$\boldsymbol{B}^T(-j\omega\boldsymbol{I}-\boldsymbol{A}^T)^{-1}$を乗じ，式(16)を得ます．

\begin{align}
\boldsymbol{B}^T(-j\omega\boldsymbol{I}-\boldsymbol{A}^T)^{-1}\boldsymbol{P}(j\omega\boldsymbol{I}-\boldsymbol{A})
&+\boldsymbol{B}^T\boldsymbol{P} 
-\boldsymbol{B}^T(-j\omega\boldsymbol{I}-\boldsymbol{A}^T)^{-1}\boldsymbol{Q}\\
&+\boldsymbol{B}^T(-j\omega\boldsymbol{I}-\boldsymbol{A}^T)^{-1}\boldsymbol{P}\boldsymbol{B}\boldsymbol{R}^{-1}\boldsymbol{B}^T\boldsymbol{P}
 = \boldsymbol{0}\tag{16}
\end{align}

右から，$(j\omega\boldsymbol{I}-\boldsymbol{A})^{-1}\boldsymbol{B}$を乗じ，式(17)を得ます．

\begin{align}
\boldsymbol{B}^T(-j\omega\boldsymbol{I}-\boldsymbol{A}^T)^{-1}\boldsymbol{P}\boldsymbol{B}
&+\boldsymbol{B}^T\boldsymbol{P}(j\omega\boldsymbol{I}-\boldsymbol{A})^{-1}\boldsymbol{B} \\
&-\boldsymbol{B}^T(-j\omega\boldsymbol{I}-\boldsymbol{A}^T)^{-1}\boldsymbol{Q}(j\omega\boldsymbol{I}-\boldsymbol{A})^{-1}\boldsymbol{B}\\
&+\boldsymbol{B}^T(-j\omega\boldsymbol{I}-\boldsymbol{A}^T)^{-1}\boldsymbol{P}\boldsymbol{B}\boldsymbol{R}^{-1}\boldsymbol{B}^T
\boldsymbol{P}(j\omega\boldsymbol{I}-\boldsymbol{A})^{-1}\boldsymbol{B}
 = \boldsymbol{0}\tag{17}
\end{align}

ここで，$\boldsymbol{R}\boldsymbol{K}=\boldsymbol{B}^T\boldsymbol{P}$を代入します．

\begin{align}
\boldsymbol{B}^T(-j\omega\boldsymbol{I}-\boldsymbol{A}^T)^{-1}\boldsymbol{K}^T\boldsymbol{R}
&+\boldsymbol{R}\boldsymbol{K}(j\omega\boldsymbol{I}-\boldsymbol{A})^{-1}\boldsymbol{B} \\
&-\boldsymbol{B}^T(-j\omega\boldsymbol{I}-\boldsymbol{A}^T)^{-1}\boldsymbol{Q}(j\omega\boldsymbol{I}-\boldsymbol{A})^{-1}\boldsymbol{B}\\
&+\boldsymbol{B}^T(-j\omega\boldsymbol{I}-\boldsymbol{A}^T)^{-1}
\boldsymbol{K}^T\boldsymbol{R}\boldsymbol{K}
(j\omega\boldsymbol{I}-\boldsymbol{A})^{-1}\boldsymbol{B}
 = \boldsymbol{0}\tag{18}
\end{align}

式(18)を整理し，式(19)を得ます．

\begin{align}
&\left[\boldsymbol{I}+\boldsymbol{B}^T(-j\omega\boldsymbol{I}-\boldsymbol{A}^T)^{-1}
\boldsymbol{K}^T\right]
\boldsymbol{R}
\left[\boldsymbol{I}+\boldsymbol{K}
(j\omega\boldsymbol{I}-\boldsymbol{A})^{-1}\boldsymbol{B}\right]\\
 &= \boldsymbol{R}
+\left[(j\omega\boldsymbol{I}-\boldsymbol{A})^{-1}\boldsymbol{B}\right]^T
\boldsymbol{Q}
\left[(j\omega\boldsymbol{I}-\boldsymbol{A})^{-1}\boldsymbol{B}\right]
\tag{19}
\end{align}

したがって，式(20)が成り立ちます．

\begin{align}
\tag{20}
\begin{split}
\left|\left|\boldsymbol{I}+\boldsymbol{H}(j\omega)\right|\right|&\ge 1\\
\boldsymbol{H}(s) &= \boldsymbol{K}
(s\boldsymbol{I}-\boldsymbol{A})^{-1}\boldsymbol{B}
\end{split}
\end{align}

式(20)から安定性について考察します．簡単のため，SISOの場合を用いて説明します．この時のナイキスト線図について，一巡伝達関数$H(j\omega)$のベクトル軌跡が，点$(-1,0)$を中心とする半径1の円内を通らないことを意味します．これより，最適レギュレータはゲイン余裕が$\infty$であり，位相余裕が$60$[deg]以上であるという安定性を有していることが分かります．

[参考]
上図は，以下のコードで強引に作りました．

Nyquist_Diagram.mlx

t = linspace(0,2*pi,100);
figure
plot(sin(t),cos(t),"blue")
text(-0.85,0.13,'60[deg]','Interpreter','latex','FontSize',11)
text(-0.85,-0.13,'-60[deg]','Interpreter','latex','FontSize',11)
hold on
plot(sin(t)-1,cos(t),"red")
hold on
plot(linspace(-1,0,100),sqrt(3)*(linspace(-1,0,100) + 1),"black")
hold on
plot(linspace(-1,0,100),-sqrt(3)*(linspace(-1,0,100) + 1),"black")
hold on
plot(linspace(-2.5,1.5,100),0*linspace(-2.5,1.5,100),"black")
axis([-2.5,1.5,-1.5,1.5]) 
grid on
h = gca;
h.XAxis.FontSize = 14;
h.YAxis.FontSize = 14;
xlabel('Real Part','FontSize',16,'Interpreter','latex')
ylabel('Imaginary Part','FontSize',16,'Interpreter','latex')
title('Nyquist Diagram','FontSize',16,'Interpreter','latex')
saveas(gcf,'Nyquist_Diagram.png')

MATLABによるLQRのシミュレーション

水平面内の船の運動をLQRによって，制御することを考えます．水平面内の船の運動方程式は次式で記述されます．

\begin{align}
\begin{split}
\frac{d}{dt}\beta&=\frac{Y_\beta}{(m+m_y)V}(\beta+\beta_g)+\frac{Y_r-(m+m_x)V}{(m+m_x)V}r+\frac{Y_\delta}{(m+m_y)V}\delta\\
\frac{d}{dt}r&=\frac{N_\beta+(m_x-m_y)V^2}{I_{zw}+J_z}(\beta+\beta_g)+\frac{N_r}{I_{zw}+J_z}r+\frac{N_\delta}{I_{zw}+J_z}\delta\\
\frac{d}{dt}\psi&=r
\end{split}
\end{align}

状態変数と入力変数の意味は以下となります．

\begin{align}
\begin{split}
\beta&:\text{横滑り角 [rad]}\\
    r&:\text{Yaw角速度 [rad/s]}\\
 \psi&:\text{Yaw角 [rad]}\\
\delta&:\text{舵角[rad]}\\
\beta_g&:\text{潮汐による横滑り角[rad]} 
\end{split}
\end{align}

また，各パラメータの意味と値は，以下となります.

\begin{align}
L&=50[\rm m]~~~~\text{船体長さ}\\
B&=8.2[\rm m]~~~~\text{横幅}\\
d&=4.0[\rm m]~~~~\text{水面下喫水}\\
m&=20\times1000[\rm kg]~~~~\text{質量}\\
C_b&=0.6~~~~\text{ブロック係数}\\
c_r&=4[\rm m]~~~~\text{舵の幅}\\
h_r&=2[\rm m]~~~~\text{舵の高さ}\\
V&=20\times0.514444[\rm m/s]~~~~\text{速度　ノット数→秒速}\\
\rho&=999[\rm kg/m^3]~~~~\text{水の密度}\\
S_R&=c_r h_r[\rm m^2]~~~~\text{舵面積}\\
A_R&=h_r h_r/SR~~~~\text{舵のアスペクト比}\\
S&=L d[\rm m^2]~~~~\text{水面下則面積}\\
A&=2d/L~~~~\text{側面積のアスペクト比}\\
Vol&=C_b L B d[\rm m^3]~~~~\text{容積}\\
I_{zw} &= (L^2 + B^2)  \rho  Vol / 20[\rm kg\cdot m^2]~~~~\text{回転楕円体を水で置き換えた時のz軸周りの慣性モーメント}\\
Y_\delta &= \frac{1}{2}  \rho V^2  SR  \frac{6.13A_R}{A_R+2.25}\\
N_\delta &= -Y_\delta  \frac{L}{2}\\ %舵が発生するモーメント}\\
Y_\beta& = -\frac{1}{2} \rho V^2 S (\frac{\pi A}{2} + 1.4 C_b \frac{B}{L}) - Y_\delta\\
Y_r &= \frac{1}{2}\rho V^2 S\frac{\pi A}{4} + Y_\delta  \frac{L}{2 V}\\
N_\beta &= -\frac{1}{2} \rho V^2 S L A + Y_\delta\frac{L}{2 V}\\
N_r &= -\frac{1}{2}\rho V^2SLA(0.54-A) - Y_\delta\frac{L^2}{4V}\\
m_x &=  \rho  Vol\left(\frac{0.5}{L/B} - 0.03\right)~~~~\text{付加質量の}x\text{成分}\\ 
m_y &= \rho Vol\left(0.0012\left(\frac{L}{B}\right)^3 - 0.0285\left(\frac{L}{B}\right)^2 + 0.2294\left(\frac{L}{B}\right) + 0.3127\right) ~~~~\text{付加質量の}y\text{成分}\\
m_z &= m_y~~~~\text{付加質量の}z\text{成分}\\
J_z &=  Izw\left(0.0014\left(\frac{L}{B}\right)^3 - 0.0378\left(\frac{L}{B}\right)^2 + 0.3567\left(\frac{L}{B}\right) - 0.3328\right) ~~~~\text{付加慣性モーメント}\\
I_z &= \frac{(L^2 + B^2)m}{12} \text{慣性モーメント}
\end{align}

詳細な説明は下記リンクの書籍を参照してください．
https://www.amazon.co.jp/gp/product/4782841051/ref=ppx_yo_dt_b_asin_title_o00_s00?ie=UTF8&psc=1

今回は，全状態が完全可観測であるとします．また，可制御性については，可制御性行列を計算することにより確認することができます(ここでは，割愛させてもらいます)．今回は，下記のプログラムを用いました．このプログラムでは，状態方程式の全状態を原点で漸近安定させる制御器を，LQRによって設計し，時間応答をプロットします．ODEの解法は4次ルンゲクッタ法を用いています．カルマンゲインの算出が$\textbf{lqr}$によってできてしまうなんて，MATLABは便利だなと再認識させられます．ちなみにソルバーの中身の代数的Riccati方程式の解は，有本ポッターの方法で計算されています．

LQR.mlx

% Set the simulation time
initial_time = 0;
final_time = 2e1;

% Define state value
n=3; % dim(state)
M=1; % dim(input)
N=final_time*1e2; % sampling number
dt = (final_time-initial_time)/N;
x = zeros(N,n); % state vector
u = zeros(N,M); % input vector
x(1,:) = [0 ;1.0; 0]; % initial condition
time = zeros(N,1);

% Caliculate Kalman filter and LQR
run('para_vessel');%parameter for vessel
run('para_system_vessel');%parameter for plant matrix
% Weight matrix
Q_lq = [10 ,0  ,0;
        0  ,0  ,0;
        0  ,0  ,50];
R_lq = 1;
% caliculate LQR
[K,S,E]=lqr(matA,matB,Q_lq, R_lq);
% Combine Kalmanfilter and LQR
Ab = matA-matB*K ;

% Solve ODE by Runge-kutta method
for i=1:N
time(i) = initial_time + dt * i;
u(i) = 0.1*randn;
p1 = xdtLQR(x(i,:)',u(i),Ab,matG);
p2 = xdtLQR(x(i,:)'+dt/2*p1,u(i),Ab,matG);
p3 = xdtLQR(x(i,:)'+dt/2*p2,u(i),Ab,matG);
p4 = xdtLQR(x(i,:)'+dt*p3,u(i),Ab,matG);
x(i+1,:) = x(i,:)' + dt/6 * ( p1 + p2*2 + p3*2 + p4);
end

% Visualize
figure
plot(time,x(1:N,1))
hold on
plot(time,x(1:N,2))
hold on
plot(time,x(1:N,3))
xlabel('Time [sec]','Fontsize',14,'Interpreter','latex');
ylabel('State','Fontsize',14,'Interpreter','latex');
legend('$\beta$ [rad]','$r$ [rad/s]','$\psi$ [rad]','Fontsize',14,'Interpreter','latex');
title('Time response','Fontsize',18,'Interpreter','latex')
grid on;
h_axis = gca;
h_axis.XAxis.FontSize = 14;
h_axis.YAxis.FontSize = 14;

xdtLQR.mlx

function y = xdtLQR(x,u,Ab,G)
y=Ab*x+G*u;
end

para_vessel.mlx

%船の緒元
L=50;%船体長さ
B=8.2;%横幅
d=4.0;%水面下喫水
m=20*1000;%質量
C_b=0.6;%ブロック係数
c_r=4;%舵の幅
h_r=2;%舵の高さ
V=20*0.514444;%速度　ノット数→秒速
rho=999;%水の密度

para_system_vessel.mlx

SR=c_r*h_r;%舵面積
AR=h_r*h_r/SR;%舵のアスペクト比
S=L*d;%水面下則面積
A=2*d/L;%側面積のアスペクト比
Vol=C_b*L*B*d;%容積

Izw = (L^2 + B^2) * rho * Vol / 20; %回転楕円体を水で置き換えた時のz軸周りの慣性モーメント
Y_delta = (1/2) * rho * V^2 * SR * 6.13 * AR / (AR+2.25);  %舵が発生する力
N_delta = -Y_delta * (L/2); %舵が発生するモーメント
Y_beta = -(1/2)*rho*V^2*S*((pi*A)/2 + 1.4*C_b*B/L) - Y_delta;%舵が与える影響
Y_r = (1/2)*rho*V^2*S*(pi*A/4) + Y_delta * L/(2*V);
N_beta = -(1/2)*rho*V^2*S*L*A + Y_delta*(L/2);
N_r = -(1/2)*rho*V^2*S*L*A*(0.54-A) - Y_delta*(L^2/(4*V)); 
%付加質量，付加慣性モーメント
mx = (0.5/(L/B) - 0.03) * rho * Vol; 
my = (0.0012*(L/B)^3 - 0.0285*(L/B)^2 + 0.2294*(L/B) + 0.3127) * rho * Vol;
mz = my;
Jz = (0.0014*(L/B)^3 - 0.0378*(L/B)^2 + 0.3567*(L/B) - 0.3328) * Izw;
Iz = (L^2 + B^2) * m / 12; %慣性モーメント


A_11 = Y_beta/((m+my)*V);
A_12 = (Y_r - (m+mx)*V)/((m+my)*V);
B_11 = Y_delta/((m+my)*V);
A_21 = (N_beta + (mx-my)*V^2)/(Iz+Jz);
A_22 = N_r/(Iz+Jz);
B_21 = N_delta/(Iz+Jz);
%-----------------------------------------------------------------
%システム行列
matA=([A_11 A_12 0;
       A_21 A_22 0;
       0 1 0]);

matB=([B_11;
       B_21;
       0]);

シミュレーションした結果，上記の結果を得ました．$t=5$[sec]までは0に収束していませんが，5[sec]以降では原点周りの状態変数の振動がほとんどなく，収束しています．とはいえ，実際の状況では全状態が完全可観測であることは稀であり，カルマンフィルタなどのオブザーバによって状態推定し，オブザーバとレギュレータを併合して制御器を設計することになります．カルマンフィルタの設計は，制御と観測の双対性から，LQRと同様に設計することができます．オブザーバととレギュレータは，独立に設計することができます(制御と観測の分離)．ここら辺の話は近いうちに記事にし，上記のモデルを用いてシミュレーションを実行しようと考えています．

最後に，重み行列を変化させたときのLQRの制御器を併合した自律系の極がどのように変化するのかを確認します．下記のコードを用いて，極をプロットしました．

root_trajectory_LQR.mlx

% Load Parameter
run('para_vessel');%parameter for vessel
run('para_system_vessel');%parameter for plant matrix

% Setting weight matrix
R = 1; 
M = [1  0  0;
     0  1  0;
     0  0  1];

% Caliculate root locus
[n,m]       = size(matA);
N           = 8e2;
root_data   = zeros(N,2*n);
for i=1:1:N
    q=2*i;
    QQ=q*M;
    [K,S,P]=lqr(matA,matB,QQ, R);
    for j=1:n
    root_data(i,2*j-1:2*j) = [real(P(j)),imag(P(j))];
    end
end

% Visualize
figure
plot(root_data(:,1),root_data(:,2),'o') 
hold on
plot(root_data(:,3),root_data(:,4),'*') 
hold on
plot(root_data(:,5),root_data(:,6),'x')
hold on
xlabel('Real Part','FontSize',16,'Interpreter','latex')
ylabel('Imagination Part','FontSize',16,'Interpreter','latex')
legend('root 1','root 2','root 3','Fontsize',18,'Interpreter','latex','Location','northwest');
grid on
h = gca;
h.XAxis.FontSize = 14;
h.YAxis.FontSize = 14;
title('Root locus for Q','FontSize',16,'Interpreter','latex')
saveas(gcf,'root_locus.png')

このような，極の変化の仕方をButterworth-patternといいます．今回は，完全可観測なので大丈夫ですが，オブザーバを用いた時は，LQRの極の位置について検討する必要があります．一般的にオブザーバの極は，レギュレータの極に対し，複素平面上で左になるようにします．これは推定状態を用いて，レギュレータを施しているためです．他にも，極・零点消去が起こらないように極を配置するなど，考慮する点はありますが，詳細はここでは割愛させて頂きます．

おわりに

最後まで読んでいただき，ありがとうございます．記事を書いていて私自身の良い復習になりました．まだまだQiita初心者ですが，今後もより精度のよい記事を投稿できるように頑張ります．