最適制御の数理

はじめに

こんにちは。東京大学計数工学科B3の石鹸です。
このブログは物工/計数 Advent Calendar 2020の18日目の記事です。
去年のAdvent Calendarには解析力学とラグランジュ未定乗数法の記事を投稿しましたが、今回は最適制御理論について紹介していきます。前半では最適制御理論について扱い、後半ではカルマンフィルタについて扱います。
前提知識ですが、制御工学/制御理論の授業で習う知識を前提としません。変分法やラグランジュ未定乗数法を使うので、知らないという人は去年のAdvent Calendarを読んでから読むと理解しやすいと思います。

最適制御理論

正準方程式とHJB方程式

まずは最適制御理論の基礎となる式を導出しましょう。

問題設定

制御対象のシステムの状態をベクトル$\boldsymbol{x}$で、制御入力をベクトル$\boldsymbol{u}$で表現します。このシステムが次の1階微分方程式に従うものとします。これを状態方程式と呼びます。¹$$ $$

\frac{d\boldsymbol{x}}{dt} 
 = \boldsymbol{f}(\boldsymbol{x},\boldsymbol{u}, t)

J[\boldsymbol{x}(t),\boldsymbol{u}(t)] = \varphi (\boldsymbol{x}(t_f)) + \int_{t_0}^{t_f} L\left(\boldsymbol{x}(t),\boldsymbol{u}(t),t\right)dt

右辺第一項は終端コストと呼ばれ、最後の瞬間における状態を評価します。
右辺第二項の被積分項はステージコストと呼ばれ、ある瞬間における状態と制御入力を評価します。例えば、状態と目標値との差分(誤差)や制御入力の大きさ(物理的な意味としては消費電力や燃料の消費量など)を評価します。

最適性条件

$J[\boldsymbol{x}(t),\boldsymbol{u}(t)]$の最適性の必要条件を考えます。システムは状態方程式$\dot{\boldsymbol{x}}=\boldsymbol{f}(\boldsymbol{x},\boldsymbol{u},t)$を満たすという拘束があるので、ラグランジュ未定乗数法を用いて考えます。この拘束は$t\in[t_0,t_f]$で常に満たされるべきなので、ラグランジュ未乗定数を$t$の関数$\boldsymbol{\lambda}(t)$として、　$$ $$

\begin{align}
\bar J[\boldsymbol{x}(t),\boldsymbol{\lambda}(t),\boldsymbol{u}(t)] &= 
 J[\boldsymbol{x}(t),\boldsymbol{u}(t)] 
 + \int_{t_0}^{t_f} \boldsymbol{\lambda}^T(t)\left\{\boldsymbol{f}(\boldsymbol{x}(t),\boldsymbol{u}(t),t)-\dot{\boldsymbol{x}}(t)\right\}dt\\
 &= \varphi (\boldsymbol{x}(t_f))
 + \int_{t_0}^{t_f} \left[ 
    \left\{
     L(\boldsymbol{x}(t),\boldsymbol{u}(t),t)+
     \boldsymbol{\lambda}^T(t)\boldsymbol{f}(\boldsymbol{x}(t), \boldsymbol{u}(t),t)
    \right\}
  -\boldsymbol{\lambda}^T(t)\dot{\boldsymbol{x}}(t)\right]dt
\end{align}

被積分項が3項になると煩わしいので、まとめましょう。{$\cdots$}で囲った2項$L+\boldsymbol{\lambda}^T\boldsymbol{f}$は$\dot{\boldsymbol{x}}(t)$に依存していないので$H(\boldsymbol{x}(t),\boldsymbol{\lambda}(t), \boldsymbol{u}(t),t)$とおいてハミルトン関数と名付けます。$$ $$
さて、変分法を用いて$\bar J$の最適性を考えます。 $\boldsymbol{x}(t)$と$\boldsymbol{u}(t)$に変分$\delta \boldsymbol{x}(t),\delta \boldsymbol{u}(t)$を加えたときの評価値の変化量$\delta {\bar J}$を計算します。変分が微小なので変化量は線形近似で考えて、次のような式変形をします。煩雑なので$H$の引数は省略します。$$ $$

\delta \bar J 
 = \frac{d\varphi}{d\boldsymbol{x}}(x(t_f))\delta\boldsymbol{x}(t_f)
 + \int_{t_0}^{t_f} \left\{
     \frac{\partial H}{\partial \boldsymbol{x}}\delta\boldsymbol{x}(t)
     + \frac{\partial H}{\partial \boldsymbol{u}}\delta\boldsymbol{u}(t)
   \right\}
 - \int_{t_0}^{t_f} \boldsymbol{\lambda}^T (t)\delta \dot{\boldsymbol{x}}(t) dt

第三項の$\delta \dot{\boldsymbol{x}}(t)$が扱いづらいので、部分積分で消しましょう。$$ $$

\int_{t_0}^{t_f} \boldsymbol{\lambda}^T (t)\delta \dot{\boldsymbol{x}}(t) dt 
  = \left[\boldsymbol{\lambda}^T (t)\delta \boldsymbol{x}(t)\right]_{t_0}^{t_f}
  - \int_{t_0}^{t_f} \dot{\boldsymbol{\lambda}}^T (t)\delta \boldsymbol{x}(t)dt 

システムの初期状態は固定されており変分を加えることはできないので、$\delta \boldsymbol{x}(t_0)=\boldsymbol{0}$です。これを用いて$\delta{\bar J}$を整理すると次のようになります。$$ $$

\delta \bar J 
 = \left( \frac{d\varphi}{d\boldsymbol{x}}(x(t_f)) - \boldsymbol{\lambda}^T(t_f)  \right)\delta\boldsymbol{x}(t_f)
 + \int_{t_0}^{t_f} \left\{
     \frac{\partial H}{\partial \boldsymbol{x}}\delta\boldsymbol{x}(t)
     + \dot{\boldsymbol{\lambda}}^T(t) 
    \right\}\delta\boldsymbol{x}(t)dt
 + \int_{t_0}^{t_f} \frac{\partial H}{\partial \boldsymbol{u}}\delta\boldsymbol{u}(t) dt

様々な変分について$\delta \bar J=0$となることが最適性の必要条件なので、変分にかかっている係数は$\boldsymbol 0$であるべきです。したがって、$$ $$

\boldsymbol{\lambda}(t_f)
 = \frac{d\varphi}{d\boldsymbol{x}}^T(x(t_f)),
\dot{\boldsymbol{\lambda}}(t)
 = -\frac{\partial H}{\partial \boldsymbol{x}}^T(t),
\frac{\partial H}{\partial \boldsymbol{u}} = \boldsymbol 0 

が必要条件であることがわかります。システムの満たす方程式はこの他に$\boldsymbol{x}$の初期値と状態方程式があったので、最適に制御されたシステムの満たす条件をまとめると以下の条件であることがわかります。$$ $$

初期値/終端値条件: \boldsymbol{x}(t_0) = \boldsymbol{x_0} ,\hspace{0.2em} 
\boldsymbol{\lambda}(t_f) 
 = \frac{d\varphi}{d\boldsymbol{x}}^T(x(t_f))\\
微分方程式: \dot{\boldsymbol{x}}(t)
 = \frac{\partial H}{\partial \boldsymbol{\lambda}}^T(t),\hspace{0.2em}
\dot{\boldsymbol{\lambda}}(t)  
 = -\frac{\partial H}{\partial \boldsymbol{x}}^T(t)\\ 
最適制御入力条件: \frac{\partial H}{\partial \boldsymbol{u}} = \boldsymbol 0

$\dot{\boldsymbol{x}}(t) = \frac{\partial H}{\partial \boldsymbol{\lambda}}^T(t)$は$H = L + \boldsymbol{\lambda}^T f$より$\frac{\partial H}{\partial \boldsymbol{\lambda}}=f$であることを用いて$\dot{\boldsymbol{x}}(t)=f$を書き直しました。ここで導出された微分方程式はオイラーラグランジュ方程式(Euler–Lagrange方程式,EL方程式)と呼ばれています。最適制御問題ではこの5つの方程式が出発点になります。$$ $$

解析力学との対応

オイラーラグランジュ方程式は解析力学の正準方程式と同じ形をしているため、正準方程式と呼ばれることもあります。実は解析力学と最適制御理論には以下の対応関係があります。この対応関係からラグランジュ乗数$\boldsymbol{\lambda}$のことを$\boldsymbol{p}$と書く流儀もあります。²$$ $$

記号	最適制御理論	解析力学
x/q	状態	力学変数
λ/p	ハミルトン乗数	運動量
L	ステージコスト	ラグランジアン
H	ハミルトン関数	ハミルトニアン

ハミルトンヤコビ理論

最適に制御されたシステムが正準方程式に従っていて解析力学と対応関係があることがわかりました。そこで、解析力学の一つの知見であるハミルトンヤコビ方程式を最適制御に応用することを考えます。今回はハミルトンヤコビ方程式の導出はせず、紹介だけしておきます。
ハミルトンヤコビ方程式とは、適切な母関数(ハミルトンの主関数)$V(\boldsymbol q,\boldsymbol P,t)$³を用いた正準変換²によるハミルトン力学の再定式化で、次のように表現されます。$$ $$

-\frac{\partial V}{\partial t}=H\left(\boldsymbol q,\left(\frac{\partial V}{\partial \boldsymbol{p}}\right)^T, t\right)

次の節ではこの式に制御入力を付け加えた ハミルトンヤコビベルマン方程式 を導出します。

HJB方程式

$\frac{\partial H}{\partial \boldsymbol{u}}=0$の式は制御入力に制限が無いことが前提でしたが、工学上の多くの場合において制御入力には制約があります。例えばモーターの場合、かけることのできる電圧は最大でも電源電圧なので、$|u|\leq V$という制限があります。そのような場合での最適性を連続時間での動的計画法によって導出した式はハミルトンヤコビベルマン方程式として知られています。$$ $$
まず、値関数$V$を次のようにおきます。$$ $$

V(\boldsymbol{x}, t) = \min_{\boldsymbol{u}[t,t_f]}\left\{\varphi (\boldsymbol{x}(t_f)) + \int_{t}^{t_f} L(\boldsymbol{x}(t),\boldsymbol{u}(t),t)dt\right\}

右辺は、$t_0$から$t_f$までのある時刻$t$において、$J[\boldsymbol{x}(t),\boldsymbol{u}(t)]$のうちまだ確定していない値(今後の制御入力によって変化しうる値)の$\min$です。$$ $$
$\boldsymbol{u}[t,t_f]$を$\boldsymbol{u}[t,t+dt]$と$\boldsymbol{u}[t+dt,t_f]$に分割して上の定義式を書き直して見ましょう。$$ $$

\begin{align}
V(\boldsymbol{x}, t) &= \min_{\boldsymbol{u}[t,t+dt]}\left\{
  \int_{t}^{t+dt} L(\boldsymbol{x}(t),\boldsymbol{u}(t),t)dt
  + \min_{\boldsymbol{u}[t+dt,t_f]}\left\{
    \varphi (\boldsymbol{x}(t_f)) 
    + \int_{t+dt}^{t_f} L(\boldsymbol{x}(t),\boldsymbol{u}(t),t)dt\right\}
\right\}\\
&=\min_{\boldsymbol{u}[t,t+dt]}\left\{
  \int_{t}^{t+dt} L(\boldsymbol{x}(t),\boldsymbol{u}(t),t)dt
  + V\left(\boldsymbol{x}(t+dt),t+dt\right) 
\right\}
\end{align}

線形近似を用いて$\min$の中身を整理すると、第一項は$$ $$

\begin{align}
\int_{t}^{t+dt} L(\boldsymbol{x}(t),\boldsymbol{u}(t),t)dt &=
L(\boldsymbol{x}(t),\boldsymbol{u}(t),t)dt
\end{align}

となりまた第二項は

\begin{align}
V\left(\boldsymbol{x}(t+dt),t+dt\right) &= 
V\left(\boldsymbol{x}(t),t\right) + \frac{\partial V}{\partial \boldsymbol{x}} \left(\boldsymbol{x}(t),t\right)\dot{\boldsymbol{x}}dt + \frac{\partial V}{\partial t} \left(\boldsymbol{x}(t),t\right)dt\\
&=V\left(\boldsymbol{x}(t),t\right) + \left(\frac{\partial V}{\partial \boldsymbol{x}} \left(\boldsymbol{x}(t),t\right)\boldsymbol{f}\left(\boldsymbol{x}(t),\boldsymbol{u}(t),t\right) + \frac{\partial V}{\partial t} \left(\boldsymbol{x}(t),t\right)\right)dt 
\end{align}

となります。したがって、$H=L+\boldsymbol{\lambda}^T\boldsymbol{f}$を用いて($\boldsymbol{\lambda}$のところを$\frac{\partial V}{\partial \boldsymbol x}^T$という縦ベクトルで置き換えます)、$$ $$

\begin{align}
V(\boldsymbol{x}, t) &= \min_{\boldsymbol{u}}\left\{ 
V\left(\boldsymbol{x},t\right)
+ \left(H\left(\boldsymbol{x}, \frac{\partial V}{\partial \boldsymbol{x}}^T, u, t \right)
+ \frac{\partial V}{\partial t} \left(\boldsymbol{x},t\right)\right)dt 
\right\} \\
&= V\left(\boldsymbol{x},t\right) +\frac{\partial V}{\partial t} \left(\boldsymbol{x},t\right)dt +
\min_{\boldsymbol{u}}\left\{ 
 H\left(\boldsymbol{x}, \frac{\partial V}{\partial \boldsymbol{x}}^T, u, t \right)dt 
\right\}
\end{align}

以上より、次を得ます。

\min_{\boldsymbol{u}}
 H\left(\boldsymbol{x}, \frac{\partial V}{\partial \boldsymbol{x}}^T, u, t \right)
+ \frac{\partial V}{\partial t} \left(\boldsymbol{x},t\right) =  0

この式は解析力学のハミルトンヤコビ方程式に制御入力についての最小化を加えたものになっており、 ハミルトンヤコビベルマン方程式 (Hamilton–Jacobi–Bellman方程式, HJB方程式)と呼ばれています。

ここまでのまとめ

最適制御の満たす条件は以下の5条件

初期値/終端値条件: \boldsymbol{x}(t_0) = \boldsymbol{x_0} ,\hspace{0.2em} 
\boldsymbol{\lambda}(t_f) 
 = \frac{d\varphi}{d\boldsymbol{x}}^T(x(t_f))\\ 
正準方程式: \dot{\boldsymbol{x}}(t)
 = \frac{\partial H}{\partial \boldsymbol{\lambda}}^T(t),\hspace{0.2em}
\dot{\boldsymbol{\lambda}}(t)  
 = -\frac{\partial H}{\partial \boldsymbol{x}}^T(t)\\ 
\displaystyle{HJB}方程式: \min_{\boldsymbol{u}}
 H\left(\boldsymbol{x}, \boldsymbol{\lambda}, u, t \right)
+ \frac{\partial V}{\partial t} \left(\boldsymbol{x},t\right) =  0
\hspace{1em} \left( \boldsymbol{\lambda}=\frac{\partial V}{\partial \boldsymbol{x}}^T \right) 

BangBang制御

この節では最適制御の例を考えます。理論を追いたい人は次の線形システムの例まで飛ばしてもらって構いません。

問題設定

以下のシステムを考えましょう。変数$x,v,u$はスカラー量です。$$ $$

\begin{align*}
\frac{d}{dt}
\left(
  \begin{array}{c}
   x \\
   v
  \end{array}
 \right)
=
\left(
\begin{array}{cc}
   0 & 1 \\
   0 & 0
  \end{array}
\right)
\left(
  \begin{array}{c}
   x \\
   v
  \end{array}
 \right)
+\left(
  \begin{array}{c}
   0 \\
   1
  \end{array}
 \right)u
\end{align*}

時刻$t_0=0$から$t_f=+\infty$の制御問題を考え、$x(0)=0,v(0)=0$とします。また、$u$には$|u|\leq 1$という制約がかかっている場合を考えます。このとき$X$を正の定数として$(x,v)=(X,0)$の状態に最速で到達する制御を考えます。$$ $$

最適制御の大まかな振る舞いを調べる

評価値$J$は$(x,v)=(X,0)$に到達するまでの時間にしたいので、$$ $$

L(\boldsymbol x,\boldsymbol u,t) = \begin{cases}
    0 \hspace{0.8em}((x,v)=(X,0))\\
    1 \hspace{0.8em}(otherwise)
  \end{cases}

とおいてあげます。これを$t$について積分すると$(x,v)=(X,0)$でないような時間が得られるので、評価値$J$は$(x,v)=(X,0)$の状態に到達するまでの時間となります。$$ $$
$(x,v)=(X,0)$における最適な制御入力が$u=0$であることは明らかなので、それ以外について調べます。$$ $$
まず正準方程式を書きます。$H=L+\boldsymbol{\lambda}^T\boldsymbol{f}$であって、$L$は$(x,v)\not =(X,0)$のとき$x,v$に依存しないので$$ $$

\dot{x}=v, \hspace{0.5em}\dot{v}=u\\ 
\dot{\lambda_x}=0,\hspace{0.5em}\dot{\lambda_v}=-\lambda_x

ここで$\lambda_x,\lambda_v$は$\boldsymbol{\lambda}$の第一,第二成分です。$$ $$
$\frac{\partial H}{\partial u}=\lambda_v$なので、最適な制御入力は$$ $$

u = \begin{cases}
    1 &(\lambda_v<0)\\
    -1 &(\lambda_v>0)
  \end{cases}

となります。このように制御入力が2通りの値を取る制御則は、BangBang制御と呼ばれます。

具体的な解

今回、例えば$\lambda_x(0)=1,\lambda_v(0)=-\sqrt X$と初期値を置いて諸々の変数の時間発展を計算してみましょう。機械的に計算するだけなので過程は省略します。$$ $$

\lambda_x(t) = -1,\hspace{1em}\lambda_v(t)=t-\sqrt X\\
u(t) = \begin{cases}
    1 &(t<\sqrt X)\\
    -1 &(t>\sqrt X)
  \end{cases}\\
v(t) = \begin{cases}
    t &(t<\sqrt X)\\
    2\sqrt X-t &(t>\sqrt X)
  \end{cases} 
\\
x(t) = \begin{cases}
    \frac 12t^2 &(t<\sqrt X)\\ 
    -X+2\sqrt X t - \frac 12t^2 &(t>\sqrt X)
  \end{cases}\\

xやvをグラフにするとこんな感じです。

結局、$t=2\sqrt X$にて$(x,v)=(X,0)$となっていることがわかります。これで条件を満たす最適な制御入力が以下であることがわかりました。⁴$$ $$

u(t) = \begin{cases}
    1 &(t\in [0,\sqrt X))\\
    -1 &(t\in [\sqrt X,2\sqrt X))\\
    0 &(t\in[2\sqrt X,+\infty))
  \end{cases}

線形システムの最適制御

ここまでベクトルや行列は太字で書いてきましたが、ここから先はうるさくなってしまうのでベクトルや行列も太字ではなく普通の文字で表現します。

問題設定

線形システムを二次形式で評価します。つまり、状態方程式と評価式は以下の形で表されます。

\frac{dx}{dt}
 = A(t)x + B(t)u
\\
J[x(t), u(t)] = \frac 12 x^T(t_f) Q_f x(t_f) + \int_{t_0}^{t_f} \frac 12 \left(x^T(t) Q(t) x(t) + u^T(t) R(t) u(t)\right) dt 

これはいわゆるLQR(Linear Quadratic optimal control Regulator)と呼ばれるものです。
$A(t),B(t)$はシステムを表す行列です。$Q_f, Q(t), R(t)$は評価の重み付けをする正定値行列です。$A,B,Q,R$は時間依存性があっても構いません。例えば、速く収束してほしいときは$\boldsymbol Q$を大きく、ゆっくり収束させたい(制御器への負荷を抑えたり消費電力を抑えたりしたい)ときは$\boldsymbol R$を大きく設定します。$$ $$

最適制御入力を求める

ハミルトン関数は

H(x,\lambda,u,t) = \frac 12\left(x^TQ(t)x+u^TR(t)u)\right) + \lambda^T(Ax+Bu)

ハミルトンヤコビベルマン方程式によれば、最適な制御入力はハミルトン関数$H$を最小化する関数でした。$R(t)$が正定値行列ならこのような$u$は一意に存在し$\frac{\partial H}{\partial u}=0$を満たすので、$$ $$

R(t)u + \lambda^TB = 0 \\
\therefore u = -R(t)^{-1}B^T\lambda 

ハミルトンヤコビベルマン方程式の$\min$の項に具体的な$u$を代入すれば、$$ $$

\begin{align}
-\frac{\partial V}{\partial t}(x,t) &= \frac 12x^TQ(t)x +  \frac 12 \left(\lambda^TBR(t)^{-T}\right)R(t)\left(R(t)^{-1}B^T\lambda \right) + \lambda^TAx - \lambda^TR(t)^{-1}B^T\lambda \\
&= \frac 12x^TQ(t)x +  \frac 12 \lambda^TBR(t)^{-1}B^T\lambda + \lambda^TAx - \lambda^TR(t)^{-1}B^T\lambda 
\end{align}

ここで、$V(x,t)=\frac 12 x^TP(t)x$と表せると仮定すると上手くいくことが知られています。$\lambda=\frac{\partial V}{\partial x}^T=P(t)x$から、(行列$P,Q,R$の引数を省略して)$$ $$

\begin{align}
-\frac{\partial V}{\partial t}(x,t)
&= \frac 12x^TQx +  \frac 12 x^TP^TBR^{-1}B^TPx + x^TP^TAx - x^TPB(R^{-1}B^TPx)\\
&= \frac 12 x^T\left\{ Q + 2P^T A  - PBR^{-1}B^TP  \right\}x\\
&= \frac 12 x^T\left\{ Q + P^T A + A^TP - PBR^{-1}B^TP  \right\}x 
\hspace{1em}\end{align}

最後の変形は、$x^TP^TAx$がスカラーゆえ転置しても同じであるから従います。$$ $$
これと$V(x,t)=\frac 12 x^TP(t)x$の時間偏微分$\frac{\partial V}{\partial t} = \frac 12 x^T\dot P(t)x$を比較して、⁵$$ $$

-\dot P(t) = Q + P^T A + A^TP - PBR^{-1}B^TP

以上より、線形システムの最適制御則が次の手順で求まることがわかりました。特に、上の議論は線形時変システムでも有効です。

u(t) = -R(t)^{-1}B^TP(t)x \\ 
-\dot P(t) = Q + P^T A + A^TP - PBR^{-1}B^TP, \hspace{1em} P(t_f)=Q_f 

無限時間LQR

この説では上の問題設定で特にシステムが線形時不変システムであって$t_f\rightarrow \infty$とした場合について考えます。この場合は$P(t)$は定数行列になることが知られています。このことについて証明はしませんが、直感的にはリッカチ微分方程式がシステムの状態に依存していないことや時間シフトについて同じ振る舞いをすることからわかります。$$ $$
さて、$P(t)$が定数行列になるとき、この定数行列は$$ $$

Q + P^T A + A^TP - PBR^{-1}B^TP = 0

という代数方程式を解くことで求められます。この方程式はリッカチ代数方程式と呼ばれていて、有本・ポッターの方法という解法が知られています。(有本・ポッターの方法については前にPythonでRiccati代数方程式の解法を実装するで少し触れたので、気になる人はそちらをご覧ください。)

カルマンフィルタと最適制御

信号処理においてよく知られるカルマンフィルタにはLQRと深い関係があります。最後にその関係性について紹介して、この記事の締めとさせていただきます。

オブザーバ

観測方程式と双対システム

線形システムを考えます。一般に、システムの状態はそのまま観測できるわけではなく、何らかの観測値が得られるものと考えられます。そこで、状態方程式に加えて状態$x$から観測/出力$y$が決定するモデル(観測方程式)を考えましょう。以下で示すのは状態から観測への変換が線形写像であるようなモデルです。$$ $$

\dot x(t) = A(t)x(t) + B(t)u(t)\\
y(t) = C(t)x(t)

このシステムのブロック線図は以下のとおりです。

ここで、ブロック線図野矢印を逆向きにしたシステムを考えましょう。入力$u$と出力$y$の関係をを逆転させることにします。このとき行列の$(i,j)$成分は第$i$成分から第$j$成分への関係を表していたので逆向きにすると第$j$成分から第$i$成分への関係を表し、転置になります。したがって次のようなブロック線図を得ます。$$ $$

このシステムの状態方程式と観測方程式は次のようになります。

\dot x(t) = A^T(t)x(t) + C^T(t)u(t)\\
y(t) = B^T(t)x(t)

これをもとのシステムの 双対システム といいます。線形システムの制御理論の美しさはこの双対性にあります。今回はLQRの双対システムについて考えて行きます。

加法的ノイズの加わるシステム

システムの出力を用いて状態を推定することを考えましょう。今回は制御入力は考えず、代わりに状態発展と観測にそれぞれガウシアンノイズ(正規分布に従うノイズ)が乗っていることを考えます。ノイズは加法的で、以下のように表現できるものとします。それぞれの式の最右辺の項がノイズです。

\dot x(t) = A(t)x(t) + {B(t)v(t)}\\ 
y(t) = C(t)x(t) + {w(t)} 

$v(t),w(t)$はノイズのランダム性を表す確率変数で、互いに独立とします。また時間についても独立、つまり以下を仮定します。⁶$$ $$

E\left[v(t)v^T(T)\right]=V\delta(t-T), 
\hspace{1em}E\left[w(t)w^T(T)\right]=W\delta(t-T),
\hspace{1em}E\left[v(t)w^T(t)\right]=0

\frac{d\hat x(t)}{dt}
= A(t)\hat x(t) + K(t)\left(y(t)-C(t)\hat x(t)\right) 

右辺第一項は時間発展の予測値で、第二項は観測の予測と実際の観測との差分$y(t)-C(t)\hat x(t)$をフィードバックして推定値の誤差を修正しています。$$ $$
ゲイン$K(t)$はどのように設計すればよいでしょうか。$X(t)=\hat x(t)-x(t)$,$Y(t)=y(t)-c(t)\hat x(t)$がそれぞれ状態推定と観測推定の誤差でありこれは加法的ノイズに起因するものなので、これが加法的ノイズに一致して$X=Bv$,$Y=w$が成立していれば完璧な推定ができているということになるでしょう。そこで、誤差と加法的ノイズとの内積の期待値を最大化することを考えてみます。最適制御の枠組みで考えたいので$-1$倍して最小化問題として捉えると、⁷$$ $$

\begin{align}
J &= -\frac 12E\left[\int_{t_0}^{\infty} \left(|v^TB^TX|^2 + |w^TY|^2 \right) dt\right]\\
&= -\frac 12E\left[\int_{t_0}^{\infty} \left(X^TB(vv^T)B^TX + Y^T(ww^T)Y \right)dt \right]\\
&= -\frac 12E\left[\int_{t_0}^{\infty} \left(X^TB(vv^T)B^TX + Y^T(ww^T)Y \right)dt \right]
\end{align}

J = \frac 12 \int_{t_0}^{\infty} \left(x^TQx + u^TRu\right) dt 

$Q\leftrightarrow B^TVB$,$R\leftrightarrow W$の対応関係を考えて、LQRのときの議論から、以下のフィードバックゲインが評価値を最小化することが言えます。$$ $$

K = PC^TW^{-1}\\
\dot P = BVB^T + P^T A^T + AP - PCTW^{-1}CP

LQRのときの場合は以下のようにゲインを決定していました。

F = -R^{-1}B^TP \hspace{1em}
(u(t) = Fx) \\ 
-\dot P = Q + P^T A + A^TP - PBR^{-1}B^TP, \hspace{1em} P(t_f)=Q_f 

観測システムとLQRとで違っている点について説明します。まずゲイン$K$の決定式(1本目)とリッカチ微分方程式(2本目)はLQRの場合に比べて$-1$倍だけ違っていますが、これは$J$が$-1$倍されているためです。$$ $$
また、LQRでは$A,B$だった部分が$A^T,C^T$になっています。これはシステムの双対性より従います。ゲインの決定式が転置の関係になっていますが、これもまた双対性によります。$$ $$

以上がLQRの双対として考えられるオブザーバの紹介になります。実はこのシステムは カルマンフィルタ に一致していま。ここからLQRとカルマンフィルタの双対関係が言えます。制御と観測の双対関係は可制御性か可観測性の双対定理「可観測なシステムの双対システムは可制御である」が有名ですが、実は最適レギュレータと最適オブザーバもまた双対であることがわかりました。この双対関係は1980年代のロバスト制御、特に$H_\infty$制御の研究において一般化プラントというものが考えられた時にさらに掘り下げられているので、興味の湧いた方はぜひ調べてみてください。⁸$$ $$

最後に

だいぶ長々と数式を書き連ねてしまいましたが、いかがだったでしょうか。自分としても記事にすることでいい復習の機会になりました。後半はだいぶ駆け足でだったように思います。
前半のハミルトンヤコビベルマン方程式の導出は理論として美しいだけでなく応用上も有意義なものなので、少しでもその良さを感じていただけていたら幸いです。自分も理論的に美しくて応用上も有意義なものが出来たらなと思うばかりです。

参考資料・関連文献

制御理論

[1]: 大塚敏之 著, 「非線形最適制御入門」
今回扱った導出はこの本の5章及び6章の記述をもとに整理したものです。関連トピックがコンパクトにまとめられていて、おすすめできる一冊です。（ただしここで言うコンパクトは任意の開被覆が有限な部分被覆を持つという意味ではありません。）
[2]: 山本直樹教授, 東京大学工学部3年次授業制御論第二(2019年度)
LQRの導出で参考にさせていただきました。

解析力学

[3]: 畑浩之 著, 益川敏英 監修, 「基幹講座 物理学 解析力学 (基幹講座物理学)」
スタンダードな解析力学の教科書です。

カルマンフィルタ

[4]: 猿渡洋教授, 東京大学工学部3年次授業「信号処理論第二」講義資料第11回,第12回
連続時間カルマンフィルタの導出について丁寧に書いてあります。
[5]: 猿渡洋教授, 東京大学工学部4年次授業「応用音響学」講義資料
離散時間カルマンフィルタについて示しています。
[6]: 大住晃, 亀山建太郎, 松田吉隆 著, 「カルマンフィルタとシステムの同定:動的逆問題へのアプローチ」
現代制御理論を学んだ人がカルマンフィルタに触れるのに向いた一冊です。前半のカルマンフィルタパートではオブザーバから状態推定のトピックに入り、最小二乗規範でカルマンフィルタを導出します。

1. システムが2階以上の微分方程式に従う場合(バネマスダンパ系やRLC共振回路など)、状態$\boldsymbol{x}$の次元を増やすことで表現します。例えば、位置$x$,速度$v$を並べたベクトルを状態として$x$の時間微分を状態$v$で表現してあげる、といった具合にです。$$ $$
   制御入力とは制御する側がある程度自在に決定できる変数のことで、目的を達成するような制御入力がどのようなものか考えるものが制御理論です。
   最適制御では、以下の評価値を最小化するように時刻$t_0$から$t_f$までの制御入力$\boldsymbol{u}$を設計します。評価値は時間の関数$\boldsymbol{x}(t),\boldsymbol{u}(t)$に対し実数値を返す関数(汎関数)として定義します。 $$ $$ ↩

2. この美しい対応関係が今回この記事を投稿しようと思ったきっかけでもあります。 ↩ ↩²

3. 普通はハミルトンの主関数は$S$と置くんですが、今回は後に出てくるハミルトンヤコビベルマン方程式との関係で$V$という記号を置いています。$$ $$ ↩

4. 今回具体的な値を入れて条件が満たされているのは、$\boldsymbol{\lambda}$の時刻$t=0$における値を'上手く'設定して上げたことに起因します。そもそも正準方程式・ハミルトンヤコビベルマン方程式を満たす具体的な$\boldsymbol{u}(t)$や$\boldsymbol{\lambda}(t_f)$を求めるのは一般には難しく、様々なアルゴリズム(勾配法、シューティング法、動的計画法など)が知られています。今回は上手く行く$\boldsymbol{\lambda}$を決め打ちしてあげる手法を使ったので、シューティング法の一種であると言えます。（実際には条件は満たされないので、計算機上の反復計算によってこの値が条件を満たすように収束させます。)$$ $$ ↩

5. 一般に、正定値対称行列$A,B$が$\forall x,\hspace{1em}x^TAx=x^TBx$を満たすなら$A=B$です。 $$ $$
   この微分方程式を$P(t_f)=Q_f$のもとで解くことで最適な制御入力を得ることができます。この微分方程式は名前がついていて、リッカチ微分方程式と呼ばれています。$$ $$ ↩

6. $\delta$関数はディラックのδ関数であり、一般的な関数ではなく超関数の枠組みで考えるものです。今回はδ関数で書いているのはこれを両辺$T$で積分して「ノイズの自己相関関数は時間差$0$で$V$,それ以外で$0$となる」という結論を得るために書いています。要するに「異なる時刻のノイズ同士は無相関である」ということと「ノイズの分散共分散行列は$V$である」という主張を同じ式に書いているということです。$$ $$
   以下のように状態の推定量$\hat x(t)$を発展させることを考えます。$$ $$ ↩

7. $\max f(x)$を達成する$x$は$\min (-1)\times f(x)$を達成します。$$ $$
   ここで、LQRの評価関数は以下でした。(ただし$t_f\rightarrow\infty$としました)$$ $$ ↩

8. 僕もよくわかってないので、調べたら僕に教えてください。 ↩