inertial navigation c9-01inertial-navigator.net/wp-content/uploads/2019/10/... · 9...

Inertial Navigation C9-01

© 2018 Futoshi Magosaki 9-1

9 慣性航法装置のカルマンフィルタ

第 7 章の離散過程の拡張カルマンフィルタのアルゴリズムに，第 8 章の慣性航法の運動

方程式を適用する．慣性センサのノイズは最も簡単なホワイトノイズであるとする．

9.1 システム方程式

システム方程式は式 (7.41) より，

( )kkkkkkk N Q0wwΓxΦx ,~,1111 −−−− +=

である．姿勢誤差ベクトル，速度誤差ベクトル，位置誤差ベクトルの 3 個のベクトルを上

から縦に並べたベクトルを状態変数とする．姿勢誤差としてクォータニオン縮減誤差を使

う場合のシステム方程式は第 8 章の式 (8.85) である．

−

+

=

b

bnb

nb

nn

f

ω

00

C0

0C

r

v

q

FF0

FFF

FFF

r

v

q

2

1

22

1

2

1

3332

232221

131211

(9.1)

拡張カルマンフィルタのシステム方程式 (7.41) と変数を対応させると，

11 12 13

21 22 23

32 33

11 1

22 2

, 2 , ,

nbn

bnb b

t t

−

= = + = =

C 0F F Fq

ωx v Φ I F F F Γ 0 C w

fr 0 F F 0 0

(9.2)

となる．状態変数 xは 9 行ベクトル，推移行列Φは 9 行 9 列の行列である．システムノイ

ズ wは 9 行 6 列の行列と 6 行ベクトルの積であるから 9 行ベクトルである．

9.2 時間更新

時間更新は式 (7.43) と式 (7.44) を使って計算できる．状態変数 xの初期値，共分散行

列 Pの初期値は既知とする．

9.2.1 姿勢の時間更新

角速度センサの出力から地球の自転角速度成分を引いて，航法座標系に対する運動角速


9-2 © 2018 Futoshi Magosaki

度を求める．

nin

bn

bib

bin

bib

bnb ωCωωωω −=−= (9.3)

ここに， bnC は航法座標系から機体座標系への方向余弦行列であり，

=

332313

322212

312111

ccc

ccc

cccnbC (9.4)

の転置行列である． ninω は航法座標系から見た地球の自転角速度であり，式 (4.39) よ

り，

=

−

=

D

N

nin 0

sin

0

cos

ω (9.5)

である．式 (9.3) を行列で表すと，

−

=

D

N

bz

by

bx

bz

by

bx

ccc

ccc

ccc

0

332313

322212

312111

(9.6)

となる．この式で求めた運動角速度を使って，クォータニオンを時間更新する．クォータ

ニオンの時間微分の式 (5.86) を使って時間積分すると，

( ) ( ) ( )

0 0 0

1 1 1

2 2 2

3 3 31 1

0

01

2 0

0

b b bx y z

b b bx z y

b b by z x

b b bk k k

z y x

q q q

q q q

q q q

q q q

− − + − +

− − − − = − − −

(9.7)

となる．ここに，Δt は時間更新の時間間隔である．

このような時間積分を数多く繰り返していくと，クォータニオンのノルムは 1 であると

いう関係がやがて崩れてくる．そこで適当なタイミングで，クォータニオンをノルムで割

って正規化する．



20

23

22

21 qqqq

q

q

qqnormalized

+++== (9.8)

正規化の別の方法を次に記す．ノルムの 1 からのずれを Δq と定義する．

( )2 2 2 21 2 3 01q q q q q = − + + + (9.9)

この式を式 (9.8) に代入すると，

( ) qqq

qqnormalized 2

1

11

−−=

−= (9.10)

となる．したがって，Δq が小さければ，

qqqnormalized

+=

2

11 (9.11)

と近似できる．式 (9.8)，式 (9.11) のいずれの式を使ってもよい．

Note

クォータニオンを q1, q2, q3, q0の順に並べる流儀の場合，式(9.7) は次式のようになる．

( ) ( ) ( )

1 1 1

2 2 2

3 3 3

0 0 01 1

0

0

2 0

0

b b bz y x

b b bz x y

b b by x z

b b bk k k

x y z

q q q

q q qt

q q q

q q q

− − + − +

− − − − = − − −

9.2.2 速度の時間更新

機体に固定された加速度センサの出力を，方向余弦行列を使って航法座標系における加

速度に変換する．



k

bz

by

bx

kk

nz

ny

nx

nnb

n

f

f

f

ccc

ccc

ccc

f

f

f

C

=

=

333231

232221

131211

,ff (9.12)

求めた加速度から地球の重力加速度成分を引いて運動加速度を求める．

−

−

=

−=

gf

f

f

a

a

a

k

nz

ny

nx

k

nz

ny

nx

nnn 0

0

,gfa (9.13)

求めた運動加速度を使って，速度を時間積分する．

k

nz

ny

nx

k

nz

ny

nx

k

nz

ny

nx

a

a

a

t

v

v

v

v

v

v

+

=

−1

(9.14)

9.2.3 位置の時間更新

位置を表す緯度，経度，高度の時間微分は，式 (8.40) より，

( )

k

D

E

N

v

hR

vhR

v

h

−

+

+

=

cos

である．この式を時間積分すると，

( )

k

D

E

E

N

N

kk v

hR

v

hR

v

t

hh

−

+

+

+

=

−

cos

1

(9.15)

となる．（問題 9.1 参照）



9.2.4 共分散行列の時間更新

共分散行列 Pの時間更新は，式 (7.44) より，

( ) ( )T

111T

111 −−−−+−−− += kkkkkkk ΓQΓΦPΦP

である．ここで，x, y, z 軸の角速度センサのノイズの分散が同じ δω2で，互いに相関が無い

ものとし，x, y, z 軸の加速度センサのノイズの分散が同じ δf 2で，互いに相関が無いものと

すると，システムノイズ行列は，

2

T 2 2

1

4

k k k t f

=

I 0 0

Γ Q Γ 0 I 0

0 0 0

(9.16)

となる．

9.3 観測方程式

観測方程式は式 (7.42) より，

( )kkkkkk N R0vvxHz ,~,+=

である．慣性航法装置における観測の代表として，位置の観測，速度の観測，姿勢の観測

のそれぞれの観測方程式を以下に示す．

9.3.1 位置の観測

移動体に搭載された衛星測位システムが，移動体の位置（緯度，経度，高度）を観測す

る場合の観測方程式を求める．観測誤差 δzは，衛星測位システムによる位置の観測値 nr~

から真値 rnを引いた差であり，



( )

n n

n n nr

nr

= −

= + + −

= +

z r r

r r v r

r v

(9.17)

となる．ここに，δrnは位置の誤差，vr は位置の観測のホワイトノイズである．したがっ

て，観測方程式は，

r

k

n

n

n

kk

v

r

v

q

I00

vxHz

+

=

+=

(9.18)

となる．

9.3.2 速度の観測

移動体に固定された速度計が，機体座標系における対地速度 vbを観測する場合の観測方

程式を求める．観測誤差 δzは，速度の観測値 bv~ から真値 vb を引いた差であり，

( )

( )( )

b b

b n bn v

b n n bn v

= −

= + −

= + + −

z v v

C v v v

C v v v v

(9.19)

となる．ここに， bnC

~は誤差を含む方向余弦行列，δvnは速度の誤差，vvは速度の観測のホ

ワイトノイズである．ここで，誤差を含む方向余弦行列 nbC

~は，式 (8.59) より，

( )( ) nb

nb CφIC −=

~ (9.20)

と表されるから，誤差を含む方向余弦行列 bnC

~は両辺を転置して，

( )( ) ( )( )+=−= φICφICCbn

nb

bn

TT~ (9.21)

となる．ここに，外積行列 (φ×) は歪対称行列であることを使った．この式を式 (9.19)



に代入すると，

( )( ) ( )( )

( )

b n n bn v

b n b n b n bn n n v

b n b nn n v

= + + + −

= + + + −

= − + +

z C I φ v v v v

C v C v C φ v v v

C v φ C v v

(9.22)

となる．ここに，第 1 行から第 2 行への変形において 2 次の項を無視した．第 2 行から第

3 行への変形において外積の順序を変えて符号を変えた．ここで，姿勢誤差の代わりに式

(5.107) のクォータニオン縮減誤差を使うと，観測方程式は，

( )

( )

2

2

k k

b n n b nn n v

n

b n b nn n v

n

= +

= − + +

= − +

z H x v

C v q C v v

q

C v C 0 v v

r

(9.23)

となる．この観測行列 Hの左側の部分行列 ( )2b nn − C v は，

−

−

−

=

022

202

220

332313

322212

312111

333231

232221

131211

NE

ND

ED

vv

vv

vv

ccc

ccc

ccc

hhh

hhh

hhh

(9.24)

である．

9.3.3 方位の観測

移動体に固定された加速度センサで重力加速度を計測することによって，水平面に対す

る傾きの角度，すなわちロール角とピッチ角は観測できるが，方位角は観測できない．方

位角を観測するためには，何らかの既知の方位ベクトルを観測する必要がある．高精度な

角速度センサがあれば，地球の自転角速度を観測して方位角（真北）を観測できる．磁気

センサがあれば，地磁気を観測して方位角（磁北）を観測できる．天体望遠鏡があれば，

恒星（例えば北極星）からの光を観測して方位角（真北）を観測できる．

機体座標系における方位ベクトル z b を観測する場合の観測方程式を求める．観測誤差

δzは，方位ベクトルの観測値 bz~ から真値 z b を引いた差であり，



( )

b b

b n bn z

= −

= + −

z z z

C z v z

(9.25)

となる．ここに， bnC は誤差を含む方向余弦行列，δzn は方位ベクトルの誤差，vzは方位ベ

クトルの観測のホワイトノイズである．式 (9.21) を代入すると，

( )( )

( )

( )

( )

b n bn z

b n b n bn n z

b nn z

b nn z

= + + −

= + + −

= +

= − +

z C I φ z v z

C z C φ z v z

C φ z v

C z φ v

(9.26)

となる．ここに，第 3 行から第 4 行への変形において外積の順序を変えて符号を変えた．

ここで，姿勢誤差の代わりにクォータニオン縮減誤差を使うと，式 (5.107) より，

nq2=φ であるから，観測方程式は，

( )

( )

2

2

k k

b n nn z

n

b n nn v

n

= +

= − +

= − +

z H x v

C z q v

q

C z 0 0 v v

r

(9.27)

となる．

角速度センサで地球の自転角速度を観測する場合，航法座標系における地球の自転角速

度ベクトルは，式 (4.39) より，

=

−

=

D

N

n 0

sin

0

cos

Ω

であるから，観測行列 Hの左側の部分行列 ( )2b nn − C z は，



−

−

=

020

202

020

332313

322212

312111

333231

232221

131211

N

ND

D

ccc

ccc

ccc

hhh

hhh

hhh

(9.28)

となる．

磁気センサで地磁気を観測する場合，航法座標系における地磁気ベクトルは，式 (5.119)

より，

=

=

IF

DIF

DIF

m

m

m

D

E

N

n

sin

sincos

coscos

m

であるから，観測行列 Hの左側の部分行列 ( )2b nn − C z は，

−

−

−

=

022

202

220

332313

322212

312111

333231

232221

131211

NE

ND

ED

mm

mm

mm

ccc

ccc

ccc

hhh

hhh

hhh

(9.29)

となる．

9.4 観測更新

カルマンゲインKkを式 (7.45) を使って計算する．

( ) ( )( )1

T Tk k k k kk k

−

− −= +K P H H P H R

ここに，9.3 節の観測方程式の場合，観測行列 Hk は 3 行 9 列の行列であるから， ( )T

kk HP −

は 9 行 3 列の行列となり， ( )T

kkk HPH − は 3 行 3 列の行列となる．観測ノイズ行列 Rk も 3

行 3 列の行列であるから， ( ) kkkk RHPH +−T

とその逆行列も 3 行 3 列の行列となる．した

がって，カルマンゲインKkは 9 行 3 列の行列となる．ここに，観測ノイズ行列 Rは，x, y,

z 軸の観測量に相関が無ければ対角行列となる．



=2

2

2

00

00

00

z

y

x

k

v

v

v

R (9.30)

状態変数の観測更新量 δxを式 (7.46) を使って計算する．9.3 節の観測方程式の場合，カ

ルマンゲイン Kk は 9 行 3 列の行列，観測誤差 δzkは 3 行 1 列のベクトルであるから，観測

更新量 δxは 9 行 1 列のベクトルとなる．

( ) ( )( )( )

1 11 12 13

21 22 232

3

1 1

2 2

3 3

81 82 83

91 92 93

ˆ

ˆ

ˆ

k kk k

n

n

n

N

E

D

q k k k

k k kq

q

z zv

z zv

z zv

k k k

k k kh

+ −= −

− = − −

x K z H x

(9.31)

観測更新量 δxの上側の 3 個は，クォータニオン縮減誤差の観測更新量であり，式

(5.106) より，

( ) ( ) ( )

0 0 1 2 31

1 1 0 3 22

2 2 3 0 1

33 3 2 1 0

n

n

n

k k k

q q q q qq

q q q q qq

q q q q qq

q q q q q

+ − −

− − = + − − − −

(9.32)

として，クォータニオンを観測更新する．次に式 (5.51) により方向余弦行列を更新する．

( ) ( )( ) ( )( ) ( )

+−−+−

−−+−+

+−−−+

=2

32

22

12

010322031

10322

32

22

12

03021

203130212

32

22

12

0

22

22

22

qqqqqqqqqqqq

qqqqqqqqqqqq

qqqqqqqqqqqqnbC



オイラー角を出力するときは，式 (5.6) により方向余弦行列からオイラー角を計算する．

11

21131

33

321 tan,sin,tanc

cc

c

c −− =−==

観測更新量 δxの中央の 3 個は，速度誤差の観測更新量であり，

( ) ( )

+

=

−+ D

E

N

kD

E

N

kD

E

N

v

v

v

v

v

v

v

v

v

(9.33)

として，速度を観測更新する．

観測更新量 δxの下側の 3 個は，位置誤差の観測更新量であり，

( ) ( )

+

=

−+hhh

kk

(9.34)

として，位置を観測更新する．

共分散行列 Pkは式 (7.47) を使って観測更新する．

( ) ( ) ( )−+ −= kkkk PHKIP

Note

クォータニオンを q1, q2, q3, q0の順に並べる流儀の場合，式(9.32) は次式のようになる．

( ) ( ) ( )

1 1 0 3 21

2 2 3 0 12

3 3 2 1 0

30 0 1 2 3

n

n

n

k k k

q q q q qq

q q q q qq

q q q q qq

q q q q q

+ − −

− − − − = + − −

9.5 例題

無人航空機の飛行シミュレーションを行う．

(1) 飛行経路とスケジュール

飛行経路を図 9.1 に，飛行スケジュールを表 9.1 に示す．静止状態から鉛直上方に上昇



した後，南北方向に細長い周回コースを一周した後，鉛直下方に下降して元の位置に戻

る．

図 9.1 飛行経路

表 9.1 飛行スケジュール

No 飛行状態時間 (s) 飛行距離 (m) 説明

1 静止 5 0 地上に静止（水平，北向き）

2 上昇 10 50 上方に加速・減速して空中に静止

3 加速 10 50 機首方向に加速して，速度 10 m/s に

4 旋回 10 100 速度 10 m/s で右回りに半周

5 直進 10 100 速度 10 m/s で距離 100 m を直進

6 旋回 10 100 速度 10 m/s で右回りに半周

7 減速 10 50 機首方向に減速して，空中に静止

8 下降 10 50 下方に加速・減速して地上に戻る

(2) センサデータの生成

飛行経路と飛行スケジュールから生成した加速度センサと角速度センサのシミュレーシ

ョンデータを図 9.2 と図 9.3 に示す．加速度センサのデータは理論値に，平均値が 0，標準

偏差が 0.01 m/s2 (1.0 mg) のホワイトノイズを加えている．角速度センサのデータは理論値

に，平均値が 0，標準偏差が 0.002 rad/s (0.11 deg/s) のホワイトノイズを加えている．時間間

隔はともに 0.1 s（サイクル 10 Hz）とした．



図 9.2 加速度センサのシミュレーションデータ

図 9.3 角速度センサのシミュレーションデータ

(3) 推移行列と可観測性

カルマンフィルタの推移行列Φは，式 (9.2) より，

11 12 13

21 22

32

1 1

2 2

2 t

= +

F F F

Φ I F F 0

0 F 0



である．ここに，部分行列（係数行列）Fijは式 (8.82)～(8.84) である．ここで，例題の飛

行速度は低速であるから移動角速度 (v/R) の項を無視し，飛行高度 h は低空であるから地

球の曲率半径 R に対して無視すると，

−

=

−

−

=

−

−

−

=

−

−

=

−

−=

−

−

=

100

0cos

10

001

0cos20

cos20sin2

0sin20

,

0

0

0

00cos

000

00sin

,

0tan

0

001

01

0

,

0cos0

cos0sin

0sin0

32

2221

131211

R

R

ff

ff

ff

R

R

R

nN

nE

nN

nD

nE

nD

F

FF

FFF

(9.35)

となる．位置の観測による可観測性行列 Uを求めると，

=

−

=

−

=

−=

=

=

0cos20

000

000

,

00cos4

000

000

2

100

0cos

10

001

,

000

00cos

2

02

0

2

2

2

2232212232

322132

212232

22322132

32

4

3

2

FFFFF

FFF

000

00FFF

0FFFF

0F0

I00

HF

HF

HF

HF

H

U

g

R

R

R

gR

g

(9.36)

となる．ここに，無人機は等速直線運動を行っているものとした．（問題 9.2 参照）

この可観測性行列 Uは第 3 列がすべて 0 であることより，行列のランクは 8 であり，9

個の状態変数すべてを観測することはできない．第 3 列がすべて 0 であることより，3 番

目の状態変数である方位角が観測できないことがわかる．無人機が加速度運動を行う場合



には，可観測性行列 Uのランクは 9 となり，9 個の状態変数すべてを観測できる．

(4) 慣性航法

カルマンフィルタの時間更新のみを行って推定した飛行経路を図 9.4 と図 9.5 に示す．

図 9.4 は上側から見た上面図，図 9.5 は南側から見た側面図である．

図 9.4 慣性航法の推定飛行経路（上面図）

図 9.5 慣性航法の推定飛行経路（側面図）



ここに，4 本の曲線は，加速度センサ 3 個と角速度センサ 3 個のセンサデータの計 6 組の

ホワイトノイズデータセットを 4 回変更して計算した結果である．4 個の終点の位置誤差

は，25 m から 130 m の範囲に分布した．一方，共分散行列 Pが予測する位置誤差を図 9.6

に示す．位置を示す緯度，経度，高度のうち，緯度と経度は角度であるから，距離に換算

している．緯度の誤差を示す p771/2 に南北方向の曲率半径 RNをかけて RN p77

1/2 とした．経

度の誤差を示す p881/2 に図 4.7 に示す地球の回転軸からの距離 RE cosλ をかけて RE cosλ p88

1/2

とした．位置誤差は時間とともに増加している．終点における位置誤差は直線距離で 99 m

であった．

図 9.6 共分散行列 Pの時間変化

(5) 複合航法

衛星測位システムによる位置の観測を利用した複合航法のシミュレーションを行う．位

置観測のシミュレーションデータを図 9.7 と図 9.8 に示す．図 9.7 は上側から見た上面図，

図 9.8 は南側から見た側面図である．観測の時間間隔は 1 s（サイクル 1 Hz）とした．



図 9.7 衛星測位システムのシミュレーションデータ（上面図）

図 9.8 衛星測位システムのシミュレーションデータ（側面図）

このような位置観測データを使って，カルマンフィルタの時間更新と観測更新を行って

推定した飛行経路を図 9.9 と図 9.10 に示す．図 9.9 は上側から見た上面図，図 9.10 は南側

から見た側面図である．推定飛行経路の曲線が鋸歯状になっているのは，時間更新と観測更



新のサイクルが異なるためである．

図 9.9 複合航法の推定飛行経路（上面図）

図 9.10 複合航法の推定飛行経路（側面図）

ここに，4 本の曲線は，加速度センサ 3 個と角速度センサ 3 個のセンサデータの 6 組と，

衛星測位システムの位置データ 3 個（緯度・経度・高度）の計 9 組のホワイトノイズデー



タセットを 4 回変更して計算した結果である．4 個の終点の位置誤差は，3.5 m から 11.1 m

の範囲に分布した．一方，共分散行列 Pが予測する位置誤差を図 9.11 に示す．縦軸の単位

を距離 (m) とするため，緯度誤差には RNをかけ，経度誤差には RE cosλをかけて表示し

た．終点における位置誤差は直線距離で約 4 m であった．

図 9.11 共分散行列 Pの時間変化

問題

問題 9.1 システムノイズ行列の式 (9.16) を証明せよ．

問題 9.2 可観測性行列の式 (9.36) を導け．

inertial navigation c9-01inertial-navigator.net/wp-content/uploads/2019/10/... · 9...

Documents