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

ＵＫＦ（Unscented Kalman Filter)
と
その応用
東京工業大学大学院
機械制御システム専攻
山北昌毅
1
状態推定の必要性
• システムの将来の挙動の予測
• 状態フィードバック
• パラメータ推定
2
状態推推定器（オブザーバ）
オープンループ推定
 xˆ[k  1]  xˆ[k ]  u[k ]

 yˆ[k ]  Cx[k ]
オブザーバ
 xˆ[k  1]  xˆ[k ]  u[k ]  K ( yˆ[k ]  y[k ])

 yˆ[k ]  Cx[k ]
エラーシステム
x[k  1]     KC  x[k ],
x[k ]: xˆ[k ]  x[k ]
3
状態推定とパラメータ推定
 x[k  1]  x[k ]  u[k ]

 y[k ]  Cx[k ]
{u[i], y[i], i  0,..., k}より x[k ]を推定する
y[k ]  a1 y[k 1] 
  y[k 1]
x[k ]  a1
 an y[k  n]  b1u[k 1] 
 y[k  n] u[k 1]
an b1
 bnu[k  n]
 a1 
 
 
a 
u[k  n]  n 
 b1 
 
 
bn 
bn 
T

 x[k  1]  Ix[k ]


 y[k ]  C[k ]x[k ] C[k ]   y[k 1]
  I,   0
 y[k  n] u[k 1]
u[k  n]
4
内容
• 最小分散推定値
• Ｋａｌｍａｎ Filter
– 線形カルマンフィルタ
– 拡張カルマンフィルタ
– Unscented Kalman Filter (UKF)
• 例題を用いた状態推定比較
• UKFとＲＨＣの併用例
• まとめ
5
•期待値 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

１次モーメント：平均
３次モーメント：歪度
２次モーメント：分散
４次モーメント：尖度
6
ガウス分布
•１次元
 x  x 2 


p( x) 
exp
2
 2 
2 2


1
X  x
•分散： EX  x2   2
•奇数次モーメント： EX  x2n1 0
•偶数次モーメント： EX  x2n  1 3 2n 1 2n
•平均： E
7
•多次元
x R
n

x  E{x},   E  X  x  X  x 
T

 1

T 1




p( x) 
exp

X

x

X

x


1/ 2
n/ 2
 2

2  
1
•２次元
  x21  x1x2 

2 


 x1x2
x2 

8
•２次元分布（相関による変化）
2


 x1x2 
2
 
x1
x   
2 


 x1x2
x2 
3

1.0 0.5


0
.
5
0
.
5


1.0 0 


0
0
.
5


 1.0  0.5



0
.
5
0
.
5


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)
y
推定量
x̂
推定ルール
g (y)

評価関数 E x  x̂
2
 を最小にする推定値
|| x  xˆ ||2  ( x  xˆ)T ( x  xˆ)  trace( x  xˆ)T ( x  xˆ)

 trace ( x  xˆ)( x  xˆ)T

E{|| x  xˆ ||2}  traceE{( x  xˆ)( x  xˆ)T }  trace
条件付き期待値
（
xˆ  Ex y と等価
x の分布の種類によらず）
11
•証明
E f ( X , Y )   f ( x, y) p( x, y)dxdy
  f ( x, y) p( x y)dx p( y)dy


 E E  f ( X , Y ) Y , E  f ( X , Y ) Y  :  f ( x, y) p( x y)dx
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  xy  xy  X   g  xy  xy  X Y ,
T
xy : E{X | Y }

 E E || g  x || Y   2E E  g  x   x  X  Y   E E || x  X || Y 
 E E || g  xy || 2  g  xy   xy  X   || xy  X ||2 Y
2
T
T
2
y
y
2
y
y
12

続き E E  g  xy   xy  X  Y
T
 

 E  g  x y  E x y  X Y 
T


 E  g  x  0  0
 E  g  xy  xy  E  X Y 
T

T
y

E g (Y )  X
2
  E E  g(Y )  x
これが最小になるのは
g(Y )  xy
2
y


Y  E E xy  X
の時。
2
Y

g (Y )と無関係
つまり、
g (Y )  EX Y  が最小分散推定値となる
13
ここまでのまとめ
•最小分散推定値は条件付期待値である。
•どうやって条件付期待値を求めるのか
•条件付確率密度関数が得られても計算が困難
•ガウス分布を仮定すると容易に計算可能
14
ガウス分布の条件付期待値



 xx  xy   E x  x x  x T



T




E
y

y
x

x
yy 
 yx

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 
T
 22 is nonsingular, 11  12 22112
is nonsingular
 X 1
 X 1Y 
1.   
1
T
1
T
1 

(
X
Y
)
Z

Y
X
Y

T
X : 11  12 22112
1
1
Y : 12 22
Z   22
2.det()  det( X ) det( Z )
(証明)
T
 I n 12  221  11  12 22112
0 
= 


T
0
I


n
12
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 
1
 11 12   X 1   1 
 T   (  X      ))
 12
22 
 2  2
1
 11 12   X 1   1 
 T   (  X      )
 12
22 
 2  2
 X 1Y   X 1  1 
 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
偏差と観測値の‘直交条件’
E{( x  xˆ ) yT }  ?
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が出る
20
線形カルマンフィルタ
•対象モデル
x[k  1]  A[k ]x[k ]  v[k ]
y[k ]  C[k ]x[k ]  w[k ]
x[k ]  Rn状態ベクトル
y[k ]  Rl 観測ベクトル
v[k ]  Rn 状態外乱ベクトル
w[k ]  Rl 観測外乱ベクトル

 xk 1  Axk  vk 


y

Cx

w
k
k 
 k

Qk 0 
v[k ]  T

T

E

[l ]
v [k l ] w [k l ]  
w[k ]
 0 Rk 
21
•線形カルマンフィルタ
初期推定値 xˆ0|1 とその予測誤差共分散 P0|1
１．カルマンゲインを計算
Wk  P
T
k|k 1 k

C  Rk
T
k k|k 1 k
C CP

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  Ak ( Pk|k
T
 Pk |k 1  Wk Ck Pk |k 1 ) Ak  Qk
22
•証明
 x[k ] 
 x[k  1]  A[k ] I 0 
 x  x[k  1] 
　
v
[
k
]
 y  y[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 ] : x[k ]  xk|k 1 , y[k ] : y[k ]  C[k ]xk|k 1
 x[k  1]  A[k ]x[k ]  v[k ] 


Pk|k 1 : E{x[k ]xT [k ]}

 y[k ]  C[k ]x[k ]  w[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 

    C P AT
T
T
T
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]の条件付期待値及び条件付分散である
23
ˆ ˆ 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  A P
A  Q  Ak Pk|k 1C Ck Pk|k 1C  R  Ck Pk |k 1 AT
T
k k |k 1 k
T
k
T
k
1
 Ak ( Pk|k 1  Pk|k 1C Ck Pk|k 1C  R  Ck Pk|k 1 ) AT  Qk
T
k
T
k
 Ak ( Pk|k 1  Wk Ck Pk|k 1 ) Ak T  Qk
1
24
従ってゲインを次のように定義し、
Wk  P
T
k|k 1 k

C  Rk
T
k k|k 1 k
C CP

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
25
シミュレーション例
•３次の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
26
シミュレーション結果
a1  2.76
a2  2.5329
a3  0.778688
R  1.0
P0  I
xˆ0  0
â1
â3
â2
観測回数
27
線形カルマンフィルタ（入力あり）
•対象モデル
xk 1  Ak xk  Bk uk  vk
yk  Ck xk  wk
uk  Rm 入力ベクトル
ほとんど同様に計算できる
Wk  P
T
k|k 1 k

C  Rk
T
k k|k 1 k
C CP

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
（条件なし期待値の計算）
28
白色化フィルター（イノベーションプロセス）
元のシステム
 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  AkWk ( yk  Ck xˆk|k 1 )
 k : yk  Ck xˆk|k 1

Kk : AkWk
システムの別表現
 xˆk 1|k  Ak xˆk|k 1  Bk uk  Kk k

 yk  Ck xˆk|k 1  k
Ak , Bk , Ckが一定の場合、 vkが白色信号であることが示せる
29
拡張カルマンフィルタ
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 ]
30
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


31
予測出力を用いた非線形カルマンフィルタ（１）
（ＵＫＦの考え方も同じ）
p( x, y)の同時分布の考え方で予測出力を用いた場合

 x  x[k  1]
 x  x[k  1] 
今ま
では




y

y
[
k

1]
y

y
[
k
]




y  g(x)
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 ]
 E{( x[k  1]  xˆ[k  1| k ])( x[k  1]  xˆ[k  1| k ])T }
y と Pyy を推定する
y : y[k  1]
y : yˆ[k  1| k ]  E{ y[k  1]}
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])
32
予測出力を用いた非線形カルマンフィルタ（２）
x[k  1]  f ( x[k ], k , v[k ])
y[k  1]  h( x[k  1], k  1, 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）
33
予測出力を用いた拡張カルマンフィルタ
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 ]
f ( x) C[k  1]  h( x)
A[ k ] 
x x  f ( xˆ[ k ],u[ k ])
x x  xˆ
[k]
等価外乱
34
34
Unscented Kalman Filter（UKF)
•拡張カルマンフィルタの問題点
•線形近似する際にヤコビアンを計算しなければならない
（不連続なシステム、Hard Nonlinearity）
•推定値にバイアスが乗ることがある
（平均値の変換は変換後の平均値になると仮定している）
•発散することもある
システムを近似するより統計量を近似するほうが容易
数カ所のサンプル点（Sigma Points）を選び、集合平均的に統計量を近似する
x と Pxx が既知
y  g(x)
y と Pyy を推定する
Unscented Transformation
（Ｕ変換）
35
Sigma Pointsの考え方
xR
x
n
を平均値
x
、分散行列
に対して、２ｎ＋１個の代表点
離散点の生起確率を
Wi
xx の確率変数ベクトルとする
i (i=0,1, ,2n)
とする。ただし、 i
2n
モーメントまでは一致させる
W
i 0
i
i
 x,
y  g ( x)
 i  g ( i )
y  yˆ
2n
  iWi ,  yy  ˆ yy
i 0
を考えて、それぞれの
の集合的統計的性質は２次の
2n
    x    x 
T
i
i
Wi   xx
i=0
y1
2n
y2
  i  yˆ  i  yˆ  Wi
i=0
T
36
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)を更新する
37
UKF:Sigma Pointｓ
i  1,2,, n
X 0 [k | k ]  xˆ[k | k ]
κ
W0 
nκ
κ R


X i [k | k ]  xˆ[k | k ]  (n   ) P[k | k ] i
1
Wi 
2(n   )
i 番目の列ベクトル

M  N とすると
M  NN T である

X i n [k | k ]  xˆ[k | k ]  (n   ) P[k | k ] i
Wi  n 
1
2(n   )
38
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
平均、分散は一致している点の集合
40
補足
P(k | k )  v1 v2
vi 

P(k k )

vn v1 v2
vn 
T
i
とすると
P(k | k )   v1 v2
  v1 v2
 v1T 
 T
 v2 
, vn 1 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 
41
Sigma Pointsを用いた推定の性質
１．
ŷ
は新の平均値
y
を２次のorderまで近似
（EKFは１次のorderまで近似）
２．
ˆ yy
3.

x
は
 yy
を３次のorderまで近似（これはEKFと同じ）
はチューニングパラメータ
がGaussianの場合
n   3
と選ぶのが良い
n  3の時が負となる フィルターが不安定化しやすい

ベイズ推定の積分近似より導かれた CKF(Cubature KF)の利用も
（代表点の選び方と生起確率が少し異なり２ｎの点を生成）
42
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 ]
43
４.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 ]
44
７.イノベーションの予測共分散を計算する
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
45
ノイズがアフィンでない場合
• 上記の説明では観測ノイズが状態変数と独立で、アフィンな形で加わっ
ていた（ノイズの影響は分散行列の和として計算可能）
• ノイズがアフィンでない場合は状態とノイズの拡大した変数を考えてシグ
マポイントを生成して同様の計算を行う。（ただし、その分計算量が大きく
なる。また、状態と両ノイズに相関がないので、平均の状態にノイズが加
わった形での評価となる。）
 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|]
不安定化への対処の方法
• 状態外乱の共分散行列を大きめに設定する
（真の値を用いずに調整パラメータと考える）
ＣＫＦアルゴリズム


2n
f ( x) px ( x)dx   wi f (i ), x～N (0, I )
i 1
 nei
(i  1, , n)
1
wi  , i  
2n
 nei n (i  n  1, , 2n)
I n  e1 e2
y～N (, )


f ( y) py ( y)dy   f ( 2 x   ) px ( x)dx

en 
47
共分散行列の予測
EKFによる共分散の予測
UKFによる共分散の予測
48
シミュレーション例
• 状態方程式
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 を
同時に推定する
49
観測方程式（モデル）
センサの脈動を
表したモデル
y
x1
50
逐次最小二乗法
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
51
シミュレーション結果
1 0 0


P[1]  0 1 0
0 0 1
x̂1
パラメータ
â
パラメータ
b̂
52
シミュレーション結果
R  10
Q0
P0  diag 10 0.5 10
x̂1
パラメータ
â
パラメータ
b̂
53
UＫＦを用いたバックラッシュ系のモデル予測制御
• モデル予測制御
⇒制約条件を考慮したシステマティックな制御系設計
問題点
• 計算時間
• 状態の観測
• ロバストパーフォーマンス
54
•
バックラッシュ
– 多くの機械システムに存在する
– 制御性能の悪化、振動現象
•
バックラッシュを考慮した制御は実用上非常に重要
– 一般には、全ての状態が直接観測されない
• バックラッシュを含むシステムの状態推定
• 微分方程式はスムーズでない⇒ＵＫＦの適用
– バックラッシュ補償
• システマティックに最適制御系を実現したい
• バックラッシュ系は非線形系（非線形最適制御フィード
バック制御）
⇒非線形モデル予測制御の適用
55
モデル
状態方程式
x1  x3
x2  x4
1
x3 
(u  D1 x3  c )
M1
1
x4 
( c  D2 x4 )
M2
観測方程式
x 
y   1 v
 x2 
v:センサノイ
ズ
M1
M2
 c  F ( )  G( , ) : 伝達トルク
  x1  x2 ,   x3  x4
F ( ) : バネ要素
G( , ) : ダンパ要素
回転系と直動系の両方を表現可能
56
モデル

伝達トルク（力）
K (  B)  D   B

 B
 c ( , )  0
K (  B)  D   B

  相対距離
2B  0 : バックラッシュ幅
K  0 : バネ定数
D  0 : ダンパ定数
57
推定シミュレーション

シミュレーション条件(回転系)
観測値(ノイズ：標準偏差0.05の正規外乱)
駆動部位置
x1
(rad)
被駆動部位置
(rad)
x2
入力(±100[Nm]の矩形波)
[Nm]
u
モデルパラメータ1
2]
モータの慣性モーメント
[kgm
M1  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)
2B  0.2
58
推定シミュレーション結果
• モデル誤差無し
EKFを1とした
誤差平均
x1
x2
x3
x4
誤差共分
x1
散
x2
x3
x4
EKF
UKF
誤差平均・共分散の絶対値の比率
 9.74104  9.72 103
x1
1
 5.01104  4.44 103
0.8
0.6
1.11104  8.73105
4.11103
1.01103
2EKF
.86 104
1.UKF
93104
1.12 104 8.19 105
1.29 4.20 101
1.18101 6.01102
0.4
0.2
x4
0
x2
x3
平均
共分散
59
推定シミュレーション結果
• モデルのバックラッシュ幅に誤差が-10%
誤差平均
x1
x2
x3
x4
誤差共分
散
x1
x2
x3
x4
EKF
EKFを1とした
UKF
誤差平均・共分散の絶対値の比率
 7.44104  7.37 104
x1
1
1.44104 1.26 104
3
3.9410
EKF
0.6
3
1.12 10
 8.96103  4.05103
2.90 104
0.8
0.4
0.2
x4
0
x2
UKF
1.95104
1.00 104 7.30 105
1.31 4.33101
1.19 101 7.20 102
x3
平均
共分散
60
制御則
• バックラッシュ補償制御
– 駆動部分を動かし、被駆動部分を早く制御
– 被駆動部分の振動を抑える
 Receding Horizon Control (RHC)を適用
– 予測ホライゾン：有限長の評価区間
– 予測されるプラントの振る舞いが最良となる入力を選択
C/GMRES法＋システムの切り替えを陽に考慮
（速度の不連続性にも対応可）
Direct Multiple Shooting （ＤＭＳ）法
61
制御則
• 入力の微分値を制御器より求める
– モデルの状態変数を拡張（新たな入力 v）
x1, x2 , x3 , x4 , u
T
– システムの切り替え時にも入力計算を安定化
 x   f ( x, u)
u    v 
 


x
f ( x ,v )
拡大システム
62
制御則
• 評価関数
– 終端コスト関数・コスト関数：参照軌道との二乗
誤差
（Δτで離散化したシステムを考える）
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
 : ( xN  xrN )T S f ( xN  xrN )
63
制御則
• システムの拘束、切り替えを含めた評価関数
M
M  ( k 1 ) / 
k 1
k 1 i  ( k  ) / 
J*    xN * (t )   k T  k ( x k (t ))  


H ( xi* (t ), vi* (t ))
– ホライゾン上でM回システムの切り替えが起こる
– 内点拘束として随伴
• Hamiltonian
H : L(x,v)   f(x, v)
  共状態変数

64
制御則
• 最適性必要条件（離散化）
*
i 1
x
 xi  f ( xi , vi )
*
*
x0*  x(t )
 *   *  H xT 
*  xT ( xN * )
Hv  0
65
制御則
• システムの切り替えを考慮
– 内点拘束で表現
( x1 ( k )  x2 ( k ))2  B2
k 
0
2
• 内点拘束による条件
T
  k 
  k
 ( k )   ( k )  
 x( k ) 


H ( k )  H ( k )
 k : ホライゾン上の切り替え時刻


66
シミュレーション
• RHC
– 制御周期 10 [ms]
– 予測ステップ 20 (200[ms])
– 評価関数のパラメータはＰＳＯなどを用いて探索
LQR制御と比較
–ただし、LQRでは２質点は常に接続状態であるとして、フィードバック
ゲインを求める（ＲＨＣの場合の重みとは異なる）
–RHCと同程度の立ち上がり時間の応答で比較
–状態推定にはともにUKFを用いる
–制御周期は１[ms]
67
制御則
• 参照軌道
– 目標状態までの軌道
xr  [xr1 xr 2 xr3 xr 4 ur ]T
• 駆動側の参照軌道
– 被駆動部に対し接触面で相対速度0
 xr1   x2  sgn(xr 2  x2 ) B
x   

x4

 r3  
• 被駆動部の参照軌道
– 被駆動部の目標位置をステップ関数で与える
 xr 2  1
 x   0
 r4   
68
シミュレーション
観測値(ノイズ:定常偏差0.05の正規外乱)
x1
駆動部位置
(rad)
x2
被駆動部位置
(rad)
u
[Nm]
モデルパラメータ2
[kgm2]
M1  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)
2B  0.04
入力
モータの慣性モーメント
アームの慣性モーメント
接触トルクのばね係数
接触トルクのダンパ係数
シャフトの摩擦係数
バックラッシュ幅
69
シミュレーション結果
• 被駆動部の応答と伝達トルク
被駆動部の応答
伝達トルク
70
シミュレーション結果
• モデルのバックラッシュ幅に±10%の誤差
被駆動部の応答
伝達トルク
71
モデルパラメータ
観測値
モータの位置
アームの位置
入力トルク
u [Nm]
モデルパラメータ
M1  0.00625 [kgm2 ]
モータの慣性モーメント
M 2  0.025
[kgm2 ]
アームの慣性モーメント
x1 (rad)
x2 (rad)
K  100
[ Nm / rad]
D  0.001
[ Nms / rad]
D1  0.01, D2  0.01[ Nms / rad]
2B  0.2
(rad)
接触トルクのばね係数
接触トルクのダンパ係数
シャフトの摩擦係数
バックラッシュ幅
72
シミュレーション結果2
• モデルパラメータ2
73
シミュレーション結果
• 入力
74
UKFを用いたモデル予測制御の課題
• 状態推定
– UKFによって制御に必要な状態量を推定
– バックラッシュを含むシステムに対してEKFよりも高い
精度
• 制御
– C/GMRES法を適用
– 素早い応答
– 振動抑制
• 課題
– 現実のバックラッシュシステムに対する本手法の有効
性の検証
75
まとめ（EKFとUKFの比較）
• UKFはEKFが適用できないような不連続
システムにも適用できる
• 非線形なシステムにおいてより正確な推定を
行える
• 実装が容易（計算時間は大きい）
• 離散UFK、連続－離散UKF(Sarkka)の有効
性の実験的検証
76
参考文献
•
•
•
•
•
•
•
•
•
•
•
•
•
片山徹著：新版応用カルマンフィルタ朝倉書店
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
77
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
種々の手法の推定法の概略
1. カルマンフィルター（ＫＦ）
p( x(0))をガウス分布に従うとして、解析的に条件付期待値を計算
２．拡張カルマンフィルター（ＥＫ F)
誤差の分布がガウス分布に従うと近似して、解析的に条件付期待値を計算
３．ＵＫＦ（無香料カルマンフィルター）
状態の分布を２次の統計量まで近似して、解析的に条件付期待値を計算
４．アンサンブルカルマンフィルター（ＥｎＫ F)
初期状態分布に基づき代表点を生成し、各代表点の値を解析的に更新する。
条件付期待値は各点より集合平均的に数値計算する。
５．パーティクルフィルター（粒子フィルター）
分布を代表点の数の分布で近似し、条件付確率分布を解析的に近似する。
条件付確率分布に従って粒子を再サンプリングし、条件付期待値を集合平均で求める。
79
種々の手法のイメージ（１）
１．カルマンフィルター
x2
t
x1
２．ＵＫＦ
x2
x1
t
80
種々の手法のイメージ（２）
１．アンサンブルカルマンフィルター
x2
t
x1
２．パーティクルフィルター
x2
x1
t
81
EnKF（アンサンブルカルマンフィルター）
① Generate N particles from
② Transform each particles as follow
The EnKF uses Monte Carlo sampling and the number of sampling
points N is not related to the dimension of the augment state.
③ Calculate mean and covariance matrix
④ Update each particles as follow,
1
 yy

( yˆk(i|k) 1  yk|k 1 )( yˆk(i|k) 1  yk|k 1 )T
PkUse
Nsame
1 kalman gain for all

i)
Pkxy  1 ( xˆk(particles.
 x )( yˆ (i )  y )T
N 1 |k 1 k|k 1 k|k 1 k|k 1

⑤ The updated state estimate is computed as the
mean of the updated particles.
Advantage of EnKF
 The EnKF can handle any non-Gaussian noises.
 The EnKF uses fewer particles than the Particle filter [Gillijns,06]
etc.
82
パーティクルフィルター
n
p
 xk  f ( xk 1, wk 1 ), xk , wk 1  R , yk , vk  R

(ただし、 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 )
dvp , dyk  : dyk1dyk 2
yvp )
h1 ( xk , yk )
dyk より
yk
p( yk | x(i ) k )  p(h1 ( xk , yk ))
h1 ( xk , yk )
: c(i ) k
yk
(iv) xkの頻度（確率）は代表点( 粒子）の数で表しているので、１つの粒子の
1
確率は全てである。よって p( x(i ) k | yk )は次式で計算される。
N
p( x(i ) k | yk ) 
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 )の分布となる）
83
課題１
次のARMAモデルで表されるシステムについて y(k )の最小分散予測
を行い、ノイズのモデルを無視した場合と予測誤差の統計的分散を
比較しなさい。
y(k )  a1 y(k 1)  a0 y(k  2)  w(k )   1w(k  2)   0 w(k  2),(k  0)
ただし、 a1  0.4, a0  0.64,  1  0.2,  0  0.9, y(1)  y(0)  0とし、 w(k )
は平均ゼロで分散0.1の正規白色ノイズであるとする。また、 y(k )の
予測とは、 y(k 1)以前のデータとシステムのパラメータのみから y(k )
を推定することである。
84
課題2-（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
85
課題2-（２）運動方程式
m l cos(1 )  m2 (a1 cos(1 )  l2 cos(1  2 )) 
g  1 1

m2l2 cos(1  2 )


l1  0.5[m], a1  1.0[m]
l2  0.25[m], a2  0.5[m]
m1  10[Kg ]
m2  5[ Kg ]
I1  5[ Kgm2 ]
I2  0.5[ Kgm2 ]
d1  d2  0[ Nms / rad ]
g  9.8[m / s2 ]
課題2-（３）
 D(q)q(t )  h(q(t ), q(t ))   (t ), q(t ) : 1(t ) 2 (t ) , uc (t ) : 1(t )  2 (t ) 
T
T
q(t )  x (t ) 
xc (t ) :   :  c1 
q(t )  xc 2 (t )
xc 2 (t )


xc (t )   1
 : fc ( xc (t ), uc (t ))
D
(
x
(
t
))(
u
(
t
)

h
(
x
(
t
),
x
(
t
)))
c1
c
c1
c2


x[k ]: xc (kTs ), u[k ]: uc (kTs ), Ts  0.01[s]
y[k ]: y(kTs )
x[k  1]  x[k ]  fc ( x[k ], u[k ])Tc : f ( x[k ], u[k ])
y[k ]  l1 cos( x1[k ])  l2 cos( x1[k ]  x2[k ])  n
87
課題2-（４）
1 (t ) 
 0.0 
 (t )
 / 4
 , 1 (t )   2 (t )  0として以下の設問に答えよ。
xc (t ) :  2  , xc (0) : 
1 (t ) 
 0 





(
t
)
0


 2 
（１） y(t )を観測して x(t )を UKFを用いて推定せよ。
ただし、物理パラメータは既知としてよい。また、観測ノイズ
nは平均値ゼロで、標準偏差が0. 01[ r ad] の正規白色ノイズとする。
（２） ( 1) の問題でm2も未知として、これも同時に推定せよ。
（持っているロードを同時に推定することに相当）
88

Download Report