測位装置
【課題】比較的大きな受信機時計ドリフトが発生した場合でも高精度でリアルタイムに現在位置を測位することができる測位装置を提供する。
【解決手段】測位装置２０は、受信信号を解析する受信信号解析部２１と、受信時刻情報を受信信号解析部２１に提供する受信機時計２２と、利用者位置、利用者速度及び受信機時刻を推定するカルマンフィルタ４０と、を備え、カルマンフィルタ４０は、状態予測を行う状態予測処理手段４１と、状態推定を行う状態推定処理部４２と、受信機時計２２の異常変化を検出する時計異常変化検出部４３と、を備え、状態予測処理手段４１は、状態推定処理部４２から得る１エポック前の状態変数ベクトルに基づいて現エポックの状態変数ベクトルを予測する状態予測処理部４１ａと、時計異常変化検出部４３の検出結果に基づいて状態予測値を修正する状態予測値修正部４１ｂと、を備える。
【解決手段】測位装置２０は、受信信号を解析する受信信号解析部２１と、受信時刻情報を受信信号解析部２１に提供する受信機時計２２と、利用者位置、利用者速度及び受信機時刻を推定するカルマンフィルタ４０と、を備え、カルマンフィルタ４０は、状態予測を行う状態予測処理手段４１と、状態推定を行う状態推定処理部４２と、受信機時計２２の異常変化を検出する時計異常変化検出部４３と、を備え、状態予測処理手段４１は、状態推定処理部４２から得る１エポック前の状態変数ベクトルに基づいて現エポックの状態変数ベクトルを予測する状態予測処理部４１ａと、時計異常変化検出部４３の検出結果に基づいて状態予測値を修正する状態予測値修正部４１ｂと、を備える。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、カルマンフィルタを用いた衛星航法システムの測位装置に関する。
【背景技術】
【０００２】
ＧＰＳといった衛星航法システムにおける測位装置は、衛星の送信電波に付加されている情報から得られる電波の送信時刻（以下「衛星送信時刻」）、衛星位置、衛星速度、受信電波の搬送波の測定により得られる利用者位置変化、衛星位置変化、受信機発振器の周波数ずれに起因するドップラシフト等から、利用者位置、利用者速度及び受信機における衛星航法システム系の時刻（以下「受信機時刻」）を推定する装置である。
【０００３】
この測位装置において、利用者位置、利用者速度及び受信機時刻の推定には、前回の状態から今回の状態への変化、及び今回状態と観測値の関係をモデル化し、そのモデル方程式をカルマンフィルタによって解く方法が一般的である。
【０００４】
カルマンフィルタにおいては、利用者位置、利用者速度及び受信機時刻を状態変数とおき、１エポック前の状態変数値とその誤差の共分散行列から予測モデルを用いて現在の状態変数値及びその共分散行列を予測する処理と、現在の観測量と状態推定値の関係式を用いて予測した状態変数値及び共分散行列から現在の状態変数値及び共分散行列を推定する処理が行われる。なお、エポックは観測データの取得時を意味している。
【０００５】
通常、カルマンフィルタを用いて利用者位置、利用者速度及び受信機時刻を推定する場合、状態変数ベクトルｘは、［数１］で表すのが一般的である。
【０００６】
【数１】
【０００７】
ここで、ｐは利用者位置（３次元ベクトル）、Δｔは受信機時計の時刻と衛星航法システムにおける時刻との差（以下「受信機時計オフセット」）、ｖは利用者速度（３次元ベクトル）、ΔＴは受信機時計における単位時間当たりの受信機時計オフセットのずれ（以下「受信機時計ドリフト」）、上付の"Ｔ"は転置行列を示す。なお、利用者位置とは受信機を利用する利用者の位置をいい、利用者速度とは利用者の移動速度をいう。
【０００８】
カルマンフィルタを用いた衛星航法システムの測位装置では、発振器を備えた受信機時計において、受信機時計ドリフトの１エポック当たりの変化量は、ガウス分布に従うと仮定することができ、その分散量は発振器の周波数安定度によって決まる。予測モデルに前記仮定を組み入れることで、受信機時計ドリフトの推定精度を向上させることが可能であり、周波数安定度の高い発振器を用いるほど推定精度は向上する。
【０００９】
通常の安価な衛星航法システムの測位装置においては、発振器に温度補償型水晶発振器（以下「ＴＣＸＯ」）が用いられる。この発振器では、温度が安定している環境では周波数安定度は比較的高いが、その一方で、急激な温度変化が発生した場合や衝撃を受けた場合には、受信機時計ドリフトが大きく変化してしまう。このような状態下でも安定した測位を行うには、受信機時計ドリフトの１エポック当たりの変化量の大きさ（分散）を大きめに設定する必要があった。しかしながら、上記分散を大きめに設定すると、受信機時計ドリフトが安定した状況下において精度が悪化してしまうという問題があった。
【００１０】
このような問題を解決する技術として、適応カルマンフィルタが用いられている（例えば、非特許文献１参照）。非特許文献１に示された適応カルマンフィルタは、観測雑音の分散に基づき与えられたシステム雑音モデルのパラメータが妥当かどうかを判断して妥当でなければ調整することができ、また、連続する数エポックにおいて平均化処理を行って状態推定の信頼性を高めることができるようになっている。
【先行技術文献】
【非特許文献】
【００１１】
【非特許文献１】Y. Bar-Shalom, X. R. Li, and T. Kirubarajan, Estimation with Applications to Tracking and Navigation: Theory, Algorithms, and Software, Wiley, New York, 2001, pp. 424-425.
【発明の概要】
【発明が解決しようとする課題】
【００１２】
しかしながら、従来の適応カルマンフィルタでは、受信機時計ドリフトの変化が安定しているにもかかわらず、観測値に大きな観測誤差が含まれている場合に受信機時計ドリフトに大きな変化が発生していると誤判断する可能性があり、測位精度の劣化をもたらすという課題があった。また、従来の適応カルマンフィルタでは、状態推定の信頼性を高めるため平均化処理を行う構成となっているので、受信機時計ドリフトの変化をリアルタイムに検出できなくなり、測位精度が劣化するという課題があった。
【００１３】
この発明は、上述した課題を解決するためのもので、比較的大きな受信機時計ドリフトが発生した場合でも高精度でリアルタイムに現在位置を測位することができる測位装置を提供することを目的とする。
【課題を解決するための手段】
【００１４】
本発明の測位装置は、衛星航法システムにおいて衛星からの測位信号を受信する受信機の利用者の位置を測位する測位装置であって、前記利用者の位置を示す利用者位置と、前記利用者の移動速度を示す利用者速度と、前記受信機が有する受信機時計の時刻と前記衛星航法システムにおける時刻との差を示す受信機時計オフセットと、前記受信機時計における単位時間当たりの前記受信機時計オフセットのずれを示す受信機時計ドリフトとを状態変数として含む状態変数ベクトルに基づき、前記衛星航法システムに関する状態予測及び状態推定を行うカルマンフィルタと、前記衛星からの測位信号を受信して前記衛星の状態を示す観測データを取得する観測データ取得手段と、を備え、前記カルマンフィルタは、前記状態変数ベクトルの予測値及び予測誤差の共分散行列を算出する状態予測手段と、前記状態予測手段の算出結果及び前記観測データに基づいて前記状態変数ベクトルの推定値及び推定誤差の共分散行列を算出して前記状態予測手段の算出結果を更新する状態推定更新手段と、前記状態予測手段が算出した状態変数ベクトルの予測値に含まれる受信機時計ドリフト予測値と、前記状態推定更新手段が算出した状態変数ベクトルの推定値に含まれる受信機時計ドリフト推定値との差分を算出する差分算出手段と、前記差分が予め定められた閾値を超えたとき、前記予測誤差の共分散行列に含まれる受信機時計ドリフトの分散を、より大きい値に修正する分散修正手段と、を備え、前記状態推定更新手段は、前記分散修正手段が前記受信機時計ドリフトの分散を修正したとき、前記状態予測手段が算出した予測誤差の共分散行列に含まれる受信機時計ドリフトの分散を修正された受信機時計ドリフトの分散に置き換えて前記状態変数ベクトルの推定値及び推定誤差の共分散行列を再計算するものである構成を有している。
【００１５】
この構成により、本発明の測位装置は、受信機時計ドリフト予測値と受信機時計ドリフト推定値との差分が予め定められた閾値を超えたとき、予測誤差の共分散行列に含まれる受信機時計ドリフトの分散を、より大きい値に修正するので、比較的大きな受信機時計ドリフトが発生した場合でも高精度でリアルタイムに現在位置を測位することができる。
【００１６】
また、本発明の測位装置は、衛星航法システムにおいて衛星からの測位信号を受信する受信機の利用者の位置を測位する測位装置であって、前記利用者の位置を示す利用者位置と、前記利用者の移動速度を示す利用者速度と、前記受信機が有する受信機時計の時刻と前記衛星航法システムにおける時刻との差を示す受信機時計オフセットと、前記受信機時計における単位時間当たりの前記受信機時計オフセットのずれを示す受信機時計ドリフトとを状態変数として含む第１の状態変数ベクトルに基づき、前記衛星航法システムに関する状態予測及び状態推定を行うカルマンフィルタと、前記衛星からの測位信号を受信して前記衛星の状態を示す観測データを取得する観測データ取得手段と、を備え、前記カルマンフィルタは、前記第１の状態変数ベクトルの予測値及び予測誤差の共分散行列を算出する状態予測手段と、前記状態予測手段の算出結果の内の前記利用者速度及び前記受信機時計ドリフトのみを状態変数として含む第２の状態変数ベクトルの予測値及び予測誤差の共分散行列と前記観測データとに基づいて前記第２の状態変数ベクトルの推定値及び推定誤差の共分散行列を算出する状態推定手段と、前記状態予測手段が算出した前記第１の状態変数ベクトルの予測値に含まれる受信機時計ドリフト予測値と、前記状態推定手段が算出した前記第２の状態変数ベクトルの推定値に含まれる受信機時計ドリフト推定値との差分を算出する差分算出手段と、前記差分が予め定められた閾値を超えたとき、前記状態予測手段が算出した予測誤差の共分散行列に含まれる受信機時計ドリフトの分散を、より大きい値に修正する分散修正手段と、前記分散修正手段が前記受信機時計ドリフトの分散を修正したとき、前記状態予測手段が算出した予測誤差の共分散行列に含まれる受信機時計ドリフトの分散を修正された受信機時計ドリフトの分散に置き換えて前記第１の状態変数ベクトルの推定値及び推定誤差の共分散行列を算出して前記状態予測手段の算出結果を更新するとともに、前記分散修正手段が前記受信機時計ドリフトの分散を修正しなかったとき、前記状態予測手段の算出結果及び前記観測データに基づいて前記第１の状態変数ベクトルの推定値及び推定誤差の共分散行列を算出して前記状態予測手段の算出結果を更新する状態推定更新手段と、を備えた構成を有している。
【００１７】
この構成により、本発明の測位装置は、利用者速度及び受信機時計ドリフトのみを状態変数として含む第２の状態変数ベクトルの推定値及び推定誤差の共分散行列を状態推定手段が算出し、状態予測手段が算出した第１の状態変数ベクトルの予測値に含まれる受信機時計ドリフト予測値と、状態推定手段が算出した第２の状態変数ベクトルの推定値に含まれる受信機時計ドリフト推定値との差分が予め定められた閾値を超えたとき、状態予測手段が算出した予測誤差の共分散行列に含まれる受信機時計ドリフトの分散を、より大きい値に修正するので、比較的大きな受信機時計ドリフトが発生した場合でも高精度でリアルタイムに現在位置を測位することができる。
【００１８】
さらに、本発明の測位装置は、前記受信機時計が、前記衛星に搭載された時計が有する発振器よりも安定度が低い発振器を備え、前記分散修正手段が、前記受信機時計の発振器の周波数短期安定度により見積もられた値を前記閾値とするものである構成を有している。
【００１９】
この構成により、本発明の測位装置は、温度補償型水晶発振器を代表とする安定度の低い発振器を用いても、高精度でリアルタイムに現在位置を測位することができ、製造コストの低減化を図ることができる。
【発明の効果】
【００２０】
本発明は、比較的大きな受信機時計ドリフトが発生した場合でも高精度でリアルタイムに現在位置を測位することができるという効果を有する測位装置を提供することができるものである。
【図面の簡単な説明】
【００２１】
【図１】本発明に係る測位信号受信機の第１実施形態におけるブロック構成図である。
【図２】本発明に係る測位装置の第１実施形態におけるブロック構成図である。
【図３】本発明に係る測位装置の第１実施形態におけるフローチャートである。
【図４】本発明に係る測位装置の第２実施形態におけるブロック構成図である。
【図５】本発明に係る測位装置の第２実施形態におけるフローチャートである。
【発明を実施するための形態】
【００２２】
まず、本発明の実施形態を説明する前にカルマンフィルタ処理について説明する。
【００２３】
（カルマンフィルタ処理）
前述の［数１］において、利用者位置ｐ、利用者速度ｖは、ＥＣＥＦ座標系のｘ軸、ｙ軸、ｚ軸を用いると、それぞれ、利用者位置ｐ＝［ｐｘ，ｐｙ，ｐｚ］Ｔ、利用者速度ｖ＝［ｖｘ，ｖｙ，ｖｚ］Ｔと表される。したがって、［数１］に示した状態変数ベクトルｘは８変数を含むものとなる。よって、その状態推定値＾ｘの誤差の共分散行列は、［数２］で表される。なお、"＾ｘ"は、ｘの上部に推定値を示すハット（＾）が付されたものを表すものとする。
【００２４】
【数２】
【００２５】
ここで、上式の行列内の各成分は［数３］で示される。但し、［数３］において、ｅｐは利用者位置ｐに関する誤差、ｅΔｔは受信機時計オフセットΔｔに関する誤差、ｅｖは利用者速度ｖに関する誤差、ｅΔＴは受信機時計ドリフトΔＴに関する誤差であり、Ｅ｛｝は期待値を意味する。
【００２６】
【数３】
【００２７】
（ｋ−１）エポックの状態推定値からｋエポックの状態を予測するモデル式（状態方程式）は、［数４］で表される。但し、ここで、ｅΔｐ（ｋ）、ｅΔΔｔ（ｋ）、ｅΔｖ（ｋ）、ｅΔΔＴ（ｋ）は、（ｋ−１）エポックの状態推定値からｋエポックでの状態を行列Ａで予測した値と、実際の値との不確定性を表すシステム雑音ベクトルである。また、Ｉは単位行列である。
【００２８】
【数４】
【００２９】
カルマンフィルタによる予測処理は、［数５］で表される。但し、ＱΔｐ（ｋ）、ＱΔｖ（ｋ）はそれぞれｅΔｐ（ｋ）、ｅΔｖ（ｋ）についての共分散行列であり、σΔΔｔ（ｋ）２、σΔΔＴ（ｋ）２はそれぞれｅΔΔｔ（ｋ）、ｅΔΔＴ（ｋ）についての分散である。また、上付（−）は予測処理で得られた値であることを意味する。
【００３０】
【数５】
【００３１】
観測量と状態推定値の関係式（観測方程式）は、［数６］で表される。但し、各変数の上付のｉ（ｉ＝１、２、・・・ｎ）は衛星の番号を示す。また、ｔｓｖは衛星の衛星送信時刻、ｔｕｓｅｒは受信機時刻、φｄｏｐｐｌｅｒはドップラ観測値、ｐｓａｔ（ｔｓｖ）は利用者位置ｐにおいて衛星送信時刻ｔｓｖの信号を取得したときの衛星位置、Ｉｏｎは電離層遅延誤差、Ｔｒは対流圏遅延量、ｅρは擬似距離ρ(ρ=ｃ（ｔｓｖ−ｔｕｓｅｒ）)の観測誤差、ｅｄｏｐｐｌｅｒはドップラ観測値の観測誤差である。
【００３２】
【数６】
【００３３】
カルマンフィルタによる推定処理は、［数７］で表される。但し、Ｋはカルマンゲイン、Ｐは推定誤差の共分散行列、Ｒは観測雑音ｅｋの共分散行列である。
【００３４】
【数７】
【００３５】
［数７］において、デザイン行列Ｈは、ｈｋ（＾ｘｋ−，０）をｘｋで微分したヤコビアン行列であり、［数８］で表される。
【００３６】
【数８】
【００３７】
ここで、ｃは光速である。また、ｈｉＡＯＡは衛星ｉから受信機への単位方向ベクトルであり、［数９］で表される。
【００３８】
【数９】
【００３９】
システムの状態を精確に推定するため、予測処理と推定処理のそれぞれに対して、システム特性に合致するようなシステム雑音の共分散行列ＱΔｐ（ｋ）、ＱΔｖ（ｋ）及び分散値σΔΔｔ（ｋ）２、σΔΔＴ（ｋ）２と、観測雑音の共分散行列Ｒｋを与えることで、推定の精度を向上させることができる。
【００４０】
（第１実施形態）
次に、本発明に係る測位装置の第１実施形態における構成について説明する。
【００４１】
図１に示すように、本実施形態における測位信号受信機１は、受信部１０、測位装置２０、制御部３０を備えている。この測位信号受信機１は、例えば、車両に搭載されるものである。
【００４２】
受信部１０は、所定の個数（例えば４個）のＧＰＳ衛星からの無線周波数（ＲＦ）帯の電波を受信するアンテナ、ＲＦ帯の信号を中間周波数帯の信号にダウンコンバートする変換回路等を備え、ダウンコンバートした受信信号を測位装置２０に出力するようになっている。
【００４３】
測位装置２０は、例えばマイクロコンピュータで構成され、入力した受信信号を解析し、車両の現在位置（利用者位置）を測位するようになっている。
【００４４】
制御部３０は、例えばマイクロコンピュータで構成され、予めメモリに記憶されたプログラムに基づいて受信部１０及び測位装置２０の動作を制御するようになっている。
【００４５】
図２は、第１実施形態における測位装置２０のブロック構成図である。図２に示すように、測位装置２０は、受信信号を解析する受信信号解析部２１と、受信時刻情報を受信信号解析部２１及び状態推定処理部４２（後述）に提供する受信機時計２２と、利用者位置、利用者速度及び受信機時刻を推定するカルマンフィルタ４０と、を備えている。本実施形態における受信機時計２２は、発振器としてＴＣＸＯを備えたものであるが、これに限定されず、他の発振器を備えた構成としてもよい。なお、受信信号解析部２１は、本発明に係る観測データ取得手段を構成する。
【００４６】
カルマンフィルタ４０は、状態予測を行う状態予測処理手段４１と、状態推定を行う状態推定処理部４２と、受信機時計２２の異常変化をリアルタイムに検出する時計異常変化検出部４３と、を備えている。なお、時計異常変化検出部４３は、本発明に係る差分算出手段を構成する。
【００４７】
状態予測処理手段４１は、状態推定処理部４２から得る１エポック前の状態変数ベクトルに基づいて現エポックの状態変数ベクトルを予測する状態予測処理部４１ａと、時計異常変化検出部４３の検出結果に基づいて状態予測値を動的に修正する状態予測値修正部４１ｂと、を備えている。なお、状態予測処理部４１ａ及び状態予測値修正部４１ｂは、それぞれ、本発明に係る状態予測手段及び分散修正手段を構成する。
【００４８】
状態推定処理部４２は、衛星に関する観測データを受信信号解析部２１から入力するとともに、状態予測処理手段４１から現エポックでの状態変数ベクトル、予測誤差の共分散行列を入力し、状態変数ベクトルを更新して状態予測処理部４１ａに出力するようになっている。なお、状態推定処理部４２は、本発明に係る状態推定更新手段を構成する。
【００４９】
次に、本実施形態における測位装置２０のカルマンフィルタ４０の動作について説明する。
【００５０】
受信信号解析部２１は、入力した受信信号を解析し、各衛星の衛星送信時刻、衛星位置、衛星速度の情報を得る。また、受信信号解析部２１は、受信した電波の搬送波に基づいてドップラ観測値を算出する。そして、受信信号解析部２１は、算出した結果を状態推定処理部４２に渡す。
【００５１】
以下、カルマンフィルタ４０の動作を図３に基づき説明する。
【００５２】
状態予測処理手段４１の状態予測処理部４１ａは、状態推定処理部４２から得る１エポック前（ｋ−１）の利用者位置及び利用者速度、推定誤差の共分散行列から、現エポックの利用者位置及び利用者速度を予測し、予測誤差の共分散行列Ｐｋ−を［数５］を用いて算出する（ステップＳ１１）。
【００５３】
ここで、モデルにおける受信機時計ドリフトの変化量は、受信機時計ドリフトの安定時におけるＴＣＸＯの周波数短期安定度で見積もった値程度とする。なお、周波数短期安定度とは、比較的短い時間周期内におけるＴＣＸＯの出力周波数の揺らぎ（不規則変動）を表したものをいう。
【００５４】
状態推定処理部４２は、受信信号解析部２１から衛星送信時刻、衛星位置、衛星速度、ドップラ観測値、受信時刻等の各データを入力する。また、状態推定処理部４２は、状態予測処理手段４１が予測した現エポックの状態変数ベクトルｘｋ−、共分散行列Ｐｋ−のデータを入力する。そして、状態推定処理部４２は、入力したこれらのデータを用いて、［数７］により、カルマンゲインを求め、状態変数ベクトル及び共分散行列を推定する（ステップＳ１２）。但し、状態推定処理部４２は、状態予測値修正部４１ｂによる修正があった場合は、受信機時計ドリフトの分散値が修正された共分散行列Ｐｋ−のデータを状態予測値修正部４１ｂから入力する。
【００５５】
時計異常変化検出部４３は、状態推定処理部４２が推定した状態変数ベクトル＾ｘｋに含まれる受信機時計ドリフトΔ＾Ｔｋと、状態予測処理部４１ａが予測した状態変数ベクトル＾ｘｋ−に含まれる受信機時計ドリフトΔ＾Ｔｋ−とを状態推定処理部４２から入力する。また、時計異常変化検出部４３は、両者の変化分の差（｜Δ＾Ｔｋ−Δ＾Ｔｋ−｜）を計算し、［数１０］により、ＴＣＸＯの周波数短期安定度により見積もった受信機時計ドリフトの標準偏差σΔΔＴと比較することにより、受信機時計２２に異常な変化があったか否かを判断する（ステップＳ１３）。なお、αは、許容量範囲を決めるスレッショルド係数である。
【００５６】
【数１０】
【００５７】
時計異常変化検出部４３は、受信機時計ドリフトの変化分が数［１０］を満たさない場合は、受信機時計２２の異常変化が発生していると判断し、σΔＴ２−より大きい値σｎｅｗΔＴ２−を受信機時計ドリフトの分散の修正値として状態予測値修正部４１ｂに出力する。なお、上記のα及びσｎｅｗΔＴ２−は、例えば実験やシミュレーション等の結果に基づいて予め定められるものである。
【００５８】
一方、時計異常変化検出部４３は、受信機時計ドリフトの変化分が数［１０］を満たす場合は、受信機時計２２の異常変化が発生していないと判断し、処理を終了する。
【００５９】
状態予測値修正部４１ｂは、時計異常変化検出部４３から受信機時計ドリフトの分散の修正値σｎｅｗΔＴ２−を受け取る。そして、状態予測値修正部４１ｂは、状態予測処理部４１ａから受け取った予測誤差の共分散行列Ｐｋ−における受信機時計ドリフトの分散σΔＴ（ｋ）２−を、時計異常変化検出部４３から受け取った受信機時計ドリフトの分散の修正値σｎｅｗΔＴ２−に置き換えることにより、予測誤差の共分散行列Ｐｋ−を修正する（ステップＳ１４）。
【００６０】
そして、ステップＳ１２に進み、状態推定処理部４２は、修正された予測誤差の共分散行列Ｐｋ−に基づいて推定処理を再度行う。
【００６１】
以上のように、本実施形態における測位装置２０によれば、受信機時計ドリフト予測値と受信機時計ドリフト推定値との差分が予め定められた閾値を超えたとき、予測誤差の共分散行列に含まれる受信機時計ドリフトの分散をより大きな値に動的に修正する構成としたので、比較的大きな受信機時計ドリフトが発生した場合でも高精度でリアルタイムに現在位置を測位することができる。
【００６２】
（第２実施形態）
本発明に係る測位装置の第２実施形態について説明する。
【００６３】
図４に示すように、本実施形態における測位装置５０は、第１実施形態における測位装置２０（図２参照）のカルマンフィルタ４０をカルマンフィルタ６０に変更したものである。したがって、カルマンフィルタ６０に係る構成について説明し、第１実施形態の説明と重複する構成の説明は省略する。
【００６４】
カルマンフィルタ６０は、状態予測を行う状態予測処理手段６１と、受信機時計２２から受信時刻情報を取得するとともに状態推定を行う状態推定処理部６２と、受信機時計２２の異常変化を検出する時計異常変化検出部６３と、を備えている。状態予測処理手段６１は、状態を予測する状態予測処理部６１ａと、時計異常変化検出部６３の検出結果に基づいて状態予測値を修正する状態予測値修正部６１ｂと、を備えている。
【００６５】
次に、本実施形態における測位装置５０のカルマンフィルタ６０の動作について図５に基づき説明する。
【００６６】
状態予測処理部６１ａは、状態推定処理部６２から得る１エポック前の利用者位置及び利用者速度、推定誤差の共分散行列から、現エポックの利用者位置及び利用者速度を予測し、予測誤差の共分散行列を、［数５］を用いて算出する（ステップＳ２１）。ここで、モデルにおける受信機時計ドリフトの変動量は、受信機時計ドリフトの安定時におけるＴＣＸＯの周波数短期安定度で見積もった値程度とする。
【００６７】
時計異常変化検出部６３は、状態予測処理部６１ａから、予測された状態変数ベクトルｘｋ―１に含まれる状態変数の内、利用者速度及び受信機時計ドリフト（ｖｘ、ｖｙ、ｖｚ、ΔＴ）と、予測誤差の共分散行列Ｐｋ―１−の部分行列である利用者速度、受信機時計ドリフトの状態変数ベクトルに関する共分散行列を受け取る。この共分散行列を［数１１］に示す。
【００６８】
【数１１】
【００６９】
また、時計異常変化検出部６３は、状態変数ベクトルを［数１２］とおき、観測方程式を［数１３］とおいて［数１４］により、時計異常検出用の状態推定処理を状態推定処理部６２に行わせる（ステップＳ２２）。この場合、状態推定処理部６２は、本発明に係る状態推定手段を構成する。
【００７０】
【数１２】
【００７１】
【数１３】
【００７２】
【数１４】
【００７３】
時計異常変化検出部６３は、ステップＳ２２において推定した受信機時計ドリフトΔ＾Ｔｋと、状態予測処理部６１ａが予測した受信機時計ドリフトΔ＾Ｔｋ−とに基づき、両者の変化分の差（｜Δ＾Ｔｋ−Δ＾Ｔｋ−｜）を計算し、［数１５］により、ＴＣＸＯの周波数短期安定度により見積もった受信機時計ドリフトの標準偏差σΔΔＴと比較することにより、受信機時計２２に異常な変化があったか否かを判断する（ステップＳ２３）。
【００７４】
【数１５】
【００７５】
時計異常変化検出部６３は、受信機時計ドリフトの変化分が［数１５］を満たさない場合は、受信機時計２２の異常変化が発生していると判断し、σΔＴ２−より大きい値σｎｅｗΔＴ２−を受信機時計ドリフトの分散の修正値として状態予測値修正部６１ｂに出力する。
【００７６】
状態予測値修正部６１ｂは、状態予測処理部６１ａから受け取った予測誤差の共分散行列Ｐｋ−における受信機時計ドリフトの分散σΔＴ（ｋ）２−を、時計異常変化検出部６３から受け取った受信機時計ドリフトの分散の修正値σｎｅｗΔＴ２−に置き換えることにより、予測誤差の共分散行列Ｐｋ−を修正する（ステップＳ２４）。
【００７７】
一方、時計異常変化検出部６３は、受信機時計ドリフトの変化分が［数１５］を満たす場合は、受信機時計２２の異常変化が発生していないと判断し、ステップＳ２１で予測された値で状態推定処理部６２に［数７］により状態推定を行わせる（ステップＳ２５）。また、状態推定処理部６２は、ステップＳ２４において予測誤差の共分散行列Ｐｋ−が修正された場合は、修正された予測誤差の共分散行列Ｐｋ−による推定処理をステップＳ２５において行う。この場合、状態推定処理部６２は、本発明に係る状態推定更新手段を構成する。
【００７８】
なお、前述の受信機時計ドリフトの分散の修正に代えて、例えば、［数１１］においてσΔＴ２−の項目をσｎｅｗΔＴ２−に変更して予測誤差の共分散行列を［数１６］とし、［数１７］を満たすような値を割り出す手法を用いてもよい。この場合、σｎｅｗΔＴ２−がσｎｅｗΔΔＴ２を用いて［数１８］により算出される。
【００７９】
【数１６】
【００８０】
【数１７】
【００８１】
【数１８】
【００８２】
ここで、［数１６］の右辺の行列においてＰｋ−の部分行列である速度ｖ、受信機時計ドリフトΔＴの状態変数ベクトルに関する共分散行列について、ｋエポックでの予測値をそのまま用いるのではなく、必要に応じて変更してもかまわない。
【００８３】
以上のように、本実施形態における測位装置５０によれば、利用者速度及び受信機時計ドリフトのみを状態変数として含む状態変数ベクトルの推定値及び推定誤差の共分散行列を算出し、この算出した状態変数ベクトルの推定値に含まれる受信機時計ドリフト推定値と、状態予測処理手段６１が算出した状態変数ベクトルの予測値に含まれる受信機時計ドリフト予測値との差分が予め定められた閾値を超えたとき、状態予測処理手段６１が算出した予測誤差の共分散行列に含まれる受信機時計ドリフトの分散を、より大きい値に修正する構成としたので、比較的大きな受信機時計ドリフトが発生した場合でも高精度でリアルタイムに現在位置を測位することができる。
【産業上の利用可能性】
【００８４】
以上のように、本発明に係る測位装置は、比較的大きな受信機時計ドリフトが発生した場合でも高精度でリアルタイムに現在位置を測位することができるという効果を有し、カルマンフィルタを用いた衛星航法システムの測位装置等として有用である。
【符号の説明】
【００８５】
１０ 受信部
２０、５０ 測位装置
２１ 受信信号解析部（観測データ取得手段）
２２ 受信機時計
３０ 制御部
４０、６０ カルマンフィルタ
４１、６１ 状態予測処理手段
４１ａ、６１ａ 状態予測処理部（状態予測手段）
４１ｂ、６１ｂ 状態予測値修正部（分散修正手段）
４２ 状態推定処理部（状態推定更新手段）
４３、６３ 時計異常変化検出部（差分算出手段）
６２ 状態推定処理部（状態推定手段、状態推定更新手段）
【技術分野】
【０００１】
本発明は、カルマンフィルタを用いた衛星航法システムの測位装置に関する。
【背景技術】
【０００２】
ＧＰＳといった衛星航法システムにおける測位装置は、衛星の送信電波に付加されている情報から得られる電波の送信時刻（以下「衛星送信時刻」）、衛星位置、衛星速度、受信電波の搬送波の測定により得られる利用者位置変化、衛星位置変化、受信機発振器の周波数ずれに起因するドップラシフト等から、利用者位置、利用者速度及び受信機における衛星航法システム系の時刻（以下「受信機時刻」）を推定する装置である。
【０００３】
この測位装置において、利用者位置、利用者速度及び受信機時刻の推定には、前回の状態から今回の状態への変化、及び今回状態と観測値の関係をモデル化し、そのモデル方程式をカルマンフィルタによって解く方法が一般的である。
【０００４】
カルマンフィルタにおいては、利用者位置、利用者速度及び受信機時刻を状態変数とおき、１エポック前の状態変数値とその誤差の共分散行列から予測モデルを用いて現在の状態変数値及びその共分散行列を予測する処理と、現在の観測量と状態推定値の関係式を用いて予測した状態変数値及び共分散行列から現在の状態変数値及び共分散行列を推定する処理が行われる。なお、エポックは観測データの取得時を意味している。
【０００５】
通常、カルマンフィルタを用いて利用者位置、利用者速度及び受信機時刻を推定する場合、状態変数ベクトルｘは、［数１］で表すのが一般的である。
【０００６】
【数１】
【０００７】
ここで、ｐは利用者位置（３次元ベクトル）、Δｔは受信機時計の時刻と衛星航法システムにおける時刻との差（以下「受信機時計オフセット」）、ｖは利用者速度（３次元ベクトル）、ΔＴは受信機時計における単位時間当たりの受信機時計オフセットのずれ（以下「受信機時計ドリフト」）、上付の"Ｔ"は転置行列を示す。なお、利用者位置とは受信機を利用する利用者の位置をいい、利用者速度とは利用者の移動速度をいう。
【０００８】
カルマンフィルタを用いた衛星航法システムの測位装置では、発振器を備えた受信機時計において、受信機時計ドリフトの１エポック当たりの変化量は、ガウス分布に従うと仮定することができ、その分散量は発振器の周波数安定度によって決まる。予測モデルに前記仮定を組み入れることで、受信機時計ドリフトの推定精度を向上させることが可能であり、周波数安定度の高い発振器を用いるほど推定精度は向上する。
【０００９】
通常の安価な衛星航法システムの測位装置においては、発振器に温度補償型水晶発振器（以下「ＴＣＸＯ」）が用いられる。この発振器では、温度が安定している環境では周波数安定度は比較的高いが、その一方で、急激な温度変化が発生した場合や衝撃を受けた場合には、受信機時計ドリフトが大きく変化してしまう。このような状態下でも安定した測位を行うには、受信機時計ドリフトの１エポック当たりの変化量の大きさ（分散）を大きめに設定する必要があった。しかしながら、上記分散を大きめに設定すると、受信機時計ドリフトが安定した状況下において精度が悪化してしまうという問題があった。
【００１０】
このような問題を解決する技術として、適応カルマンフィルタが用いられている（例えば、非特許文献１参照）。非特許文献１に示された適応カルマンフィルタは、観測雑音の分散に基づき与えられたシステム雑音モデルのパラメータが妥当かどうかを判断して妥当でなければ調整することができ、また、連続する数エポックにおいて平均化処理を行って状態推定の信頼性を高めることができるようになっている。
【先行技術文献】
【非特許文献】
【００１１】
【非特許文献１】Y. Bar-Shalom, X. R. Li, and T. Kirubarajan, Estimation with Applications to Tracking and Navigation: Theory, Algorithms, and Software, Wiley, New York, 2001, pp. 424-425.
【発明の概要】
【発明が解決しようとする課題】
【００１２】
しかしながら、従来の適応カルマンフィルタでは、受信機時計ドリフトの変化が安定しているにもかかわらず、観測値に大きな観測誤差が含まれている場合に受信機時計ドリフトに大きな変化が発生していると誤判断する可能性があり、測位精度の劣化をもたらすという課題があった。また、従来の適応カルマンフィルタでは、状態推定の信頼性を高めるため平均化処理を行う構成となっているので、受信機時計ドリフトの変化をリアルタイムに検出できなくなり、測位精度が劣化するという課題があった。
【００１３】
この発明は、上述した課題を解決するためのもので、比較的大きな受信機時計ドリフトが発生した場合でも高精度でリアルタイムに現在位置を測位することができる測位装置を提供することを目的とする。
【課題を解決するための手段】
【００１４】
本発明の測位装置は、衛星航法システムにおいて衛星からの測位信号を受信する受信機の利用者の位置を測位する測位装置であって、前記利用者の位置を示す利用者位置と、前記利用者の移動速度を示す利用者速度と、前記受信機が有する受信機時計の時刻と前記衛星航法システムにおける時刻との差を示す受信機時計オフセットと、前記受信機時計における単位時間当たりの前記受信機時計オフセットのずれを示す受信機時計ドリフトとを状態変数として含む状態変数ベクトルに基づき、前記衛星航法システムに関する状態予測及び状態推定を行うカルマンフィルタと、前記衛星からの測位信号を受信して前記衛星の状態を示す観測データを取得する観測データ取得手段と、を備え、前記カルマンフィルタは、前記状態変数ベクトルの予測値及び予測誤差の共分散行列を算出する状態予測手段と、前記状態予測手段の算出結果及び前記観測データに基づいて前記状態変数ベクトルの推定値及び推定誤差の共分散行列を算出して前記状態予測手段の算出結果を更新する状態推定更新手段と、前記状態予測手段が算出した状態変数ベクトルの予測値に含まれる受信機時計ドリフト予測値と、前記状態推定更新手段が算出した状態変数ベクトルの推定値に含まれる受信機時計ドリフト推定値との差分を算出する差分算出手段と、前記差分が予め定められた閾値を超えたとき、前記予測誤差の共分散行列に含まれる受信機時計ドリフトの分散を、より大きい値に修正する分散修正手段と、を備え、前記状態推定更新手段は、前記分散修正手段が前記受信機時計ドリフトの分散を修正したとき、前記状態予測手段が算出した予測誤差の共分散行列に含まれる受信機時計ドリフトの分散を修正された受信機時計ドリフトの分散に置き換えて前記状態変数ベクトルの推定値及び推定誤差の共分散行列を再計算するものである構成を有している。
【００１５】
この構成により、本発明の測位装置は、受信機時計ドリフト予測値と受信機時計ドリフト推定値との差分が予め定められた閾値を超えたとき、予測誤差の共分散行列に含まれる受信機時計ドリフトの分散を、より大きい値に修正するので、比較的大きな受信機時計ドリフトが発生した場合でも高精度でリアルタイムに現在位置を測位することができる。
【００１６】
また、本発明の測位装置は、衛星航法システムにおいて衛星からの測位信号を受信する受信機の利用者の位置を測位する測位装置であって、前記利用者の位置を示す利用者位置と、前記利用者の移動速度を示す利用者速度と、前記受信機が有する受信機時計の時刻と前記衛星航法システムにおける時刻との差を示す受信機時計オフセットと、前記受信機時計における単位時間当たりの前記受信機時計オフセットのずれを示す受信機時計ドリフトとを状態変数として含む第１の状態変数ベクトルに基づき、前記衛星航法システムに関する状態予測及び状態推定を行うカルマンフィルタと、前記衛星からの測位信号を受信して前記衛星の状態を示す観測データを取得する観測データ取得手段と、を備え、前記カルマンフィルタは、前記第１の状態変数ベクトルの予測値及び予測誤差の共分散行列を算出する状態予測手段と、前記状態予測手段の算出結果の内の前記利用者速度及び前記受信機時計ドリフトのみを状態変数として含む第２の状態変数ベクトルの予測値及び予測誤差の共分散行列と前記観測データとに基づいて前記第２の状態変数ベクトルの推定値及び推定誤差の共分散行列を算出する状態推定手段と、前記状態予測手段が算出した前記第１の状態変数ベクトルの予測値に含まれる受信機時計ドリフト予測値と、前記状態推定手段が算出した前記第２の状態変数ベクトルの推定値に含まれる受信機時計ドリフト推定値との差分を算出する差分算出手段と、前記差分が予め定められた閾値を超えたとき、前記状態予測手段が算出した予測誤差の共分散行列に含まれる受信機時計ドリフトの分散を、より大きい値に修正する分散修正手段と、前記分散修正手段が前記受信機時計ドリフトの分散を修正したとき、前記状態予測手段が算出した予測誤差の共分散行列に含まれる受信機時計ドリフトの分散を修正された受信機時計ドリフトの分散に置き換えて前記第１の状態変数ベクトルの推定値及び推定誤差の共分散行列を算出して前記状態予測手段の算出結果を更新するとともに、前記分散修正手段が前記受信機時計ドリフトの分散を修正しなかったとき、前記状態予測手段の算出結果及び前記観測データに基づいて前記第１の状態変数ベクトルの推定値及び推定誤差の共分散行列を算出して前記状態予測手段の算出結果を更新する状態推定更新手段と、を備えた構成を有している。
【００１７】
この構成により、本発明の測位装置は、利用者速度及び受信機時計ドリフトのみを状態変数として含む第２の状態変数ベクトルの推定値及び推定誤差の共分散行列を状態推定手段が算出し、状態予測手段が算出した第１の状態変数ベクトルの予測値に含まれる受信機時計ドリフト予測値と、状態推定手段が算出した第２の状態変数ベクトルの推定値に含まれる受信機時計ドリフト推定値との差分が予め定められた閾値を超えたとき、状態予測手段が算出した予測誤差の共分散行列に含まれる受信機時計ドリフトの分散を、より大きい値に修正するので、比較的大きな受信機時計ドリフトが発生した場合でも高精度でリアルタイムに現在位置を測位することができる。
【００１８】
さらに、本発明の測位装置は、前記受信機時計が、前記衛星に搭載された時計が有する発振器よりも安定度が低い発振器を備え、前記分散修正手段が、前記受信機時計の発振器の周波数短期安定度により見積もられた値を前記閾値とするものである構成を有している。
【００１９】
この構成により、本発明の測位装置は、温度補償型水晶発振器を代表とする安定度の低い発振器を用いても、高精度でリアルタイムに現在位置を測位することができ、製造コストの低減化を図ることができる。
【発明の効果】
【００２０】
本発明は、比較的大きな受信機時計ドリフトが発生した場合でも高精度でリアルタイムに現在位置を測位することができるという効果を有する測位装置を提供することができるものである。
【図面の簡単な説明】
【００２１】
【図１】本発明に係る測位信号受信機の第１実施形態におけるブロック構成図である。
【図２】本発明に係る測位装置の第１実施形態におけるブロック構成図である。
【図３】本発明に係る測位装置の第１実施形態におけるフローチャートである。
【図４】本発明に係る測位装置の第２実施形態におけるブロック構成図である。
【図５】本発明に係る測位装置の第２実施形態におけるフローチャートである。
【発明を実施するための形態】
【００２２】
まず、本発明の実施形態を説明する前にカルマンフィルタ処理について説明する。
【００２３】
（カルマンフィルタ処理）
前述の［数１］において、利用者位置ｐ、利用者速度ｖは、ＥＣＥＦ座標系のｘ軸、ｙ軸、ｚ軸を用いると、それぞれ、利用者位置ｐ＝［ｐｘ，ｐｙ，ｐｚ］Ｔ、利用者速度ｖ＝［ｖｘ，ｖｙ，ｖｚ］Ｔと表される。したがって、［数１］に示した状態変数ベクトルｘは８変数を含むものとなる。よって、その状態推定値＾ｘの誤差の共分散行列は、［数２］で表される。なお、"＾ｘ"は、ｘの上部に推定値を示すハット（＾）が付されたものを表すものとする。
【００２４】
【数２】
【００２５】
ここで、上式の行列内の各成分は［数３］で示される。但し、［数３］において、ｅｐは利用者位置ｐに関する誤差、ｅΔｔは受信機時計オフセットΔｔに関する誤差、ｅｖは利用者速度ｖに関する誤差、ｅΔＴは受信機時計ドリフトΔＴに関する誤差であり、Ｅ｛｝は期待値を意味する。
【００２６】
【数３】
【００２７】
（ｋ−１）エポックの状態推定値からｋエポックの状態を予測するモデル式（状態方程式）は、［数４］で表される。但し、ここで、ｅΔｐ（ｋ）、ｅΔΔｔ（ｋ）、ｅΔｖ（ｋ）、ｅΔΔＴ（ｋ）は、（ｋ−１）エポックの状態推定値からｋエポックでの状態を行列Ａで予測した値と、実際の値との不確定性を表すシステム雑音ベクトルである。また、Ｉは単位行列である。
【００２８】
【数４】
【００２９】
カルマンフィルタによる予測処理は、［数５］で表される。但し、ＱΔｐ（ｋ）、ＱΔｖ（ｋ）はそれぞれｅΔｐ（ｋ）、ｅΔｖ（ｋ）についての共分散行列であり、σΔΔｔ（ｋ）２、σΔΔＴ（ｋ）２はそれぞれｅΔΔｔ（ｋ）、ｅΔΔＴ（ｋ）についての分散である。また、上付（−）は予測処理で得られた値であることを意味する。
【００３０】
【数５】
【００３１】
観測量と状態推定値の関係式（観測方程式）は、［数６］で表される。但し、各変数の上付のｉ（ｉ＝１、２、・・・ｎ）は衛星の番号を示す。また、ｔｓｖは衛星の衛星送信時刻、ｔｕｓｅｒは受信機時刻、φｄｏｐｐｌｅｒはドップラ観測値、ｐｓａｔ（ｔｓｖ）は利用者位置ｐにおいて衛星送信時刻ｔｓｖの信号を取得したときの衛星位置、Ｉｏｎは電離層遅延誤差、Ｔｒは対流圏遅延量、ｅρは擬似距離ρ(ρ=ｃ（ｔｓｖ−ｔｕｓｅｒ）)の観測誤差、ｅｄｏｐｐｌｅｒはドップラ観測値の観測誤差である。
【００３２】
【数６】
【００３３】
カルマンフィルタによる推定処理は、［数７］で表される。但し、Ｋはカルマンゲイン、Ｐは推定誤差の共分散行列、Ｒは観測雑音ｅｋの共分散行列である。
【００３４】
【数７】
【００３５】
［数７］において、デザイン行列Ｈは、ｈｋ（＾ｘｋ−，０）をｘｋで微分したヤコビアン行列であり、［数８］で表される。
【００３６】
【数８】
【００３７】
ここで、ｃは光速である。また、ｈｉＡＯＡは衛星ｉから受信機への単位方向ベクトルであり、［数９］で表される。
【００３８】
【数９】
【００３９】
システムの状態を精確に推定するため、予測処理と推定処理のそれぞれに対して、システム特性に合致するようなシステム雑音の共分散行列ＱΔｐ（ｋ）、ＱΔｖ（ｋ）及び分散値σΔΔｔ（ｋ）２、σΔΔＴ（ｋ）２と、観測雑音の共分散行列Ｒｋを与えることで、推定の精度を向上させることができる。
【００４０】
（第１実施形態）
次に、本発明に係る測位装置の第１実施形態における構成について説明する。
【００４１】
図１に示すように、本実施形態における測位信号受信機１は、受信部１０、測位装置２０、制御部３０を備えている。この測位信号受信機１は、例えば、車両に搭載されるものである。
【００４２】
受信部１０は、所定の個数（例えば４個）のＧＰＳ衛星からの無線周波数（ＲＦ）帯の電波を受信するアンテナ、ＲＦ帯の信号を中間周波数帯の信号にダウンコンバートする変換回路等を備え、ダウンコンバートした受信信号を測位装置２０に出力するようになっている。
【００４３】
測位装置２０は、例えばマイクロコンピュータで構成され、入力した受信信号を解析し、車両の現在位置（利用者位置）を測位するようになっている。
【００４４】
制御部３０は、例えばマイクロコンピュータで構成され、予めメモリに記憶されたプログラムに基づいて受信部１０及び測位装置２０の動作を制御するようになっている。
【００４５】
図２は、第１実施形態における測位装置２０のブロック構成図である。図２に示すように、測位装置２０は、受信信号を解析する受信信号解析部２１と、受信時刻情報を受信信号解析部２１及び状態推定処理部４２（後述）に提供する受信機時計２２と、利用者位置、利用者速度及び受信機時刻を推定するカルマンフィルタ４０と、を備えている。本実施形態における受信機時計２２は、発振器としてＴＣＸＯを備えたものであるが、これに限定されず、他の発振器を備えた構成としてもよい。なお、受信信号解析部２１は、本発明に係る観測データ取得手段を構成する。
【００４６】
カルマンフィルタ４０は、状態予測を行う状態予測処理手段４１と、状態推定を行う状態推定処理部４２と、受信機時計２２の異常変化をリアルタイムに検出する時計異常変化検出部４３と、を備えている。なお、時計異常変化検出部４３は、本発明に係る差分算出手段を構成する。
【００４７】
状態予測処理手段４１は、状態推定処理部４２から得る１エポック前の状態変数ベクトルに基づいて現エポックの状態変数ベクトルを予測する状態予測処理部４１ａと、時計異常変化検出部４３の検出結果に基づいて状態予測値を動的に修正する状態予測値修正部４１ｂと、を備えている。なお、状態予測処理部４１ａ及び状態予測値修正部４１ｂは、それぞれ、本発明に係る状態予測手段及び分散修正手段を構成する。
【００４８】
状態推定処理部４２は、衛星に関する観測データを受信信号解析部２１から入力するとともに、状態予測処理手段４１から現エポックでの状態変数ベクトル、予測誤差の共分散行列を入力し、状態変数ベクトルを更新して状態予測処理部４１ａに出力するようになっている。なお、状態推定処理部４２は、本発明に係る状態推定更新手段を構成する。
【００４９】
次に、本実施形態における測位装置２０のカルマンフィルタ４０の動作について説明する。
【００５０】
受信信号解析部２１は、入力した受信信号を解析し、各衛星の衛星送信時刻、衛星位置、衛星速度の情報を得る。また、受信信号解析部２１は、受信した電波の搬送波に基づいてドップラ観測値を算出する。そして、受信信号解析部２１は、算出した結果を状態推定処理部４２に渡す。
【００５１】
以下、カルマンフィルタ４０の動作を図３に基づき説明する。
【００５２】
状態予測処理手段４１の状態予測処理部４１ａは、状態推定処理部４２から得る１エポック前（ｋ−１）の利用者位置及び利用者速度、推定誤差の共分散行列から、現エポックの利用者位置及び利用者速度を予測し、予測誤差の共分散行列Ｐｋ−を［数５］を用いて算出する（ステップＳ１１）。
【００５３】
ここで、モデルにおける受信機時計ドリフトの変化量は、受信機時計ドリフトの安定時におけるＴＣＸＯの周波数短期安定度で見積もった値程度とする。なお、周波数短期安定度とは、比較的短い時間周期内におけるＴＣＸＯの出力周波数の揺らぎ（不規則変動）を表したものをいう。
【００５４】
状態推定処理部４２は、受信信号解析部２１から衛星送信時刻、衛星位置、衛星速度、ドップラ観測値、受信時刻等の各データを入力する。また、状態推定処理部４２は、状態予測処理手段４１が予測した現エポックの状態変数ベクトルｘｋ−、共分散行列Ｐｋ−のデータを入力する。そして、状態推定処理部４２は、入力したこれらのデータを用いて、［数７］により、カルマンゲインを求め、状態変数ベクトル及び共分散行列を推定する（ステップＳ１２）。但し、状態推定処理部４２は、状態予測値修正部４１ｂによる修正があった場合は、受信機時計ドリフトの分散値が修正された共分散行列Ｐｋ−のデータを状態予測値修正部４１ｂから入力する。
【００５５】
時計異常変化検出部４３は、状態推定処理部４２が推定した状態変数ベクトル＾ｘｋに含まれる受信機時計ドリフトΔ＾Ｔｋと、状態予測処理部４１ａが予測した状態変数ベクトル＾ｘｋ−に含まれる受信機時計ドリフトΔ＾Ｔｋ−とを状態推定処理部４２から入力する。また、時計異常変化検出部４３は、両者の変化分の差（｜Δ＾Ｔｋ−Δ＾Ｔｋ−｜）を計算し、［数１０］により、ＴＣＸＯの周波数短期安定度により見積もった受信機時計ドリフトの標準偏差σΔΔＴと比較することにより、受信機時計２２に異常な変化があったか否かを判断する（ステップＳ１３）。なお、αは、許容量範囲を決めるスレッショルド係数である。
【００５６】
【数１０】
【００５７】
時計異常変化検出部４３は、受信機時計ドリフトの変化分が数［１０］を満たさない場合は、受信機時計２２の異常変化が発生していると判断し、σΔＴ２−より大きい値σｎｅｗΔＴ２−を受信機時計ドリフトの分散の修正値として状態予測値修正部４１ｂに出力する。なお、上記のα及びσｎｅｗΔＴ２−は、例えば実験やシミュレーション等の結果に基づいて予め定められるものである。
【００５８】
一方、時計異常変化検出部４３は、受信機時計ドリフトの変化分が数［１０］を満たす場合は、受信機時計２２の異常変化が発生していないと判断し、処理を終了する。
【００５９】
状態予測値修正部４１ｂは、時計異常変化検出部４３から受信機時計ドリフトの分散の修正値σｎｅｗΔＴ２−を受け取る。そして、状態予測値修正部４１ｂは、状態予測処理部４１ａから受け取った予測誤差の共分散行列Ｐｋ−における受信機時計ドリフトの分散σΔＴ（ｋ）２−を、時計異常変化検出部４３から受け取った受信機時計ドリフトの分散の修正値σｎｅｗΔＴ２−に置き換えることにより、予測誤差の共分散行列Ｐｋ−を修正する（ステップＳ１４）。
【００６０】
そして、ステップＳ１２に進み、状態推定処理部４２は、修正された予測誤差の共分散行列Ｐｋ−に基づいて推定処理を再度行う。
【００６１】
以上のように、本実施形態における測位装置２０によれば、受信機時計ドリフト予測値と受信機時計ドリフト推定値との差分が予め定められた閾値を超えたとき、予測誤差の共分散行列に含まれる受信機時計ドリフトの分散をより大きな値に動的に修正する構成としたので、比較的大きな受信機時計ドリフトが発生した場合でも高精度でリアルタイムに現在位置を測位することができる。
【００６２】
（第２実施形態）
本発明に係る測位装置の第２実施形態について説明する。
【００６３】
図４に示すように、本実施形態における測位装置５０は、第１実施形態における測位装置２０（図２参照）のカルマンフィルタ４０をカルマンフィルタ６０に変更したものである。したがって、カルマンフィルタ６０に係る構成について説明し、第１実施形態の説明と重複する構成の説明は省略する。
【００６４】
カルマンフィルタ６０は、状態予測を行う状態予測処理手段６１と、受信機時計２２から受信時刻情報を取得するとともに状態推定を行う状態推定処理部６２と、受信機時計２２の異常変化を検出する時計異常変化検出部６３と、を備えている。状態予測処理手段６１は、状態を予測する状態予測処理部６１ａと、時計異常変化検出部６３の検出結果に基づいて状態予測値を修正する状態予測値修正部６１ｂと、を備えている。
【００６５】
次に、本実施形態における測位装置５０のカルマンフィルタ６０の動作について図５に基づき説明する。
【００６６】
状態予測処理部６１ａは、状態推定処理部６２から得る１エポック前の利用者位置及び利用者速度、推定誤差の共分散行列から、現エポックの利用者位置及び利用者速度を予測し、予測誤差の共分散行列を、［数５］を用いて算出する（ステップＳ２１）。ここで、モデルにおける受信機時計ドリフトの変動量は、受信機時計ドリフトの安定時におけるＴＣＸＯの周波数短期安定度で見積もった値程度とする。
【００６７】
時計異常変化検出部６３は、状態予測処理部６１ａから、予測された状態変数ベクトルｘｋ―１に含まれる状態変数の内、利用者速度及び受信機時計ドリフト（ｖｘ、ｖｙ、ｖｚ、ΔＴ）と、予測誤差の共分散行列Ｐｋ―１−の部分行列である利用者速度、受信機時計ドリフトの状態変数ベクトルに関する共分散行列を受け取る。この共分散行列を［数１１］に示す。
【００６８】
【数１１】
【００６９】
また、時計異常変化検出部６３は、状態変数ベクトルを［数１２］とおき、観測方程式を［数１３］とおいて［数１４］により、時計異常検出用の状態推定処理を状態推定処理部６２に行わせる（ステップＳ２２）。この場合、状態推定処理部６２は、本発明に係る状態推定手段を構成する。
【００７０】
【数１２】
【００７１】
【数１３】
【００７２】
【数１４】
【００７３】
時計異常変化検出部６３は、ステップＳ２２において推定した受信機時計ドリフトΔ＾Ｔｋと、状態予測処理部６１ａが予測した受信機時計ドリフトΔ＾Ｔｋ−とに基づき、両者の変化分の差（｜Δ＾Ｔｋ−Δ＾Ｔｋ−｜）を計算し、［数１５］により、ＴＣＸＯの周波数短期安定度により見積もった受信機時計ドリフトの標準偏差σΔΔＴと比較することにより、受信機時計２２に異常な変化があったか否かを判断する（ステップＳ２３）。
【００７４】
【数１５】
【００７５】
時計異常変化検出部６３は、受信機時計ドリフトの変化分が［数１５］を満たさない場合は、受信機時計２２の異常変化が発生していると判断し、σΔＴ２−より大きい値σｎｅｗΔＴ２−を受信機時計ドリフトの分散の修正値として状態予測値修正部６１ｂに出力する。
【００７６】
状態予測値修正部６１ｂは、状態予測処理部６１ａから受け取った予測誤差の共分散行列Ｐｋ−における受信機時計ドリフトの分散σΔＴ（ｋ）２−を、時計異常変化検出部６３から受け取った受信機時計ドリフトの分散の修正値σｎｅｗΔＴ２−に置き換えることにより、予測誤差の共分散行列Ｐｋ−を修正する（ステップＳ２４）。
【００７７】
一方、時計異常変化検出部６３は、受信機時計ドリフトの変化分が［数１５］を満たす場合は、受信機時計２２の異常変化が発生していないと判断し、ステップＳ２１で予測された値で状態推定処理部６２に［数７］により状態推定を行わせる（ステップＳ２５）。また、状態推定処理部６２は、ステップＳ２４において予測誤差の共分散行列Ｐｋ−が修正された場合は、修正された予測誤差の共分散行列Ｐｋ−による推定処理をステップＳ２５において行う。この場合、状態推定処理部６２は、本発明に係る状態推定更新手段を構成する。
【００７８】
なお、前述の受信機時計ドリフトの分散の修正に代えて、例えば、［数１１］においてσΔＴ２−の項目をσｎｅｗΔＴ２−に変更して予測誤差の共分散行列を［数１６］とし、［数１７］を満たすような値を割り出す手法を用いてもよい。この場合、σｎｅｗΔＴ２−がσｎｅｗΔΔＴ２を用いて［数１８］により算出される。
【００７９】
【数１６】
【００８０】
【数１７】
【００８１】
【数１８】
【００８２】
ここで、［数１６］の右辺の行列においてＰｋ−の部分行列である速度ｖ、受信機時計ドリフトΔＴの状態変数ベクトルに関する共分散行列について、ｋエポックでの予測値をそのまま用いるのではなく、必要に応じて変更してもかまわない。
【００８３】
以上のように、本実施形態における測位装置５０によれば、利用者速度及び受信機時計ドリフトのみを状態変数として含む状態変数ベクトルの推定値及び推定誤差の共分散行列を算出し、この算出した状態変数ベクトルの推定値に含まれる受信機時計ドリフト推定値と、状態予測処理手段６１が算出した状態変数ベクトルの予測値に含まれる受信機時計ドリフト予測値との差分が予め定められた閾値を超えたとき、状態予測処理手段６１が算出した予測誤差の共分散行列に含まれる受信機時計ドリフトの分散を、より大きい値に修正する構成としたので、比較的大きな受信機時計ドリフトが発生した場合でも高精度でリアルタイムに現在位置を測位することができる。
【産業上の利用可能性】
【００８４】
以上のように、本発明に係る測位装置は、比較的大きな受信機時計ドリフトが発生した場合でも高精度でリアルタイムに現在位置を測位することができるという効果を有し、カルマンフィルタを用いた衛星航法システムの測位装置等として有用である。
【符号の説明】
【００８５】
１０ 受信部
２０、５０ 測位装置
２１ 受信信号解析部（観測データ取得手段）
２２ 受信機時計
３０ 制御部
４０、６０ カルマンフィルタ
４１、６１ 状態予測処理手段
４１ａ、６１ａ 状態予測処理部（状態予測手段）
４１ｂ、６１ｂ 状態予測値修正部（分散修正手段）
４２ 状態推定処理部（状態推定更新手段）
４３、６３ 時計異常変化検出部（差分算出手段）
６２ 状態推定処理部（状態推定手段、状態推定更新手段）
【特許請求の範囲】
【請求項１】
衛星航法システムにおいて衛星からの測位信号を受信する受信機の利用者の位置を測位する測位装置であって、
前記利用者の位置を示す利用者位置と、前記利用者の移動速度を示す利用者速度と、前記受信機が有する受信機時計の時刻と前記衛星航法システムにおける時刻との差を示す受信機時計オフセットと、前記受信機時計における単位時間当たりの前記受信機時計オフセットのずれを示す受信機時計ドリフトとを状態変数として含む状態変数ベクトルに基づき、前記衛星航法システムに関する状態予測及び状態推定を行うカルマンフィルタと、
前記衛星からの測位信号を受信して前記衛星の状態を示す観測データを取得する観測データ取得手段と、を備え、
前記カルマンフィルタは、
前記状態変数ベクトルの予測値及び予測誤差の共分散行列を算出する状態予測手段と、
前記状態予測手段の算出結果及び前記観測データに基づいて前記状態変数ベクトルの推定値及び推定誤差の共分散行列を算出して前記状態予測手段の算出結果を更新する状態推定更新手段と、
前記状態予測手段が算出した状態変数ベクトルの予測値に含まれる受信機時計ドリフト予測値と、前記状態推定更新手段が算出した状態変数ベクトルの推定値に含まれる受信機時計ドリフト推定値との差分を算出する差分算出手段と、
前記差分が予め定められた閾値を超えたとき、前記予測誤差の共分散行列に含まれる受信機時計ドリフトの分散を、より大きい値に修正する分散修正手段と、を備え、
前記状態推定更新手段は、前記分散修正手段が前記受信機時計ドリフトの分散を修正したとき、前記状態予測手段が算出した予測誤差の共分散行列に含まれる受信機時計ドリフトの分散を修正された受信機時計ドリフトの分散に置き換えて前記状態変数ベクトルの推定値及び推定誤差の共分散行列を再計算するものであることを特徴とする測位装置。
【請求項２】
衛星航法システムにおいて衛星からの測位信号を受信する受信機の利用者の位置を測位する測位装置であって、
前記利用者の位置を示す利用者位置と、前記利用者の移動速度を示す利用者速度と、前記受信機が有する受信機時計の時刻と前記衛星航法システムにおける時刻との差を示す受信機時計オフセットと、前記受信機時計における単位時間当たりの前記受信機時計オフセットのずれを示す受信機時計ドリフトとを状態変数として含む第１の状態変数ベクトルに基づき、前記衛星航法システムに関する状態予測及び状態推定を行うカルマンフィルタと、
前記衛星からの測位信号を受信して前記衛星の状態を示す観測データを取得する観測データ取得手段と、を備え、
前記カルマンフィルタは、
前記第１の状態変数ベクトルの予測値及び予測誤差の共分散行列を算出する状態予測手段と、
前記状態予測手段の算出結果の内の前記利用者速度及び前記受信機時計ドリフトのみを状態変数として含む第２の状態変数ベクトルの予測値及び予測誤差の共分散行列と前記観測データとに基づいて前記第２の状態変数ベクトルの推定値及び推定誤差の共分散行列を算出する状態推定手段と、
前記状態予測手段が算出した前記第１の状態変数ベクトルの予測値に含まれる受信機時計ドリフト予測値と、前記状態推定手段が算出した前記第２の状態変数ベクトルの推定値に含まれる受信機時計ドリフト推定値との差分を算出する差分算出手段と、
前記差分が予め定められた閾値を超えたとき、前記状態予測手段が算出した予測誤差の共分散行列に含まれる受信機時計ドリフトの分散を、より大きい値に修正する分散修正手段と、
前記分散修正手段が前記受信機時計ドリフトの分散を修正したとき、前記状態予測手段が算出した予測誤差の共分散行列に含まれる受信機時計ドリフトの分散を修正された受信機時計ドリフトの分散に置き換えて前記第１の状態変数ベクトルの推定値及び推定誤差の共分散行列を算出して前記状態予測手段の算出結果を更新するとともに、前記分散修正手段が前記受信機時計ドリフトの分散を修正しなかったとき、前記状態予測手段の算出結果及び前記観測データに基づいて前記第１の状態変数ベクトルの推定値及び推定誤差の共分散行列を算出して前記状態予測手段の算出結果を更新する状態推定更新手段と、を備えたことを特徴とする測位装置。
【請求項３】
前記受信機時計は、前記衛星に搭載された時計が有する発振器よりも安定度が低い発振器を備え、
前記分散修正手段は、前記受信機時計の発振器の周波数短期安定度により見積もられた値を前記閾値とするものであることを特徴とする請求項１又は請求項２に記載の測位装置。
【請求項１】
衛星航法システムにおいて衛星からの測位信号を受信する受信機の利用者の位置を測位する測位装置であって、
前記利用者の位置を示す利用者位置と、前記利用者の移動速度を示す利用者速度と、前記受信機が有する受信機時計の時刻と前記衛星航法システムにおける時刻との差を示す受信機時計オフセットと、前記受信機時計における単位時間当たりの前記受信機時計オフセットのずれを示す受信機時計ドリフトとを状態変数として含む状態変数ベクトルに基づき、前記衛星航法システムに関する状態予測及び状態推定を行うカルマンフィルタと、
前記衛星からの測位信号を受信して前記衛星の状態を示す観測データを取得する観測データ取得手段と、を備え、
前記カルマンフィルタは、
前記状態変数ベクトルの予測値及び予測誤差の共分散行列を算出する状態予測手段と、
前記状態予測手段の算出結果及び前記観測データに基づいて前記状態変数ベクトルの推定値及び推定誤差の共分散行列を算出して前記状態予測手段の算出結果を更新する状態推定更新手段と、
前記状態予測手段が算出した状態変数ベクトルの予測値に含まれる受信機時計ドリフト予測値と、前記状態推定更新手段が算出した状態変数ベクトルの推定値に含まれる受信機時計ドリフト推定値との差分を算出する差分算出手段と、
前記差分が予め定められた閾値を超えたとき、前記予測誤差の共分散行列に含まれる受信機時計ドリフトの分散を、より大きい値に修正する分散修正手段と、を備え、
前記状態推定更新手段は、前記分散修正手段が前記受信機時計ドリフトの分散を修正したとき、前記状態予測手段が算出した予測誤差の共分散行列に含まれる受信機時計ドリフトの分散を修正された受信機時計ドリフトの分散に置き換えて前記状態変数ベクトルの推定値及び推定誤差の共分散行列を再計算するものであることを特徴とする測位装置。
【請求項２】
衛星航法システムにおいて衛星からの測位信号を受信する受信機の利用者の位置を測位する測位装置であって、
前記利用者の位置を示す利用者位置と、前記利用者の移動速度を示す利用者速度と、前記受信機が有する受信機時計の時刻と前記衛星航法システムにおける時刻との差を示す受信機時計オフセットと、前記受信機時計における単位時間当たりの前記受信機時計オフセットのずれを示す受信機時計ドリフトとを状態変数として含む第１の状態変数ベクトルに基づき、前記衛星航法システムに関する状態予測及び状態推定を行うカルマンフィルタと、
前記衛星からの測位信号を受信して前記衛星の状態を示す観測データを取得する観測データ取得手段と、を備え、
前記カルマンフィルタは、
前記第１の状態変数ベクトルの予測値及び予測誤差の共分散行列を算出する状態予測手段と、
前記状態予測手段の算出結果の内の前記利用者速度及び前記受信機時計ドリフトのみを状態変数として含む第２の状態変数ベクトルの予測値及び予測誤差の共分散行列と前記観測データとに基づいて前記第２の状態変数ベクトルの推定値及び推定誤差の共分散行列を算出する状態推定手段と、
前記状態予測手段が算出した前記第１の状態変数ベクトルの予測値に含まれる受信機時計ドリフト予測値と、前記状態推定手段が算出した前記第２の状態変数ベクトルの推定値に含まれる受信機時計ドリフト推定値との差分を算出する差分算出手段と、
前記差分が予め定められた閾値を超えたとき、前記状態予測手段が算出した予測誤差の共分散行列に含まれる受信機時計ドリフトの分散を、より大きい値に修正する分散修正手段と、
前記分散修正手段が前記受信機時計ドリフトの分散を修正したとき、前記状態予測手段が算出した予測誤差の共分散行列に含まれる受信機時計ドリフトの分散を修正された受信機時計ドリフトの分散に置き換えて前記第１の状態変数ベクトルの推定値及び推定誤差の共分散行列を算出して前記状態予測手段の算出結果を更新するとともに、前記分散修正手段が前記受信機時計ドリフトの分散を修正しなかったとき、前記状態予測手段の算出結果及び前記観測データに基づいて前記第１の状態変数ベクトルの推定値及び推定誤差の共分散行列を算出して前記状態予測手段の算出結果を更新する状態推定更新手段と、を備えたことを特徴とする測位装置。
【請求項３】
前記受信機時計は、前記衛星に搭載された時計が有する発振器よりも安定度が低い発振器を備え、
前記分散修正手段は、前記受信機時計の発振器の周波数短期安定度により見積もられた値を前記閾値とするものであることを特徴とする請求項１又は請求項２に記載の測位装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図２】
【図３】
【図４】
【図５】
【公開番号】特開２０１２−５２９５４（Ｐ２０１２−５２９５４Ａ）
【公開日】平成２４年３月１５日（２０１２．３．１５）
【国際特許分類】
【出願番号】特願２０１０−１９６７９５（Ｐ２０１０−１９６７９５）
【出願日】平成２２年９月２日（２０１０．９．２）
【出願人】（０００００４３３０）日本無線株式会社 (1,186)
【Ｆターム（参考）】
【公開日】平成２４年３月１５日（２０１２．３．１５）
【国際特許分類】
【出願日】平成２２年９月２日（２０１０．９．２）
【出願人】（０００００４３３０）日本無線株式会社 (1,186)
【Ｆターム（参考）】
[ Back to top ]