非線形カルマンフィルタ入門

確率システムの状態推定入門
東京工業大学大学院
機械制御システム専攻
山北昌毅
1
内容
• 最小分散推定値
• Ｋａｌｍａｎ Filter
– 線形カルマンフィルタ
– 拡張カルマンフィルタ
– Unscented Kalman Filter (UKF)
• 例題を用いた状態推定比較
– UKFとＲＨＣの併用例
• Kalman-Bucy Filter-UKBF
• 状態拘束・非ガウス性外乱に対する対
– 混合ガウス分布
– アンサンブルカルマンフィルター
• まとめ
2
モデルを使った内部信号の推定
1 
 0
0
 x   1 u
x k
d

 
 
 m
 m 
m 
y  1 0  x,
m  1.0, d  0.2, k  1.0
観測値yには離散化誤差
1.2 
K 

0.2 
•期待値 x

x  EX    xp( x)dx

•分散  x2
  E( X  x )   ( x  x ) 2 p( x)dx
2
x
2


• k 次統計モーメント



E ( X  x )   ( X  x ) k p( x)dx
k

１次モーメント：平均
３次モーメント：歪度
２次モーメント：分散
４次モーメント：尖度
4
ガウス分布
•１次元
p ( x) 
 x  x 2 

exp 
2

2

2 2


1
•平均： EX   x


•分散： E  X  x2   2


•偶数次モーメント： E X  x  1 3  2n 1
•奇数次モーメント： E  X  x2n1  0
2n
2n
5
•多次元
xR
n

  E  X  x  X  x 
T

 1

T 1




p( x) 
exp

X

x

X

x


1/ 2
n/2
 2

2  
1
•２次元
  x21

 x1 x2
xx
 x2
1 2
2



6
•２次元分布（相関による変化）
2


2
 
x1


x 

 x1 x2
3
xx
 x2
1 2
2



1.0 0.5


0
.
5
0
.
5


1.0 0 


 0 0.5
 1.0  0.5



0
.
5
0
.
5


7
多次元ガウス分布確率変数の発生
Xi  R
N (0,1)
Y  Rn
N (  , Q)
Q  LLT , (コレスキー分解） L  chol(Q)T
 X1 
X 
Y  LX   , X   2 
 
 
Xn 
E{Y }  E{LX  }  LE{ X }    
Cov{Y }  E{LXX T LT }  LE{ XX T }LT  LILT  Q
8
本当にガウス分布になる？
1 
px ( X )  
e
i 1
2
n
X i2
2
1

(2 )
n
2
e

XT X
2
Y  LX    X  L1 (Y   )
dY  LdX  dX  L1dY
 p ( X )dX  1   p ( L
1
x
x
X
(Y   )) det( L1 ) dY
X
det( L1 ) 
1
1

det( L)
|Q|
1
p y (Y ) 
|Q|
1

(2 )
n
2
p (Y )
y
1
(2 )
e

n
2
e

( L1 (Y   ))T ( L1 (Y   )
2
1

(2 )
n
2
e

(Y   )T L T L1 (Y   )
2
|Q|
(Y   )T Q 1 (Y   )
2
|Q|
9
Fact
正規分布を持つ確率変数のAffine変換された確率変数の確率
分布は正規分布となる。つまり、
xを正規性の確率変数としてyを次の式で定義する
y  Ax  b x  R , y  R , A  R
n
m
mn
,b  R
m
yは正規性確率変数となり平均、分散は次の式で計算される
E{ y}: y  AE{x}  b  Ax  b
E{( y  y )( y  y )T }:  yy  E{ A( x  x )( x  x )T AT }
 AE{( x  x )( x  x )T }AT  AAT
10
最小分散推定値
推定したい
パラメータ x
観測量
p( y x)
推定量 xˆ
y
推定ルール
g ( y)

評価関数 E x  xˆ
2
 を最小にする推定値
条件付き期待値
xˆ  Ex y と等価
（ x の分布の種類によらず）
11
•証明
p( x, y)
E  f ( X , Y )   f ( x, y) p( x, y)dxdy, p( x | y) :
p( y )
  f ( x, y ) p ( x y )dx p ( y )dy
 EEf ( X , Y ) Y 

E g (Y )  X
2
  E E g(Y )  X Y 
2
2


 E  E  g (Y )  EX Y  EX Y  X Y  

 

 E E g  x y  x y  X  g  x y  x y  X Y

 E E g  xy
T
2

 x y  X  2g  x y  x y  X Y
2
T
期待値をとると
0
12

続き

E g (Y )  X
2
  E  E  g (Y )  x
 E 

y
 xy  X
2
Y


2

E  g (Y )  x y Y   E x y  X


2

Y 

g (Y )と無関係
これが最小になるのは
g (Y )  xy
2
の時。
つまり、
g (Y )  EX Y  が最小分散推定値となる
13
ここまでのまとめ
•最小分散推定値は条件付期待値である。
•どうやって条件付期待値を求めるのか
•条件付確率密度関数が得られても計算が困難
•ガウス分布を仮定すると容易に計算可能
14
ガウス分布の条件付期待値

 xx

 yx


 xy   E  x  x x  x T


 yy   E  y  y  x  x T
A0 yy   x y
 Ex  x  y  y  
 E y  y  y  y  
T
T
A0   x y yy1
xˆ  Ex y x  A0  y  y 

E x  Ex y x  Ex y y
T

xx
 A0 yx
Txy  Tyy A0T より上の共分散は  xx  A0 yy A0T
注：ガウス分布でないとき線形最小分散推定値となる
xˆ  Ay  b
15
証明
[補題]
 11 12 
 T
, T  

12  22 
1 T
 22 is nonsingular, 11  12 22
12 is nonsingular
 X 1

 X 1Y
1.   
1
T
1
T
1 

(
X
Y
)
Z

Y
X
Y

1 T
X : 11  12 22
12
1
1
Y : 12  22
Z   22
2.det()  det( X ) det( Z )
(証明)
In
= 
0
1
1 T
 11  12  22
12  22
12

In  
12
0 

 22 
16
条件付確率密度関数
p( X ) 
1
1
T 1
Exp
(

(
X


)
 ( X   ))
n/2
1/ 2
(2 ) (det())
2
X : 1  2
n 
T
1
T 1
n/2
1/ 2
Exp
(

(



)

(



))
d


(2

)
(det(

))

2
  X1  
p      p ( X 1, X 2 )
 X2 
1
1  X 1   1  T

Exp
(

(    )
( n1  n2 ) / 2
1/ 2
(2 )
(det())
2  X 2   2 
 X   
(  1    1  )T
 X 2   2 
 11
 T
 12
 11
 T
 12
1
12   X 1   1 
(      ))

 22   X 2   2 
1
12   X 1   1 
(    )

 22   X 2   2 
  X 1  1 
 X 1Y
 X 1  1   X 1

 

1
T
1
T
1  
 X 2   2   ( X Y ) Z  Y X Y   X 2   2 
 ( X 1  1  Y ( X 2  2 ))T X 1 ( X 1  1  Y ( X 2  2 ))  ( X 2  2 )T Z 1 ( X 2  2 )
T
17
p( X 2 )   p (1 , X 2 )d 1 
1
1
T
1
Exp
(

(
X


)
Z
( X 2  2 )) 
2
2
( n1  n2 )/2
1/2
2
(2 )
(det())
T
1
Exp
((
X



Y
(
X


))
X
( X 1  1  Y ( X 2  2 ))d 1 
1
1
2
2

1
1
T
1
Exp
(

(
X


)
Z
( X 2  2 ))
2
2
n2 /2
1/2
2
(2 ) (det( Z ))
p( X 1 , X 2 )
p( X 1 | X 2 ) 
p( X 2 )


1
1
T
1
Exp
(

(
X



Y
(
X


))
X
( X 1  1  Y ( X 2  2 )))
1
1
2
2
n1 /2
1/2
2
(2 ) (det( X ))
E{ X 1 | X 2 }  1  Y ( X 2  2 )
Var{ X 1 | X 2 }  X
18
確率密度関数のレベル集合との関係
xˆ
( y, x )
 yy ’ 大きく ’ xyの効果が小さく
y
19
拘束条件付ノルム最小化と直行条件
T
ˆ
Min || x  x ||:|| e || s.t. c xˆ  d
x
 e, y  xˆ  0 for cT y  d
y
xˆ
20
偏差と観測値の‘直交条件’
E{( x  xˆ ) y T }  ?
xˆ  x  Y ( y  y ),
Y  12  122
E{( x  x  Y ( y  y )) y T }  E{( x  x  Y ( y  y ))( y  y  y )T }
 12  0  Y  22  Y 0  12  12  0
逆にYを知らなくとも、
12  Y  22  0
より、Y  12  122が出る
21
Bayes情報行列とCramer・Rao不等式（１）
[仮定]
p ( x, y ) p ( x, y )
,
が存在し、 x, yについて絶対可積分可能である
xi
xi x j
[ 定義]
仮定の下で、ベイズ情報行列Jは下記で定義される
T
T
  log p ( x, y )  log p ( x, y ) 
  log p ( x | y )  log p ( x | y) 
nn
J : E 
 E
  R

x

x

x

x




[定理]事後Cramer・ Rao不等式
J  0のとき以下の不等式が成り立つ。

Pxx : E x  f ( y )  x  f ( y ) 
T
 J
この関係式はどんな f ()にもなりたち、下限である Jは分布により決まる
1
つまり、どんな推定を用いても誤差分散の意味でこれより
良いものは作れないことを示している！
ただし、
b( x) :  ( f ( y )  x) p( y | x) dy
と定義すると lim b( x) p( x)  0であるとする。
xi 
22
練習問題
 x
 y  : z
 
 0  1 1.5 
N  ,

0
1.5
4





yの実現値{ yi }からそれとペアーに実現された{xˆi }を推定したい
{ yi }
23
最小分散推定
24
種々の手法の推定法の概略
1. カルマンフィルター（ＫＦ）
p( x(0))をガウス分布に従うとして、解析的に条件付期待値を計算
２．拡張カルマンフィルター（ＥＫ F)
誤差の分布がガウス分布に従うと近似して、解析的に条件付期待値を計算
３．ＵＫＦ（無香料カルマンフィルター）
状態の分布を２次の統計量まで近似して、解析的に条件付期待値を計算
４．アンサンブルカルマンフィルター（ＥｎＫ F)
初期状態分布に基づき代表点を生成し、各代表点の値を解析的に更新する。
条件付期待値は各点より集合平均的に数値計算する。
５．パーティクルフィルター（粒子フィルター）
分布を代表点の数の分布で近似し、条件付確率分布を解析的に近似する。
条件付確率分布に従って粒子を再サンプリングし、条件付期待値を集合平均で求める。
25
種々の手法のイメージ（１）
１．カルマンフィルター
x2
t
x1
２．ＵＫＦ
x2
x1
t
26
種々の手法のイメージ（２）
１．アンサンブルカルマンフィルター
x2
t
x1
２．パーティクルフィルター
x2
x1
t
27
パーティクルフィルター
 xk  f ( xk 1 , wk 1 ), xk , wk 1  R n , yk , vk  R p

(ただし、 vk  h 1 ( xk , yk )が存在）
 yk  h( xk , vk ) 1.x0の分布に従って x0(i ) (i  1,
, N )を生成(k  0)
2.以下を繰り返す(k  k  1)
(v)条件付期待値はx(i ) kの単純平均
(i ) wk 1の分布に従って w(i ) k 1を生成
(ii ) x (i ) k  f ( x (i ) k 1 , w( i ) k 1 )を計算(i  1,
, N)
(iii ) p( yk | x (i ) k )を次の式によって計算
dvk 
h 1 ( xk , yk )
dyk (dvk  : dvk1dvk 2
yk
1=  p(vk )dvk    p (vk )
d vp , dyk  : dyk 1dyk 2
yvp )
h 1 ( xk , yk )
dyk より
yk
h 1 ( xk , yk )
p( yk | x k )  p(h ( xk , yk ))
: c (i ) k
yk
1
(i )
(iv) xkの頻度（確率）は代表点( 粒子）の数で表しているので、１つの粒子の
確率は全て
p ( x ( i ) k | yk ) 
1
である。よって p ( x (i ) k | yk )は次式で計算される。
N
p ( yk | x ( i ) k ) p ( x ( i ) k )
N
 p ( yk | x ( i ) k ) p ( x ( i ) k )
i 1

c(i ) k / N
N
 c(i ) k / N
i 1

c(i ) k
N
c
i 1
(i )
k
この p( x (i ) k | yk )に基づいて x ( i ) kを再サンプリングする。
( x (i ) kの分布はp ( xk | yk )の分布となる）
28
線形カルマンフィルタ
•対象モデル
x[ k  1]  A[k ] x[k ]  v[k ]
y[k ]  C[k ] x[ k ]  w[k ]
x[k ]  R n状態ベクトル
y[k ]  Rl 観測ベクトル
v[k ]  R n 状態外乱ベクトル
w[k ]  R l 観測外乱ベクトル
 v[k ]  T
 Q 0 
T

E
v
[ k ] w [ k ] 
 [k ]




w[k ]
  0 R
29
•線形カルマンフィルタ
初期推定値
xˆ0|1 とその予測誤差共分散 P0|1
１．カルマンゲインを計算

Wk  Pk |k 1C Ck Pk |k 1C  Rk
T
k
T
k

1
２．前回の予測推定値を観測値との誤差で修正
xˆk|k  xˆk|k 1  Wk ( yk  Ck xˆk|k 1 )
３．予測推定値を計算
xˆk 1|k  Ak xˆk|k
４．推定誤差共分散行列を更新
Pk|k  Pk|k 1  Wk Ck Pk|k 1
Pk 1|k  Ak Pk|k AkT  Qk
30
•証明
 x[ k ] 
x
[
k

1]
A
[
k
]
I
0

 

　
v
[
k
]
 y[ k ]  C[ k ] 0 I  


 
  w[ k ]


両辺の条件無しの期待値をとる。（ｋ－１時刻までの情報による期待値）
 xˆk 1|k 1   A[k ] xˆk |k 1 
 yˆ
  C[k ] xˆ 
k |k 1 
 k |k 1  
誤差ベクトルの同時確率密度関数の分散は以下のように計算される
 x[ k  1]  A[ k ] x[ k ]  v[ k ] 
 y[ k ]   C[ k ] x[ k ]  w[ k ]

 

 x[k ] : x[k ]  xk |k 1 , y[k ] : y[k ]  C[k ]xk |k 1

Pk |k 1 : E{x[k ]xT [k ]}


A[k ]  Ak

T
T
T

Ak Pk |k 1CkT 
  x[k  1] x [ k  1] x[ k  1] y [ k ] 
  Ak Pk |k 1 Ak  Q
E 


T
T
T
T
C
P
A
C
P
C

R
y
[
k
]
x
[
k

1]
y
[
k
]
y
[
k
]


k
k
|
k

1
k
k
|
k

1
k


 

ただし、xi|j , Pi| jはそれぞれy[ j ]まで観測された
ときのx[i]の条件付期待値及び条件付分散である
 x  x[k  1] 

と考える
y

y
[
k
]


31
ˆ ˆ k+1| kは
ここでX=x[ k+1] , Y=y[ k] と考えると x=x
x=Ak xˆk |k 1
y  Ck xˆk |k 1
yy  Ck Pk |k 1CkT  R
 xy  Ak Pk |k 1CkT
として
z  x   xy  yy1 ( y[k ]  y )
 Ak ( xˆk |k 1  Pk |k 1CkT (Ck Pk |k 1CkT  R ) 1 ( y[k ]  y ))
 zz  Ak Pk |k 1 A  Q  Ak Pk |k 1C
T
k
 Ak ( Pk |k 1  Pk |k 1C
T
k
C P
k
k |k 1C
T
k
T
k
 Ak ( Pk |k 1  Wk Ck Pk |k 1 ) Ak T  Qk
C P C  R 
 R C P )A
k
k |k 1
T
k
1
1
T
k
k |k 1
Ck Pk |k 1 AT
 Qk
32
従ってゲインを次のように定義し、

Wk  Pk |k 1C Ck Pk |k 1C  Rk
T
k
T
k

1
xˆk|k、 Pk|kを次の式で定義すると、
xˆk|k  xˆk|k 1  Wk ( yk  Ck xˆk|k 1 )
Pk|k  Pk|k 1  Wk Ck Pk|k 1
最適な推定値は次式で与えられる
xˆk 1|k  Ak xˆk|k
Pk 1|k  Ak Pk|k AkT  Qk
33
2次形式評価関数最小化としての解
Min J  ( x  xˆ )T P 1 ( x  xˆ )  (Cx  y )T R 1 (Cx  y )
x
xˆ : xˆk |k 1 , y : yk , P : Pk |k 1 , R : Rk
J
 2 P 1 ( x  xˆ )  2C T R 1 (Cx  y )  0
x



P 1  C T R 1C x  P 1 xˆ  C T R 1 y  0  x  P 1  C T R 1C
 P
1
1
xˆ  C T R 1 y

ここで逆行列の補題より
P
1
 C T R 1C

1
 P  PC T R 1 ( I  CPC T R 1 )1 C T P  P  PC T ( R  CPC T ) 1 C T P




x  P  PC T ( R  CPC T ) 1 C T P P 1 xˆ  C T R 1 y  ( I  PC T ( R  CPC T ) 1 C ) xˆ  PC T R 1 y


K

 xˆ  KCxˆ  I  PC T ( R  CPC T ) 1 C PC T R 1 y
 xˆ  KCxˆ  PC T ( I  ( R  CPC T ) 1 CPC T ) R 1 y


 xˆ  KCxˆ  PC T ( R  CPC T ) 1 R  CPC T  CPC T R 1 y
K
 xˆ  KCxˆ  Ky  xˆ  K ( y  Cxˆ )
34
線形カルマンフィルタ（入力あり）
•対象モデル
xk 1  Ak xk  Bk uk  vk
uk  Rm 入力ベクトル
yk  Ck xk  wk
ほとんど同様に計算できる

Wk  Pk |k 1C Ck Pk |k 1C  Rk
T
k
T
k

1
xˆk|k  xˆk|k 1  Wk ( yk  Ck xˆk|k 1 )
xˆk 1|k  Ak xˆk|k  Bk uk
Pk|k  Pk|k 1  Wk Ck Pk|k 1
Pk 1|k  Ak Pk|k AkT  Qk
（条件なし期待値の計算）
35
白色化フィルター（イノベーションプロセス）
元のシステム
 xk 1  Ak xk  Bk uk  vk

 yk  Ck xk  wk
状態推定器
xˆk |k  xˆk |k 1  Wk ( yk  Ck xˆk |k 1 )
xˆk 1|k  Ak xˆk |k  Bk uk
 Ak  xˆk |k 1  Wk ( yk  Ck xˆk |k 1 )   Bk uk  Ak xˆk |k 1  Bk uk  Ak Wk ( yk  Ck xˆk |k 1 )
 k : yk  Ck xˆk |k 1

 K k : Ak Wk
システムの別表現
 xˆk 1|k  Ak xˆk |k 1  Bk uk  K k k

 yk  Ck xˆk |k 1   k
Ak , Bk , Ck が一定の場合、 vk が白色信号であることが示せる
36
ARモデルのパラメータ推定（１）
•３次のARモデルのパラメータ推定
yk  a1 yk 1  a2 yk 2  a3 yk 3  wk
xk  a1 a2 a3 
T
•状態空間モデル
1

xk 1   1  xk

1
yk  yk 1
yk 2
yk 3  xk  wk
37
ARモデルのパラメータ推定（2
）
a1  2.76
a2  2.5329
a3  0.778688
aˆ1
aˆ3
R  1 .0
P0  I
xˆ0  0
aˆ2
観測回数
38
拡張カルマンフィルタ
x[k  1]  f ( x[k ], u[k ])  v[k ]
y[k ]  h( x[k ])  w[k ]
x[k  1]  f ( xˆ[k ], u[k ])  A[k ]( x[k ]  xˆ[k ])  v[k ]
y[k ]  h( xˆ[k ])  C[k ]( x[k ]  xˆ[k ])  w[k ]
f ( x)
A[k ] 
x x  xˆ
[k ]
h( x)
C[ k ] 
x x  xˆ
[k ]
39
x[ k  1]  f ( xˆ[ k ], u[ k ])  A[ k ]( x[ k ]  xˆ[ k ])  v[k ]
xˆ[k  1] : f ( xˆ[ k ], u[ k ])
x[k  1]  A[k ]x[k ]  v[k ], x[k ] : x[k ]  xˆ[k ]
•拡張カルマンフィルタの更新式


1
W [k ]  A[k ]P[k ]C [k ] C[k ]P[k ]C [k ]  R[k ]
xˆ[k  1]  f ( xˆ[k ], u[k ])  W [k ] y[k ]  h( xˆ[k ], u[k ])
P[k  1]  A[k ]P[k ] AT [k ]  W [k ] C[k ]P[k ]C T [k ]  R[k ] W T [k ]
T
T


40
予測出力を用いた非線形カルマンフィルタ（１）
（ＵＫＦの考え方も同じ）
p( x, y )の同時分布の考え方で予測出力を用いた場合

 x  x[k  1]
 x  x[k  1] 
今ま
では 



y

y
[
k

1]
y

y
[
k
]




x
と
y  g (x)
Pxx が既知
x : x[k  1]
x : xˆ[k  1 | k ]  E{x[k  1]}  E{ f ( x[k ], k , v[k ])
Pxx : P[k  1 | k ]
y
と
Pyy を推定する
y : y[k  1]
y : yˆ[k  1 | k ]  E{ y[k  1]}
 E{( x[k  1]  xˆ[k  1 | k ])( x[ k  1]  xˆ[ k  1 | k ]) }
T
Pyy : E{( y[k  1]  yˆ[k  1])( y[ k  1]  yˆ[ k  1])T }
y[k  1]  h( x[k  1], k  1, w[k  1])
: g ( x[k  1])
41
予測出力を用いた非線形カルマンフィルタ（２）
x[k  1]  f ( x[k ], k , v[k ])
y[k  1]  h( x[k  1], k , w[k  1])
xˆ (k  1| k  1)  xˆ (k  1| k )  W (k  1)v(k  1)
P(k  1 | k  1)  P(k  1 | k )  W (k  1) Pvv (k  1 | k )W T (k  1)
W (k  1)  Pxy (k  1 | k ) Pvv1 (k  1 | k )
v[k  1]  y[k  1]  y[k  1 | k ]
システムを逐次線形近似（EKF）
統計モーメントを近似（UKF）
42
予測出力を用いた拡張カルマンフィルタ
x[k  1]  f ( x[k ], u[k ])  v[k ]
y[k  1]  h( x[k  1])  w[k  1]
x[k  1]  f ( xˆ[k ], u[k ])  A[k ]( x[k ]  xˆ[k ])  v[k ]
y[ k  1]  h( f ( xˆ[ k ], u[ k ])  A( k )( x[ k ]  xˆ[k ])  v[k ])  w[k  1]
 h( f ( xˆ[ k ], u[ k ]))  C (k  1) A(k )( x[ k ]  xˆ[ k ])  w[ k  1]  C ( k  1)v[ k ]
h( x)
f ( x)
C[ k  1] 
A[k ] 
x x  f ( xˆ[ k ],u[ k ])
x x  xˆ
[k ]
等価外乱
43
43
Unscented Kalman Filter（UKF)
•拡張カルマンフィルタの問題点
•線形近似する際にヤコビアンを計算しなければならない
（不連続なシステム、Hard Nonlinearity）
•推定値にバイアスが乗ることがある
（平均値の変換は変換後の平均値になると仮定している）
•発散することもある
システムを近似するより統計量を近似するほうが容易
数カ所のサンプル点（Sigma Points）を選び、集合平均的に統計量を近似する
x
と
Pxx が既知
y  g (x)
y
と
Pyy を推定する
Unscented Transformation
（Ｕ変換）
44
拡張カルマンフィルターの問題点
y
y  f ( x)  x 2
p( y)
p '( y)
y
x
p ()
x
p ( x)
45
Sigma Pointsの考え方
xR
x
n
を平均値
x
、分散行列
に対して、２ｎ＋１個の代表点
離散点の生起確率を
Wi
 xx の確率変数ベクトルとする
i (i=0,1, ,2n)
とする。ただし、  i
2n
モーメントまでは一致させる
W
i 0
i
i
 x,
の集合的統計的性質は２次の
2n

 x  i  x  Wi   xx
T
i
i=0
y  g ( x)
 i  g ( i )
2n
を考えて、それぞれの
y1
y2
2n
T
y  yˆ :   iWi ,  yy  ˆ yy :   i  yˆ  i  yˆ  Wi
i 0
i=0
46
Sigma Pointsを用いた推定の性質
１．
yˆ
は新の平均値
y
を２次のorderまで近似
（EKFは１次のorderまで近似）
２．
3.
ˆ

x
yy
は

yy
を３次のorderまで近似（これはEKFと同じ）
はチューニングパラメータ
がGaussianの場合
n   3
と選ぶのが良い
47
UKFの計算手順
１．適切なサンプル点（Sigma Points）を推定値 xˆ (k k )と
共分散 P(k k )から選ぶ
２．Sigma Pointsを基に予測値 xˆ (k  1 | k ) 、 yˆ (k  1 | k ) を
計算する
３．予測共分散 Pxy (k  1 | k )、 Pyy (k  1 | k )を計算する
４．カルマンゲイン W (k  1)を計算する
5．観測値
y (k  1) が得られる
6．推定値 xˆ (k  1 k  1) と共分散 P(k  1 k  1)を更新する
48
UKF:Sigma Pointｓ
i  1,2,, n
X 0 [k | k ]  xˆ[k | k ]
κ
W0 
nκ
X i [k | k ]  xˆ[k | k ] 
κ R


(n   ) P[k | k ] i
1
Wi 
2(n   )
X i n [k | k ]  xˆ[k | k ] 
Wi  n 
i 番目の列ベクトル

M  N とすると
M  NN T である

(n   ) P[k | k ] i
1
2(n   )
49
行列の平方根
P  MM
T
M : P のコレスキー分解等
MATLABでは
N=chol(P);
M=N’;
または特異値分解を使って
P0
T
P  UU
M U 

 1








 n 
50
UKF：Sigma Pointsの性質
平均
2n
W X [k k ]  xˆ[k k ]
i 0
分散
i
i
P  Wi X i [k k ]  xˆ[k k ]X i [k k ]  xˆ[k k ]
2n
i 0
n
T

   P(k k )   P (k k ) 
  2Wi (n   )
i 1
n
i 1
 P(k k )

P(k k )
i

T
P(k k )
i
T
i
i
平均、分散は一致している点の集合
51
補足
P(k | k )  v1 v2
vi 

P(k k )

vn v1 v2
i
とすると
P(k | k )   v1
  v1
vn 
T
v2
v2
, vn 1
 v1T 
 T
 v2 
vn   
 
vn 1 
v T 
 n 
 v1T 
 T 
n
v2 
T

vn 1 
 vn vn   v j v j T


j 1
 T
vn 1 
52
UKF：共分散行列の更新
1.Sigma Pointsを状態遷移関数で遷移させる
X i [k  1 | k ]  f ( X i [k | k ], u[k ])
2.遷移させたSigma Pointsの集合平均で予測平均を近似する
2n
xˆ [ k  1 | k ]  Wi X i [ k  1 | k ]
i 0
３.遷移させたSigma Pointsの集合分散で予測分散を近似する
2n
P[ k  1 | k ]  Wi X i [ k  1 | k ]  xˆ [ k  1 | k ]
T
i 0
 X i [k  1 | k ]  xˆ [k  1 | k ]
53
４.Sigma Pointsを観測関数で遷移させる
Y i [k  1 | k ]  h( X i [k  1 | k ], u[k ])
５.遷移させた点の集合平均で予測観測値を近似する
2n
yˆ [ k  1 | k ]  WiY i [ k  1 | k ]
i 0
６.遷移させたSigma Pointsの集合分散で予測分散を近似する
2n
Pyy [ k  1 | k ]  Wi Y i [ k  1 | k ]  yˆ [ k  1 | k ]
T
i 0
 Y i [k  1 | k ]  yˆ [k  1 | k ]
2n
Pxy [k  1 | k ]  Wi X i [ k  1 | k ]  xˆ [ k  1 | k ]
T
i 0
 Y i [k  1 | k ]  yˆ [k  1 | k ]
54
７.イノベーションの予測共分散を計算する
v[ k  1]  y[ k  1]  yˆ[ k  1 | k ]  y[ k  1 | k ]  w[ k  1]
Pvv [k  1 | k ]  R[k  1]  Pyy [k  1 | k ]
８.カルマンゲインを計算する
W [k  1]  Pxy [k  1 | k ] Pvv1[k  1 | k ]
９.観測値 y[ k  1] から推定値を更新する
xˆ[k  1 | k  1]  xˆ[k  1 | k ]  W [k  1]v[k  1]
１０.予測誤差共分散を更新する
P[k  1 | k  1]  P[k  1 | k ]  W [k  1]Pvv [k  1 | k ]W [k  1]
T
55
ノイズがアフィンでない場合
• 上記の説明では観測ノイズが状態変数と独立で、アフィンな形で加わっ
ていた（ノイズの影響は分散行列の和として計算可能）
• ノイズがアフィンでない場合は状態とノイズの拡大した変数を考えてシグ
マポイントを生成して同様の計算を行う。（ただし、その分計算量が大きく
なる。また、状態と両ノイズに相関がないので、平均の状態にノイズが加
わった形での評価となる。）
 x[k  1]  f ( x[k ], v[k ]) : f a ( xa [k ])

 y[k  1]  h( f ( x[k ], v[k ]), w[k  1])) : ha ( xa [k ])
 x[k ] 
xa [k ] :  v[k ]   R 2 n  p


 w[k  1]
 P[k | k ]

 xˆ[k | k ]

xa   0  , Pxa xa  
Q[k ]




 0 
R[k  1 | k  1]
共分散行列の予測
EKFによる共分散の予測
UKFによる共分散の予測
57
ＥＫＦとＵＫＦの比較（１）
58
ＥＫＦとＵＫＦの比較（1）
 x1  1 
 x   
x   2   2
 x3  1 
   
 x4   2 
1
1
y
y  l1 cos(1 )  l2 cos(1  2 )  n
平面2リンクマニピュレータ
2009/08/24
59
ＥＫＦとＵＫＦの比較（２）
2009/08/24
60
ＥＫＦとＵＫＦの比較（２）
2009/08/24
61
状態とパラメータの同時推定（１）
• 状態方程式
dx1 (t )
  a x1 (t )   b   u (t )
dt
• 観測方程式
y(t )  x1 (t )  4 sin( x1 (t ))  w(t )
y (t ) を観測
状態
 :既知
a、b :未知パラメータ
x1 (t ) と未知パラメータ a、 b を
同時に推定する
62
状態とパラメータの同時推定（２）
観測方程式（モデル）
センサの脈動を
表したモデル
y
x1
63
状態とパラメータの同時推定（３）
R  10
Q0
P0  diag 10 0.5 10
xˆ1
パラメータ
aˆ
パラメータ
bˆ
64
状態とパラメータの同時推定（３）
離散化の影響
T  200 [msec]
y  x1  sin( x1 )
xˆ1
x  xˆ
パラメータ
aˆ
パラメータ
bˆ
逐次最小二乗法
y[k ]  [k  1] 
T
未知パラメータベクトル
出力
ˆ[k ]  ˆ[k  1] 
出力、入力から計算される非線形
関数ベクトル


P[k  2] [k  1]
T ˆ
y
[
k
]


[
k

1
]
 [k  1]
T
[k  1] P[k  2][k  1]
P[k  2][k  1] T [k  1]P[k  2]
P[k  1]  P[k  2] 
1   T [k  1]P[k  2][k  1]
P[1]  0
66
シミュレーション結果
1 0 0


P[1]  0 1 0
0 0 1
パラメータ
aˆ
xˆ1
パラメータ
bˆ
67
適応オブザーバ
 
x  a A x  b u
y  1,0,,0 x  x1

A：既知の n  (n  1) 行列
a, b：未知の n 1 ベクトル

x   k A x  k  a y  b u
y  hT x  x1
k： k A  が漸近安定になるように決める
x  K x  g y  b u
y  hT x  x1
g、 bを推定する
68
適応オブザーバ
オブザーバ
xˆ  K xˆ  gˆ y  bˆ u  v1  v2
yˆ  hT xˆ  xˆ1
  gˆ1 ˆ xˆ2
aˆ 
,b 
bˆ1
bˆ1
e  xˆ  x ,   gˆ  g ,   bˆ  b
誤差方程式
e  K e   y  u  v1  v2
e1  xˆ1  x1
e  0 ,   0 ,   0 となるように  ,  , vを決める
1 , v2
69
シミュレーション結果
  90
2
  5110
d 2  0.01
パラメータ
aˆ
xˆ1
パラメータ
bˆ
70
UＫＦを用いたバックラッシュ系のモデル予測制御
• モデル予測制御
⇒制約条件を考慮したシステマティックな制御系設計
問題点
• 計算時間
• 状態の観測
• ロバストパーフォーマンス
71
•
バックラッシュ
– 多くの機械システムに存在する
– 制御性能の悪化、振動現象
•
バックラッシュを考慮した制御は実用上非常に重要
– 一般には、全ての状態が直接観測されない
• バックラッシュを含むシステムの状態推定
• 微分方程式はスムーズでない⇒ＵＫＦの適用
– バックラッシュ補償
• システマティックに最適制御系を実現したい
• バックラッシュ系は非線形系（非線形最適制御フィード
バック制御）
⇒非線形モデル予測制御の適用
72
モデル
状態方程式
x1  x3
x 2  x4
1
x3 
(u  D1 x3   c )
M1
x 4 
1
( c  D2 x4 )
M2
観測方程式
x 
y   1 v
 x2 
v:センサノイ
ズ
M1
M2
 c  F ( )  G ( , ) : 伝達トルク
  x1  x2 ,   x3  x4
F ( ) : バネ要素
G ( , ) : ダンパ要素
回転系と直動系の両方を表現可能
73
モデル

伝達トルク（力）
 K (  B)  D

 c ( , )  0
 K (  B)  D

 B
 B
  B
  相対距離
2 B  0 : バックラッシュ幅
K  0 : バネ定数
D  0 : ダンパ定数
74
推定シミュレーション

シミュレーション条件(回転系)
観測値(ノイズ：標準偏差0.05の正規外乱)
駆動部位置
(rad)
x1
被駆動部位置
(rad)
x2
入力(±100[Nm]の矩形波)
[Nm]
u
モデルパラメータ1
2]
モータの慣性モーメント
[kgm
M 1  1.0
2]
アームの慣性モーメント
[kgm
M 2  2.0
接触トルクのばね係数
[Nm/rad]
K  1000
[Nms/rad] 接触トルクのダンパ係数
D  10.0
シャフトの摩擦係数
D1  0.1, D2  0.1 [Nms/rad]
バックラッシュ幅
(rad)
2 B  0.2
75
推定シミュレーション結果
• モデル誤差無し
EKFを1とした
Offset
x1
x2
EKF
UKF
 9.74  10
1044  9.72  10
1033
44
1.11 10
10
Variance
EKF
x3
x44
x1
1
 5.01 10
1044  4.44  10
1033
x3
x44
x1
x2
誤差平均・共分散の絶対値の比率
3
4.11 10
103
2.86  1044
0.8
0.6
55
 8.73 10
10
3
1.01 10
103
0.4
0.2
x4
0
x2
UKF
1.93 1044
1.12  1044 8.19  1055
1.29
29 4.20  1011
1.18 1011 6.01 1022
x3
平均
共分散
76
推定シミュレーション結果
• モデルのバックラッシュ幅に誤差が-10%
Offset
EKF
EKFを1とした
UKF
誤差平均・共分散の絶対値の比率
x1
 7.44  104  7.37  104
x2
 1.44  10 4  1.26  10 4
x3
x4
Variance
3
3.94  10
0.8
0.6
3
1.12  10
 8.96  103  4.05 103
EKF
x1
1
0.4
0.2
x4
0
x2
UKF
x1
2.90  104
x2
x3
1.00  10 4 7.30  105
1.31 4.33 101
x4
1.19  101
1.95  104
7.20  10 2
x3
平均
共分散
77
制御則
• 評価関数
– 終端コスト関数・コスト関数：参照軌道との二乗
誤差
（Δτで離散化したシステムを考える）
N 1
J    xN (t )    L( xi (t ), vi (t ))
*
*
*
*
i 0
1
2
1
L : {( x  xr )T Q ( x  xr )  vT Rv}
2
 : ( x N  xrN )T S f ( x N  xrN )
78
シミュレーション
• RHC
– 制御周期 10 [ms]
– 予測ステップ 20 (200[ms])
– 評価関数のパラメータはＰＳＯなどを用いて探索
LQR制御と比較
–ただし、LQRでは２質点は常に接続状態であるとして、フィードバック
ゲインを求める（ＲＨＣの場合の重みとは異なる）
–RHCと同程度の立ち上がり時間の応答で比較
–状態推定にはともにUKFを用いる
–制御周期は１[ms]
79
制御則
• 参照軌道
– 目標状態までの軌道
xr  [ xr1 xr 2 xr 3 xr 4 ur ]T
• 駆動側の参照軌道
– 被駆動部に対し接触面で相対速度0
 xr1   x2  sgn( xr 2  x2 ) B
x   

x
4

 r3  
• 被駆動部の参照軌道
– 被駆動部の目標位置をステップ関数で与える
 xr 2   1 
 x   0 
 r4   
80
シミュレーション
観測値(ノイズ:定常偏差0.05の正規外乱)
x1
駆動部位置
(rad)
x2
被駆動部位置
(rad)
u
入力
[Nm]
モデルパラメータ2
モータの慣性モーメント
[kgm2]
M 1  1.0
アームの慣性モーメント
[kgm2]
M 2  2.0
接触トルクのばね係数
[Nm/rad]
K  2000
[Nms/rad] 接触トルクのダンパ係数
D  10.0
シャフトの摩擦係数
D1  0.01, D2  0.01 [Nms/rad]
バックラッシュ幅
(rad)
2 B  0.04
81
シミュレーション結果
• 被駆動部の応答と伝達トルク
被駆動部の応答
伝達トルク
82
シミュレーション結果
• モデルのバックラッシュ幅に±10%の誤差
被駆動部の応答
伝達トルク
83
モデルパラメータ
観測値
モータの位置
アームの位置
x2 (rad)
入力トルク
u [Nm]
モデルパラメータ
M 1  0.00625 [kgm2 ]
モータの慣性モーメント
M 2  0.025
[kgm2 ]
アームの慣性モーメント
x1 (rad)
K  100
[ Nm / rad ]
D  0.001
[ Nm s / rad ]
D1  0.01, D2  0.01[ Nm s / rad ]
2 B  0 .2
(rad )
接触トルクのばね係数
接触トルクのダンパ係数
シャフトの摩擦係数
バックラッシュ幅
84
シミュレーション結果2
• モデルパラメータ2
85
シミュレーション結果
• 入力
86
ハイブリッドＵＫＦ
連続時間での状態の予測＋離散時間での更新
対象のシステム
dx(t )  f c ( x(t ), t )dt  L(t )d  (t )

dy(t )  hc ( x(t ), t )dt  V (t )d (t )
dx(t )  f c ( x(t ), t )dt  L(t )d  (t )

 yk  h( x(tk ), tk )  rk
列毎の写像の変換
f : Rn  Rm
X i  R n , Yi  R m
Yi  f ( X i )
Y  [Y1 ,
Y  f (X )
, Yd ]  R md , X  [ X 1 ,
, X d ]  R nd
87
連続系のKalman Filter（１）
P(t ) : E{x(t ) xT (t )}, x(t ) : x(t )  xˆ (t )
 xˆ  (t  t ) : xˆ (t )  f c ( xˆ (t ), t )（t 状態の予測値）
 

 yˆ (t  t ) : hc ( xˆ (t  t ), t  t )（t 出力の予測値: 変化分）
 


T
ˆ
ˆ
P
(
t


t
)
:

E
{(
x
(
t


t
)

x
(
t


t
))(
x
(
t


t
)

x
(
t


t
))
（} 予測の共分散）

S (t  t ) : E{( y (t  )  yˆ  (t  t ))( y (t  )  yˆ  (t  t ))T }
C (t  t ) : E{(( x(t  t )  xˆ  (t  t ))( y (t  )  yˆ  (t  t ))T }
K (t  t ) : C (t  t ) S 1 (t  t )
xˆ (t  t )  xˆ  (t  t )  K (t  t )(y (t )  yˆ  (t  t )), y (t ) : y (t  t )  y (t )
連続系のKalman Filter（２）
P  (t  t ) : E{( x(t  t )  xˆ  (t  t ))( x(t  t )  xˆ  (t  t ))T }
 E{( x(t )  xˆ (t )  ( f c ( x(t ), t )  f c ( xˆ (t ), t )) t  L(t )  (t )  o( t ))( x(t ) )T }
x (t )
fc ( t )
 E{x(t ) xT (t )}  E{x(t ) f Tc (t )  f c (t ) xT (t )}t  E{x(t )( L (t ) (t ))T  L(t )  (t ) xT (t )}
0
 E{L(t ) (t )( L(t )  (t ))T }  o( t )
 P(t )  E{x(t ) f Tc (t )  f c (t ) xT (t )}t  L(t )Q(t ) LT (t ) t  o( t )
S (t  t ) : E{( y (t  )  yˆ  (t  t ))( y (t  )  yˆ  (t  t ))T }
y ( t t )
 E{((hc ( x(t ), t )  hc ( xˆ (t ), t )) t  V (t )  (t )  o( t ))( hc
)T }
hc ( t )
 V (t ) R (t )V T (t ) t  o( t )
C (t  t )  E{x(t  t ) y (t  t )T }  E{x(t  t )hc (t )}t  o( t )
89
連続系のKalman Filter（３）
K (t  t )  C (t  t ) S 1 (t  t )
 ( E{x(t  t )hcT (t )}t  o(t ))(V (t ) R (t )V T (t ) t  o( t )) 1
o(t )
o( t ) 1
)(V (t ) R (t )V T (t ) 
)
t
t
o(t )
 E{x(t  t )hcT (t )}(V (t ) R (t )V T (t )) 1 
t
 ( E{x(t  t )hcT (t )} 
K (t )  lim K (t  t )  E{x (t  t )hcT (t )}(V (t ) R (t )V T (t )) 1
t 0
xˆ (t  t )  xˆ  (t  t )  K (t  t )(y (t )  yˆ  (t  t ))
 xˆ (t )  f c ( xˆ (t ), t ) t  K (t  t )( y (t )  yˆ  (t  t ))
dxˆ (t )
dy
 f c ( xˆ (t ), t )  K (t )( z (t )  hc ( x(t ), t )), z (t ) :
dt
dt
90
連続系のKalman Filter（４）
P(t  t )  P  (t  t )  K (t  t ) S (t  t ) K T (t  t )
 P (t )  E{x(t ) f Tc (t )  f c (t ) xT (t )}t  L(t )Q(t ) LT (t ) t  K (t  t ) S (t  t ) K T (t  t )  o( t )
dP(t )
 E{x(t ) f Tc (t )  f c (t ) xT (t )}t  L(t )Q(t ) LT (t ) t  K (t )(V (t ) R(t )V T (t )) 1 K (t )
dt
91
連続系のLITシステムのKalman Filter
dx(t )  ( Ax(t )  Bu (t ))dt  L(t )d  (t )

dy  Cx(t )dt  V (t )d (t )
E{x(t ) f Tc (t )  f c (t ) xT (t )}  E{x (t )( Ax (t ))T  Ax (t ) xT (t )}
 P(t ) AT  AP (t )
E{x(t )hcT (t )}  E{x (t )(Cx (t ))T }  P (t )C T
K (t )  P (t )C T (V (t ) R (t )V T (t )) 1
dP (t )
 P(t ) AT  AP (t )  L(t )Q(t ) LT (t )  K (t )(V (t ) R (t )V T (t )) 1 K (t )
dt
dP (t )
 P(t ) AT  AP (t )  L(t )Q(t ) LT (t )  P (t )C TV (t ) R (t )V T (t ) K (t )CP (t )
dt
92
２次形式最適制御との双対性（１）
Kalman Filter
対象のシステム
dx(t )  ( Ax(t )  Bu (t ))dt  L(t )d  (t )

dy  Cx(t )dt  V (t )d (t )
誤差の共分散行列の更新式
dP(t )
 P(t ) AT  AP(t )  L(t )Q(t ) LT (t )  P(t )C TV (t ) R (t )V T (t ) K (t )CP (t )
dt
２次形式最適制御問題
対象のシステム
dx(t )
 Ax(t )  Bu (t )
dt
2次形式評価関数
T
J ( x)   xT (t )Qx(t )  u T (t ) Ru (t )dt
0
Ricatti方程式
dP(t )
 P(t ) A  AT P (t )  Q  P (t ) BR 1BT P (t )
dt
２次形式最適制御との双対性（２）
誤差の共分散行列の更新式
dP (t )
 P(t ) AT  AP(t )  L(t )Q(t ) LT (t )  P(t )C TV (t ) R(t )V T (t ) K (t )CP (t )
dt
Ricatti方程式
dP (t )
 P(t ) A  AT P (t )  Q  P (t ) BR 1BT P (t )
dt
双対関係
制御　推定
A　 AT
B　 C T
Q　 L(t )Q(t ) LT (t )
R　 V (t ) R (t )V T (t )
行列表現のＵＴ
 w  [W0 ,W1 , ,W2 n ]T

W  ( I  [ w, , w])  diag (W0 ,
c  n  

, W2 n ))  ( I  [ w,
, w])T
2 n 1

X  [m, , m]  c [0, P ,  P ], xˆ  Xw

Y  g (X ), yˆ  Yw

 Pˆ  YW YT

T
 Pˆx  XW Y
95
2n
P   Wi  Xi  xˆ  Xi  xˆ 
T
証明
i 0
 X0  xˆ
  X   xˆ
  X  Xw 1
 X  I   w
W0
X2 n  xˆ  

W0
xˆ  

W0
1 

W0
w 

 XW X T ,W :  I   w
T
   X0  xˆ  




W2 n   X  xˆ T 
 2n


T
 X  xˆ
ˆ 
x



W2 n 

T
 X  Xw 1
1



W2 n 

T
 I  w
T

w
X




W2 n 
W0

 I  w
w 
 

W2 n 
w
T
96
行列表現のＵＫＦアルゴリズム
予測
X (k  1| k  1)  [ xˆ (k  1| k  1), , xˆ ( k  1| k  1)]  c [0, P( k  1| k  1),  P( k  1| k  1)]
 
X (k | k  1)  f (X (k  1| k  1), k  1)

 xˆ (k | k  1)  X (k | k  1) w
 P (k | k  1)  X  (k | k  1)W X  (k | k  1)T  Q(k  1)

更新
X (k | k  1)  [ xˆ (k | k  1), , xˆ (k | k  1)]  c [0, P(k | k  1),  P( k | k  1)]

Y (k | k  1)  h(X (k | k  1))

 yˆ (k | k  1)  Y (k | k  1) w
 P (k | k  1)  Y (k | k  1)W [Y (k | k  1)]T  R(k )
 
 Px (k | k  1)  X (k | k  1)W [Y (k | k  1)]T

W (k )  Px (k | k  1) P (k | k  1)

 xˆ (k | k )  xˆ (k | k  1)  W (k )( y(k )  yˆ (k | k  1))
 P(k | k )  P(k | k  1)  W (k ) P (k | k  1)W (k )T


97
連続系のＵＫＦアルゴリズム（２）
（ＵＫＢＦ）
一般の式で次の近似を用いる
 E{x(t ) f cT (t )}  X (t )Wf cT ( X (t ), t )

T
T
 E{x(t )hc (t )}  X (t )Whc ( X (t ), t )
アルゴリズム
 dm(t )
 dt  f ( X (t ), t ) w  K (t )[ z (t )  h( X (t ), t ) w]

 dP(t )  X (t )Wf T ( X (t ), t )  f ( X (t ), t )WX T (t )  L(t )Q (t ) LT (t )  K (t )V (t ) R (t )V T (t ) K T (t )
c
c
 dt
 K (t )  X (t )WhT ( X (t ), t )[V (t ) Rc (t )V T (t ))]1

 X (t )  [m(t ), , m(t )]  c [0, P(t ),  P(t )]
98
ハイブリッドＵＫＦアルゴリズム
Rc (t )  
 dm(t )
 dt  f ( X (t ), t ) w

 dP(t )  X (t )Wf T ( X (t ), t )  f ( X (t ), t )WX T (t )  L(t )Q (t ) LT (t )
c
 dt
更新のアルゴリズムは離散の場合と同じ
99
領域拘束を考慮した状態推定
 x(k )  f ( x(k  1), u (k  1), w(k  1), k  1)

 y (k )  h( x(k ), v(k ), k )
a(k )   T (k ) xk  b(k ),  (k )  R ns
iT (k ) x(k )  ai (k ), ci  1 if iT (k ) x(k )  ai (k )

T
No
constraint,
c

0
if
a
(
k
)



i
i
i ( k ) x( k )  bi ( k )
 T
T

(
k
)
x
(
k
)

b
(
k
),
c

1
if
b
(
k
)


i
i
i
i (k ) x(k )
 i
100
Truncated UKF
• PDFの打ち切り (shimada,98)
拘束ありの PDF
拘束なしの PDF
xˆ (k | k ), P(k | k )
x(k | k ), P(k | k )
• UKFへの応用
通常のUKF
( xˆ(k | k ), P(k | k )
( xˆ(k  1| k  1), P(k  1| k  1)
( x(k | k ), P(k | k ))
PDF truncation
Generate Sigma
points
UKF Update
101
Step1 : Initialization Step
UKF Based Proposal method
Constrained Unscented Gaussian Sum Filter
Approximate
を混合ガウス分布で近似する
H terms
Process noise
I terms
Measurement noise
Gaussian sum approximate
EM-Algorithm
G terms
2009/12/16
IEEE CDC 2009
102
Step2 : Truncated Unscented Kalman Filtering
UKF Based Proposal method
Constrained Unscented Gaussian Sum Filter
Calculate constrained Gaussian PDF by PDF truncation for each Gaussian PDFs.
PDF
truncation
unconstrained Gaussian PDF
H terms
Process noise
constrained Gaussian PDF
I terms
Measurement noise
PDF
truncation
PDF
truncation
G terms
2009/12/16
PDF
truncation
IEEE CDC 2009
103
Constrained Unscented Gaussian Sum Filter（１）
GHI 個のUKFを異なるノイズによって並列に計算
GHI個のガウス分布が得られる
are derived
H terms
I terms
Process noise
G terms
Measurement noise
PDF
truncation
Time update
Measurement update
PDF
truncation
Time update
Measurement update
PDF
truncation
Time update
Measurement update
GHI UKFs
104
Constrained Unscented Gaussian Sum Filter（２）
推定値はそれらの平均値の重み付平均値で求める
H terms
Process noise
G terms
I terms
Measurement noise
PDF
truncation
Time update
Measurement update
PDF
truncation
Time update
Measurement update
PDF
truncation
Time update
Measurement update
Mixture
GHI terms
105
Constrained Unscented Gaussian Sum Filter（３）
数値的計算量の指数的増大の問題：
混合する分布の数が指数的に増大する
1回目の推定
GHI
2回目の推定
GHI×HI
3回目の推定
GHI×(HI)2
・・・
N回目の推定
GHI×(HI)n-1
計算量を抑えるために“pruning”（枝狩り）を行う
一定以下の重みを持つ要素を捨てる
例)
Constrained
UKF
0.8
次の推定に用いる
Constrained
UKF
0.17
Constrained
UKF
0.03
この要素は捨てる
106
アンサンブルカルマンフィルター(EnKF)
予測
 xˆ (i ) (k | k  1)  f ( xˆ (i ) (k  1| k  1), u (k  1), w(i ) (k  1), k  1)
 (i )
(i )
(i )
 yˆ (k | k  1)  h( xˆ (k | k  1), v (k ), k ), (i  1, , N )
更新
1 N (i )

 xˆ (k | k  1)  N  x (k | k  1)
i 1


1 N (i )
ˆ
y
(
k
|
k

1)

yˆ ( k |k 1)


N

i 1

N
 P (k )  1
{xˆ (k | k  1)  xˆ (i ) (k | k  1)}  { yˆ (k | k  1)  yˆ (i ) (k | k  1)}T

x

N  1 i 1

N
 P (k )  1
{ yˆ (k | k  1)  yˆ (i ) (k | k  1)}  { yˆ (k | k  1)  yˆ (i ) (k | k  1)}T



N  1 i 1
1

 K (k )  Px (k )( P (k ))
 (i )
(i )
(i )

 xˆ (k | k )  xˆ (k | k  1)  K (k )( y (k )  yˆ (k | k  1))
107
計算の簡略化：粒子のプロジェクション
状態拘束を考え、pdf truncationを用いると計算量が増大する
分布のパラメータを修正するより、粒子の方を修正する
x (i ) (k | k )  min x ( i ) ( k |k )[( x (i ) ( k | k )  xˆ (i ) ( k | k ))T I ( x (i ) ( k | k )  xˆ (i ) ( k | k ))]
s.t.
ci (iT (k ) x (i ) (k | k )  ai (k ))  0

T
(i )
ci (i (k ) x (k | k )  bi (k ))  0)(i  1,
, s)
108
数値例（１）
追跡したい軌道
対象の非線形システム
幅 2[m]
ノイズの性質
mean
variance
状態拘束
109
数値例（２）
50回のモンテカルロシミュレーションの結果
計算時間
CPU time
評価指標
推定精度
アルゴリズム
TUKF CUGSF CEnKF(50)
E-CEnKF(50)
E-CEnKF(100)
RMSE of x1 [m]
1.60
1.00
0.94
0.97
0.93
RMSE of x2 [m]
0.54
0.53
0.49
0.49
0.48
Time [ms]
2.72
54.6
94.3
12.7
25.1
※ (50),(100) は粒子数を表す
• 非ガウス性ノイズを陽に仮定するCUGSF, CEnKF、 E-CEnKF はＴＵＫＦよりも良い推定
精度を持つ
• EnKFを基本とするアルゴリズムはCUGSFよりも良い推定を与える
• E-CEnKFはCEnKFの推定精度は同程度であるが非常に高速である
110
数値例（３）
TUKF高速であるが非ガウス性ノイズをどの程度扱えるか？
観測ノイズだけを 0.8 から1.6 ま0.2,刻みで変えて50-回のモンテカルロシミュレーションを行う
TUKFはＲを増加させると性能が非常に悪くなる
CUGSF,E-CEnKFは性能の劣化は小さい
111
数値例：同時推定（１）
•状態とパラメータの一部を同時に推定する問題を考える
•状態と同時にパラメータ “b” も推定する
公称値：0.5 ,不確定性の範囲：±20%
拡大系を考える
X3の状態拘束
注意:
フィルターの安定性を確保するために観測方程式を修正している
112
数値例：同時推定（２）
２００ステップのパラメータ推定結果
(横軸：真値、縦軸：推定値)
RMSE of x1 RMSE of x2
TUKF
1.68
0.94
CUGSF
1.86
0.90
ECEnKF
1.20
0.52
E-CEnKFが状態だけでなくパラメータに関しても良い推定を与えている
113
まとめ
• 本講義では、線形システムに対する状態推定の基
本と非線形システムに対する状態推定手法をＵＫＦ
、UKBFを中心に解説した
• 非ガウス性のノイズや状態拘束に対する対処につ
いても説明した
• その応用例を数値シミュレーションにより示した
• 実際の適用に当たっては、パーティクルフィルターな
ど他の手法との比較、組み合わせが重要である
114
その他のアプリケーション
• EnKFを無駄時間観測や観測順序が乱れた
信号からの推定
(IEEE Aero-space Conference, 2011)
• U変換を利用した確率システムの制御
(SICE 第2回プラントモデリングシンポジウム,2011)
115
参考文献（１）
1. 片山徹著：新版応用カルマンフィルタ朝倉書店
2. A.H.Jazwinski: Stochastic Processes and Filtering Theory, New York:Academic, 1970
3. G.C.Goodwin and K.S.Sin :ADAPTIVE FILTERING PREDICTION AND
CONTROL , PRENTICE-HALL (1984)
4. C.Chui and G.Chen: Kalman Filtering with Real-Time
Applications, 4th ed.,
Springer (2009)
5. 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)
6. T.Lefebvre,et.al「Comment on “A New Method for the Nonlinear Transformation of
Means and Covariances in Filters and Estimators」
7. S.J.Julier and J.K.Uhmann 「A General Method for Approximating Nonlinear
Transformation of Probability Distributions」 [Online]1996
8. 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)
9. S.Julier and J. Uhlmann : Unscented Filtering and Nonlinear Estimation,
Proceedings of The IEEE, Vol. 92, No. 3,
(2004)
10. M.Yamakita et. al. : Comparative Study of Simultaneous Parameter-State
Estimations, Proc. of CCA 2004 (2004)
11. 山北：UKFって何？,,システム制御情報学会 (2006)
12. M.Saito, M.Yamakita: MPC for a Simplified Transmission Model with Backlash
Using UKF, Proc. of CCA2006, pp.527/532 (2006)
13. S.Sarkka: On Unscented Kalman Filtering for Sate Estimation of Continuous-Time
116
Nonlinear Systems, IEEE Trans. Autom Contr., Vol.52, No.9 (2007)
参考文献（２）
14. S.Ishihara, M.Yamakita: Efficient Unscented Filtering for Nonlinear Systems with
State Constraints, Proc. of ECC09 (2009)
15. S.Ishihara, M.Yamakita: Constrained State Estimation for Nonlinear Systems with
non-Gaussian Noise, Proc. of CDC09 (2009)
16. 石原新士、山北昌毅：非ガウス雑音を受ける領域拘束付き非線形システムの状態推定, 電
気学会論文誌 C, Vol. 129,No. 11 (2009)
17. D.Simon and TL.China: Kalman filtering with state equality constraints, IEEE Trans.
On Aerospace and Electronic Systems, vol. 38, No.1, pp.128/136 (2002)
18. 片山徹：非線形カルマンフィルタ（２０１１）
117

Download Report