JP6901864B2 - 測位計算装置、測位計算方法及び測位計算プログラム - Google Patents

Description

本発明は、衛星測位に関する。

ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ）等の衛星測位システム（ＧＮＳＳ：Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ）では、３次元座標値と測位計算装置（ＧＮＳＳ受信機）の時計誤差を求めるために、測位計算装置は、４機以上の測位衛星からの測位信号を連続して受信する必要がある。
しかし、都市部においては、高層ビル等の影響により衛星視界が遮られ、測位信号を受信できる衛星数が極端に減少し、測位精度が劣化するばかりではなく、測位そのものができない場合が頻発する。
この点につき、特許文献１では、測位信号を受信できる測位衛星数が３機の場合は、気圧計によって測定された高度を使用して測位を行う測位計算装置が開示されている。

特開平５−４５４３６号公報

特許文献１の技術では、測位計算装置に気圧計を備える必要があるため、部品点数が多くなるという課題がある。また、特許文献１の技術は、気圧計により測定された高度を使用するため、測位精度が劣化するという課題がある。

この発明は、上記のような課題を解決することを主な目的の一つとしており、測位信号を受信できる測位衛星数が３機以下の場合にも、高精度な測位が可能な測位計算装置の部品点数を増加させずに得ることを主な目的とする。

本発明に係る測位計算装置は、
測位衛星からの測位信号を受信して測位計算を行う測位計算装置であって、
時間外挿計算を行って、前記測位計算装置の推定三次元位置と誤差共分散とを算出する時間外挿計算部と、
前記測位信号から得られた観測値を用いた観測更新計算を行って、前記時間外挿計算部により算出された前記測位計算装置の推定三次元位置と誤差共分散とを更新する観測更新計算部と、
既定の供給元から位置情報あるいは姿勢情報とその分散を取得し、取得した前記位置情報あるいは姿勢情報と前記分散と前記観測更新計算部での更新により得られた前記測位計算装置の推定三次元位置あるいは姿勢と誤差共分散とに基づき、前記測位計算装置の推定三次元位置と誤差共分散とを再計算する再計算部と、
前記観測更新計算部での更新により得られた誤差共分散と前記再計算部での再計算により得られた誤差共分散とを比較し、前記観測更新計算部での更新により得られた前記測位計算装置の推定三次元位置と前記再計算部での再計算により得られた前記測位計算装置の推定三次元位置のうちのいずれかを選択し、選択した前記測位計算装置の推定三次元位置を出力する選択部とを有する。

本発明に係る測位計算装置は、
測位衛星からの測位信号を受信して測位計算を行う測位計算装置であって、
時間外挿計算を行って、前記測位計算装置の推定三次元位置と誤差共分散とを算出する時間外挿計算部と、
前記測位信号から得られた観測値を用いた観測更新計算を行って、前記時間外挿計算部により算出された前記測位計算装置の推定三次元位置と誤差共分散とを更新する観測更新計算部と、
前記観測更新計算部での更新により得られた推定三次元位置の水平位置に対応する高度値と前記高度値の誤差の分散とをデータベースから取得し、取得した前記高度値と前記分散と前記観測更新計算部での更新により得られた前記測位計算装置の推定三次元位置と誤差共分散とに基づき、前記測位計算装置の推定三次元位置と誤差共分散とを再計算する再計算部と、
前記観測更新計算部での更新により得られた誤差共分散と前記再計算部での再計算により得られた誤差共分散とを比較し、前記観測更新計算部での更新により得られた前記測位計算装置の推定三次元位置と前記再計算部での再計算により得られた前記測位計算装置の推定三次元位置のうちのいずれかを選択し、選択した前記測位計算装置の推定三次元位置を出力する選択部とを有する。

前記選択部は、
前記観測更新計算部での更新により得られた誤差共分散が前記再計算部での再計算により得られた誤差共分散よりも以下の場合は、前記観測更新計算部での更新により得られた前記測位計算装置の推定三次元位置を選択し、
前記再計算部での再計算により得られた誤差共分散が前記観測更新計算部での更新により得られた誤差共分散よりも小さい場合は、前記再計算部での再計算により得られた前記測位計算装置の推定三次元位置を選択する。

前記選択部は、
測位計算に使用した衛星数がある閾値より大きい場合は、前記観測更新計算部での更新により得られた前記測位計算装置の推定三次元位置を選択し、
測位計算に使用した衛星数がある閾値より小さい場合は、前記再計算部での再計算により得られた前記測位計算装置の推定三次元位置を選択する。

前記時間外挿計算部は、
前記選択部により選択された前記測位計算装置の推定三次元位置と、前記選択部により選択された前記測位計算装置の推定三次元位置に対応する誤差共分散とに基づき、時間外挿計算を行う。

本発明に係る測位計算方法は、
測位衛星からの測位信号を受信して測位計算を行うコンピュータが、時間外挿計算を行って、前記コンピュータの推定三次元位置と誤差共分散とを算出する時間外挿計算処理と、
前記コンピュータが、前記測位信号から得られた観測値を用いた観測更新計算を行って、前記時間外挿計算処理により算出された前記コンピュータの推定三次元位置と誤差共分散とを更新する観測更新計算処理と、
前記コンピュータが、前記観測更新計算処理での更新により得られた推定三次元位置の水平位置に対応する高度値と前記高度値の誤差の分散とをデータベースから取得し、取得した前記高度値と前記分散と前記観測更新計算処理での更新により得られた前記コンピュータの推定三次元位置と誤差共分散とに基づき、前記コンピュータの推定三次元位置と誤差共分散とを再計算する再計算処理と、
前記コンピュータが、前記観測更新計算処理での更新により得られた誤差共分散と前記再計算処理での再計算により得られた誤差共分散とを比較し、前記観測更新計算処理での更新により得られた前記コンピュータの推定三次元位置と前記再計算処理での再計算により得られた前記コンピュータの推定三次元位置のうちのいずれかを選択し、選択した前記コンピュータの推定三次元位置を出力する選択処理とを有する。

本発明に係る測位計算プログラムは、
測位衛星からの測位信号を受信して測位計算を行うコンピュータに、
時間外挿計算を行って、前記コンピュータの推定三次元位置と誤差共分散とを算出する時間外挿計算処理と、
前記測位信号から得られた観測値を用いた観測更新計算を行って、前記時間外挿計算処理により算出された前記コンピュータの推定三次元位置と誤差共分散とを更新する観測更新計算処理と、
前記観測更新計算処理での更新により得られた推定三次元位置の水平位置に対応する高度値と前記高度値の誤差の分散とをデータベースから取得し、取得した前記高度値と前記分散と前記観測更新計算処理での更新により得られた前記コンピュータの推定三次元位置と誤差共分散とに基づき、前記コンピュータの推定三次元位置と誤差共分散とを再計算する再計算処理と、
前記観測更新計算処理での更新により得られた誤差共分散と前記再計算処理での再計算により得られた誤差共分散とを比較し、前記観測更新計算処理での更新により得られた前記コンピュータの推定三次元位置と前記再計算処理での再計算により得られた前記コンピュータの推定三次元位置のうちのいずれかを選択し、選択した前記コンピュータの推定三次元位置を出力する選択処理とを実行させる。

本発明では、推定三次元位置の水平位置に対応する高度値と当該高度値の誤差の分散とをデータベースから取得し、取得した高度値と分散とを用いた推定三次元位置と誤差共分散とを再計算する。更に、本発明では、観測更新により得られた誤差共分散と再計算により得られた誤差行分散とを比較して、観測更新により得られた推定三次元位置と再計算により得られた推定三次元位置のうちのいずれかを選択する。
このため、本発明によれば、測位信号を受信できる測位衛星数が３機以下の場合にも、高精度な測位が可能な測位計算装置の部品点数を増加させずに得ることができる。

実施の形態１に係る測位システムの構成例を示す図。 実施の形態１に係る測位計算装置の構成例を示す図。 実施の形態１に係る測位計算部の構成例を示す図。 実施の形態１に係るカルマンフィルタの構成例を示す図。 実施の形態１に係る時間外挿計算、観測更新計算、再計算の詳細を示す図。 従来のカルマンフィルタの構成例と時間外挿計算及び観測更新計算の詳細を示す図。

以下、本発明の実施の形態について、図を用いて説明する。以下の実施の形態の説明及び図面において、同一の符号を付したものは、同一の部分または相当する部分を示す。

実施の形態１．
＊＊＊構成の説明＊＊＊
図１は、本実施の形態に係る測位システムの構成例を示す図である。
図１では、補強信号の配信に準天頂衛星を用いた例を示す。
準天頂衛星の代わりに放送衛星等の静止衛星を用いてもよく、また、補強信号を広域に均一に放送できる機能を持つ衛星であれば他の衛星でもよい。
また、図１では、測位衛星としてＧＰＳ衛星を使用した例を示す。
ＧＰＳ衛星の代わりに、ＧＬＯＮＡＳＳ、Ｇａｌｉｌｅｏ、ＢｅｉＤｏｕ等のＧＮＳＳ、準天頂衛星等のＲＮＳＳ（Ｒｅｇｉｏｎａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ）を用いてもよい。

図１において、測位計算装置１００は、例えば自動車などの移動体に搭載される。
測位計算装置１００は、ＧＰＳ衛星３００から送信される測位信号５００を受信する。
また、測位計算装置１００は、準天頂衛星２００（ＱＺＳとも表記する）から送信される補強信号４００を受信する。
準天頂衛星２００は、図１に図示していない地上局から補強信号を受信し、受信した補強信号を補強信号４００として配信する。
補強信号４００は、ＧＰＳ衛星３００に起因する誤差、信号伝搬経路に起因する誤差を通知する情報である。

ＧＰＳ衛星３００は、測位信号５００を送信する。
本実施の形態に係る測位計算装置１００は、ＧＰＳ衛星３００が３機以下の場合でも、高精度に測位を行うことができる。

図２は、測位計算装置１００の構成例を示す。
測位計算装置１００は、ハードウェアとして、プロセッサ９０１、記憶装置９０２を備える。
また、測位計算装置１００は、機能構成として、測位信号受信部１０１、補強信号受信部１０２、測位計算部１０３、地図表示部１０４及び地図データベース１０５を備える。
記憶装置９０２には、図示していないが、測位信号受信部１０１、補強信号受信部１０２、測位計算部１０３及び地図表示部１０４の機能を実現するプログラムが記憶されている。
そして、プロセッサ９０１がこれらプログラムを実行して、後述する測位信号受信部１０１、補強信号受信部１０２、測位計算部１０３及び地図表示部１０４の動作を行う。
図２では、プロセッサ９０１が測位信号受信部１０１、補強信号受信部１０２、測位計算部１０３及び地図表示部１０４の機能を実現するプログラムを実行している状態を模式的に表している。
記憶装置９０２は、また、地図データベース１０５を実現する。

測位信号受信部１０１は、ＧＰＳ衛星３００からの測位信号５００を受信する。
補強信号受信部１０２は、準天頂衛星２００からの補強信号４００を受信する。
測位計算部１０３は、測位信号受信部１０１により受信された測位信号と補強信号受信部１０２により受信された補強信号とを用いて、測位計算装置１００の所在位置の測位計算を行う。
地図表示部１０４は、測位計算部１０３により算出された測位計算装置１００の所在位置が示される地図を測位計算装置１００に備わっているディスプレイ装置（図２では不図示）に表示する。
地図データベース１０５は、地図データを保持する。地図データは、複数のグリッドに分割されている。そして、各グリッドの規定の点（例えば、グリッドの左下の点）に、当該点に対応する地理位置の緯度経度及び高度の情報が設定されている。

図３は、測位計算部１０３の内部構成を示す。
図３に示すように、測位計算部１０３は、観測値算出部１３１とカルマンフィルタ１３２で構成される。
観測値算出部１３１は、測位信号と補強信号とに基づき観測値を算出する。観測値算出部１３１は、一重差計算及び二重差計算を行って観測値を算出する。観測値には、受信機雑音が残っているが、受信機雑音は、カルマンフィルタ１３２による統計処理により取り除くことができる。
観測値算出部１３１は、公知の構成であるため、詳細な説明は省略する。
カルマンフィルタ１３２は、時間外挿計算、観測更新計算及び再計算を行う。

カルマンフィルタ１３２の詳細を説明する前に、従来のカルマンフィルタを説明する。
図６は、従来のカルマンフィルタ１６２の構成と、従来のカルマンフィルタ１６２で行われる動作を示す。
図６の（ａ）は、カルマンフィルタ１６２で行われる時間外挿計算と観測更新計算の詳細を示す。図６の（ｂ）は、カルマンフィルタ１６２の構成例を示す。

図６の（ａ）及び図６の（ｂ）において、ｙ_Ｌ，ｔは、時刻０からｔまでの観測値である。観測値ｙ_Ｌ，ｔには、測位計算装置１００の推定三次元位置が含まれている。
η_Ｌ，ｔは、時刻ｔにおける状態ベクトルを示す変数パラメータである。状態ベクトルにも、測位計算装置１００の推定三次元位置が含まれる。
η_{Ｌ，ｔ｜ｔ}は、時刻ｔにおける状態ベクトルの拘束条件無の観測更新値である。拘束条件の詳細は後述する。
η_{Ｌ，ｔ｜ｔ−１}は、時刻ｔにおける状態ベクトルの拘束条件無の時間外挿値である。
η_{Ｌ，ｔ−１｜ｔ−１}は、時刻ｔ−１における状態ベクトルの拘束条件無の観測更新値である。
Σ_{Ｌ，ｔ｜ｔ}は、時刻ｔにおける誤差共分散行列の拘束条件無の更新値である。
Σ_{Ｌ，ｔ｜ｔ−１}は、時刻ｔにおける誤差共分散行列の拘束条件無の時間外挿値である。
Σ_{Ｌ，ｔ−１｜ｔ−１}は、時刻ｔ−１における誤差共分散行列の拘束条件無の更新値である。
Ｋ_Ｌ，ｔは、時刻ｔにおけるカルマンゲインである。
Ｆ_Ｌ，ｔは、時刻ｔにおける遷移行列である。
Ｈ_Ｌ，ｔは、時刻ｔにおける観測行列である。
Ｒ_Ｌ，ｔは、時刻ｔにおける観測量に含まれると予想される観測ノイズである。
ν_Ｌ，ｔは、図６の（ａ）の「状態方程式更新」における「（ｙ_Ｌ，ｔ−Ｈ_Ｌ，ｔη_{Ｌ，ｔ｜ｔ−１}）」である。
また、上付き添え字の「Ｔ」は転置行列を示す。

カルマンフィルタ１６２は、観測更新計算部１６２１、時間遅延部１６２２及び時間外挿計算部１６２３で構成される。

観測更新計算部１６２１は、図６の（ａ）に示す「カルマンゲイン計算」、「共分散更新」及び「状態方程式更新」を行う。
すなわち、観測更新計算部１６２１は、図６の（ａ）に示す「カルマンゲイン計算」を行って、時刻ｔにおけるカルマンゲインＫ_Ｌ，ｔを得る。
また、観測更新計算部１６２１は、図６の（ａ）に示す「共分散更新」を行って、時刻ｔにおける誤差共分散列の更新値Σ_{Ｌ，ｔ｜ｔ}を得る。
更に、観測更新計算部１６２１は、図６の（ａ）に示す「状態方程式更新」を行って、時刻ｔにおける状態ベクトルの更新値η_{Ｌ，ｔ｜ｔ}を得る。そして、状態ベクトルの更新値η_{Ｌ，ｔ｜ｔ}を推定値として地図表示部１０４に出力する。
更に、観測更新計算部１６２１は、状態ベクトルの更新値η_{Ｌ，ｔ｜ｔ}と共分散行列Σ_{Ｌ，ｔ｜ｔ}を、時間遅延部１６２２に出力する。

時間遅延部１６２２は、観測更新計算部１６２１により得られた状態ベクトルの更新値η_{Ｌ，ｔ｜ｔ}を遅延させて、状態ベクトルの観測更新値η_{Ｌ，ｔ−１｜ｔ−１}を時間外挿計算部１６２３に出力する。また、時間遅延部１６２２は、共分散行列Σ_{Ｌ，ｔ｜ｔ}を遅延させて、共分散行列の更新値Σ_{Ｌ，ｔ−１｜ｔ−１}を時間外挿計算部１６２３に出力する。

時間外挿計算部１６２３は、図６の（ａ）に示す「状態方程式外挿」及び「共分散外挿」を行う。
すなわち、時間外挿計算部１６２３は、図６の（ａ）に示す「状態方程式外挿」を行って、状態ベクトルの更新値η_{Ｌ，ｔ｜ｔ−１}を得る。また、時間外挿計算部１６２３は、図６の（ａ）に示す「共分散外挿」を行って、共分散行列の時間外挿値Σ_{Ｌ，ｔ｜ｔ−１}を得る。更に、時間外挿計算部１６２３は、状態ベクトルの更新値η_{Ｌ，ｔ｜ｔ−１}と共分散行列の時間外挿値Σ_{Ｌ，ｔ｜ｔ−１}を観測更新計算部１６２１に出力する。

図４は、本実施の形態に係るカルマンフィルタ１３２の構成例を示す。
図４を参照して、カルマンフィルタ１３２の構成例を説明する。

図４に示すように、カルマンフィルタ１３２は、観測更新計算部１３２１、時間遅延部１３２２、時間外挿計算部１３２３、再計算部１３２４及び選択部１３２５で構成される。
観測更新計算部１３２１は観測更新計算部１６２１に対応し、時間遅延部１３２２は時間遅延部１６２２に対応し、時間外挿計算部１３２３は時間外挿計算部１６２３に対応する。すなわち、再計算部１３２４及び選択部１３２５に対応する構成は、カルマンフィルタ１６２には存在しない。

観測更新計算部１３２１は、測位信号から得られた観測値ｙ_Ｌ，ｔを用いた観測更新計算を行って、時間外挿計算部１３２３により算出された状態ベクトルと誤差共分散とを更新する。
前述したように状態ベクトルには、測位計算装置１００の推定三次元位置が含まれるため、観測更新計算部１３２１は、観測更新計算を行って、時間外挿計算部１３２３により算出された測位計算装置１００の推定三次元位置と誤差共分散とを更新する。
観測更新計算部１３２１は、観測更新計算部１６２１で行われる観測更新計算と同様の計算を行う。
観測更新計算部１３２１で行われる動作は、観測更新計算処理に相当する。

時間遅延部１３２２は、観測更新計算部１３２１により得られた状態ベクトルの更新値を遅延させる。
時間遅延部１３２２は、時間遅延部１６２２で行われる時間遅延計算と同様の計算を行う。

時間外挿計算部１３２３は、時間外挿計算を行って、状態ベクトル（測位計算装置１００の推定三次元位置）と誤差共分散とを算出する。
時間外挿計算部１３２３は、時間外挿計算部１６２３で行われる時間外挿計算と同様の計算を行う。
時間外挿計算部１３２３で行われる動作は、時間外挿計算処理に相当する。

再計算部１３２４は、観測更新計算部１３２１での更新により得られた測位計算装置１００の推定三次元位置の水平位置に対応する高度値と当該高度値の誤差の分散とを拘束条件として地図データベース１０５から取得する。そして、再計算部１３２４は、地図データベース１０５から取得した高度値と分散と観測更新計算部１３２１での更新により得られた状態ベクトル（測位計算装置１００の推定三次元位置）と誤差共分散とに基づき、状態ベクトルと誤差共分散とを再計算する。
なお、再計算部１３２４における再計算の詳細は後述する。
再計算部１３２４で行われる動作は、再計算処理に相当する。

選択部１３２５は、観測更新計算部１３２１での更新により得られた誤差共分散と再計算部１３２４での再計算により得られた誤差共分散とを比較する。
また、選択部１３２５は、観測更新計算部１３２１での更新により得られた状態ベクトル（測位計算装置１００の推定三次元位置）と再計算部１３２４での再計算により得られた状態ベクトル（測位計算装置１００の推定三次元位置）のうちのいずれかを選択する。
より具体的には、選択部１３２５は、観測更新計算部１３２１での更新により得られた誤差共分散が再計算部１３２４での再計算により得られた誤差共分散以下の場合は、観測更新計算部１３２１での更新により得られた状態ベクトルを選択する。
一方、再計算部１３２４での再計算により得られた誤差共分散が観測更新計算部１３２１での更新により得られた誤差共分散よりも小さい場合は、再計算部１３２４での再計算により得られた状態ベクトルを選択する。
そして、選択部１３２５は、選択した状態ベクトルを出力する。つまり、選択部１３２５は、選択した状態ベクトルを地図表示部１０４に出力する。また、選択部１３２５は、選択した状態ベクトルと、選択した状態ベクトルに対応する誤差共分散を時間遅延部１３２２に出力する。

時間外挿計算部１３２３では、選択部１３２５により選択された状態ベクトルと、当該状態ベクトルに対応する誤差共分散とに基づき、時間外挿計算を行う。

＊＊＊動作の説明＊＊＊
次に、図４及び図５を参照して、本実施の形態に係るカルマンフィルタ１３２の動作例を説明する。
なお、図５は、カルマンフィルタ１３２で行われる時間外挿計算と観測更新計算と再計算の詳細を示す。

図４及び図５において、η＾_{Ｌ，ｔ｜ｔ}は、時刻ｔにおける状態ベクトルの拘束条件有の更新値である（なお、「η＾」は、「η」の真上に「＾」が位置していることを意味する）。
Σ＾_{Ｌ，ｔ｜ｔ}は、時刻ｔにおける共分散行列の拘束条件有の更新値である（なお、「Σ＾」は、「Σ」の真上に「＾」が位置していることを意味する）。
Ｚ^〜 _Ｌ，ｔは、時刻ｔにおける既知の高度値である（なお、「Ｚ^〜」は、「Ｚ」の真上に「^〜」が位置していることを意味する）。
σ^２ _ＬＺは、観測更新計算部１３２１による水平位置における既知の高度値Ｚ^〜 _Ｌ，ｔ（以下、Ｚ^〜 _Ｌ，ｔという）とその真値（以下、Ｚ_Ｌ，ｔという）の差の誤差共分散行列である。本実施の形態では、σ^２ _ＬＺは、ホワイトガウスノイズと仮定する。
なお、Ｚ^〜 _Ｌ，ｔとσ^２ _ＬＺは、再計算部１３２４が地図データベース１０５から拘束条件として取得する。
ｂは状態ベクトルのｚ座標の位置を指定する行列である。
具体的には、ｂ＝［０，０，１，０，０，０，０，０，０，０，０（ｋ）］^Ｔである。
０（ｋ）は、０がｋ個続くことを意味する。Ｔは転置行列を意味する。
ｂｂ^Ｔは、ｂとｂ^Ｔ（ｂの転置行列）の行列の積の演算である。
また、図６に示す記号と同じ記号は同じ意味であるので、説明を省略する。

観測更新計算部１３２１は、図５に示す「カルマンゲイン計算」、「共分散更新」及び「状態方程式更新」を行う。
すなわち、観測更新計算部１３２１は、図５に示す「カルマンゲイン計算」を行って、時刻ｔにおけるカルマンゲインＫ_Ｌ，ｔを得る。また、観測更新計算部１３２１は、図５に示す「共分散更新」を行って、時刻ｔにおける誤差共分散列の更新値Σ_{Ｌ，ｔ｜ｔ}を得る。更に、観測更新計算部１６２１は、図５に示す「状態方程式更新」を行って、時刻ｔにおける状態ベクトルの更新値η_{Ｌ，ｔ｜ｔ}を得る。そして、観測更新計算部１３２１は、状態ベクトルの更新値η_{Ｌ，ｔ｜ｔ}と誤差共分散行列Σ_{Ｌ，ｔ｜ｔ}を、再計算部１３２４に出力する。

再計算部１３２４は、地図データベース１０５から、観測更新計算部１３２１により得られた状態ベクトルの更新値η_{Ｌ，ｔ｜ｔ}に対応する地点の既知の高度値Ｚ^〜 _Ｌ，ｔと、その高度値とその真値Ｚ_Ｌ，ｔとの差分に対応する誤差共分散行列σ^２ _ＬＺを取得する。
つまり、再計算部１３２４は、状態ベクトルの更新値η_{Ｌ，ｔ｜ｔ}の推定三次元位置の水平位置に対応する既知の高度値Ｚ^〜 _Ｌ，ｔを地図データベース１０５から取得する。また、当該高度値Ｚ^〜 _Ｌ，ｔとその高度値の真値Ｚ_Ｌ，ｔとの差分に対応する誤差共分散行列σ^２ _ＬＺを地図データベース１０５から取得する。
また、再計算部１３２４は、観測更新計算部１３２１から状態ベクトルの更新値η_{Ｌ，ｔ｜ｔ}の推定三次元位置と、観測更新計算部１３２１からの誤差共分散行列の時間外挿値Σ_{Ｌ，ｔ｜ｔ−１}、地図データベース１０５から取得した既知の高度値Ｚ^〜 _Ｌ，ｔと誤差行分散行列σ^２ _ＬＺとを用いて、測位計算装置１００の状態ベクトルと誤差共分散行列とを再計算する。再計算により得られる状態ベクトルはη＾_{Ｌ，ｔ｜ｔ}である。また、再計算により得られる誤差共分散行列はΣ＾_{Ｌ，ｔ｜ｔ}である。
再計算部１３２４は、具体的には、図５に示す再計算を行う。
そして、再計算部１３２４は、再計算により得られた状態ベクトルη＾_{Ｌ，ｔ｜ｔ}と、再計算により得られた誤差共分散行列Σ＾_{Ｌ，ｔ｜ｔ}を選択部１３２５に出力する。
また、再計算部１３２４は、観測更新計算部１３２１から取得した状態ベクトルη_{Ｌ，ｔ｜ｔ}と誤差共分散行列Σ_{Ｌ，ｔ｜ｔ}を選択部１３２５に出力する。

選択部１３２５は、再計算部１３２４から取得した誤差共分散行列Σ_{Ｌ，ｔ｜ｔ}と誤差共分散行列Σ＾_{Ｌ，ｔ｜ｔ}を比較する。
誤差共分散行列Σ_{Ｌ，ｔ｜ｔ}が再計算により得られた誤差共分散行列Σ＾_{Ｌ，ｔ｜ｔ}以下であれば、選択部１３２５は、状態ベクトルη_{Ｌ，ｔ｜ｔ}を選択する。
一方、誤差共分散行列Σ_{Ｌ，ｔ｜ｔ}が再計算により得られた誤差共分散行列Σ＾_{Ｌ，ｔ｜ｔ}よりも大きければ、選択部１３２５は、再計算により得られた状態ベクトルη＾_{Ｌ，ｔ｜ｔ}を選択する。
そして、選択部１３２５は、選択した状態ベクトルを推定値として地図表示部１０４に出力する。
また、選択部１３２５は、選択した状態ベクトルと、選択した状態ベクトルに対応する誤差共分散行列とを時間遅延部１３２２に出力する。

時間遅延部１３２２は、選択部１３２５から出力された状態ベクトルを遅延させて、状態ベクトルη_{Ｌ，ｔ−１｜ｔ−１}として時間外挿計算部１３２３に出力する。また、時間遅延部１３２２は、選択部１３２５から出力された誤差共分散行列を遅延させて誤差共分散行列Σ_{Ｌ，ｔ−１｜ｔ−１}として時間外挿計算部１３２３に出力する。

時間外挿計算部１３２３は、状態ベクトルη_{Ｌ，ｔ−１｜ｔ−１}と、誤差共分散行列Σ_{Ｌ，ｔ−１｜ｔ−１}とに基づき、時間外挿計算を行う。時間外挿計算自体は、時間外挿計算部１６２３と同じであるため、詳細な説明を省略する。

なお、上記では、再計算部１３２４は、地図データベース１０５から取得した高度値と分散と観測更新計算部１３２１での更新により得られた状態ベクトル（測位計算装置１００の推定三次元位置）と誤差共分散とに基づき、状態ベクトルと誤差共分散とを再計算している。これに代えて、再計算部１３２４は、既定の供給元から位置情報あるいは姿勢情報とその分散を取得し、取得した前記位置情報あるいは姿勢情報と前記分散と観測更新計算部１３２１での更新により得られた測位計算装置１００の推定三次元位置あるいは姿勢と誤差共分散とに基づき、測位計算装置１００の推定三次元位置と誤差共分散とを再計算するようにしてもよい。前記既定の供給元とは、外部あるいはＩＮＳ（Ｉｎｅｒｔｉａｌ Ｎａｖｉｇａｔｉｏｎ Ｓｙｓｔｅｍ）等の出力機器あるいはそのセンサあるいはデータベースである。
また、状態ベクトルの選択の条件としては、測位計算に使用した衛星数がある規定の閾値以下（例えば、３機以下の場合）の場合には、再計算部１３２４での再計算により得られた状態ベクトルを選択する。それ以外の場合（例えば、４機以上の場合）には、観測更新計算部１３２１での更新により得られた状態ベクトルを選択することも可能である。

＊＊＊実施の形態の効果の説明＊＊＊
以上、本実施の形態に係る測位計算部１０３は、推定三次元位置の水平位置に対応する高度値と当該高度値の誤差の分散とを地図データベース１０５から取得し、取得した高度値と分散とを用いた推定三次元位置と誤差共分散とを再計算する。更に、測位計算部１０３は、観測更新により得られた誤差共分散と再計算により得られた誤差行分散とを比較して、観測更新により得られた推定三次元位置と再計算により得られた推定三次元位置のうちのいずれかを選択する。
このため、本実施の形態によれば、測位信号を受信できる測位衛星数が３機以下の場合にも、高精度な測位が可能な測位計算装置を、部品点数を増加させずに得ることができる。

また、本実施の形態による具体的な効果は、高さとしてＺ^〜 _Ｌ，ｔを拘束条件とすることにより、その確率密度関数がη_Ｌ，ｔ＝η＾_{Ｌ，ｔ｜ｔ}のとき、以下のように最大値が得られることである。

このため、拘束条件を用いない場合に比べて、以下の条件の時に、改善が期待できる。また、その共分散は、Σ_{Ｌ，ｔ｜ｔ}となることを表している。

なお、確率密度関数は以下の通りである。

１００ 測位計算装置、１０１ 測位信号受信部、１０２ 補強信号受信部、１０３ 測位計算部、１０４ 地図表示部、１０５ 地図データベース、１３１ 観測値算出部、１３２ カルマンフィルタ、１６２ カルマンフィルタ、２００ 準天頂衛星、３００ ＧＰＳ衛星、４００ 補強信号、５００ 測位信号、１３２１ 観測更新計算部、１３２２ 時間遅延部、１３２３ 時間外挿計算部、１３２４ 再計算部、１３２５ 選択部、１６２１ 観測更新計算部、１６２２ 時間遅延部、１６２３ 時間外挿計算部、９０１ プロセッサ、９０２ 記憶装置。

Claims (7)

1. 測位衛星からの測位信号を受信して測位計算を行う測位計算装置であって、
   時間外挿計算を行って、前記測位計算装置の推定三次元位置と誤差共分散とを算出する時間外挿計算部と、
   前記測位信号から得られた観測値を用いた観測更新計算を行って、前記時間外挿計算部により算出された前記測位計算装置の推定三次元位置と誤差共分散とを更新する観測更新計算部と、
   既定の供給元から前記測位計算装置の位置情報あるいは姿勢情報と当該位置情報あるいは姿勢情報の誤差の分散を取得し、取得した前記位置情報あるいは姿勢情報と前記分散と前記観測更新計算部での更新により得られた前記測位計算装置の推定三次元位置あるいは姿勢と誤差共分散とに基づき、前記測位計算装置の推定三次元位置と誤差共分散とを再計算する再計算部と、
   前記観測更新計算部での更新により得られた誤差共分散と前記再計算部での再計算により得られた誤差共分散とを比較し、前記観測更新計算部での更新により得られた前記測位計算装置の推定三次元位置と前記再計算部での再計算により得られた前記測位計算装置の推定三次元位置のうちのいずれかを選択し、選択した前記測位計算装置の推定三次元位置を出力する選択部とを有する測位計算装置。
2. 測位衛星からの測位信号を受信して測位計算を行う測位計算装置であって、
   時間外挿計算を行って、前記測位計算装置の推定三次元位置と誤差共分散とを算出する時間外挿計算部と、
   前記測位信号から得られた観測値を用いた観測更新計算を行って、前記時間外挿計算部により算出された前記測位計算装置の推定三次元位置と誤差共分散とを更新する観測更新計算部と、
   前記観測更新計算部での更新により得られた推定三次元位置の水平位置に対応する高度値と前記高度値の誤差の分散とをデータベースから取得し、取得した前記高度値と前記分散と前記観測更新計算部での更新により得られた前記測位計算装置の推定三次元位置と誤差共分散とに基づき、前記測位計算装置の推定三次元位置と誤差共分散とを再計算する再計算部と、
   前記観測更新計算部での更新により得られた誤差共分散と前記再計算部での再計算により得られた誤差共分散とを比較し、前記観測更新計算部での更新により得られた前記測位計算装置の推定三次元位置と前記再計算部での再計算により得られた前記測位計算装置の推定三次元位置のうちのいずれかを選択し、選択した前記測位計算装置の推定三次元位置を出力する選択部とを有する測位計算装置。
3. 前記選択部は、
   前記観測更新計算部での更新により得られた誤差共分散が前記再計算部での再計算により得られた誤差共分散よりも以下の場合は、前記観測更新計算部での更新により得られた前記測位計算装置の推定三次元位置を選択し、
   前記再計算部での再計算により得られた誤差共分散が前記観測更新計算部での更新により得られた誤差共分散よりも小さい場合は、前記再計算部での再計算により得られた前記測位計算装置の推定三次元位置を選択する請求項２に記載の測位計算装置。
4. 測位衛星からの測位信号を受信して測位計算を行う測位計算装置であって、
   時間外挿計算を行って、前記測位計算装置の推定三次元位置と誤差共分散とを算出する時間外挿計算部と、
   前記測位信号から得られた観測値を用いた観測更新計算を行って、前記時間外挿計算部により算出された前記測位計算装置の推定三次元位置と誤差共分散とを更新する観測更新計算部と、
   前記観測更新計算部での更新により得られた推定三次元位置の水平位置に対応する高度値と前記高度値の誤差の分散とをデータベースから取得し、取得した前記高度値と前記分散と前記観測更新計算部での更新により得られた前記測位計算装置の推定三次元位置と誤差共分散とに基づき、前記測位計算装置の推定三次元位置と誤差共分散とを再計算する再計算部と、
   測位計算に使用した衛星数がある閾値より大きい場合は、前記観測更新計算部での更新により得られた前記測位計算装置の推定三次元位置を選択し、
   測位計算に使用した衛星数がある閾値より小さい場合は、前記再計算部での再計算により得られた前記測位計算装置の推定三次元位置を選択する選択部とを有する測位計算装置。
5. 前記時間外挿計算部は、
   前記選択部により選択された前記測位計算装置の推定三次元位置と、前記選択部により選択された前記測位計算装置の推定三次元位置に対応する誤差共分散とに基づき、時間外挿計算を行う請求項２又は４に記載の測位計算装置。
6. 測位衛星からの測位信号を受信して測位計算を行うコンピュータが、時間外挿計算を行って、前記コンピュータの推定三次元位置と誤差共分散とを算出する時間外挿計算処理と、
   前記コンピュータが、前記測位信号から得られた観測値を用いた観測更新計算を行って、前記時間外挿計算処理により算出された前記コンピュータの推定三次元位置と誤差共分散とを更新する観測更新計算処理と、
   前記コンピュータが、前記観測更新計算処理での更新により得られた推定三次元位置の水平位置に対応する高度値と前記高度値の誤差の分散とをデータベースから取得し、取得した前記高度値と前記分散と前記観測更新計算処理での更新により得られた前記コンピュータの推定三次元位置と誤差共分散とに基づき、前記コンピュータの推定三次元位置と誤差共分散とを再計算する再計算処理と、
   前記コンピュータが、前記観測更新計算処理での更新により得られた誤差共分散と前記再計算処理での再計算により得られた誤差共分散とを比較し、前記観測更新計算処理での更新により得られた前記コンピュータの推定三次元位置と前記再計算処理での再計算により得られた前記コンピュータの推定三次元位置のうちのいずれかを選択し、選択した前記コンピュータの推定三次元位置を出力する選択処理とを有する測位計算方法。
7. 測位衛星からの測位信号を受信して測位計算を行うコンピュータに、
   時間外挿計算を行って、前記コンピュータの推定三次元位置と誤差共分散とを算出する時間外挿計算処理と、
   前記測位信号から得られた観測値を用いた観測更新計算を行って、前記時間外挿計算処理により算出された前記コンピュータの推定三次元位置と誤差共分散とを更新する観測更新計算処理と、
   前記観測更新計算処理での更新により得られた推定三次元位置の水平位置に対応する高度値と前記高度値の誤差の分散とをデータベースから取得し、取得した前記高度値と前記分散と前記観測更新計算処理での更新により得られた前記コンピュータの推定三次元位置と誤差共分散とに基づき、前記コンピュータの推定三次元位置と誤差共分散とを再計算する再計算処理と、
   前記観測更新計算処理での更新により得られた誤差共分散と前記再計算処理での再計算により得られた誤差共分散とを比較し、前記観測更新計算処理での更新により得られた前記コンピュータの推定三次元位置と前記再計算処理での再計算により得られた前記コンピュータの推定三次元位置のうちのいずれかを選択し、選択した前記コンピュータの推定三次元位置を出力する選択処理とを実行させる測位計算プログラム。

Images