計測自動制御学会東北支部 第 243 回研究集会 (2008.6.20)
資料番号 243-11

カルマンフィルタを利用した速度計測法について
The Measurement of Velocity Using Kalman Filter○石黒智子，一條健司，成田明子，吉岡良雄○ Ishiguro Tomoko, Ichijo Kenji, Narita Akiko, Yoshioka Yoshio
弘前大学

Hirosaki University
キーワード : 速度計測 (measurment of velocity)，高速カルマンフィルタ (fast kalman filter)，最適推定 (optimalestimation)，相互相関 (Cross-Correlation)

連絡先 : 〒 036-8224 弘前市文京町３弘前大学大学院 理工学研究科 吉岡研究室
吉岡良雄，Tel.: (0172)48-2101，Fax.: (0172)39-3513，E-mail: slyoshi@eit.hirosaki-u.ac.jp

1.

いうものである．しかし，1) ではこの手法を用いはじめにた速度計測プログラムが作られ，動作確認が行わ
カルマンフィルタとは，線形動的システムの状態推定を行うために作られたアルゴリズムである．

1960年代に発表されてから様々な分野で用いられ，れているが，人工的に作成したデータを用いた確認のみであり，実測データによる確認はまだ行われていない．

今では宇宙工学，制御工学，通信工学にとどまらず，土木工学，経済学，統計学，オペレーション
ズ・リサーチなどでも見ることができる 3) ．また，そこで，本研究では，速度推定対象を車両とし，磁力計による実測データ 2) を用いて，本手法が実用向きな手法かどうかを検証する．

応用例として，線形動的システムそのものの推定に使うこともでき，実際にはこちらの方が状態推定よりも使われることが多い．カルマンフィルタ

2.

ム推定を普及させた大きな要因である米国の宇宙船軌道推定も，システムそのものの推定にカルマンフィルタを用いた一例である 4) ．

カルマンフィルタによるシステ

カルマンフィルタによるシステム推定は，Fig. 1に示されているようなモデルが対象である．直接

このように，多種多様な場面への適合性だけで観測可能な入力 x と出力 y から，線形動的システムはなくその実用性も評価されているカルマンフィを構成する各パラメータを推定する．カルマンフィ

ルタは，現在，物体の速度推定への適用手法が提

ルタは雑音による影響を考慮したうえで設計され案されている 1) ．これは，2 つのセンサを利用し，ているため，雑音が白色性，ガウス性を有してい

各センサの近くを物体が通過したときの時間差をれば，元の信号が雑音による影響を大きく受けてカルマンフィルタで推定して，速度を計算するといても正確な推定結果を得られる確率が高いとい–1–

じ構成であることがわかる．そのため，Fig. 2 のう特徴がある．

線形動的システムの推定にはカルマンフィルタがシステム雑音

入力 x(観測雑音あり)

Fig. 1

線形動的システム適用可能である．また，x と y の間に生じる時間差

出力 y(観測雑音あり)

は，x と y をそれぞれ入力，出力にもつ線形動的システムのパラメータと関係があるため，カルマンフィルタによるシステム推定は時間差の推定にも線形動的システムの推定つながり，最終的には物体の速度推定に至ることができる．

3.

物体の速度計測システム 1)

3.1

構成

3.2

手順

時刻tにおけるシステムの入出力は以下の式で表Fig. 2 に，物体の速度計測システムの構成を示

わされる．

す．速度測定対象の物体がある道をまっすぐに進M−1X

んでいる場合，その進行方向の上流側と下流側にそれぞれ同等の性能を持つセンサ X，Y を離して設置する．各センサにて，対象物体の通過時を含むデータを測定すると，センサ間に距離があるため，センサ X による測定値と Y による測定値には時間差が生じる．この時間差を推定し，センサ間の距離を求めた時間差で割ると，対象の速度が得られる．なお，センサ間の距離を短くとることで，センサで挟まれた区間での対象の速度が一定であるとみなすことができる．

測定値
1.5

センサX

10.5

0.5

00-0.5-0.5

i=0bi xt−i + et(1)i=0

ただし，xt はセンサ X での観測値，yt はセンサ Y での観測値，et は雑音，M は窓幅，{ai } は y の自己相関係数 (要素数 M )，{bj } は x と y の相互相関係数 (要素数 M ) である．この未知パラメータ {ai }，{bj } をカルマンフィルタで推定する．なお，計算時間のことを考慮して，ここでは高速カルマンフィルタを用いる．

{ai }，{bj } を推定したら，{bj } に注目する．{bj }と yt との観測時刻の差 τ が求める時間差の最も確センサYからしい値である．
センサ同士の距離を求めた時間差で割ると，速-1-1

M−1
X

中の最大成分を bmax とすると，bmax に対応した x測定値
1.51

ai yt−i =-1.5-1.50

2000

4000

6000
時刻[ms]物体
センサＸ(速度推定対象)

雑音

02000

4000

6000
時刻[ms]度が算出される．

センサＹ進行

Fig. 24.

検証

4.1

検証に用いたデータ

物体の速度計測システムの構成検証には 2 種類のデータを用いた．一つは，x にここで，2つのセンサで挟まれた部分が線形動的システムであると仮定し，センサ X から得られるデータを線形動的システムへの入力 x，Y から得られるデータを出力 y とすると，Fig. 2 は Fig. 1 と同磁力計Xによる実測データ，y にはxを遅らせたデータを入れて正規化したもの(以下，データ1)である．
もう一つは，x，y それぞれに磁力計 X，Yによる実測データを入れて正規化したもの (以下，データ 2)–2–である．なお，データ2は2)より時間差は152[ms]で0.20-0.2

推定時間差[ms]2001801601401201008060402000
2000-0.4-0.6-0.8Fig. 5あり，データ 1 はそれにあわせて y を x から 152[ms]だけ遅らせて作成した．データ 1 のグラフを Fig. 3に，データ 2 のグラフを Fig. 4 に示す．
正規化された値
0.60.4x(実測)

y(xの遅延)-1-1.202000

Fig. 3

4000

4.3

6000
時刻[ms]

推定値

4000

6000
時刻[ms]

データ 1 による推定結果

データ 2 による検証結果

Fig. 6 に，{ai }，{bj } の要素数 M を 200 として推定したときの結果を示す．データ 1のときとは異な

データ 1

り，推定値が収束する部分がなく，時間差の推定がうまく行われなかった．

正規化された値
0.6
0.4

x(実測)

y(実測)
推定時間差[ms]3000.2
0-0.2-0.4-0.6-0.8

200-1-1.210015002000

4000506000
時刻[ms]00Fig. 42000

データ 2Fig. 64.2

推定値

250

データ 1 による検証結果

40006000
時刻[ms]

データ 2 による推定結果

そこで，本研究ではこの問題に対し，M の大き

Fig. 5 に，{ai }，{bj } の要素数 M を 200 として推さの変更を試みた．M の大きさは推定結果に大き定したときの結果を示す．座標(a, b)が，時刻aまでく影響し，対象のシステムに対して適当な値を与のxとy の値から推定された時間差bを表わす．図よえる必要があるからである．Fig. 7 は M を 160 に設り，推定値は時刻 5011 で設定した時間差 152[ms] に定した場合のデータ 2 による推定結果，Fig. 8 は Mなり，その後は収束している．従って，実測データを240に設定した場合の推定結果である．これらのを元にしたデータ (ただし時間差は人工的に作成)

図より，M を変更しても推定結果の特徴に大きなでの推定は可能であることがわかる．

変化はみられないことがわかる．したがって，データ 2 による推定がうまく行われない原因は M の大–3–作成したデータを設定して行ったときは適切な時きさではないと考えられる．

間差を推定でき，実測データを元にしたデータで推定時間差[ms]300の推定は可能であることがわかった．しかし，x，推定値250

y をともに実測データに設定すると正しい時間差200を推定することはできなかった．今後は，x，y が150ともに実測データの場合に推定できなかった原因100を究明する方針である．

50
002000Fig. 74000

参考文献

6000
時刻[ms]

1) 古川 玲: 流速計測システムのソフトウェア開発，平成 18 年度弘前大学卒業研究
2) R. Koide, S. Kitamura, T. Nagase, T. Araki,M. Araki and H. Ono: A Punctilious Detection
Method for Measuring Vehicles’ Speeds，ISPACS
2006，967/970 (2006)

M =160 での推定結果

推定時間差[ms]3003) 片 山 徹: 応 用 カ ル マ ン フィル タ，5/5，朝 倉 書 店(1983)

推定値

4) 有本 卓: カルマン・フィルター，194/205，産業図書株式会社 (1977)

250
200
150
100
50002000

Fig. 8

5.

4000

6000
時刻[ms]

M =240 での推定結果

まとめと今後の課題

カルマンフィルタは線形動的システムの状態推定だけでなく，線形動的システムそのものの推定にも用いられているアルゴリズムである．1) では，カルマンフィルタによるシステム推定を応用して，2 つのセンサを用いて物体の速度を推定する手法が提案されている．しかし，実測データでの動作確認をまだ行っておらず，実用向きな手法かどうかの検討が必要であった．そこで，本研究では速度推定対象を車両に設定し，磁力計による実測デー
タ 2) を用いて本手法の有効性を検証した．その結果，xに実測データ，y にxを元に人工的に時間差を–4–