Embed Size (px)
DESCRIPTION
ＵＫＦ（ Unscented Kalman Filter ) と その応用. 東京工業大学大学院 機械制御システム専攻 山北 昌毅. 状態推定の必要性. システムの将来の挙動の予測 状態フィードバック パラメータ推定. 状態推推定器（オブザーバ）. 状態推定とパラメータ推定. 内容. 最小分散推定値 Ｋａｌｍａｎ Filter 線形カルマンフィルタ 拡張カルマンフィルタ Unscented Kalman Filter (UKF) 例題を用いた状態推定比較 UKF とＲＨＣの併用例 まとめ. 期待値. 分散. 次統計モーメント. - PowerPoint PPT Presentation
Citation preview
1
ＵＫＦ（ Unscented Kalman Filter)
とその応用
東京工業大学大学院機械制御システム専攻
山北 昌毅
状態推定の必要性
• システムの将来の挙動の予測
• 状態フィードバック
• パラメータ推定
2
状態推推定器（オブザーバ）
3
ˆ ˆ[ 1] [ ] [ ]
ˆ[ ] [ ]
ˆ ˆ ˆ[ 1] [ ] [ ] ( [ ] [ ])
ˆ[ ] [ ]
ˆ[ 1] [ ], [ ] : [ ] [ ]
x k x k u k
y k Cx k
x k x k u k K y k y k
y k Cx k
x k KC x k x k x k x k
オープンループ推定
オブザーバ
エラーシステム
状態推定とパラメータ推定
4
1 1
1
1
1 1
[ 1] [ ] [ ]
[ ] [ ]
{ [ ], [ ], 0,..., } [ ]
[ ] [ 1] [ ] [ 1] [ ]
[ 1] [ ] [ 1] [ ]
[ ]
[ 1] [ ]
[ ]
n n
n
n
T
n n
x k x k u k
y k Cx k
u i y i i k x k
y k a y k a y k n b u k b u k n
a
ay k y k n u k u k n
b
b
x k a a b b
x k Ix k
y k
より を推定する
, 0
[ ] [ 1] [ ] [ 1] [ ][ ] [ ]
I
C k y k y k n u k u k nC k x k
5
内容
• 最小分散推定値• Ｋａｌｍａｎ Filter
– 線形カルマンフィルタ– 拡張カルマンフィルタ– Unscented Kalman Filter (UKF)
• 例題を用いた状態推定比較• UKF とＲＨＣの併用例• まとめ
6
x•期待値
dxxpxXxXE kk )()()(
2x•分散
dxxxpXEx )(
dxxpxxxXEx )()()( 222
• 次統計モーメントk
１次モーメント：平均２次モーメント：分散
３次モーメント：歪度４次モーメント：尖度
7
ガウス分布
2
2
2 2exp
2
1)(
xx
xp
xXE •平均：
22 xXE•分散：
012 nxXE•奇数次モーメント：
nn nxXE 22 1231 •偶数次モーメント：
•１次元
8
•多次元nRx { },
Tx E x E X x X x
xXxXxp T
n
12/12/ 2
1exp
2
1)(
•２次元
2
2
221
211
xxx
xxx
9
•２次元分布（相関による変化）
2
2
221
211
xxx
xxx
5.00
00.1
5.05.0
5.00.1
5.05.0
5.00.1
3
2x
10
Fact正規分布を持つ確率変数の Affine 変換された確率変数の確率分布は正規分布となる。つまり、
xを正規性の確率変数 yとして を次の式で定義する
, , ,n m m n my Ax b x R y R A R b R
yは正規性確率変数となり平均、分散は次の式で計算される
{ }: { }E y y AE x b Ax b
{( )( ) }: { ( )( ) }
{( )( ) }
T T Tyy
T T T
E y y y y E A x x x x A
AE x x x x A A A
11
最小分散推定値
x推定したいパラメータ
y観測量
)( xyp
評価関数 を最小にする推定値
2xxE
条件付き期待値 と等価
yxEx ˆ
（ の分布の種類によらず）x
x推定量
推定ルール)(yg
2
2
ˆ ˆ ˆ ˆ ˆ|| || ( ) ( ) ( ) ( )
ˆ ˆ( )( )
ˆ ˆ ˆ{|| || } {( )( ) }
T T
T
T
x x x x x x trace x x x x
trace x x x x
E x x traceE x x x x trace
12
•証明 dxdyyxpyxfYXfE ),(),(),(
dyypdxyxpyxf )()(),(
( , ) , ( , ) : ( , ) ( )E E f X Y Y E f X Y Y f x y p x y dx 2 2
( ) ( )E g Y X E E g Y X Y
2( )E E g Y E X Y E X Y X Y
2 2
2 2
, : { | }
|| || 2 || ||
|| || 2 || ||
T
y y y y y
T
y y y y
T
y y y y
E E g x x X g x x X Y x E X Y
E E g x g x x X x X Y
E E g x Y E E g x x X Y E E x X Y
13
続き
0 0
T T
y y y y
T
y y
T
y
E E g x x X Y E g x E x X Y
E g x x E X Y
E g x
22 2( ) ( ) y yE g Y X E E g Y x Y E E x X Y
これが最小になるのはyxYg )( の時。
YXEYg )( が最小分散推定値となる
つまり、
)(Yg と無関係
14
ここまでのまとめ
•どうやって条件付期待値を求めるのか
•最小分散推定値は条件付期待値である。
•条件付確率密度関数が得られても計算が困難
•ガウス分布を仮定すると容易に計算可能
15
ガウス分布の条件付期待値
yyAxyxEx 0ˆ
yyxExyxExET yxxx A 0
yxyyA 0
TT
TT
yyyx
xyxx
yyyyExxyyE
yyxxExxxxE
注：ガウス分布でないとき線形最小分散推定値となる bAyx ˆ
10
yyyxA
TTyy
Txy A0 より上の共分散
は
Tyyxx AA 00
16
11 12
12 22
122 11 12 22 12
1 11
1 1 1
111 12 22 12
112 22
22
112 22
[ ]
,
is nonsingular, is nonsingular
1. ( )
:
:
2.det( ) det( )det( )
( )
=0
TT
T
T T
T
n
n
X X Y
X Y Z Y X Y
X
Y
Z
X Z
I
I
補題
証明1
11 12 22 12
12 22
0T
T
証明
17
条件付確率密度関数
1 2
1/ 2 1/ 2
1 2
1 / 2 1/ 2
11, 2
2
1 1 11 12
( ) / 2 1/ 22 2 12 22
1 1( ) ( ( ) ( ))
(2 ) (det( )) 2
:
1( ( ) ( )) (2 ) (det( ))
2
( )
1 1( ( )
(2 ) (det( )) 2
Tn
T
n
T n
TTn n
p X Exp X X
X
Exp d
Xp p X X
X
XExp
X
1
1 1
2 2
1
1 1 11 12 1 1
2 2 12 22 2 2
1 11 1 1 1
1 1 12 2 2 2
11 1 2 2 1 1 2 2
( ))
( ) ( )
( )
( ( )) ( ( ))
TT
T
T T
T
X
X
X X
X X
X XX X Y
X XX Y Z Y X Y
X Y X X X Y X
1
2 2 2 2( ) ( )TX Z X
18
1 2
2
1
12 1 2 1 2 2 2 2( )/2 1/2
11 1 2 2 1 1 2 2 1
12 2 2 2/2 1/2
1 21 2
2
/2
1 1( ) ( , ) ( ( ) ( ))
2(2 ) (det( ))
(( ( )) ( ( ))
1 1( ( ) ( ))
2(2 ) (det( ))
( , )( | )
( )
1
(2 ) (det(
Tn n
T
Tn
n
p X p X d Exp X Z X
Exp X Y X X X Y X d
Exp X Z XZ
p X Xp X X
p X
1 2
1
1 2 2
1
1 1 2 21
2
1 1 2 2/2
1( ( ( )) ( (
{ | } ( )
{ | }
)))2))
T
E X X
Exp
Y X
Var X X X
X Y X X X Y XX
確率密度関数のレベル集合との関係
19
( , )y x
y
x
yy xy ’ 大 ’きく の効果が小さく
偏差と観測値の‘直交条件’
20
112 22
12 22 12 12
12 22
112 22
ˆ{( ) } ?
ˆ ( ),
{( ( )) } {( ( ))( ) }
0 0 0
0
T
T T
E x x y
x x Y y y Y
E x x Y y y y E x x Y y y y y y
Y Y
Y
Y
Y
逆に を知らなくとも、
より、 が出る
21
線形カルマンフィルタ•対象モデル
nRx k ][ 状態ベクトル
][][][][
][][][]1[
kkkk
kkkk
wxCy
vxAx
観測ベクトル
lRy k ][
状態外乱ベクトル
nRv k ][
観測外乱ベクトル
lRw k ][
0[ ][ ] [ ] [ ]
0[ ]T T k
k
QkE k l k l l
Rk
vv w
w
1k k k
k k k
x Ax v
y Cx w
22
•線形カルマンフィルタ初期推定値 とその予測誤差共分
散1|0ˆ x 1|0 P
1
1|1|
k
Tkkkk
Tkkkk RCPCCPW
)ˆ(ˆˆ 1|1|| kkkkkkkkk xCyWxx
kkkkk xAx ||1 ˆˆ
１．カルマンゲインを計算
２．前回の予測推定値を観測値との誤差で修正
４．推定誤差共分散行列を更新
３．予測推定値を計算
1|1|| kkkkkkkk PCWPP
( )1| | | | 1 | 1T T
A P P W C P A Qk k k k k k k k k k k k k k k k k kP A P A Q
23
•証明[ ]
[ 1] [ ][ ]
[ ] [ ][ ]
0
0
[ 1]
[ ]
kk k
kk k
k
x x k
y y
xx A I
vky C I
w
と考える
1| 1 | 1
| 1 | 1
[ ]
[ ]
ˆ ˆ
ˆ ˆk k k k
k k k k
k
k
x A x
y C x
誤差ベクトルの同時確率密度関数の分散は以下のように計算される
| 1 | 1
| 1
[ 1] [ ] [ ] [ ]
[ ] [ ] [ ] [ ]
[ ] : [ ] , [ ] : [ ] [ ]
: { [ ] [ ]}
[ ]
k k k kT
k k
k
k k k k
k k k k
x k x k x y k y k C k xx A x v
P E x k x ky C x w
A k A
| 1 | 1
| 1 | 1
[ 1] [ 1] [ 1] [ ]
[ ] [ 1] [ ] [ ]
T TT Tk k k k k k k k
T TT Tk k k k k k k
k k k k
k k k k
A P A Q A P Cx x x yE
C P A C P C Ry x y y
両辺の条件無しの期待値をとる。（ｋ－１時刻までの情報による期待値）
|, [ ]
[ ]
i jP y j
x ii | jxただし、 はそれぞれ まで観測された
ときの の条件付期待値及び条件付分散である
24
| 1
| 1
| 1
| 1
1
1| 1 | 1 | 1
ˆ ˆ
ˆ
ˆ
( [ ] )
ˆ( ( ) ( [ ] ))
k k k
k k k
Tk k k k
Txy k k k k
xy yy
T Tk k k k k k k k k k
A x
y C x
C P C R
A P C
z x y k y
A x P C C P C R y k y
k+1|k
yy
X=x[k+1], Y=y[k]ここで と考える x=xと は
x=
として
1
| 1 | 1 | 1 | 1
1
| 1 | 1 | 1 | 1
| 1 | 1
( )
( )
T T T Tzz k k k k k k k k k k k k k k k
T T Tk k k k k k k k k k k k k k
Tk k k k k k k k k
A P A Q A P C C P C R C P A
A P P C C P C R C P A Q
A P W C P A Q
25
従ってゲインを次のように定義し、
1
1|1|
k
Tkkkk
Tkkkk RCPCCPW
)ˆ(ˆˆ 1|1|| kkkkkkkkk xCyWxx
kkkkk xAx ||1 ˆˆ
最適な推定値は次式で与えられる
1|1|| kkkkkkkk PCWPP
kTkkkkkk QAPAP ||1
| |ˆk k k kx P、 を次の式で定義すると、
26
シミュレーション例•３次の AR モデルのパラメータ推定
kkkkk wyayayay 332211
Tk aaax 321
kk xx
1
1
1
1
kkkkkk wxyyyy 321
•状態空間モデル
27
シミュレーション結果
76.21 a
5329.22 a
778688.03 a
0.1RIP 0
0ˆ0 x
1a
2a
3a
観測回数
28
線形カルマンフィルタ（入力あり）
•対象モデル m
k Ru 入力ベクトル
ほとんど同様に計算できる
1
1|1|
k
Tkkkk
Tkkkk RCPCCPW
)ˆ(ˆˆ 1|1|| kkkkkkkkk xCyWxx
1|1|| kkkkkkkk PCWPP
kTkkkkkk QAPAP ||1
1k k k k k k
k k k k
x A x B u v
y C x w
1| |ˆ ˆk k k k k k kx A x B u （条件なし期待値の計算）
白色化フィルター（イノベーションプロセス）
29
1
| | 1 | 1
1| |
| 1 | 1 | 1 | 1
| 1
ˆ ˆ ˆ( )
ˆ ˆ
ˆ ˆ ˆ ˆ
ˆ:
) )
:
( (
k k k k k k
k k k k
k k k k k k k k k
k k k k k k
k k k
k
k k k k k k k k k k k k k k k k k k k k
k
k
k k
k k
y C x
x A x B u v
y C x w
x x W y C x
x A x B
K A W
u
A x W y C x B u A x B u A W y C x
元のシステム
状態推定器
システムの
1| | 1
| 1
ˆ ˆ
ˆ
, ,
k k k k k k k k k
k k k k k
k k k k
x A x B u K
y C x
A B C v
別表現
が一定の場合、 が白色信号であることが示せる
30
拡張カルマンフィルタ[ 1] [ ] [ ]( , ) [ ]k k kx f x u v k
[ ] [ ] [ ]( )k k ky h x w
[ 1] [ ] [ ] [ ] [ ] [ ]ˆ ˆ( , ) ( ) [ ]k k k k k kx f x u A x x v k
][][][][][][ )ˆ()ˆ( kkkkkk wxxCxhy
][ˆ
)(][
kxxx
xfA k
][ˆ
)(][
kxxx
xhC k
31
•拡張カルマンフィルタの更新式
),ˆ(),ˆ(ˆ ][][][][][][]1[ kkkkkkk uxhyWuxfx
1][][][][][][][][
kkkkkkkk RCPCCPAW TT
][][][][][][][][][]1[ kkkkkkkkkk TTT WRCPCWAPAP
[ 1] [ ] [ ] [ ] [ ] [ ]
[ ] [ ]
ˆ ˆ( , ) ( ) [ ]
ˆ ˆ[ 1] : ( , )
ˆ[ 1] [ ] [ ] [ ], [ ] : [ ] [ ]
k k k k k k
k k
x f x u A x x v k
x k f x u
x k A k x k v k x k x k x k
（ＵＫＦの考え方も同じ）
32
と が既知x xxP y yyP と を推定
する)(xgy
: [ 1]
ˆ: [ 1 | ] { [ 1]} { ( [ [ ]], , )
: [ 1 | ]
ˆ ˆ {( [ 1] [ 1 | ])( [ 1] [ 1 | ]) }
xx
T
x x k
x x k k E x k E f x k k
P P k k
E x k x k k x k
v k
x k k
[ 1] ( [ 1], 1, [ 1])
: ( [ 1])
y k wh x k k k
g x k
予測出力を用いた非線形カルマンフィルタ（１）
: [ 1]
ˆ: [ 1 | ] { [ 1]}
ˆ ˆ: {( [ 1] [ 1])( [ 1] [ 1]) }Tyy
y y k
y y k k E y k
P E y k y k y k y k
( , )
[ 1] [
[
]
1]
1
[ ]y y k y
p x y
x x k x x k
y k
の同時分布の考え方で予測出力を用いた場合
今までは
33
予測出力を用いた非線形カルマンフィルタ（２）
[ 1] ( [ ], , [ ])x k f x k k v k [ 1] ( [ 1], 1, [ 1])y k h x k k w k
ˆ ˆ( 1| 1) ( 1| ) ( 1) ( 1)x k k x k k W k v k )1()|1()1()|1()1|1( kWkkPkWkkPkkP T
vv
)|1()|1()1( 1 kkPkkPkW vvxy
システムを逐次線形近似（ EKF ）
統計モーメントを近似（ UKF ）
]|1[]1[]1[ kkkk yyv
予測出力を用いた拡張カルマンフィルタ
3434
[ 1] [ ] [ ]( , ) [ ]k k kx f x u v k
]1[]1[]1[ )( kkk wxhy
[ 1] [ ] [ ] [ ] [ ] [ ]ˆ ˆ( , ) ( ) [ ]k k k k k kx f x u A x x v k
ˆ[ 1] [ ], [ ]) ( )( [ ] [ ]
[ ], [ ])) ( ) [ ] [ ] [ 1] ( 1) [ ]
ˆ( ( ) [ ]) [ 1]
ˆ ˆ( ( ( 1) ( )
k k u k A k x k x k
k u k A k k k k C k v k
y h f x v k w k
h f x C k x x w
][ˆ
)(][
kxxx
xfA k
ˆ( [ ], [ ])
[ 1]( )
x f x k u k
kh x
Cx
等価外乱
35
Unscented Kalman Filter （ UKF)
•拡張カルマンフィルタの問題点•線形近似する際にヤコビアンを計算しなければなら
ない
（不連続なシステム、 Hard Nonlinearity ）•推定値にバイアスが乗ることがある
システムを近似するより統計量を近似するほうが容易数カ所のサンプル点（ Sigma Points ）を選び、集合平均的に統計量を近似する
と が既知x xxP y yyP と を推定
する)(xgy
Unscented Transformation
（Ｕ変換）
•発散することもある
（平均値の変換は変換後の平均値になると仮定している）
36
Sigma Points の考え方nx R x
xxを平均値 、分散行列 の確率変数ベクトルとする
xに対して、２ｎ＋１個の代表点 (i=0,1, ,2n)i を考えて、それぞれの
離散点の生起確率を iWとする。ただし、 i の集合的統計的性質は２次の
モーメントまでは一致させる 2 2n
i i0 i=0
, n
T
i i i xxi
W x x x W
( )
( )i i
y g x
g
1y2y
2 2n
i i0 i=0
ˆˆ ˆ ˆ , n
T
i i yy yy ii
y y W y y W
37
UKF の計算手順
３．予測共分散 、 を計算する
)|1( kkPxy )|1( kkPyy
２． Sigma Points を基に予測値 、 を
)|1(ˆ kkx 計算する
)|1(ˆ kky
４．カルマンゲイン を計算する)1( kW
6 ．推定値 と共分散 を更新する
)11(ˆ kkx )11( kkP
１．適切なサンプル点（ Sigma Points ）を推定値 と
)(ˆ kkx共分散 から選ぶ
)( kkP
5 ．観測値 が得られる
)1( ky
38
UKF:Sigma Pointｓ
κn
κW
0
]|[]|[ ˆ kkkk x0X
i
kkPnkkxkk ]|[)(]|[ˆ]|[ iX
)(2
1
nWi
i
kkPnkkxkk ]|[)(]|[ˆ]|[ niX
)(2
1
n
W ni
NM とするとTNNM である 番目の列ベク
トルi
ni ,,2,1
Rκ
40
UKF ： Sigma Points の性質
][ˆ][2
0
kkxkkWn
ii
iX
平均
分散
Tn
ii kkxkkkkxkkWP ][ˆ][][ˆ][
2
0
ii XX
1
2 ( ) ( ) ( )n T
ii ii
W n P k k P k k
1
( ) ( )n T
i ii
P k k P k k
)( kkP 平均、分散は一致している点の集合
41
補足
1 2 1 2( | )
( )
T
n n
ii
P k k v v v v v v
v P k k
とすると
1
2
1 2 1
1
1
21 2 1
1
1
( | ) ,
T
T
n
n
T
T nT
n j jj
T
n
Tn
Tn
n
n
v
v
P k k v v v
v
v
vv v v
v
v v
v
v
v v
42
1． は新の平均値 を２次の order まで近似
（ EKF は１次の order まで近似）
2． は を３次の order まで近似（これは EKF と同じ）
3. はチューニングパラメータ
が Gaussian の場合 と選ぶのが良い
Sigma Points を用いた推定の性質
y y
ˆyy
x
3n
yy
3
CKF(Cubature KF)
n
の時 が負となる フィルターが不安定化しやすい
ベイズ推定の積分近似より導かれた の利用も（代表点の選び方と生起確率が少し異なり２ｎの点を生成）
43
UKF ：共分散行列の更新
),( ][]|[]|1[ kkkkk uf ii XX
]|1[]|1[2
0
ˆ kkkkn
iiWx
iX
n
ii kkkkkk xWP
2
0
]|1[]|1[]|1[ ˆiX Tkkkk x ]|1[]|1[ ˆ iX
1.Sigma Points を状態遷移関数で遷移させる
2.遷移させた Sigma Points の集合平均で予測平均を近似する
３ .遷移させた Sigma Points の集合分散で予測分散を近似する
44
),( ][]|1[]|1[ kkkkk uh ii XY
４ .Sigma Points を観測関数で遷移させる
５ .遷移させた点の集合平均で予測観測値を近似する
６ .遷移させた Sigma Points の集合分散で予測分散を近似する
n
iiyy kkkkkk yWP
2
0
]|1[]|1[]|1[ ˆiY Tkkkk y ]|1[]|1[ ˆ iY
]|1[]|1[2
0
ˆ kkkkn
iiWy
iY
n
iixy kkkkkk xWP
2
0
]|1[]|1[]|1[ ˆiX Tkkkk y ]|1[]|1[ ˆ iY
45
[ 1] [ 1] [ 1 | ] [ 1 | ] [ 1]ˆv k k k k k k w ky y y ７ . イノベーションの予測共分散を計算する
８ . カルマンゲインを計算する
]|1[]1[]|1[ kkkkk yyvv PRP
]|1[]|1[]1[ 1 kkkkk vvxy PPW
９ . 観測値 から推定値を更新する
]1[ ky
]1[]1[]|1[]1|1[ ˆˆ kkkkkk vWxx
１０ . 予測誤差共分散を更新する
]1[]|1[]1[]|1[]1|1[ kkkkkkkk Tvv WPWPP
ノイズがアフィンでない場合• 上記の説明では観測ノイズが状態変数と独立で、アフィンな形で加わっていた（ノイズの影響は分散行列の和として計算可能）
• ノイズがアフィンでない場合は状態とノイズの拡大した変数を考えてシグマポイントを生成して同様の計算を行う。（ただし、その分計算量が大きくなる。また、状態と両ノイズに相関がないので、平均の状態にノイズが加わった形での評価となる。）
2
[ 1] ( [ ], [ ]) : ( [ ])
[ 1] ( ( [ ], [ ]), [ 1])) : ( [ ])
[ ]
[ ] : [ ]
[ 1]
ˆ [ | ][ | ]
, [ ]0
[ 1|]0a a
a a
a a
n pa
a x x
x k f x k v k f x k
y k h f x k v k w k h x k
x k
x k Rv k
w k
P k kx k k
x P Q k
R k
不安定化への対処の方法
• 状態外乱の共分散行列を大きめに設定する （真の値を用いずに調整パラメータと考える）
47
ＣＫＦアルゴリズム
2
1
1 2
( ) ( ) ( ), (0, )
( 1, , )1,
2 ( 1, , 2 )
n
x i ii
ii i
i n
n n
f x p x dx w f x N I
ne i nw
n ne i n n
I e e e
～ ( , )
( ) ( ) ( 2 ) ( )y x
y N
f y p y dy f x p x dx
～
48
共分散行列の予測
EKF による共分散の予測
UKF による共分散の予測
49
シミュレーション例
• 状態方程式 )()()(
11 tubtxadt
tdx
• 観測方程式 1 1sin(( ) ( ) 4 (( )) )xy t tx t w t
を観測
)(ty
状態 と未知パラメータ 、 を
同時に推定する
)(1 tx a b
:既知
、 :未知パラメータa b
50
観測方程式（モデル）
1x
y
センサの脈動を
表したモデル
51
逐次最小二乗法
]1[ˆ]1[][]1[]2[]1[
]1[]2[]1[ˆ][ˆ
kkkykkPk
kkPkk T
T
Tkky ]1[][
出力、入力から計算される非線形関数ベクトル
未知パラメータベクトル
出力
]1[]2[]1[1
]2[]1[]1[]2[]2[]1[
kkPk
kPkkkPkPkP
T
T
0]1[ P
52
シミュレーション結果
パラメータa
1x
パラメータb
100
010
001
]1[P
53
シミュレーション結果
パラメータa
1x
パラメータb
10R0Q
100.510diag0 P
54
U ＫＦを用いたバックラッシュ系のモデル予測制御
• モデル予測制御
⇒制約条件を考慮したシステマティックな制御系設計
問題点
• 計算時間• 状態の観測•ロバストパーフォーマンス
55
• バックラッシュ– 多くの機械システムに存在する– 制御性能の悪化、振動現象
• バックラッシュを考慮した制御は実用上非常に重要– 一般には、全ての状態が直接観測されない
• バックラッシュを含むシステムの状態推定• 微分方程式はスムーズでない⇒ＵＫＦの適用
– バックラッシュ補償• システマティックに最適制御系を実現したい• バックラッシュ系は非線形系（非線形最適制御フィードバック制
御）⇒非線形モデル予測制御の適用
56
モデル
vx
xy
2
1
)(1
)(1
422
4
311
3
42
31
xDM
x
xDuM
x
xx
xx
c
c
状態方程式
ダンパ要素
バネ要素
伝達トルク
:),(
:)(
,
:),()(
4321
G
F
xxxx
GFc
v:センサノイズ
観測方程式
M1
M2
回転系と直動系の両方を表現可能
57
モデル
ダンパ定数バネ定数バックラッシュ幅
相対距離
:0
:0
:02
)(
0
)(
),(
D
K
B
BDBK
B
BDBK
c
伝達トルク（力）
58
推定シミュレーション
観測値 ( ノイズ：標準偏差 0.05 の正規外乱 )
(rad)(rad)
駆動部位置被駆動部位置
[Nm] 入力 (±100[Nm] の矩形波 )
モデルパラメータ 1
[kgm2][kgm2]
[Nm/rad][Nms/rad][Nms/rad]
(rad)
モータの慣性モーメントアームの慣性モーメント接触トルクのばね係数接触トルクのダンパ係数
シャフトの摩擦係数バックラッシュ幅
2
1
x
x
2.02
1.0,1.0
0.10
1000
0.2
0.1
21
2
1
B
DD
D
K
M
M
u
シミュレーション条件 ( 回転系 )
59
推定シミュレーション結果• モデル誤差無し
誤差平均 EKF UKF
誤差共分散 EKF UKF
4
3
2
1
x
x
x
x
3
4
4
4
1011.4
1011.1
1001.5
1074.9
3
5
3
3
1001.1
1073.8
1044.4
1072.9
4
3
2
1
x
x
x
x
1
4
4
1018.1
29.1
1012.1
1086.2
2
1
5
4
1001.6
1020.4
1019.8
1093.1
0
0.2
0.4
0.6
0.8
1x1
x2
x3
x4
平均共分散
EKF を 1 とした
誤差平均・共分散の絶対値の比率
60
推定シミュレーション結果• モデルのバックラッシュ幅に誤差が -10%
誤差平均 EKF UKF
誤差共分散 EKF UKF
4
3
2
1
x
x
x
x
3
3
4
4
1096.8
1094.3
1044.1
1044.7
3
3
4
4
1005.4
1012.1
1026.1
1037.7
4
3
2
1
x
x
x
x
1
4
4
1019.1
31.1
1000.1
1090.2
2
1
5
4
1020.7
1033.4
1030.7
1095.1
0
0.2
0.4
0.6
0.8
1x1
x2
x3
x4
平均共分散
EKF を 1 とした
誤差平均・共分散の絶対値の比率
61
制御則• バックラッシュ補償制御
– 駆動部分を動かし、被駆動部分を早く制御– 被駆動部分の振動を抑える
Receding Horizon Control (RHC) を適用– 予測ホライゾン：有限長の評価区間– 予測されるプラントの振る舞いが最良となる入力を選択
C/GMRES法＋システムの切り替えを陽に考慮 （速度の不連続性にも対応可）Direct Multiple Shooting （ＤＭＳ）法
62
制御則• 入力の微分値を制御器より求める
– モデルの状態変数を拡張（新たな入力 v ）
– システムの切り替え時にも入力計算を安定化
),(
),(
vxfx
v
uxf
u
x
拡大システム
Tuxxxx ,,,, 4321
63
制御則• 評価関数
–終端コスト関数・コスト関数：参照軌道との二乗誤差（ Δτ で離散化したシステムを考える）
1
0
**** ))(),(()(JN
iiiN tvtxLtx
})(){(2
1:
)()(2
1:
RvvxxQxxL
xxSxx
Tr
Tr
rNNfT
rNN
64
制御則
M
k
M
k
k
kiiik
TkN tvtxHtxtx
k1 1
/)1(
/)(
**** ))(),(())(()(J
–ホライゾン上で M 回システムの切り替えが起こる
– 内点拘束として随伴
• システムの拘束、切り替えを含めた評価関数
• Hamiltonian
共状態変数
)f(x,)L(:H vvx,
65
制御則
0
)(
H
)(
),(
**
**
*0
***1
v
NTx
Tx
iiii
H
x
txx
vxfxx
• 最適性必要条件 （離散化）
66
制御則• システムの切り替えを考慮
– 内点拘束で表現
02
))()(( 2221
Bxx kkk
え時刻ホライゾン上の切り替:
)()(
)()()(
k
kk
k
T
k
kkk
HH
x
• 内点拘束による条件
67
シミュレーション• RHC
– 制御周期 10 [ms]– 予測ステップ 20 (200[ms])– 評価関数のパラメータはＰＳＯなどを用いて探索
LQR 制御と比較–ただし、 LQR では２質点は常に接続状態であるとして、フィードバックゲインを求める（ＲＨＣの場合の重みとは異なる）
–RHC と同程度の立ち上がり時間の応答で比較
–状態推定にはともに UKF を用いる
–制御周期は１ [ms]
68
制御則
• 駆動側の参照軌道– 被駆動部に対し接触面で相対速度 0
4
222
3
1 )sgn(
x
Bxxx
x
x r
r
r
• 被駆動部の参照軌道– 被駆動部の目標位置をステップ関数で与え
る2
4
1
0r
r
x
x
• 参照軌道– 目標状態までの軌道
Trrrrrr uxxxxx ][ 4321
69
シミュレーション観測値 ( ノイズ : 定常偏差 0.05 の正規外乱 )
(rad)(rad)
駆動部位置被駆動部位置
[Nm] 入力モデルパラメータ 2
[kgm2][kgm2]
[Nm/rad][Nms/rad][Nms/rad]
(rad)
モータの慣性モーメントアームの慣性モーメント接触トルクのばね係数接触トルクのダンパ係数
シャフトの摩擦係数バックラッシュ幅
2
1
x
x
04.02
01.0,01.0
0.10
2000
0.2
0.1
21
2
1
B
DD
D
K
M
M
u
70
シミュレーション結果• 被駆動部の応答と伝達トルク
被駆動部の応答 伝達トルク
71
シミュレーション結果• モデルのバックラッシュ幅に ±10% の誤差
被駆動部の応答 伝達トルク
72
モデルパラメータ観測値
モータの位置アームの位置入力トルク
モデルパラメータモータの慣性モーメン
トアームの慣性モーメン
ト接触トルクのばね係数接触トルクのダンパ係数
シャフトの摩擦係数バックラッシュ幅
)(
)(
2
1
radx
radx
)(2.02
]/[01.0,01.0
]/[001.0
]/[100
][025.0
][00625.0
21
22
21
radB
radNmsDD
radNmsD
radNmK
kgmM
kgmM
][Nmu
73
シミュレーション結果 2
• モデルパラメータ 2
74
シミュレーション結果
• 入力
75
UKF を用いたモデル予測制御の課題
• 状態推定– UKF によって制御に必要な状態量を推定– バックラッシュを含むシステムに対して EKF よりも高い精度
• 制御– C/GMRES法を適用– 素早い応答– 振動抑制
• 課題– 現実のバックラッシュシステムに対する本手法の有効性の検証
76
まとめ（ EKF と UKF の比較）
• UKF は EKF が適用できないような不連続 システムにも適用できる• 非線形なシステムにおいてより正確な推
定を 行える• 実装が容易（計算時間は大きい）• 離散 UFK 、連続－離散 UKF(Sarkka) の有
効性の実験的検証
77
参考文献• 片山徹著：新版応用カルマンフィルタ 朝倉書店• A.H.Jazwinski: Stochastic Processes and Filtering Theory, New York:Academic, 1970• G.C.Goodwin and K.S.Sin :ADAPTIVE FILTERING PREDICTION AND CONTROL ,
PRENTICE-HALL (1984)• C.Chui and G.Chen: Kalman Filtering with Real-Time Applications, 4th ed., Springer
(2009)• S.J.Julier and J.K.Uhmann :A New Method for the Nonlinear Transformation of Means and
Covariances in Filters and Estimators」 ,IEEE Trans.Autom.Contr. Vol.45,No.3 (2000)• T.Lefebvre,et.al「Comment on “A New Method for the Nonlinear Transformation of Means
and Covariances in Filters and Estimators」• S.J.Julier and J.K.Uhmann 「 A General Method for Approximating Nonlinear Transformation
of Probability Distributions」 [Online]1996 • E.A.Wan and R. Merwe : The Square-Root Unscented Kalman Filter for State and Parameter
Estimation, Proc. Of Int. Conf. on Acoustics, Speech, and Signal Processing (2001) • S.Julier and J. Uhlmann : Unscented Filtering and Nonlinear Estimation, Proceedings of The
IEEE, Vol. 92, No. 3, (2004)• M.Yamakita et. al. : Comparative Study of Simultaneous Parameter-State Estimations, Proc.
of CCA 2004 (2004)• 山北： UKF って何？ ,, システム制御情報学会 (2006) • M.Saito, M.Yamakita: MPC for a Simplified Transmission Model with Backlash Using UKF,
Proc. of CCA2006, pp.527/532 (2006)• S.Sarkka: On Unscented Kalman Filtering for Sate Estimation of Continuous-Time Nonlinear
Systems, IEEE Trans. Autom Contr., Vol.52, No.9 (2007)
• I.Arasaratnam and S.Haykin: Cubature Kalman Filter, IEEE Trans. On AC, Vol. 54, No.6, 2009
78
種々の手法の推定法の概略
79
F)
F)
( (0))p x
２．
誤差の分布がガウス分布に従うと近似して、解析的に条件付期待値を計算
３． 状態の分布を２
拡張カルマ
次の統計量まで近似して、解析的に条件付期待値を計算
４． 初期状態分布に基づき代表点を生成し、各代
ンフィルター（ＥＫ
ＵＫＦ（無香料カルマンフィルター）
アンサンブルカルマンフィルター点
（ＥｎＫ表
1.
をガウス分布に従うとして、解析的に条件付カルマンフィルタ
期待値を計算ー（ＫＦ）
の値を解析的に更新する。 条件付期待値は各点より集合平均的に数値計算する。
５． 分布を代表点の数の分布で近似し、条件付確率分布を解析的に近似する。 条件付確率分布に従って粒子を再サンプリン
パーティクルフィルター（粒子フィルター
グし、条件付期待値を集合平均で
）
求める。
種々の手法のイメージ（１）
80
t1x
2x
１．カルマンフィルター
t1x
2x２．ＵＫＦ
種々の手法のイメージ（２）
81
t1x
2x
１．アンサンブルカルマンフィルター
t1x
2x２．パーティクルフィルター
82
EnKF （アンサンブルカルマンフィルター）The EnKF uses Monte Carlo sampling and the number of sampling points N is not related to the dimension of the augment state.
② Transform each particles as follow
① Generate N particles from
The EnKF can handle any non-Gaussian noises.
The EnKF uses fewer particles than the Particle filter [Gillijns,06] etc.
Advantage of EnKF
⑤ The updated state estimate is computed as the mean of the updated particles.
④ Update each particles as follow,
Use same kalman gain for all particles.
③ Calculate mean and covariance matrix
( ) ( )| 1 | 1 | 1 | 1
( ) ( )| 1 | 1 | 1 | 1
1ˆ ˆ( )( )
11
ˆ ˆ( )( )1
yy i i Tk k k k k k k k k
xy i i Tk k k k k k k k k
P y y y yN
P x x y yN
パーティクルフィルター
83
1 1 1
1
( )0 0
( )1 1
( ) ( ) ( )1 1
( , ), ,
( , ) ( ( , )
1. ( 1, , ) ( 0)
2. (
( )
( ) ( , ) ( 1, ,
,
)
)
1
)
(
,k k k k k
k k k k k k
i
ik k
i i ik
n pk k
k k
R y v R
k
x f x w x w
y h x v v h x y
x x i N k
k
i w w
ii x f x w i N
iii
ただし、 が存在）
の分布に従って を生成
以下を繰り返す
の分布に従って を生成
を計算
( )
1
1 2 1 2
1
1( 1
)
)
(
( , )( : , : )
( , )( ) ( )
( , )( | ) ( ( , )) :
(
( | )
)
k kk k k k k vp k k k vp
k
k kk k k k
i
k
i k kk k k k
k
i
k k
k
k
h x ydv dy dv dv dv d dy dy dy y
y
h x yp v dv p v dy
y
h x yp y x p h x y
y
p y x
iv x
c
を次の式によって計算
1= より
の頻度（確率）は代 (表点 粒子）の数
( )
(( )( ) ( ) ( )
( ) ( ) ( )
1 1
( ) ( )
( )
( )
1
)
1
( | ) ( ) /
( | ) ( )
( | )
( | )/
( | )
( | )
i i ik k k k
N Ni i i
k k k
ik
ki i
i ik k
ik k
ik
k
ik k k
i
i
k N
k
N
p y x p x c N
p y x p x c N
p x y x
p x y
p x
x p x
c
y
cy
で表しているので、１つの粒子の
再サンプリング
確率は全て である。よって は次式で計算される。
この に基づいて を する。
( の分布は の分布となる）
( ) ( ) ikv x条件付期待値は の単純平均
課題１
84
1 0 1 0
1 0 1 0
ARMA ( )
( ) ( 1) ( 2) ( ) ( 2) ( 2), ( 0)
0.4, 0.64, 0.2, 0.9, ( 1) (0) 0 ( )
0.1
y k
y k a y k a y k w k w k w k k
a a y y w k
次の モデルで表されるシステムについて の最小分散予測を行い、ノイズのモデルを無視した場合と予測誤差の統計的分散を比較しなさい。
ただし、 とし、
は平均ゼロで分散 の正規白色ノイズであるとする。 ( )
( 1) ( )
y k
y k y k
また、 の予測とは、 以前のデータとシステムのパラメータのみからを推定することである。
課題 2- （ 1 ）
2009/08/24 85
1
1
1 1
2 2
3 1
4 2
x
xx
x
x
y1 1 2 1 2cos( ) cos( )y l l n
平面 2リンクマニピュレータ
課題 2- （２） 運動方程式
1 1
2 2
1
2
21
22
1 2
2
0.5[ ], 1.0[ ]
0.25[ ], 0.5[ ]
10[ ]
5[ ]
5[ ]
0.5[ ]
0[ / ]
9.8[ / ]
l m a m
l m a m
m Kg
m Kg
I Kgm
I Kgm
d d Nms rad
g m s
1 1 1 2 1 1 2 1 2
2 2 1 2
cos( ) ( cos( ) cos( ))
cos( )
m l m a lg
m l
課題 2- （３）
87
1 2 1 2
1
2
21
1 1 2
( ) ( ) ( ( ), ( )) ( ), ( ) : ( ) ( ) , ( ) : ( ) ( )
( )( )( ) : :
( )( )
( )( ) : ( ( ), ( ))
( ( ))( ( ) ( ( ), ( )))
[ ] : ( ), [ ] :
T T
c
cc
c
cc c c c
c c c c
c s
D q q t h q t q t t q t t t u t t t
x tq tx t
x tq t
x tx t f x t u t
D x t u t h x t x t
x k x kT u k
1 1 2 1 2
( ), 0.01[ ]
[ ] : ( )
[ 1] [ ] ( [ ], [ ]) : ( [ ], [ ])
[ ] cos( [ ]) cos( [ ] [ ])
c s s
s
c c
u kT T s
y k y kT
x k x k f x k u k T f x k u k
y k l x k l x k x k n
課題 2- （４）
88
1
21 2
1
2
( ) 0.0
( ) / 4( ) : , (0) : ( ) ( ) 0
( ) 0
( ) 0
( ) ( )
c c
t
tx t x t t
t
t
y t x t UKF
n
, として以下の設問に答えよ。
（１） を観測して を を用いて推定せよ。 ただし、物理パラメータは既知としてよい。また、観測ノイズ は平均値ゼロ 0. 01[rad]で、標準偏差 が の正規白色ノイズとする。
(1)（２） の 2m問題で も未知として、これも同時に推定せよ。
（持っているロードを同時に推定することに相当）
