非線形システムを各時刻において，線形化を行い確率密度関数（...

EKF・・・非線形システムを各時刻において，線形化を行い

それぞれの時刻において時変カルマンフィルタを適用する．

UKF・・・非線形システムを各時刻における線形近似ではなく

確率密度関数（確率分布）を正規分布で近似するという統計量の近似に基づく．

第９回確率システム制御特論

7.4 数値シミュレーション例

二つの数値シミュレーション例を用いて，EKFとUKFの性能比較を行う．

𝑥1(𝑡)𝑥2(𝑡)

地球

大気圏

𝑥3(𝑡)

𝑥1(𝑡):高度

𝑥2(𝑡):速度

𝑥3(𝑡):弾道係数



例題7.3

d

d𝑡

𝑥1(𝑡)𝑥2(𝑡)𝑥3(𝑡)

=

𝑥2(𝑡)

0.5𝜌0exp −𝑥1(𝑡)

𝜂𝑥22 𝑡 𝑥3 𝑡 − 𝑔

0

𝜌0 : 海抜高度における空気密度𝜂 : 空気密度と高度の関係を定義する定数𝑔 : 重力加速度𝑇 : サンプリング周期 0.5 s



状態方程式

𝑦 𝑘 = 𝑀2 + 𝑥1 𝑘 − 𝑎 2 + 𝑤(𝑘)

𝑀 : レーダと物体の間の水平距離𝑎 : レーダの高度（三平方の定理より）

𝑤 𝑘 : 平均値0，分散 4 × 103[m2]正規性白色雑音



観測方程式

シミュレーション条件

パラメータ数値[単位]

𝜌0 1.23 kg/m3

𝜂 6 × 103 m

𝑔 9.81 m/s2

𝑀 3 × 104 m

𝑎 3 × 104 m



初期値

𝒙0 = 9 × 104 −6 × 103 3 × 10−3 𝑇

状態推定値の初期値

𝜅 = 0

共分散行列の初期値



𝑃(0) =9 × 103 0 0

0 4 × 105 00 0 0.4

UKFの設定パラメータ

d

d𝑡

𝑥1(𝑡)𝑥2(𝑡)𝑥3(𝑡)

=

𝑥2(𝑡)

0.5𝜌0exp −𝑥1(𝑡)


0

𝑥1(𝑘 + 1)𝑥2(𝑘 + 1)𝑥3(𝑘 + 1)

=

𝑥1 𝑘 + 𝑇𝑥2(𝑡)

𝑥2(𝑘) + 𝑇 0.5𝜌0exp −𝑥1(𝑡)


𝑥3(𝑘)

オイラー法により周期 𝑇 で離散化



状態方程式

𝑥 𝑘 + 1 = 𝑓 𝑥 𝑘 + 𝑣 𝑘

= 0.2𝑥 𝑘 +25𝑥 𝑘

1 + 𝑥2(𝑘)+ 8 cos 1.2𝑘 + 𝑣 𝑘

状態方程式

𝑣 𝑘 ,𝑤 𝑘 :互いに独立な正規性白色雑音それらの平均値は0である．



例題7.4

𝑦 𝑘 = ℎ 𝑥 𝑘 + 𝑤 𝑘 =1

20𝑥2 𝑘 + 𝑤(𝑘)

𝜎𝑣2 = 1, 𝜎𝑤

2 = 3とする．


A.1 連続時間モデルから離散時間モデルへの変換

連続時間モデル ⇒ 離散時間関数

𝑥 𝑘 + 1 − 𝑥 𝑘

ℎ= 𝑓𝑐 (𝑡, 𝑥(𝑡))

𝑥 𝑡 + ℎ = 𝑓d(𝑡, 𝑥(𝑡))d

d𝑡𝑥 𝑡 = 𝑓𝑐 (𝑡, 𝑥(𝑡))

𝑥 𝑘 + 1 − 𝑥 𝑘 = ℎ ∙ 𝑓𝑐 (𝑡, 𝑥(𝑡))

𝑥 𝑘 + 1 = 𝑥 𝑘 + ℎ ∙ 𝑓𝑐 (𝑡, 𝑥(𝑡))

𝑘2 = 𝑓𝑐 𝑡 +ℎ

2, 𝑥 𝑡 +

ℎ

2∙ 𝑘1

𝑘1 = 𝑓𝑐 (𝑡, 𝑥(𝑡)) 𝑘3 = 𝑓𝑐 𝑡 +ℎ

2, 𝑥 𝑡 +

ℎ

2∙ 𝑘2

𝑘4 = 𝑓𝑐 (𝑡 + ℎ, 𝑥(𝑡) ∙ 𝑘3)

𝑥 𝑘 + 1 = 𝑥 𝑘 + ℎ/6 ∙ (𝑘1 + 2𝑘2 + 2𝑘3 + 𝑘4)

オイラー法

ルンゲクッタ法

線形状態方程式

𝒙 𝑘 + 1 = 𝑨 𝜽 𝒙 𝑘 + 𝒃𝑣 𝒌 (7.68)

𝑦 𝑘 = 𝒄𝑇 𝜽 𝒙 𝑘 + 𝑤(𝑘) (7.69)

𝑨，𝒄：未知

未知パラメータ（一定値を仮定）

𝜽 𝑘 + 1 = 𝜽(𝑘) (7.70)

𝒛 𝑘 =𝒙(𝑘)𝜽(𝑘)

新しい状態変数を導入

(7.71)

7.5 状態と未知パラメータの同時推定


𝒇 𝒛 𝑘 =𝑨 𝜽 𝟎𝟎 𝑰

𝒛(𝑘)

𝒈 𝒛 𝑘 =𝒃𝟎

ℎ 𝒛 𝑘 = 𝒄𝑇 𝜽 𝒙(𝑘)

𝒛 𝑘 + 1 = 𝒇 𝒛 𝑘 + 𝒈 𝒛 𝑘 𝑣(𝑘)

𝑦 𝑘 = ℎ 𝒛 𝑘 + 𝑤(𝑘)

拡大系を構成してEKFやUKFを適用する．

ただし

(7.72)

(7.73)

(7.76)

(7.75)

(7.74)

未知パラメータや状態の初期値を真値の近くに選ぶ必要がある

※推定値の大域的な収束性が保証されていない



例題7.5

状態方程式

d

d𝑡


=0 1

−𝐾

𝑀−𝐶

𝑀


+01

𝑀

𝑢(𝑡)

𝑦 𝑡 = 1 0𝑥1(𝑡)𝑥2(𝑡)

(7.77)

(7.78)

質量：𝑀 = 2 （既知）バネ定数：𝐾 = 0.7 （既知）粘性係数：𝐶 = 1 （未知）離散化周期：𝑇 = 0.01𝑠観測雑音𝑤(𝑘)：分散：0.1，平均値0

二つの状態と未知である粘性抵抗𝐶を推定



d

d𝑡


=0 1

−𝐾

𝑀−𝐶(𝑘)

𝑀


+01

𝑀

𝑢(𝑡) 𝑦 𝑡 = 1 0𝑥1(𝑡)𝑥2(𝑡)



𝐶 𝑘 + 1 = 𝐶(𝑘)

𝑥1(𝑘 + 1)𝑥2(𝑘 + 1)

=1 𝑇

−𝐾

𝑀𝑇 1 −

𝐶(𝑘)

𝑀𝑇

𝑥1(𝑘)𝑥2(𝑘)

+01

𝑀𝑇

𝑢(𝑘)

𝑦 𝑘 = 1 0𝑥1(𝑘)𝑥2(𝑘)

Dオイラー法による離散化

拡大系の構成

※問題ではルンゲクッタ法による離散化を指定しているので皆さんやってみよう！



𝑥1(𝑘 + 1)𝑥2(𝑘 + 1)

𝐶 𝑘 + 1=

1 𝑇 0

−𝐾

𝑀𝑇 1 −

𝐶(𝑘)

𝑀𝑇 0

0 0 1


𝐶 𝑘+

01

𝑀𝑇

0

𝑣(𝑘)

𝑦 𝑘 = 1 0 0


𝐶 𝑘

𝒛 𝑘 + 1 = 𝒇 𝒛 𝑘 + 𝒈 𝒛 𝑘 𝑣(𝑘)

𝑦 𝑘 = ℎ 𝒛 𝑘

統合慣性航法システム

8.1 相補フィルタ


Integrated inertial navigation system

慣性航法システム（INS: inertial Navigation System）とGPSのような他のセンサからの航法データとを統合するシステム．カルマンフィルタを用いてセンサフュージョン（sensor fusion）を行う手法が提案されている．

衛星航法システムとは、衛星航法のシステムを指す。衛星航法とは，複数の航法衛星（人工衛星の一種）が航法信号を地上の不特定多数に向けて電波送信（放送）し，それを受信する受信機を用いる方式の航法（自己の位置や進路を知る仕組み・方法）を指す．

日本では「衛星測位」及び衛星測位システムと呼ぶことが多い．2011年4月からは国土地理院では全地球型のシステム（全地球航法衛星システム）を，GNSS（Grovel Navigation

satellite systems）と呼称することになった．なお，よく誤解されるが，GPSはあくまでも衛星測位システムの中の1つであり，衛星測位システムそのものを指すものではない．

システム国信号方式軌道遠地点と近地点

衛星数周波数状態

GPS アメリカ CDMA20,200 km, 12.0h

≥ 24機

1.57542 GHz（L1信号）1.2276 GHz（L2信号）

運用中

GLONASS ロシア FDMA/CDMA19,100 km, 11.3h

24機（CDMA対

応機を打ち上げた場合は30機）

約 1.602 GHz (SP)約 1.246 GHz (SP)

再構築後運用中 CDMAは準備中

Galileo ヨーロッパ共同体

CDMA23,222 km, 14.1h

2機の試験機が周回中22機の衛星の

運用予算が認可

1.164-1.215 GHz (E5a and E5b)1.215-1.300 GHz (E6)1.559-1.592 GHz (E2-L1-E11)

準備中

Compass 中国 CDMA21,150 km, 12.6h

35機[8]

B1: 1,561098 GHzB1-2: 1.589742 GHzB2: 1.207.14 GHzB3: 1.26852 GHz

15機運用中、20機追加予定

http://ja.wikipedia.org/wiki/CDMA

http://ja.wikipedia.org/wiki/FDMA




http://ja.wikipedia.org/wiki/%E8%A1%9B%E6%98%9F%E6%B8%AC%E4%BD%8D%E3%82%B7%E3%82%B9%E3%83%86%E3%83%A0#cite_note-9

𝐺1(𝑠)

𝐺2(𝑠)

𝑠 𝑡 + 𝑛1(𝑡)

𝑠 𝑡 + 𝑛2(𝑡)

𝑥(𝑡) ≈ 𝑠(𝑡)+

+

𝑠(𝑡)：信号𝑛1(𝑡)，𝑛2 𝑡 :雑音

𝑥(𝑡)：フィルタリングによる推定値

𝑋 𝑠 = 1 − 𝐺(𝑠) 𝑆 𝑠 + 𝑁1(𝑠) + 𝐺(𝑠) 𝑆 𝑠 + 𝑁2(𝑠)

= 𝑆 𝑠 + 1 − 𝐺 𝑠 𝑁1 𝑠 + 𝐺(𝑠)𝑁2(𝑠)


1 − 𝐺(𝑠)

𝐺(𝑠)


𝐺(𝑠)𝑠 𝑡 + 𝑛2(𝑡)

𝑠 𝑡 + 𝑛1(𝑡) 𝑥(𝑡) ≈ 𝑠(𝑡)+

−

−

+

𝑛1(𝑡) − 𝑛2(𝑡)

𝑛1 𝑡 になってほしいなぁ

相補フィルタの差分・フィードフォーワード構成



𝑋 𝑠 = 1 − 𝐺(𝑠) 𝑆 𝑠 + 𝑁1(𝑠) + 𝐺(𝑠) 𝑆 𝑠 + 𝑁2(𝑠)

= 𝑆 𝑠 + 1 − 𝐺 𝑠 𝑁1 𝑠 + 𝐺(𝑠)𝑁2(𝑠)

+

+

1

𝑇𝑠 + 1

𝑇

タコメータ信号

加速度計信号

出力速度信号

𝑇 (時定数)のみを設計すればよい

𝐺 𝑠 =1

𝑇𝑠 + 1

1

𝑠 𝜃 + 𝑛2

𝜃

+

+

1 − 𝐺 𝑠 =𝑇𝑠

𝑇𝑠 + 1 𝜃 + 𝑛2′

フィルタリングされた速度信号

HPF

LPF

𝜃 + 𝑛1

タコメータ信号

加速度計信号

𝑇

𝑇𝑠 + 1




慣性航法システムINS : Internal navigation system


ジャイロスコープ

加速度センサ

積分

積分

角速度姿勢

加速度速度積分位置

積分により姿勢や位置の誤差は不安定になりドリフト誤差をもつ．

補助センサ（GPSやドップラーレーダなど）

ℎ 𝒙∗ = 真の位置，速度など + 𝑛1

𝑛1:慣性系誤差

𝑧 𝑘 = 真の位置，速度など + 𝑛2

𝑛2: 補助センサ誤差

ℎ 𝒙∗ − 𝑧 𝑘 = 𝑛1 − 𝑛2

計測の差をとると，雑音成分のみが残る．

𝑛1 − 𝑛2 カルマンフィルタ

𝑛1

ℎ 𝒙∗ − 𝑛1 = 真の位置，速度などの推定

ねらい


INS（慣性航法システム）

補助センサ（GPS） −

+カルマンフィルタ

ℎ 𝒙∗

𝑧 𝑘 ℎ 𝒙∗ − 𝑧 𝑘

（真の位置・速度）＋（慣性系誤差）

（真の位置・速度）＋（補助源誤差）

（慣性系誤差）－（補助源誤差）（慣性系誤差推定値）

+

−

推定された慣性出力

差分演算によって非線形性を相殺できるので線形カルマンフィルタを利用できる．

慣性系誤差を記述する状態空間モデルが必要．


色々な補助センサを利用できる．

相補フィルタは早い即応性能を実現し瞬時に雑音を除去できる．

非線形システムを各時刻において，線形化を行い 確率密度関数（...

Documents

非線形システムを各時刻において，線形化を行い確率密度関数（...