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

(1)

1

確率システムの状態推定入門

東京工業大学大学院

機械制御システム専攻

(2)

2

内容

• 最小分散推定値

• Ｋａｌｍａｎ Filter

– 線形カルマンフィルタ

– 拡張カルマンフィルタ

– Unscented Kalman Filter (UKF)

• 例題を用いた状態推定比較

– UKFとＲＨＣの併用例

• Kalman-Bucy Filter-UKBF

• 状態拘束・非ガウス性外乱に対する対

– 混合ガウス分布

– アンサンブルカルマンフィルター

• まとめ

(3)

3

x

•期待値





_

  







x

X

x

p

x

dx

X

E

(

)

k

(

)

k

(

)

2 x



•分散

 

_

  



E

X

xp

x

dx

x

(

)



X

x



x

p

x

dx

E

x

(

)

(

)

(

)

2 2 2















• 次統計モーメント

k

１次モーメント：平均

２次モーメント：分散

３次モーメント：歪度

４次モーメント：尖度

(4)

4

ガウス分布

















_





₂ 2 2

exp

₂

2

1 )

(





x

p

 

X

x

E



•平均：









2







2

x

X

E

•分散：







X



x

2n1





0 E

•奇数次モーメント：







n







n

x

X

E



2



1 

3 





2 

1 

2

•偶数次モーメント：

•１次元

(5)

5

•多次元

n

R

x







E





X



x



X



x



T



 





















_

_

_







x

X

x

X

x

p

T n 1 2 / 1 2 /

₂

1 exp

2

1 )

(



•２次元

















₂ 2 2 2 1 2 1 1 x x x x x x



(6)

6

•２次元分布（相関による変化）

















₂ 2 2 2 1 2 1 1 x x x x x x



















5 .

0

0 .

1 















5 .

0

5 .

0

5 .

0

0 .

1 

















5 .

0

5 .

0

5 .

0

0 .

1 













3

2 x

(7)

7

Fact

正規分布を持つ確率変数のAffine変換された確率変数の確率

分布は正規分布となる。つまり、

xを正規性の確率変数としてyを次の式で定義する

n

,

m

,

m n

,

m

y



Ax b x





R y



R

A



R



b



R

yは正規性確率変数となり平均、分散は次の式で計算される

{ }:

{ }

E y

 

y

AE x

 

b

Ax



b

{(

)(

) }:

{ (

)(

)

}

{(

)(

) }

T

yy

T

E y

y y

y

E A x

x x

x

A

AE x

x x

x

A

A A



  







 

(8)

8

最小分散推定値

x

推定したい

パラメータ

観測量

y

)

( x

y

p

評価関数を最小にする推定値



2



ˆx

x

E



条件付き期待値と等価

_x

ˆ



_E

 

_x

_y

（の分布の種類によらず）

x

xˆ

推定量

推定ルール

)

( y

g

(9)

9

•証明



( , )



( , ) ( , )

,

( | ) :

( , )

( )

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

f

X

Y



E

(

,

)





g

Y

X



E



E



g

Y

X

Y



E

(

)



2



(

)



2

   













_











_

_

_



E

g

(

Y

)

E

X

Y

E

X

Y

X

2

Y



 









E

g

x

X

g

x

X

Y



E



_y



_y



T



_y



_y







 









E

g

x

X

g

x

X

Y



E



_y



_y







_y T _y





2 2

2

0 期待値をとると

(10)

10

続き



g

Y

X



E



E



g

Y

x

X

Y



E

(

)



2



(

)



_y 2



_y



2

















_

_

_











_



E

g

(

Y

)

x

_y 2

Y

E

x

_y

X

2

Y

これが最小になるのは

y

x

Y

g

(

)



の時。

 

X

Y

E

Y

g

(

)



が最小分散推定値となる

つまり、

)

(Y

g

_と無関係

(11)

11

ここまでのまとめ

•どうやって条件付期待値を求めるのか

•最小分散推定値は

条件付期待値

_である。

•条件付確率密度関数が得られても計算が困難

•ガウス分布を仮定すると容易に計算可能

(12)

12

ガウス分布の条件付期待値

 

x

y

x

A



y



E

x

ˆ





₀



 







 





x

E

x

y

x

E

x

y



E



T





_xx



A

₀



_yx

y x yy

A

₀

















































_



























T T T T yy yx xy xx

y

E

x

y

E

y

x

E

x

E





注：ガウス分布でないとき線形最小分散推定値となる

b

Ay

x

ˆ





1 0 





_x_y _yy

A

T T yy T xy





A

0



より上の共分散は

T yy xx



A

0



A

0



(13)

13 11 12 12 22 1 22 11 12 22 12 1 1 1 1 1 1 1 11 12 22 12 1 12 22 22 1 12 22

[

]

,

is nonsingular,

is nonsingular

1. (

)

:

2.det( )

det( ) det( )

(

)

=

0

T T T T T T n n

X

X Y

Z

Y X Y

X

Y

Z

X

Z

I

         







 

_

_

  









    







  

_

_







     

  

 

 



 



 



補題

証明

1 11 12 22 12 12 22

0

T 



    



 

_







証明

(14)

14

条件付確率密度関数





1 2 1 / 2 1/ 2 1 2 1 / 2 1/ 2 1 1, 2 2 1 1 11 12 ( ) / 2 1/ 2 2 2 12 22

1

1 ( )

(

)

(

))

(2 )

(det( ))

2 :

1 (

(

)

(

))

(2 )

(det( ))

2 (

)

1

1 (

(

)

(2 )

(det( ))

2

T n T n T n T T n n

p X

Exp

X

Exp

d

X

p

p X X

X

Exp

X





 



 

  







  



































_

_















   





_

_{    }





_

_{   }



1 1 1 2 2 1 1 1 11 12 1 1 2 2 12 22 2 2 1 1 1 1 1 1 1 1 1 2 2 2 2 1 1 1 2 2 1 1 2 2

(

))

(

)

(

)

(

)

(

))

(

))

T T T T T T

X

X Y

X

X Y

Z

Y X Y

X

Y X

X

Y X



       

 

  



 

  

 

  





   

 

  





   

_

 

  



   

 

  





















_

_

_

_

_

_























1 2 2 2 2

(

X



)

T

Z



(

X



)





(15)

15 1 2 2 1 1 2 1 2 1 ₍ _{)/ 2} _{1/ 2} 2 2 2 2 1 1 1 2 2 1 1 2 2 1 1 2 2 2 2 / 2 1/ 2 1 2 1 2 2 / 2

1

1 (

)

( ,

)

(

)

(

))

2 (2 )

(det( ))

((

(

))

(

))

1

1 (

(

)

(

))

2 (2 )

(det( ))

(

,

)

(

|

)

(

)

1 (2 )

(det(

T n n T T n n

p X

p

X d

Exp

X

Z

X

Exp X

Y X

X

Y X

d

Exp

X

Z

X

Z

p X X

p X

X

p X















   



















1 2 1 1 2 2 1 1 1 2 2 1 2 1 1 2 2 / 2

1 (

(

))

(

{

|

}

(

)

{

|

}

)))

2 ))

T

E X

X

Exp

Y X

Var X

X

Y X

X

Y X

X



















(16)

確率密度関数のレベル集合との関係

16

( , )

y x

y

ˆx

yy xy

 

’大きく’

 

の効果が小さく

(17)

拘束条件付ノルム最小化と直行条件

17

ˆ

||

|| :|| || . . c

ˆ

,

0 T

Min

x

e

s t

x

d

e y

x







 

x

ˆx

y

(18)

偏差と観測値の‘直交条件’

18 1 12 22 12 22 12 12 12 22 1 12 22

ˆ

{(

)

}

?

ˆ

(

),

{(

(

))

}

{(

(

))(

) }

0

T T T

E x

x y

x

Y y

y

Y

E x

x

Y y

y y

E x

x

Y y

y

Y

 





 



  

 





 



 

     

    

   

  

逆にを知らなくとも、

より、

が出る

(19)

種々の手法の推定法の概略

19 F) F)

( (0))

p x

２．　誤差の分布がガウス分布に従うと近似して、解析的に条件付期待値を計算３．　状態の分布を２拡張カルマ次の統計量まで近似して、解析的に条件付期待値を計算４．　初期状態分布に基づき代表点を生成し、各代ンフィルター（ＥＫＵＫＦ（無香料カルマンフィルター）アンサンブルカルマンフィルター点（ＥｎＫ表

1. をガウス分布に従うとして、解析的に条件付

カルマンフィルタ

期待値を計算

ー（ＫＦ）

の値を解析的に更新する。　条件付期待値は各点より集合平均的に数値計算する。５．　分布を代表点の数の分布で近似し、条件付確率分布を解析的に近似する。　条件付確率分布に従って粒子を再サンプリンパーティクルフィルター（粒子フィルターグし、条件付期待値を集合平均で）求める。

(20)

種々の手法のイメージ（１）

20

t

1

x

2

x

１．カルマンフィルター

t

1

x

2

x

２．ＵＫＦ

(21)

種々の手法のイメージ（２）

21

t

1

x

2

x

１．アンサンブルカルマンフィルター

t

1

x

2

x

２．パーティクルフィルター

(22)

パーティクルフィルター

22 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 i k k i i i k n p k 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 k k k k k k vp k k k vp k k k k k k k i k i k k k k k k k i k k k k h x y dv dy dv dv dv d dy dy dy y y h x y p v dv p v dy y h x y p y x p h x y y p y x iv x c                    



を次の式によって計算　　　　1= より　　の頻度（確率）は代表点(粒子）の数 ( ) ( ( ) ( ) ( ) ( ) ( ) ( ) ( ) 1 1 ( ) ( ) ( ) ( ) 1 ) 1 ( | ) ( ) / ( | ) ( ) ( | ) ( | ) / ( | ) ( | ) i i i k k k k N N i i i k k k i k k i i i i k k i k k i k k i k 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 c y      



で表しているので、１つの粒子の　再サンプリング確率は全てである。よっては次式で計算される。　　このに基づいてをする。 ( の分布はの分布となる） ( ) ( )v 条件付期待値はxi kの単純平均

(23)

23

線形カルマンフィルタ

•対象モデル

n

R

x

[

k

]



状態ベクトル

]

[

]

[

]

[

]

[

]

[

]

[

]

[

]

1 [

k

w

x

C

y

v

x

A

x











観測ベクトル

l

R

y

[

k

]



状態外乱ベクトル

n

R

v

[

k

]



観測外乱ベクトル

l

R

w

[

k

]







[

]

0

0 ]

[

]

[

]

[

]

[

k

R

Q

k

E

v

T

w

T

w

v

_





























_

(24)

24

•線形カルマンフィルタ

初期推定値とその予測誤差共分散

x

ˆ

₀_|_₁

_P

₀_|_₁





1 1 | 1 |   





T _k k k k k T k k k k

P

C

P

C

R

W

)

ˆ

(

ˆ

_k

_|

_k



x

_k

_|

_k

_

₁



W

_k

y

_k



C

_k

x

_k

_|

_k

_

₁

x

k k k k k

A

x

ˆ

_₁_|



ˆ

_|

１．カルマンゲインを計算

２．前回の予測推定値を観測値との誤差で修正

４．推定誤差共分散行列を更新

３．予測推定値を計算

1 | 1 | |k



k k



k k k k k

P

W

C

P

k T k k k k k k

A

P

A

Q

P

_₁_|



_|



(25)

25

•証明

[ ] [ 1] [ ] [ ] [ ] [ ] [ ] 0 0 k k k k k k k x x A I v y 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 k T k k k k k k k k k k k x k x k x y k y k C k x x A x v P E x k x k y C x w A k A             _   _     _      _ _  | 1 | 1 | 1 | 1 [ 1] [ 1] [ 1] [ ] [ ] [ 1] [ ] [ ] T T T T k k k k k k k k T T T T k k k k k k k k k k k k k k k

A P

A

Q

A P

C

x

y

E

C P

A

C P

C

R

y

x

y

       







 







_{  }











_











 







両辺の条件無しの期待値をとる。（ｋ－１時刻までの情報による期待値）

|

,

[ ]

i j

P

y j

x i

i|j

ただし、x

はそれぞれ

まで観測された

ときの

の条件付期待値及び条件付分散である

[ 1] [ ] x x k y y k      _    　と考える

(26)

26 | 1 | 1 | 1 | 1 1 1 | 1 | 1 | 1

ˆ ˆ

ˆ

( [ ]

)

ˆ

(

) ( [ ]

))

k k k k k k T k k k k T xy k k k k xy yy T T k 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 T zz k k k k k k k k k k k k k k k T T T k k k k k k k k k k k k k k T k 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

           

 

 















(27)

27

従ってゲインを次のように定義し、





1 1 | 1 |   





P

C

P

C

R

W

)

ˆ

(

ˆ

_k

_|

_k



x

_k

_|

_k

_

₁



W

_k

y

_k



C

_k

x

_k

_|

_k

_

₁

x

k k k k k

A

x

ˆ

_₁_|



ˆ

_|

最適な推定値は次式で与えられる

1 | 1 | |k



k k



k k k k k

P

W

C

P

k T k k k k k k

A

P

A

Q

P

_₁_|



_|



| |

ˆ

_{k k} _{k k}

x

、

P

を次の式で定義すると、

(28)

28

線形カルマンフィルタ（入力あり）

•対象モデル

m

k

R

u



入力ベクトル

ほとんど同様に計算できる





1 1 | 1 |   





P

C

P

C

R

W

)

ˆ

(

ˆ

_k

_|

_k



x

_k

_|

_k

_

₁



W

_k

y

_k



C

_k

x

_k

_|

_k

_

₁

x

1 | 1 | |k



k k



k k k k k

P

W

C

P

k T k k k k k k

A

P

A

Q

P

_₁_|



_|



1 k

k

x

A x

B u

v

y

C x

w











1| |

ˆ

_k _k _k

ˆ

_{k k} _k _k

x

_



A x



B u

_{（条件なし期待値の計算）}

(29)

白色化フィルター（イノベーションプロセス）

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)

30

ARモデルのパラメータ推定（１）

•３次のARモデルのパラメータ推定

k

a

y

a

y

a

y

w

y



₁

_

₁



₂

_

₂



₃

_

₃







T k

a

x



₁ ₂ ₃ k k

x

















1



_k _k _k



_k _k k

y

x

w

y



_₁ _₂ _₃



•状態空間モデル

(31)

31

ARモデルのパラメータ推定（2

）

76 .

2

1 

a

5329

.

2

2 



a

778688

.

0

3 

a

0 .

1 

R

I

P

₀



0 ˆ

₀



x

1

ˆa

2

ˆa

3

ˆa

観測回数

(32)

32

拡張カルマンフィルタ

[

k

1]

(

[ ]

k

,

[ ]

k

)

[ ]

x





f x

u



v k

[ ]

k

(

[ ]

k

)

[ ]

k

y



h x



w

[

k

1]

(

ˆ

[ ]

k

,

[ ]

k

)

[ ]

k

(

[ ]

k

ˆ

[ ]

k

)

[ ]

x





f x

u



A

x



x



v k

]

[

]

[

]

[

]

[

]

[

]

[

k

h

(

x

ˆ

k

)

C

k

(

x

k

x

ˆ

k

)

w

k

y









] [ ˆ

)

(

]

[

k x x

x

f

A

k







] [ ˆ

)

(

]

[

k x x

x

h

C

k







(33)

33

•拡張カルマンフィルタの更新式



(

ˆ

,

)



)

,

ˆ

(

ˆ

[

k

1 ]

f

x

[

k

]

u

[

k

]

W

[

k

]

y

[

k

]

h

x

[

k

]

u

[

k

]

x













1 ]

[

]

[

]

[

]

[

]

[

]

[

]

[

]

[

k



A

k

P

k

C

k

C

k

P

k

C

k



R

k



W

T



[

]

[

]

[

]

[

]



[

]

[

]

[

]

[

]

[

]

1 [

k

A

k

P

k

A

T

k

W

k

C

k

P

k

C

T

k

R

k

W

T

k

P









[

1]

[ ]

ˆ

(

,

)

(

)

[ ]

ˆ

[

1] :

(

ˆ

,

)

ˆ

[

1]

[ ] [ ]

[ ], [ ] :

[ ]

k

x

f x

u

A

x

v k

x k

f x

u

x k

A k x k

v k

x k











 







(34)

（ＵＫＦの考え方も同じ）

34

とが既知

_x

xx

P

y



g

(x

)

とを推定する

y

P

yy : [ 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

h x k

k

w

k

g x k

 







予測出力を用いた非線形カルマンフィルタ（１）

: [ 1] ˆ : [ 1 | ] { [ 1]} ˆ ˆ : {( [ 1] [ 1])( [ 1] [ 1]) }T yy 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





























_





_

の同時分布の考え方で予測出力を用いた場合

　　　　今までは

(35)

35

予測出力を用いた非線形カルマンフィルタ（２）

[

1]

( [ ], , [ ])

x k

 

f x k k v k

[

1]

( [

1], , [

1])

y k

 

h x k



k w k



ˆ

₍

_1|

₁₎

ˆ

₍

_{1| )}

₍

_{1) (}

₁₎

x k



k

 

x k



k



W k



v k



)

1 (

)

|

1 (

)

1 (

)

|

1 (

)

1 |

1 (

k



k





P

k



k



W

k



P

k



k

W

k



P

_vv

T

)

|

1 (

)

|

1 (

)

1 (

k

P

k

P

1 k

k

W





_xy



_vv





システムを逐次線形近似（EKF）

統計モーメントを近似（UKF）

]

|

1 [

]

1 [

]

1 [

k

y

k

y

k

v











(36)

予測出力を用いた拡張カルマンフィルタ

36 36

[

k

1]

(

[ ]

k

,

[ ]

k

)

[ ]

x





f x

u



v k

]

1 [

]

1 [

]

1 [

k





h

(

x

k



)



w

k



y

[

k

1]

(

ˆ

[ ]

k

,

[ ]

k

)

[ ]

k

(

[ ]

k

ˆ

[ ]

k

)

[ ]

x





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

C k

v k

y

h f x

v k

w k

h f x

C k

x

w





 















] [ ˆ

)

(

]

[

k x x

x

f

A

k







ˆ ( [ ], [ ])

[

1]

( )

x f x k u k

k

h x

C

x

_







等価外乱

(37)

37

Unscented Kalman Filter（UKF)

•拡張カルマンフィルタの問題点

•線形近似する際にヤコビアンを計算しなければならない

（不連続なシステム、Hard Nonlinearity）

•推定値にバイアスが乗ることがある

システムを近似するより統計量を近似するほうが容易

数カ所のサンプル点（Sigma Points）を選び、集合平均的に統計量を近似する

とが既知

_x

xx

P

y



g

(x

)

とを推定する

y

P

yy

Unscented Transformation

（Ｕ変換）

•発散することもある

（平均値の変換は変換後の平均値になると仮定している）

(38)

拡張カルマンフィルターの問題点

38

x

y

2

( )

y



f x



x

()

p

x

( )

p x

y

( )

p y

'( )

p y

(39)

39

Sigma Pointsの考え方

n

x



R

を平均値

x

、分散行列



_xx

の確率変数ベクトルとする

x

に対して、

２ｎ＋１個

の代表点



_i

(i=0,1,

,2n)

を考えて、それぞれの

離散点の生起確率を

W

_i

とする。ただし、



_i

の集合的統計的性質は２次の

モーメントまでは一致させる







2 2n i i 0 i=0

,

n T i i i xx i

W

x

W







 



( )

(

)

i i

y

g x

g







1

y

2

y







2 2n i i 0 i=0

ˆ

:

,

:

ˆ

n T i i yy yy i i

y



W



y



y

W



 



   





(40)

40

１．は新の平均値を２次のorderまで近似

（EKFは１次のorderまで近似）

２．はを３次のorderまで近似（これはEKFと同じ）

3. はチューニングパラメータ

がGaussianの場合と選ぶのが良い

Sigma Pointsを用いた推定の性質

ˆy

_y

ˆ

yy



x



3 n

 



yy



(41)

41

UKFの計算手順

３．予測共分散、を計算する

P

_xy

(

k



1 |

k

)

P

_yy

(

k



1 |

k

)

２．Sigma Pointsを基に予測値、を

_x

ˆ

₍

_k



₁

_|

_k

₎

計算する

)

|

1 (

ˆ

k

y



４．カルマンゲインを計算する

_W

₍

_k



₁

₎

6．推定値と共分散を更新する

_x

ˆ

₍

_k



₁

_k



₁

₎

P

(

k



1 k



1 )

１．適切な

サンプル点（Sigma Points）

を推定値と

x

ˆ k

(

k

)

共分散から選ぶ

P

( k

k

)

5．観測値が得られる

_y

₍

_k



₁

₎

(42)

42

UKF:Sigma Pointｓ

κ

n

κ

W





0

]

|

[

]

|

[

k



x

ˆ

k

0 X



n

P

k



_i

k

x

k

|

]

ˆ

[

|

]

(

)

[

|

]

[







i

X

)

(

2

1 





n

W

_i



n

P

k



_i

k

x

k

|

]

ˆ

[

|

]

(

)

[

|

]

[









n i

X

)

(

2

1 







n

W

_i _n

N

M



とすると

T

NN

M



である

番目の列ベクトル

_i



n



i



1 

,

2 ,

,

R



κ

(43)

44

UKF：Sigma Pointsの性質

]

[

ˆ

]

[

2

0 k

k

x

k

W

n

i







i

X

平均

分散







T n i i

k

x

k

x

k

W

P

[

]

ˆ

[

]

[

]

ˆ

[

]

2 0







 i i

X



 



1

2 (

)

(

)

(

)

n _T i i i i

W n



P k k









 



1

(

)

(

)

n _T i i i

P k k





)

( k

k

P



平均、分散は一致している点の集合

(44)

45

補足









1



2 1 2

( | )

(

)

T n n i i

P k k

v

P k k



とすると









1 2 1 2 1 1 1 2 1 2 1 1 1

( | )

,

T T n n T T _n T n j j j T n T n T n n n

v

P k k

v

v v

v

v v

    

















































_

_



















(45)

46

UKF：共分散行列の更新

)

,

(

[

|

]

[

]

|

1 [

k

f

_i

k

u

k

i

X





]

|

1 [

]

|

1 [

2

0 ˆ

k

n

i

W

x









X

_i















n i i

k

W

x

P

2 0

]

|

1 [

]

|

1 [

]

|

1 [

X

_i

ˆ





T

k

1 |

]

x

[

1 |

]

[





ˆ





X

_i

1.Sigma Pointsを状態遷移関数で遷移させる

2.遷移させたSigma Pointsの集合平均で予測平均を近似する

３.遷移させたSigma Pointsの集合分散で予測分散を近似する

(46)

47

)

,

(

[

1 |

]

[

]

|

1 [

k



k



h

_i

k



k

u

k

i

X

Y

４.Sigma Pointsを観測関数で遷移させる

５.遷移させた点の集合平均で予測観測値を近似する

６.遷移させたSigma Pointsの集合分散で予測分散を近似する















n i i yy

k

W

k

y

k

P

2 0

]

|

1 [

]

|

1 [

]

|

1 [

Y

_i

ˆ





T

k

1 |

]

y

[

1 |

]

[





ˆ





Y

_i

]

|

1 [

]

|

1 [

2

0 ˆ

k

n

i

W

y









Y

_i















n i i xy

k

W

k

x

k

P

2 0

]

|

1 [

]

|

1 [

]

|

1 [

X

_i

ˆ





T

k

1 |

]

y

[

1 |

]

[





ˆ





Y

_i

(47)

48

[

1]

[

1]

ˆ

[

1 | ]

[

1 | ]

[

1]

v k





y

k





y

k



k



y

k



k



w k



７.イノベーションの予測共分散を計算する

８.カルマンゲインを計算する

]

|

1 [

]

1 [

]

|

1 [

k

_yy

k

vv

R

P







]

|

1 [

]

|

1 [

]

1 [

k

P

_xy

k

P

_vv

1 k

k

W











９.観測値から推定値を更新する

y

[

k



1 ]

]

1 [

]

1 [

]

|

1 [

]

1 |

1 [

ˆ

k



k





x

k



k



W

k



v

k



x

１０.予測誤差共分散を更新する

]

1 [

]

|

1 [

]

1 [

]

|

1 [

]

1 |

1 [

k



k





P

k



k



W

k



P

_vv

k



k

W

T

k



P