計測自動制御学会東北文部第164回研究集会り9％．1l．22）
控料香り・16ノt−1

FADDREVAアルゴリズムにもとづく滞在時間最小型
カルマンフィルタのVLSIアーキテクチャ
VLSIArchitccturcofKalJTlanFi］ter＄

withALc且S（LatencybasedonFADJ）EEVAA）gorithm
O高橋 強●，恒川 佳隆●●，三浦 守…
KyoTakallaShi，YoshitakaTsunekawa，MamoruMiura

一岩手県立高度技術専門学院，…岩手大学●lwatepTe igh−teChcollege ■IwateUniversity
キーワード：カルマンフィルター（KalmanFilters〕，滞在時間（htency），Faddeevaアルゴリズム（FaddeevaAlgorithm），シストリッタアレー（SystolicATray）

連絡先：〒023 岩手県水沢市佐倉河字東広町66−2 岩手県立高度技術専門学院
制御システム科 高橋 軌Tel．0197−2ユー4422，Fax．01耶−23−6柑9

1．はじめにカルマンフィルタは宇宙工学の分野で人工衛星の軌道計算に用いられて以来、コンピュータの発達とともに応用分野を拡げ、現在では通信工学、制御工学にとどまらず統計学など幅広い分野で応用されている11。しかし計算の複雑さから、実時間処理に対してはその適用範圃が一般に制限されてきた。近年進展のめぎましいディジタルシグナルプロセッサ（I）jgitalSignalProcessor；DSP）を用いて低次数のカルマンフィルタを実現しても、実時間処理は非常に困難である。最近、処理速度をl占】上させる目的で、シストリッタアレイを用いた研究が多くなされていが3）。ところが間のかかる除算を最後に1度だけ行えばよいことに着目し、滞在時間の増加を回避している。最後に滞在時間島小型カルマンフイ／レタの構成を示し、滞在時間を評価している。
これにより滞在時間が非常に小さく、本アーキテクチャが有効性であることを示す。

2．カルマンフィルタ
次のようなた≧0に対して定義される線形・有限次元の解散時間システムがある。
∫…＝巧ェI＋G−W▲γ農＝〃上ユ上＋Ⅴた

シストリッタアレイでは、高い処理速度が得られる反面、滞在時間（Latency）は大きくなり、フィードバック制御に応用する場合、シ号と呼ばれるそれぞれJJ次元、椚次元のベクステムの安定性を損なうという問題があトルであり、W上とⅤ上はそれぞれシステム雑る。

本研究では、カルマンフィルタの滞在時間を最小化するために、Fadde即且アルゴリズム2・j）を用いて処理を高度に並列化したVLS

ここで、J上は状態ベクトル、γ鳥 は観測信音、観測姓音と呼ばれるJ次元及びm次元のベクトルである。また、巧を状態遷移行列、Gたを駆動行列、〃鳥を観測行列と呼び、一Iアーキテクチャを提案する。本アーキテク
チャでは、まずカルマンフィルタ本来の並列性に着目し、各パラメータ計算を可能な限り般に確定した〝X〝、けXJ、脚X〃行列である。

並列化している。つぎに、演算を並列化する∑v慮を持つガウス白色雑音であるとするとまた、W倉とvよは互いに独立で、それぞれ平均値ベクトル零、既知の共分散行列∑押上とためにfaddeevaアルゴリズムを適用し、特にカルマンゲインを高速に計算して滞在時間カルマンフィルタは次のように表される。

を大幅に減少させている。これは、処理の並列化とともに、本来、次数回分必要な処稚時′ヽ′ヽ J岬＝布目＋∬直上一札拓「−）（2■1）

（2．2）′ヽ ∬慮＋岬＝q丘叩の検討′ヽノヽ 杭＝∑叩1可【坑∑恥．〟ヱ◆十∑v上】￣■

Fadd8eVaアルゴリズムにおいて左上行列（2．3）∑叩＝∑帰一l一片艦〃た∑帰一1（ヱ・4）∑叫＝巧∑印打＋G鳥∑w農GJ′ヽ

ただし、初期値は、j。ト1＝も，∑q＿Ⅰ＝出。であるヰ）。

計算が続けられない。そこで、ピボット選択（ユ・5）を行い対角要素が零にならないように行の入れかえ操作を行う必要がある。3x3行列の例を岡1に示す。ここで要素の右屑■括弧は、何段目の処理に対する入力信号であるかを表す。

︵▲︷︼■U占︷．1・lむt■p‖り1‖∴り占■U2hリ2√n■d︵− ′l 一r︑▲J▲H ▲U −■Ud柑相好
柑相好．〃・．揖．止り咄鮒脚n−封l封1
可扇﹁﹂郎ノロ〃dO

d‖＋d‖ほ‖H現場境■■コ121

1．．ヽ′．■ノ．叫J 爪﹂
可l一亜墟掘⁝ ‖鞘恥ぢ
0 0

未知の行列∬を含む方程式Ar＝βがあ〃αα3．1IT且ddeeY且アルゴリズムの概要す。

α■Jる。

り、逆行列d￣lを求めずにC芳＋乃を求める問題を考える。この過程を以下のように示α−d▲める方法で、ガウスの消去法さ）を用いて行列を三角化及び零化することにより実行されけー‖りlり才打∫・■−■−−

行列の計算を行列演儲を値域計許せずに求日誌詫刀←州法誌乃←Ⅲり叩謁乃■■

Faddeevaアルゴリズムは、行列の乗算や逆の上三角化は、ガウスの消去法を用いて行うが、この場合、一般に対角要素が草になると

愕﹁

3．Ⅰ厄ddeevaアルゴリズム

3．三上三角化におけるピボット選択

岡l ガウスの消去法におけるピボット選択

これら零判断と行の入れ替え操作は滞在時間を大幅に増加させ、しかもアーキテクチ
ャを複雑にする。しかしカルマンフィルタに適用する場合、行列Aに相当する式（ヱ，3）の［］内は正則であるため零ピボットが現れな

ここで、Ⅳ＝u￣lとすると、
β＋坪署＝β＋Cd￣1月＝C方＋上）となり、以下の様になる。

い1・6）。したがって、判断と行の入れ替え操作は不要であり、Faddeevaのアルゴリズムを適用することにより滞在時間の大幅な減少と、VLSIアーキテクチャを単純で規則的な構成にすることが可能になる。

4．滞在時間最′ト型アーキテクチャ
4．1パラメータ計算の並列化
滞在時間を減少させるために、まず式（ユ）に示されるパラメータ計算を並列化する。図2に、カルマンフィルタのブロック図を示

以上は、左下行列を零化することで、同時に既知の行列d、月、C、凸からC∬＋月が右下行列に得られることを示している。

このアルゴリズムの特徴は、前向き処理のみで実行可能なことである。このことはVLSI実現において、データの流れを単純化でき、規則的にハードウエアを構成することが可能になる。

′ヽ」■■ す。同固より∑叩と丘仰の計臥∑た叫とノヽ ∫仰の計算を並夕耶こ実行可能であることがわかる。

ノヽ
→ ∑た叫

（4・8）

・丘叫＝丘榊＋杭レ上一航町）′ヽ・T▲声・l→γ嘉一坑j叩＿l（4・9）γ慮一旦j和卜．
′ヽ →J叩 （4・10）・一点トl

以上より、カルマンフィルタの処理は図3のようになり、Faddeevaアルゴリズムを用いることでカルマンゲインの一一部も並列化でき

図2 カルマンフィルタのブロック回る。

さらに各計算に下addeevaアルゴリズムを適用した場合、計算手順は以￣Fのようになる。
・杭＝三脚叫〃ふた−Ⅰ打ニ＋叫⊥l∧ →∑畔．〝ご（4▲l）

12三 34杭∧∑Ⅴ鳥′■ヽ

∑鳥＋岬．▲ →〃た∑叫＿．可＋∑v上（4．2）
図3 計算の並列化→叫〝ん．〃ト∑v」￣l

4．三 上三角化と零化の並列化（4・3）

ここでは、Faddeevaアルゴリズムにより演算レベルでも更に並列化が可能であることを示す。真た、データフローを用いることにより滞在時l捌を蛙小化することが叫能であるため、ここではデータフローを用いることにする。

Fadde即aアルゴリズムにおいて、上三角化を行ってから零化を行うと滞在時間が増加してしまう。しかし、それらは同じ対角要素ノヽ．■l▲■． ● ∑畔＝∑叫一l一打上〃た∑町トlを用いて実行されることに着日すると、図4→ 〃上∑叩−Ⅰ（4・5）′ヽ→ ∑棚（4・6）ノヽ∧ ・∑叫＝巧∑岬灯＋G点∑w鳥GJ′l】五粁＝巧∑叫打＋∑w上→∑岬げの様に処理を並列化することが可能になる。また本来、図4の各計算段階手引こ除算が行われるが、除算は他の演算に比較して演算時間が数十倍かかるため¶、これも滞在時間を人晰に増加させる原因になる。しかし、各計算段階で除算を行わずに係数とし残しておき、それらの分母をすべて乗算した後、一度だけ除算を実行することで同様の結果を（4・7）得ることができる。更に、求めたいのは左下行列であるので、除算結果は行列月に対して作用させるだけでよい。

る。

hリヽ．hりt．nごJ

J叫J巧一武Jhりコ‖ご上目り

ヰ﹂喝ヰ′叫東山J■
一L

一ノさユリハリl■
亡
ピ
亡
一 一 一
■ユnリ
リサnリ

一′し一．ぺ一√d﹁′巧︼l′ヽウ一′lt一一﹂C︷．1一nリ︼■lリ丁−hリ1． Hりユnユn⊃．‖町．祝．ノ山﹂

l．町止り﹂朗JHりlhりリl︻上α〃つ・■l∧U山肌．．出﹂扇りノ山﹁止り山りlハU

l・l哺hh堀■■■戎璃戎nUり3封り∋α・‖．t．1■′．叫′叫′咽d■Ut︼Jα0′l▲

ざり王lりユ封︼引︼山﹁出二郎−＋．1′・一ヽ■′■■■一■■一▲丁−︳− 7■一n2封︐¶リコ′亡−．d∧UtJ．dt

■ヽ﹂

d仁−nV

■IU

打′ljJ

■UlJノ郎．山叩．J仇︒■．■l／lα︵■l裏﹁朗∴明日二封ヽこ〃︼r〃‖．d﹁■l■．d．d爪V爪V

一一■一−■llJl一tJ︸tJ

爪じ爪じ一一■■ヽ一．︼lヽ︳J．d−dJ丁−∃

Jl一l︼t一■l︸

内部は図5
叩け印招けα d1りl¶り2封l

鵬哩柑＋．←ヰヰメ一一Y武一h堀戎m

ラメータを行列A、B、Cに対応させて用い

山﹁夷．埠l︵7一一lヱj−打．1．■■一〃β

rl一丁一︵■J■−■l一J・l■■ロ．d．d．ddl■﹂﹂﹁一J J﹁︼J′・一■︵﹁ ノll︺

︳4﹁▲．d．αJ∫・1．．︼．1つ小．−．iノ一三フ■﹂．

C＝dβを実行1

BMI柑粛凋で割る■■陸

川津 −1鱈

ヽJ l＋．−J＝鳥，・・・，〝﹁−−く−−−一′二︐レ几l

ロ−1かヽ■′＝慮＋1，…，邦，〝

律腹d

圧打αα

周4 上三角化と零化の並列化＋ヽ′堕
終 了

川比

図8 Fad（kevaアルゴリズムの基本モジュールBM2

図6 行列乗算のデータフロー＝1，‥・，〃＝1，…，円鴇／￣

図5 各段のデータフロー

句…Jま㌻

ルマンフィルタの計算式に用いる場合、各パ■イエリlnり︐■lりユn・ユαd・nりl0 0 0 d皇子I』のi行目 βのj行目
■

ニミ

∀∀∀

ち−

占‖叫ヱ ち一鞘α‖lq oDJ拾る。
〃lを用いる。BMlを図7に示す。これをカ′ヽ．■一■−n√t−1一ldd5．実現法
小になる。

ロー（図5、図6）を用いて、カルマンフィルタを構成する2種類の基本モジュールを構成する。

5．1行列乗算の基本モジュール

次に、カルマンゲイン以外の計算は左上行列が単位行列であるため、単なる行列の乗算になる。滞在時問が最小になるのは、岡6に示すデータフローで計算される場合であ前章で示したように、式（4．3）以外の計算は単なる行列の乗算となるため、図6のデータフローを適用した基本モジュールBM零化
上三角化

前章で検討した滞在時閉息小のデータフ

以上より、各計算段階は図5に示すデータフローにより実行される場合、滞在時間が最図7 行列乗算の基本モジュール

［PBlの滞在時間］1｝lま1はBMlを並列に用いるため、滞在5．ユ 上三角化と零化の基本モジュール

時間J〃りはJ劇〃に等しい。

式（4．3）に対して用いる基本モジュール
nM2は、図5に示されるデータフローを用いて構成するとき酢田l崩帥は昆小になる。Ii
M2を回さ＝こ示す。なお、行列A、Ii、C、［PB2の滞在時間］｛ Dはそれぞれ、坑∫岬＿′〃ご＋凪いJ、可、ロに対応する。

5．3 滞在時間最小型カルマンフィルタの構成

これまで検討した基本モジュールBMl、BM2を用いて、滞在時間島小型カルマンフィルタを構成する。岡9に全体の構成を示す。ここで、PBl、PB2はプロセッシングブロックで、それらの構成を囲10、図11に示す。なお、次数玖J，椚を3とした。
同国より提案アーキテクチャでは、ほぼ2種類の基本モジュールBMl、BM2のみで構成されており、高次のカルマンフィルタに対しては、BMl、BM2を増やすだけで構成することができる。したがって、提案したアーキテクチャはモジュール性、規則性に優れⅤⅠ一Sl実現向きである。

1｝1i2の押掛細IJ♪朋は、Plj2−＊のな

かでは処理時間の最も大きいPB2−Dの処理時間、除算1回、乗算1回分の処理時間になる。したがって、／肌＝J。＋中州＋′郎）になる。
以上より、図9に示されるカルマンフィル
タ全体の滞脚寺岡どょは、
どょ＝′用J＋dJ用ヱ

＝Jd＋「〝＋坤…＋「〃＋占わgヱ叫郎になる。

図12は、次数に対する滞在時間をDSPl個で実現した場合と比較した図である。なお、J∬＝50耶，J榊＝50叫ん＝2480旧7）とした。同園より、滞在時間は非常に小さい借となり、本アーキテクチャが滞在時間の減少に非常に有効であることがわかる。
滞在時間［〃S］【x柑5】

6．滞在時間の評価
本アーキテクチャの滞在時聞を評価し有効性を示す。まず、BMl、BM2の滞在時間を求め、それらをもちいて全体の滞在時間を求める。ここで、乗算の処理時間をJm、加減算の処卿寺間をJ心、除算の処理時間をJdとする。
［BMlの滞在時間］〃〉く〃行列に用いるBMlの処理時間
J餌ノは、姐列化により乗算1臥加減算
J瑠ヱ〃回分の処理時間になり、

J射ノ＝J椚＋f血，わgヱ〝となる。
［RM2の滞在時間］〃〉く〃行列に用いるBM2の処理時間r鮒ヱ は、並列化により乗算1回、加減算l回分の処理時間になり、J月〟ヱ＝J舶＋J桝となる。

1020304050607080gOlOO
次 数図12 滞在時間の比較▲＝提案したアーキテクチャ●：DS P図11（b）プロセッシングブロック2￣A（PB2−A）の構成

脚貯増）

貯昭一噌）

図11（c）プロセッシングブロック2￣B（PB−2B）の構成

図9 滞在時間最小型カルマンフィルタの全件構成

行列A

行列B

第1行 第2行 窮3行

萌1列 窮2列 萌3列（t，1） r2，11 r3，11 什21 （且21 −3．ヱ1 rユ．11 は21 日．31
行研IC

図10 プロセッシングブロック（PBl）の構成
内都は基本モジュールBMl（．）は行列の要素を表す

囲11（a）プロセッシングブロック1（PBl）の構成

紺 d王I珊

瑠 c‡主，現一 寸主I 抑留珊 祀

智づ㌣

智亡皇‡I者

4日；lヰ1，者

留電I

d㌣

翫柑 亡崇I翫柑 一軒柑境一 相打；王l仁王㌢

臨＝．Ⅰ（d）プロセッシングブロック2−C（PB2−C）の構成

紺づ：−相可川呈l鵡，町 村 即断i；ld三川壬I檻柑 瑚ヰFI柑

ぜ増子I

貯削崇I 相好 柑噌，貯 留I断雲I柑貯 電l貯 噌I

可7，掘I

堵I

再訂I

甜 くヂ柑 相島三；一 瑠明㌢瑠卿

祀I

紺 亘㌣珊堀I咄I 噌境，堀I

貯噌一

噌I

昭ぜ

固11（e）プロセッシングブロック2−I〕（PB2−D）の構成

噸I

堵I甘口雲I巧㍗噌I亜I貯 亜I

相即壬言｝相場I d芸；一昭■ 将

可ごIぜ

堀1密

堀1脚 堀】

瑠■ 留■崇I柑柑

司㌣瑠

堵，J紆

7．あとがき本研究では、Faddeev且アルゴリズムを用いた滞在時間墳小型カルマンフィルタのVL
S【アーキテクチャを提案した。これにより、きわめて小さい滞在時間を達成できることがわかった。今後、ハードウエア設計を行い評価を進める予定である。

参考文献
1）片一山 軌 応用力ルマンフィルタ ．朝倉書店
2）H・G・Yeh，llSystolicJmplementationonKImanFilters，lEEETrans・，Acoust．SpeechSignalProcessiong

VO‖旦pp．1514−1517，Sept．198B
3）Kung，Hwang，

SystolicAfrayDesignfbrKalmanFiltering，lEEETrans．，SignalProcessing，VO）．39，No．1，Jan．1991
4）中野，西山． パソコンで解くカルマンフィルタ ，丸善出版
5）G．M．Megson， AFastFaddeevArray ，rEEETrans．Comp．，VO）．4t．No．）2，Dec，t992
6）Gi帖enStrang， 線形代数とその応用 ，産業国苦7）TMS320c5Xユーザーズマニュアル，TEXASINSTRUMENTS，1994