特許 6178776 速度計測装置

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】6178776

(24)【登録日】2017年7月21日

(45)【発行日】2017年8月9日

(54)【発明の名称】速度計測装置

(51)【国際特許分類】

   G01P 21/02 20060101AFI20170731BHJP

   G01C 21/28 20060101ALI20170731BHJP

   G01S 19/52 20100101ALI20170731BHJP

【ＦＩ】

   G01P21/02

   G01C21/28

   G01S19/52

【請求項の数】7

【全頁数】19

(21)【出願番号】特願2014-221142(P2014-221142)

(22)【出願日】2014年10月30日

(65)【公開番号】特開2016-90247(P2016-90247A)

(43)【公開日】2016年5月23日

【審査請求日】2016年2月19日

(73)【特許権者】

【識別番号】000145806

【氏名又は名称】株式会社小野測器

(74)【代理人】

【識別番号】100094330

【弁理士】

【氏名又は名称】山田 正紀

(74)【代理人】

【識別番号】100109689

【弁理士】

【氏名又は名称】三上 結

(72)【発明者】

【氏名】市川 和宏

【審査官】 山下 雅人

(56)【参考文献】

【文献】 特開２００４−２７１３３９（ＪＰ，Ａ）

【文献】 特開平１０−３０７０３２（ＪＰ，Ａ）

【文献】 特開平０９−０４９８５２（ＪＰ，Ａ）

【文献】 特開２００５−１６４５９０（ＪＰ，Ａ）

【文献】 特開２００６−２２７０１９（ＪＰ，Ａ）

【文献】 特開２０１２−１７３１９０（ＪＰ，Ａ）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｇ０１Ｐ ２１／０２

Ｇ０１Ｃ ２１／１０−２１／３６

Ｇ０１Ｓ １９／００−１９／５５

(57)【特許請求の範囲】

【請求項1】

ＧＰＳ受信機由来のＧＰＳ速度に基づいて補正されたＩＭＵ由来の車速を表わす１次ＩＭＵ速度に基づいて、車速を表わすＣＡＮ速度のゲインを補正するゲイン補正部と、
前記１次ＩＭＵ速度と前記ＣＡＮ速度との間の相関演算により、該１次ＩＭＵ速度に対する該ＣＡＮ速度の、該１次ＩＭＵ速度と該ＣＡＮ速度との間の誤差が最小となる遅れ時間を推定する遅れ推定部と、
ＩＭＵ由来の速度を補正することにより２次ＩＭＵ速度を生成する速度補正部であって、
前記２次ＩＭＵ速度を、前記遅れ推定部で推定された前記ＣＡＮ速度の遅れ時間だけ遅延させることで前記ＣＡＮ速度と時間的に同期した遅延２次ＩＭＵ速度を生成する同期化処理部、
前記ゲイン補正部で補正されたＣＡＮ速度と前記同期化処理部で生成された遅延２次ＩＭＵ速度との間の差分を生成する差分器、
前記差分器で生成された差分を入力し、運動方程式に基づいて、ＩＭＵ由来の現時点の速度を補正するカルマンゲインを生成するカルマンゲイン算出部、および
前記カルマンゲイン算出部で算出されたカルマンゲインを使って、ＩＭＵ由来の現時点の速度を補正することで前記２次ＩＭＵ速度を生成する補正速度生成部を有する速度補正部とを備えたことを特徴とする速度計測装置。

【請求項2】

前記ゲイン補正部が、所定の判定基準に基づいて判定された、前記ＧＰＳ速度に基づく補正がなされた前記１次ＩＭＵ速度が入力されているタイミングであること、および、定速走行中であることを条件として、前記ＣＡＮ速度のゲインを補正することを特徴とする請求項１記載の速度計測装置。

【請求項3】

前記遅れ推定部が、所定の判定基準に基づいて判定された、前記ＧＰＳ速度に基づく補正がなされた前記１次ＩＭＵ速度が入力されているタイミングであること、および、車速が変化しているタイミングであることを条件として、前記ＣＡＮ速度の遅れ時間を推定するものであることを特徴とする請求項１又は２記載の速度計測装置。

【請求項4】

前記速度補正部が、
前記２次ＩＭＵ速度を、前記遅れ推定部より推定された前記ＣＡＮ速度の遅れ時間に応じた遅延量だけ遅延させることにより、前記ＣＡＮ速度と同期させる同期化処理部と、 前記ゲイン補正部でゲイン補正されたＣＡＮ速度と、前記同期化処理部で遅延された２次ＩＭＵ速度との差分値を算出する差分値算出部と、
前記差分値算出部で算出された差分値を入力してカルマンゲインを算出することにより、前記２次ＩＭＵ速度算出のための補正値を生成するカルマンゲイン算出部と、
ＩＭＵ由来の速度を前記カルマンゲイン算出部で生成された補正値に基づいて補正することにより、前記２次ＩＭＵ速度を生成する補正速度生成部とを備えたことを特徴とする請求項１から３のうちいずれか１項記載の速度計測装置。

【請求項5】

ＩＭＵ由来の速度を前記ＧＰＳ速度に基づいて補正することにより前記１次ＩＭＵ速度を生成する１次ＩＭＵ速度生成部をさらに備えたことを特徴とする請求項１から４のうちいずれか１項記載の速度計測装置。

【請求項6】

前記速度補正部が、前記１次ＩＭＵ速度とは独立に生成したＩＭＵ由来の速度を補正することにより前記２次ＩＭＵ速度を生成するものであることを特徴とする請求項１から５のうちいずれか１項記載の速度計測装置。

【請求項7】

前記速度補正部が、前記１次ＩＭＵ速度を補正することにより前記２次ＩＭＵ速度を生成するものであることを特徴とする請求項１から６のうちいずれか１項記載の速度計測装置。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、車両に搭載されてその車両の速度を計測する速度計測装置に関する。

【背景技術】

【0002】

車両に搭載されたＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）
とＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機の双方とを使って、その車両の速度を高精度に計測する速度計測装置が知られている（例えば、特許文献１，２参照）。

【0003】

ここで、ＩＭＵからは、例えば１００Ｈｚ等のクロックに同期して、ほぼリアルタイムで更新されたデータが出力される。ただし、ＩＭＵでは直接的には加速度が計測され、その加速度を積分することにより速度が計測される。このためＩＭＵで計測される速度は、ＩＭＵで直接に計測される加速度のオフセット誤差が順次累積されることになる。したがって、この速度データを補正しないままにしておくと真の値から徐々に離れて行き、次第に大きな誤差を含むデータとなってしまうおそれがある。

【0004】

一方、ＧＰＳ受信機での速度計測法の１つとして、ＧＰＳ衛星から送られてくる電波の搬送波のドプラシフト量に基づいて速度（以下、「ドプラ速度」と称することがある）を計測する計測法が知られている。この計測法を採用すると高精度な速度計測を行なうことができる。しかしながら、例えば、木々やビル群等によって規定数（４個）未満のＧＰＳ衛星からしか電波を受信することができない状況下にある場合には正確な速度計測を行なうことができないなど、高精度な速度計測を行なうことのできる条件には制約がある。

【0005】

特許文献１，２には、ＩＭＵ由来の速度データをＧＰＳ受信機由来のドプラ速度データに基づいて補正して出力速度データを生成する構成が開示されている。具体的には、それらの特許文献１，２には、ＧＰＳ受信機由来の速度データと出力速度データとの差分データを生成し、その差分データに基づいて、ＩＭＵ由来の速度データを補正するための補正データを算出し、その補正データを用いてＩＭＵ由来の速度データを補正して出力速度データを生成する速度計測装置が提案されている。

【0006】

ここで、特許文献１には、ＧＰＳ由来のドプラ速度データの信頼度を評価して係数を算出し、その係数に応じて、信頼度が高いときにＩＭＵ由来の速度データを強く補正し、信頼度が低いときには補正量を低減させることが提案されている。

【0007】

また、特許文献２には、ＧＰＳ受信機由来の速度データが、例えば２０Ｈｚ等のクロックに同期した、ＩＭＵと比べゆっくりとした周期でしか速度データが得られないＧＰＳ受信機を採用し、そのＧＰＳ受信機由来の速度データに補間処理を施した構成が開示されている。

【先行技術文献】

【特許文献】

【0008】

【特許文献1】特開２０１２−４２３１８号公報

【特許文献2】特開２０１３−１７０９０４号公報

【発明の概要】

【発明が解決しようとする課題】

【0009】

ここで、上掲の特許文献１，２に開示されている速度計測装置の場合、ＧＰＳ受信機由来の速度データの信頼性が高い場合には高精度な出力速度データが生成される。しかしながら、上記のような、規定数未満のＧＰＳ衛星からの電波しか受信できない場合などのように、ＧＰＳ受信機由来の速度データの信頼性が低い場合には、オフセット誤差が順次累積されるという、ＩＭＵ由来の速度データの短所がそのままあらわれた出力速度データが得られることになる。

【0010】

本発明は、上記事情に鑑み、ＧＰＳ受信機から信頼度の低い速度データしか得られないタイミングにおいても、高精度に補正された速度データを生成することのできる速度計測装置を提供することを目的とする。

【課題を解決するための手段】

【0011】

上記目的を達成する本発明の速度計測装置は、
ＧＰＳ受信機由来のＧＰＳ速度に基づいて補正されたＩＭＵ由来の車速を表わす１次ＩＭＵ速度に基づいて、車速を表わすＣＡＮ速度のゲインを補正するゲイン補正部と、
１次ＩＭＵ速度とＣＡＮ速度との間の相関演算により、１次ＩＭＵ速度に対するＣＡＮ速度の、１次ＩＭＵ速度とＣＡＮ速度との間の誤差が最小となる遅れ時間を推定する遅れ推定部と、
ＩＭＵ由来の速度を補正することにより２次ＩＭＵ速度を生成する速度補正部であって、
２次ＩＭＵ速度を、上記遅れ推定部で推定されたＣＡＮ速度の遅れ時間だけ遅延させることでＣＡＮ速度と時間的に同期した遅延２次ＩＭＵ速度を生成する同期化処理部、
上記ゲイン補正部で補正されたＣＡＮ速度と同期化処理部で生成された遅延２次ＩＭＵ速度との間の差分を生成する差分器、
前記差分器で生成された差分を入力し、運動方程式に基づいて、ＩＭＵ由来の現時点の速度を補正するカルマンゲインを生成するカルマンゲイン算出部、および
カルマンゲイン算出部で算出されたカルマンゲインを使って、ＩＭＵ由来の現時点の速度を補正することで上記２次ＩＭＵ速度を生成する補正速度生成部を有する速度補正部とを備えたことを特徴とする。

【0012】

本発明の速度計測装置は、車両内に構築されたネットワークであるＣＡＮ（Ｃｏｎｔｒｏｌ Ａｒｅａ Ｎｅｔｗｏｒｋ）に流れる、車輪の回転等に基づいて計測されるＣＡＮ速度のゲインを補正するとともに、そのＣＡＮ速度の遅れ時間を推定し、ゲインが補正されたＣＡＮ速度に基づき、かつそのＣＡＮ速度が推定された遅れ時間だけ遅れていることを考慮してＩＭＵ由来の速度を補正することにより２次ＩＭＵ速度を生成するものである。

【0013】

このため、ＧＰＳ由来の速度データで補正された１次ＩＭＵ速度の信頼度が低いタイミングであっても、高精度の２次ＩＭＵ速度が得られる。したがって、常に高精度の速度データが生成されることになる。

【0014】

ここで、本発明の速度計測装置において、上記ゲイン補正部が、所定の判定基準に基づいて判定された、ＧＰＳ速度に基づく補正がなされた１次ＩＭＵ速度が入力されているタイミングであること、および、定速走行中であることを条件として、ＣＡＮ速度のゲインを補正することを特徴とする。

【0015】

ＧＰＳ速度に基づいて補正がなされていて、かつ定速走行中であれば、１次ＩＭＵ速度に基づいてＣＡＮ速度のゲインを高精度に補正することができる。このようにして、ＧＰＳ速度に基づいて補正がなされている１次ＩＭＵ速度を使ってＣＡＮ速度のゲインを補正しておき、ゲインが補正されたＣＡＮ速度を使ってＩＭＵ由来の速度を補正することにより、ＧＰＳ速度の信頼度が低いタイミングにおいても高精度の２次ＩＭＵ速度が生成される。

【0016】

また、本発明の速度計測装置において、上記遅れ推定部が、所定の判定基準に基づいて判定された、ＧＰＳ速度に基づく正確な補正がなされた１次ＩＭＵ速度が入力されているタイミングであること、および、車速が変化しているタイミングであることを条件として、ＣＡＮ速度の遅れ時間を推定するものであることが好ましい。

【0017】

１次ＩＭＵ速度がＧＰＳ速度に基づいて補正がなされていて、かつ、車速が変化しているタイミングであれば、１次ＩＭＵ速度に基づいてＣＡＮ速度の遅れ時間を高精度に推定することができる。このようにして推定された遅れ時間を使って２次ＩＭＵ速度をＣＡＮ速度に同期させることにより、ＧＰＳ速度の信頼度が低いタイミングにおいても高精度の２次ＩＭＵ速度が生成される。

【0018】

さらに、本発明の速度計測装置において、上記速度補正部が、
上記２次ＩＭＵ速度を、遅れ推定部より推定されたＣＡＮ速度の遅れ時間に応じた遅延量だけ遅延させることにより、ＣＡＮ速度と同期させる同期化処理部と、
ゲイン補正部でゲイン補正されたＣＡＮ速度と、同期化処理部で遅延された２次ＩＭＵ速度との差分値を算出する差分値算出部と、
差分値算出部で算出された差分値を入力してカルマンゲインを算出することにより、２次ＩＭＵ速度算出のための補正値を生成するカルマンゲイン算出部と、
ＩＭＵ由来の速度をカルマンゲイン算出部で生成された補正値に基づいて補正することにより、２次ＩＭＵ速度を生成する補正速度生成部とを備えたものであることが好ましい。

【0019】

この構成を備えることにより、ＩＭＵ由来の速度から高精度に補正された２次ＩＭＵ速度が生成される。

【0020】

さらに、本発明の速度計測装置は、ＩＭＵ由来の速度をＧＰＳ速度に基づいて補正することにより１次ＩＭＵ速度を生成する１次ＩＭＵ速度生成部をさらに備えたものであってもよい。

【0021】

本発明の速度計測装置は、１次ＩＭＵ速度を別装置から受け取るものであってもよいが、上記のように、１次ＩＭＵ速度を生成する１次ＩＭＵ速度生成部が組み込まれたものであってもよい。

【0022】

また、本発明の速度計測装置において、上記速度補正部は、１次ＩＭＵ速度とは独立に生成したＩＭＵ由来の速度を補正することにより２次ＩＭＵ速度を生成するものであってもよく、あるいは、上記速度補正部は、１次ＩＭＵ速度を補正することにより２次ＩＭＵ速度を生成するものであってもよい。

【発明の効果】

【0023】

以上の本発明によれば、ＧＰＳ受信機から信頼度の低い速度データしか得られないタイミングにおいても、高精度に補正された速度データを生成することができる。

【図面の簡単な説明】

【0024】

【図1】本発明の第１実施形態としての速度計測装置を表わしたブロック図である。

【図2】図１に１つのブロックで示すゲイン補正部の内部構成を表わしたブロック図である。

【図3】図１に１つのブロックで示す遅れ推定部の内部構成を表わしたブロック図である。

【図4】遅延部および誤差演算部における処理内容を示した模式図である。

【図5】図１に１つのブロックで示す速度補正部の内部構成を表わしたブロック図である。

【図6】図１に示す速度計測装置から出力された最終ＩＭＵ速度を、衛星捕捉オン、オフと対応づけて示した図である。

【図7】本発明の第２実施形態としての速度計測装置を表わしたブロック図である。

【図8】１次ＩＭＵ速度生成装置の構成を示すブロック図である。

【発明を実施するための形態】

【0025】

以下、本発明の実施の形態を説明する。

【0026】

図１は、本発明の第１実施形態としての速度計測装置を表わしたブロック図である。

【0027】

この速度計測装置１００Ａは、ゲイン補正部１０と、遅れ推定部２０と、速度補正部３０と、切替スイッチ４０とを備えている。

【0028】

ゲイン補正部１０には、ＣＡＮ速度と１次ＩＭＵ速度とが入力され、ＣＡＮ速度が１次ＩＭＵ速度と同じ速度となるようにＣＡＮ速度のゲイン補正が行なわれる。

【0029】

ここで、ＣＡＮ速度は、車両の車輪の回転等に基づいて計測され、車両内に構築されたＣＡＮ（Ｃｏｎｔｒｏｌ Ａｒｅａ Ｎｅｔｗｏｒｋ）に流れる速度信号により得られる速度である。このＣＡＮ速度は、車両の正確な速度にほぼ比例しているが、不正確な速度であり、かつ、応答遅れがある。

【0030】

一方、１次ＩＭＵ速度は、車両に搭載されたＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）由来の速度であって、同じ車両に搭載されたＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機由来の速度によって補正された速度である。ＩＭＵは、慣性計測装置と呼ばれ、３軸のジャイロセンサと３軸の加速度センサが組み込まれていて、３次元の角速度と３方向の加速度が求められる。ＩＭＵからは、それらの角速度と加速度がリアルタイムで、かつ例えば１０ｍｓ等の周期で計測されて出力される。このＩＭＵから出力される角速度および加速度は、そのＩＭＵが搭載されている車両の進行方向を基準としたときの角速度および加速度である。

【0031】

また、ＧＰＳ受信機での速度計測法の１つとして、ＧＰＳ衛星から送られてくる電波の搬送波のドプラシフト量に基づいて速度（以下、「ドプラ速度」と称することがある）を計測する計測法が知られている。この計測法を採用すると、高精度な速度計測を行なうことができる。しかしながら、前述の通り、例えば、木々やビル等によって規定数（４個）未満の数のＧＰＳ衛星からしか電波を受信することができない状態にあると正確な速度計測が不能となるなど、高精度な速度計測を行なうことのできる条件には制約がある。このＧＰＳ受信機からは、車両の進行方向基準ではなく、地軸基準（北方向、東方向、下方向）の各速度が算出される。また、このＧＰＳ受信機で得られる速度は、一例として、１００ｍｓ等、ＩＭＵよりも更新速度が遅く、かつ一例として、１４０〜１５０ｍｓ程度の時間遅れとジッタ（時間遅れの変動）を伴っている。

【0032】

ここで説明している１次ＩＭＵ速度は、ＩＭＵ由来の加速度等に基づいて算出された、リアルタイム性が確保され、かつＧＰＳ受信機由来の速度に基づいて補正された車両の進行方向の速度をいう。ＩＭＵ由来の加速度等に基づいて算出される速度をＧＰＳ受信機由来の速度に基づいて如何にして補正するかという点については、特定の補正演算に限るものではないが、ここでは後で、その一例について説明する。また、ＩＭＵ加速度も、上述のＩＭＵから出力された車両の進行方向の加速度である。

【0033】

図１に示す速度計測装置には、さらに、「勾配」、「衛星捕捉オン／オフ」、および「リバース」の各情報が入力される。

【0034】

「勾配」は、現在走行中の道路の勾配を表わす情報である。

【0035】

この「勾配」の情報は、ＩＭＵ由来の情報であってもよく、ＧＰＳ受信機由来の情報であってもよく、あるいはそれら双方の情報を統合させた情報であってもよい。ただし、通常、ＣＡＮからは「勾配」の情報を得ることはできない。

【0036】

「衛星捕捉オン／オフ」は、ＧＰＳ受信機において正確な速度計測に必要な数以上の数のＧＰＳ衛星からの電波を受信できる状況にある（オン）か否か（オフ）かをあらわす情報である。本実施形態では、この「衛星捕捉オン／オフ」の情報を、ＩＭＵ由来の速度がＧＰＳ受信機由来の速度に基づいて正しく補正されているか否かの情報として採用している。ただし、ＧＰＳ受信機由来の速度の信頼性を評価する指標は、「衛星捕捉オン／オフ」の情報のみではない。例えばＧＰＳ受信機において速度分散に基づいて算出される、ドプラ速度の精度の指標を表わすドプラ精度指標値を採用し、ＩＭＵ由来の速度がＧＰＳ由来の速度に基づいて正しく補正されているか否かを、そのドプラ精度指標値に基づいて判定してもよい。あるいは、正しく補正されているか否かを、前掲の特許文献１において提案されている良否係数に基づいて判定してもよい。

【0037】

図２は、図１に１つのブロックで示すゲイン補正部の内部構成を表わしたブロック図である。

【0038】

このゲイン補正部１０では、ＣＡＮ速度を１次ＩＭＵ速度に一致させるべく、ＣＡＮ速度のゲインが補正される。

【0039】

このゲイン補正部１０は、判定部１１を備えている。この判定部１１では、後述する補正係数算出部１３においてＣＡＮ速度のゲイン補正するための正確な補正係数αを算出することが可能なタイミングであるか否かが判定される。

【0040】

この判定部１１では、以下の判定条件に基づく判定が行なわれ、正確な補正係数αの算出が可能なタイミングのときにイネーブル信号ＥＮが出力される。

【0041】

・車両が前進していること
車両がバックしているタイミングを除外するための判定基準である。前進しているか否かは、ＣＡＮに流れる「リバース」の情報から知ることができる。

【0042】

・ＣＡＮ速度および１次ＩＭＵ速度の双方が１０ｋｍ／ｈ以上の速度を示していること
車両がＣＡＮ速度を補正することができる速度で走行しているタイミングを狙うための判定基準である。判定部１１には、ＣＡＮ速度および１次ＩＭＵ速度が入力され、この判定基準はそれらに基づいて行なわれる。

【0043】

・路面の勾配が±５％以内であること
水平な道路を走行しているタイミングを狙うための判定基準である。これは、ＩＭＵあるいはＧＰＳ受信機から得られる「勾配＝ｔｅｎ（上下方向の速度／水平方向の速度）」の情報に基づいて判定される。

【0044】

・ＧＰＳ受信機で計測される速度の信頼性が高いこと
ＧＰＳ受信機由来の速度によって正確に補正された１次ＩＭＵ速度が得られているタイミングを狙うための判定基準である。本実施形態では、「衛星捕捉オン／オフ」の情報に基づいて判定される。

【0045】

・速度変動が±５％以内であること
ＣＡＮ速度は応答遅れが大きいため、応答遅れが問題とならない定速走行時を狙うための判定基準である。判定部１１には、１次ＩＭＵ速度が微分器１２を経由することで算出される加速度が入力され、その加速度に基づいて判定される。

【0046】

この図２に示すゲイン補正部１０には、補正係数算出部１３が備えられており、判定部１１において上記の判定基準の全てを満たしたタイミングで補正係数算出部１３に向けてイネーブル信号ＥＮが出力される。この補正係数算出部１３では、イネーブル信号ＥＮが入力されている間、補正係数αが繰り返し算出される。一方、この補正係数算出部１３では、判定部１１からのイネーブル信号ＥＮが途切れたときは、その直前に算出した補正係数αがそのまま維持される。

【0047】

この補正係数算出部１３には、差分器１３１が備えられており、この差分器１３１では、入力されてきた１次ＩＭＵ速度と、後述する補正部１４においてゲイン補正された後のＣＡＮ速度との差分値Ｖ_eが算出される。

【0048】

この差分値Ｖ_eは、累積値算出器１３２に入力され、差分値Ｖ_eの累積値

【0049】

【数1】

【0050】

が算出される。

【0051】

ここでｄｔは、演算の繰り返し周期（本実施形態では１０ｍｓ）である。

【0052】

本実施形態では、ＣＡＮ速度および１次ＩＭＵ速度は１００Ｈｚのクロックに同期して１０ｍｓ間隔で入力され、１回ごとの演算が１０ｍｓ間隔で算出される。

【0053】

累積値算出部１３２で算出された累積値は増幅器（減衰器）１３３においてあらかじめ定められた倍率（減衰率）だけ増幅（減衰）されることで、補正係数αが算出される。この算出された補正係数αは補正部１４の乗算器１４１に入力される。

【0054】

乗算器１４１では、入力されてきたＣＡＮ速度に補正係数αが乗算されて加算部１４２に入力される。加算器１４２では、入力されてきたＣＡＮ速度と、補正係数αが乗算されたＣＡＮ速度とが加算されて、ゲイン補正後のＣＡＮ速度が生成される。

【0055】

すなわち、この補正部１４では
ＣＡＮ速度（ゲイン補正）＝（１＋α）・ＣＡＮ速度
の演算が行なわれる。

【0056】

この図２に示すゲイン補正部１０では、このようにしてゲイン補正後のＣＡＮ速度が算出される。

【0057】

図３は、図１に１つのブロックで示す遅れ推定部の内部構成を表わしたブロック図である。

【0058】

この遅れ推定部２０では、リアルタイム性が確保されている１次ＩＭＵ速度を基準としたときのＣＡＮ速度の遅れ時間の推定が行なわれる。

【0059】

この遅れ推定部２０にも、判定部２１および１次ＩＭＵ速度を微分する微分器２２が備えられている。

【0060】

ゲイン補正部１０の判定部１１は、定速走行していることが、イネーブル信号ＥＮを出力する１つの条件であったが、この遅れ推定部２０の判定部２１では、それに代わり、加速あるいは減速を行なっている（１例として加速度±３ｍ／ｓ²を越えている）タイミングであることが、１つの条件となる。定速走行時には遅れ時間を推定することができないからである。その他の判定条件は、ゲイン補正部１０における判定条件と同一であり、ここでの重複説明は省略する。

【0061】

この判定部２１から出力されたイネーブル信号ＥＮは誤差演算部２４に入力される。この誤差演算部２４では、ゲイン補正後のＣＡＮ速度と、遅延部２３で遅延を受けた１次ＩＭＵ速度とが入力され、イネーブル信号ＥＮの入力を受けて、以下に説明する誤差演算が行なわれる。また、この誤差演算部２４では、イネーブル信号ＥＮが中断されたときは、その直前の演算結果が保持され、イネーブル信号ＥＮが再入力されると演算が再開される。

【0062】

図４は、遅延部２３および誤差演算部２４における処理内容を示した模式図である。

【0063】

図４（Ａ）には、ある時点で遅延部２３等に格納されているデータを示している。ここでａ_i（ｉ＝０，１，２，・・・）は１０ｍｓごとに入力されてくる１次ＩＭＵ速度を表わしている。ａ₀の次（１０ｍｓ後）にａ₁が入力され、その後（さらに１０ｍｓ後）にａ₂が入力され、・・・のようにａ_iのｉの数値が大きいデータほど後に入力されたデータである。

【0064】

遅延部２３には、入力されてきたデータの格納領域が３０用意されていて、図の左側から入力されたデータａ₀は、１０ｍｓごとに右に１つずつシフトし、図４（Ａ）に示す時点では遅延部２３の一番右側の格納領域に移動している。

【0065】

図４（Ａ）に示す時点では、一番左側の格納領域にはデータａ₃₀が格納され、その右隣りの格納領域にはデータａ₂₉が格納されている。そして今まさに、データａ₃₀が入力されている。また、ｂ_i（ｉ＝３０，３１，・・・）はゲイン補正後のＣＡＮ速度を表わしており、１次ＩＭＵ速度と同期して、今まさにデータｂ₃₀が入力されてきている。

【0066】

図３に示すように、誤差演算部２４は、差分器２４１と、２乗演算器２４２と、積算器２４３とから構成されている。

【0067】

図４に戻って誤差演算部２４における演算処理について説明する。

【0068】

図３に示す差分器２４１は、図４に示す３０個の差分器２４１＿１，２４１＿２，２４１＿３，・・・，２４１＿３０からなる。これら３０個の差分器２４１＿１，２４１＿２，２４１＿３，・・・，２４１＿３０では、それぞれ、ａ₃₀−ｂ₃₀，ａ₂₉−ｂ₃₀，ａ₂₈−ｂ₃₀，・・・ａ₀−ｂ₃₀が算出される。すなわち、差分器２４１＿１では、今回入力された同時刻の２つのデータａ₃₀，ｂ₃₀どうしの差分が算出され、差分器２４１＿２では１周期前（１０ｍｓ前）に入力された１次ＩＭＵ速度ａ₂₉と今回入力されたＣＡＮ速度（ゲイン補正）ｂ₃₀との差分が算出され、差分器２４１＿３では、２周期前（２０ｍｓ前）に入力された１次ＩＭＵ速度ａ₂₈と今回入力されたＣＡＮ速度（ゲイン補正）ｂ₃₀との差分が算出され、以下同様にして、差分器２４１＿３０では３０周期前（３００ｍｓ前）に入力された１次ＩＭＵ速度ａ₀と今回入力されたＣＡＮ速度（ゲイン補正）ｂ₃₀との差分が算出される。

【0069】

図３に示す２乗演算器２４２は、図４に示す３０個の２乗演算器２４２＿１，２４２＿２，２４２＿３，・・・，２４２＿３０の集合体である。図４（Ａ）に示す時点において、これら３０個の２乗演算器２４２＿１，２４２＿２，２４２＿３，・・・，２４２＿３０では、それぞれ
（ａ₃₀−ｂ₃₀）²，（ａ₂₉−ｂ₃₀）²，（ａ₂₈−ｂ₃₀）²，・・・（ａ₀−ｂ₃₀）²
が算出される。

【0070】

さらに、図３に示す積算器２４３は、図４に示す３０個の積算器２４３＿１，２４３＿２，２４３＿３，・・・，２４３＿３０の集合体である。３０個の積算器２４３＿１，２４３＿２，２４３＿３，・・・，２４３＿３０では、それぞれ、各２乗演算器２４２＿１，２４２＿２，２４２＿３，・・・，２４２＿３０から１周期ごとに出力される誤差の各２乗値が積算される。

【0071】

図４（Ｂ），（Ｃ）には、図４（Ａ）に示した時点よりも、それぞれ１周期（１０ｍｓ）後、２周期（２０ｍｓ）後の演算結果が示されている。このように、各周期ごと（１０ｍｓごと）に演算が行なわれ、３０個の積算器２４３＿１，２４３＿２，２４３＿３，・・・，２４３＿３０では、それぞれ、
・・・＋（ａ₃₀−ｂ₃₀）²＋（ａ₃₁-ｂ₃₁）²＋（ａ₃₂-ｂ₃₂）²＋・・・
・・・＋（ａ₂₉−ｂ₃₀）²＋（ａ₃₀-ｂ₃₁）²＋（ａ₃₁-ｂ₃₂）²＋・・・
・・・＋（ａ₂₈−ｂ₃₀）²＋（ａ₂₉-ｂ₃₁）²＋（ａ₃₀-ｂ₃₂）²＋・・・
・・・・・・
・・・＋（ａ₀−ｂ₃₀）²＋（ａ₁-ｂ₃₁）²＋（ａ₂-ｂ₃₂）²＋・・・
が算出される。

【0072】

すなわち、各積算器２４３＿１，２４３＿２，２４３＿３，・・・，２４３＿３０では、１次ＩＭＵ速度ａ_iとＣＡＮ速度（ゲイン補正）ｂ_iに関し、それぞれ、同一時点どうしの２乗誤差の積算値、１周期ずれた時点どうしの２乗誤差の積算値、２周期ずれた時点どうしの２乗誤差の積算値、・・・、３０周期ずれた時点どうしの２乗誤差の積算値が算出される。

【0073】

図３に示す誤差演算部２４では、上記の通りの演算が行なわれ、その演算結果は、遅れ時間算出部２５に入力される。遅れ時間算出部２５では、図４に示す３０個の積算器２４３＿１，２４３＿２，２４３＿３，・・・，２４３＿３０の出力どうしを比較し、最小値の出力の積算器を選定することで、遅れ時間Ｉが推定される。具体的には、同一時刻どうしの２乗誤差の積算値を算出している積算器２４３＿１の出力値が最小値のときは、遅れ時間Ｉ＝０ｍｓ、１周期（１０ｍｓ）ずれた時点どうしの２乗誤差の積算値を算出している積算器２４３＿２の出力値が最小値のときは、遅れ時間Ｉ＝１０ｍｓ、２周期（２０ｍｓ）ずれた時点どうしの２乗誤差の積算値を算出している積算器２４３＿２の出力値が最小値のときは、遅れ時間Ｉ＝２０ｍｓ、・・・、３０周期（３００ｍｓ）ずれた時点どうしの２乗誤差の積算値を算出している積算器２４３＿３０の出力値が最小値のときは遅れ時間Ｉ＝３００ｍｓと推定される。

【0074】

図１，図３に示す遅れ推定部２０では、以上の演算により、１次ＩＭＵ速度に対するＣＡＮ速度の遅れ時間Ｉが推定される。

【0075】

図５は、図１に１つのブロックで示す速度補正部の内部構成を表わしたブロック図である。

【0076】

この速度補正部３０には、補正速度生成部３１、カルマンゲイン算出部３２、差分器３３、および同期化処理部３４が備えられている。

【0077】

補正速度生成部３１は、ＩＭＵ加速度を入力し、運動方程式に従って、ＩＭＵ加速度からＩＭＵ速度を算出する。ここでは、図１に示す入力側の１次ＩＭＵ速度と区別するために、補正速度生成部３１から出力されるＩＭＵ速度を２次ＩＭＵ速度と称する。

【0078】

ＩＭＵ由来の３軸の角速度および方向の加速度のデータを入力し、３次元の運動方程式に基づいて３次元のＩＭＵ速度を算出するストラップダウンナビゲータが広く知られている。この図５に示す補正速度生成部３１は、そのストラップダウンナビゲータにおける処理を車両の進行方向のみの１次元の処理に簡略化した構成であり、したがって、良く知られた構成のストラップダウンナビゲータから自明であるため、補正速度生成部３１における処理の詳細説明は割愛する。

【0079】

補正速度生成部３１で算出された２次ＩＭＵ速度は、同期化処理部３４に入力される。この同期化処理部３４では、その入力された２次ＩＭＵ速度が遅れ推定部２０で求められたＣＡＮ速度の遅れ時間に応じた遅延量だけ遅延され、ＣＡＮ速度との同期がとられる。

【0080】

同期化処理部３４で遅延された２次ＩＭＵ速度は、差分器３３に入力され、その差分器３３に入力されてきたＣＡＮ速度（ゲイン補正）との差分値δｘが算出される。

【0081】

この算出された差分値δｘは、カルマンゲイン算出部３２に入力され、このカルマンゲイン算出部３２では、運動方程式に基づくカルマンフィルタよりカルマンゲインＫが算出される。カルマンフィルタおよびカルマンゲインについては広く知られたアルゴリズムであり、ここでの詳細説明は割愛する。

【0082】

カルマンゲイン算出部３２ではカルマンゲインＫが算出されて、補正値Ｋδｘが出力される。この補正値Ｋδｘは、補正速度生成部３１に入力されて、２次ＩＭＵ速度を算出する際の補正値として使用される。

【0083】

図１に戻って、本実施形態の速度計測装置１００Ａについての説明を続ける。
この速度計測装置１００Ａには、切替スイッチ４０が備えられている。この切替スイッチ４０は、衛星捕捉のオン、オフに応じて、衛星捕捉オンのときはこの速度計測装置１００Ａに入力されてきた１次ＩＭＵ速度をそのまま最終ＩＭＵ速度として出力し、衛星捕捉オフのときは、この速度計測装置１００Ａで生成された２次ＩＭＵ速度を最終ＩＭＵ速度として出力するように切り替えられる。

【0084】

図６は、図１に示す速度計測装置１００Ａから出力された最終ＩＭＵ速度を、衛星捕捉オン、オフと対応づけて示した図である。

【0085】

衛星捕捉オンのときは、切替スイッチ４０は１次ＩＭＵ速度を出力する側に切り替えられていて、その１次ＩＭＵ速度が最終ＩＭＵ速度として出力される。すなわち、図６における衛生捕捉オンの区間における最終ＩＭＵ速度は１次ＩＭＵ速度である。この１次ＩＭＵ速度は、ＩＭＵ由来であってリアルタイム性が確保され、かつ衛星捕捉オンのときにはＧＰＳ受信機由来のドプラ速度によって高精度に補正された速度である。

【0086】

一方、衛星捕捉オフのときは、切替スイッチ４０は、２次ＩＭＵ速度を出力する側に切り替えられていて、その２次ＩＭＵ速度が最終ＩＭＵ速度として出力される。衛星捕捉オフのとき、ＧＰＳ受信機では正確な速度計測が行なわれないため、１次ＩＭＵ速度はＧＰＳ受信機由来の速度によっては補正されないか、あるいは不正確な補正しかなされていない。図１に示す速度計測装置１００Ａは、１次ＩＭＵ速度に基づいてＣＡＮ速度のゲインおよび遅れ時間を求めておき、１次ＩＭＵ速度の精度が低下したタイミングでは、その求めておいたゲインで補正されたＣＡＮ速度および遅れ時間を使って正確な２次ＩＭＵ速度を算出している。

【0087】

したがって、図６から分かるように、衛星捕捉オン区間における１次ＩＭＵ速度と衛星捕捉オフ区間における２次ＩＭＵ速度との間の切り替わりのタイミングにおいても、滑らかに連続した高精度の最終ＩＭＵ速度が得られる。

【0088】

図７は、本発明の第２実施形態としての速度計測装置を表わしたブロック図である。ここでは、前述の第１実施形態（主に図１参照）との相違点について説明する。

【0089】

この図７に示す第２実施形態の速度計測装置１００Ｂも、第１実施形態の速度計測装置１００Ａと同様、ゲイン補正部１０、遅れ推定部２０、および速度補正部３０’を備えている。ただし、この第２実施形態の速度計測装置１００Ｂでは、第１実施形態の速度計測装置１００Ａに備えられていた切替スイッチ４０（図１参照）は不要である。この第２実施形態の速度計測装置１００Ｂにおけるゲイン補正部１０および遅れ推定部２０は、第１実施形態の速度計測装置１００におけるゲイン補正部１０および遅れ推定部２０とそれぞれ同一であり、ここでの説明は省略する。ただし、速度補正部３０’については、第１実施形態における速度補正部３０（図１参照）と比べ、以下の通りの変更点が存在する。すなわち、第１実施形態における速度補正部３０（図１参照）には、ＩＭＵ由来の車両の進行方向の加速度が入力され、その速度補正部３０では、その入力された加速度から２次ＩＭＵ速度が算出される。これに対し、この第２実施形態の速度補正部３０’には、加速度に代わり、１次ＩＭＵ速度が入力される。したがって、この速度補正部３０’では、１次ＩＭＵ速度から２次ＩＭＵ速度が算出される。このため、この速度算出部３０’では速度（１次ＩＭＵ速度）から速度（２次ＩＭＵ速度）を算出するための運動方程式が採用されている。この点の変更についても、従来広く知られている技術からして自明であるため、詳細説明は省略する。

【0090】

速度補正部３０’からは２次ＩＭＵ速度が出力される。この２次ＩＭＵ速度は、衛星捕捉オンのときは高精度の１次ＩＭＵ速度に更に補正処理がなされた速度である。これは無用の処理ではあるが、この補正処理が行なわれたことによる問題が生じることはなく、高精度の速度が出力される。また、衛星捕捉オフのときは、１次ＩＭＵ速度の精度は低下するものの、前述の第１実施形態における速度補正部３０と同様、速度補正部３０’により正確に補正された２次ＩＭＵ速度が生成される。

【0091】

次に、１次ＩＭＵ速度生成のアルゴリズムについて簡単に説明する。

【0092】

図８は、１次ＩＭＵ速度生成装置の構成を示すブロック図である。

【0093】

この１次ＩＭＵ速度生成装置５０は、本発明にいう１次ＩＭＵ速度生成部の一例に相当する。

【0094】

この図８に示す１次ＩＭＵ速度生成装置５０は、ＩＭＵ１からのデータをＧＰＳ受信機２からのデータで補正することにより、上述の１次ＩＭＵ速度を生成する装置である。

【0095】

ＩＭＵ１からは、１００Ｈｚのクロックに同期して、３次元の角速度と３方向の加速度のデータが出力され、ストラップダウンナビゲータ部５１に入力される。このストラップダウンナビゲータ部５１では、ＩＭＵ１から出力された車両の進行方向基準の角速度と加速度に基づいて、地軸基準の速度データが算出される。また、このストラップダウンナビゲータ部５１では、その速度データがさらに積算処理されて地軸基準の位置データも生成される。このストラップダウンナビゲータ部５１ではさらに、その生成された速度データと位置データが補正データに基づいて補正されて、補正後の速度データと補正後の位置データが出力される。このストラップダウンナビゲータ部５１で算出される速度データおよび位置データは、いずれも地軸基準の３次元データである。ストラップダウンナビゲータ部５１自体は良く知られた構成であり、ここでのこれ以上の説明は省略する。

【0096】

この図８において、補正データは、Ｋδｘ又は０と表記されている。ストラップダウンナビゲータ部５１では、補正データ＝Ｋδｘのときは、その補正データに基づいた補正が行なわれ、補正データ＝０のときは、補正は行なわれず、ＩＭＵ由来のデータのみに基づいて算出された速度データおよび位置データが出力される。

【0097】

ここで、補正データ＝ＫδｘのときのＫはカルマン係数、δｘは差分値である。この差分値δｘには速度の差分と位置の差分との双方が含まれている。この差分値δｘの内容については後述する。

【0098】

ＧＰＳ受信機２からはドプラ速度を表わす速度データと、位置データとが出力される。これらの速度データおよび位置データは、いずれも地軸基準の３次元データである。ＧＰＳ受信機２から出力される速度データおよび位置データは、通常、ＩＭＵ１から出力されるデータと比べ、例えば１０Ｈｚあるいは２０Ｈｚといった遅いクロックに同期して更新されるデータである。しかも更新されるデータは実時間からすると既に例えば１００ｍｓ〜１５０ｍｓ程度遅延し、しかも数ｍｓ程度のジッタ（遅延のゆれ）を伴ったデータである。すなわちＧＰＳ受信機２から出力されるデータは、規定数以上のＧＰＳ衛星からの電波が正しく受信されるなどの速度計測の条件が整っているタイミングにおいて、高精度な速度計測が行なわれるものの、上記の通りの遅れを伴い、かつ更新速度の遅いデータとなっている。また、速度計測の条件が整わないときは、正確な速度計測は不能となる。

【0099】

ここでは、速度計測の条件が整っているか否かを、規定数以上のＧＰＳ衛星からの電波が正しく受信されているか否かを表わす衛星捕捉オン／オフで代表させている。

【0100】

ＧＰＳ受信機２から出力される速度データおよび位置データは差分器５２に入力される。

【0101】

一方、ストラップダウンナビゲータ部５１から出力された速度データおよび位置データは同期化処理部５３に入力される。この同期化処理部５３は、入力された速度データおよび位置データを遅延させるものである。上述の通り、ＧＰＳ受信機２から出力される速度データおよび位置データは時間遅れを伴うデータである。そこで、この同期化処理部５３では、ＧＰＳ受信機２から出力されるデータと同期させるべく、ストラップダウンナビゲータ部５１から出力されたデータが遅延処理される。

【0102】

同期化処理部５３で遅延処理を受けたストラップダウンナビゲータ部５１の出力データは、ＧＰＳ受信機２から出力されたデータとともに差分器５２に入力される。

【0103】

差分器５２では、それら双方のデータの差分値δｘが算出される。

【0104】

具体的には、ＧＰＳ受信機２から出力された速度データＶ₁および位置データＰ₁を、それぞれ、
Ｖ₁＝（Ｖ_1n，Ｖ_1e，Ｖ_1d）
Ｐ₁＝（Ｐ_1n，Ｐ_1e，Ｐ_1d）
但し、
Ｖ_1nは、速度の北向きの成分
Ｖ_1eは、速度の東向きの成分
Ｖ_1dは、速度の下向きの成分
Ｐ_1nは、位置の北向きの成分
Ｐ_1eは、位置の東向きの成分
Ｐ_1dは、位置の下向きの成分
を表わす。
とし、同期化処理部５３で遅延処理を受けた速度データＶ₂および位置データＰ₂を、それぞれ、
Ｖ₂＝（Ｖ_2n，Ｖ_2e，Ｖ_2d）
Ｐ₂＝（Ｐ_2n，Ｐ_2e，Ｐ_2d）
但し、
Ｖ_2nは、速度の北向きの成分
Ｖ_2eは、速度の東向きの成分
Ｖ_2dは、速度の下向きの成分
Ｐ_2nは、位置の北向きの成分
Ｐ_2eは、位置の東向きの成分
Ｐ_2dは、位置の下向きの成分
を表わす。
としたとき、差分器５２では、差分値δｘ、すなわち

【0105】

【数2】

【0106】

が算出される。

【0107】

ここで、
δＰは位置誤差
δＶは速度誤差
であって、
δＰ＝（δＰ_n，δＰ_e，δＰ_d）
＝（Ｐ_1n−Ｐ_2n，Ｐ_1e−Ｐ_2e，Ｐ_1d−Ｐ_2d）
δＶ＝（δＶ_n，δＶ_e，δＶ_d）
＝（Ｖ_1n−Ｖ_2n，Ｖ_1e−Ｖ_2e，Ｖ_1d−Ｖ_2d）
である。

【0108】

差分器５２で算出された差分値δｘを表わす差分データは、切替スイッチ５４に入力される。

【0109】

この切替スイッチ５４は、ＧＰＳ受信機２から出力される衛星捕捉情報が規定数以上の衛星を正しく捉えていることを表わす「衛星捕捉オン」のときは、差分器５２で算出された差分値δｘを拡張カルマンフィルタ部５５に伝え、衛星捕捉情報が規定数未満の衛星しか正しく捉えられていないことを表わす「衛星捕捉オフ」のときは、値‘０’を拡張カルマンフィルタ部５５に伝える。すなわち、ＧＰＳ受信機２で速度が高精度に計測されているときは差分値δｘの信頼性が高く、その差分値δｘがそのまま拡張カルマンフィルタ部５５の入力として反映され、ＧＰＳ受信機２で計測される速度の信頼性が低いときは差分値δｘの信頼性が低いため、値‘０’が拡張カルマンフィルタ部５５の入力となることを意味している。

【0110】

拡張カルマンフィルタ部３０では運動方程式に基づいてカルマン係数Ｋを算出しＩＭＵ１由来のデータを補正するため補正データを生成してストラップダウンナビゲータ部５１に向けて出力する。この補正データは、衛星捕捉オンのときは、補正データ＝Ｋ・δｘとなり、衛星捕捉オフのときは、補正データ＝０となる。

【0111】

なお、この拡張カルマンフィルタ部３０自体は広く知られているため、ここでのこれ以上の詳細な説明は省略する。

【0112】

ストラップダウンナビゲータ部５１では、衛星捕捉オンの期間では、ＩＭＵ１由来の速度データおよび位置データが拡張カルマンフィルタ部５５から受け取った補正データ＝Ｋ・δｘに基づいて補正される。

【0113】

ストラップダウンナビゲータ部５１から出力される速度データおよび位置データは、ＩＭＵ由来の１００Ｈｚのクロックに同期したリアルタイム性が確保されたデータである。さらに、このストラップダウンナビゲータ部５１から出力される速度データおよび位置データは、衛星捕捉オンの期間においては、ＧＰＳ受信機２由来のデータによってオフセット誤差等がキャンセルされた高精度なデータである。ただし、このストラップダウンナビゲータ部５１から出力される速度データおよび位置データは、衛星捕捉オフの期間中は補正が行なわれないため、この衛星捕捉オフの期間中は、オフセット誤差等が順次累積され、時間経過とともに正確な速度や位置から外れたデータとなる。

【0114】

１次ＩＭＵ速度生成部５６は、ストラップダウンナビゲータ部５１から出力された速度データから車両の進行方向の１次元の速度を算出し、１次ＩＭＵ速度として出力する。この１次ＩＭＵ速度はストラップダウンナビゲータ部５１の出力データに基づいており、したがって衛星捕捉オンのときは高精度な速度を表わしており、衛星捕捉オフのときは次第にずれた速度を表わしている。

【0115】

この図８に示す１次ＩＭＵ速度生成装置５０から出力された１次ＩＭＵ速度は、図１に示す速度計測装置１００Ａあるいは図７に示す速度計測装置１００Ｂに入力される。

【0116】

なお、この図８は、ＩＭＵ１由来のデータとＧＰＳ受信機２由来のデータとに基づいて１次ＩＭＵ速度を生成する構成の一例であり、本発明にいう１次ＩＭＵ速度生成部は、この図８に示す構成に限られるものではない。

【符号の説明】

【0117】

１ ＩＭＵ
２ ＧＰＳ受信機
１０ ゲイン補正部
１１，２１ 判定部
１２，２２ 微分器
１３ 補正係数算出部
１４ 補正部
２０ 遅れ推定部
２３ 遅延部
２４ 誤差演算部
２５ 遅れ時間算出部
３０ 速度補正部
３１ 補正速度生成部
３２ カルマンゲイン算出部
３３，５２ 差分器
３４，５３ 同期化処理部
４０，５４ 切替スイッチ
５０ １次ＩＭＵ速度生成装置
５１ ストラップダウンナビゲータ部
５５ 拡張カルマンフィルタ部
５６ １次ＩＭＵ速度生成部
１００Ａ，１００Ｂ 速度計測装置
１３１ 差分器
１３２ 累積値算出部
１３３ 増幅器（減衰器）
１４１ 乗算器
１４２ 加算器
２４１，２４１＿１，・・・，２４１＿３０ 差分器
２４２，２４２＿１，・・・，２４２＿３０ ２乗演算器
２４３，２４３＿１，・・・，２４３＿３０ 積算器

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】