JP2005024535A - 位置推定装置 - Google Patents

Abstract

【課題】全送信機の中にマルチパスの影響を受けたものがあったり、送信機から受信機までの伝播経路上に問題がある場合においても、精度の高い測位を行うことが可能な位置推定装置を得る。
【解決手段】位置が既知である複数の送信機１から送信された電波を、位置が未知である受信機２で受信して、受信機位置を推定する位置推定装置であって、受信可能な全送信機１の中から、所定の最低数以上の送信機の組み合わせを生成する送信機組み合わせ変更器３と、当該組み合わせ毎に、送信機１から受信機２までの伝播時間を計測して得る送受信機間の距離を基に測位解を算出する測位解算出器６と、過去に計測された受信機の位置から現在の位置を予測する予測器８と、当該予測位置と当該測位解との予測残差に基づいて、送信機組み合わせ変更手段３による送信機１の組み合わせを選択する測位解選択器９とを備えている。
【選択図】図１

Description

この発明は位置推定装置に関し、特に、ＧＰＳ測位システムなどに使用され、電波の伝播時間または時間差や位相差を利用して受信機位置または送信機位置の位置を推定するための位置推定装置に関するものである。

位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、送信機から受信機までの伝播時間を計測して受信機位置を測位するという従来の方式が提案されている（例えば、非特許文献１参照。）。

上記非特許文献１に示されるように、この種の従来の方式においては、次の式（１）〜式（１０）のような測位方程式を解く方式が一般的である。式（１）は測位方程式である。

ここで、δｒは擬似距離誤差ベクトル、Ａは方向余弦ベクトル、δｕは受信機位置誤差ベクトルを表す。
擬似距離誤差ベクトルは式（２）のとおりである。

式（２）のｒ_ｉ（ｉ＝１，・・・，Ｎの自然数であって、Ｎ≧４；３次元測位の場合）は、ｉ番目の送信機位置から受信機位置までの擬似距離であり、伝播時間と光速の積から求める。
式（２）のｆ_ｉ（ｘ_０，ｙ_０，ｚ_０，ｔ_０）（ｉ＝１，・・・，Ｎの自然数であって、Ｎ≧４；３次元測位の場合）は概算距離であり、反復演算で得られる前回の受信機位置の測位値（ｘ_０，ｙ_０，ｚ_０）と受信機時計誤差ｔ_０、既知であるｉ番目の送信機位置（ｘ_ｉ，ｙ_ｉ，ｚ_ｉ）から式（３）を用いて算出される。ｃは光速である。

方向余弦ベクトルは式（４）のとおりである。

方向余弦ベクトルの各要素Ａ_ｉ，ｘ，Ａ_ｉ，ｙ，Ａ_ｉ，ｚ（ｉ＝１，・・・，Ｎの自然数であって、Ｎ≧４；３次元測位の場合）は、それぞれ、式（５）、式（６）、式（７）のとおりである。

受信機位置誤差ベクトルは、式（８）のとおりである。

式（８）の（ｘハット，ｙハット，ｚハット）が算出する測位解であり、ｔハットは副次的に求められる受信機時計誤差である。

３次元空間での測位においては、
もし、受信できる送信機数が３個以下の場合、式（１）の方程式は不定解となり、測位は不可能である。
もし、受信できる送信機数が４個の場合、式（９）のように式（１）の解を求め、測位することができる。

もし、受信できる送信機数が５個以上の場合、式（１０）のように式（１）の最小二乗解を求め、測位することができる。

なお、２次元空間での測位においては、
もし、受信できる送信機数が２個以下の場合、式（１）の方程式は不定解となり、測位は不可能である。
もし、受信できる送信機数が３個の場合、式（９）のように式（１）の解を求め、測位することができる。
もし、受信できる送信機数が４個以上の場合、式（１０）のように式（１）の最小二乗解を求め、測位することができる。

受信できる送信機数が、測位に必要な最低数（３次元測位の場合は４個、２次元測位の場合は３個）より多い場合は式（１０）に示した最小二乗法を用いて測位解を求めることができることを上で述べたが、上記の非特許文献１のように全ての送信機を用いて測位解を求める方法の他に、送信機を適宜選択して測位解を求める方式もよく用いられる（例えば、特許文献１参照。）。

上記の特許文献１に示された従来の方式では、８個の衛星（送信機）から、受信可能な任意の４個の衛星を組み合わせて測位解を求め、それぞれの測位解に対する誤差の評価指標ＤＯＰとＵＥＲＥの積算値を元に各測位解の誤差を評価し、積算値の最も小さい測位解を、測位結果として選択する。
ここで、ＤＯＰは衛星の配置に依存する測位精度の評価指標であり、例えば、３次元測位の場合、式（１１）とおくと、式（１２）で与えられる。

また、ＵＥＲＥは、衛星の内部時計の誤差や軌道のずれ等の情報として衛星から放送されるＳＶａｃｃｕｒａｃｙの値に応じた誤差評価指標である。

特開平６−５９０１４号公報 日本測地学会編著、「ＧＰＳ−人工衛星による精密測位システム−」第３版、初版１９８９年１１月１５日、ｐ．１２４−１３１

上記の非特許文献１に示されている従来の方式は、常に全送信機を用いて測位演算を行うため、送信機の中にマルチパスの影響を受けた送信機があったり、伝播経路上に問題のある送信機があったりすると測位精度が劣化するという問題点があった。

また、上記の特許文献１に示された他の従来の方式は、冗長に用意された送信機数から、測位に必要な数の任意の送信機の組み合わせ毎に測位解を求め、送信機の配置による測位誤差や衛星の内部時計の誤差や軌道のずれ等、衛星自らが提供するＳＶａｃｃｕｒａｃｙ情報に関する誤差に起因して発生する測位誤差の評価指標を送信機の選択基準として、これらの誤差により不具合となった送信機を含まない組み合わせを選択しているが、上記マルチパスや伝搬経路上の問題により測位精度が劣化する場合を評価することができないという問題点があった。

この発明はかかる問題点を解決するためになされたもので、全送信機の中の一個または複数個の送信機にマルチパスの影響を受けたものがあったり、送信機から受信機までの伝播経路上に問題がある場合においても、精度の高い測位を行うことが可能な位置推定装置を得ることを目的とする。

この発明は、位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に受信機位置を推定する位置推定装置であって、受信可能な全送信機の中から、受信機位置の測定に必要な所定の最低数以上の送信機の組み合わせを生成する送信機組み合わせ変更手段と、上記送信機組み合わせ変更手段による上記送信機の組み合わせ毎に、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、上記受信機位置の測位解を算出する測位解算出手段と、過去に計測された受信機の位置から現在の受信機の位置を予測する予測手段と、上記予測手段で算出された予測位置と上記測位解算出手段で算出された上記測位解との予測残差に基づいて、上記送信機組み合わせ変更手段による上記送信機の組み合わせを選択する選択手段とを備えた位置推定装置である。

この発明は、位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に受信機位置を推定する位置推定装置であって、受信可能な全送信機の中から、受信機位置の測定に必要な所定の最低数以上の送信機の組み合わせを生成する送信機組み合わせ変更手段と、上記送信機組み合わせ変更手段による上記送信機の組み合わせ毎に、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、上記受信機位置の測位解を算出する測位解算出手段と、過去に計測された受信機の位置から現在の受信機の位置を予測する予測手段と、上記予測手段で算出された予測位置と上記測位解算出手段で算出された上記測位解との予測残差に基づいて、上記送信機組み合わせ変更手段による上記送信機の組み合わせを選択する選択手段とを備えた位置推定装置であるので、全送信機の中の一個または複数個の送信機にマルチパスの影響を受けたものがあったり、送信機から受信機までの伝播経路上に問題がある場合においても、精度の高い測位を行うことが可能である。

実施の形態１．
図１はこの発明の実施の形態１による位置推定装置を示す構成図であり、図において、１は空間に電波を放出する複数の送信機、２は送信機１からの電波を受信する受信機、３は測位を行うための送信機１の組み合わせを変更する送信機組み合わせ変更器、４は組み合わせ変更器３で組み合わせた送信機１から受信機２までの伝播時間から、送受信機１，２間の擬似距離を算出する擬似距離算出器、５は送受信機１，２の位置情報を用いて方向余弦ベクトルを算出する方向余弦ベクトル算出器、６は擬似距離と方向余弦ベクトルとから測位解を算出する最小二乗測位解算出器、７は最小二乗測位解算出器６で算出された測位解を記憶する測位解記憶器、８は平滑器１０で算出された平滑ベクトルが遅延回路１１を介して入力され、受信機２の位置の予測ベクトル及び予測誤差共分散行列を算出する予測器、９は、予測器８から予測ベクトルを入力し、測位解記憶器７から測位解を入力して、予測位置に近い測位解を選択する測位解選択器、１０は測位解選択器９から入力された測位解と、予測器８から入力された予測ベクトル及び予測誤差共分散行列とから平滑ベクトル及び平滑誤差共分散行列を算出する平滑器、１１は平滑器１０で算出した平滑ベクトル及び平滑誤差共分散行列を１サンプリング時刻だけ遅延する遅延回路である。

次に動作について説明する。
最初に、この実施の形態１による位置推定装置の動作原理を説明する。なお、ここでは、３次元位置を推定する場合を仮定する。
位置が既知である複数の送信機１から送信された電波を、位置が未知である受信機２で受信して得られる送受信機１，２間の伝播時間から、式（１）の測位方程式を解くことによって受信機２の位置を測位する原理は、上記の非特許文献１に記載された従来の技術と同じである。
受信機２の運動を、過去の測位結果を用いて予測する方法について説明する。
受信機２の運動モデルを式（１３）に示す。ここで、ｘ_ｋアンダーバーはサンプリング時刻ｔ_ｋにおける受信機２の運動諸元の真値を表す状態ベクトルであり、受信機２の位置ベクトルを式（１４）、速度ベクトルを式（１５）とすると、受信機２の状態ベクトルは式（１６）で表される。ここで、サンプリング時刻ｔ_ｋ（ｋ＝１，２，・・・，ｎ）は、測位が行われるタイミングを表す離散時刻であり、以降、時刻ｔ_ｋと呼ぶ。

Φ_ｋ−１は時刻ｔ_ｋ−１から時刻ｔ_ｋへの状態ベクトルの推移行列であり、式（１７）で表される。また、ｗ_ｋアンダーバーは時刻ｔ_ｋにおける駆動雑音ベクトルであり、Γ_１（ｋ）は時刻ｔ_ｋにおける駆動雑音ベクトルの変換行列である。例えば、受信機の運動モデルを等速直線運動と仮定したことによる打ち切り誤差項をΓ_１（ｋ−１）ｗ_ｋ−１アンダーバーとみれば、ｗ_ｋアンダーバーは加速度ベクトル相当であり、Γ_１（ｋ−１）は式（１８）で表される。なお、Ｔはサンプリング間隔、Ｉは３行３列の単位行列である。

また、平均を表す記号としてＥを用いると、ｗ_ｋアンダーバーは平均の３次元正規分布白色雑音であり、式（１９）及び式（２０）とする。ただし、０アンダーバーは零ベクトルであり、Ｑ_ｋは時刻ｔ_ｋにおける駆動雑音共分散行列である。

受信機２の位置の観測モデルを式（２１）で表す。ここで、ｕ_ｋアンダーバーは測位解選択器９によって選択された測位解、Ｈは観測行列で、式（２２）で表される。
また、ｖ_ｋアンダーバーは時刻ｔ_ｋにおいて選択された送信機１と受信機２の距離の観測雑音ベクトルであり、式（２３）で表す。観測雑音ベクトルは平均０アンダーバーの３次元正規分布白色雑音に従うものと仮定し、式（２４）及び式（２５）とする。なお、Ｒ_ｋは時刻ｔ_ｋにおける送信機１と受信機２の距離の観測誤差共分散行列である。時刻ｔ_ｋまでの間に受信機２の追尾に用いた測位解の全体をＵ_ｋとし、式（２６）で表す。

また、Γ_２（ｋ）はＮ個の送受信機１，２間距離から３次元位置への観測雑音ベクトルの変換行列であり、式（２７）、式（２８）で表される。

ここで、ｈ_ｉ（ｘ_ｋアンダーバー）はｉ番目の送信機１と受信機２の距離であり、式（２９）で表される。また、行列Ｂのｉ行目の要素は式（３０）〜式（３２）で表される。ｘ_ｋアンダーバー（−）は時刻ｔ_ｋにおける受信機位置の予測ベクトルであり、算出方法は次節で説明する。

次に、時刻ｔ_ｋ−１までの測位解Ｕ_ｋ−１が得られているときの予測ベクトルの算出方法について述べる。時刻ｔ_ｋおける受信機の状態ベクトルｘ_ｋアンダーバーの予測ベクトルを（ｘ_ｋアンダーバー）ハット（−）、予測誤差共分散行列をＰ_ｋ（−）とすると、それぞれ、条件付平均ベクトルおよび条件付共分散行列で定義され、式（３３）及び式（３４）で表される。ここで、（ｘ_ｋ−１アンダーバー）ハット（＋）及びＰ_ｋ−１（＋）は、それぞれ、時刻ｔ_ｋ−１の平滑ベクトル及び平滑誤差共分散行列である。これらの算出方法については次に述べる。

以上、受信機２の運動を、過去の測位結果を用いて予測する方法について説明した。

次に、測位解記憶器７に記憶された時刻ｔ_ｋにおける複数の測位解の選択方法について述べる。測位解の選択には、式（３４）で表される過去の測位情報に基づく受信機２の現時刻の位置予測ベクトルｕ_ｋアンダーバー（−）を利用する。

すなわち、測位解記憶器７に記憶された時刻ｔ_ｋにおけるｍ番目の測位解をｕ_ｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）とすると、式（３６）における予測残差ｄ_ｋ，ｍアンダーバーが最小となる測位解ｕ_ｋアンダーバーを測位精度が最も高いものと判断し、選択する。なお、予測残差ｄ_ｋ，ｍアンダーバーとは、すなわち、受信機２の予測位置と測定位置との残差である。もし、送信機組み合わせ変更器３で選択された送信機１の組み合わせの中の１個または複数個の送信機１にマルチパスの影響を受けた送信機があったり、伝播経路上に問題のある送信機があると、その予測残差は大きな値となるので、予測残差の最も小さい送信機１の組み合わせを選択すれば全ての送信機が正常で伝播経路上にも問題がない送信機の組み合わせを決定することができ、受信機２の位置をより高精度に推定することができる。

以上、測位解の選択方法について述べた。

次に、平滑ベクトルの算出方法について述べる。ゲイン行列Ｋ_ｋ、平滑ベクトル（ｘ_ｋアンダーバー）ハット（＋）及び平滑誤差共分散行列Ｐ_ｋ（＋）は通常のカルマンフィルタの理論により、式（３７）〜式（３９）で与えられる。ここで、Ｒ_ｋは式（２５）で表される、時刻ｔ_ｋにおける送信機１と受信機２の距離の観測誤差共分散行列である。以上、平滑ベクトルの算出方法について述べた。

以上、実施の形態１による位置推定装置の動作原理について説明した。

次に、実施の形態１による位置推定装置の具体的な動作を図１を用いて説明する。
複数の送信機１から発射された電波は、送信機１から受信機２への距離に基づく遅延時間後に受信機２に到達する。
受信機２では各々の送信機１からの信号を弁別して受信する。送信機組み合わせ変更器３では、その全受信可能な送信機１の中から、測位に必要な所定の最低数以上、全発信機数以下の数で、適当な送信機１の組み合わせを選択する。擬似距離算出器４では、選択した組み合わせの送信機１の信号の伝播遅延時間から擬似距離ｒ_ｉ（ｉ＝１，２，・・・，Ｎ）を算出する。

方向余弦ベクトル算出器５では、予め設定された送信機１の位置情報から式（４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、擬似距離ｒ_ｉ（ｉ＝１，２，・・・，Ｎ）と方向余弦ベクトルＡを式（１０）に代入して、最小二乗測位解ｕ_ｋ、ｍアンダーバーを算出する。送信機組み合わせ変更器３により送信機１の組み合わせを変えて、以上の測位演算と距離残差の算出を所定のＭ回繰り返し、その測位解ｕ_ｋ、ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）をそれぞれ測位解記憶器７に記憶する。

予測器８では、１サンプリング前の平滑ベクトル（ｘ_ｋ−１アンダーバー）ハット（＋）と平滑誤差共分散行列Ｐ_ｋ−１（＋）を遅延回路１１を介して入力し、式（３３）〜式（３５）に従い、予測ベクトル（ｘ_ｋアンダーバー）ハット（−）、予測誤差共分散行列Ｐ_ｋ−１（−）、位置予測ベクトル（ｕ_ｋアンダーバー）（−）をそれぞれ算出する。測位解選択器９では、予測器８から位置予測ベクトル（ｕ_ｋアンダーバー）（−）を入力し、測位解記憶器７に記憶された測位解ｕ_ｋ、ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）の中から、式（３６）で表される予測残差ｄ_ｋ、ｍが最も小さい送信機１の組み合わせの測位解ｕ_ｋアンダーバーを選択して、最終的な測位解とする。

平滑器１０では、測位解選択器で選択した測位解ｕ_ｋアンダーバーと、予測器８で算出した予測ベクトル（ｘ_ｋアンダーバー）ハット（−）及び予測誤差共分散行列Ｐ_ｋ（−）を用いて平滑ベクトル（ｘ_ｋアンダーバー）ハット（＋）及び平滑誤差共分散行列Ｐ_ｋ（＋）を算出する。

以上のように、本実施の形態に係る位置推定装置は、予測残差の最も小さい送信機１の組み合わせを選択するので、従来の方式に比べ、信頼性が高く、全ての送信機が正常で伝播経路上にも問題ないと考えられる送信機の組み合わせを選択して受信機の位置を推定することができる。

実施の形態２．
図２は、本実施の形態２による位置推定装置を示す構成図であり、図において、図１と同一符号である１〜８、１０、１１は実施の形態１と同一または相当部分を示すのでその説明を省略する。

１２は平滑器１０で算出した平滑誤差共分散行列を遅延回路を介して入力し、測位解の予測残差の確率密度関数を算出する予測残差確率密度算出器、１３は予測器８から予測ベクトルを、測位解記憶器７から測位解を、予測残差確率密度算出器１２から測位解の予測残差の確率密度関数をそれぞれ入力し、各測位解の予測残差の確率密度から測位に適さない測位解を排除するゲート判定器、１４はゲート判定器１３において排除されなかった測位解と、予測残差確率密度算出器１２で算出した測位解の予測残差の確率密度関数をそれぞれ入力し、測位解の信頼度を算出する測位解信頼度算出器である。

次に動作について説明する。
最初に、この実施の形態２による位置推定装置の動作原理を説明する。なお、ここでは、３次元位置を推定する場合を仮定する。
位置が既知である複数の送信機１から送信された電波を、位置が未知である受信機２で受信して得られる送受信機間の伝播時間から、（１）式の測位方程式を解くことによって受信機位置を測位する原理は従来の技術における非特許文献１と同じである。

また、測位対象の運動を、過去の測位結果を用いて予測する方法は、上述の実施の形態１と同じである。

次に、測位に適さない測位解を排除する方法について述べる。実施の形態１では、予測残差の最も小さい測位解のみを選択して位置を推定しているが、予測残差の大きさはマルチパス等のセンサ不具合による影響以外に、送信機１と受信機２の幾何学的配置関係にも影響を受ける。センサ不具合がなく、送受信機の幾何学的配置関係のみにより予測残差が大きくなっている測位解は、利用した方が高い精度が得られる可能性がある。そこで、送受信機の幾何学的配置関係による影響を考慮し、センサ不具合の影響を受けた測位解のみを排除する。

過去の測位情報Ｕ_ｋ−１より得られる測位解の予測確率分布Ｐ［ｕ_ｋ，ｍアンダーバー｜Ｕ_ｋ−１］は、式（４０）に示す条件付確率密度関数で表される。すなわち、測位解は、式（３５）で与えられる位置予測ベクトルｕ_ｋアンダーバー（−）を平均とし、式（４１）で与えられるＳ_ｋを共分散行列とする３次元正規分布ｇ_３（ｕ_ｋ，ｍアンダーバー；ｕ_ｋ（−），Ｓ_ｋ）に従うとする。この確率密度関数の式（４１）の右辺第２項は送受信機の幾何学的配置関係を考慮した項となっている。そして、式（４２）を満たす測位解を測位に利用し、満たさない測位解を排除する。ここで、ｑは目標との相関範囲を決めるパラメータであり、自由度３のχ自乗分布により算出する。

次に、式（４２）を満たす全ての測位解に対し（便宜上、その総数をＭとする）、測位解の確率密度ｒ_ｋ，ｍを式（４３）に従い算出し、これを正規化した式（４４）から当該測位解の信頼度β_ｋ，ｍを算出する。そして、式（４５）に従い、この信頼度で測位解を重み付けした平均値ｕ_ｋアンダーバーを受信機２の推定位置とする。

信頼度で測位解を重み付けした平均値ｕ_ｋアンダーバーを用いて平滑ベクトルを算出する方法は実施の形態１と同じである。なお、平滑ベクトルの位置ベクトルを受信機２の推定位置としてもよい。
以上、実施の形態２による位置推定装置の動作原理について説明した。

次に、実施の形態２による位置推定装置の具体的な動作を図２に従い説明する。
複数の送信機１から発射された電波は、送信機１から受信機２への距離に基づく遅延時間後に受信機２に到達する。受信機２では各々の送信機１からの信号を弁別して受信する。送信機組み合わせ変更器３では、その全受信可能な送信機１の中から、測位に必要な所定の最低数以上、全発信機数以下の数で、適当な送信機１の組み合わせを選択する。擬似距離算出器４では、選択した組み合わせの送信機１の信号の伝播遅延時間から擬似距離ｒ_ｉ（ｉ＝１，２，・・・，Ｎ）を算出する。

方向余弦ベクトル算出器５では、予め設定された送信機の位置情報から式（４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、擬似距離ｒ_ｉ（ｉ＝１，２，・・・，Ｎ）と方向余弦ベクトルＡを式（１０）に代入して、最小二乗測位解ｕ_ｋ，ｍアンダーバーを算出する。送信機組み合わせ変更器３により送信機１の組み合わせを変えて、以上の測位演算と距離残差の算出をＭ回繰り返し、その測位解ｕ_ｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）をそれぞれ測位解記憶器７に記憶する。

予測器８では、１サンプリング前の平滑ベクトル（（ｘ_ｋ−１アンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐ_ｋ−１（＋）を遅延回路１１を介して入力し、式（３３）〜式（３５）に従い予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）、予測誤差共分散行列Ｐ_ｋ（−）、位置予測ベクトル（ｕ_ｋアンダーバー）（−）をそれぞれ算出する。

予測残差確率密度算出器１２では、予測器８から予測誤差共分散行列Ｐ_ｋ（−）を入力し、これと、予め設定された送信機と受信機の距離の観測誤差共分散行列とから、式（２７）及び式（４１）に従い、測位解の予測確率密度関数Ｐ［ｕ_ｋ，ｍアンダーバー｜Ｕ_ｋ−１］の共分散行列Ｓ_ｋを算出する。ゲート判定器１３では、予測器８から位置予測ベクトルｕ_ｋアンダーバー（−）を、予測残差確率密度算出器１２から共分散行列Ｓ_ｋを入力し、測位解記憶器７に記憶された測位解ｕ_ｋ、ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）の中から、式（４２）を満たすものを選択し、満たさないものを排除する。

測位解信頼度算出器１４では、予測器８で算出した位置予測ベクトルｕ_ｋアンダーバー（−）と予測残差確率密度算出器１２で算出した共分散行列Ｓ_ｋとをゲート判定器１３を介して入力し、式（４２）を満たす測位解ｕ_ｋ、ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）の信頼度β_ｋ、ｍを、式（４４）に従い算出するとともに、この信頼度で測位解を重み付けした平均値ｕ_ｋアンダーバーを式（４５）に従い算出する。

平滑器１０では、測位解信頼度算出器１４で算出した測位解ｕ_ｋアンダーバーと、予測器８で算出した予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐ_ｋ（−）を用いて平滑ベクトル（（ｘ_ｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐ_ｋ（＋）を算出する。

以上のように本実施の形態は、測位解の予測確率分布を利用して送受信機１，２の幾何学的配置関係のみによって測位誤差が大きくなっている測位解と、センサ不具合の影響を受けた測位解を判別し、後者のみを排除した残りの測位解の全てを用いて受信機２の位置を推定するので、上記の実施の形態２より高い精度で受信機２の位置を推定することができる可能性が高い。また、予測確率分布を利用して算出した信頼度による測位解の重み付き平均値を推定結果としているため、信頼性の低い測位解の悪影響を少なくすることができる。

実施の形態３．
図３はこの実施の形態３による位置推定装置を示す構成図であり、図において、図１と同一符号である１〜７は実施の形態１と同一または相当部分を示すのでその説明を省略する。

１５は最小二乗測位解算出器６で算出した距離残差を記憶する距離残差記憶器、１３は測位解記憶器７から測位解を、距離残差記憶器１５から距離残差を入力し、各測位解の距離残差の確率密度から測位に適さない測位解を排除するゲート判定器、１４はゲート判定器１３において排除されなかった測位解とをそれぞれ入力し、測位解の信頼度を算出する測位解信頼度算出器である。

次に動作について説明する。
最初に、この実施の形態３による位置推定装置の動作原理を説明する。なお、ここでは、３次元位置を推定する場合を仮定する。位置が既知である複数の送信機１から送信された電波を、位置が未知である受信機２で受信して得られる送受信機間の伝播時間から、（１）式の測位方程式を解くことによって受信機位置を測位する原理は従来の技術における従来の方式１と同じである。

次に、測位に適さない測位解を排除する方法について述べる。実施の形態１では、予測残差大きさを基に測位解の精度を判断しているが、マルチパス等のセンサ不具合の影響により測位解の精度が悪くなっている場合には、式（１０）に従って測位解を算出する際に得られる距離残差も大きくなっていると考えられる。そこで、この実施の形態３では、距離残差を利用することによって、測位に適さない測位解を判定する。

すなわち、距離残差の確率分布を、平均０、分散μ^２の１次元正規分布と仮定すると、式（４６）で表される各測位解毎に得られる擬似距離ｒ_ｉと式（１０）の反復演算により得られた測位解ｆ_ｉ（ｘハット，ｙハット，ｚハット，ｔハット）の差のベクトルである距離残差の２乗ノルムρ_ｍ ^２が式（４７）を満たす測位解を測位に利用し、満たさない測位解を排除する（ｍは送信機１の組み合わせ番号）。ここで、ｗは目標との相関範囲を決めるパラメータであり、自由度３のχ自乗分布により算出する。

次に、式（４７）を満たす全ての測位解に対し（便宜上、その総数をＭとする）、測位解の確率密度ｒ’_ｋ、ｍを式（４８）に従い算出し、これを正規化した式（４９）から当該測位解の信頼度β’_ｋ、ｍを算出する。そして、式（５０）に従い、この信頼度で測位解を重み付けした平均値ｕ_ｋアンダーバーを受信機２の推定位置とする。

信頼度で測位解を重み付けした平均値ｕ_ｋアンダーバーを用いて平滑ベクトルを算出する方法は実施の形態１と同じである。
以上、実施の形態３による位置推定装置の動作原理について説明した。

次に、実施の形態３による位置推定装置の具体的な動作を図３に従い説明する。複数の送信機１から発射された電波は、送信機１から受信機２への距離に基づく遅延時間後に受信機２に到達する。受信機２では各々の送信機１からの信号を弁別して受信する。送信機組み合わせ変更器３では、その全受信可能な送信機１の中から、測位に必要な所定の最低数以上、全発信機数以下の数で、適当な送信機１の組み合わせを選択する。擬似距離算出器４では、選択した組み合わせの送信機１の信号の伝播遅延時間から擬似距離を算出する。

方向余弦ベクトル算出器５では、予め設定された送信機１の位置情報から式（４７）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、擬似距離ｒ_ｉ（ｉ＝１，２，・・・，Ｎ）と方向余弦ベクトルＡを式（１０）に代入して、最小二乗測位解ｕ_ｋ、ｍアンダーバーを算出する。送信機組み合わせ変更器３により送信機１の組み合わせを変えて、以上の測位演算と距離残差の算出をＭ回繰り返し、その測位解ｕ_ｋ、ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）をそれぞれ測位解記憶器７に記憶する。

距離残差記憶器１５では、最小二乗測位解算出器６から、擬似距離ｒ_ｉと測位解ｆ_ｉ（ｘハット，ｙハット，ｚハット，ｔハット）を入力し、式（４６）に従い距離残差の２乗ノルムρ_ｍ ^２を算出し、これを記憶する。ゲート判定器１３では、距離残差記憶器１５から入力した距離残差の２乗ノルムρ_ｍ ^２と、予め設定された距離残差の確率分布の分散μ^２とから、測位解記憶器７に記憶された測位解ｕ_ｋ、ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）の中から、式（４７）を満たすものを選択し、満たさないものを排除する。

測位解信頼度算出器１４では、距離残差の２乗ノルムρ_ｍ ^２と、予め設定された距離残差の確率分布の分散μ^２をゲート判定器１３を介して入力し、式（４７）を満たす全ての測位解に対し、測位解の信頼度β_ｋ，ｍを、式（４８）〜式（４９）に従い算出するとともに、この信頼度で測位解を重み付けした平均値ｕ_ｋアンダーバーを受信機２の推定位置として式（５０）に従い算出する。

以上のように、本実施の形態は、測位解の距離残差の確率分布を利用して不具合の影響を受けた測位解を判別し、後者のみを排除した残りの測位解の全てを用いて受信機２の位置を推定するので、従来の方式に比べ、信頼性が高く、全ての送信機１が正常で伝播経路上にも問題ないと考えられる送信機１の組み合わせを選択して受信機２の位置を推定することができる。

実施の形態４．
上記の実施の形態１では、位置が既知である複数の送信機１からの電波を、位置が未知である受信機２で受信して、送信機１から受信機２までの伝播時間を計測して受信機位置を測位する方式への本発明の応用例を示したが、本実施の形態は、逆に、位置が未知である送信機１からの電波を、位置が既知である複数の受信機２で受信して、送信機１から受信機２までの伝播時間を計測して送信機位置を測位する方式へ本発明を適用した適用例について説明する。

図４はこの実施の形態４による位置推定装置を示す構成図であり、図において、図１と同一符号である１、２、４〜１１は実施の形態１と同一または相当部分を示すのでその説明を省略する。

１６は測位を行うための受信機２の組み合わせを変更する受信機組み合わせ変更器である。

次に、実施の形態４による位置推定装置の具体的な動作を図４に従い説明する。送信機１から発射された電波は、各々の伝播経路の伝播時間後に複数の受信機２で各々の送信機１からの信号を弁別して受信する。受信機組み合わせ変更器１６では、その全受信可能な受信機２の中から、測位に必要な所定の最低数以上、全発信機数以下の数で、適当な受信機２の組み合わせを選択する。擬似距離算出器４では、選択した組み合わせの受信機２の信号の伝播遅延時間から擬似距離ｒ_ｉ（ｉ＝１，２，・・・，Ｎ）を算出する。

方向余弦ベクトル算出器５では、予め設定された受信機の位置情報から式（４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、擬似距離ｒ_ｉ（ｉ＝１，２，・・・，Ｎ）と方向余弦ベクトルＡを式（１０）に代入して、最小二乗測位解ｕ_ｋ，ｍアンダーバーを算出する。受信機組み合わせ変更器１６により受信機２の組み合わせを変えて、以上の測位演算と距離残差の算出をＭ回繰り返し、その測位解ｕ_ｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）をそれぞれ測位解記憶器７に記憶する。

予測器８では、１サンプリング前の平滑ベクトル（（ｘ_ｋ−１アンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐ_ｋ−１（＋）を遅延回路１１を介して入力し、式（３３）〜式（３５）に従い予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）、予測誤差共分散行列Ｐ_ｋ（−）、位置予測ベクトル（ｕ_ｋアンダーバー）（−）をそれぞれ算出する。測位解選択器９では、予測器８から位置予測ベクトル（ｕ_ｋアンダーバー）（−）を入力し、測位解記憶器７に記憶された測位解ｕ_ｋ、ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）の中から、式（３６）で表される予測残差ｄ_ｋ，ｍが最も小さい受信機の組み合わせの測位解ｕ_ｋアンダーバーを選択して、最終的な測位解とする。

平滑器１０では、測位解選択器で選択した測位解ｕ_ｋアンダーバーと、予測器８で算出した予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐ_ｋ（−）を用いて平滑ベクトル（（ｘ_ｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐ_ｋ（＋）を算出する。

以上のように、位置が未知である送信機１からの電波を、位置が既知である複数の受信機２で受信して、送信機１から受信機２までの伝播時間を計測して送信機位置を測位する方式に本発明を応用しても、実施の形態１と同様に、送信機１の位置をより高精度に測位することができる。

実施の形態５．
実施の形態２では、位置が既知である複数の送信機１からの電波を、位置が未知である受信機２で受信して、送信機１から受信機２までの伝播時間を計測して受信機位置を測位する方式への応用例であるが、本発明は、位置が未知である送信機１からの電波を、位置が既知である複数の受信機２で受信して、送信機１から受信機２までの伝播時間を計測して送信機位置を測位する方式にも適用できるので、本実施の形態においては、当該適用例について説明する。

図５はこの実施の形態５による位置推定装置を示す構成図であり、図において、図１と同一符号である１、２、４〜８、１０〜１４は実施の形態２と同一または相当部分を示すのでその説明を省略する。

１６は測位を行うための受信機２の組み合わせを変更する受信機組み合わせ変更器である。

次に、実施の形態５による位置推定装置の具体的な動作を図５に従い説明する。送信機１から発射された電波は、各々の伝播経路の伝播時間後に複数の受信機２で各々の送信機からの信号を弁別して受信する。受信機組み合わせ変更器１６では、その全受信可能な受信機２の中から、測位に必要な所定の最低数以上、全受信機数以下の数で、適当な受信機２の組み合わせを選択する。擬似距離算出器４では、選択した組み合わせの送信機１の信号の伝播遅延時間から擬似距離ｒ_ｉ（ｉ＝１，２，・・・，Ｎ）を算出する。

方向余弦ベクトル算出器５では、予め設定された送信機の位置情報から式（４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、擬似距離ｒ_ｉ（ｉ＝１，２，・・・，Ｎ）と方向余弦ベクトルＡを式（１０）に代入して、最小二乗測位解ｕ_ｋ，ｍアンダーバーを算出する。受信機組み合わせ変更器１６により受信機２の組み合わせを変えて、以上の測位演算と距離残差の算出をＭ回繰り返し、その測位解ｕ_ｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）をそれぞれ測位解記憶器７に記憶する。

予測器８では、１サンプリング前の平滑ベクトル（（ｘ_ｋ−１アンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐ_ｋ−１（＋）を遅延回路１１を介して入力し、式（３３）〜式（３５）に従い予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）、予測誤差共分散行列Ｐ_ｋ（−）、位置予測ベクトルｕ_ｋアンダーバー（−）をそれぞれ算出する。

予測残差確率密度算出器１２では、予測器８から予測誤差共分散行列Ｐ_ｋ（−）を入力し、これと、予め設定された送信機１と受信機２の距離の観測誤差共分散行列とから、式（２７）及び式（４１）に従い、測位解の予測確率密度関数
Ｐ［ｕ_ｋ，ｍアンダーバー｜Ｕ_ｋ−１］の共分散行列Ｓ_ｋを算出する。ゲート判定器１３では、予測器８から位置予測ベクトルｕ_ｋアンダーバー（−）を、予測残差確率密度算出器１２から共分散行列Ｓ_ｋを入力し、測位解記憶器７に記憶された測位解ｕ_ｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）の中から、式（４２）を満たすものを選択し、満たさないものを排除する。

測位解信頼度算出器１４では、予測器８で算出した位置予測ベクトルｕ_ｋアンダーバー（−）と予測残差確率密度算出器１２で算出した共分散行列Ｓ_ｋとをゲート判定器１３を介して入力し、式（４２）を満たす測位解ｕ_ｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）の信頼度β_ｋ，ｍを、式（４４）に従い算出するとともに、この信頼度で測位解を重み付けした平均値ｕ_ｋアンダーバーを式（４５）に従い算出する。

平滑器１０では、測位解信頼度算出器１４で算出した測位解ｕ_ｋアンダーバーと、予測器８で算出した予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐ_ｋ（−）を用いて平滑ベクトル（（ｘ_ｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐ_ｋ（＋）を算出する。

以上のようにして、位置が未知である送信機１からの電波を、位置が既知である複数の受信機２で受信して、送信機１から受信機２までの伝播時間を計測して送信機位置を測位する方式に本発明を応用しても、実施の形態２と同様に、送信機１の位置をより高精度に測位することができる。

実施の形態６．
実施の形態３では、位置が既知である複数の送信機１からの電波を、位置が未知である受信機２で受信して、送信機１から受信機２までの伝播時間を計測して受信機位置を測位する方式への応用例であるが、本発明は、位置が未知である送信機１からの電波を、位置が既知である複数の受信機２で受信して、送信機１から受信機２までの伝播時間を計測して送信機位置を測位する方式にも適用できるので、本実施の形態においては、当該適用例について説明する。

図６はこの実施の形態６による位置推定装置を示す構成図であり、図において、図１と同一符号である１、２、４〜７、１３〜１５は実施の形態３と同一または相当部分を示すのでその説明を省略する。

１６は測位を行うための受信機２の組み合わせを変更する受信機組み合わせ変更器である。

次に、実施の形態６による位置推定装置の具体的な動作を図６に従い説明する。送信機１から発射された電波は、各々の伝播経路の伝播時間後に複数の受信機２で各々の送信機からの信号を弁別して受信する。受信機組み合わせ変更器１６では、その全受信可能な受信機２の中から、測位に必要な所定の最低数以上、全受信機数以下の数で、適当な受信機２の組み合わせを選択する。擬似距離算出器４では、選択した組み合わせの送信機１の信号の伝播遅延時間から擬似距離
ｒ_ｉ（ｉ＝１，２，・・・，Ｎ）を算出する。

方向余弦ベクトル算出器５では、予め設定された送信機の位置情報から式（４７）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、擬似距離ｒ_ｉ（ｉ＝１，２，・・・，Ｎ）と方向余弦ベクトルＡを式（１０）に代入して、最小二乗測位解ｕ_ｋ，ｍアンダーバーを算出する。受信機組み合わせ変更器１６により受信機の組み合わせを変えて、以上の測位演算と距離残差の算出をＭ回繰り返し、その測位解ｕ_ｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）をそれぞれ測位解記憶器７に記憶する。

距離残差記憶器１５では、最小二乗測位解算出器６から、擬似距離ｒ_ｉと測位解ｆ_ｉ（ｘハット，ｙハット，ｚハット，ｔハット）を入力し、式（４６）に従い距離残差の２乗ノルムρ_ｍ ^２を算出し、これを記憶する。ゲート判定器１３では、距離残差記憶器１５から入力した距離残差の２乗ノルムρ_ｍ ^２と、予め設定された距離残差の確率分布の分散μ^２とから、測位解記憶器７に記憶された測位解ｕ_ｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）の中から、式（４７）を満たすものを選択し、満たさないものを排除する。

測位解信頼度算出器１４では、距離残差の２乗ノルムρ_ｍ ^２と、予め設定された距離残差の確率分布の分散μ^２をゲート判定器１３を介して入力し、式（４７）を満たす全ての測位解に対し、測位解の信頼度β_ｋ，ｍを、式（４８）〜式（４９）に従い算出するとともに、この信頼度で測位解を重み付けした平均値ｕ_ｋアンダーバーを受信機の推定位置として式（５０）に従い算出する。

以上のようにして、位置が未知である送信機１からの電波を、位置が既知である複数の受信機２で受信して、送信機１から受信機２までの伝播時間を計測して送信機位置を測位する方式に本発明を応用しても、実施の形態３と同様に、送信機の位置をより高精度に測位することができる。

実施の形態７．
実施の形態１は、位置が既知である複数の送信機１からの電波を、位置が未知である受信機２で受信して、送信機１から受信機２までの伝播時間を計測して受信機位置を測位する方式において、電波の伝播時間を用いた測位方式に本発明を適用したものであるが、本発明は、複数の送信機１から受信機２への電波の伝播時間差や位相差を用いた測位方式にも適用できるので、本実施の形態においては、当該適用例について説明する。

図７はこの実施の形態７による位置推定装置を示す構成図であり、図において、図１と同一符号である１〜３、５〜１１は実施の形態１と同一または相当部分を示すのでその説明を省略する。

１７は送信機組み合わせ変更器３で組み合わせた送信機から受信機への電波の伝播時間差もしくは位相差から、送受信機間の擬似距離差を算出する擬似距離差算出器である。

次に複数の送信機１から受信機２への電波の伝播時間差や位相差を用いた測位方法について説明する。測位演算は式（１０）の代わりに式（５１）を用いる。

但し、式（５１）のδｕ、Ａ、δｒはそれぞれ式（５２）、式（５３）、および式（５４）である。

式（５３）のΔｒ_ｉ，ｊ（ｉ，ｊ＝１，・・・，Ｎの自然数であって、Ｎ≧４；三次元測位の場合）はｉ番目の送信機１から受信機２までの距離とｊ番目の送信機から受信機までの距離との差であり、伝播時間差による測位の場合は伝播時間差と光速の積から求める。また、電波の位相差を用いる測位の場合には位相差と電波の波長の積から求める。

式（５３）、式（５４）のΔｆ_ｉ，ｊ（ｘ_０，ｙ_０，ｚ_０）はｉ番目の送信機位置から受信機位置までの距離とｊ番目の送信機位置から受信機位置までの距離の概算距離差であり、反復演算で得られる前回の測位値（ｘ_０，ｙ_０，ｚ_０）と、既知であるｉ番目の送信機位置（ｘ_ｉ，ｙ_ｉ，ｚ_ｉ）と、同じく既知であるｊ番目の発信機位置（ｘ_ｊ，ｙ_ｊ，ｚ_ｊ）から、式（５５）を用いて算出する。

次に、実施の形態７による位置推定装置の具体的な動作を図７に従い説明する。複数の送信機１から発射された電波は、送信機から受信機への距離に基づく遅延時間後に受信機２に到達する。受信機２では各々の送信機からの信号を弁別して受信する。送信機組み合わせ変更器３では、その全受信可能な送信機の中から、測位に必要な所定の最低数以上、全発信機数以下の数で、適当な送信機の組み合わせを選択する。擬似距離算出器１７では、選択した組み合わせの２個の送信機の信号の伝播時間差もしくは位相差から擬似距離差ｒ_ｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）を算出する。

方向余弦ベクトル算出器５では、予め設定された送信機の位置情報から式（５４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、擬似距離差ｒ_ｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）と方向余弦ベクトルＡを式（５１）に代入して、最小二乗測位解ｕ_ｋ，ｍアンダーバーを算出する。送信機組み合わせ変更器３により送信機の組み合わせを変えて、以上の測位演算と距離残差の算出をＭ回繰り返し、その測位解ｕ_ｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）をそれぞれ測位解記憶器７に記憶する。

予測器８では、１サンプリング前の平滑ベクトル（（ｘ_ｋ−１アンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐ_ｋ−１（＋）を遅延回路１１を介して入力し、式（３３）〜式（３５）に従い予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）、予測誤差共分散行列Ｐ_ｋ（−）、位置予測ベクトルｕ_ｋ（−）をそれぞれ算出する。測位解選択器９では、予測器８から位置予測ベクトルｕ_ｋアンダーバー（−）を入力し、測位解記憶器７に記憶された測位解ｕ_ｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）の中から、式（３６）で表される予測残差ｄ_ｋ，ｍが最も小さい送信機の組み合わせの測位解ｕ_ｋアンダーバーを選択して、最終的な測位解とする。

平滑器１０では、測位解選択器で選択した測位解ｕ_ｋアンダーバーと、予測器８で算出した予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐ_ｋ（−）を用いて平滑ベクトル（（ｘ_ｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐ_ｋ（＋）を算出する。

以上のようにして、位置が既知である複数の送信機１から送信された電波を、位置が未知である受信機２で受信して、受信機位置での各送信機の信号間の伝播時間差または位相差を計測して受信機位置を測位する方式に本発明を応用しても、実施の形態１と同様に、受信機２の位置をより高精度に測位することができる。

実施の形態８．
実施の形態２は、位置が既知である複数の送信機１からの電波を、位置が未知である受信機２で受信して、送信機１から受信機２までの伝播時間を計測して受信機位置を測位する方式において、電波の伝播時間を用いた測位方式に本発明を適用したものであるが、本発明は、複数の送信機１から受信機２への電波の伝播時間差や位相差を用いた測位方式にも適用できるので、本実施の形態においては、当該適用例について説明する。

図８はこの実施の形態８による位置推定装置を示す構成図であり、図において、図２と同一符号である１〜３、５〜８、１０〜１４は実施の形態２と同一または相当部分を示すのでその説明を省略する。

１７は送信機組み合わせ変更器３で組み合わせた送信機１から受信機２への電波の伝播時間差もしくは位相差から、送受信機間の擬似距離差を算出する擬似距離差算出器である。

なお、複数の送信機１から受信機２への電波の伝播時間差や位相差を用いた測位方法は実施の形態７と同じである。

次に、実施の形態８による位置推定装置の具体的な動作を図８に従い説明する。複数の送信機１から発射された電波は、送信機１から受信機２への距離に基づく遅延時間後に受信機２に到達する。受信機２では各々の送信機１からの信号を弁別して受信する。送信機組み合わせ変更器３では、その全受信可能な送信機１の中から、測位に必要な所定の最低数以上、全発信機数以下の数で、適当な送信機１の組み合わせを選択する。擬似距離差算出器１７では、選択した組み合わせの２個の送信機１の信号の伝播時間差もしくは位相差から擬似距離差ｒ_ｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）を算出する。

方向余弦ベクトル算出器５では、予め設定された送信機の位置情報から式（５４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、擬似距離差ｒ_ｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）と方向余弦ベクトルＡを式（５１）に代入して、最小二乗測位解ｕ_ｋ，ｍアンダーバーを算出する。送信機組み合わせ変更器３により送信機１の組み合わせを変えて、以上の測位演算と距離残差の算出をＭ回繰り返し、その測位解ｕ_ｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）をそれぞれ測位解記憶器７に記憶する。

予測器８では、１サンプリング前の平滑ベクトル（（ｘ_ｋ−１アンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐ_ｋ−１（＋）を遅延回路１１を介して入力し、式（３３）〜式（３５）に従い予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）、予測誤差共分散行列Ｐ_ｋ（−）、位置予測ベクトルｕ_ｋアンダーバー（−）をそれぞれ算出する。

予測残差確率密度算出器１２では、予測器８から予測誤差共分散行列Ｐ_ｋ（−）を入力し、これと、予め設定された送信機１と受信機２の距離の観測誤差共分散行列とから、式（２７）及び式（４１）に従い、測位解の予測確率密度関数
Ｐ［ｕ_ｋ，ｍアンダーバー｜Ｕ_ｋ−１］の共分散行列Ｓ_ｋを算出する。ゲート判定器１３では、予測器８から位置予測ベクトルｕ_ｋアンダーバー（−）を、予測残差確率密度算出器１２から共分散行列Ｓ_ｋを入力し、測位解記憶器７に記憶された測位解ｕ_ｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）の中から、式（４２）を満たすものを選択し、満たさないものを排除する。

測位解信頼度算出器１４では、予測器８で算出した位置予測ベクトルｕ_ｋアンダーバー（−）と予測残差確率密度算出器１２で算出した共分散行列Ｓ_ｋとをゲート判定器１３を介して入力し、式（４２）を満たす測位解ｕ_ｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）の信頼度β_ｋ，ｍを、式（４４）に従い算出するとともに、この信頼度で測位解を重み付けした平均値ｕ_ｋアンダーバーを式（４５）に従い算出する。

平滑器１０では、測位解信頼度算出器１４で算出した測位解ｕ_ｋアンダーバーと、予測器８で算出した予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐ_ｋ（−）を用いて平滑ベクトル（（ｘ_ｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐ_ｋ（＋）を算出する。

以上のようにして、位置が既知である複数の送信機１から送信された電波を、位置が未知である受信機２で受信して、受信機位置での各送信機１の信号間の伝播時間差または位相差を計測して受信機位置を測位する方式に本発明を応用しても、実施の形態２と同様に、受信機２の位置をより高精度に測位することができる。

実施の形態９．
実施の形態３は、位置が既知である複数の送信機１からの電波を、位置が未知である受信機２で受信して、送信機１から受信機２までの伝播時間を計測して受信機位置を測位する方式において、電波の伝播時間を用いた測位方式に本発明を適用したものであるが、本発明は、複数の送信機１から受信機２への電波の伝播時間差や位相差を用いた測位方式にも適用できるので、本実施の形態においては、当該適用例について説明する。

図９はこの実施の形態９による位置推定装置を示す構成図であり、図において、図３と同一符号である１〜３、５〜７、１３、１４は実施の形態３と同一または相当部分を示すのでその説明を省略する。

１７は送信機組み合わせ変更器３で組み合わせた送信機１から受信機２への電波の伝播時間差もしくは位相差から、送受信機間の擬似距離差を算出する擬似距離差算出器、１８は最小二乗測位解算出器６で算出した距離差残差を記憶する距離差残差記憶器である。

なお、複数の送信機１から受信機２への電波の伝播時間差や位相差を用いた測位方法は実施の形態７と同じである。また、距離差残差の２乗ノルムは式（５６）で表される。

次に、実施の形態９による位置推定装置の具体的な動作を図９に従い説明する。複数の送信機１から発射された電波は、送信機１から受信機２への距離に基づく遅延時間後に受信機２に到達する。受信機２では各々の送信機１からの信号を弁別して受信する。送信機組み合わせ変更器３では、その全受信可能な送信機１の中から、測位に必要な所定の最低数以上、全発信機数以下の数で、適当な送信機１の組み合わせを選択する。擬似距離差算出器１７では、選択した組み合わせの２個の送信機１の信号の伝播時間差もしくは位相差から擬似距離差ｒ_ｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）を算出する。

方向余弦ベクトル算出器５では、予め設定された送信機の位置情報から式（５４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、擬似距離差ｒ_ｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）と方向余弦ベクトルＡを式（５１）に代入して、最小二乗測位解ｕ_ｋ，ｍアンダーバーを算出する。送信機組み合わせ変更器３により送信機の組み合わせを変えて、以上の測位演算と距離残差の算出をＭ回繰り返し、その測位解ｕ_ｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）をそれぞれ測位解記憶器７に記憶する。

距離残差記憶器１５では、最小二乗測位解算出器６から、擬似距離差ｒ_ｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）と測位解ｆ_ｉ，ｊ（ｘハット，ｙハット，ｚハット）を入力し、式（５６）に従い距離差残差の２乗ノルムρ_ｍ ^２を算出し、これを記憶する。ゲート判定器１３では、距離残差記憶器１５から入力した距離残差の２乗ノルムρ_ｍ ^２と、予め設定された距離差残差の確率分布の分散μ^２とから、測位解記憶器７に記憶された測位解ｕ_ｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）の中から、式（４７）を満たすものを選択し、満たさないものを排除する。

測位解信頼度算出器１４では、距離差残差の２乗ノルムρ_ｍ ^２と、予め設定された距離差残差の確率分布の分散μ^２をゲート判定器１３を介して入力し、式（４７）を満たす全ての測位解に対し、測位解の信頼度β_ｋ，ｍを、式（４８）〜式（４９）に従い算出するとともに、この信頼度で測位解を重み付けした平均値ｕ_ｋアンダーバーを受信機の推定位置として式（５０）に従い算出する。

以上のようにして、位置が既知である複数の送信機１から送信された電波を、位置が未知である受信機２で受信して、受信機位置での各送信機１の信号間の伝播時間差または位相差を計測して受信機位置を測位する方式に本発明を応用しても、実施の形態３と同様に、受信機の位置をより高精度に測位することができる。

実施の形態１０．
実施の形態４は、位置が未知である送信機１から送信された電波を、位置が既知である複数の受信機２で受信して、送信機１から受信機２までの伝播時間を計測して送信機位置を測位する方式において、電波の伝播時間を用いた測位方式に本発明を適用したものであるが、本発明は、複数の受信機から送信機への電波の伝播時間差や位相差を用いた測位方式にも適用できるので、本実施の形態においては、当該適用例について説明する。

図１０はこの実施の形態１０による位置推定装置を示す構成図であり、図において、図４と同一符号である１、２、５〜１１、１６は実施の形態４と同一または相当部分を示すのでその説明を省略する。

１７は受信機組み合わせ変更器１６で組み合わせた送信機から受信機への電波の伝播時間差もしくは位相差から、送受信機間の擬似距離差を算出する擬似距離差算出器である。

なお、複数の送信機１から受信機２への電波の伝播時間差や位相差を用いた測位方法は実施の形態７と同じである。

次に、実施の形態１０による位置推定装置の具体的な動作を図１０に従い説明する。送信機１から発射された電波は、各々の伝播経路の伝播時間後に複数の受信機２で送信機からの信号を受信する。受信機組み合わせ変更器１６では、その全受信可能な受信機２の中から、測位に必要な所定の最低数以上、全受信機数以下の数で、適当な受信機２の組み合わせを選択する。擬似距離差算出器１７では、選択した組み合わせの２個の送信機１の信号の伝播時間差もしくは位相差から擬似距離差ｒ_ｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）を算出する。

方向余弦ベクトル算出器５では、予め設定された送信機の位置情報から式（５４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、擬似距離差ｒ_ｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）と方向余弦ベクトルＡを式（５１）に代入して、最小二乗測位解ｕ_ｋ，ｍアンダーバーを算出する。受信機組み合わせ変更器１６により受信機の組み合わせを変えて、以上の測位演算と距離残差の算出をＭ回繰り返し、その測位解ｕ_ｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）をそれぞれ測位解記憶器７に記憶する。

予測器８では、１サンプリング前の平滑ベクトル（（ｘ_ｋ−１アンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐ_ｋ−１（＋）を遅延回路１１を介して入力し、式（３３）〜式（３５）に従い予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）、予測誤差共分散行列Ｐ_ｋ（−）、位置予測ベクトルｕ_ｋアンダーバー（−）をそれぞれ算出する。測位解選択器９では、予測器８から位置予測ベクトルｕ_ｋアンダーバー（−）を入力し、測位解記憶器７に記憶された測位解ｕ_ｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）の中から、式（３６）で表される予測残差ｄ_ｋ，ｍが最も小さい受信機の組み合わせの測位解ｕ_ｋアンダーバーを選択して、最終的な測位解とする。

平滑器１０では、測位解選択器で選択した測位解ｕ_ｋアンダーバーと、予測器８で算出した予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐ_ｋ（−）を用いて平滑ベクトル（（ｘ_ｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐ_ｋ（＋）を算出する。

以上のようにして、位置が未知である送信機１から送信された電波を、位置が既知である複数の受信機２で受信して、受信機位置での送信機１の信号間の伝播時間差または位相差を計測して送信機位置を測位する方式に本発明を応用しても、実施の形態４と同様に、送信機１の位置をより高精度に測位することができる。

実施の形態１１．
実施の形態５は、位置が未知である送信機１から送信された電波を、位置が既知である複数の受信機２で受信して、送信機１から受信機２までの伝播時間を計測して送信機位置を測位する方式において、電波の伝播時間を用いた測位方式に本発明を適用したものであるが、本発明は、複数の受信機２から送信機１への電波の伝播時間差や位相差を用いた測位方式にも適用できるので、本実施の形態においては、当該適用例について説明する。

図１１はこの実施の形態１１による位置推定装置を示す構成図であり、図において、図５と同一符号である１、２、５〜８、１０〜１４、１６は実施の形態５と同一または相当部分を示すのでその説明を省略する。

１７は送信機組み合わせ変更器３で組み合わせた送信機から受信機への電波の伝播時間差もしくは位相差から、送受信機間の擬似距離差を算出する擬似距離差算出器である。

なお、複数の送信機１から受信機２への電波の伝播時間差や位相差を用いた測位方法は実施の形態７と同じである。

次に、実施の形態１１による位置推定装置の具体的な動作を図１１に従い説明する。送信機１から発射された電波は、各々の伝播経路の伝播時間後に複数の受信機２で送信機１からの信号を受信する。受信機組み合わせ変更器１６では、その全受信可能な受信機２の中から、測位に必要な所定の最低数以上、全受信機数以下の数で、適当な受信機２の組み合わせを選択する。擬似距離差算出器１７では、選択した組み合わせの２個の送信機の信号の伝播時間差もしくは位相差から擬似距離差ｒ_ｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）を算出する。

方向余弦ベクトル算出器５では、予め設定された受信機の位置情報から式（５４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、擬似距離差ｒ_ｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）と方向余弦ベクトルＡを式（５１）に代入して、最小二乗測位解ｕ_ｋ，ｍアンダーバーを算出する。受信機組み合わせ変更器１６により受信機２の組み合わせを変えて、以上の測位演算と距離差残差の算出をＭ回繰り返し、その測位解ｕ_ｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）をそれぞれ測位解記憶器７に記憶する。

予測器８では、１サンプリング前の平滑ベクトル（（ｘ_ｋ−１アンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐ_ｋ−１（＋）を遅延回路１１を介して入力し、式（３３）〜式（３５）に従い予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）、予測誤差共分散行列Ｐ_ｋ（−）、位置予測ベクトルｕ_ｋアンダーバー（−）をそれぞれ算出する。

予測残差確率密度算出器１２では、予測器８から予測誤差共分散行列Ｐ_ｋ（−）を入力し、これと、予め設定された送信機と受信機の距離の観測誤差共分散行列とから、式（２７）及び式（４１）に従い、測位解の予測確率密度関数
Ｐ［ｕ_ｋ，ｍアンダーバー｜Ｕ_ｋ−１］の共分散行列Ｓ_ｋを算出する。ゲート判定器１３では、予測器８から位置予測ベクトルｕ_ｋアンダーバー（−）を、予測残差確率密度算出器１２から共分散行列Ｓ_ｋを入力し、測位解記憶器７に記憶された測位解ｕ_ｋ，ｍ（ｍ＝１，２，・・・，Ｍ）の中から、式（４２）を満たすものを選択し、満たさないものを排除する。

測位解信頼度算出器１４では、予測器８で算出した位置予測ベクトルｕ_ｋアンダーバー（−）と予測残差確率密度算出器１２で算出した共分散行列Ｓ_ｋとをゲート判定器１３を介して入力し、式（４２）を満たす測位解ｕ_ｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）の信頼度β_ｋ，ｍを、式（４４）に従い算出するとともに、この信頼度で測位解を重み付けした平均値ｕ_ｋアンダーバーを式（４５）に従い算出する。

平滑器１０では、測位解信頼度算出器１４で算出した測位解ｕ_ｋアンダーバーと、予測器８で算出した予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐ_ｋ（−）を用いて平滑ベクトル（（ｘ_ｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐ_ｋ（＋）を算出する。

以上のようにして、位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、受信機位置での送信機の信号間の伝播時間差または位相差を計測して送信機位置を測位する方式に本発明を応用しても、実施の形態５と同様に、送信機の位置をより高精度に測位することができる。

実施の形態１２．
実施の形態６は、位置が未知である送信機１から送信された電波を、位置が既知である複数の受信機２で受信して、送信機１から受信機２までの伝播時間を計測して送信機位置を測位する方式において、電波の伝播時間を用いた測位方式に本発明を適用したものであるが、本発明は、複数の受信機から送信機への電波の伝播時間差や位相差を用いた測位方式にも適用できるので、本実施の形態においては、当該適用例について説明する。

図１２はこの実施の形態１２による位置推定装置を示す構成図であり、図において、図６と同一符号である１、２、５〜７、１３、１４、１６は実施の形態６と同一または相当部分を示すのでその説明を省略する。

１７は受信機組み合わせ変更器１６で組み合わせた送信機１から受信機２への電波の伝播時間差もしくは位相差から、送受信機間の擬似距離差を算出する擬似距離差算出器である。

なお、複数の送信機１から受信機２への電波の伝播時間差や位相差を用いた測位方法は実施の形態７と同じである。また、距離差残差の２乗ノルムは実施の形態９と同じである。

次に、実施の形態１２による位置推定装置の具体的な動作を図１２に従い説明する。送信機１から発射された電波は、各々の伝播経路の伝播時間後に複数の受信機２で送信機１からの信号を受信する。受信機組み合わせ変更器１６では、その全受信可能な受信機２の中から、測位に必要な所定の最低数以上、全受信機数以下の数で、適当な受信機２の組み合わせを選択する。擬似距離差算出器１７では、選択した組み合わせの２個の送信機１の信号の伝播時間差もしくは位相差から擬似距離差ｒ_ｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）を算出する。

方向余弦ベクトル算出器５では、予め設定された送信機の位置情報から式（５４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、擬似距離差ｒ_ｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）と方向余弦ベクトルＡを式（５１）に代入して、最小二乗測位解ｕ_ｋ，ｍアンダーバーを算出する。受信機組み合わせ変更器１６により受信機の組み合わせを変えて、以上の測位演算と距離残差の算出をＭ回繰り返し、その測位解ｕ_ｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）をそれぞれ測位解記憶器７に記憶する。

距離残差記憶器１５では、最小二乗測位解算出器６から、擬似距離差ｒ_ｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）と測位解ｆ_ｉ，ｊ（ｘハット，ｙハット，ｚハット）を入力し、式（５６）に従い距離差残差の２乗ノルムρ_ｍ ^２を算出し、これを記憶する。ゲート判定器１３では、距離残差記憶器１５から入力した距離差残差の２乗ノルムρ_ｍ ^２と、予め設定された距離差残差の確率分布の分散μ^２とから、測位解記憶器７に記憶された測位解ｕ_ｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）の中から、式（４７）を満たすものを選択し、満たさないものを排除する。

測位解信頼度算出器１４では、距離差残差の２乗ノルムρ_ｍ ^２と、予め設定された距離差残差の確率分布の分散μ^２をゲート判定器１３を介して入力し、式（４７）を満たす全ての測位解に対し、測位解の信頼度β_ｋ，ｍを、式（４８）〜式（４９）に従い算出するとともに、この信頼度で測位解を重み付けした平均値ｕ_ｋアンダーバーを受信機の推定位置として式（５０）に従い算出する。

以上のようにして、位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、受信機位置での送信機の信号間の伝播時間差または位相差を計測して送信機位置を測位する方式に本発明を応用しても、実施の形態６と同様に、送信機の位置をより高精度に測位することができる。

実施の形態１３．
図１３は、本実施の形態１３による位置推定装置を示す構成図であり、図において、図１と同一符号である１、２、４〜６、８、１０、１１は実施の形態１と同一または相当部分を示すのでその説明を省略する。

１８は予測距離残差確率密度算出器２０の出力する予測距離残差の確率密度関数を入力し、受信機２の出力する各送信機と受信機との間の距離のうち測位に適する距離を選択して送信機を選択する距離による送信機選択器、１９は予測器８の出力する予測位置を各送信機と受信機との間の予測距離に変換する距離変換器、２０は距離変換器１９の出力する各送信機と受信機との間の予測距離を入力し、予測距離残差の確率密度関数を算出する予測距離残差確率密度算出器である。

次に動作について説明する。
最初に、この実施の形態１３による位置推定装置の動作原理を説明する。なお、ここでは、３次元位置を推定する場合を仮定する。

測位対象の運動を、過去の測位結果を用いて予測する方法は、実施の形態１と同じである。

次に、マルチパス等の影響を受けていない測位に適した送信機１を判定する方法について述べる。実施の形態１では、送信機組み合わせ変更器３により送信機１の組み合わせを変えて算出した複数の測位解を判定及び選択の対象としているが、この実施の形態１３では、測位解を求める前の段階、すなわち、受信機２の出力する送受信機１、２間の距離を判定及び選択の対象とする。

ｉ番目の送信機１に対する予測距離ｒ_ｋ，ｉ（−）は、式（２９）より式（５７）で表される。もし、送信機１の一部がマルチパスの影響を受けていたり、伝播経路上に問題があると、その距離の予測残差は大きな値となる。そこで、予測距離と、ｉ番目の送信機１と受信機２の擬似距離ｒ_ｋ，ｉ（ｉ＝１，２，・・・，Ｎ）との差である予測残差ｄ’_ｋ，ｉがパラメータξに対して式（５８）を満たす擬似距離を測位の対象として選択する。これにより、送信機が正常で伝播経路上にも問題がない送信機の組み合わせを決定することができ、これらの擬似距離を用いて測位を行うことにより受信機２の位置をより高精度に推定することができる。

一方、送信機を選択する別な方法として距離の予測残差の確率分布を利用する方法がある。過去の測位情報Ｕ_ｋ−１より得られる距離残差の予測確率分布Ｐ［ｒ_ｋ，ｉ｜Ｕ_ｋ―１］は、式（５９）に示す条件付確率密度関数で表される。すなわち、距離残差は、式（５７）で与えられる予測距離ｒ_ｋ，ｉ（−）を平均とし、式（６０）で与えられるＳ’_ｋ，ｉを分散とする１次元正規分布ｇ_１（ｒ_ｋ，ｉ；ｒ_ｋ，ｉ（−），Ｓ’_ｋ，ｉ）に従うとする。そして、式（６１）を満たす擬似距離を測位に利用し、満たさない擬似距離を排除する。ここで、ｑ’は目標との相関範囲を決めるパラメータであり、自由度１のχ自乗分布により算出する。Ｈ_２，ｉ（（（ｘ_ｋアンダーバー）ハット）（−））は、３次元直交座標の誤差共分散をｉ番目の送信機１と受信機２の距離の分散に変換する行列であり、式（６２）及び式（６３）に示すように３次元直交座標の誤差を予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）の周りで線形近似することによって得る。式（６３）の行列の要素の算出式は式（３０）〜式（３２）を参照する。

以上、距離による送信機１の選択方法について述べた。

次に、前述の方法で選択された送信機１の擬似距離を用いて、受信機２の位置を測位する。この原理は従来の技術における非特許文献１と同じであるため省略する。

次に、前述の測位結果を用いて平滑ベクトルを算出する。この原理は実施の形態１と同じであるため省略する。

以上、実施の形態１３による位置推定装置の動作原理について説明した。

次に、実施の形態１３による位置推定装置の具体的な動作を図１３に従い説明する。
複数の送信機１から発射された電波は、送信機１から受信機２への距離に基づく遅延時間後に受信機２に到達する。受信機２では各々の送信機１からの信号を弁別して受信する。擬似距離算出器４では、送信機１の信号の伝播遅延時間から擬似距離ｒ_ｋ，ｉ（ｉ＝１，２，・・・，Ｎ）を算出する。

距離による送信機選択器１８では、予測距離ｒ_ｋ，ｉ（−）と距離の予測残差の確率密度関数の分散Ｓ’_ｋ，ｉを予測距離残差確率密度算出器２０から入力し、擬似距離ｒ_ｋ，ｉ（ｉ＝１，２，・・・，Ｎ）の中から、式（６１）を満たすものを選択し、満たさないものを排除する。

方向余弦ベクトル算出器５では、予め設定された送信機の位置情報から式（４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、距離による送信機選択器１８で選択された擬似距離ｒ_ｋ，ｉと方向余弦ベクトルＡを式（１０）に代入して、最小二乗測位解ｕ_ｋアンダーバーを算出する。

平滑器１０では、最小二乗測位解算出器６で算出した測位解ｕ_ｋアンダーバーと、予測器８で算出した予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐ_ｋ（−）を用いて平滑ベクトル（ｘ_ｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列（Ｐ_ｋ（＋））を算出する。

予測器８では、１サンプリング前の平滑ベクトル（ｘ_ｋ−１アンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐ_ｋ−１（＋）を遅延回路１１を介して入力し、式（３３）〜式（３５）に従い予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）、予測誤差共分散行列Ｐ_ｋ（−）、位置予測ベクトル（ｕ_ｋアンダーバー）（−）をそれぞれ算出する。距離変換機１９では、予測器８で算出した予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）を式（５７）に従い距離の予測ベクトルに変換する。

予測距離残差確率密度算出器２０では、予測器８から予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）と予測誤差共分散行列Ｐ_ｋ（−）を入力し、これと、予め設定された送信機１と受信機２の距離の観測誤差分散σ_ｋ，ｉとから、式（６０）に従い、距離の予測残差の確率密度関数Ｐ［ｒ_ｋ，ｉ｜Ｕ_ｋ−１］の分散Ｓ’_ｋ，ｉを算出する。

以上のように、本実施の形態は、距離の予測残差もしくは距離の予測残差の確率密度関数を利用して選択した送信機１の擬似距離を用いて受信機２の位置を推定するので、動作が異常であったり伝播経路上に問題がある送信機の影響による測位精度の劣化を防ぐことができる。また、実施の形態１〜１２のように、複数の測角解を求める必要がないため演算負荷が軽い。

実施の形態１４．
図１４は、本実施の形態１４による位置推定装置を示す構成図であり、図において、図１と同一符号である１、２、４〜６、８、１０、１１は実施の形態１と、図１３と同一符号である１８〜２０は実施の形態１３と同一または相当部分を示すのでその説明を省略する。

２１は、距離による送信機選択器１８において選択された送信機の数によって、平滑する方法を切り替えるフィルタ切り替え器、２２は、距離の観測行列を算出する距離観測行列算出器、２３は、距離による送信機選択器１８の出力する擬似距離と、距離観測行列算出器２２の出力する距離の観測行列と、予測器の出力する前サンプリング時刻の予測ベクトルと予測誤差共分散行列とを入力し、平滑ベクトルを算出する距離による位置平滑器である。

次に動作について説明する。
最初に、この実施の形態１４による位置推定装置の動作原理を説明する。なお、ここでは、３次元位置を推定する場合を仮定する。

測位対象の運動を、過去の測位結果を用いて予測する方法は、実施の形態１と同じである。

また、マルチパス等の影響を受けていない測位に適した送信機１を判定し、選択する方法は実施の形態１３と同じである。

さて、上述の実施の形態１３では、測位に適した送信機１を選択した結果、その数が３次元測位を行う上で最低必要な４個未満の場合には測位できない。この場合は、選択された擬似距離を直接用いて平滑ベクトルを算出する。次にこの方法について説明する。
選択された送信機１に対応する擬似距離をｒ_ｋ，ｉ（ｉ＝１，２，・・・，ｌ）とし、これを式（６４）に示すようにベクトル表記する。この場合、観測モデルは式（６５）で表される。

式（６５）で表される距離の観測モデルは非線形であるため、通常のカルマンフィルタでは平滑ベクトルを算出できない。そこで、式（６７）及び式（６８）で示すように、この観測モデルを予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）の周りで線形近似した拡張カルマンフィルタを利用する。

拡張カルマンフィルタの理論より、ゲイン行列Ｋ_ｋ、平滑ベクトル（（ｘ_ｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐ_ｋ（＋）は、式（６９）〜式（７１）で与えられる。以上、平滑ベクトルの算出方法について述べた。

次に、送信機１を選択した結果、その数が４個以上の場合の処理について説明する。この場合、非特許文献１と同様に選択した送信機１の擬似距離を用いて、受信機２の位置を測位する。この原理は従来の技術における非特許文献１と同じであるため省略する。また、この測位結果を用いて平滑ベクトルを算出する原理は実施の形態１と同じであるため省略する。

以上、実施の形態１４による位置推定装置の動作原理について説明した。

次に、実施の形態１４による位置推定装置の具体的な動作を図１４に従い説明する。
複数の送信機１から発射された電波は、送信機１から受信機２への距離に基づく遅延時間後に受信機２に到達する。受信機２では各々の送信機１からの信号を弁別して受信する。擬似距離算出器４では、送信機１の信号の伝播遅延時間から擬似距離ｒ_ｋ，ｉ（ｉ＝１，２，・・・，Ｎ）を算出する。

距離による送信機選択器１８では、予測距離ｒ_ｋ，ｉ（−）と距離の予測残差の確率密度関数の分散Ｓ’_ｋ，ｉを予測距離残差確率密度算出器２０から入力し、擬似距離ｒ_ｋ，ｉ（ｉ＝１，２，・・・，Ｎ）の中から、式（６１）を満たすものを選択し、満たさないものを排除する。

フィルタ切り替え器２１では、距離による送信機選択器１８で送信機１を選択した結果、その数が測位を行う上で十分な数である４個以上の場合に方向余弦ベクトル算出器５に処理を移行し、４個未満の場合に距離観測行列算出器２２に処理を移行する。

方向余弦ベクトル算出器５では、距離による送信機選択器１８で選択された送信機１の数が４個以上の場合に、予め設定された送信機の位置情報から式（４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、距離による送信機選択器１８で選択された擬似距離ｒ_ｋ，ｉと方向余弦ベクトルＡを式（１０）に代入して、最小二乗測位解ｕ_ｋアンダーバーを算出する。

平滑器１０では、最小二乗測位解算出器６で算出した測位解ｕ_ｋアンダーバーと、予測器８で算出した予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐ_ｋ（−）を用いて平滑ベクトル（（ｘ_ｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐ_ｋ（＋）を算出する。

距離観測行列算出器２２では、距離による送信機選択器１８で選択された送信機１の数が４個未満の場合に、予測器８で算出した予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）から式（６８）に従い距離の観測行列Ｈ_２（（（ｘ_ｋアンダーバー）ハット）（−））を算出する。

距離による位置平滑器２３では、距離による送信機選択器１８で選択された擬似距離ベクトルｒ_ｋアンダーバーと、距離観測行列算出器２２で算出した距離の観測行列Ｈ_２（（（ｘ_ｋアンダーバー）ハット）（−））と、予測器８で算出した予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐ_ｋ（−）を用いて平滑ベクトル（（ｘ_ｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐ_ｋ（＋）を算出する。

予測器８では、距離による送信機選択器１８で選択された送信機１の数が４個以上の場合には平滑器１０で算出した遅延回路１１を介して入力し、一方、選択された送信機１の数が４個未満の場合には距離による位置平滑器２３で算出した１サンプリング前の平滑ベクトル（（ｘ_ｋアンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐ_ｋ−１（＋）を遅延回路１１を介して入力し、式（３３）〜式（３５）に従い予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）、予測誤差共分散行列Ｐ_ｋ（−）、位置予測ベクトル（ｕ_ｋアンダーバー）（−）をそれぞれ算出する。距離変換機１９では、予測器８で算出した予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）を式（５７）に従い距離の予測ベクトルに変換する。

予測距離残差確率密度算出器２０では、予測器８から予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）と予測誤差共分散行列Ｐ_ｋ（−）を入力し、これと、予め設定された送信機１と受信機２の距離の観測誤差分散σ_ｋ，ｉとから、式（６０）に従い、距離の予測残差の確率密度関数Ｐ［ｒ_ｋ，ｉ｜Ｕ_ｋ―１］の分散Ｓ’_ｋ，ｉを算出する。

以上のように本実施の形態は、距離の予測残差もしくは距離の予測残差の確率密度関数を利用して選択した送信機１の擬似距離を用いて受信機２の位置を推定するので、動作が異常であったり伝播経路上に問題がある送信機の影響による測位精度の劣化を防ぐことができる。また、実施の形態１〜１２のように、複数の測角解を求める必要がないため演算負荷が軽い。
更に、上述の実施の形態１３では、距離による送信機選択器１８において選択された送信機の数が４個未満の場合に測位ができない問題があるが、本実施例では距離による位置平滑器２３において、擬似距離を直接用いて位置を平滑する構成としているので、測位ができない場合においても、精度よく受信機の位置を推定することができる。

実施の形態１５．
上記の実施の形態１３では、位置が既知である複数の送信機１からの電波を、位置が未知である受信機２で受信して、送信機１から受信機２までの伝播時間を計測して受信機位置を測位する方式への本発明の応用例を示したが、本実施の形態は、逆に、位置が未知である送信機１からの電波を、位置が既知である複数の受信機２で受信して、送信機１から受信機２までの伝播時間を計測して送信機位置を測位する方式へ本発明を適用した適用例について説明する。

図１５はこの実施の形態１５による位置推定装置を示す構成図であり、図において、図１３と同一符号である１、２、４〜６、８、１０、１１、１９、２０は実施の形態１３と同一または相当部分を示すのでその説明を省略する。

２４は予測距離残差確率密度算出器２０の出力する予測距離残差の確率密度関数を入力し、各受信機２の出力する送信機と各受信機の間の擬似距離のうち測位に適する擬似距離を選択する距離による受信機選択器である。

次に、実施の形態１５による位置推定装置の具体的な動作を図１５に従い説明する。送信機１から発射された電波は、送信機１から受信機２への距離に基づく遅延時間後に受信機２に到達する。擬似距離算出器４では送信機１の信号の伝播遅延時間から擬似距離ｒ_ｋ，ｉ（ｉ＝１，２，・・・，Ｎ）を算出する。

距離による受信機選択器２４では、予測距離ｒ_ｋ，ｉ（−）と距離の予測残差の確率密度関数の分散Ｓ’_ｋ，ｉを予測距離残差確率密度算出器２０から入力し、擬似距離ｒ_ｋ，ｉ（ｉ＝１，２，・・・，Ｎ）の中から、式（６１）を満たすものを選択し、満たさないものを排除する。

方向余弦ベクトル算出器５では、予め設定された受信機の位置情報から式（４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、距離による送信機選択器１８で選択された擬似距離ｒ_ｋ，ｉと方向余弦ベクトルＡを式（１０）に代入して、最小二乗測位解ｕ_ｋアンダーバーを算出する。

平滑器１０では、最小二乗測位解算出器６で算出した測位解ｕ_ｋアンダーバーと、予測器８で算出した予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐ_ｋ（−）を用いて平滑ベクトル（（ｘ_ｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐ_ｋ（＋）を算出する。

予測器８では、１サンプリング前の平滑ベクトル（（ｘ_ｋアンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐ_ｋ―１（＋）を遅延回路１１を介して入力し、式（３３）〜式（３５）に従い予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）、予測誤差共分散行列Ｐ_ｋ（−）、位置予測ベクトル（ｕ_ｋアンダーバー）（−）をそれぞれ算出する。距離変換機１９では、予測器８で算出した予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）を式（５７）に従い距離の予測ベクトルに変換する。

予測距離残差確率密度算出器２０では、予測器８から予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）と予測誤差共分散行列Ｐ_ｋ（−）を入力し、これと、予め設定された送信機１と受信機２の距離の観測誤差分散σ_ｋ，ｉとから、式（６０）に従い、距離の予測残差の確率密度関数Ｐ［ｒ_ｋ，ｉ｜Ｕ_ｋ―１］の分散Ｓ’_ｋ，ｉを算出する。

以上のように、位置が未知である送信機１からの電波を、位置が既知である複数の受信機２で受信して、送信機１から受信機２までの伝播時間を計測して送信機位置を測位する方式に本発明を応用しても、実施の形態１３と同様に、送信機１の位置をより高精度に測位することができる。

実施の形態１６．
上記の実施の形態１４では、位置が既知である複数の送信機１からの電波を、位置が未知である受信機２で受信して、送信機１から受信機２までの伝播時間を計測して受信機位置を測位する方式への応用例であるが、本発明は、位置が未知である送信機１からの電波を、位置が既知である複数の受信機２で受信して、送信機１から受信機２までの伝播時間を計測して送信機位置を測位する方式にも適用できるので、本実施の形態においては、当該適用例について説明する。

図１６はこの実施の形態１６による位置推定装置を示す構成図であり、図において、図１４と同一符号である１、２、４〜６、８、１０、１１、１９〜２３は実施の形態１４と同一または相当部分を示すのでその説明を省略する。

２４は予測距離残差確率密度算出器２０の出力する予測距離残差の確率密度関数を入力し、各受信機２の出力する送信機と各受信機の間の擬似距離のうち測位に適する擬似距離を選択する距離による受信機選択器である。

次に、実施の形態１６による位置推定装置の具体的な動作を図１６に従い説明する。送信機１から発射された電波は、送信機１から受信機２への距離に基づく遅延時間後に受信機２に到達する。擬似距離算出器４では送信機１の信号の伝播遅延時間から擬似距離ｒ_ｋ，ｉ（ｉ＝１，２，・・・，Ｎ）を算出する。

距離による受信機選択器２４では、予測距離ｒ_ｋ，ｉ（−）と距離の予測残差の確率密度関数の分散Ｓ’_ｋ，ｉを予測距離残差確率密度算出器２０から入力し、擬似距離ｒ_ｋ，ｉ（ｉ＝１，２，・・・，Ｎ）の中から、式（６１）を満たすものを選択し、満たさないものを排除する。

フィルタ切り替え器２１では、距離による受信機選択器２４で受信機２を選択した結果、その数が測位を行う上で十分な数である４個以上の場合に方向余弦ベクトル算出器５に処理を移行し、４個未満の場合に距離観測行列算出器２２に処理を移行する。

方向余弦ベクトル算出器５では、距離による受信機選択器２４で選択された受信機２の数が４個以上の場合に、予め設定された受信機の位置情報から式（４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、距離による受信機選択器２４で選択された擬似距離ｒ_ｋ，ｉと方向余弦ベクトルＡを式（１０）に代入して、最小二乗測位解ｕ_ｋアンダーバーを算出する。

平滑器１０では、最小二乗測位解算出器６で算出した測位解ｕ_ｋアンダーバーと、予測器８で算出した予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐ_ｋ（−）を用いて平滑ベクトル（（ｘ_ｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐ_ｋ（＋）を算出する。

距離観測行列算出器２２では、距離による受信機選択器２４で選択された受信機２の数が４個未満の場合に、予測器８で算出した予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）から式（６８）に従い距離の観測行列Ｈ_２（（（ｘ_ｋアンダーバー）ハット）（−））を算出する。

距離による位置平滑器２３では、距離による受信機選択器２４で選択された擬似距離ベクトルｒ_ｋアンダーバーと、距離観測行列算出器２２で算出した距離の観測行列Ｈ_２（（（ｘ_ｋアンダーバー）ハット）（−））と、予測器８で算出した予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐ_ｋ（−）を用いて平滑ベクトル（（ｘ_ｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐ_ｋ（＋）を算出する。

予測器８では、距離による受信機選択器２４で選択された受信機２の数が４個以上の場合には平滑器１０で算出した遅延回路１１を介して入力し、一方、選択された受信機２の数が４個未満の場合には距離による位置平滑器２３で算出した１サンプリング前の平滑ベクトル（（ｘ_ｋアンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐ_ｋ―１（＋）を遅延回路１１を介して入力し、式（３３）〜式（３５）に従い予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）、予測誤差共分散行列Ｐ_ｋ（−）、位置予測ベクトルｕ_ｋアンダーバー（−）をそれぞれ算出する。距離変換機１９では、予測器８で算出した予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）を式（５７）に従い距離の予測ベクトルに変換する。

予測距離残差確率密度算出器２０では、予測器８から予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）と予測誤差共分散行列Ｐ_ｋ（−）を入力し、これと、予め設定された送信機１と受信機２の距離の観測誤差分散σ_ｋ，ｉとから、式（６０）に従い、距離の予測残差の確率密度関数Ｐ［ｒ_ｋ，ｉ｜Ｕ_ｋ―１］の分散Ｓ’_ｋ，ｉを算出する。

以上のように、位置が未知である送信機１からの電波を、位置が既知である複数の受信機２で受信して、送信機１から受信機２までの伝播時間を計測して送信機位置を測位する方式に本発明を応用しても、実施の形態１４と同様に、送信機１の位置をより高精度に測位することができる。

実施の形態１７．
上述の実施の形態１３は、位置が既知である複数の送信機１からの電波を、位置が未知である受信機２で受信して、送信機１から受信機２までの伝播時間を計測して受信機位置を測位する方式において、電波の伝播時間を用いた測位方式に本発明を適用したものであるが、本発明は、複数の送信機１から受信機２への電波の伝播時間差や位相差を用いた測位方式にも適用できるので、本実施の形態においては、当該適用例について説明する。

図１７はこの実施の形態１７による位置推定装置を示す構成図であり、図において、図１３と同一符号である１、２、５、６、８、１０、１１、１７は実施の形態１３と同一または相当部分を示すのでその説明を省略する。

２５は予測距離差残差確率密度算出器２７の出力する予測距離差残差の確率密度関数を入力し、受信機２の出力する各送信機と受信機の間の擬似距離差のうち測位に適する擬似距離差を選択する距離差による送信機選択器、２６は予測器８の出力する予測位置を各送信機と受信機の予測距離差に変換する距離差変換器、２７は距離差変換器２６の出力する各送信機と受信機の予測距離差を入力し、予測距離差残差の確率密度関数を算出する予測距離差残差確率密度算出器である。

複数の送信機１から受信機２への電波の伝播時間差や位相差を用いてマルチパス等の影響を受けていない測位に適した送信機１を判定する方法について述べる。

ｉ番目の送信機１に対する予測距離差Δｒ_{ｋ，ｉ，ｊ}（−）は既知であるｉ番目の送信機位置（ｘ_ｉ，ｙ_ｉ，ｚ_ｉ）と、同じく既知であるｊ番目の発信機位置（ｘ_ｊ，ｙ_ｊ，ｚ_ｊ）から、式（７２）を用いて算出する。もし、送信機１の一部がマルチパスの影響を受けていたり、伝播経路上に問題があると、その距離差の予測残差は大きな値となる。そこで、予測距離差と、ｉ番目の送信機１と受信機２の擬似距離差Δｒ_{ｋ，ｉ，ｊ}（ｉ＝１，２，・・・，Ｎ）との差である予測残差τ_ｋ，ｉがパラメータηに対して式（７３）を満たす擬似距離差を測位の対象として選択する。これにより、送信機が正常で伝播経路上にも問題がない送信機の組み合わせを決定することができ、これらの擬似距離差を用いて測位を行うことにより受信機２の位置をより高精度に推定することができる。

一方、送信機を選択する別な方法として距離差の予測残差の確率分布を利用する方法がある。過去の測位情報Ｕ_ｋ−１より得られる距離差残差の予測確率分布Ｐ［Δｒ_{ｋ，ｉ、ｊ}｜Ｕ_ｋ―１］は、式（７４）に示す条件付確率密度関数で表される。すなわち、距離差残差は、式（７２）で与えられる予測距離差Δｒ_{ｋ，ｉ、ｊ}（−）を平均とし、式（７５）で与えられるΩ_{ｋ，ｉ，ｊ}を分散とする１次元正規分布ｇ_１（Δｒ_{ｋ，ｉ、ｊ}；Δｒ_{ｋ，ｉ、ｊ}（−），Ω_{ｋ，ｉ，ｊ}）に従うとする。そして、式（７６）を満たす擬似距離差を測位に利用し、満たさない擬似距離差を排除する。ここで、κは目標との相関範囲を決めるパラメータであり、自由度１のχ自乗分布により算出する。Ｈ_{３，ｉ，ｊ}（（（ｘ_ｋアンダーバー）ハット）（−））は、３次元直交座標の誤差共分散をｉ番目の送信機と受信機２の距離と、ｊ番目の送信機と受信機２の距離との差の分散に変換する行列であり、式（７７）及び式（７８）に示すように３次元直交座標の誤差を予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）の周りで線形近似することによって得る。

以上、距離差による送信機１の選択方法について述べた。

なお、前述の方法で選択された２つの送信機と受信機２の擬似距離差を用いて、受信機２の位置を測位する原理は実施の形態１７と同じ原理であるため省略する。

次に、実施の形態１７による位置推定装置の具体的な動作を図１７に従い説明する。
複数の送信機１から発射された電波は、送信機１から受信機２への距離に基づく遅延時間後に受信機２に到達する。受信機２では各々の送信機１からの信号を弁別して受信する。擬似距離差算出器１７では、２つの送信機１の信号の伝播時間差もしくは位相差から擬似距離差Δｒ_{ｋ，ｉ，ｊ}（ｉ，ｊ＝１，２，・・・，Ｎ）を算出する。

距離差による送信機選択器２５では、予測距離差Δｒ_{ｋ，ｉ，ｊ}（−）と擬似距離差の予測残差の確率密度関数の分散Ω_{ｋ，ｉ，ｊ}を予測距離差残差確率密度算出器２７から入力し、擬似距離差Δｒ_{ｋ，ｉ，ｊ}（ｉ，ｊ＝１，２，・・・，Ｎ）の中から、式（７６）を満たすものを選択し、満たさないものを排除する。

方向余弦ベクトル算出器５では、予め設定された送信機の位置情報から式（５４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、距離差による送信機選択器２５で選択された擬似距離差Δｒ_{ｋ，ｉ，ｊ}と方向余弦ベクトルＡを式（５１）に代入して、最小二乗測位解ｕ_ｋアンダーバーを算出する。

平滑器１０では、最小二乗測位解算出器６で算出した測位解ｕ_ｋアンダーバーと、予測器８で算出した予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐ_ｋ（−）を用いて、式（３７）〜式（３９）に従い平滑ベクトル（（ｘ_ｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐ_ｋ（＋）を算出する。

予測器８では、１サンプリング前の平滑ベクトル（（ｘ_ｋアンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐ_ｋ−１（＋）を遅延回路１１を介して入力し、式（３３）〜式（３５）に従い予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）、予測誤差共分散行列Ｐ_ｋ（−）、位置予測ベクトル（ｕ_ｋアンダーバー）（−）をそれぞれ算出する。距離差変換機２６では、予測器８で算出した予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）を式（７２）に従い距離差の予測ベクトルに変換する。

予測距離差残差確率密度算出器２７では、予測器８から予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）と予測誤差共分散行列Ｐ_ｋ（−）を入力し、これと、予め設定された２つの送信機と受信機２の距離差の観測誤差分散σ’_{ｋ，ｉ，ｊ}とから、式（７５）に従い、距離差の予測残差の確率密度関数Ｐ［Δｒ_{ｋ，ｉ，ｊ}｜Ｕ_ｋ―１］の分散Ω_ｋ，ｉを算出する。

以上のようにして、位置が既知である複数の送信機１から送信された電波を、位置が未知である受信機２で受信して、受信機位置での各送信機の信号間の伝播時間差または位相差を計測して受信機位置を測位する方式に本発明を応用しても、実施の形態１３と同様に、受信機２の位置をより高精度に測位することができる。

実施の形態１８．
上記の実施の形態１４は、位置が既知である複数の送信機１からの電波を、位置が未知である受信機２で受信して、送信機１から受信機２までの伝播時間を計測して受信機位置を測位する方式において、電波の伝播時間を用いた測位方式に本発明を適用したものであるが、本発明は、複数の送信機１から受信機２への電波の伝播時間差や位相差を用いた測位方式にも適用できるので、本実施の形態においては、当該適用例について説明する。

図１８はこの実施の形態１８による位置推定装置を示す構成図であり、図において、図１４と同一符号である１、２、５、６、８、１０、１１、１７は実施の形態１３と同一または相当部分を示すのでその説明を省略する。

２５は予測距離差残差確率密度算出器２７の出力する予測距離差残差の確率密度関数を入力し、受信機２の出力する各送信機と受信機の間の擬似距離差のうち測位に適する擬似距離差を選択する距離差による送信機選択器、２６は予測器８の出力する予測位置を各送信機と受信機の予測距離差に変換する距離差変換器、２７は距離差変換器２６の出力する各送信機と受信機の予測距離差を入力し、予測距離差残差の確率密度関数を算出する予測距離差残差確率密度算出器、２８は距離差の観測行列を算出する距離差観測行列算出器、２９は距離差による送信機選択器２５の出力する擬似距離差と、距離差観測行列算出器２８の出力する距離差の観測行列と、予測器８の出力する前サンプリング時刻の予測ベクトルと予測誤差共分散行列とを入力し、平滑ベクトルを算出する距離差による位置平滑器である。

距離差による送信機１の選択方法、及び、選択された２つの送信機と受信機２の擬似距離差を用いて、受信機２の位置を測位する原理は実施の形態１７と同じ原理であるため省略する。

次に距離差の観測行列の算出方法及び距離差による位置平滑器の動作原理について説明する。
選択された送信機１に対応する擬似距離差をΔｒ_{ｋ，ｉ，ｊ}（ｉ，ｊ＝１，２，・・・，ｌ）とし、これを式（７９）に示すようにベクトル表記する。この場合、観測モデルは式（８０）で表される。

式（８０）で表される距離差の観測モデルは非線形であるため、通常のカルマンフィルタでは平滑ベクトルを算出できない。そこで、式（８２）及び式（８３）で示すように、この観測モデルを予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）の周りで線形近似した拡張カルマンフィルタを利用する。

拡張カルマンフィルタの理論より、ゲイン行列Ｋ_ｋ、平滑ベクトル（（ｘ_ｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐ_ｋ（＋）は、式（８４）〜式（８６）で与えられる。以上、平滑ベクトルの算出方法について述べた。

次に、実施の形態１８による位置推定装置の具体的な動作を図１８に従い説明する。
複数の送信機１から発射された電波は、送信機１から受信機２への距離に基づく遅延時間後に受信機２に到達する。受信機２では各々の送信機１からの信号を弁別して受信する。擬似距離差算出器１７では、２つの送信機１の信号の伝播時間差もしくは位相差から擬似距離差Δｒ_{ｋ，ｉ，ｊ}（ｉ，ｊ＝１，２，・・・，Ｎ）を算出する。

距離差による送信機選択器２５では、予測距離差Δｒ_{ｋ，ｉ，ｊ}（−）と擬似距離差の予測残差の確率密度関数の分散Ω_{ｋ，ｉ，ｊ}を予測距離差残差確率密度算出器２７から入力し、擬似距離差Δｒ_{ｋ，ｉ，ｊ}（ｉ，ｊ＝１，２，・・・，Ｎ）の中から、式（７６）を満たすものを選択し、満たさないものを排除する。

フィルタ切り替え器２１では、距離差による送信機選択器２５で送信機１のペアを選択した結果、そのペアの数が測位を行う上で十分な数である４個以上の場合に方向余弦ベクトル算出器５に処理を移行し、４個未満の場合に距離差観測行列算出器２８に処理を移行する。

方向余弦ベクトル算出器５では、距離差による送信機選択器２５で選択された送信機１の数が４個以上の場合に、予め設定された送信機の位置情報から式（５４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、距離差による送信機選択器２５で選択された擬似距離差Δｒ_{ｋ，ｉ，ｊ}と方向余弦ベクトルＡを式（５１）に代入して、最小二乗測位解ｕ_ｋアンダーバーを算出する。

平滑器１０では、最小二乗測位解算出器６で算出した測位解ｕ_ｋアンダーバーと、予測器８で算出した予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐ_ｋ（−）を用いて、式（３７）〜式（３９）に従い平滑ベクトル（（ｘ_ｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐ_ｋ（＋）を算出する。

距離差観測行列算出器２８では、距離差による送信機選択器２５で選択された送信機１の数が４個未満の場合に、予測器８で算出した予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）から式（８３）に従い距離差の観測行列Ｈ_４（（（ｘ_ｋアンダーバー）ハット）（−））を算出する。

距離差による位置平滑器２９では、距離差による送信機選択器２５で選択された擬似距離差ベクトルΔｒ_ｋアンダーバーと、距離差観測行列算出器２８で算出した距離差の観測行列Ｈ_４（（（ｘ_ｋアンダーバー）ハット）（−））と、予測器８で算出した予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐ_ｋ（−）を用いて、式（８４）〜式（８６）に従い平滑ベクトル（（ｘ_ｋアンダーバー）ハット）（＋））及び平滑誤差共分散行列Ｐ_ｋ（＋）を算出する。

予測器８では、距離差による送信機選択器２５で選択された送信機１の数が４個以上の場合には平滑器１０で算出した遅延回路１１を介して入力し、一方、選択された送信機１の数が４個未満の場合には距離差による位置平滑器２９で算出した１サンプリング前の平滑ベクトル（（ｘ_ｋアンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐ_ｋ―１（＋）を遅延回路１１を介して入力し、式（３３）〜式（３５）に従い予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）、予測誤差共分散行列Ｐ_ｋ（−）、位置予測ベクトルｕ_ｋアンダーバー（−）をそれぞれ算出する。距離差変換機２６では、予測器８で算出した予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）を式（７２）に従い距離差の予測ベクトルに変換する。

予測距離差残差確率密度算出器２７では、予測器８から予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）と予測誤差共分散行列Ｐ_ｋ（−）を入力し、これと、予め設定された２つの送信機と受信機２の距離差の観測誤差分散σ’_{ｋ，ｉ，ｊ}とから、式（７５）に従い、距離差の予測残差の確率密度関数Ｐ［Δｒ_{ｋ，ｉ，ｊ}｜Ｕ_ｋ―１］の分散Ω_ｋ，ｉを算出する。

以上のようにして、位置が既知である複数の送信機１から送信された電波を、位置が未知である受信機２で受信して、受信機位置での各送信機の信号間の伝播時間差または位相差を計測して受信機位置を測位する方式に本発明を応用しても、実施の形態１４と同様に、受信機２の位置をより高精度に測位することができる。

実施の形態１９．
上記の実施の形態１７では、位置が既知である複数の送信機１からの電波を、位置が未知である受信機２で受信して、送信機１から受信機２への電波の伝播時間差や位相差を計測して受信機位置を測位する方式への本発明の応用例を示したが、本実施の形態は、逆に、位置が未知である送信機１からの電波を、位置が既知である複数の受信機２で受信して、送信機１から受信機２送信機１から受信機２への電波の伝播時間差や位相差を計測して送信機位置を測位する方式へ本発明を適用した適用例について説明する。

図１９はこの実施の形態１９による位置推定装置を示す構成図であり、図において、図１７と同一符号である１、２、５、６、８、１０、１１、１７、２６、２７は実施の形態１７と同一または相当部分を示すのでその説明を省略する。

３０は予測距離差残差確率密度算出器２７の出力する予測距離差残差の確率密度関数を入力し、各受信機２の出力する送信機１と受信機２の擬似距離差のうち測位に適する擬似距離差を選択する距離差による受信機選択器である。

次に、実施の形態１９による位置推定装置の具体的な動作を図１９に従い説明する。送信機１から発射された電波は、送信機１から受信機２への距離に基づく遅延時間後に受信機２に到達する。擬似距離差算出器１７では、２つの送信機１の信号の伝播時間差もしくは位相差から擬似距離差Δｒ_{ｋ，ｉ，ｊ}（ｉ，ｊ＝１，２，・・・，Ｎ）を算出する。

距離差による受信機選択器３０では、予測距離差Δｒ_{ｋ，ｉ，ｊ}（−）と擬似距離差の予測残差の確率密度関数の分散Ω_{ｋ，ｉ，ｊ}を予測距離差残差確率密度算出器２７から入力し、擬似距離差Δｒ_{ｋ，ｉ，ｊ}（ｉ，ｊ＝１，２，・・・，Ｎ）の中から、式（７６）を満たすものを選択し、満たさないものを排除する。

方向余弦ベクトル算出器５では、予め設定された受信機の位置情報から式（５４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、距離差による受信機選択器３０で選択された擬似距離差Δｒ_{ｋ，ｉ，ｊ}と方向余弦ベクトルＡを式（５１）に代入して、最小二乗測位解ｕ_ｋアンダーバーを算出する。

平滑器１０では、最小二乗測位解算出器６で算出した測位解ｕ_ｋアンダーバーと、予測器８で算出した予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐ_ｋ（−）を用いて、式（３７）〜式（３９）に従い平滑ベクトル（（ｘ_ｋアンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐ_ｋ（＋）を算出する。

予測器８では、１サンプリング前の平滑ベクトル（（ｘ_ｋ−１アンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐ_ｋ−１（＋）を遅延回路１１を介して入力し、式（３３）〜式（３５）に従い予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）、予測誤差共分散行列Ｐ_ｋ（−）、位置予測ベクトル（（ｕ_ｋアンダーバー）ハット）（−）をそれぞれ算出する。距離差変換機２６では、予測器８で算出した予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）を式（７２）に従い距離差の予測ベクトルに変換する。

予測距離差残差確率密度算出器２７では、予測器８から予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）と予測誤差共分散行列Ｐ_ｋ（−）を入力し、これと、予め設定された２つの送信機と受信機２の距離差の観測誤差分散σ’_{ｋ，ｉ，ｊ}とから、式（７５）に従い、距離差の予測残差の確率密度関数Ｐ［Δｒ_{ｋ，ｉ，ｊ}｜Ｕ_ｋ―１］の分散Ω_ｋ，ｉを算出する。

以上のようにして、位置が未知である送信機１から送信された電波を、位置が既知である複数の受信機２で受信して、受信機位置での送信機１の信号間の伝播時間差または位相差を計測して送信機位置を測位する方式に本発明を応用しても、実施の形態１７と同様に、送信機１の位置をより高精度に測位することができる。

実施の形態２０．
上記の実施の形態１８では、位置が既知である複数の送信機１からの電波を、位置が未知である受信機２で受信して、送信機１から受信機２までの伝播時間を計測して受信機位置を測位する方式への応用例であるが、本実施の形態は、逆に、位置が未知である送信機１からの電波を、位置が既知である複数の受信機２で受信して、送信機１から受信機２送信機１から受信機２への電波の伝播時間差や位相差を計測して送信機位置を測位する方式へ本発明を適用した適用例について説明する。

図２０はこの実施の形態２０による位置推定装置を示す構成図であり、図において、図１８と同一符号である１、２、５、６、８、１０、１１、１７、２６〜２９は実施の形態１８と同一または相当部分を示すのでその説明を省略する。

３０は予測距離差残差確率密度算出器２７の出力する予測距離差残差の確率密度関数を入力し、各受信機２の出力する送信機と各受信機の間の擬似距離差のうち測位に適する擬似距離差を選択する距離差による受信機選択器である。

次に、実施の形態２０による位置推定装置の具体的な動作を図２０に従い説明する。送信機１から発射された電波は、送信機１から受信機２への距離に基づく遅延時間後に受信機２に到達する。擬似距離差算出器１７では、２つの送信機１の信号の伝播時間差もしくは位相差から擬似距離差Δｒ_{ｋ，ｉ，ｊ}（ｉ，ｊ＝１，２，・・・，Ｎ）を算出する。

距離差による受信機選択器３０では、予測距離差Δｒ_{ｋ，ｉ，ｊ}（−）と擬似距離差の予測残差の確率密度関数の分散Ω_{ｋ，ｉ，ｊ}を予測距離差残差確率密度算出器２７から入力し、擬似距離差Δｒ_{ｋ，ｉ，ｊ}（ｉ，ｊ＝１，２，・・・，Ｎ）の中から、式（７６）を満たすものを選択し、満たさないものを排除する。

フィルタ切り替え器２１では、距離差による受信機選択器３０で受信機１のペアを選択した結果、そのペアの数が測位を行う上で十分な数である４個以上の場合に方向余弦ベクトル算出器５に処理を移行し、４個未満の場合に距離差観測行列算出器２８に処理を移行する。

方向余弦ベクトル算出器５では、距離差による受信機選択器３０で選択された受信機２の数が４個以上の場合に、予め設定された送信機の位置情報から式（５４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、距離差による受信機選択器３０で選択された擬似距離差Δｒ_{ｋ，ｉ，ｊ}と方向余弦ベクトルＡを式（５１）に代入して、最小二乗測位解ｕ_ｋアンダーバーを算出する。

平滑器１０では、最小二乗測位解算出器６で算出した測位解と、予測器８で算出した予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）と予測誤差共分散行列Ｐ_ｋ（−）を用いて、式（３７）〜式（３９）に従い、平滑ベクトル（（ｘ_ｋアンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐ_ｋ（＋）を算出する。

距離差観測行列算出器２８では、距離差による受信機選択器３０で選択された受信機２の数が４個未満の場合に、予測器８で算出した予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）から式（８３）に従い距離差の観測行列Ｈ_４（（（ｘ_ｋアンダーバー）ハット）（−））を算出する。

距離差による位置平滑器２９では、距離差による受信機選択器３０で選択された擬似距離差ベクトルΔｒ_ｋアンダーバーと、距離差観測行列算出器２８で算出した距離差の観測行列Ｈ_４（（（ｘ_ｋアンダーバー）ハット）（−））と、予測器８で算出した予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐ_ｋ（−）を用いて、式（８４）〜式（８６）に従い、平滑ベクトル（（ｘ_ｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐ_ｋ（＋）を算出する。

予測器８では、距離差による受信機選択器３０で選択された受信機２の数が４個以上の場合には平滑器１０で算出した遅延回路１１を介して入力し、一方、選択された受信機２の数が４個未満の場合には距離差による位置平滑器２９で算出した１サンプリング前の平滑ベクトル（（ｘ_ｋ−１アンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐ_ｋ−１（＋）を遅延回路１１を介して入力し、式（３３）〜式（３５）に従い予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）、予測誤差共分散行列Ｐ_ｋ（−）、位置予測ベクトル（ｕ_ｋアンダーバー）（−）をそれぞれ算出する。距離差変換機２６では、予測器８で算出した予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）を式（７２）に従い距離差の予測ベクトルに変換する。

予測距離差残差確率密度算出器２７では、予測器８から予測ベクトル（（ｘ_ｋアンダーバー）ハット）（−）と予測誤差共分散行列Ｐ_ｋ（−）を入力し、これと、予め設定された２つの送信機と受信機２の距離差の観測誤差分散σ’_{ｋ，ｉ，ｊ}とから、式（７５）に従い、距離差の予測残差の確率密度関数Ｐ［Δｒ_{ｋ，ｉ，ｊ}｜Ｕ_ｋ―１］の分散Ω_ｋ，ｉを算出する。

以上のようにして、位置が未知である送信機１から送信された電波を、位置が既知である複数の受信機２で受信して、受信機位置での送信機１の信号間の伝播時間差または位相差を計測して送信機位置を測位する方式に本発明を応用しても、実施の形態１８と同様に、送信機１の位置をより高精度に測位することができる。

この発明の実施の形態１に係る位置推定装置の構成を示したブロック図である。 この発明の実施の形態２に係る位置推定装置の構成を示したブロック図である。 この発明の実施の形態３に係る位置推定装置の構成を示したブロック図である。 この発明の実施の形態４に係る位置推定装置の構成を示したブロック図である。 この発明の実施の形態５に係る位置推定装置の構成を示したブロック図である。 この発明の実施の形態６に係る位置推定装置の構成を示したブロック図である。 この発明の実施の形態７に係る位置推定装置の構成を示したブロック図である。 この発明の実施の形態８に係る位置推定装置の構成を示したブロック図である。 この発明の実施の形態９に係る位置推定装置の構成を示したブロック図である。 この発明の実施の形態１０に係る位置推定装置の構成を示したブロック図である。 この発明の実施の形態１１に係る位置推定装置の構成を示したブロック図である。 この発明の実施の形態１２に係る位置推定装置の構成を示したブロック図である。 この発明の実施の形態１３に係る位置推定装置の構成を示したブロック図である。 この発明の実施の形態１４に係る位置推定装置の構成を示したブロック図である。 この発明の実施の形態１５に係る位置推定装置の構成を示したブロック図である。 この発明の実施の形態１６に係る位置推定装置の構成を示したブロック図である。 この発明の実施の形態１７に係る位置推定装置の構成を示したブロック図である。 この発明の実施の形態１８に係る位置推定装置の構成を示したブロック図である。 この発明の実施の形態１９に係る位置推定装置の構成を示したブロック図である。 この発明の実施の形態２０に係る位置推定装置の構成を示したブロック図である。

符号の説明

１ 送信機、２ 受信機、３ 送信機組み合わせ変更器、４ 疑似距離算出器、５ 方向余弦ベクトル算出器、６ 最小二乗測位解算出器、７ 測位解記憶器、８ 予測器、９ 測位解選択器、１０ 平滑器、１１ 遅延回路、１２ 予測残差確率密度算出器、１３ ゲート判定器、１４ 測位解信頼度算出器、１５ 距離残差記憶器、１６ 受信機組み合わせ変更器、１７ 疑似距離差算出器、１８ 距離による送信機選択器、１９ 距離変換器、２０ 予測距離残差確率密度算出器、２１ フィルタ切り替え器、２２ 距離観測行列算出器、２３ 距離による位置平滑器、２４ 距離による受信機選択器、２５ 距離差による送信機選択器、２６ 距離差変換器、２７ 予測距離差残差確率密度算出器、２８ 距離差観測行列算出器、２９ 距離差による位置平滑器、３０ 距離差による受信機選択器。

Claims (36)

1. 位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に受信機位置を推定する位置推定装置であって、
   受信可能な全送信機の中から、受信機位置の測定に必要な所定の最低数以上の送信機の組み合わせを生成する送信機組み合わせ変更手段と、
   上記送信機組み合わせ変更手段による上記送信機の組み合わせ毎に、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、上記受信機位置の測位解を算出する測位解算出手段と、
   過去に計測された受信機の位置から現在の受信機の位置を予測する予測手段と、
   上記予測手段で算出された予測位置と上記測位解算出手段で算出された上記測位解との予測残差に基づいて、上記送信機組み合わせ変更手段による上記送信機の組み合わせを選択する選択手段と
   を備えたことを特徴とする位置推定装置。
2. 位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に受信機位置を推定する位置推定装置であって、
   受信可能な全送信機の中から、受信機位置の測定に必要な所定の最低数以上の送信機の組み合わせを生成する送信機組み合わせ変更手段と、
   上記送信機組み合わせ変更手段による上記送信機の組み合わせ毎に、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、上記受信機位置の測位解を算出する測位解算出手段と、
   上記送信機組み合わせ変更手段による上記送信機の組み合わせ毎に、上記測位解算出手段で算出された上記測位解の予測確率分布を算出する予測確率分布算出手段と、
   上記予測確率分布における上記測位解の確率密度を基に、上記送信機組み合わせ変更手段による上記送信機の組み合わせを選択する選択手段と
   を備えたことを特徴とする位置推定装置。
3. 位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に受信機位置を推定する位置推定装置であって、
   受信可能な全送信機の中から、受信機位置の測定に必要な所定の最低数以上の送信機の組み合わせを生成する送信機組み合わせ変更手段と、
   上記送信機組み合わせ変更手段による上記送信機の組み合わせ毎に、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、上記受信機位置の測位解を算出する測位解算出手段と、
   上記送信機組み合わせ変更手段による上記送信機の組み合わせ毎に、上記測位解算出手段による上記測位解の予測確率分布を算出する予測確率分布算出手段と、
   上記予測確率分布における上記測位解の確率密度に基いて、上記測位解の信頼度を算出する信頼度算出手段と、
   上記信頼度により上記測位解を重み付けする重み付け手段と
   を備えたことを特徴とする位置推定装置。
4. 位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に受信機位置を推定する位置推定装置であって、
   受信可能な全送信機の中から、受信機位置の測定に必要な所定の最低数以上の送信機の組み合わせを生成する送信機組み合わせ変更手段と、
   上記送信機から上記受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に、最小二乗法を用いて上記受信機位置の測位解を算出する最小二乗測位解算出手段と、
   上記送信機組み合わせ変更手段による上記送信機の組み合わせ毎に、送信機から受信機までの伝播時間から計算される伝播距離と上記測位解との距離残差の確率分布を算出する確率分布算出手段と、
   上記確率分布における上記距離残差の確率密度を基に、上記送信機組み合わせ変更手段による上記送信機の組み合わせを選択する選択手段と
   を備えたことを特徴とする位置推定装置。
5. 位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に受信機位置を推定する位置推定装置であって、
   受信可能な全送信機の中から、受信機位置の測定に必要な所定の最低数以上の送信機の組み合わせを生成する送信機組み合わせ変更手段と、
   上記送信機から上記受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に、最小二乗法を用いて上記受信機位置の測位解を算出する最小二乗測位解算出手段と、
   上記送信機組み合わせ変更手段による上記送信機の組み合わせ毎に、送信機から受信機までの伝播時間から計算される伝播距離と上記測位解との距離残差の確率分布を算出する確率分布算出手段と、
   上記確率分布における上記距離残差の確率密度を基に、上記測位解の信頼度を算出する信頼度算出手段と、
   上記信頼度により測位解を重み付けする重み付け手段と
   を備えたことを特徴とする位置推定装置。
6. 位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に送信機位置を推定する位置推定装置であって、
   受信可能な全受信機の中から、送信機位置の測定に必要な所定の最低数以上の受信機の組み合わせを生成する受信機組み合わせ変更手段と、
   上記受信機組み合わせ変更手段による上記受信機の組み合わせ毎に、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、上記送信機位置の測位解を算出する測位解算出手段と、
   過去に計測された送信機の位置から現在の位置を予測する予測手段と、
   上記予測手段で算出された予測位置と上記測位解算出手段で算出された上記測位解との予測残差に基づいて、上記受信機組み合わせ変更手段による上記受信機の組み合わせを選択する選択手段と
   を備えたことを特徴とする位置推定装置。
7. 位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に送信機位置を推定する位置推定装置であって、
   受信可能な全受信機の中から、送信機位置の測定に必要な所定の最低数以上の受信機の組み合わせを変更する受信機組み合わせ変更手段と、
   上記受信機組み合わせ変更手段による上記受信機の組み合わせ毎に、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、上記送信機位置の測位解を算出する測位解算出手段と、
   上記受信機組み合わせ変更手段による上記受信機の組み合わせ毎に、上記測位解算出手段で算出された上記測位解の予測確率分布を算出する予測確率分布算出手段と、
   上記予測確率分布における上記測位解の確率密度を基に、上記受信機組み合わせ変更手段による上記受信機の組み合わせを選択する選択手段と
   を備えたことを特徴とする位置推定装置。
8. 位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に送信機位置を推定する位置推定装置であって、
   受信可能な全受信機の中から、送信機位置の測定に必要な所定の最低数以上の受信機の組み合わせを生成する受信機組み合わせ変更手段と、
   上記受信機組み合わせ変更手段による上記受信機の組み合わせ毎に、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、上記送信機位置の測位解を算出する測位解算出手段と、
   上記受信機組み合わせ変更手段による上記受信機の組み合わせ毎に、上記測位解算出手段による上記測位解の予測確率分布を算出する予測確率分布算出手段と、
   上記予測確率分布における上記測位解の確率密度に基いて、上記測位解の信頼度を算出する信頼度算出手段と、
   上記信頼度により上記測位解を重み付けする重み付け手段と
   を備えたことを特徴とする位置推定装置。
9. 位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に送信機位置を推定する位置推定装置であって、
   受信可能な全受信機の中から、送信機位置の測定に必要な所定の最低数以上の受信機の組み合わせを生成する受信機組み合わせ変更手段と、
   上記送信機から上記受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に、最小二乗法を用いて上記送信機位置の測位解を算出する最小二乗測位解算出手段と、
   上記受信機組み合わせ変更手段による上記受信機の組み合わせ毎に、送信機から受信機までの伝播時間から計算される伝播距離と上記測位解との距離残差の確率分布を算出する確率分布算出手段と、
   上記確率分布における上記距離残差の確率密度を基に、上記受信機組み合わせ変更手段による上記受信機の組み合わせを選択する選択手段と
   を備えたことを特徴とする位置推定装置。
10. 位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に送信機位置を推定する位置推定装置であって、
    受信可能な全受信機の中から、送信機位置の測定に必要な所定の最低数以上の受信機の組み合わせを生成する受信機組み合わせ変更手段と、
    上記送信機から上記受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に、最小二乗法を用いて上記送信機位置の測位解を算出する最小二乗測位解算出手段と、
    上記受信機組み合わせ変更手段による上記受信機の組み合わせ毎に、送信機から受信機までの伝播時間から計算される伝播距離と上記測位解との距離残差の確率分布を算出する確率分布算出手段と、
    上記確率分布における上記距離残差の確率密度を基に、上記測位解の信頼度を算出する信頼度算出手段と、
    上記信頼度により上記測位解を重み付けする重み付け手段と
    を備えたことを特徴とする位置推定装置。
11. 位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に受信機位置を推定する位置推定装置であって、
    受信可能な全送信機の中から、受信機位置の測定に必要な所定の最低数以上の送信機の組み合わせを生成する送信機組み合わせ変更手段と、
    上記送信機組み合わせ変更手段による上記送信機の組み合わせ毎に、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に上記受信機位置の測位解を算出する測位解算出手段と、
    過去に計測された受信機の位置から現在の位置を予測する予測手段と、
    上記予測手段で算出された予測位置と上記測位解算出手段で算出された上記測位解との予測残差に基づいて、上記送信機組み合わせ変更手段による上記送信機の組み合わせを選択する選択手段と
    を備えたことを特徴とする位置推定装置。
12. 位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に受信機位置を推定する位置推定装置であって、
    受信可能な全送信機の中から、受信機位置の測定に必要な所定の最低数以上の送信機の組み合わせを生成する送信機組み合わせ変更手段と、
    上記送信機組み合わせ変更手段による上記送信機の組み合わせ毎に、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に上記受信機位置の測位解を算出する測位解算出手段と、
    上記送信機組み合わせ変更手段による上記送信機の組み合わせ毎に、上記測位解算出手段で算出された上記測位解の予測確率分布を算出する予測確率分布算出手段と、
    上記予測確率分布における上記測位解の確率密度を基に、上記送信機組み合わせ変更手段による上記送信機の組み合わせを選択する選択手段と
    を備えたことを特徴とする位置推定装置。
13. 位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に受信機位置を推定する位置推定装置であって、
    受信可能な全送信機の中から、受信機位置の測定に必要な所定の最低数以上の送信機の組み合わせを生成する送信機組み合わせ変更手段と、
    上記送信機組み合わせ変更手段による上記送信機の組み合わせ毎に、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に上記受信機位置の測位解を算出する測位解算出手段と、
    上記送信機組み合わせ変更手段による上記送信機の組み合わせ毎に、上記測位解算出手段による上記測位解の予測確率分布を算出する予測確率分布算出手段と、
    上記予測確率分布における上記測位解の確率密度に基いて、上記測位解の信頼度を算出する信頼度算出手段と、
    上記信頼度により上記測位解を重み付けする重み付け手段と
    を備えたことを特徴とする位置推定装置。
14. 位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に受信機位置を推定する位置推定装置であって、
    受信可能な全送信機の中から、受信機位置の測定に必要な所定の最低数以上の送信機の組み合わせを生成する送信機組み合わせ変更手段と、
    受信機位置での各送信機からの信号間の伝播時間差または位相差に基づいて、最小二乗法を用いて上記受信機位置の測位解を算出する最小二乗測位解算出手段と、
    上記送信機組み合わせ変更手段による上記送信機の組み合わせ毎に、上記伝播時間差または上記位相差から計算される伝播距離差と上記測位解との距離残差の確率分布を算出する確率分布算出手段と、
    上記確率分布における上記距離残差の確率密度を基に、上記送信機組み合わせ変更手段による上記送信機の組み合わせを選択する選択手段と
    を備えたことを特徴とする位置推定装置。
15. 位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に受信機位置を推定する位置推定装置であって、
    受信可能な全送信機の中から、受信機位置の測定に必要な所定の最低数以上の送信機の組み合わせを生成する送信機組み合わせ変更手段と、
    受信機位置での各送信機からの信号間の伝播時間差または位相差に基づいて、最小二乗法を用いて上記受信機位置の測位解を算出する最小二乗測位解算出手段と、
    上記送信機組み合わせ変更手段による上記送信機の組み合わせ毎に、上記伝播時間差または上記位相差から計算される伝播距離差と上記測位解との距離残差の確率分布を算出する確率分布算出手段と、
    上記確率分布における上記距離残差の確率密度を基に、上記測位解の信頼度を算出する信頼度算出手段と、
    上記信頼度により測位解を重み付けする重み付け手段と
    を備えたことを特徴とする位置推定装置。
16. 位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、各受信機における受信信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に送信機位置を推定する位置推定装置であって、
    受信可能な全受信機の中から、送信機位置の測定に必要な所定の最低数以上の受信機の組み合わせを生成する受信機組み合わせ変更手段と、
    上記受信機組み合わせ変更手段による上記受信機の組み合わせ毎に、各受信機における受信信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に上記送信機位置の測位解を算出する測位解算出手段と、
    過去に計測された送信機の位置から現在の位置を予測する予測手段と、
    上記予測手段で算出された予測位置と上記測位解算出手段で算出された上記測位解との予測残差に基づいて、上記受信機組み合わせ変更手段による上記受信機の組み合わせを選択する選択手段と
    を備えたことを特徴とする位置推定装置。
17. 位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、各受信機における受信信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に送信機位置を推定する位置推定装置であって、
    受信可能な全受信機の中から、送信機位置の測定に必要な所定の最低数以上の受信機の組み合わせを変更する受信機組み合わせ変更手段と、
    上記受信機組み合わせ変更手段による上記受信機の組み合わせ毎に、各受信機における受信信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に上記送信機位置の測位解を算出する測位解算出手段と、
    上記受信機組み合わせ変更手段による上記受信機の組み合わせ毎に、上記測位解算出手段で算出された上記測位解の予測確率分布を算出する予測確率分布算出手段と、
    上記予測確率分布における上記測位解の確率密度を基に、上記受信機組み合わせ変更手段による上記受信機の組み合わせを選択する選択手段と
    を備えたことを特徴とする位置推定装置。
18. 位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、各受信機における受信信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に送信機位置を推定する位置推定装置であって、
    受信可能な全受信機の中から、送信機位置の測定に必要な所定の最低数以上の受信機の組み合わせを生成する受信機組み合わせ変更手段と、
    上記受信機組み合わせ変更手段による上記受信機の組み合わせ毎に、各受信機における受信信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に上記送信機位置の測位解を算出する測位解算出手段と、
    上記受信機組み合わせ変更手段による上記受信機の組み合わせ毎に、上記測位解算出手段による上記測位解の予測確率分布を算出する予測確率分布算出手段と、
    上記予測確率分布における上記測位解の確率密度を基いて、上記測位解の信頼度を算出する信頼度算出手段と、
    上記信頼度により上記測位解を重み付けする重み付け手段と
    を備えたことを特徴とする位置推定装置。
19. 位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、各受信機における受信信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に送信機位置を推定する位置推定装置であって、
    受信可能な全受信機の中から、送信機位置の測定に必要な所定の最低数以上の受信機の組み合わせを生成する受信機組み合わせ変更手段と、
    各受信機における受信信号間の伝播時間差または位相差に基づいて、最小二乗法を用いて上記送信機位置の測位解を算出する最小二乗測位解算出手段と、
    上記受信機組み合わせ変更手段による上記受信機の組み合わせ毎に、各受信機における受信信号間の伝播時間差または位相差を計測して、上記伝播時間差または上記位相差から計算される伝播距離差と上記測位解との距離残差の確率分布を算出する確率分布算出手段と、
    上記確率分布における上記距離残差の確率密度を基に、上記受信機組み合わせ変更手段による上記送信機の組み合わせを選択する選択手段と
    を備えたことを特徴とする位置推定装置。
20. 位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、各受信機における受信信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に送信機位置を推定する位置推定装置であって、
    受信可能な全受信機の中から、送信機位置の測定に必要な所定の最低数以上の受信機の組み合わせを生成する受信機組み合わせ変更手段と、
    上記受信機における受信信号間の伝播時間差または位相差に基づいて、最小二乗法を用いて上記送信機位置の測位解を算出する最小二乗測位解算出手段と、
    上記受信機組み合わせ変更手段による上記受信機の組み合わせ毎に、各受信機における受信信号間の伝播時間差または位相差を計測して、上記伝播時間差または上記位相差から計算される伝播距離差と上記測位解との距離残差の確率分布を算出する確率分布算出手段と、
    上記確率分布における上記距離残差の確率密度を基に、上記測位解の信頼度を算出する信頼度算出手段と、
    上記信頼度により上記測位解を重み付けする重み付け手段と
    を備えたことを特徴とする位置推定装置。
21. 位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に受信機位置を推定する位置推定装置であって、
    過去に計測された受信機の位置から現在の各送信機と受信機の間の観測距離を予測する予測手段と、
    上記予測手段で算出された予測距離と観測された送受信機間の距離との予測残差に基づいて、送信機を選択する選択手段と、
    上記選択手段で選択された送信機を用いて、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、上記受信機位置の測位解を算出する測位解算出手段と
    を備えたことを特徴とする位置推定装置。
22. 位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に受信機位置を推定する位置推定装置であって、
    過去に計測された受信機の位置から現在の各送信機と受信機の間の距離を予測する予測手段と、
    上記予測手段で算出された予測距離と観測された送受信機間の観測距離との予測残差の確率分布を算出する予測確率分布算出手段と、
    上記予測確率分布における予測残差の確率密度に基づいて、送信機を選択する選択手段と、
    上記選択手段で選択された送信機を用いて、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、上記受信機位置の測位解を算出する測位解算出手段と
    を備えたことを特徴とする位置推定装置。
23. 位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に受信機位置を推定する位置推定装置であって、
    過去に計測された受信機の位置から現在の各送信機と受信機の間の距離を予測する予測手段と、
    上記予測手段で算出された予測距離と観測された送受信機間の観測距離との予測残差に基づいて、送信機を選択する選択手段と、
    上記選択手段で選択された送信機の数に応じて、第１および第２の平滑手段のいずれか一方に切り替えるフィルタ切り替え手段と、
    上記選択手段で選択された送信機を用いて、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、上記受信機位置の測位解を算出する測位解算出手段と、
    上記選択手段で選択された送信機を用いて、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、受信機の位置を推定する距離による位置推定手段と
    を備え、
    上記第１の平滑手段は、上記測位解と上記予測距離とに基づいて、平滑距離を算出して、上記予測手段に出力し、
    上記第２の平滑手段は、疑似距離と上記予測距離と上記観測距離とに基づいて、１サンプリング前の平滑距離を算出して、上記予測手段に出力する
    ことを特徴とする位置推定装置。
24. 位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に受信機位置を推定する位置推定装置であって、
    過去に計測された受信機の位置から現在の各送信機と受信機の間の距離を予測する予測手段と、
    上記予測手段で算出された予測距離と観測された送受信機間の観測距離との予測残差の確率分布を算出する予測確率分布算出手段と、
    上記予測確率分布における予測残差の確率密度に基づいて、送信機を選択する選択手段と、
    上記選択手段で選択された送信機の数に応じて、第１および第２の平滑手段のいずれか一方に切り替えるフィルタ切り替え手段と、
    上記選択手段で選択された送信機を用いて、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、上記受信機位置の測位解を算出する測位解算出手段と、
    上記選択手段で選択された送信機を用いて、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、受信機の位置を推定する距離による位置推定手段と
    を備え、
    上記第１の平滑手段は、上記測位解と上記予測距離とに基づいて、平滑距離を算出して、上記予測手段に出力し、
    上記第２の平滑手段は、疑似距離と上記予測距離と上記観測距離とに基づいて、１サンプリング前の平滑距離を算出して、上記予測手段に出力する
    ことを特徴とする位置推定装置。
25. 位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に送信機位置を推定する位置推定装置であって、
    過去に計測された送信機の位置から現在の各受信機と送信機の間の距離を予測する予測手段と、
    上記予測手段で算出された予測距離と観測された送受信機間の観測距離との予測残差に基づいて、受信機を選択する選択手段と、
    上記選択手段で選択された受信機を用いて、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、上記送信機位置の測位解を算出する測位解算出手段と
    を備えたことを特徴とする位置推定装置。
26. 位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に送信機位置を推定する位置推定装置であって、
    過去に計測された送信機の位置から現在の各送信機と受信機の間の距離を予測する予測手段と、
    上記予測手段で算出された予測距離と観測された送受信機間の観測距離との予測残差の確率分布を算出する予測確率分布算出手段と、
    上記予測確率分布における予測残差の確率密度に基づいて、受信機を選択する選択手段と、
    上記選択手段で選択された受信機を用いて、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、上記送信機位置の測位解を算出する測位解算出手段と
    を備えたことを特徴とする位置推定装置。
27. 位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に送信機位置を推定する位置推定装置であって、
    過去に計測された送信機の位置から現在の各受信機と送信機の間の距離を予測する予測手段と、
    上記予測手段で算出された予測距離と観測された送受信機間の観測距離との予測残差に基づいて、受信機を選択する選択手段と、
    上記選択手段で選択された受信機の数に応じて、第１および第２の平滑手段のいずれか一方に切り替えるフィルタ切り替え手段と、
    上記選択手段で選択された受信機を用いて、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、上記送信機位置の測位解を算出する測位解算出手段と、
    上記選択手段で選択された受信機を用いて、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、送信機の位置を推定する距離による位置推定手段と、
    を備え、
    上記第１の平滑手段は、上記測位解と上記予測距離とに基づいて、平滑距離を算出して、上記予測手段に出力し、
    上記第２の平滑手段は、疑似距離と上記予測距離と上記観測距離とに基づいて、１サンプリング前の平滑距離を算出して、上記予測手段に出力する
    ことを特徴とする位置推定装置。
28. 位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に送信機位置を推定する位置推定装置であって、
    過去に計測された送信機の位置から現在の各送信機と受信機の間の距離を予測する予測手段と、
    上記予測手段で算出された予測距離と観測された送受信機間の観測距離との予測残差の確率分布を算出する予測確率分布算出手段と、
    上記予測確率分布における予測残差の確率密度に基づいて、受信機を選択する選択手段と、
    上記選択手段で選択された受信機の数に応じて、第１および第２の平滑手段のいずれか一方に切り替えるフィルタ切り替え手段と、
    上記選択手段で選択された受信機を用いて、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、上記送信機位置の測位解を算出する測位解算出手段と、
    上記選択手段で選択された受信機を用いて、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、送信機の位置を推定する距離による位置推定手段と
    を備え、
    上記第１の平滑手段は、上記測位解と上記予測距離とに基づいて、平滑距離を算出して、上記予測手段に出力し、
    上記第２の平滑手段は、疑似距離と上記予測距離と上記観測距離とに基づいて、１サンプリング前の平滑距離を算出して、上記予測手段に出力する
    ことを特徴とする位置推定装置。
29. 位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に受信機位置を推定する位置推定装置であって、
    過去に計測された受信機の位置から現在の受信機位置での各送信機からの信号間の伝搬時間差または位相差を予測する予測手段と、
    上記予測手段で算出された予測時間差または予測位相差と観測された時間差または位相差との予測残差に基づいて、送信機を選択する選択手段と、
    上記選択手段で選択された送信機を用いて、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に上記受信機位置の測位解を算出する測位解算出手段と
    を備えたことを特徴とする位置推定装置。
30. 位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に受信機位置を推定する位置推定装置であって、
    過去に計測された受信機の位置から現在の受信機位置での各送信機からの信号間の伝搬時間差または位相差を予測する予測手段と、
    上記予測手段で算出された予測時間差または予測位相差と観測された時間差または位相差との予測残差の確率分布を算出する予測確率分布算出手段と、
    上記予測確率分布における予測残差の確率密度に基づいて、送信機を選択する選択手段と、
    上記選択手段で選択された送信機を用いて、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に上記受信機位置の測位解を算出する測位解算出手段と
    を備えたことを特徴とする位置推定装置。
31. 位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に受信機位置を推定する位置推定装置であって、
    過去に計測された受信機の位置から現在の受信機位置での各送信機からの信号間の伝搬時間差または位相差を予測する予測手段と、
    上記予測手段で算出された予測時間差または予測位相差と観測された時間差または位相差との予測残差に基づいて、送信機を選択する選択手段と、
    上記選択手段で選択された送信機の数に応じて、第１および第３の平滑手段のいずれか一方に切り替えるフィルタ切り替え手段と、
    上記選択手段で選択された送信機を用いて、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に上記受信機位置の測位解を算出する測位解算出手段と、
    上記選択手段で選択された送信機を用いて、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に、受信機の位置を推定する時間差または位相差による位置推定手段と、
    を備え、
    上記第１の平滑手段は、上記測位解と上記予測距離とに基づいて、平滑距離を算出して、上記予測手段に出力し、
    上記第３の平滑手段は、疑似距離差と上記予測距離と上記観測距離の距離差とに基づいて、１サンプリング前の平滑距離を算出して、上記予測手段に出力する
    ことを特徴とする位置推定装置。
32. 位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に受信機位置を推定する位置推定装置であって、
    過去に計測された受信機の位置から現在の受信機位置での各送信機からの信号間の伝搬時間差または位相差を予測する予測手段と、
    上記予測手段で算出された予測時間差または予測位相差と観測された時間差または位相差との予測残差の確率分布を算出する予測確率分布算出手段と、
    上記予測確率分布における予測残差の確率密度に基づいて、送信機を選択する選択手段と、
    上記選択手段で選択された送信機の数に応じて、第１および第２の平滑手段のいずれか一方に切り替えるフィルタ切り替え手段と、
    上記選択手段で選択された送信機を用いて、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に、上記受信機位置の測位解を算出する測位解算出手段と、
    上記選択手段で選択された送信機を用いて、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に、受信機の位置を推定する時間差または位相差による位置推定手段と、
    を備え、
    上記第１の平滑手段は、上記測位解と上記予測距離とに基づいて、平滑距離を算出して、上記予測手段に出力し、
    上記第３の平滑手段は、疑似距離差と上記予測距離と上記観測距離の距離差とに基づいて、１サンプリング前の平滑距離を算出して、上記予測手段に出力する
    ことを特徴とする位置推定装置。
33. 位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、各受信機における受信信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に送信機位置を推定する位置推定装置であって、
    過去に計測された送信機の位置から現在の受信機位置での各送信機からの信号間の伝搬時間差または位相差を予測する予測手段と、
    上記予測手段で算出された予測時間差または予測位相差と観測された時間差または位相差との予測残差に基づいて、受信機を選択する選択手段と、
    上記選択手段で選択された受信機を用いて、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に上記送信機位置の測位解を算出する測位解算出手段と、
    を備えたことを特徴とする位置推定装置。
34. 位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、各受信機における受信信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に送信機位置を推定する位置推定装置であって、
    過去に計測された送信機の位置から現在の受信機位置での各送信機からの信号間の伝搬時間差または位相差を予測する予測手段と、
    上記予測手段で算出された予測時間差または予測位相差と観測された時間差または位相差との予測残差の確率分布を算出する予測確率分布算出手段と、
    上記予測確率分布における予測残差の確率密度に基づいて、受信機を選択する選択手段と、
    上記選択手段で選択された受信機を用いて、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に上記送信機位置の測位解を算出する測位解算出手段と、
    を備えたことを特徴とする位置推定装置。
35. 位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、各受信機における受信信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に送信機位置を推定する位置推定装置であって、
    過去に計測された送信機の位置から現在の受信機位置での各送信機からの信号間の伝搬時間差または位相差を予測する予測手段と、
    上記予測手段で算出された予測時間差または予測位相差と観測された時間差または位相差との予測残差に基づいて、受信機を選択する選択手段と、
    上記選択手段で選択された受信機の数に応じて、第１および第３の平滑手段のいずれか一方に切り替えるフィルタ切り替え手段と、
    上記選択手段で選択された受信機を用いて、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に上記送信機位置の測位解を算出する測位解算出手段と、
    上記選択手段で選択された受信機を用いて、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に、送信機の位置を推定する時間差または位相差による位置推定手段と
    を備え、
    上記第１の平滑手段は、上記測位解と上記予測距離とに基づいて、平滑距離を算出して、上記予測手段に出力し、
    上記第３の平滑手段は、疑似距離差と上記予測距離と上記観測距離の距離差とに基づいて、１サンプリング前の平滑距離を算出して、上記予測手段に出力する
    ことを特徴とする位置推定装置。
36. 位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、各受信機における受信信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に送信機位置を推定する位置推定装置であって、
    過去に計測された送信機の位置から現在の受信機位置での各送信機からの信号間の伝搬時間差または位相差を予測する予測手段と、
    上記予測手段で算出された予測時間差または予測位相差と観測された時間差または位相差との予測残差の確率分布を算出する予測確率分布算出手段と、
    上記予測確率分布における予測残差の確率密度に基づいて、受信機を選択する選択手段と、
    上記選択手段で選択された受信機の数に応じて、第１および第３の平滑手段のいずれか一方に切り替えるフィルタ切り替え手段と、
    上記選択手段で選択された受信機を用いて、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に上記送信機位置の測位解を算出する測位解算出手段と、
    上記選択手段で選択された受信機を用いて、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に、送信機の位置を推定する時間差または位相差による位置推定手段と、
    を備え、
    上記第１の平滑手段は、上記測位解と上記予測距離とに基づいて、平滑距離を算出して、上記予測手段に出力し、
    上記第３の平滑手段は、疑似距離差と上記予測距離と上記観測距離の距離差とに基づいて、１サンプリング前の平滑距離を算出して、上記予測手段に出力する
    ことを特徴とする位置推定装置。

Images