LQG制御とは
カルマンフィルタ＋LQR設計です．
理論的には，LQRではノイズを考えていませんが，LQGではノイズがシステム内部（状態，出力）に入るとして，次のLQRとほぼ同じ，目標値を最小化することを目標にします．
J=E[x^T(N)Sx(N) + \Sigma^{N-1}_{j=0} \{x^T(j)Qx(j) + u^T(j) Ru(j) \} ]
上の式は，最終時刻を$N$とする有限時間の目標量ですが，無限時間で設計することもできます．
ノイズが乗っているので，$J$は確定値にならず，期待値$E$を用いることになります．
仮定するシステムは
x[k+1]=Ax[k]+Bu[k] + B_w w[k]\\
y[k]= Cx[k] + v[k]\\
E[w w^T]=W\\
E[v v^T] = V\\
とします．
最終結果
オブザーバシステム
\hat{x}[k+1]=A\hat{x}[k]+Bu[k] +L(y[k] - C\hat{x}[k])\\
ゲインは次のリカッチ方程式を解いて求める
P=APA^T -APC^T(V+CPC^T)^{-1}CPA^T +B_wWB_w^T\\
L= PC^T(V+CPC^T)^{-1}\\
参考： https://qiita.com/hiRina/items/42b37748606acac256ef
状態フィードバック
u[k]=F\hat{x}[k]
ゲインは次のリカッチ方程式を解いて求める
P = A^T P A +Q -A^TPB(R+B^TPB^T)^{-1} B^T P A\\
F= -(R+B^TPB^T)^{-1} B^T P A
Scilabで設計したもので比較
https://help.scilab.org/docs/6.0.0/ja_JP/lqg.html
をもとに，計算します．
clear();
zeta = 0.03978
wn = 11.23
a = 0.01314
A=[-2*zeta*wn wn^2
1 0]
B=[a*wn^2
0]
C=[1 0]
D=[0]
x0=[0
0.05]
sl = syslin('c',A,B,C,D,x0);
sl2= dscr(sl,0.01); //離散化
//-----------離散化したＡ，Ｂ，Ｃ，Ｄについてプログラム用に書き下す
AD=sl2(2);
BD=sl2(3);
CD=sl2(4);
printf("AD,BD");
disp(AD);
disp(BD);
//----------LQG設計---------------
R = [1];
Q = [1 0
0 1];
V= [1];
W=[1 0
0 1];
bigQ = blockdiag(Q,R);
bigR = blockdiag(W,V);
[P,r]=lqg2stan(sl2,bigQ,bigR);
[K]=lqg(P,r);
disp(K)
これでコントローラKが設計できます．
オブザーバシステムのA行列 = (A -LC +BF) ＝ コントローラのA行列
フィードバックゲイン F = コントローラのC行列
コントローラのB行列 = L
のはずです．
実際，確認します．
L=K(3)
F=K(4)
disp("AD-LC+BF")
disp(AD-L*CD + BD*F)
disp(K(2))
disp("same?")
確認できました．
P=APA^T -APC^T(V+CPC^T)^{-1}CPA^T +B_wWB_w^T\\
L= APC^T(V+CPC^T)^{-1}\\
を解いて，一致するか確認できました．
同様に，
P = A^T P A +Q -A^TPB(R+B^TPB^T)^{-1} B^T P A\\
F= -(R+B^TPB^T)^{-1} B^T P A
も確認できました．
clear();
zeta = 0.03978
wn = 11.23
a = 0.01314
A=[-2*zeta*wn wn^2
1 0]
B=[a*wn^2
0]
C=[1 0]
D=[0]
x0=[0
0.05]
sl = syslin('c',A,B,C,D,x0);
sl2= dscr(sl,0.01); //離散化
//-----------離散化したＡ，Ｂ，Ｃ，Ｄについてプログラム用に書き下す
AD=sl2(2);
BD=sl2(3);
CD=sl2(4);
printf("AD,BD");
disp(AD);
disp(BD);
//----------LQG設計---------------
R = [1];
Q = [1 0
0 1];
V= [1];
W=[1 0
0 1];
bigQ = blockdiag(Q,R);
bigR = blockdiag(W,V);
[P,r]=lqg2stan(sl2,bigQ,bigR);
[K]=lqg(P,r);
disp(K)
L=K(3)
F=K(4)
disp("AD-LC+BF")
disp(AD-L*CD + BD*F)
disp(K(2))
disp("same?")
//------------ric L --------------------
AD2 =AD;
//離散時間リカッチ方程式を解く
B_ric = CD'*inv(V)*CD;
Q2 = W;
[X1,X2] = riccati(AD2',B_ric,Q2,'d')
X = X1*inv(X2)
disp("ric_L")
disp(X)
hoge = AD2*X*AD2'+Q-(AD2*X*CD')*inv(V+CD*X*CD')*(CD*X*AD2')
disp(hoge)
disp("上の二つが一致するならば、離散リカッチ方程式が成立している")
L2 =AD2*X*CD'*inv(V+CD*X*CD')
disp("L")
disp(L2)
disp(L)
disp("same?")
//------------ric F----------------------
//離散時間リカッチ方程式を解く
B_ric = BD*inv(R)*BD'
[X1,X2] = riccati(AD,B_ric,Q,'d')
//disp(X1)
//disp(X2)
X = X1*inv(X2)
disp("ric_F");
disp(X)
hoge = AD'*X*AD+Q-(AD'*X*BD)*inv(R+BD'*X*BD)*(BD'*X*AD)
disp(hoge)
disp("上の二つが一致するならば、離散リカッチ方程式が成立している")
F2 = -1*inv(R+BD'*X*BD)*(BD'*X*AD)
disp("F");
disp(F2);
disp(F)
disp("same?")
以上より，scilabの関数でLQG設計した場合と，リカッチ方程式による設計の結果が一致したので，正しいといえると思う．
なぜか
J=E[x^T(N)Sx(N) + \Sigma^{N-1}_{j=0} \{x^T(j)Qx(j) + u^T(j) Ru(j) \} ]
状態量がわかっている，つまり$x[k]$が計測できている場合
https://www.youtube.com/watch?v=OK0ZN9PwraQ&list=PLzn6LN6WhlN1AC6_oVP86gQobLEOrxWtx&index=8
を参考にしてもらえば，（離散LQRに近いです，違う点は期待値の部分の処理が入ることです．）
P[k] = A^T P[k+1] A +Q -A^TP[k+1] B(R+B^TP[k+1]B^T)^{-1} B^T P[k+1] A\\
u[k]= -(R+B^TP[k+1]B^T)^{-1} B^T P[k+1] A x[k]
が最適だとわかります．
ここでは，$x[k]$が計測できている場合としましたが，実際には$y[k]$から推定する必要があります．
J=E[x^T(N)Sx(N) + \Sigma^{N-1}_{j=0} \{x^T(j)Qx(j) + u^T(j) Ru(j) \} ]
を推定$\hat{x}$で分離します．
https://www.youtube.com/watch?v=OK0ZN9PwraQ&list=PLzn6LN6WhlN1AC6_oVP86gQobLEOrxWtx&index=8
の1:00:59 の式を見てもらうとわかりやすいですが，Jは「カルマンフィルタの設計によって定まる値」と「推定値$\hat{x}$,$u$」の項に分けられます．$x= (x -\hat{x}) + \hat{x}) $を代入し，$ (x -\hat{x})$を一つの変数とみた．$ (x -\hat{x})$の部分はカルマンフィルタの設計での目標関数と一致する．
J=\hat{J} + E[(\hat{x}(N) - x[N] ) ^T S(\hat{x}(N) - x[N] ) + \Sigma^{N-1}_{j=0} \{(\hat{x} - x ) ^T[j]Q(\hat{x} - x ) [j] + u^T(j) Ru(j) \} ]\\
=\hat{J} + \Sigma Tr(QZ[j])+Tr(SZ[N])\\
\hat{J} = E[\hat{x}^T(N)S\hat{x} + \Sigma^{N-1}_{j=0} \{\hat{x}^T[j]Q\hat{x}[j] + u^T(j) Ru(j) \} ]
$ \Sigma Tr(QZ[j])+Tr(SZ[N]) $は入力には依存しない，カルマンフィルタの設計で定まる最適カルマンゲイン時の目標関数値です．
$\hat{J} $の最適化は，先ほどの状態量がわかっている場合に相当するので
P[k] = A^T P[k+1] A +Q -A^TP[k+1] B(R+B^TP[k+1]B^T)^{-1} B^T P[k+1] A\\
u[k]= -(R+B^TP[k+1]B^T)^{-1} B^T P[k+1] A \hat{x}[k]
で定まります．
カルマンフィルタについて
https://www.youtube.com/watch?v=TszpvZK9Bro&list=PLzn6LN6WhlN1AC6_oVP86gQobLEOrxWtx&index=6
がわかりやすいです．
カルマンフィルタは
E[ \| x[k] - \hat{x[k]} \| ^2 | Y_k]
を最小化することを目標にします．つまり，推定値と実際の値の差の二乗つまり，誤差の二乗．の期待値を最小化することを目標にします．
いろいろ式をいじっていくと,次の式に基づいて計算することになります
P=APA^T -APC^T(V+CPC^T)^{-1}CPA^T +B_wWB_w^T\\
L= PC^T(V+CPC^T)^{-1}\\
入力決定時のリカッチ方程式と比較
対応は
B^T <-> C\\
A^T <->A\\
V<->R\\
B_wWB_w^T<->Q\\
かなり似た式です．
後でちゃんと書きます．
とりあえず寝よう．