チュートリアル 2: 連続状態の最適制御

ボーナス: 最適制御

Neuromatch Academyによる

コンテンツ作成者: Zhengwei Wu, Shreya Saxena, Xaq Pitkow

コンテンツレビュアー: Karolina Stosio, Roozbeh Farhoodi, Saeed Salehi, Ella Batty, Spiros Chavlis, Matt Krause, Michael Waskom

制作編集者: Spiros Chavlis

チュートリアルの目的

このチュートリアルでは、連続制御タスクを実装します。線形動的システムの制御入力を設計して、目標状態に到達させます。ここでの状態は連続値であり、 $-\infty$ から $\infty$ までの任意の実数を取ります。

チュートリアル1で二値状態の制御について学び、昨日は確率的動力学、潜在状態、観測について学びました。今回は状態を完全に観測できる場合の制御器設計（線形二次レギュレータ - LQR）と、状態を部分的にしか観測できない場合の制御器設計（線形二次ガウス - LQG）という新しい概念を紹介します。

このチュートリアル全体で扱う例は、宇宙空間でジェットパックを使ってネズミを捕まえようとするネコです。

このチュートリアルで学ぶこと：

最適制御の考え方を使っておもちゃの例（宇宙でネズミを捕まえるアストロキャット）を解く
状態を完全に観測できる場合の最適制御器（LQR）を設計する
状態を部分的にしか観測できない場合の最適制御器（LQG）を設計する

# @title Tutorial slides
# @markdown These are the slides for all videos in this tutorial.
from IPython.display import IFrame
link_id = "8j5rs"
print(f"If you want to download the slides: https://osf.io/download/{link_id}/")
IFrame(src=f"https://mfr.ca-1.osf.io/render?url=https://osf.io/{link_id}/?direct%26mode=render%26action=download%26mode=render", width=854, height=480)

セットアップ

# @title Install and import feedback gadget


from vibecheck import DatatopsContentReviewContainer
def content_review(notebook_section: str):
    return DatatopsContentReviewContainer(
        "",  # No text prompt
        notebook_section,
        {
            "url": "https://pmyvdlilci.execute-api.us-east-1.amazonaws.com/klab",
            "name": "neuromatch_cn",
            "user_key": "y1x3mpx5",
        },
    ).render()


feedback_prefix = "W3D3_T2"

# Imports
import numpy as np
import matplotlib.pyplot as plt
from matplotlib import gridspec
from collections import namedtuple

# @title Figure Settings
import logging
logging.getLogger('matplotlib.font_manager').disabled = True

import ipywidgets as widgets
from IPython.display import HTML
%matplotlib inline
%config InlineBackend.figure_format = 'retina'
plt.style.use("https://raw.githubusercontent.com/NeuromatchAcademy/course-content/main/nma.mplstyle")

# @title Plotting Functions

def plot_vs_time(s, slabel, color, goal=None, ylabel=None, show=True):
  plt.plot(s, color, label=slabel)
  if goal is not None:
    plt.plot(goal, 'm--', label='goal $g$')
  plt.xlabel("Time", fontsize=14)
  plt.legend(loc="upper right")

  if ylabel:
    plt.ylabel(ylabel, fontsize=14)
  if show:
    plt.show()


def plot_kf_state_vs_time(latent_states, estimates, title, goal=None):

  fig = plt.figure(figsize=(12, 4))
  plt.suptitle(title, y=1.05)
  gs = gridspec.GridSpec(1, 2, width_ratios=[1, 2])

  ax0 = plt.subplot(gs[0])
  ax0.plot(latent_states, estimates, 'r.')
  ax0.plot(latent_states, latent_states, 'b')
  ax0.set_xlabel('Latent State')
  ax0.set_ylabel('Estimated State')
  ax0.set_aspect('equal')

  ax1 = plt.subplot(gs[1])
  ax1.plot(latent_states, 'b', label = 'Latent State')
  ax1.plot(estimates, 'r', label = 'Estimated State')

  if goal is not None:
    ax1.plot(goal, 'm--', label = 'goal')

  ax1.set_xlabel('Time')
  ax1.set_ylabel('State')
  ax1.legend(loc="upper right")
  plt.tight_layout()
  plt.show()

# @title Helper Functions

# Global variables
T = 50
standard_normal_noise = np.random.randn(T)
standard_normal_noise_meas = np.random.randn(T)
gaussian = namedtuple('Gaussian', ['mean', 'cov'])

class ExerciseError(AssertionError):
  pass


def test_lds_class(lds_class):
  from math import isclose
  ldsys = lds_class(ini_state=2., noise_var=0.)
  if not isclose(ldsys.dynamics(.9)[1], 1.8):
    raise ExerciseError("'dynamics' method is not correctly implemented!")
  if not isclose(ldsys.dynamics_openloop(.9, 2., np.zeros(ldsys.T)-1.)[1], -0.2):
    raise ExerciseError("'dynamics_openloop' method is not correctly implemented!")
  if not isclose(ldsys.dynamics_closedloop(.9, 2., np.zeros(ldsys.T)+.3)[0][1], 3.):
    raise ExerciseError("s[t] in 'dynamics_closedloop' method is not correctly implemented!")
  if not isclose(ldsys.dynamics_closedloop(.9, 2., np.zeros(ldsys.T)+.3)[1][0], .6):
    raise ExerciseError("a[t] in 'dynamics_closedloop' method is not correctly implemented!")
  ldsys.noise_var = 1.
  if isclose(ldsys.dynamics(.9)[1], 1.8):
    raise ExerciseError("Did you forget to add noise to your s[t+1] in 'dynamics'?")
  if isclose(ldsys.dynamics_openloop(.9, 2., np.zeros(ldsys.T)-1.)[1], -0.2):
    raise ExerciseError("Did you forget to add noise to your s[t+1] in 'dynamics_openloop'?")
  if isclose(ldsys.dynamics_closedloop(.9, 2., np.zeros(ldsys.T)+.3)[0][1], 3.):
    raise ExerciseError("Did you forget to add noise to your s[t+1] in 'dynamics_closedloop'?")
  if not isclose(ldsys.dynamics_closedloop(.9, 2., np.zeros(ldsys.T)+.3)[1][0], .6):
    raise ExerciseError("Your input a[t] should not be noisy in 'dynamics_closedloop'.")

  print('Well Done!')


def test_lqr_class(lqr_class):
  from math import isclose
  lqreg = lqr_class(ini_state=2., noise_var=0.)
  lqreg.goal = np.array([-2, -2])
  s = np.array([1, 2])
  a = np.array([3, 4])
  if not isclose(lqreg.calculate_J_state(s), 25):
    raise ExerciseError("'calculate_J_state' method is not correctly implemented!")
  if not isclose(lqreg.calculate_J_control(a), 25):
    raise ExerciseError("'calculate_J_control' method is not correctly implemented!")

  print('Well Done!')

セクション 1: 開ループ制御と閉ループ制御による線形動的システム（LDS）の探索

# @title Video 1: Flying Through Space
from ipywidgets import widgets
from IPython.display import YouTubeVideo
from IPython.display import IFrame
from IPython.display import display


class PlayVideo(IFrame):
  def __init__(self, id, source, page=1, width=400, height=300, **kwargs):
    self.id = id
    if source == 'Bilibili':
      src = f'https://player.bilibili.com/player.html?bvid={id}&page={page}'
    elif source == 'Osf':
      src = f'https://mfr.ca-1.osf.io/render?url=https://osf.io/download/{id}/?direct%26mode=render'
    super(PlayVideo, self).__init__(src, width, height, **kwargs)


def display_videos(video_ids, W=400, H=300, fs=1):
  tab_contents = []
  for i, video_id in enumerate(video_ids):
    out = widgets.Output()
    with out:
      if video_ids[i][0] == 'Youtube':
        video = YouTubeVideo(id=video_ids[i][1], width=W,
                             height=H, fs=fs, rel=0)
        print(f'Video available at https://youtube.com/watch?v={video.id}')
      else:
        video = PlayVideo(id=video_ids[i][1], source=video_ids[i][0], width=W,
                          height=H, fs=fs, autoplay=False)
        if video_ids[i][0] == 'Bilibili':
          print(f'Video available at https://www.bilibili.com/video/{video.id}')
        elif video_ids[i][0] == 'Osf':
          print(f'Video available at https://osf.io/{video.id}')
      display(video)
    tab_contents.append(out)
  return tab_contents


video_ids = [('Youtube', 'MLUTR8z16jI'), ('Bilibili', 'BV1Zv411B7WV')]
tab_contents = display_videos(video_ids, W=854, H=480)
tabs = widgets.Tab()
tabs.children = tab_contents
for i in range(len(tab_contents)):
  tabs.set_title(i, video_ids[i][0])
display(tabs)

# @title Submit your feedback
content_review(f"{feedback_prefix}_Flying_Through_Space_Video")

この例では、ネコが宇宙空間でネズミを捕まえようとしています。ネズミの位置は目標状態 $g$ で、ここでは静的な目標です。後で時間変化する目標 $g(t)$ を扱います。ネコの位置はシステムの状態 $s_t$ です。状態は内部動力学を持ち、ネコが宇宙空間でゆっくり漂う様子を想像してください。この動力学は、次の時刻の状態 $s_{t+1}$ が現在の状態 $s_t$ の線形関数で表されます。状態にはプロセスノイズがあり（小さなジェットパックのエンジンの腐食による意図しない動きのようなもの）、ここではガウスノイズ $w_t$ としてモデル化しています。

制御入力またはアクション $a_t$ はジェットパックの動作で、次の時刻の状態 $s_{t+1}$ に $Ba_t$ の影響を与えます。このチュートリアルでは、既知の状態動力学のもとで目標 $g$ に到達するためのアクション $a_t$ を設計します。

したがって、線形離散時間システムは以下の式で進化します：

\begin{align}
$s_{t+1}$ &= D $s_t$ + B $a_t$ + $w_t$ $\tag{1}$ \
$s_{0}$ &= $s_{init}$
\end{align}

ここで

$t$ : 時刻ステップで、 $1$ から $T$ まで。 $T$ は時間の長さ。

$s_t$ : 時刻 $t$ の状態。

$a_t$ : 時刻 $t$ のアクション（「制御入力」とも呼ばれる）。

$w_t$ : 時刻 $t$ のガウスノイズ。

$D$ : 遷移行列。

$B$ : 入力行列。

簡単のため、1次元の場合を考えます。この場合、行列はスカラーに簡略化され、状態、制御、ノイズも1次元です。具体的には、 $D$ と $B$ はスカラーです。

目標 $g$ は原点、すなわち $g=0$ とします（演習1と2.2で）。後で時間変化する目標状態 $g(t)$ のシナリオを探ります。

安定性

システムは安定であるとは、任意の有限初期状態 $s_{init}$ に対して出力が有限に保たれることを意味し、 $|D| < 1$ のときに成り立ちます。状態動力学が安定ならば、状態は最終的に $0$ に収束します（制御は不要です！）。しかし、 $|D| > 1$ または目標 $g \neq 0$ の場合は、適切なアクションの列を選ぶことがタスク達成に不可欠です。

開ループ制御と閉ループ線形制御

開ループ制御では、 $a_t$ は $s_t$ の関数ではありません。閉ループ線形制御では、 $a_t$ は状態 $s_t$ の線形関数です。具体的には、 $a_t$ は制御ゲイン $L_t$ と $s_t$ の積、すなわち $a_t = L_t s_t$ です。

次の演習では、制御がない場合、開ループ制御ポリシーに従う場合、閉ループ線形制御の場合にシステムがどうなるかを探ります。

コーディング演習 1: 状態進化方程式の実装

以下のクラスメソッドで状態進化方程式を実装してください。ケースは以下の通りです：

(a) 制御なし: def dynamics

(b) 開ループ制御: def dynamics_openloop

ヒント: 上記の式(1)を参照してください。提供されたコードは同じ記法を使っています。

class LDS:
  """
    T: Length of timeline (global, fixed variable)
    standard_normal_noise: Global noise of length T drawn from N(0, 1)
    noise: Gaussian noise N(mean, var) = mean + sqrt(var) * standard_normal_noise
    ...
  """
  def __init__(self, ini_state: float, noise_var: float, static_noise=False):
    self.ini_state = ini_state
    self.noise_var = noise_var
    self.T = T
    self.static_noise = static_noise

  def dynamics(self, D: float):
    s = np.zeros(T)  # states initialization
    s[0] = self.ini_state
    if self.static_noise:
      noise = np.sqrt(self.noise_var) * standard_normal_noise
    else:
      noise = np.sqrt(self.noise_var) * np.random.randn(T)

    for t in range(T - 1):
      ####################################################################
      ## Insert your code here to fill with the state dynamics equation
      ## without any control input
      ## complete the function and remove
      raise NotImplementedError("Exercise: Please complete 'dynamics'")
      ####################################################################
      # calculate the state of t+1
      s[t + 1] = ...

    return s

  def dynamics_openloop(self, D: float, B: float, a: np.ndarray):

    s = np.zeros(T)  # states initialization
    s[0] = self.ini_state
    if self.static_noise:
      noise = np.sqrt(self.noise_var) * standard_normal_noise
    else:
      noise = np.sqrt(self.noise_var) * np.random.randn(T)

    for t in range(T - 1):
      ####################################################################
      ## Insert your code here to fill with the state dynamics equation
      ## with open-loop control input a[t]
      ## complete the function and remove
      raise NotImplementedError("Please complete 'dynamics_openloop'")
      ####################################################################
      # calculate the state of t+1
      s[t + 1] = ...

    return s

  def dynamics_closedloop(self, D: float, B: float, L: np.ndarray):

    s = np.zeros(T)  # states initialization
    a = np.zeros(T - 1)
    s[0] = self.ini_state

    if self.static_noise:
      noise = np.sqrt(self.noise_var) * standard_normal_noise
    else:
      noise = np.sqrt(self.noise_var) * np.random.randn(T)

    for t in range(T - 1):
      ####################################################################
      ## Insert your code here to fill with the state dynamics equation
      ## with closed-loop control input as a function of control gain L.
      ## complete the function and remove
      raise NotImplementedError("Please complete 'dynamics_closedloop'")
      ####################################################################
      # calculate the current action
      a[t] = ...
      # calculate the next state
      s[t + 1] = ...

    return s, a


# Test your function
test_lds_class(LDS)

以下のように表示されるはずです

Well Done!

解答を見る$

# @title Submit your feedback
content_review(f"{feedback_prefix}_Implement_state_evolution_equations_Exercise")

インタラクティブデモ 1.1: 制御なし、開ループ制御、閉ループ制御の比較

上記のコードがテストを通過したら、以下のインタラクティブデモを使って異なる制御入力の効果を可視化してください。

(a) 制御なしの場合、 $D$ の値によって2つの異なる結果があることがわかりますか？なぜでしょう？

(b) 開ループ制御はうまく機能しますか？ノイズが大きい（困難な）条件では問題がありますか？

# @markdown Make sure you execute this cell to enable the widget!

# @markdown Play around (attentively) with `a` and `L` to see the effect on the open-loop controlled and closed-loop controlled state.

display(HTML('''<style>.widget-label { min-width: 15ex !important; }</style>'''))

@widgets.interact(D=widgets.FloatSlider(0.95, description="D", min=0.85, max=1.05),
                  L=widgets.FloatSlider(-0.3, description="L", min=-0.6, max=0.),
                  a=widgets.FloatSlider(-1., description="a", min=-2., max=1.),
                  B=widgets.FloatSlider(2., description="B", min=1., max=3.),
                  noise_var=widgets.FloatSlider(.1, description="noise_var", min=0., max=.2),
                  ini_state=widgets.FloatSlider(2., description="ini_state", min=2., max=10.))

def simulate_lds(D=0.95, L=-0.3, a=-1., B=2., noise_var=0.1, ini_state=2.):
  """
    ...
  """
  static_noise = True

  # linear dynamical system
  lds = LDS(ini_state, noise_var, static_noise)

  # No control
  s_no_control=lds.dynamics(D)

  # Open loop control
  at = np.append(a, np.zeros(T - 1))
  s_open_loop = lds.dynamics_openloop(D, B, at)

  # Closed loop control
  Lt =  np.zeros(T) + L
  s_closed_loop, a_closed_loop = lds.dynamics_closedloop(D, B, Lt)

  plt.figure(figsize=(10, 6))
  plt.plot(s_no_control, 'b', label='No control')
  plt.plot(s_open_loop, 'g', label='Open Loop with a = {}'.format(a))
  plt.plot(s_closed_loop, 'r', label='Closed Loop with L = {}'.format(L))
  plt.plot(np.zeros(T), 'm--', label='goal')
  plt.title('LDS State Evolution')
  plt.ylabel('State', fontsize=14)
  plt.xlabel('Time', fontsize=14)
  plt.legend(loc="upper right")
  plt.show()

解答を見る$

# @title Submit your feedback
content_review(f"{feedback_prefix}_No_control_closed_lopp_open_loop_Interactive_Demo_and_Discussion")

インタラクティブデモ 1.2: 閉ループ設定のさらなる探索

以下のセルを実行すると、制御ゲイン $L$ の関数として状態と目標の平均二乗誤差（MSE）が可視化されます。U字型の曲線が見え、最小のMSEがあります。最小MSEを達成する制御ゲインは、ここでは数値的最適値と呼ばれる定数制御ゲインの『最適』値です。

緑の破線は $L = -\frac{D}{B}$ を示しており、 $D=1.1$ 、 $B=2$ の場合です。なぜこれが状態 $s$ と目標 $g=0$ のMSEを最小化する理論的な最適制御ゲインなのでしょうか？定数ゲイン $L$ で状態がどのように進化するかを調べてみましょう。

\begin{align}
$s_{t+1}$ &= D $s_t$ + B $a_t$ + $w_t$ \
&= D $s_t$ + $B(Ls_t)$ + $w_t$ \
&= ( $D+BL)s_t$ + $w_t$ $\tag{2} \end{align}$

# @markdown Execute this cell to visualize MSE between state and goal, as a function of control gain
def calculate_plot_mse():
  D, B, noise_var, ini_state = 1.1, 2., 0.1, 2.
  num_iterations = 50
  num_candidates = 100

  control_gain_array = np.linspace(0.1, 1., num_candidates)
  mse_array = np.zeros([num_candidates, num_iterations])

  for j in range(num_iterations):
    for i in range(num_candidates):
      lds = LDS(ini_state, noise_var)
      L = - np.ones(T) * control_gain_array[i]
      s, a = lds.dynamics_closedloop(D, B, L)
      mse_array[i, j] = np.sum(s**2)

  opt = -control_gain_array[np.argmin(np.mean(mse_array, axis=1))]
  plt.figure()
  plt.plot(-control_gain_array, np.mean(mse_array, axis=1), 'b')
  plt.axvline(x=-D/B, color='g', linestyle='--')
  plt.xlabel("control gain (L)", fontsize=14)
  plt.ylabel("MSE between state and goal" , fontsize=14)
  plt.title(f"MSE vs control gain, L*={opt:0.2f}", fontsize=20)
  plt.show()
  return opt


opt = calculate_plot_mse()

次に、制御ゲインを変化させたときのシステムの挙動を可視化します。

次のデモを使って、過大または過小な $L$ の値を使った場合のシステムの挙動を探ってください。スライダーの初期位置は最適制御ゲイン $L^*$ に対応し、最小MSEをもたらす値です。

# @markdown Make sure you execute this cell to enable the widget!

# @markdown Explore different values of control gain `L` (close to optimal, over- and under- ambitious) \\

display(HTML('''<style>.widget-label { min-width: 15ex !important; }</style>'''))

@widgets.interact(L=widgets.FloatSlider(opt, description="L",
                                        min=-1.05, max=0.051))

def simulate_L(L):
  D, B, noise_var, ini_state = 1.1, 2., 0.1, 2.
  static_noise  = True
  lds = LDS(ini_state, noise_var, static_noise)
  # Closed loop control with the numerical optimal control gain
  Lt = np.ones(T) * L
  s_closed_loop_choice, _ = lds.dynamics_closedloop(D, B, Lt)
  # Closed loop control with the theoretical optimal control gain
  L_theory = - D / B * np.ones(T)
  s_closed_loop_theoretical, _ = lds.dynamics_closedloop(D, B, L_theory)
  # Plotting closed loop state evolution with both theoretical and numerical optimal control gains
  plt.figure(figsize=(10, 6))
  plot_vs_time(s_closed_loop_theoretical,
               'Closed Loop (Theoretical optimal control gain)', 'b',
               show=False)
  plot_vs_time(s_closed_loop_choice,
               f'Closed Loop (your choice of L = {L:.2f})', 'g',
               goal=np.zeros(T), ylabel="State", show=False)
  plt.title(f'Closed-loop State Evolution. L* = {opt:.2f}')
  plt.show()

解答を見る$

# @title Submit your feedback
content_review(f"{feedback_prefix}_Closed_loop_exploration_Interactive_Demo_and_Discussion")

セクション 2: 線形二次レギュレータ（LQR）を用いた最適制御入力の設計

# @title Video 2: Linear quadratic regulator (LQR)
from ipywidgets import widgets
from IPython.display import YouTubeVideo
from IPython.display import IFrame
from IPython.display import display


class PlayVideo(IFrame):
  def __init__(self, id, source, page=1, width=400, height=300, **kwargs):
    self.id = id
    if source == 'Bilibili':
      src = f'https://player.bilibili.com/player.html?bvid={id}&page={page}'
    elif source == 'Osf':
      src = f'https://mfr.ca-1.osf.io/render?url=https://osf.io/download/{id}/?direct%26mode=render'
    super(PlayVideo, self).__init__(src, width, height, **kwargs)


def display_videos(video_ids, W=400, H=300, fs=1):
  tab_contents = []
  for i, video_id in enumerate(video_ids):
    out = widgets.Output()
    with out:
      if video_ids[i][0] == 'Youtube':
        video = YouTubeVideo(id=video_ids[i][1], width=W,
                             height=H, fs=fs, rel=0)
        print(f'Video available at https://youtube.com/watch?v={video.id}')
      else:
        video = PlayVideo(id=video_ids[i][1], source=video_ids[i][0], width=W,
                          height=H, fs=fs, autoplay=False)
        if video_ids[i][0] == 'Bilibili':
          print(f'Video available at https://www.bilibili.com/video/{video.id}')
        elif video_ids[i][0] == 'Osf':
          print(f'Video available at https://osf.io/{video.id}')
      display(video)
    tab_contents.append(out)
  return tab_contents


video_ids = [('Youtube', 'NZSwDy7wtIs'), ('Bilibili', 'BV1sz411v7za')]
tab_contents = display_videos(video_ids, W=854, H=480)
tabs = widgets.Tab()
tabs.children = tab_contents
for i in range(len(tab_contents)):
  tabs.set_title(i, video_ids[i][0])
display(tabs)

# @title Submit your feedback
content_review(f"{feedback_prefix}_LQR_Video")

セクション 2.1 システムへの制約の導入

ここからはシステムに追加の制約を課していきます。例えば、上記で異なる $s_{init}$ の値を試した場合、ネズミに短時間で到達するために非常に大きな $a_t$ の値が必要になることがわかったでしょう。しかし、ジェットパックの設計上、1ステップで大量の燃料を使うのは危険かもしれません。爆発したくないので、良好な制御を維持しつつアクション $a_t$ はできるだけ小さくしたいのです。

さらに、演習1では制御ゲインを静的な $L_t \equiv L$ に制限していました。もし変化させられるならどうしますか？

これが最適制御入力を設計するより原理的な方法につながります。

コスト関数の設定

有限時間ホライズンのLQR問題では、コスト関数は以下のように定義されます：

\begin{eqnarray}
J({ $\bf s},{\bf a})$ &=& $J_{state}({$ $\bf s}) + \rho J_{control}({\bf a})$ \
&=& $\sum_{t = 0}^{T} (s_{t}-g)^2 + \rho \sum_{t=0}^{T-1}a_{t}^2 \tag{3} \end{eqnarray}$

ここで $\rho$ は制御努力のコストの重みで、目標に到達していないコストと比較されます。 ${\bf a} = \{a_t\}_{t=0}^{T-1}$ 、 ${\bf s} = \{s_t\}_{t=0}^{T}$ です。これは二次コスト関数です。演習2では $g=0$ のみを扱い、その場合 $J_{state}({\bf s})$ は $\sum_{t = 0}^{T} s_{t}^2$ と表せます。演習3では非ゼロの時間変化する目標を扱います。

LQR問題の目的は、 $J({\bf s},{\bf a})$ を最小化する制御 ${\bf a}$ を見つけることです。つまり、各時刻の制御ゲインを見つけることになります：

\text{argmin} _{\{L_t\}_{t=0}^{T-1}} J({\bf s},{\bf a}) \tag{4}

ここで $a_t = L_t s_t$ です。

セクション 2.2 LQRの解法

式(4)の解、すなわち有限時間ホライズンのLQRは動的計画法で求められます。詳細はStephen Boydの講義資料を参照してください。

無限時間ホライズンの場合はリカッチ方程式を用いた閉形式解が得られ、制御ゲインは時間不変、すなわち $L_t \equiv L$ となります。演習4でこれを使います。詳細はこちらのStephen Boydの講義資料を参照してください。

このセクション全体の追加参考文献：

Bertsekas, Dimitri P. Dynamic programming and optimal control. Vol. 1. No. 2. Belmont, MA: Athena scientific, 1995.

コーディング演習 2.2: コスト関数の実装

コスト関数 $J({\bf s}, {\bf a})$ は2つの部分、 $J_{state}({\bf s})$ と $J_{control}({\bf a})$ に分けられます。

これら2つを、以下のLQR用ヘルパークラスのメソッド def calculate_J_state と def calculate_J_control に実装してください。

class LQR(LDS):
  def __init__(self, ini_state, noise_var, static_noise=False):
    super().__init__(ini_state, noise_var, static_noise)
    self.T = T
    self.goal = np.zeros(T)  # The class LQR only supports g=0

  def control_gain_LQR(self, D, B, rho):
    P = np.zeros(self.T)  # Dynamic programming variable
    L = np.zeros(self.T - 1)  # control gain
    P[-1] = 1
    for t in range(self.T - 1):
        P_t_1 = P[self.T - t - 1]
        P[self.T - t-2] = (1 + P_t_1 * D**2 - D * P_t_1 * B / (rho + P_t_1 * B**2) * B * P_t_1 * D)
        L[self.T - t-2] = - (1 / (rho + P_t_1 * B**2) * B * P_t_1 * D)
    return L

  def calculate_J_state(self, s:np.ndarray):
    ########################################################################
    ## Insert your code here to calculate J_state(s) (see Eq. 3)
    ## complete the function and remove
    raise NotImplementedError("Please complete 'calculate_J_state'")
    ########################################################################
    # calculate the state
    J_state = ...

    return J_state

  def calculate_J_control(self, a:np.ndarray):
    ########################################################################
    ## Insert your code here to calculate J_control(a) (see Eq. 3).
    ## complete the function and remove
    raise NotImplementedError("Please complete 'calculate_J_control'")
    ########################################################################
    # calculate the control
    J_control = ...

    return J_control


# Test class
test_lqr_class(LQR)

以下のように表示されるはずです

Well Done!

解答を見る$

# @title Submit your feedback
content_review(f"{feedback_prefix}_Implement_the_cost_function_Exercise")

インタラクティブデモ 2.2: 原点へのLQR制御

この演習では、新しいLQRコントローラを使って静的な目標 $g=0$ を追跡します。ここでは、 $\rho$ （制御努力コストの重み）を変化させることで、状態軌道、選択されるアクション、制御ゲインにどのような影響があるかを探ります。

$\rho$ の値を変えてみて、状態のシーケンス、アクションの大きさ、制御ゲインの変動にどのような影響があるかを確認してください。
$\rho$ が大きすぎる場合と小さすぎる場合、制御ゲインにはどのような変化が見られますか？
$\rho$ の異なる値に対して、アクションの大きさはどのように変化しますか？

# @markdown Make sure you execute this cell to enable the widget!

# @markdown Explore different values of control gain `L` (close to optimal, over- and under- ambitious) \\

display(HTML('''<style>.widget-label { min-width: 15ex !important; }</style>'''))

@widgets.interact(rho=widgets.FloatSlider(25.,
                                          description="ρ",
                                          min=0., max=50.))

def simulate_rho(rho=1.):
  D, B, ini_state, noise_var = 1.1, 2., 1., .1  # state parameter
  static_noise = True
  lqr = LQR(ini_state, noise_var, static_noise)
  L = lqr.control_gain_LQR(D, B, rho)
  s_lqr, a_lqr = lqr.dynamics_closedloop(D, B, L)

  plt.figure(figsize=(14, 4))
  plt.suptitle('LQR Control for rho = {}'.format(rho), y=1.05)

  plt.subplot(1, 3, 1)
  plot_vs_time(s_lqr,'State evolution','b',goal=np.zeros(T), show=False)
  plt.ylabel('State $s_t$')

  plt.subplot(1, 3, 2)
  plot_vs_time(a_lqr,'LQR Action','b', show=False)
  plt.ylabel('Action $a_t$')

  plt.subplot(1, 3, 3)
  plot_vs_time(L,'Control Gain','b', show=False)
  plt.ylabel('Control Gain $L_t$')

  plt.tight_layout()
  plt.show()

解答を見る$

# @title Submit your feedback
content_review(f"{feedback_prefix}_LQR_to_the_origin_Interactive_Demo_and_Discussion")

セクション 2.3: 状態コストと制御コストのトレードオフ

演習 2.1 では、クラス LQR のメソッド内で $J_{state}$ と $J_{control}$ を計算するコードを実装しました。

ここでは、 $\rho$ の値を変化させながらこれらをプロットし、状態コストと制御コストのトレードオフを探ります。

# @markdown Execute this cell to visualize the tradeoff between state and control cost
def calculate_plot_costs():

  D, B, noise_var, ini_state = 1.1, 2., 0.1, 10.
  num_iterations = 50
  num_candidates = 100

  rho_array = np.linspace(0.2, 40, num_candidates)
  J_state = np.zeros([num_candidates, num_iterations])
  J_control = np.zeros([num_candidates, num_iterations])

  for j in range(num_iterations):
    for i in np.arange(len(rho_array)):
      lqr = LQR(ini_state, noise_var)
      L = lqr.control_gain_LQR(D, B, rho_array[i])
      s_lqr, a_lqr = lqr.dynamics_closedloop(D, B, L)
      J_state[i, j] = lqr.calculate_J_state(s_lqr)
      J_control[i, j] = lqr.calculate_J_control(a_lqr)
  J_state = np.mean(J_state, axis=1)
  J_control = np.mean(J_control, axis=1)

  fig = plt.figure(figsize=(6, 6))
  plt.plot(J_state, J_control, '.b')
  plt.xlabel("$J_{state} = \sum_{t = 0}^{T} (s_{t}-g)^2$", fontsize=14)
  plt.ylabel("$J_{control} = \sum_{t=0}^{T-1}a_{t}^2$" , fontsize=14)
  plt.title("Error vs control effort", fontsize=20)
  plt.show()


calculate_plot_costs()

最適な閉ループ線形制御の下で、状態コストと制御コストのトレードオフを示す『C』字型の曲線の下半分が見られるはずです。

望ましい状態コストの値に対して、この曲線よりも低い制御コストを達成することはできません。同様に、望ましい制御コストの値に対しては、その分の状態コストを受け入れなければなりません。例えば、燃料が限られていて最大制御コストが $J_{control}^{max}$ に制限されている場合を考えます。

この場合、上のグラフで示される対応する $J_{state}$ よりも高い精度で状態を追跡することはできないことを示せます。これはシステム設計や制御の検討において重要な曲線です。

セクション 3: 時変目標追跡のためのLQR制御

# @title Video 3: Tracking a moving goal
from ipywidgets import widgets
from IPython.display import YouTubeVideo
from IPython.display import IFrame
from IPython.display import display


class PlayVideo(IFrame):
  def __init__(self, id, source, page=1, width=400, height=300, **kwargs):
    self.id = id
    if source == 'Bilibili':
      src = f'https://player.bilibili.com/player.html?bvid={id}&page={page}'
    elif source == 'Osf':
      src = f'https://mfr.ca-1.osf.io/render?url=https://osf.io/download/{id}/?direct%26mode=render'
    super(PlayVideo, self).__init__(src, width, height, **kwargs)


def display_videos(video_ids, W=400, H=300, fs=1):
  tab_contents = []
  for i, video_id in enumerate(video_ids):
    out = widgets.Output()
    with out:
      if video_ids[i][0] == 'Youtube':
        video = YouTubeVideo(id=video_ids[i][1], width=W,
                             height=H, fs=fs, rel=0)
        print(f'Video available at https://youtube.com/watch?v={video.id}')
      else:
        video = PlayVideo(id=video_ids[i][1], source=video_ids[i][0], width=W,
                          height=H, fs=fs, autoplay=False)
        if video_ids[i][0] == 'Bilibili':
          print(f'Video available at https://www.bilibili.com/video/{video.id}')
        elif video_ids[i][0] == 'Osf':
          print(f'Video available at https://osf.io/{video.id}')
      display(video)
    tab_contents.append(out)
  return tab_contents


video_ids = [('Youtube', 'HOoqM7kBWSY'), ('Bilibili', 'BV1up4y1S7gg')]
tab_contents = display_videos(video_ids, W=854, H=480)
tabs = widgets.Tab()
tabs.children = tab_contents
for i in range(len(tab_contents)):
  tabs.set_title(i, video_ids[i][0])
display(tabs)

# @title Submit your feedback
content_review(f"{feedback_prefix}_Tracking_a_moving_goal_Video")

より現実的な状況では、マウスは常に動き回っています。マウスがある場所から別の場所へ跳ねる動きを予測できるとしましょう。これが目標軌道 $g_t$ となります。

目標状態 $g_t$ が $0$ でない場合、コスト関数は次のようになります。

J({\bf a}) = \sum_{t = 0}^{T} (s_{t}- g_t) ^2 + \rho \sum_{t=0}^{T-1}(a_{t}-\bar a_t)^2

ここで、 $\bar a_t$ は目標軌道に基づく望ましいアクションです。つまり、コントローラは次の時刻の目標を考慮し、次の時刻の状態を目標に近づけるための予備的な制御アクションを設計します。具体的には、ノイズ $w_t$ を考慮しない場合、 $s_{t+1}=g_{t+1}$ となるように $\bar a_t$ を設計したいです。したがって、式 $(1)$ から、

\begin{eqnarray}
$g_{t+1}$ &=& D $s_t$ + B $\bar a_t$ \
$\bar a_{t}$ &=& $\frac{- Ds_t + g_{t+1}}{B} \end{eqnarray}$

最終的な制御アクション $a_t$ は、この望ましいアクション $\bar a_t$ に制御ゲイン $L_t(s_t - g_t)$ を掛けた項を加えて得られます。

# @markdown Execute this cell to include class for LQR control to desired time-varying goal

class LQR_tracking(LQR):
  def __init__(self, ini_state, noise_var, goal):
    super().__init__(ini_state, noise_var)
    self.T = T
    self.goal = goal

  def dynamics_tracking(self, D, B, L):

    s = np.zeros(self.T) # states initialization
    s[0] = self.ini_state

    noise = np.sqrt(self.noise_var) * standard_normal_noise

    a = np.zeros(self.T) # control initialization
    a_bar = np.zeros(self.T)
    for t in range(self.T - 1):
      a_bar[t] = ( - D * s[t] + self.goal[t + 1]) / B
      a[t] =  L[t] * (s[t] - self.goal[t]) + a_bar[t]
      s[t + 1] = D * s[t] + B * a[t] + noise[t]

    return s, a, a_bar

  def calculate_J_state(self,s):
    J_state = np.sum((s-self.g)**2)
    return J_state

  def calculate_J_control(self, a, a_bar):
    J_control = np.sum((a-a_bar)**2)
    return J_control

インタラクティブデモ 3: 時変目標へのLQR制御

以下のデモを使って、LQRが時変目標をどのように追跡するかを探ってみましょう。まず、正弦波の目標関数 sin から始め、 $\rho$ とプロセスノイズ分散の異なる値でシステムの反応を調べます。次に、ステップ関数やランプなど他の時変目標も試してみましょう。

# @markdown Make sure you execute this cell to enable the widget!

# @markdown Explore different values of control gain `L` (close to optimal, over- and under- ambitious) \\

display(HTML('''<style>.widget-label { min-width: 15ex !important; }</style>'''))

@widgets.interact(rho=widgets.FloatSlider(20., description="ρ", min=0.1, max=40.),
                  noise_var=widgets.FloatSlider(0.1, description="noise_var", min=0., max=1.),
                  goal_func=widgets.RadioButtons(options=['sin', 'step', 'ramp'],
                                                 description='goal_func:',
                                                 disabled=False))

def simulate_tracking(rho, noise_var, goal_func):
  D, B, ini_state = 1.1, 1., 0.
  if goal_func == 'sin':
    goal = np.sin(np.arange(T) * 2 * np.pi * 5 / T)
  elif goal_func == 'step':
    goal = np.zeros(T)
    goal[int(T / 3):] = 1.
  elif goal_func == 'ramp':
    goal = np.zeros(T)
    goal[int(T / 3):] = np.arange(T - int(T / 3)) / (T - int(T / 3))

  lqr_time = LQR_tracking(ini_state, noise_var, goal)
  L = lqr_time.control_gain_LQR(D, B, rho)
  s_lqr_time, a_lqr_time, a_bar_lqr_time = lqr_time.dynamics_tracking(D, B, L)

  plt.figure(figsize=(13, 5))
  plt.suptitle('LQR Control for time-varying goal', y=1.05)
  plt.subplot(1, 2, 1)
  plot_vs_time(s_lqr_time, 'State evolution $s_t$', 'b',
               goal, ylabel="State", show=False)
  plt.subplot(1, 2, 2)
  plot_vs_time(a_lqr_time, 'Action $a_t$', 'b',
               ylabel="Action", show=False)
  plt.show()

解答を見る$

# @title Submit your feedback
content_review(f"{feedback_prefix}_ LQR_control_to_desired_time_varying_goal_Interactive_Demo_and_Discussion")

セクション 4: 線形二次ガウス（LQG）コントローラを用いた部分観測状態の制御

セクション 4.1 LQGコントローラの紹介

# @title Video 4: Linear Quadratic Gaussian (LQG) Control
from ipywidgets import widgets
from IPython.display import YouTubeVideo
from IPython.display import IFrame
from IPython.display import display


class PlayVideo(IFrame):
  def __init__(self, id, source, page=1, width=400, height=300, **kwargs):
    self.id = id
    if source == 'Bilibili':
      src = f'https://player.bilibili.com/player.html?bvid={id}&page={page}'
    elif source == 'Osf':
      src = f'https://mfr.ca-1.osf.io/render?url=https://osf.io/download/{id}/?direct%26mode=render'
    super(PlayVideo, self).__init__(src, width, height, **kwargs)


def display_videos(video_ids, W=400, H=300, fs=1):
  tab_contents = []
  for i, video_id in enumerate(video_ids):
    out = widgets.Output()
    with out:
      if video_ids[i][0] == 'Youtube':
        video = YouTubeVideo(id=video_ids[i][1], width=W,
                             height=H, fs=fs, rel=0)
        print(f'Video available at https://youtube.com/watch?v={video.id}')
      else:
        video = PlayVideo(id=video_ids[i][1], source=video_ids[i][0], width=W,
                          height=H, fs=fs, autoplay=False)
        if video_ids[i][0] == 'Bilibili':
          print(f'Video available at https://www.bilibili.com/video/{video.id}')
        elif video_ids[i][0] == 'Osf':
          print(f'Video available at https://osf.io/{video.id}')
      display(video)
    tab_contents.append(out)
  return tab_contents


video_ids = [('Youtube', 'c_D7iDLT_bw'), ('Bilibili', 'BV1xZ4y1u73B')]
tab_contents = display_videos(video_ids, W=854, H=480)
tabs = widgets.Tab()
tabs.children = tab_contents
for i in range(len(tab_contents)):
  tabs.set_title(i, video_ids[i][0])
display(tabs)

# @title Submit your feedback
content_review(f"{feedback_prefix}_LQG_Video")

実際には、コントローラは状態を完全に観測できません。例えば、宇宙空間でのジェットパックは地上のミッションコントロールによって制御されるかもしれません。この場合、レーダーを通じて状態 $s_t$ のノイズを含む測定値 $m_t$ が得られ、コントローラは (1) 真の状態を推定し、(2) その推定に基づいてアクションを設計する必要があります。

幸いにも、分離原理により (1) と (2) を別々に行うことが最適であることが分かっています。これにより問題はずっと簡単になり、それぞれのステップのやり方は既に学んでいます。

状態推定
測定値から状態を復元できますか？
昨日学んだように、状態 $\hat{s}_t$ は測定値 $m_t$ から カルマンフィルタ を用いて推定できます。
アクション設計
上記のセクション2と3で学んだLQRコントローラは状態に基づいてアクションを設計します。分離原理により、LQRで状態の代わりに 推定された 状態を使えば十分であることが示されています。すなわち、

a_t = L_t \hat s_t

状態の動態は次のようになります。

s_{t+1} = D s_t + B a_t + w_t

ここで $w_t$ はプロセスノイズ（proc_noise）であり、観測／測定は次のように表されます。

m_t = C s_t + v_t

$C$ は観測行列、 $v_t$ は測定ノイズ（meas_noise）です。

(1) 状態推定と (2) LQRを用いたアクション設計の組み合わせは 線形二次ガウス（LQG） と呼ばれます。昨日、カルマンフィルタのコードを完成させました。これを基にLQGコントローラを実装します。これらの演習では、セクション2と同様に目標 $g=0$ に戻ります。

インタラクティブデモ 4.1: 線形閉ループコントローラと連携したカルマンフィルタ（LQG制御）

KalmanFilter クラスとそのメソッド get_estimate(self, m) を確認して、フィルタリングの仕組みを復習しましょう。昨日の実装との唯一の違いは、今日のカルマンフィルタは推定時にアクションを考慮している点です。

また、LQG クラスも確認してください。このクラスと LQR クラスの違いは、システムが状態のノイズを含む測定値を返す点だけです。

class KalmanFilter():
  def __init__(self, transition_matrix, transition_covariance,
               observation_matrix, observation_covariance,
               initial_state_mean, initial_state_covariance):
    self.D = transition_matrix
    self.Q = transition_covariance
    self.C = observation_matrix
    self.R = observation_covariance
    self.prior = gaussian(initial_state_mean, initial_state_covariance)

  def get_estimate(self, m, a):

    predicted_estimate = self.D * self.prior.mean + a
    predicted_covariance = self.D**2 * self.prior.cov + self.Q

    innovation_estimate = m - self.C * predicted_estimate
    innovation_covariance = self.C**2 * predicted_covariance + self.R

    # Kalman gain is the weight given to the innovation (ie., the difference between the measurement and the predicted measurement)
    K = predicted_covariance * self.C / innovation_covariance
    updated_mean = predicted_estimate + K * innovation_estimate
    updated_cov = (1 - K * self.C) * predicted_covariance
    posterior = gaussian(updated_mean, updated_cov)

    # Current posterior becomes next-step prior
    self.prior = posterior

    return posterior.mean


class LQG():
  def __init__(self, transition_matrix, transition_covariance,
               observation_matrix, observation_covariance, initial_state_mean,
               initial_state_covariance, ntrials=1, static_noise=False):
    self.D = transition_matrix
    self.Q = transition_covariance
    self.C = observation_matrix
    self.R = observation_covariance
    self.static_noise = static_noise
    self.ntrials = ntrials
    self.t = 0
    self.latent_states = np.zeros([T, ntrials])
    self.latent_states[0] = initial_state_mean + np.sqrt(initial_state_covariance) * standard_normal_noise[0]

  def step(self, action):
    self.t += 1
    if self.static_noise:
      self.latent_states[self.t] = self.D * self.latent_states[self.t-1] + action + np.sqrt(self.Q) * standard_normal_noise[self.t-1]
      measurement = self.C * self.latent_states[self.t] + np.sqrt(self.R) * standard_normal_noise_meas[self.t]
    else:
      self.latent_states[self.t] = self.D * self.latent_states[self.t-1] + action + np.sqrt(self.Q) * np.random.randn(self.ntrials)
      measurement = self.C * self.latent_states[self.t] + np.sqrt(self.R) * np.random.randn(self.ntrials)
    return measurement

  def get_control_gain_infinite(self, rho):
    P = np.zeros(T)
    L = np.zeros(T - 1)
    P[-1] = 1

    for t in range(T - 1):
        P_t_1 = P[T - t - 1]
        P[T - t-2] = (1 + P_t_1 * self.D**2 - self.D * P_t_1 / (rho + P_t_1) * P_t_1 * self.D)
        L[T - t-2] = - (1 / (rho + P_t_1)* P_t_1 * self.D)

    return L[0]


def control_policy_LQG(control_gain, estimated_state):
  current_action =  control_gain * estimated_state
  return current_action

以下のインタラクティブデモを使ってカルマンフィルタの性能を観察してください。パラメータ C は観測行列をスケールします。

C=0 のときはどうなりますか？

# @markdown Make sure you execute this cell to enable the widget!
display(HTML('''<style>.widget-label { min-width: 15ex !important; }</style>'''))

@widgets.interact(C = widgets.FloatSlider(1., description="C", min=0., max=3.),
                  proc_noise = widgets.FloatSlider(.1, description="proc_noise", min=0.0, max=1.),
                  meas_noise = widgets.FloatSlider(.2, description="meas_noise", min=0.1, max=1.))

def simulate_kf_no_control(C, proc_noise, meas_noise):

  D = 0.9
  ini_state_mean = 5.
  ini_state_cov = .1
  estimates = np.zeros(T)
  estimates[0] = ini_state_mean

  filter = KalmanFilter(transition_matrix=D,
                        transition_covariance=proc_noise,
                        observation_matrix=C,
                        observation_covariance=meas_noise,
                        initial_state_mean=ini_state_mean,
                        initial_state_covariance=ini_state_cov)

  system = LQG(transition_matrix=D,
               transition_covariance=proc_noise,
               observation_matrix=C,
               observation_covariance=meas_noise,
               initial_state_mean=ini_state_mean,
               initial_state_covariance=ini_state_cov,
               static_noise=False)

  action = 0
  for t in range(1, T):
    measurement = system.step(action)
    estimates[t] = filter.get_estimate(measurement, action)

  plot_kf_state_vs_time(system.latent_states, estimates,
                        'State estimation with KF (Stable system without control input)')

解答を見る$

# @title Submit your feedback
content_review(f"{feedback_prefix}_LQG_Control_Interactive_Demo_and_Discussion")

インタラクティブデモ 4.2: 変化する制御ゲインでのLQGコントローラ出力

ここで、カルマンフィルタと閉ループフィードバックを持つコントローラを実装します。まず任意の制御ゲインと固定の測定ノイズ値を使います。次に、 $\rho$ （制御努力の重み）の異なる値に対して計算したLQRシステムの制御ゲインを使います。

任意の定数制御ゲインで閉ループ制御下のシステム動態 $s_t$ を可視化し、制御ゲインを変化させてみましょう。
他のスライダーも操作してみてください。プロセスノイズが高い（低い）場合はどうなりますか？測定ノイズはどうでしょう？

# @markdown Make sure you execute this cell to enable the widget!

display(HTML('''<style>.widget-label { min-width: 15ex !important; }</style>'''))

@widgets.interact(C = widgets.FloatSlider(1., description="C", min=0., max=3.),
                  L = widgets.FloatSlider(-.3, description="L", min=-.5, max=0.),
                  proc_noise = widgets.FloatSlider(.1, description="proc_noise", min=0.0, max=1.),
                  meas_noise = widgets.FloatSlider(.2, description="meas_noise", min=0.1, max=1.))


def simulate_kf_with_control(C, L, proc_noise, meas_noise):

  D = 1.1
  ini_state_mean = 5.
  ini_state_cov = .1
  estimates = np.zeros(T)
  estimates[0] = ini_state_mean
  control_gain = L

  filter = KalmanFilter(transition_matrix=D,
                        transition_covariance=proc_noise,
                        observation_matrix=C,
                        observation_covariance=meas_noise,
                        initial_state_mean=ini_state_mean,
                        initial_state_covariance=ini_state_cov)

  system = LQG(transition_matrix=D,
               transition_covariance=proc_noise,
               observation_matrix=C,
               observation_covariance=meas_noise,
               initial_state_mean=ini_state_mean,
               initial_state_covariance=ini_state_cov,
               static_noise=True)

  action = 0
  for t in range(1, T):
    measurement = system.step(action)
    estimates[t] = filter.get_estimate(measurement, action)
    action = control_policy_LQG(control_gain, estimates[t])

  title = f'State estimation with KF (control gain = {control_gain})'
  plot_kf_state_vs_time(system.latent_states, estimates, title,
                        goal=np.zeros(T))

# @title Submit your feedback
content_review(f"{feedback_prefix}_LQC_controller_varying_gains_Interactive_Demo")

インタラクティブデモ 4.3: 制御努力コストの重みを変化させたLQGコントローラ

次に、パラメータ $\rho$ を変化させたときのLQGコントローラの性能を見てみましょう。ここでは無限地平線のシステムから得られた制御ゲインを用います。この場合、最適制御ゲインは定数になります。

$\rho$ の値を $0$ から大きな値まで変化させて、状態への影響を観察してください。

# @markdown Make sure you execute this cell to enable the widget!

display(HTML('''<style>.widget-label { min-width: 15ex !important; }</style>'''))

@widgets.interact(rho=widgets.FloatSlider(25., description="ρ", min=0., max=50.))

def simulate_kf_with_lqg(rho):

  D=1.1
  C=1.
  ini_state_mean = 1.
  ini_state_cov = 2.
  proc_noise=0.1
  meas_noise=0.2
  estimates = np.zeros(T)
  estimates[0] = ini_state_mean


  filter = KalmanFilter(transition_matrix=D,
                      transition_covariance=proc_noise,
                      observation_matrix=C,
                      observation_covariance=meas_noise,
                      initial_state_mean=ini_state_mean,
                      initial_state_covariance=ini_state_cov)

  system = LQG(transition_matrix=D,
              transition_covariance=proc_noise,
              observation_matrix=C,
              observation_covariance=meas_noise,
              initial_state_mean=ini_state_mean,
              initial_state_covariance=ini_state_cov,
              static_noise=True)

  control_gain = system.get_control_gain_infinite(rho)

  action = 0
  for t in range(1, T):
    measurement = system.step(action)
    estimates[t] = filter.get_estimate(measurement, action)
    action = control_policy_LQG(control_gain, estimates[t])

  title = 'State estimation with KF (LQG controller)'
  plot_kf_state_vs_time(system.latent_states, estimates, title,
                        goal=np.zeros(T))

# @title Submit your feedback
content_review(f"{feedback_prefix}_LQC_controller_varying_weight_Interactive_Demo")

インタラクティブデモ 4.4: プロセスノイズと測定ノイズは制御された状態と望ましいアクションにどのように影響するか？

プロセスノイズ $w_t$ （proc_noise）と測定ノイズ $v_t$ （meas_noise）は、制御された状態に非常に異なる影響を与えます。

これを可視化するために、スライダーを操作してプロセスノイズと測定ノイズが制御状態にどのように影響するかの直感を掴んでください。この2つのノイズ源はどのように異なりますか？

# @markdown Make sure you execute this cell to enable the widget!

display(HTML('''<style>.widget-label { min-width: 15ex !important; }</style>'''))

@widgets.interact(proc_noise=widgets.FloatSlider(.1, description="proc_noise",
                                                 min=0.1, max=1.),
                  meas_noise=widgets.FloatSlider(.2, description="meas_noise",
                                                 min=0.1, max=1.)
                  )

def lqg_slider(proc_noise, meas_noise):

  D = 1.1
  C = 1.
  rho = 1.
  ini_state_mean = 1.
  ini_state_cov = 2.
  estimates = np.zeros(T)
  estimates[0] = ini_state_mean

  filter = KalmanFilter(transition_matrix=D,
                      transition_covariance=proc_noise,
                      observation_matrix=C,
                      observation_covariance=meas_noise,
                      initial_state_mean=ini_state_mean,
                      initial_state_covariance=ini_state_cov)

  system = LQG(transition_matrix=D,
              transition_covariance=proc_noise,
              observation_matrix=C,
              observation_covariance=meas_noise,
              initial_state_mean=ini_state_mean,
              initial_state_covariance=ini_state_cov,
              static_noise=True)

  control_gain = system.get_control_gain_infinite(rho)

  action = 0
  for t in range(1, T):
    measurement = system.step(action)
    estimates[t] = filter.get_estimate(measurement, action)
    action = control_policy_LQG(control_gain, estimates[t])

  title = 'State estimation with KF (LQG controller)'
  plot_kf_state_vs_time(system.latent_states, estimates, title,
                        goal=np.zeros(T))

解答を見る$

# @title Submit your feedback
content_review(f"{feedback_prefix}_Process_noise_measurements_noise_Interactive_Demo")

セクション 4.2 LQGにおけるノイズの影響

最後に、プロセスノイズと測定ノイズのレベルを変化させたときに状態コストと制御コストがどのように変化するかを定量化します。そのために、多数のシミュレーションを実行し、プロセスノイズと測定ノイズのレベルを段階的に変えながら、MSEと制御コストを追跡します。

不安定なシステムにおいてプロセスノイズと測定ノイズを増加させた影響を観察してください。結果をどのように解釈しますか？

# @markdown Execute this cell to to quantify the dependence of state and control

# @markdown Note: Running many simulations takes time, you need to wait ~2 seconds before observing the consequences of changing the parameter D

display(HTML('''<style>.widget-label { min-width: 15ex !important; }</style>'''))

@widgets.interact(D=widgets.FloatSlider(description="D",
                                        min=0.01, max=4., value=1.1,
                                        step=0.01)
                  )

def transit_matrix_slider(D):
  C = 1  # observation matrix
  ini_state = 5  # initial state mean
  ini_state_cov = 1  # initial state covariance
  rho = 1  # control effort parameter
  n_iter = 50
  n_ops = 20
  process_noise_var = .1
  measurement_noise_var = .2

  # Implement LQG control over n_iter iterations, and record the MSE between state and goal
  MSE_array_N_meas = []
  MSE_array_N_proc = []
  Jcontrol_array_N_meas = []
  Jcontrol_array_N_proc = []

  meas_noise_array = np.linspace(0.1, 3, n_ops)
  proc_noise_array = np.linspace(0.1, 3, n_ops)

  # Try several proc noises, but same measurement var
  MSE_array_proc = []
  Jcontrol_array_proc = []

  for proc_noise in proc_noise_array:
    transition_covariance = proc_noise
    observation_covariance = measurement_noise_var

    # Controller
    lqg = LQG(D, transition_covariance, C, observation_covariance, ini_state, ini_state_cov, n_iter)
    control_gain_lqg = lqg.get_control_gain_infinite(rho)

    # Filtering
    filter = KalmanFilter(D, transition_covariance, C, observation_covariance, ini_state, ini_state_cov)

    filtered_state_means_impl = np.zeros([T, n_iter])
    filtered_state_covariances_impl = np.zeros([T, n_iter])
    measurement = np.zeros([T, n_iter])
    action_cost = np.zeros([T, n_iter])

    action = np.zeros(n_iter)
    for t in range(1, T):
      measurement[t] = lqg.step(action)

      filter.get_estimate(measurement[t], action)

      filtered_state_means_impl[t] = filter.prior.mean
      filtered_state_covariances_impl = filter.prior.cov

      action = control_gain_lqg * filter.prior.mean
      action_cost[t] = rho * action**2

    state_cost = lqg.latent_states**2

    MSE_array_proc.append(np.cumsum(state_cost))
    Jcontrol_array_proc.append(np.cumsum(action_cost))

  MSE_array_proc = np.array(MSE_array_proc)
  Jcontrol_array_proc = np.array(Jcontrol_array_proc)

  # Try several measurement noises, but same proc var
  MSE_array_meas = []
  Jcontrol_array_meas = []

  for meas_noise in meas_noise_array:
    transition_covariance = process_noise_var
    observation_covariance = meas_noise

    # Controller
    lqg = LQG(D, transition_covariance, C, observation_covariance, ini_state, ini_state_cov, n_iter)
    control_gain_lqg = lqg.get_control_gain_infinite(rho)

    # Filtering
    filter = KalmanFilter(D, transition_covariance, C, observation_covariance, ini_state, ini_state_cov)

    filtered_state_means_impl = np.zeros([T, n_iter])
    filtered_state_covariances_impl = np.zeros([T, n_iter])
    measurement = np.zeros([T, n_iter])
    action_cost = np.zeros([T, n_iter])

    action = np.zeros(n_iter)
    for t in range(1, T):
      measurement[t] = lqg.step(action)

      filter.get_estimate(measurement[t], action)
      filtered_state_means_impl[t] = filter.prior.mean
      filtered_state_covariances_impl = filter.prior.cov

      action = control_gain_lqg * filter.prior.mean
      action_cost[t] = rho * action**2

    state_cost = lqg.latent_states**2

    MSE_array_meas.append(np.cumsum(state_cost))
    Jcontrol_array_meas.append(np.cumsum(action_cost))

  MSE_array_meas = np.array(MSE_array_meas)
  Jcontrol_array_meas = np.array(Jcontrol_array_meas)

  # Compute statistics
  MSE_array_proc_mean = np.mean(np.array(MSE_array_proc), axis = 1)
  MSE_array_proc_std = np.std(np.array(MSE_array_proc), axis = 1)
  MSE_array_meas_mean = np.mean(np.array(MSE_array_meas), axis = 1)
  MSE_array_meas_std = np.std(np.array(MSE_array_meas), axis = 1)

  Jcontrol_array_proc_mean = np.mean(np.array(Jcontrol_array_proc), axis = 1)
  Jcontrol_array_proc_std = np.std(np.array(Jcontrol_array_proc), axis = 1)
  Jcontrol_array_meas_mean = np.mean(np.array(Jcontrol_array_meas), axis = 1)
  Jcontrol_array_meas_std = np.std(np.array(Jcontrol_array_meas), axis = 1)

  # Visualize the quantification
  f, axs = plt.subplots(2, 2, sharex=True, sharey=True, figsize=(10, 8))

  axs[0, 0].plot(proc_noise_array, MSE_array_proc_mean, 'r-')
  axs[0, 0].fill_between(proc_noise_array,
                        MSE_array_proc_mean+MSE_array_proc_std,
                        MSE_array_proc_mean-MSE_array_proc_std,
                        facecolor='tab:gray', alpha=0.5)
  axs[0, 0].set_title('Effect of process noise')
  axs[0, 0].set_ylabel('State Cost\n(MSE between state and goal)')

  axs[0, 1].plot(meas_noise_array, MSE_array_meas_mean, 'r-')
  axs[0, 1].fill_between(meas_noise_array,
                        MSE_array_meas_mean + MSE_array_meas_std,
                        MSE_array_meas_mean - MSE_array_meas_std,
                        facecolor='tab:gray', alpha=0.5)
  axs[0, 1].set_title('Effect of measurement noise')

  axs[1, 0].plot(proc_noise_array, Jcontrol_array_proc_mean, 'r-')
  axs[1, 0].fill_between(proc_noise_array,
                        Jcontrol_array_proc_mean + Jcontrol_array_proc_std,
                        Jcontrol_array_proc_mean - Jcontrol_array_proc_std,
                        facecolor='tab:gray', alpha=0.5)
  axs[1, 0].set_xlabel('Process Noise')
  axs[1, 0].set_ylabel('Cost of Control')

  axs[1, 1].plot(meas_noise_array, Jcontrol_array_meas_mean, 'r-')
  axs[1, 1].fill_between(meas_noise_array,
                        Jcontrol_array_meas_mean + Jcontrol_array_meas_std,
                        Jcontrol_array_meas_mean - Jcontrol_array_meas_std,
                        facecolor='tab:gray', alpha=0.5)
  axs[1, 1].set_xlabel('Measurement Noise')

  plt.show()

解答を見る$

# @title Submit your feedback
content_review(f"{feedback_prefix}_Noise_effects_on_the_LQG_Discussion")

まとめ

このチュートリアルでは、最適方策の考え方をAstrocatの例に拡張しました。状態を完全に観測できる場合の最適コントローラ設計（線形二次レギュレータ - LQR）と、状態を部分的にしか観測できない場合の設計（線形二次ガウス - LQG）について学びました。