AWK で十分ですよ - その6 - カルマンフィルタ

2012年 12月 1日掲載


「今宵の AWK は一味ちがうぞ」

西山清,中野道雄著「パソコンで解くカルマンフィルタ」は,丁寧に大変わかりやすく書かれており,掲載されているプログラムが N88BASIC ということを差し引いても,買って読む価値のある本であった(今はもう売ってない).

今回は,同書の「第5章 カルマンフィルタの実際」の「5-1 基本問題」を,AWK で解いてみる.


まずは,システム雑音 wk と,観測雑音 vk の作成である. その2 で作った normal_random_numbers.awk を改造して,ガウス性の白色雑音を作る white_noise.awk にする.

BEGIN{
  pi = atan2(0, -1)

  sigma = sqrt(sigma2)

  while(i < n){
    x1 = rand()
    x2 = rand()

    if(x1 != 0){
      print sigma * sqrt(-2 * log(x1)) * cos(2 * pi * x2)
      i++

      if(i >= n) break

      print sigma * sqrt(-2 * log(x1)) * sin(2 * pi * x2)
      i++
    }
  }
}

このスクリプトに,データの個数 n と,分散 sigma2 を -vオプションで指定する. 例題では,σw2 = 3.6×103,σv2 = 3.24×104 だから,

$ awk -v n=100 -v sigma2=3600 -f white_noise.awk > w.txt
$ awk -v n=100 -v sigma2=32400 -f white_noise.awk > v.txt

と,ここで問題発生.システム雑音 w.txt と観測雑音 v.txt をプロットしてみると(プロットには,その4 で作った plot1pbm.awk を使用),

$ awk -f plot1pbm.awk w.txt > w.pbm

w.pbm

$ awk -f plot1pbm.awk v.txt > v.pbm

v.pbm

まったく同じ格好である(sigma2 が違うので値は違うが). これは,AWK の rand()関数が,毎回同じ乱数列を発生するためである. srand()関数を使って,乱数の種(seed)を変えてやらなくてはならない. ということで,white_noise.awk を作り直し.

BEGIN{
  pi = atan2(0, -1)

  sigma = sqrt(sigma2)

  srand(seed)

  while(i < n){
    x1 = rand()
    x2 = rand()

    if(x1 != 0){
      print sigma * sqrt(-2 * log(x1)) * cos(2 * pi * x2)
      i++

      if(i >= n) break

      print sigma * sqrt(-2 * log(x1)) * sin(2 * pi * x2)
      i++
    }
  }
}

-vオプションで,seed を変えることで,乱数列を変更できるようにした.

$ awk -v n=100 -v sigma2=3600 -v seed=1 -f white_noise.awk > w.txt
$ awk -v n=100 -v sigma2=32400 -v seed=2 -f white_noise.awk > v.txt

できあがった w.txt, v.txt は,下図のようになる.

$ awk -f plot1pbm.awk w.txt > w.pbm

w.pbm

$ awk -f plot1pbm.awk v.txt > v.pbm

v.pbm

これで,第1ステップクリア.


次は,状態値 xk と観測値 yk の作成である. wk と vk を使って,基本システムの式

から,xk と yk を作成するスクリプト make_xy.awk は,

BEGIN{
  f = 0.95
  g = 0.6
  h = 1.0

  m_x0 = 0

  x = m_x0   # x_0
}
{
  w = $1
  v = $2

  y = h * x + v   # y_k = h x_k + v

  print x, y

  x = f * x + g * w   # x_k+1 = f x_k + g w
}

と書ける.状態値 x.txt と観測値 y.txt は,以下のようにして作る.

$ paste w.txt v.txt | awk -f make_xy.awk > xy.txt
$ awk '{print $1}' xy.txt > x.txt
$ awk '{print $2}' xy.txt > y.txt

x.txt と y.txt は下図のようになる.

$ awk -f plot1pbm.awk x.txt > x.pbm

x.pbm

$ awk -f plot1pbm.awk y.txt > y.pbm

y.pbm

x.txt と y.txt のグラフを 2つならべて比較するには,次のようにすればよい.

$ cat x.txt y.txt | awk -f plot1pbm.awk > xy.pbm

xy.pbm

左半分が状態値 x.txt で,右半分が観測値 y.txt である.

これで,第2ステップクリア.


最後はカルマンフィルタによる状態推定である. カルマンフィルタの式,

を,AWK で書いた kalman_filter.awk は,

BEGIN{
  f = 0.95
  g = 0.6
  h = 1.0

  s2_w = 3600
    # σ2_w

  s2_v = 32400
    # σ2_v

  m_x0 = 0
    # E{x_0}

  s2_x0 = 1.0
    # σ2_x0

  x_k1 = m_x0
    # x~_0|-1 = E{x_0}

  s2_k1 = s2_x0
    # σ~2_0|-1 = σ2_x0
}
{
  y_k = $1

  k_k = s2_k1 * h / (s2_v + h ^ 2 * s2_k1)
    # K_k = h σ~2_k|k-1 / (σ2_v + h^2 σ~2_k|k-1 )

  s2_k = s2_k1 - k_k * h * s2_k1
    # σ~2_k|k = σ~2_k|k-1 - K_k h σ~2_k|k-1

  s2_k1 = f ^ 2 * s2_k + g ^ 2 * s2_w
    # σ~2_k+1|k = f^2 σ~2_k|k + g^2 σ2_w

  x_k = x_k1 + k_k * (y_k - h * x_k1)
    # x~_k|k = x~_k|k-1 + K_k (y_k - h x~_k|k-1)

  x_k1 = f * x_k
    # x~_k+1|k = f x~_k|k

  print x_k, s2_k
    # print x~_k|k, σ~2_k|k
}

となる. ここで,ややこしい変数の命名規則は,x~ を「x」,σ~2 を「s2」とし,添え字の k|k は「_k」,k+1|k は「_k1」としてみた.

このスクリプトに,観測値 y.txt をあたえると,状態推定値と誤差の分散の推定値が出力される.

$ awk -f kalman_filter.awk y.txt > xa_sa2.txt

状態推定値は,

$ awk '{print $1}' xa_sa2.txt > xa.txt
$ awk -f plot1pbm.awk xa.txt > xa.pbm

xa.pbm

となり,もとの状態値 x.txt,観測値 y.txt とならべてプロットしてみると,

$ cat x.txt y.txt xa.txt | awk -f plot1pbm.awk > x_y_xa.pbm

 x_y_xa.pbm

となる.左が xk,真ん中が yk,右が x~k|k である.

誤差の分散の推定値 σ~k|k2 を見てみると,

$ awk '{print $2}' xa_sa2.txt > sa2.txt
$ awk -f plot1pbm.awk sa2.txt > sa2.pbm

sa2.pbm

$ tail sa2.txt
4785.72
4785.72
4785.72
4785.72
4785.72
4785.72
4785.72
4785.72
4785.72
4785.72

一定値に収束している様子がわかる.

はい,これでカルマンフィルタ終了.


さて,今回は 3つのスクリプトを作ったわけだが,パラメータを変えて試すときに,make_xy.awk と kalman_filter.awk 内の定数を,いちいち書きなおさなくてはならないのは大変めんどうだ. white_noise.awk のように -vオプションで指定するようにして,シェルスクリプトにまとめて書くと楽になるだろう(本稿ではそこまではやらない).


今回の AWKスクリプト:


過去の記事:


[ KDO ホームにもどる]


(C) 2012 KDO