Kalman Filterを使用した...

1 NPO法人M2M研究会・株式会社 HIRO ICT研究所

Kalman Filterを使用した

見守りロボットの設計検討資料

～OpenCV4Androidを使用して～

2014/7/2(v.31)

NPO法人 M2M研究会

株式会社 HIRO ICT研究所


1. 見守りロボットの概要

見守りロボットの機能・移動機能：高齢者のそばに移動する

・顔認識機能：高齢者の顔を認識する

・音声による会話機能：高齢者に話しかける

・音楽機能：高齢者の好みの音楽（クラシック、カラオケなど）を演奏する

見守りロボットの特徴

・多くの部品を市販品で構成できる

・クラウド上のi-見守りサーバと連携して、見守り情報を「見える化」できる

・ベッドセンサー、インスリン注射センサー、投薬支援箱などを接続できる

各種センサー

（BLE接続）

i-見守り君 BLEチップ Roomba

M2M

DB i-見守り

サーバ LTE通信

インスリン見守り君

Roomba

ROIマイコン BLE通信

独居老人

音声

会話

介護ロボット


2. ハードウェア構成

i-見守りサーバ＋ i-見守り君（Android）・Roombaのリモコン機能や音声会話機能などサポートする

・カルマンフィルタ機能を搭載し、見守りロボットの位置制御を行う

・位置測定は、i-Beaconを使用する

・i-見守りサーバに対し、LTE通信により、見守り情報を報告する

Roomba＋BLEチップ・BCM20736を使用して、シリアル信号によりRoombaを駆動する

・スマホ（Android）とBLE通信により指示・報告を行う

・USBカメラ（2台）を搭載し、位置測定や顔認識を行う

BLE通信 i-見守り君 BLEチップ Roomba

M2M

DB i-見守り

サーバ

LTE通信

USBカメラ

i-見守り

センサー i-Beacon i-Beacon 位置測定


3. ソフトウェア構成

i-見守りサーバ・CentOS 6.4（Linux OS）・Apache 2.2.15（Webサーバ）・CakePHP 2.2（フレームワーク）・MySQL 5.1.61（データベース）・Asterisk 1.8 （SIPサーバ）

i-見守り君（Nesxus 7 2013）・Android 4.4.2

・OpenCV Manager 2.18

・アプリケーション：i-見守り君

BCM20736

・BCM20736用ファームウェア

Roomba 800シリーズ・Roomba Open Interface（ROI）を使用


4. OpenCVとM2Mアプリケーション

OpenCV（Open Source Computer Vision Library）とは・オープンソースのコンピューター・ビジョン・ライブラリ

・コンピューターで画像や動画を処理するのに必要な、さまざま機能が実装

・BSDライセンスで配布されていることから、商用目的でも利用できる

・マルチプラットフォーム対応されているため、幅広い場面で利用できる

→Windows, Android, iPhone, Linux

OpenCVの開発

・OpenCVは、最初はIntelで開発された

・その後、Willow Garage（ロボティクス関連の開発）によって開発

・現在は、Itseez（コンピュータービジョンの技術開発）によって開発

MATLABとの比較・MATLABは、シュミレータなので実機に移植できない

・MATLABは、演算が高精度なので実機と異なる場合がある

・MATLABは、函数の中身がブラックボックスなので実機への実装が難しい

・MATLABは、高価なので一般には簡単に利用できない


5. OpenCV4Android

OpenCV for Androidの意味・OpenCV4Anfroidは、NDKによるライブラリ

・Android NDK（Native Development Kit）とは： →Androidアプリの一部または全部をC/C++言語で開発するためのキット

NDKには面倒なCygwinは必要なし・Windows上でNDKを用いた開発を行うためには： →CygwinをインストールしてLinuxエミュレーション環境を作る必要あり

・しかし、EclipseとNDKを連携するプラグインが公開された： →EclipseのみでNDKを用いたアプリケーションの開発が可能になった

OpenCV4Anfroidの主な機能

・行列演算機能：高度な浮動小数点演算が可能

・画像処理機能：カメラ処理、画像変換、トラッキングとモーション、投影等

・機械学習機能：マハラビノス距離、K-means、ベイズ分類器、二分決定木等

・ KalmanFilterクラス：各種センサーのデータを解析し、有用な情報を抽出する


6. KalmanFilter with OpenCV4Android（1）

ソフトウェアセンサーとしての利用・タブレット（Android）は、M2Mゲートウェイとしてのフレームワーク

・自分自身のセンサー機能＋Bluetooth接続による外部センサー機能

・ハードウェアセンサーでは測定できないデータをソフトウェアセンサーで推定

・測定する対象システムをモデル化することで、その状態を推定できる

センサーデータの見える化ツールとして利用・センサーデータの収集状況をリアルタイムに把握できる

→わざわざサーバにデータを取りに行く必要がない

・サーバにゴミデータを送る必要がなく、センサー値も見える化できる

→大半の処理はタブレット（Android）で処理するので、サーバの負荷が軽い

BLE通信 i-見守り君 BLEチップ Roomba

M2M

DB i-見守り

サーバ

LTE通信

USBカメラ

i-見守り

センサー i-Beacon i-Beacon 位置測定

見える化

ツール



ベッドセンサのデータ見える化ツール・ベッドセンサーのようなノイズの多いセンサーから有益なデータを採取できる

→心臓音、呼吸音などをノイズから抽出する

・ノイズの除去：システム雑音と観測雑音の共分散行列を事前に把握

・心臓音の抽出：心臓音の帯域のみ通過する帯域フィルターで観測する

・呼吸音の抽出：呼吸音の帯域のみ通過する帯域フィルターで観測する

・圧力分布の抽出：コールマット、ベッドコール、サイドコール等の代替を行う

音声によるガイダンス機能＆会話機能・テキスト読み上げ機能（KDDI研）により、薬や注射の時間などをアナウンス

・被験者の音声を解析（Google）して会話を行い、状況をサーバに報告する

システム

観測値

ku

kw

システム雑音

KF

フィルタ

ky観測装置

kv観測雑音

kx 状態

kx

状態推定値ベッドセンサー

制御入力

見える化



見守りロボットの走行制御に利用・見守りロボットの制御モデルをAndroidタブレット上に作成する

・制御モデル＋Kalman Filterにより、見守りロボットを制御する

・BLE接続された各種センサーのデータを解析し、有用な情報を抽出する

見守りロボットの位置制御・i-Beaconのアンテナから電波を受信し、電波強度とアンテナIDを計測する

・三角測量法により、自分の位置を計算する

・KFにより、他のセンサー値（ジャイロ、加速度センサー）で補正を行う

システム

観測値

ku

kw

システム雑音

KF

フィルタ

ky観測装置

kv観測雑音

kx 状態

kx

状態推定値位置情報

見える化

i-Beacon

補助

センサー

位置計算

補正値



システム

観測値

ku

制御入力

kw

システム雑音

KF

フィルタ

ky観測装置

kv観測雑音

kx 状態

kx

状態推定値

見守りロボットの走行制御・オドメトリ方式：車輪の回転角の積分により、位置や姿勢を推定する

・舵取り制御：変位と変位の微分に比例した舵取りを行う

・KFにより、位置や姿勢の推定および舵取り制御を行う

見守りロボットの走行制御・オドメトリ方式：車輪の回転角の積分により、位置や姿勢を推定する



F


10. 最初のカルマンフィルタ・アプリケーション（1）

円周上を動くボール・状態方程式：・状態（x）：角度（θ）と角速度（ω）→初期状態は、N（0, 0.1）で与える

・制御入力（u）：なし

・システム行列（F）：[1, 1; 0, Δt]

・誤差の共分散行列（P）：[1, 0; 0, 1]から開始

・観測行列（H）：[1, 0]

・駆動行列（G）：[1, 0; 0, 1]

・システムノイズ（w）：正規分布ーN（0, Q）・観測ノイズ（v）：正規分布ーN（0, R）・OpenCV4Androidを使用したAndroid上のアプリケーション（Java）として作成

円周上を動くボールの酔歩状態（x）の推定を行う・状態方程式と出力方程式は、以下の通リ

kGwkGukFxkx 1 kvkDukHxky

kw

kwG

k

kt

k

k

2

1

10

1

1

1

kv

k

kky

01

kwttkk 11

kwkk 21

10

1 tF

01H

10

01G

kvkky



カルマンフィルタのアルゴリズムを体験する・状態（x）、観測値（y）、状態の推定値（x’）を画面に表示する

・最初は、制御入力（u）は、なしとする

・システムノイズ（w）：当初は1e-5（0.00001）に設定→後で色々、変更してみる

・観測ノイズ（v）：最初は1e-1（0.1）に設定→後で色々、変更してみる

・システム行列（A）：当初は[1, 1; 0, Δt]に設定→後で色々、変更してみる

・誤差の共分散行列（P）：起動時は単位行列に設定する

・観測行列（C）：当初は[1, 0]に設定→後で色々、変更してみる

・状態方程式はとする

カルマンフィルタにより、酔歩状態（x）の推定を行う・システムノイズと観測ノイズの正規分布特性を利用すればできる筈（？） →入力信号のパワースペクトル☓応答函数2＝出力信号のパワースペクトル

→入力信号のパワースペクトル☓応答函数＝入出力信号のパワースペクトル

kGwkGukFxkx 1

0 0m l

uy mllgmg ku ky

kg uS yS

jeG

u

j

y SeGS2

u

j

uy SeGS



カルマンフィルタのアルゴリズムを理解する・多次元正規分布理論と最小二乗法による計算式の理解

・行列式の演算を理解する →行列の乗算、逆行列式など

・次ステップで拡張カルマンフィルタ、パーティクルフィルタの理解を行う

ビジネス用途への応用・ベッドセンサー等の見える化ツールとして

・ロボットの位置制御：iBeacon計測＋ステレオカメラによる距離計算、目標認識

・ロボットの走行制御：RoombaをKFアプリケーションから遠隔制御する

i-見守り君 BLEチップ Roomba

M2M

DB i-見守り

サーバ LTE通信

Roomba

ROIマイコン BLE通信

i-Beacon i-Beacon 位置測定ベッドセンサー

センサー

マイコン


13. 多次元正規分布

多次元標準分布（二次元）・状態（z）：・zの確率密度函数： p（z）・状態（z）の長さ：||z||

・定数（d）： p（z）の積分値が1にする

・共分散行列V[z]：単位行列I

スケーリングとシフト・Dは対角行列：・xの平均値：E[x] = μ と分散

さらに回転を加える・直交行列（Q）をかける：・直交行列とは：QTQ=QQT=I

・分散QD2QTの正規分布を作るには： →V = QD2QT ⇔ QTV = D2

→ 固有値の計算：λ1, λ2, …

zzzzzz T

n 22

2

2

1 ...n

d2

1

2

1

z

zz

2

2

1exp zdzp

IzVzzCov

zzCovzVzV

10

01

,

,

212

211

Dzx

2

2

2

12

0

0

DxV

Qxy

TTT QQDQxQVQQyV 2

2

2

2

1

0

0

2

1

0

0

D

2

1

0

0

D

2

12

0

0

VQQDxV T


14. 多次元正規分布の性質（1）

同時分布の確率密度函数 – 変数変換・線形変換の例：・変換後の確率分布：

・変換後の確率分布：

・式をV = QD2QT で表す：

・待値0の場合の確率分布：

期待値μ の場合の確率分布・とおいて

・とおいて

xVx

Vxp

T

n

1

2

1exp

det2

1

yxpA

wzp yxwz ,det

1, ,,

byaxz dycxw

y

xA

w

z

dc

baA

w

zA

y

x1

211

2

1exp

2

1

det

1

det

1yA

AyAp

Ayp

nzy

AzQDzQxy QDA

yVyyAAyyAyAyA TTTT 111112

1

TTT AAAIAAzAVAzVV

2detdetdetdetdet AAAAAV TT

111111 AAAAAAVTTT

yVy

Vyp T

ny

1

2

1exp

det2

1

yy~

yVy

Vypyp

T

nyy

~~

2

1exp

det2

1~~ 1~

xy ~



期待値ベクトルと共分散行列を指定すれば分布が決まる・N(μ,V)の式から明らか

相関がないだけで独立だと断言できる・X≡(X,Y)T が二次元正規分布の場合、Cov[X,Y]=0ならば：

・確率密度函数は、x1 のみの式とx2 のみの式となる：独立

多次元正規分布を線形変換したら、また多次元正規分布になる・y=Axの時：以下の形になり、また多次元正規分布になる

白色ノイズへの適用・正規分布の白色ノイズは相関がなく独立：

・白色ノイズを線形変換しても、正規分布になる

xVx

Vxp

T

n

1

2

1exp

det2

1

2

2

2

1

212

211

0

0

,

,

xVxxCov

xxCovxVxVV

2

2

2

11

/10

0/1

V

2

2

2

22

2

1

2

111

22exp

2

1exp

xxdxVxdxp

T

yAVyA

VyAp

Ayp

T

nxy

1111

2

1exp

det2

1

det

1

2

xx kxkxE



多次元正規分布（楕円体）の切り口（条件付き分布）・xがn次元正規分布N(0,V)で、x1 = cの時、残りの条件付き分布

→ n-1次元正規分布になる（平均が０でない場合も同じ）

・条件付き分布の期待値ベクトルと共分散ベクトルの計算：以下に対し、

→

・ X=cが与えられた時のYの条件付き分布は： →

・(x,y)T が以下の2次元正規分布の時、 x=cの場合のｙの条件付き分布は：

→

nnnn

n

n

nnxx

x

x

c

xxcdcxxp

2

111

22221

11211

22|~ ,..,,2

1exp|,..,

1

Y

X

WN~

,~

DB

BAN

T,

BABDcABN TT 11 ,

db

baN ,

a

bdc

a

bN

2

,



最小二乗法ではなくカルマンフィルタのアルゴリズム算出が可能・(x,y)T が2次元正規分布で、xとyが独立の場合、同時分布の確率密度函数：

・確率モデル：t=1,2,…に関して、X0, W1 , W2 , …, Z1, Z2 , …は独立で

ただし,

とおいて

2

2

22

2

2,

2exp

2

1

2exp

2

1,

yxypxpyxp yxyx

2

2

2

2

22,

222

1exp

2

1,

yxyxp yx

2

00 ,0 NX

y

xyxyxp yx 2

2

22,

/10

0/1,

2

1exp

2

1,

iii WXX 1 iii ZXY 1

2,0 NWt 2,0 NZt

0

110

111

X

W

Z

X

Yi

i

i

i

110

111J 1,0 VN

X

Y

i

i

2

1

2

1

2

1

22

1

2

0

2

2

1

00

00

00

TJJV 22

01

2

1 XV



Y1=y1が与えられた時のX1の条件付き分布・(x,y)T が以下の2次元正規分布の時、 x=cの場合のｙの条件付き分布が

→

だから、Y1=y1が与えられた時のX1の条件付き分布は、

・t=2に関して、Y1=y1 とY2=y2が与えられた時のX2の条件付き分布は、

・以下、同様に繰り返すと、カルマンフィルタのアルゴリズムになる

db

baN ,

a

bdc

a

bN

2

,

2

1112|~ ,|,..,1

Nyxxp nyx

22

1

1

2

11111 |

yyYXE

22

1

22

1111

2

1 |

yYXV

2

221,23,|~ ,|,..,21

Nyyxxp nyyx

22

2

1

2

2

2

2

22

2

12

2

21112222 ,|

yyyYyYXE

22

2

22

211222

2

2 ,|

yYyYXV

2

2

2

1112

2

2 | yYXV


19. 白色ノイズを利用したシステム同定

期待値白色ノイズの周波数特性・自己相関函数：・パワースペクトル密度：・正規分布の白色ノイズは相関がなく独立

・白色ノイズを線形変換しても、正規分布になる

時系列モデル・AR（Auto-Regressive）モデル： →システムの極の情報だけを用いるモデル

・MA（Moving Average）モデル： →システムの零点の情報だけを用いるモデル

・ARMA（Auto-Regressive Moving Average）モデル： →システムの極と零点の情報を用いるモデル

白色化フィルタ（ARモデル）・時系列データが白色ノイズではない時、A（z）の推定値を求めることができる

・ A（z）を使用したフィルタを白色化フィルタと呼び、有色性信号を白色化できる

時系列モデル

H(z)

kwzHky kw

白色雑音

ttkxkxEt xx 2

22

x

tj

x

tj

xx dtetdtetS

zA

zH1

zBzH

zA

zBzH

時系列


20. 相補フィルタ

速度フィードバックの重要性・閉ループシステムの安定性の改善に速度フィードバックが有効

・タコメータ信号にはノイズn1が含まれ、LPFを適用すると位相遅れが生ずる

加速度計を加えて速度情報を高精度化した相補フィルタを構成・タコメータ信号にはノイズn1はLPF（一次遅れ系：G(s)）で除去する

・加速度計信号にはノイズn2が含まれ、積分処理で速度を計算する

・更に、1-G(s)なるHPFでノイズn2を除去して、タコメータの信号を加える

フィルタリングされた

速度信号

タコメータ信号

1n

加速度計信号

1

1

TssG

LPF

s

1

HPF

1

1

Ts

TssG

2n'

2n

1Ts

T


21. センサーフュージョン

センサーフュージョンとは・異なる特性のセンサの出力を組み合わせて、知りたい量を推定する仕組み

・仕組み（アルゴリズム）の種類：相補フィルタ、カルマンフィルタ、パーティクルフィルタ

相補フィルタ・相補フィルタは，センサの周波数特性に着目する

→例：低周波特性がよいセンサと高周波特性がよいセンサを組合わせる

・センサ単体よりも周波数領域特性を向上できる

カルマンフィルタ・LPFやHPFのような周波数領域ではなく、時間領域で設計されるフィルタ

・信号の特性（位相差）を損なわずにノイズを除去できる相補フィルタ的利用

パーティクルフィルタ・カルマンフィルタの線形、ガウス分布と言う制限をなくした時間領域フィルタ

・モンテカルロ法を用いて最適解を求める。実装はKFより簡単。


22. カルマンフィルタを使用した相補フィルタ

慣性航法システム（INS）のセンサーは、積分要素が多く不安定・センサーとしてジャイロスコープを持ち、角速度を計測する

→角速度を積分して姿勢角を求める

・センサーとして加速度計を持ち、加速度を計測する

→加速度を積分して速度、位置を計算する

補助センサーとして、GPSやドプラーレーダを使用する・INSの観測値： h(x*) = （真の位置、速度等） + n1 （慣性系誤差）・補助センサーの観測値： z(k) = （真の位置、速度等） + n2 （補助系誤差）・ h(x*) – z(k) = n1 - n2 （雑音成分のみ）となるので、カルマンフィルターで除去する

修正された慣性出力

（真の位置、速度など）＋（慣性系誤差）

補助センサー 2nkz

慣性航法

システム

カルマン

フィルタ

1

* nxh

（真の位置、速度など）＋（補助系誤差）

kzxh *

慣性系誤差推定値

1

* nxh

1n


23. カルマンフィルタは、ソフトウェアセンサー

数学モデルベースのアプローチ（システム同定）・入出力データ（時間領域の実データ）→数学モデルに変換

・パラメトリックなモデル（数値化）：伝達函数、状態方程式

・ノンパラメトリックなモデル（グラフ化）：インパルス応答、周波数伝達函数

・カルマンフィルタ：ハードウェアセンサーでは測定不可データをモデルで推定

和分、差分

z変換対

ステップ応答

カーブフィッティング

インパルス応答

g(k) 伝達函数

G(z)

周波数伝達函数

G(ejωT)

状態方程式

(A, B, C, D)

フーリエ

変換対

入出力データ

u(k), y(k)

実現

jwTez

数学モデル

実データ


24. 雑音を考慮した線形システムと一段先予測

雑音を考慮した離散時間LTIシステム・観測値（y）：・LTIシステム伝達函数：・雑音モデル（成形フィルタ）：

一段先予測・時刻（k-1）までに測定された入出力データに基づいた出力ｙの予測値

・証明

kuzGzHkyzHky ,,,1|ˆ 11

LTIシステム

G(z)

雑音モデル

H(z)

kwzHkv

出力

kwzHkuzGky

ku

入力

kw

白色雑音

......1 1

1

n

k

k zngzgzkgzG

kuzG

......111 1

1

n

k

k znhzhzkhzH

kwzHkuzGky

1|ˆ1|ˆ kkvkuzGkky

kvzHkvzH

zHkwzHkwzihkkv

i

i 1

1

11

11|ˆ

kuzGkyzHkuzGkvqHkuzGkky 11

111|ˆ


25. ARXモデル（最小二乗モデル）

式誤差モデル・システムの入出力関係を以下の差分方程式で表す

・誤差e(k)が外乱項として含まれているので、式誤差モデルと呼ぶ

ARXモデル（線形回帰モデル）・式誤差モデルの外乱項e（k）に白色ノイズw（k）を選択：ARXモデルと呼ぶ

kwnkubkubnkyakyaky bnaan b ...1...1 11

kwzH

出力

ku

入力

kw

白色雑音

kuzG

zwzukwzbzbzyzaza b

b

a n

n

n

an ......1 1

1

1

1

kenkubkubnkyakyaky bnaan b ...1...1 11

zA

zBzG

zA

zH1

an

an zazazA ...1 1

1 b

b

n

n zbzbzB ...1

1

kwkuzBkyzA

kwzA

kuzA

zBky

1

出力

ku

入力

kw

白色雑音

kuzB zB

zA

1 ky ky


26. 離散時間LTIシステムの状態空間モデル

離散時間ＬTIシステムの状態空間モデル・連続系の状態空間モデル：・離散系の状態空間モデル： →

・F=（I+TA）、G=TBとおいて離散系の状態空間モデルを得る

・伝達函数：

スペクトル密度函数による離散時間ＬTIシステムの表現・出力y（k）の自己相関函数：・u(k)とy（k）の相互相関函数：

tButAxtx tDutHxty

kTBukTAxkxkx 1

kDukGuFzIHky 1

kTBukxTAIkx 1

kGukFxkx 1 kGukxFzI

kGuFzIkx1

DGFzIHku

kyzG

1

kykyEy

0 0m l

uy mllgmg

ku ky kg uS yS

jeG

u

j

y SeGS2

u

j

uy SeGS

kykuEuy

kDukHxky


27. パラメトリックモデルのシステム同定法（1）

パラメータ推定のための評価函数・評価函数として予測誤差εの大きさを測る正のスカラー函数ｌ(l, θ,ε)を定義

・評価函数の最小値を与える推定値θ を求める

最小二乗推定値（ARXモデルの場合）・スカラー値函数として二次函数を選択：・一段先予測値が推定値θに関して線形を仮定：・予測誤差：

・評価函数をθで微分して0と最適なθを得る：・N個の入出力データに基づく未知パラメータの最小二乗推定値は、

次の正規方程式を満たす：・は既知でありm☓ m 正定値行列

・は未知でありm☓ 1 ベクトル

N

k

N kklN

J1

,,,1

|ˆ, kykyk

,,,, 2 kkkl

kky T |ˆ

kkykykyk T |ˆ,

N

k

TTT

N NcNfNRkkyN

J1

22

1

N

k

T kkN

NR1

1

N

k

kykN

Nf1

1

N

k

kyN

Nc1

21

022 NfNNRJN

NfNNR N

NR Nf


28. パラメトリックモデルのシステム同定法（2）

時刻kにおけるARXモデル・時刻kにおけるARXモデル：・k=1からNまで行列で表すと：

正規方程式（最小二乗法による推定→連立一次方程式）・R(N)とf(N)：・評価函数：

・正規方程式：

・一括最小二乗法：

一括最小二乗法の課題→逐次最小二乗法（カルマンフィルタ）・全てのデータを蓄積する必要がある：サーバでしか処理できない

・逆行列演算を含むので、大量な計算が必要になる

kvkkvkyky T |ˆ

NvNNy

TNyyyNy ...21

TNvvvNv ...21

NUNYN

NNN

NRT

1 NyN

NNf

T

1

21 NNy

NJN

NyNN

NNNN

TT

1ˆ1

NyNN

NNN

NTT

11ˆ

1


29. 逐次最小二乗法（1）

逐次最小二乗法（RLS：Recursive Least-Squares Method）・一括処理システム同定法： →サーバでの処理向き

・逐次システム同定法→スマホ、マイコンなどでのリアルタイム処理向き

・故障診断などは、オンラインでモデルパラメータを推定する必要がある

→一括処理同定法のような逆行列演算は必要ない

・一括最低二乗法の式を変形

共分散行列の導入・共分散行列の定義：

・同様にして

・従って

・しかし、このままではP(N)のリアタイムな計算が難しい→解決策は次ページ

NyNN

NNN

NTT 11ˆ

1

1

1

1

kk

NNP T

k

NNNPNNkkkkNP TTTN

k

TN

k

111

11

1

kykkkNk

T

k 1

1

1

ˆ

NyNkykkykN

k

N

k

1

11

NyNNNPNPNyNkykNPNN

k

1ˆ1ˆ 11

1

NyNNNNPNN T 1ˆ1ˆˆ

1ˆ1ˆˆ NNNyNNPNN T


30. 逐次最低二乗法（2）

逆行列補題（Woodburyの公式）で共分散行列を変形・逆行列補題（Woodburyの公式）：・共分散行列の式：

・一括処理最低二乗法の式：

111111 CABCAIBAABCA

11 1 NNNPNP T

11111

NPNNNPNINNPNPNP TT

NNPN

NPNNNPNPNP

T

T

11

111

NNNPNNNNyNNPNN T 1ˆ1ˆ1ˆˆ

N

NNPN

NPNNNPNPNNP

T

T

11

111

NNPN

NNPNNNPNNPNNP

T

T

11

111

NNPN

NNPNNP

T

11

1

NNNPN

NNPNN

T

11

11ˆˆ

1ˆ NNNyN T

N

NNPN

NPNNNPNPNNP

T

T

11

111



逐次最小二乗法（RLS）のまとめ・３つの式が重要（RLS三羽鴉）：

・初期値：（γは正定数）・時間更新式：

逐次最小二乗法（RLS）の利点・計算式が簡単：数列の漸化式

・逆行列の演算を含んでいない：入力信号のPE性が弱い場合も対応可能

・P（0）は、∞が望ましい（103～104）がγの選定には注意（ロバスト性に影響）

NNNPN

NNPNN

T

11

11ˆˆ

1ˆ NNNyN T

NNPN

NPNNNPNPNP

T

T

11

111

0ˆ0ˆ IP 0

kkkPk

kkPkk

T

11

11ˆˆ

1ˆ kkkyk T

kkPk

kPkkkPkPkP

T

T

11

111



パラメータ推定問題の状態空間定式化・真の値θがランダムウォークと考える：システムパラメータθが確率変数

ただし（白色ノイズ）（ARXモデルの場合）・状態方程式への対応：

逐次最小二乗法（RLS）のカルマンフィルタへの応用・カルマンフィルタのアルゴリズム：

・カルマンフィルタのアルゴリズムとRLS法との対応

とおく

1ˆˆ kkkykykyk T

kkPk

kPkkkPRkPkP

T

v

T

1

111

2

kkQkK

11** kkk RkkE T

kvkkky T *

kkxkx 1

kvkkky T *

kkKkk 1ˆˆ

kkPk

kPkQ

T

v 1

12

0R 12 v


33. 参考：逆行列補題（Woodburyの公式）

逆行列補題による誤差共分散行列の計算・カルマンフィルタの逐次的アルゴリズムで使用される公式

より一般の形式・一般の形式で、D=Iとすると上式となる

・右辺が（A+BDC）の逆行列になっているか確かめる

111111 CABCAIBAABCA

1111111 CABCADBAABDCA

111111 CABCADBAABDCA

111111111111 CABCADBBDCACABCADBAABDCAAA

11111111111 CABCADBBDCACABCADBDDBDCAI

1111111 CABCADBCADBDBDCAI

IBDCABDCAI 11


34. 参考：自己相関函数と相互相関函数

平均値と自己相関函数・平均値：・自己相関函数：・弱定常（二次定常）：（一定）（tに依存しない）・分散（τ=0の時の自己相関函数）：・エルゴート性（時間平均）=（集合平均）：

相互相関函数・相互相関函数：

・分散函数：・共分散函数：

・相互係数： μx=0, μy=0の時

・xとyが無相関とは：・xが無相関とは：

dxxpxtxEtx

212121 ,, dxdxxxpxxtxtxEttx

xx t xx tt ,

220 xx txE

2

2

1lim

T

TT

x dttxtxT

2

2

1lim

T

TT

x dttxT

xx

2

2

1lim

T

TT

xy dttytxT

tytxE

2

xxxxx txtxEC

yxxyyxxy tytxEC

00 yx

xy

xyCC

C

0xy

00 yx

xy

xy

2

xx


35. 参考：パワースペクトルとフーリエ解析

パワースペクトル密度函数（S）は、自己相関函数のFourier変換・ウィーナー・ヒンチンの定理： ⇔

・相互スペクトル密度函数： ⇔

・コーヒレンス函数：（関連度函数）

コーヒレンス函数による推定（1より小さい場合）・xとyが線形にない

・yがx以外の信号の影響を受けている

・yに雑音が混入している

離散時間不規則信号のスペクトル解析・パワースペクトル密度函数：

・相互スペクトル密度函数：・直接法（ペリオドグラム法）：・間接法（コレログラム法）：自己相関函数をフリーエ変換する

dtetS tj

xx

deSt tj

xx

2

1

dtetS tj

xyxy

deSt tj

xyx

2

1

yx

xy

xySS

S2

10 xy

t

tj

xx etS

t

tj

xyxy etS


36. 参考：状態方程式と観測値

状態方程式とその解・状態変数：x(t) , 観測値：y(t), システムノイズ： w(t), 観測ノイズ： v(t)

・システム行列： A , 制御行列：B , 観測行列： H, 直達行列：D

・駆動行列：G

・状態遷移行列

・状態方程式の解

観測値＝自由応答＋強制応答・自由応答（第1項）：・強制応答（第2項）：

tGwtButAxtx

tvtDutHxty

dGwBuexetxt

tAAt

00

tvtDudGwBueCxCetyt

tAAt

00

0xCeAt

tvtDudGwBueCt

tA

0

....!

1...

2

1 22 kkAt tAk

tAAtIet


37. 参考：インパルス応答とステップ応答

インパルス応答・u(t) = δ(t) の場合：システムノイズも観測ノイズもないものとする

・インパルス応答h(t)を使用して、強制応答を表現できる

ステップ応答・u(t)：t > 0 でu0（定数）、その他で 0

周波数応答・u(t) = u0 e

jωt の場合、過度応答＋周波数応答

duthtyt

0

tDBCetDdBeCth Att

tA

0

0

0000

0DudeCBuDudBueCty

ttA

ttA

0

1 uDBIeCAty At

tuDBAIjCxCety At 1

0

tujGxCety At 0


38. 参考：状態フィードバックと最適制御（1）

状態フィードバック・古典制御：観測値（y）をフィードバックする

・現代制御：状態（x）をフィードバックする（新しい入力をvとする）

・状態方程式は以下のようになる

最適制御・どのようなFを指定すると最適な制御ができるか：評価函数の導入

tvtKxtu

tBvtxBKFtvtKxBtFxtx

tDvxxDKHxvtKxDtHxty

tv ty

K

B z-1 H

F

tx

D

tFx

tu


39. 参考：状態フィードバックと最適制御（2）

最適制御のための二次形式評価函数の導入・最適制御入力（u）とRiccati状態方程式（P）

最適制御・状態フィードバック系の安定性：漸近安定

だから

だから、以下の閉ループ系は安定

・最適性の証明

ft

TT dttRututQxtxJ0

tFxtu PBRF T1

01 QPBPBRPAPA TT

tRututxPBPBRPAPAtxtRututQxtx TTTTTT 1

txPAPAtxtRututPxBPBRtx TTTTT 1

tBvtxPBBRAtBvtxBFAtx T 1

RFFQPBFABFAP TTRFPBT

01 KRRKQ T

tPxxtxPtxtTxBtutPButxtRututPxBPBRtx TTTTTTTT 1

tPxtxdt

dxPxBRtuRxPxBRtu TTTT 11


40. 参考：オブサーバ（観測器）

観測値（y）と制御入力（u）の情報から、状態（x）を推定する・状態の誤差をとすると、t→∞で誤差が0になる A-LH が存在

ku

ty

L

ty

観測値の時系列

線形システムの

モデルをコピー

tx

状態推定値

制御入力

観測値の推定値

tyty ˆ

観測対象の線形システム

B z-1 H

F

オブザーバ

B z-1 H

F

tx txHˆ

txtxte ˆ

tytyLtButxFtButFxtxtxte ˆˆˆ

teLHAtxtxLHtxtxFte ˆˆ

tHx tx

txtxLC ˆ

tx

tx


41. オブザーバとしてのカルマンフィルタ

観測値（y）とシステムノイズ（w）の情報から、状態（x）を推定する・事後推定値 = 事前推定値 + カルマンゲイン☓出力予想誤差

線形システム

x(k)

ku

ky

モデルに基く

Kalman Filter

0x

Kalman Gain

K(k)

kv

0P kyˆ

初期値

観測値の時系列

線形システムの

モデルをコピー

事後状態推定値

制御入力観測ノイズ

観測値の事前推定値

（一段先予測値）

kyky ˆ

kwシステムノイズ

kxCkyKkxkykyKkBukxFkx ˆˆˆ11ˆˆ

1 VHPK T

kxHkyKkxkx ˆˆˆ

kykyK ˆ

kxHkyKkxkx ˆˆˆ


42. ウィナーフィルタとカルマンフィルタの比較

ウィナーフィルタでは・状態x(t), 観測値y(t), 観測雑音v(t)はガウス過程

・アルゴリズムは、オフライン的でフィルタは非因果的

カルマンフィルタでは・観測信号からノイズを濾過し原信号を取り出す信号処理アルゴリズム

・モデルベースのアプローチ

システム

観測値

ku

外部入力

kw

システム雑音

フィルタ ky

観測装置

kv観測雑音

kx 状態

kx

状態推定値

観測値

kx

原信号フィルタ

ky

kv観測雑音

kx

状態推定値


43. イノベーション過程（出力予測誤差）

線形予測器・状態の推定値を事前推定値と観測値に関して線形と仮定する

・Bayes推定：事後推定値 = G(k)・事前推定値 + g(k)・最新の観測値

が成り立つ必要がある（事前推定値と観測値は直交）

ただし出力予測誤差：

イノベーション過程（出力予測誤差）・一段先予測値：・イノベーション過程（出力予測誤差）：

・事後推定値 = 事前推定値 + カルマンゲイン・最新の観測値

kykKkkxkGkx 1|ˆˆ

CkKIkG

1|ˆ1|ˆ1|ˆˆ kkxCkykKkkxkykKkkxCkKIkx

kykKkkxkkykykKkkxkx ~1|ˆ1|ˆ1|ˆˆ

1|ˆ1|ˆ kkxCkky 1|ˆ~ kkykyky

1|ˆ1|ˆ kkxCkky

1|ˆ~ kkykyky

1|ˆ~ kkykyky

1|ˆ1|ˆ~ kkxCkvkCxkkxCkyky

kvkkxCkvkxkkxkxCky )1|(~1|ˆ~


44. カルマンフィルタのアルゴリズム

RLS法はカルマンフィルタの特殊ケース・ARXモデルに対するカルマンフィルタの時間更新式

・ただし

カルマンゲイン（スカラ値）の計算・事前共分散行列を使用する

1ˆˆ kkkykykyk T

RkPkkKIkP T 1

kkQkK

1ˆ1ˆ1ˆˆ kkkykKkkkKkk T

kkPk

kPkQ

T

v 1

12

kkPk

kPkkkPRkPkP

T

v

T

1

111

2

T

v

T

T

v CkkPkC

CkkP

kkPk

kkPkK

1|

1|

1

122

0R


45. カルマンフィルタのサイクル

状態空間モデル・時刻k-1における事後推定値（x）から事前推定値（x‘）を計算する

予測ステップ・時刻k-1における事後推定値（x）から事前推定値（x‘）を計算する

・predict()メソッドを使用する→ statePre （x）が計算される

・事前誤差共分散行列（P’）は、以下のように計算する

フィルタリングステップ・時刻kにおける事後推定値（x）を計算する

・correct()メソッドを使用する→ statePost （x）が更新される

・事後推定値（x） = 事前推定値（x‘） + カルマンゲイン☓出力予想誤差

・事後誤差共分散行列（P）：・カルマンゲイン（K）：

1|ˆ1|ˆˆ kkxHkyKkkxkx

111ˆ1|ˆ kGwkBukxFkkx

T

w

T GGFkFPkkP 211|

RkkPHkKIkP 1|

121|1|

v

TT HkkHPHkkPkK

kGwkBukFxkx 1 kvkDukHxky

0R


46. KalmanFilterクラス class KalmanFilter

{

public:

// Public Member Functions

（省略） // Public Attributes

Mat controlMatrix; // 制御行列 (F)：（制御していない場合は，これは利用されません） Mat errorCovPost; // 更新された誤差の共分散行列 (P(k)): P(k) = (I-K(k)*H)*P'(k)

Mat errorCovPre; // 今の時刻の誤差行列 (P'): P'(k) = F*P(k-1)*FT + Q

Mat gain; // 最適カルマンゲイン (K): K(k) = P'(k)*HT*(H*P'(k)*HT+R)-1

Mat measurementMatrix; // 観測行列 (H)

Mat measurementNoiseCov;// 観測ノイズの共分散行列 (R)

Mat processNoiseCov; // システムノイズ（時間遷移に関するノイズ）の共分散行列 (Q)

Mat statePost; // 更新された状態の推定値 (x): x(k) = x'(k) + K(k)*(y(k) - H*x'(k))

Mat statePre; // 状態の推定値 (x'): x(k) = A*x(k-1) + B*u(k)

Mat temp1; // 予備1





Mat transitionMatrix; // システムの時間遷移に関する線形モデル (A)

};


47. KalmanFilterクラスのメンバー（1）

controlMatrix：制御行列（B）・制御入力がない場合は、空である

errorCovPost：誤差の事後共分散行列（P）・事後共分散行列：P(k) = (I - K(k)*H)*P’(k)

・correct()メソッドで計算される

errorCovPre：誤差の事前共分散行列（P’）・事前共分散行列: P’(k) = F*P(k-1)*FT + Q

・predict()メソッドで計算される

gain：最適カルマンゲイン（K）・K(k) = P’(k)*HT * (H*P’(k)*HT + R)-1


measurementMatrix：観測行列（H）・観測行列 (H)：今回は単位行列とする

measurementNoiseCov：観測ノイズの共分散行列（R:1e-1）・共分散行列 (R)：カルマンゲイン（K:gain）の計算で使用

K(k) = P‘(k)*HT*(H*P’(k)*HT + R) -1

01


48. KalmanFilterクラスのメンバー（2）

processNoiseCov：システムノイズの共分散行列（Q:1e-5）・共分散行列 (Q)：事前誤差共分散行列（P’:errorCovPre）の計算で使用

P‘(k) = F*P*FT + Q


statePost：状態の事後推定値 (x) ・事前推定値（x '）と観測値（ｙ）から事後推定値（x: statePost）を更新する

・ x(k) = x'(k) + K(k)*(y(k) – H*x'(k))


statePre：状態の事前推定値 (x') ・今までの事後推定値（x）から、（事前推定値（x ‘: statePre ）を計算する

・ x' (k) = F*x(k-1) + B*u(k)


transitionMatrix: システムの時間遷移に関するモデル（A）・システムに依存する行列を設定する

・今回のシステム行列 (F)

10

11F


49. KalmanFilterクラスのメソッドの処理

予測フェーズ・Mat predict(Mat control);

・今迄の事後推定値（x: statePost）から、事前推定値（x‘ : statePre）を計算

x’(k) = F*x(k-1) + B*u(k)

・事前誤差共分散行列（P’:errorCovPre）の計算

P‘(k) = F*P*FT + Q

・現在の事後推定値（x’ : statePre）を戻り値として返す

更新フェーズ・void correct(Mat measurement);

・カルマンゲイン（K:gain）の計算

K(k) = P‘*HT*(H*P’*HT + R)-1

・観測値（y: measurement ）から状態の事後推定値）の計算

x(k) = x’(k) + K(k)(y(k) – H*x’(k))

・誤差の事後共分散行列（P:errorCovPost）の計算

P(k) = (I – K*H)*P’(k)


50. KFアプリケーションのサイクル

1.KalmanFilterクラスの生成 // KalmanFilter(int dynamParams, int measureParams, int controlParams=0);

// dynamParams=状態変数の次元数(2)、measureParams=観測変数の次元数(1)

KalmanFilter KF(2, 1, 0);

2.状態（点の角度）を推定する // statePost から statePre を計算する

Mat prediction = KF.predict(control);

// 観測値(y=角度)を観測ノイズ共分散行列から計算する

// 平均値=Scalar::all(0), 分散=Scalar::all(KF.measurementNoiseCov.at<float>(0))

randn( measurement, Scalar::all(0), Scalar::all(KF.measurementNoiseCov.at<float>(0)));

3.モデルの状態を更新する // モデル状態の更新 – mesuarement（観測値） KF.correct(measurement);

// プロセスノイズ(w)を共分散行列から計算する（sqrtは配列要素の平方根を求める関数） // 平均値=Scalar(0), 分散=Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0)))

randn( processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0))));

// 新しい状態変数（state）を計算する = システム行列（A）*今までの状態変数 + プロセスノイズ

state = KF.transitionMatrix*state + processNoise;

// 2.へ行く。ただし、終了条件があれば4.に行く。

4. KalmanFilterクラスの開放または再設定


51. JavaのKalmanFilterクラス

Matクラス・Matクラスは，実際のデータへのポインタと，様々なプロパティ（幅，高さ，ビット深度など）を保持します。

・AndriodのBitMapは、Matクラスに変換する必要あり(bitmapToMat)

KalmanFilterクラスのConstructer ・KalmanFilter()

・KalmanFilter(int dynamParams, int measureParams)

・KalmanFilterint dynamParams, int measureParams,

int controlParams, int type

次のモデルの推定メソッド・Mat predict()

・Mat predict(Mat control) – 入力制御信号がある時

モデル状態の更新メソッド・Mat correct( Mat measurement )


52. KFアプリケーションの見える化機能

KFアプリケーション画面（2014/6/30現在）・画面上部：各種データ（行列）の値がリアルタイムに表示される

・画面下部：状態（x）、観測値（y）、状態推定値（x’）の位置が表示される

i-見守り君初期画面 KFアプリケーション画面


53. 見守りロボットへの適用

見守りロボットの位置の把握・i-Beaconのアンテナから電波を受信し、電波強度とアンテナIDを計測する

・三角測量法により、自分の位置を計算する

・KFにより、他のセンサー値（ジャイロ、加速度センサー）で補正を行う

見守りロボットの位置制御、走行制御・オドメトリ方式：車輪の回転角の積分により、位置や姿勢を推定する



システム

観測値

ku

制御入力

kw

システム雑音

KF

フィルタ

ky観測装置

kv観測雑音

kx 状態

kx

状態推定値

F


54. ベッドセンサーへの適用

安価なベッドセンサーの課題・安価なベッドセンサーから心臓の鼓動音や呼吸音などを検出する

・ノイズが多いのでカルマンフィルターを使用して解析する

・また、相関解析法、周波数応答法、スペクトル解析法なども利用する

相関解析法・システム同定実験データから対象のインパルス応答を調べる

・白色系入力を用いてたインパルス応答を調べる

周波数応答法・正弦波入力の応答によりLTIシステムを調べる

・定常状態において出力も正弦波だが、ゲインと位相が異なる

・信号にノイズが含まれる場合は、相関を用いた周波数応答法を使用する

スペクトル解析法・信号にノイズが含まれる場合に、そのパワースペクトル密度も推定できる

・他のシステム同定結果のリファレンス（参考値）として利用できる


速度制御系の等価構造・速度制御系は、ベクトル制御系の上位の制御系である

55. 見守りロボットのモータ制御モデル（工事中）

sC

sD*

2m

ic

ic

s

mm DsJ

1

* m2

モータ電流制御変換器

*

2m

m2

R Invertor

PG

TR

S

TS

sin

cos

sin

cos

ti1

tv1

*

1tv*

1sv*

1rv*

1ri

*速度

制御

si1ri1

PMSMモータのベクトル制御系・PMSMモータのベクトル制御系の代表的な構造を示す


主なROIコマンド・START：スタート指令。passiveモードへ遷移。最初にかならずこれをやる

・CONTROL ： passive -> safeモードに遷移

・SAFE： full -> safeモードに遷移

・FULL： safe -> fullモードに遷移

・POWER： sleepモードに遷移

・SPOT/CLEAN/MAX: full/safe -> passive モードに遷移

・DRIVE：パラメータV, Rはsignedの２バイト[vh][vl][rh][rl]

→並進速度V[mm/s], 回転半径R[mm]で動く。正にすると左前に進む

-500 < V < 500, -2000 < R < 2000

32768 = 0x8000hのとき直進

-1で時計回り、1で反時計回りにその場回転

56. Roombaの制御

三端子レギュレータ

（秋月電子：TA48M05F） Bluetooth を使用した接続例

（Strawberry Linux： BlueSMiRF ）接続端子 Roomba


BCM20736とは・BCM20736は、ARM CM3 マイクロコントローラー・ユニット（MCU）、無線周波数（RF）、組み込みBluetooth Smartスタックのすべてを1チップに統合

・A4WP（Alliance for Wireless Power）標準のワイヤレス充電のサポートが組み込むことが可能になり、 Internet of Thingsのエコシステムに対応する

・高度に統合された小型設計はシングル・モードで1.2v以上のコイン電池動作に最適化された電源で動作し、消費電力を削減し、ウェアラブル・デバイスのバッテリ寿命を延ばすことが可能となる

・パッケージは6.5×6.5mmの小型パッケージで提供され、同社の既存のBluetooth Smart SoCとのピン互換性を確保している

57. 見守りロボットの制御プロセッサ

BCM20736 開発キット

Kalman Filterを使用した...

Documents

Transcript of Kalman Filterを使用した...