測位方法、プログラム及び測位装置
【課題】IMM演算を用いた測位方法において、モデル確率を正しく求めるための新たな手法を提案すること。
【解決手段】カルマンフィルタ処理それぞれの尤度指標値「Vx」が算出され、この尤度指標値「Vx」のカルマンフィルタ処理間の相対値が算出される。そして、算出された相対値を用いてカルマンフィルタ処理それぞれのモデル確率「μ」が算出され、このモデル確率「μ」を用いてカルマンフィルタ処理それぞれの処理結果が合成されることで測位位置が決定される。
【解決手段】カルマンフィルタ処理それぞれの尤度指標値「Vx」が算出され、この尤度指標値「Vx」のカルマンフィルタ処理間の相対値が算出される。そして、算出された相対値を用いてカルマンフィルタ処理それぞれのモデル確率「μ」が算出され、このモデル確率「μ」を用いてカルマンフィルタ処理それぞれの処理結果が合成されることで測位位置が決定される。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、測位装置が、各カルマンフィルタ処理それぞれをモデルとみなし、前記カルマンフィルタ処理それぞれの処理結果を所定のモデル確率で重みづけして合成するIMM演算を行って測位する測位方法等に関する。
【背景技術】
【0002】
人工衛星を利用した測位システムとしては、GPS(Global Positioning System)が広く知られており、携帯型電話機やカーナビゲーション装置等に内蔵された測位装置に利用されている。GPSでは、自機の位置を示す3次元の座標値と、時計誤差との4つのパラメータの値を、複数のGPS衛星の位置や各GPS衛星から自機までの擬似距離等の情報に基づいて求める測位演算を行うことで、自機の現在位置を測位する。
【0003】
測位演算としては、最小二乗法を用いた測位演算や、カルマンフィルタを用いた測位演算が知られている。そして、カルマンフィルタを用いた測位演算を応用した技術として、測位装置の移動状態に対応する複数のカルマンフィルタモデルを用意して各モデルについて測位演算を行い、その測位演算の結果を合成して測位位置を決定するインタラクティブミキシングモデル(Interactive Mixing Model)演算(以下、「IMM演算」と称す。)が知られている(例えば、非特許文献1)。
【非特許文献1】Yaakov Bar-Shalom, X.Rong Li, Thiagalingam Kirubarajan著、「Estimation with Applications to Tracking and Navigation」、(米国)、JOHN WILEY & SONS INC、2001年、p.92−93、453−457
【発明の開示】
【発明が解決しようとする課題】
【0004】
IMM演算による測位では、カルマンフィルタの入力値となる外部観測量の尤もらしさ(以下、「尤度」と称す。)を、尤度関数と呼ばれる正規分布(ガウス分布)に従った関数から算出し、この算出した尤度を用いて、各カルマンフィルタモデルの確率(以下、「モデル確率」と称す。)を算出する。そして、算出したモデル確率を用いて各カルマンフィルタモデルの処理結果を合成することで、測位装置の現在位置を求める。
【0005】
しかし、尤度関数はEXP(Exponential)関数で表されるため、EXP関数の指数部分が非常に小さな値(例えば「−20以下」)になると、尤度は限りなく「0」に近くなり、計算機で計算しようとしても演算可能な数値範囲の関係から結局「0」になってしまう。モデル確率は尤度を用いて算出するため、尤度が「0」になってしまうと、モデル確率を正しく算出することができず、その結果、測位位置を求めることができないという大きな問題があった。
【0006】
本発明は、上述した課題に鑑みて為されたものである。
【課題を解決するための手段】
【0007】
以上の課題を解決するための第1の発明は、測位装置が、各カルマンフィルタ処理それぞれをモデルとみなし、前記カルマンフィルタ処理それぞれの処理結果を所定のモデル確率で重みづけして合成するIMM演算を行って測位する測位方法であって、前記カルマンフィルタ処理それぞれの尤度指標値を算出することと、前記算出された尤度指標値の前記カルマンフィルタ処理間の相対値を算出することと、前記算出された相対値を用いて前記カルマンフィルタ処理それぞれの前記モデル確率を算出することと、前記算出されたモデル確率を用いて前記カルマンフィルタ処理それぞれの処理結果を合成することで測位位置を決定することと、を含む測位方法である。
【0008】
また、他の発明として、各カルマンフィルタ処理それぞれをモデルとみなし、前記カルマンフィルタ処理それぞれの処理結果を所定のモデル確率で重みづけして合成するIMM演算を行って測位する測位装置であって、前記カルマンフィルタ処理それぞれの尤度指標値を算出する指標値算出部と、前記算出された尤度指標値の前記カルマンフィルタ処理間の相対値を算出する相対値算出部と、前記算出された相対値を用いて前記カルマンフィルタ処理それぞれの前記モデル確率を算出するモデル確率算出部と、前記算出されたモデル確率を用いて前記カルマンフィルタ処理それぞれの処理結果を合成することで測位位置を決定する測位位置決定部と、を備えた測位装置を構成してもよい。
【0009】
この第1の発明等によれば、カルマンフィルタ処理それぞれの尤度指標値が算出され、この尤度指標値のカルマンフィルタ処理間の相対値が算出される。そして、算出された相対値を用いてカルマンフィルタ処理それぞれのモデル確率が算出され、このモデル確率を用いてカルマンフィルタ処理それぞれの処理結果が合成されることで測位位置が決定される。
【0010】
ここで、尤度指標値とは、カルマンフィルタの入力値となる外部観測量の尤度を算出するための尤度関数の指数部分のことを指す。IMM演算におけるモデル確率は、尤度指標値を用いて算出した外部観測量の尤度を基に算出することができるが、尤度指標値が非常に小さな値である場合には、計算機で計算しようとしても演算可能な数値範囲の関係で尤度が「0」になってしまい、モデル確率を正しく求めることができない。
【0011】
本願発明者は、この問題を解決するため、尤度指標値をそのまま用いるのではなく、複数のカルマンフィルタ処理間の尤度指標値の相対値を用いることで、モデル確率を正しく求めることができることを発見した。これは、個々の尤度指標値が非常に小さな値であったとしても、尤度指標値の相対値は概ね「0」の近傍の値となり、指数関数に当該相対値を与えて計算すると「0」とはならない点に着目したものである。かかる原理により、モデル確率を正しく算出することが可能となり、その結果、正確な測位位置の算出を実現し得る。
【0012】
また、第2の発明として、第1の発明の測位方法であって、前記モデル確率を算出することは、前記相対値が小さいほど値が小さくなる所定の指数関数を用いて前記モデル確率を算出することである測位方法を構成してもよい。
【0013】
この第2の発明によれば、カルマンフィルタ処理間の尤度指標値の相対値が小さいほど値が小さくなる指数関数(例えば、尤度指標値の相対値を変数とするEXP関数)を用いてモデル確率が算出される。
【0014】
また、第3の発明として、第2の発明の測位方法であって、前記モデル確率を算出することは、前記相対値を用いて算出した前記所定の指数関数の値が極限値相当値を示す条件である所定の極限値条件を満たすか否かを判定することと、前記所定の極限値条件を満たす場合に、前記指数関数の値を極限値とみなして前記モデル確率を算出することと、を含む測位方法を構成してもよい。
【0015】
この第3の発明によれば、カルマンフィルタ処理間の尤度指標値の相対値を用いて算出した指数関数の値が極限値条件を満たす場合は、当該指数関数の値を極限値とみなしてモデル確率が算出される。これにより、指数関数の値が0に収束したり、+∞に発散する場合であっても、計算機によってモデル確率を求めることが可能となる。
【0016】
また、第4の発明として、第3の発明の測位方法であって、前記所定の極限値条件には、最大値相当値を示す条件と最小値相当値を示す条件とが含まれ、前記モデル確率を算出することは、前記所定の極限値条件を満たす場合に、前記最大値相当値を示す条件を満たすか、前記最小値相当値を示す条件を満たすかに応じて、前記モデル確率を1又は0とすることを含む測位方法を構成してもよい。
【0017】
この第4の発明によれば、カルマンフィルタ処理間の尤度指標値の相対値を用いて算出した指数関数の値が最大値相当値を示す条件を満たすか、最小値相当値を満たすかに応じて、モデル確率が1又は0とされる。
【0018】
また、第5の発明として、第1〜第4の何れかの発明の測位方法であって、前記複数のカルマンフィルタ処理には、前記測位装置の移動状態として、停止状態にあることを前提としたカルマンフィルタ処理と、等速移動状態にあることを前提としたカルマンフィルタ処理と、定加速度状態にあることを前提としたカルマンフィルタ処理との3種類のカルマンフィルタ処理が少なくとも含まれる測位方法を構成してもよい。
【0019】
この第5の発明によれば、測位装置の移動状態として、停止状態と、等速移動状態と、定加速度状態とに対応する3種類のカルマンフィルタ処理が行われ、これらのカルマンフィルタ処理の処理結果が合成される。
【0020】
また、第6の発明として、第1〜第5の何れかの発明の測位方法を前記測位装置に内蔵されたコンピュータに実行させるためのプログラムを構成してもよい。
【発明を実施するための最良の形態】
【0021】
1.原理
図1は、本実施形態におけるインタラクティブミキシングモデル(Interactive Mixing Model:以下、「IMM」と称す。)の概要を説明するための図である。ここでは、説明を分かりやすくするため、複数の処理回路ブロックにおける演算によってIMM演算が実現されるものとして説明するが、単独のプロセッサによってIMM演算を実現することも可能であり、図2及び図3の処理フローの説明においては単独のプロセッサにより実行するものとして説明する。
【0022】
IMMは、複数のカルマンフィルタモデル(Kalman Filter Model:以下、「KFモデル」と称す。)それぞれの処理結果をモデル確率「μ」と呼ばれる重みで合成することで、最終的な出力値を決定するモデルである。図1では、第1KFモデル〜第nKFモデルの「n個」のKFモデルを含むIMMを図示している。
【0023】
また、説明の簡明化のため、各KFモデルに対して、単一の外部観測量(本実施形態では「メジャメント実測値」が相当する。)「Z」が与えられるものとして説明するが、複数のメジャメント実測値が与えられる場合についても同様である。メジャメント実測値は、IMM演算を適用するシステムに応じて適宜設定することができる。
【0024】
IMM演算では、ミキサが、各KFモデル間の遷移確率を示すモデル間遷移確率「p」と、1つ前のステップにおいてモデル確率更新部により更新されたモデル確率「μ:μ1,μ2・・・,μn」と、各KFモデルそれぞれにおいて算出された状態ベクトルの計算値「X:X1,X2,・・・,Xn」及び誤差共分散の計算値「P:P1,P2,・・・,Pn」とを用いて、各KFモデルそれぞれの状態ベクトル及び誤差共分散の今回の計算における初期値「X0:X01,X02,・・・,X0n」及び「P0:P01,P02,・・・,P0n」を算出し、計算結果を、対応するKFモデルにそれぞれ出力する。
【0025】
また、ミキサは、モデル間遷移確率「p」と、1つ前のステップにおいてモデル確率更新部により更新されたモデル確率「μ」とを用いて、規格化定数「CN:C1,C2,・・・,Cn」と呼ばれる定数を算出して、モデル確率更新部に出力する。
【0026】
各KFモデルは、ミキサから入力した初期値「X0」及び「P0」と、メジャメント実測値「Z:Z1,Z2,・・・,Zn」とを用いて、公知のカルマンフィルタと同様、予測演算と補正演算とを行うことで、状態ベクトル及び誤差共分散の今回の計算値を算出して、モデル確率更新部に出力する。また、各KFモデルは、尤度指標値「Vx:Vx1,Vx2,・・・,Vxn」と重み係数「β:β1,β2,・・・,βn」と呼ばれる値を算出して、モデル確率更新部に出力する。
【0027】
尤度指標値「Vx」は、メジャメント実測値「Z」の尤もらしさ(以下、「尤度」と称す。)を算出するための関数である尤度関数の指数部分として表される値である。また、重み係数「β」は、尤度関数の係数部分として表される値である。
【0028】
モデル確率更新部は、ミキサから入力した規格化定数「CN」と、KFモデルから入力した尤度指標値「Vx」及び重み係数「β」とを用いて、モデル確率「μ」を新たに算出・更新する。そして、KFモデルから入力した状態ベクトル及び誤差共分散それぞれを、算出したモデル確率「μ」で重み付けして合成することで、状態ベクトルの出力値「X」及び誤差共分散の出力値「P」を算出する。
【0029】
次に、実際の数式を参照しながら、単一のプロセッサでIMM演算を実現する場合の処理の流れについて説明する。以下の数式において、各変数の添え字の「j」は、j番目のKFモデルに対応する変数であることを示しているものとする。
【0030】
図2は、プロセッサの一種であるCPU(Central Processing Unit)が実行するIMM演算処理の流れを示すフローチャートである。
先ず、CPUは、モデル間遷移確率「p」を取得する(ステップS1)。モデル間遷移確率「p」は、次式(1)に示すようなn×nの行列で与えられる。
【数1】
【0031】
行列の成分「pnm」は、n番目のKFモデルからm番目のKFモデルへの遷移の確率を示している。例えば、行列の成分「p11」は、第1KFモデルから第1KFモデル(自身)への遷移の確率を示しており、行列の成分「p12」は、第1KFモデルから第2KFモデルへの遷移の確率を示している。また、各行「i」の成分を合算した値は「1」である。すなわち、「pi1+pi2+・・・+pin=1」である。
【0032】
次いで、CPUは、次式(2)に従って規格化定数「CN」を算出する(ステップS3)。
【数2】
式(2)からわかるように、規格化定数「CN」は、モデル間遷移確率「p」の転置行列と、モデル確率「μ」との積で表される。
【0033】
次いで、CPUは、次式(3)に従って、合成確率「γ」を算出する(ステップS5)。
【数3】
式(3)からわかるように、合成確率「γ」は、モデル間遷移確率「p」の各成分と、モデル確率「μ」の各成分と、規格化定数「CN」の各成分とを用いて算出される。
【0034】
その後、CPUは、次式(4)及び(5)に従って、KF処理における状態ベクトルの初期値「X0」及び誤差共分散の初期値「P0」を算出する(ステップS7)。
【数4】
【数5】
【0035】
次いで、CPUは、各KFモデルそれぞれについて、ループAの処理を実行する(ステップS9〜S23)。ループAでは、CPUは、次式(6)及び(7)に従って予測演算を行って、状態ベクトル及び誤差共分散を予測する(ステップS11)。
【数6】
【数7】
但し、添え字の「−」は予測値であることを示している。また、「φ」及び「Q」は、それぞれ状態遷移行列及びプロセスノイズと呼ばれる行列であり、各KFモデル毎に設定される行列である。
【0036】
次いで、CPUは、外部(例えば外部観測量を検出するセンサ)からメジャメント実測値「Z」を取得する(ステップS13)。また、CPUは、ステップS11で予測した状態ベクトルの予測値「X−」を用いて、外部観測量の予測値(以下、「メジャメント予測値」と称す。)を算出する(ステップS15)。メジャメント予測値は、所定の観測行列「H」と状態ベクトルの予測値「X−」との積「HX−」で表される。
【0037】
その後、CPUは、補正演算を行って、ステップS11で予測した状態ベクトル及び誤差共分散を補正する(ステップS17)。具体的には、先ず次式(8)に従って、カルマンゲイン「K」と呼ばれるゲインを算出する。
【数8】
但し、E[・]は平均値(期待値)を示している。また、「R」は測定誤差行列と呼ばれるメジャメント実測値「Z」に含まれる誤差を示す行列である。
【0038】
そして、CPUは、メジャメント実測値「Z」とメジャメント予測値「HX−」とを用いて、次式(9)及び(10)に従って、状態ベクトル及び誤差共分散を補正する。
【数9】
【数10】
但し、添え字の「+」は補正値であることを示している。また、「I」は単位行列である。
【0039】
次いで、CPUは、次式(11)に従って、尤度指標値「Vx」を算出する。
【数11】
【0040】
また、CPUは、次式(12)に従って、重み係数「β」を算出する。
【数12】
但し、「N」は取得したメジャメント実測値の個数を示している。
【0041】
全てのKFモデルについてステップS11〜S21の処理を行った後、CPUは、ループAを終了する。その後、CPUは、各KFモデル同士の相対的な組合せそれぞれにおける尤度指標値「Vx」の相対値を算出する(ステップS25)。例えば、第1KFモデル〜第3KFモデルの3つのKFモデルが存在する場合は、「Vx1−Vx2」、「Vx2−Vx1」、「Vx1−Vx3」、「Vx3−Vx1」、「Vx2−Vx3」及び「Vx3−Vx2」の6つの相対的な組合せに対して、それぞれの値を、尤度指標値の相対値として算出する。
【0042】
その後、CPUは、ステップS25で算出した相対値それぞれが、所定の最大値相当値(例えば「+20」)以上となったか、また、所定の最小値相当値(例えば「−20」)以下となったか否かを判定する(ステップS27)。
【0043】
そして、CPUは、次式(13−2)で与えられるモデル確率「μ」の式の分母において、相対値が最大値相当値以上となったと判定した項は「+∞」、相対値が最小値相当値以下となったと判定した項は「0」として、各KFモデルそれぞれについてモデル確率「μ」を算出する(ステップS29)。
【数13】
【0044】
より具体的には、相対値が最大値相当値以上となった項が1つでも存在する場合は、モデル確率「μ」を「0」とし、全ての項の相対値が最小値相当値以下となった場合は、モデル確率「μ」を「1」とする。
【0045】
尚、式(13−1)における「λ」は、次式(14)で与えられる尤度であり、「C」は、次式(15)で与えられる規格化定数である。
【数14】
【数15】
【0046】
次いで、CPUは、ステップS29で算出したモデル確率「μ」を用いて、次式(16)及び(17)に従って、状態ベクトル及び誤差共分散の出力値「X」及び「P」を決定する(ステップS31)。そして、CPUは、IMM演算処理を終了する。
【数16】
【数17】
【0047】
2.実験結果
次に、KFモデルが、第1KFモデル及び第2KFモデルの2つである場合に、各KFモデルそれぞれについてモデル確率「μ」を算出した場合の実験結果について説明する。ここでは、第1KFモデルの尤度指標値「Vx1」を「−20」、第1KFモデルの尤度指標値「Vx2」を「−21」として実験を行った。KFモデルが2つである場合は、次式(18)及び(19)に従ってモデル確率「μ1」及び「μ2」を算出する。
【数18】
【数19】
【0048】
図4は、従来の手法に則って、式(18−1)及び(19―1)によりモデル確率「μ1」及び「μ2」を算出した場合の各種パラメータの値を示した図である。この結果を見ると、「λ1」及び「λ2」が共に「0」であるために、「λ1C1」及び「λ2C2」が「0」となり、「C=λ1C1+λ2C2」も「0」となっている。これにより、式(18−1)及び(19−1)の分母がそれぞれ「0」となるため、モデル確率「μ1」及び「μ2」は不定となっている。
【0049】
この様子を定性的に示したグラフが、図6である。同図において、横軸は「Vx1」、縦軸は「y=exp(Vx1)」をそれぞれ示している。上記の実験では、「Vx1」の値が「−20」であるが、このグラフを見ると、「y」の値はほぼ「0」となることがわかる。また、「Vx2」の値は「−21」であるが、この場合も同様である。
【0050】
すなわち、従来の手法では、各KFモデルそれぞれについて算出された尤度指標値「Vx」が非常に小さな負の領域(図6のハッチング領域)に集中すると、EXP関数の値が演算可能な数値範囲の関係で「0」となり、式(14)により算出される尤度「λ」は「0」になってしまうという問題がある。尤度が「0」になると、式(13−1)によってモデル確率「μ」を算出することができない。
【0051】
図5は、本実施形態の手法に則って、式(18−2)及び(19−2)によりモデル確率「μ1」及び「μ2」を算出した場合の各種パラメータの値を示した図である。この結果を見ると、尤度指標値「Vx1」及び「Vx2」ではなく、その相対値「Vx2−Vx1」及び「Vx1−Vx2」を用いて計算を行ったことで、モデル確率「μ1」及び「μ2」が正しく求められていることがわかる。
【0052】
この様子を定性的に示したグラフが、図7である。同図において、横軸は「Vx1−Vx2」、縦軸は「y=exp(Vx1−Vx2)」をそれぞれ示している。上記の実験では、「Vx1」の値が「−20」であり、「Vx2」の値が「−21」であるため、「Vx1−Vx2」の値は「1」となるが、この場合は「y」の値を正しく求めることができる。また、「Vx2−Vx1」の値は「−1」となるが、この場合も同様に「y」の値を正しく求めることができる。
【0053】
本願発明者は、各KFモデルそれぞれについて算出された尤度指標値「Vx」が非常に小さな値であったとしても、尤度指標値の相対値は概ね「0」の近傍の領域(図7のハッチング領域)に集中する点に着目した。そして、式(14)に従って尤度「λ」を算出することなく、式(13−1)を展開・変形することで得られる式(13−2)を用いて、尤度指標値の相対値からモデル確率「μ」を直接算出することで、尤度指標値「Vx」の大きさに左右されずにモデル確率「μ」を求めることができることを発見した。
【0054】
3.実施例
次に、測位装置を備えた電子機器の一種である携帯型電話機に上述した原理を適用した場合の実施例について説明する。但し、上述した原理の適用可能な実施例がこれに限定されるわけではない。
【0055】
3−1.機能構成
図8は、本実施例における携帯型電話機1の機能構成を示すブロック図である。携帯型電話機1は、GPSアンテナ10と、GPS受信部20と、TCXO(Temperature Compensated Crystal Oscillator)40と、ホストCPU(Central Processing Unit)50と、操作部60と、表示部70と、携帯電話用アンテナ80と、携帯電話用無線通信回路部90と、ROM(Read Only Memory)100と、RAM(Random Access Memory)110とを備えて構成される。
【0056】
GPSアンテナ10は、GPS衛星から発信されているGPS衛星信号を含むRF(Radio Frequency)信号を受信するアンテナであり、受信した信号をGPS受信部20に出力する。尚、GPS衛星信号は、衛星毎に異なる拡散符号の一種であるPRN(Pseudo Random Noise)コードで直接スペクトラム拡散方式により変調された1.57542[GHz]の通信信号である。PRNコードは、コード長1023チップを1PNフレームとする繰返し周期1msの擬似ランダム雑音符号である。
【0057】
GPS受信部20は、GPSアンテナ10から出力された信号に基づいて携帯型電話機1の現在位置を測位する測位回路であり、いわゆるGPS受信機に相当する機能ブロックである。GPS受信部20は、RF(Radio Frequency)受信回路部21と、ベースバンド処理回路部30とを備えて構成される。尚、RF受信回路部21と、ベースバンド処理回路部30とは、それぞれ別のLSI(Large Scale Integration)として製造することも、1チップとして製造することも可能である。
【0058】
RF受信回路部21は、RF信号の処理回路ブロックであり、TCXO40により生成された発振信号を分周或いは逓倍することで、RF信号乗算用の発振信号を生成する。そして、生成した発振信号を、GPSアンテナ10から出力されたRF信号に乗算することで、RF信号を中間周波数の信号(以下、「IF(Intermediate Frequency)信号」と称す。)にダウンコンバートし、IF信号を増幅等した後、A/D変換器でデジタル信号に変換して、ベースバンド処理回路部30に出力する。
【0059】
ベースバンド処理回路部30は、RF受信回路部21から出力されたIF信号に対して相関処理等を行ってGPS衛星信号を捕捉・抽出し、データを復号して航法メッセージや時刻情報等を取り出して測位演算を行う回路部である。ベースバンド処理回路部30は、演算制御部31と、ROM35と、RAM37とを備えて構成される。また、演算制御部31は、メジャメント取得演算部32と、測位演算部33とを備えて構成される。
【0060】
尚、メジャメント取得演算部32と、測位演算部33とは、それぞれ別のLSIとして製造することも、1チップとして製造することも可能である。また、本実施形態においては現在位置の測位演算そのものは測位演算部33で実行することとして説明するが、測位演算部33で実行する処理の一部又は全部をホストCPU50で実行することとしてもよいのは勿論である。
【0061】
メジャメント取得演算部32は、RF受信回路部21から出力された受信信号(IF信号)から、GPS衛星信号の捕捉・追尾を行う回路部であり、相関演算部321を備えて構成されている。メジャメント取得演算部32は、捕捉・追尾したGPS衛星信号の受信周波数やコード位相等の情報を取得し、メジャメント実測値として測位演算部33に出力する。
【0062】
相関演算部321は、受信信号に含まれるPRNコードとレプリカコードとの相関を、例えばFFT演算を用いて算出するコヒーレント処理を行い、このコヒーレント処理の結果である相関値を所定秒数分(例えば「1秒分」)積算して積算相関値を算出するインコヒーレント処理を行うことで、GPS衛星信号を捕捉する。レプリカコードとは、擬似的に発生させた捕捉しようとするGPS衛星信号に含まれるPRNコードを模擬した信号である。
【0063】
捕捉しようとするGPS衛星信号が間違いなければ、そのGPS衛星信号に含まれるPRNコードとレプリカコードとは一致し(捕捉成功)、間違っていれば一致しない(捕捉失敗)。そのため、算出された積算相関値のピークを判定することによってGPS衛星信号の捕捉が成功したか否かを判定でき、レプリカコードを次々に変更して、同じ受信信号との相関演算を行うことで、GPS衛星信号を捕捉することが可能となる。
【0064】
また、相関演算部321は、上述したコヒーレント処理を、レプリカコードの発生信号の周波数、及び、PRNコードとレプリカコードとを相関演算する際の位相を変更しつつ行っている。レプリカコードの発生信号の周波数と受信信号の周波数とが一致し、且つ、PRNコードとレプリカコードとの位相が一致した場合に、積算相関値が最大となる。
【0065】
より具体的には、捕捉対象のGPS衛星信号に応じた所定の周波数及びコード位相の範囲をサーチ範囲として設定する。そして、このサーチ範囲内で、PRNコードの開始位置(コード位相)を検出するための位相方向の相関演算と、周波数を検出するための周波数方向の相関演算とを行う。サーチ範囲は、周波数についてはGPS衛星信号の搬送波周波数である1.57542[GHz]を中心とする所定の周波数掃引範囲、コード位相についてはPRNコードのチップ長である1023チップのコード位相範囲内に定められる。
【0066】
測位演算部33は、メジャメント取得演算部32から入力したメジャメント実測値を用いてIMM演算を行って携帯型電話機1の現在位置を測位する測位演算を行うプロセッサである。
【0067】
TCXO40は、所定の発振周波数で発振信号を生成する温度補償型水晶発振器であり、生成した発振信号をRF受信回路部21及びベースバンド処理回路部30に出力する。
【0068】
ホストCPU50は、ROM100に記憶されているシステムプログラム等の各種プログラムに従って携帯型電話機1の各部を統括的に制御するプロセッサである。ホストCPU50は、測位演算部33から入力した出力位置をプロットしたナビゲーション画面を、表示部70に表示させる。
【0069】
操作部60は、例えばタッチパネルやボタンスイッチ等により構成される入力装置であり、押下されたキーやボタンの信号をホストCPU50に出力する。この操作部60の操作により、通話要求やメールの送受信要求等の各種指示入力がなされる。
【0070】
表示部70は、LCD(Liquid Crystal Display)等により構成され、ホストCPU50から入力される表示信号に基づいた各種表示を行う表示装置である。表示部70には、ナビゲーション画面や時刻情報等が表示される。
【0071】
携帯電話用アンテナ80は、携帯型電話機1の通信サービス事業者が設置した無線基地局との間で携帯電話用無線信号の送受信を行うアンテナである。
【0072】
携帯電話用無線通信回路部90は、RF変換回路、ベースバンド処理回路等によって構成される携帯電話の通信回路部であり、携帯電話用無線信号の変調・復調等を行うことで、通話やメールの送受信等を実現する。
【0073】
ROM100は、ホストCPU50が携帯型電話機1を制御するためのシステムプログラムや、ナビゲーション機能を実現するための各種プログラムやデータ等を記憶している。
【0074】
RAM110は、ホストCPU50により実行されるシステムプログラム、各種処理プログラム、各種処理の処理中データ、処理結果などを一時的に記憶するワークエリアを形成している。
【0075】
3−2.データ構成
図9は、ベースバンド処理回路部30のROM35に格納されたデータの一例を示す図である。ROM35には、測位演算部33により読み出され、ベースバンド処理(図13参照)として実行されるベースバンド処理プログラム351が記憶されている。また、ベースバンド処理プログラム351には、IMM演算処理(図2及び図3参照)として実行されるIMM演算プログラム3511がサブルーチンとして含まれている。
【0076】
ベースバンド処理とは、測位演算部33が、IMM演算処理を行って出力位置を決定し、当該出力位置をホストCPU50に出力する処理である。ベースバンド処理については、フローチャートを用いて詳細に後述する。
【0077】
IMM演算処理とは、測位演算部33が、複数のKFモデルに基づくKF処理の処理結果を、算出・更新したモデル確率で合成することで、出力位置を決定する処理である。本実施例では、図11に示すように、携帯型電話機1の移動状態を示すKFモデルとして、停止状態にあることを示す定点KFモデルと、等速移動状態にあることを示す等速度KFモデルと、定加速度状態にあることを示す定加速度KFモデルとの3種類のKFモデルの処理結果に基づくIMM演算を行って現在位置を測位する。
【0078】
図10は、ベースバンド処理回路部30のRAM37に格納されるデータの一例を示す図である。RAM37には、IMMパラメータデータ371と、捕捉衛星別メジャメントデータ373と、出力位置データ375とが記憶される。
【0079】
IMMパラメータデータ371は、IMM演算処理において用いられるIMM演算の各種パラメータ(以下、「IMMパラメータ」と称す。)の値が記憶されたデータであり、IMM演算処理において測位演算部33により更新される。
【0080】
図12は、捕捉衛星別メジャメントデータ373のデータ構成例を示す図である。捕捉衛星別メジャメントデータ373は、IMM演算処理に使用するメジャメント情報についてのデータであり、捕捉衛星3731と、メジャメント実測値3733と、メジャメント予測値3735とが対応付けて記憶される。捕捉衛星3731には、当該捕捉衛星の番号が記憶され、メジャメント実測値3733及びメジャメント予測値3735には、当該捕捉衛星から受信したGPS衛星信号の受信周波数やコード位相の実測値及び予測値がそれぞれ記憶される。
【0081】
例えば、捕捉衛星「S1」についてのメジャメント実測値は、受信周波数が「SFreq1」、コード位相が「SCP1」であり、メジャメント予測値は、受信周波数が「EFreq1」、コード位相が「ECP1」である。
【0082】
IMM演算処理では、測位演算部33は、メジャメント取得演算部32により取得演算されたGPS衛星信号の受信周波数及びコード位相の情報を、メジャメント実測値として取得する。また、測位演算部33は、KF処理において予測演算により予測した状態ベクトルの予測値と観測行列とを用いてGPS衛星信号の受信周波数及びコード位相の情報を予測して、メジャメント予測値とする。
【0083】
出力位置データ375は、ホストCPU50に出力する位置として決定された出力位置のデータであり、IMM演算処理において測位演算部33により更新される。
【0084】
3−3.処理の流れ
図13は、測位演算部33によりROM35に記憶されているベースバンド処理プログラム351が読み出されて実行されることで、携帯型電話機1において実行されるベースバンド処理の流れを示すフローチャートである。ベースバンド処理は、RF受信回路部21によるGPS衛星信号の受信と併せて、測位演算部33が、操作部60に測位開始指示の操作がなされたことを検出した場合に実行を開始する処理であり、各種アプリケーションの実行といった各種の処理と並行して行われる処理である。
【0085】
尚、携帯型電話機1の電源のON/OFFとGPSの起動/停止とを連動させ、携帯型電話機1の電源投入操作を検出した場合に処理の実行を開始させることにしてもよい。原則として、測位演算は「1秒」毎に行われるものとする。また、特に説明しないが、以下のベースバンド処理の実行中は、GPSアンテナ10によるRF信号の受信や、RF受信回路部21によるIF信号へのダウンコンバート、メジャメント取得演算部32によるGPS衛星信号の受信周波数やコード位相の情報の取得・算出等が随時行われている状態にあるものとする。
【0086】
先ず、測位演算部33は、初期設定を行う(ステップA1)。具体的には、IMMパラメータを初期化し、RAM37のIMMパラメータデータ371に記憶させる。次いで、測位演算部33は、ROM35に記憶されているIMM演算プログラム3511を読み出して実行することで、IMM演算処理を行う(ステップA3)。
【0087】
ステップA3のIMM演算処理では、測位演算部33は、図2及び図3のフローチャートに従って処理を実行する。この場合において、測位演算部33は、携帯型電話機1の3次元の位置「(x,y,z)」、3次元の速度「(u,v,w)」、3次元の加速度「(ax,ay,az)」、クロックバイアス「b」及びクロックドリフト「d」を成分とする次式(20)で表される11次元の状態ベクトル「X」を用いてIMM演算を行う。
【数20】
【0088】
また、測位演算部33は、定点KFモデル用の状態遷移行列として次式(21)で与えられる「φ1」、等速度KFモデル用の状態遷移行列として次式(22)で与えられる「φ2」、定加速度KFモデル用の状態遷移行列として次式(23)で与えられる「φ3」を用いてKF処理を行う。
【0089】
【数21】
【数22】
【数23】
【0090】
より具体的には、測位演算部33は、図2のIMM演算処理のステップS11における予測演算において、状態遷移行列「φ1」〜「φ3」を用いて、式(6)及び式(7)に従って状態ベクトル及び誤差共分散の予測値「X−:X−1,X−2,X−3」及び「P−:P−1,P−2,P−3」を算出する。尚、状態遷移行列は、11×11の行列であり、その行と列の並びは、式(20)の状態ベクトル「X」の成分の並びに対応している。
【0091】
式(21)の状態遷移行列「φ1」では、3次元の位置に対応する3×3の行列部分の対角成分は「1」になっているが、3次元の速度及び加速度に対応する6×6の行列部分の成分は全て「0」になっており、携帯型電話機1が停止状態にあることを表していることがわかる。
【0092】
式(22)の状態遷移行列「φ2」では、3次元の位置及び速度に対応する6×6の行列部分の対角成分は「1」になっているが、3次元の加速度に対応する3×3の行列部分の成分は全て「0」になっており、携帯型電話機1が等速移動状態にあることを表していることがわかる。
【0093】
式(23)の状態遷移行列「φ3」では、3次元の位置、速度及び加速度に対応する9×9の行列部分の対角成分が「1」となっており、携帯型電話機1が定加速度状態にあることを表していることがわかる。
【0094】
また、測位演算部33は、定点KFモデル、等速度KFモデル及び定加速度KFモデルそれぞれについて、次式(24)で表される観測行列「H」を用いて計算を行う。
【数24】
【0095】
より具体的には、測位演算部33は、図2のIMM演算処理のステップS15において、観測行列「H」を用いてメジャメント予測値「HX−:H1X−1,H2X−2,H3X−3」を算出する。また、ステップS17の補正演算において、観測行列「H」を用いて式(8)〜式(10)に従って状態ベクトル及び誤差共分散の補正値「X:X1,X2,X3」及び「P:P1,P2,P3」を算出する。そして、ステップS19及びS21において、観測行列「H」を用いて式(11)及び式(12)に従って尤度指標値「Vx:Vx1,Vx2,Vx3」及び重み係数「β:β1,β2,β3」を算出する。
【0096】
ここで、観測行列「H」に含まれる「Dx」、「Dy」及び「Dz」は、次式(25−1)〜(25−3)でそれぞれ与えられる。
【数25】
【0097】
但し、「xu」、「yu」及び「zu」は、それぞれKF処理の予測演算により予測された携帯型電話機1の予測位置の3次元の成分をそれぞれ示しており、「xs」、「ys」及び「zs」は、観測対象とするGPS衛星の位置の3次元の成分をそれぞれ示している。また、「R」は、携帯型電話機1の予測位置と観測対象とするGPS衛星の位置間の距離を示している。
【0098】
そして、測位演算部33は、定点KFモデル、等速度KFモデル及び定加速度KFモデルそれぞれについてステップS19で算出した尤度指標値「Vx」の相対値「Vx1−Vx2」、「Vx2−Vx1」、「Vx1−Vx3」、「Vx3−Vx1」、「Vx2−Vx3」及び「Vx3−Vx2」を算出し(ステップS25)、これらの相対値と、ステップS21で算出した重み係数「β:β1,β2,β3」と、ステップS3で算出した規格化定数「CN:C1,C2,C3」とを用いて式(13−2)に従ってモデル確率「μ:μ1,μ2,μ3」を算出する(ステップS27、S29)。
【0099】
そして、測位演算部33は、算出したモデル確率「μ:μ1,μ2,μ3」を用いて、式(16)及び式(17)に従って状態ベクトル「X:X1,X2,X3」及び誤差共分散「P:P1,P2,P3」を算出することで、状態ベクトルの出力値「X」及び誤差共分散の出力値「P」を決定する(ステップS31)。
【0100】
ステップA3でIMM演算処理を行った後、測位演算部33は、IMM演算処理で決定した状態ベクトルの出力値「X」に含まれる携帯型電話機1の3次元の位置「(x,y,z)」を出力位置に決定し、ホストCPU50に出力する(ステップA5)。
【0101】
その後、測位演算部33は、操作部60を介してユーザにより測位終了指示がなされたか否かを判定し(ステップA7)、なされなかったと判定した場合は(ステップA7;No)、ステップA3に戻る。また、測位終了指示がなされたと判定した場合は(ステップA7;Yes)、ベースバンド処理を終了する。
【0102】
4.作用効果
本実施形態によれば、カルマンフィルタ処理それぞれの尤度指標値「Vx」が算出され、この尤度指標値「Vx」のカルマンフィルタ処理間の相対値が算出される。そして、算出された相対値を用いてカルマンフィルタ処理それぞれのモデル確率「μ」が算出され、このモデル確率「μ」を用いてカルマンフィルタ処理それぞれの処理結果が合成されることで測位位置が決定される。
【0103】
ここで、尤度指標値「Vx」とは、カルマンフィルタの入力値となる外部観測量の尤度「λ」を算出するための尤度関数の指数部分のことを指す。IMM演算におけるモデル確率「μ」は、尤度指標値「Vx」を用いて算出した外部観測量の尤度「λ」を基に算出することができるが、尤度指標値「Vx」が非常に小さな値である場合には、計算機で計算しようとしても尤度「λ」の返し値が「0」になってしまい、モデル確率「μ」を正しく求めることができない。
【0104】
この問題を解決するため、尤度指標値「Vx」をそのまま用いるのではなく、複数のカルマンフィルタ処理間の尤度指標値「Vx」の相対値を用いる。これは、個々の尤度指標値「Vx」が非常に小さな値であったとしても、尤度指標値「Vx」の相対値は概ね「0」の近傍の値となり、指数関数に当該相対値を与えて計算すると「0」とはならない点に着目したものである。かかる原理により、モデル確率「μ」を正しく算出することが可能となり、その結果、正確な測位位置の算出を実現し得る。
【0105】
5.変形例
5−1.電子機器
本発明は、測位装置を備えた電子機器であれば何れの電子機器にも適用可能である。例えば、ノート型パソコンやPDA(Personal Digital Assistant)、カーナビゲーション装置等についても同様に適用可能である。
【0106】
5−2.衛星測位システム
上述した実施形態では、衛星測位システムとしてGPSを例に挙げて説明したが、WAAS(Wide Area Augmentation System)、QZSS(Quasi Zenith Satellite System)、GLONASS(GLObal NAvigation Satellite System)、GALILEO等の他の衛星測位システムであってもよい。
【0107】
5−3.処理の分化
測位演算部33が実行する処理の一部又は全部を、ホストCPU50が実行することにしてもよい。例えば、メジャメント取得演算部32から取得したメジャメント実測値を用いて、ホストCPU50がIMM演算を行って出力位置を決定し、当該出力位置を表示部70に表示させる。
【図面の簡単な説明】
【0108】
【図1】IMMの説明図。
【図2】IMM演算処理の流れを示すフローチャート。
【図3】IMM演算処理の流れを示すフローチャート。
【図4】従来の手法によりモデル確率を求めた実験結果を示す図。
【図5】実施形態の手法によりモデル確率を求めた実験結果を示す図。
【図6】従来の手法によるモデル確率算出の説明図。
【図7】実施形態の手法によるモデル確率算出の説明図。
【図8】携帯型電話機の機能構成を示すブロック図。
【図9】ベースバンド処理回路部のROMに格納されたデータの一例を示す図。
【図10】ベースバンド処理回路部のRAMに格納されるデータの一例を示す図。
【図11】実施例におけるIMMの説明図。
【図12】捕捉衛星別メジャメントデータのデータ構成の一例を示す図。
【図13】ベースバンド処理の流れを示すフローチャート。
【符号の説明】
【0109】
1 携帯型電話機 、 10 GPSアンテナ、 20 GPS受信部、
21 RF受信回路部、 30 ベースバンド処理回路部、 31 演算制御部、
32 メジャメント取得演算部、 33 測位演算部、 35 ROM、
37 RAM、 40 TCXO、 50 ホストCPU、 60 操作部、
70 表示部、 80 携帯電話用アンテナ、 90 携帯電話用無線通信回路部、
100 ROM、 110 RAM
【技術分野】
【0001】
本発明は、測位装置が、各カルマンフィルタ処理それぞれをモデルとみなし、前記カルマンフィルタ処理それぞれの処理結果を所定のモデル確率で重みづけして合成するIMM演算を行って測位する測位方法等に関する。
【背景技術】
【0002】
人工衛星を利用した測位システムとしては、GPS(Global Positioning System)が広く知られており、携帯型電話機やカーナビゲーション装置等に内蔵された測位装置に利用されている。GPSでは、自機の位置を示す3次元の座標値と、時計誤差との4つのパラメータの値を、複数のGPS衛星の位置や各GPS衛星から自機までの擬似距離等の情報に基づいて求める測位演算を行うことで、自機の現在位置を測位する。
【0003】
測位演算としては、最小二乗法を用いた測位演算や、カルマンフィルタを用いた測位演算が知られている。そして、カルマンフィルタを用いた測位演算を応用した技術として、測位装置の移動状態に対応する複数のカルマンフィルタモデルを用意して各モデルについて測位演算を行い、その測位演算の結果を合成して測位位置を決定するインタラクティブミキシングモデル(Interactive Mixing Model)演算(以下、「IMM演算」と称す。)が知られている(例えば、非特許文献1)。
【非特許文献1】Yaakov Bar-Shalom, X.Rong Li, Thiagalingam Kirubarajan著、「Estimation with Applications to Tracking and Navigation」、(米国)、JOHN WILEY & SONS INC、2001年、p.92−93、453−457
【発明の開示】
【発明が解決しようとする課題】
【0004】
IMM演算による測位では、カルマンフィルタの入力値となる外部観測量の尤もらしさ(以下、「尤度」と称す。)を、尤度関数と呼ばれる正規分布(ガウス分布)に従った関数から算出し、この算出した尤度を用いて、各カルマンフィルタモデルの確率(以下、「モデル確率」と称す。)を算出する。そして、算出したモデル確率を用いて各カルマンフィルタモデルの処理結果を合成することで、測位装置の現在位置を求める。
【0005】
しかし、尤度関数はEXP(Exponential)関数で表されるため、EXP関数の指数部分が非常に小さな値(例えば「−20以下」)になると、尤度は限りなく「0」に近くなり、計算機で計算しようとしても演算可能な数値範囲の関係から結局「0」になってしまう。モデル確率は尤度を用いて算出するため、尤度が「0」になってしまうと、モデル確率を正しく算出することができず、その結果、測位位置を求めることができないという大きな問題があった。
【0006】
本発明は、上述した課題に鑑みて為されたものである。
【課題を解決するための手段】
【0007】
以上の課題を解決するための第1の発明は、測位装置が、各カルマンフィルタ処理それぞれをモデルとみなし、前記カルマンフィルタ処理それぞれの処理結果を所定のモデル確率で重みづけして合成するIMM演算を行って測位する測位方法であって、前記カルマンフィルタ処理それぞれの尤度指標値を算出することと、前記算出された尤度指標値の前記カルマンフィルタ処理間の相対値を算出することと、前記算出された相対値を用いて前記カルマンフィルタ処理それぞれの前記モデル確率を算出することと、前記算出されたモデル確率を用いて前記カルマンフィルタ処理それぞれの処理結果を合成することで測位位置を決定することと、を含む測位方法である。
【0008】
また、他の発明として、各カルマンフィルタ処理それぞれをモデルとみなし、前記カルマンフィルタ処理それぞれの処理結果を所定のモデル確率で重みづけして合成するIMM演算を行って測位する測位装置であって、前記カルマンフィルタ処理それぞれの尤度指標値を算出する指標値算出部と、前記算出された尤度指標値の前記カルマンフィルタ処理間の相対値を算出する相対値算出部と、前記算出された相対値を用いて前記カルマンフィルタ処理それぞれの前記モデル確率を算出するモデル確率算出部と、前記算出されたモデル確率を用いて前記カルマンフィルタ処理それぞれの処理結果を合成することで測位位置を決定する測位位置決定部と、を備えた測位装置を構成してもよい。
【0009】
この第1の発明等によれば、カルマンフィルタ処理それぞれの尤度指標値が算出され、この尤度指標値のカルマンフィルタ処理間の相対値が算出される。そして、算出された相対値を用いてカルマンフィルタ処理それぞれのモデル確率が算出され、このモデル確率を用いてカルマンフィルタ処理それぞれの処理結果が合成されることで測位位置が決定される。
【0010】
ここで、尤度指標値とは、カルマンフィルタの入力値となる外部観測量の尤度を算出するための尤度関数の指数部分のことを指す。IMM演算におけるモデル確率は、尤度指標値を用いて算出した外部観測量の尤度を基に算出することができるが、尤度指標値が非常に小さな値である場合には、計算機で計算しようとしても演算可能な数値範囲の関係で尤度が「0」になってしまい、モデル確率を正しく求めることができない。
【0011】
本願発明者は、この問題を解決するため、尤度指標値をそのまま用いるのではなく、複数のカルマンフィルタ処理間の尤度指標値の相対値を用いることで、モデル確率を正しく求めることができることを発見した。これは、個々の尤度指標値が非常に小さな値であったとしても、尤度指標値の相対値は概ね「0」の近傍の値となり、指数関数に当該相対値を与えて計算すると「0」とはならない点に着目したものである。かかる原理により、モデル確率を正しく算出することが可能となり、その結果、正確な測位位置の算出を実現し得る。
【0012】
また、第2の発明として、第1の発明の測位方法であって、前記モデル確率を算出することは、前記相対値が小さいほど値が小さくなる所定の指数関数を用いて前記モデル確率を算出することである測位方法を構成してもよい。
【0013】
この第2の発明によれば、カルマンフィルタ処理間の尤度指標値の相対値が小さいほど値が小さくなる指数関数(例えば、尤度指標値の相対値を変数とするEXP関数)を用いてモデル確率が算出される。
【0014】
また、第3の発明として、第2の発明の測位方法であって、前記モデル確率を算出することは、前記相対値を用いて算出した前記所定の指数関数の値が極限値相当値を示す条件である所定の極限値条件を満たすか否かを判定することと、前記所定の極限値条件を満たす場合に、前記指数関数の値を極限値とみなして前記モデル確率を算出することと、を含む測位方法を構成してもよい。
【0015】
この第3の発明によれば、カルマンフィルタ処理間の尤度指標値の相対値を用いて算出した指数関数の値が極限値条件を満たす場合は、当該指数関数の値を極限値とみなしてモデル確率が算出される。これにより、指数関数の値が0に収束したり、+∞に発散する場合であっても、計算機によってモデル確率を求めることが可能となる。
【0016】
また、第4の発明として、第3の発明の測位方法であって、前記所定の極限値条件には、最大値相当値を示す条件と最小値相当値を示す条件とが含まれ、前記モデル確率を算出することは、前記所定の極限値条件を満たす場合に、前記最大値相当値を示す条件を満たすか、前記最小値相当値を示す条件を満たすかに応じて、前記モデル確率を1又は0とすることを含む測位方法を構成してもよい。
【0017】
この第4の発明によれば、カルマンフィルタ処理間の尤度指標値の相対値を用いて算出した指数関数の値が最大値相当値を示す条件を満たすか、最小値相当値を満たすかに応じて、モデル確率が1又は0とされる。
【0018】
また、第5の発明として、第1〜第4の何れかの発明の測位方法であって、前記複数のカルマンフィルタ処理には、前記測位装置の移動状態として、停止状態にあることを前提としたカルマンフィルタ処理と、等速移動状態にあることを前提としたカルマンフィルタ処理と、定加速度状態にあることを前提としたカルマンフィルタ処理との3種類のカルマンフィルタ処理が少なくとも含まれる測位方法を構成してもよい。
【0019】
この第5の発明によれば、測位装置の移動状態として、停止状態と、等速移動状態と、定加速度状態とに対応する3種類のカルマンフィルタ処理が行われ、これらのカルマンフィルタ処理の処理結果が合成される。
【0020】
また、第6の発明として、第1〜第5の何れかの発明の測位方法を前記測位装置に内蔵されたコンピュータに実行させるためのプログラムを構成してもよい。
【発明を実施するための最良の形態】
【0021】
1.原理
図1は、本実施形態におけるインタラクティブミキシングモデル(Interactive Mixing Model:以下、「IMM」と称す。)の概要を説明するための図である。ここでは、説明を分かりやすくするため、複数の処理回路ブロックにおける演算によってIMM演算が実現されるものとして説明するが、単独のプロセッサによってIMM演算を実現することも可能であり、図2及び図3の処理フローの説明においては単独のプロセッサにより実行するものとして説明する。
【0022】
IMMは、複数のカルマンフィルタモデル(Kalman Filter Model:以下、「KFモデル」と称す。)それぞれの処理結果をモデル確率「μ」と呼ばれる重みで合成することで、最終的な出力値を決定するモデルである。図1では、第1KFモデル〜第nKFモデルの「n個」のKFモデルを含むIMMを図示している。
【0023】
また、説明の簡明化のため、各KFモデルに対して、単一の外部観測量(本実施形態では「メジャメント実測値」が相当する。)「Z」が与えられるものとして説明するが、複数のメジャメント実測値が与えられる場合についても同様である。メジャメント実測値は、IMM演算を適用するシステムに応じて適宜設定することができる。
【0024】
IMM演算では、ミキサが、各KFモデル間の遷移確率を示すモデル間遷移確率「p」と、1つ前のステップにおいてモデル確率更新部により更新されたモデル確率「μ:μ1,μ2・・・,μn」と、各KFモデルそれぞれにおいて算出された状態ベクトルの計算値「X:X1,X2,・・・,Xn」及び誤差共分散の計算値「P:P1,P2,・・・,Pn」とを用いて、各KFモデルそれぞれの状態ベクトル及び誤差共分散の今回の計算における初期値「X0:X01,X02,・・・,X0n」及び「P0:P01,P02,・・・,P0n」を算出し、計算結果を、対応するKFモデルにそれぞれ出力する。
【0025】
また、ミキサは、モデル間遷移確率「p」と、1つ前のステップにおいてモデル確率更新部により更新されたモデル確率「μ」とを用いて、規格化定数「CN:C1,C2,・・・,Cn」と呼ばれる定数を算出して、モデル確率更新部に出力する。
【0026】
各KFモデルは、ミキサから入力した初期値「X0」及び「P0」と、メジャメント実測値「Z:Z1,Z2,・・・,Zn」とを用いて、公知のカルマンフィルタと同様、予測演算と補正演算とを行うことで、状態ベクトル及び誤差共分散の今回の計算値を算出して、モデル確率更新部に出力する。また、各KFモデルは、尤度指標値「Vx:Vx1,Vx2,・・・,Vxn」と重み係数「β:β1,β2,・・・,βn」と呼ばれる値を算出して、モデル確率更新部に出力する。
【0027】
尤度指標値「Vx」は、メジャメント実測値「Z」の尤もらしさ(以下、「尤度」と称す。)を算出するための関数である尤度関数の指数部分として表される値である。また、重み係数「β」は、尤度関数の係数部分として表される値である。
【0028】
モデル確率更新部は、ミキサから入力した規格化定数「CN」と、KFモデルから入力した尤度指標値「Vx」及び重み係数「β」とを用いて、モデル確率「μ」を新たに算出・更新する。そして、KFモデルから入力した状態ベクトル及び誤差共分散それぞれを、算出したモデル確率「μ」で重み付けして合成することで、状態ベクトルの出力値「X」及び誤差共分散の出力値「P」を算出する。
【0029】
次に、実際の数式を参照しながら、単一のプロセッサでIMM演算を実現する場合の処理の流れについて説明する。以下の数式において、各変数の添え字の「j」は、j番目のKFモデルに対応する変数であることを示しているものとする。
【0030】
図2は、プロセッサの一種であるCPU(Central Processing Unit)が実行するIMM演算処理の流れを示すフローチャートである。
先ず、CPUは、モデル間遷移確率「p」を取得する(ステップS1)。モデル間遷移確率「p」は、次式(1)に示すようなn×nの行列で与えられる。
【数1】
【0031】
行列の成分「pnm」は、n番目のKFモデルからm番目のKFモデルへの遷移の確率を示している。例えば、行列の成分「p11」は、第1KFモデルから第1KFモデル(自身)への遷移の確率を示しており、行列の成分「p12」は、第1KFモデルから第2KFモデルへの遷移の確率を示している。また、各行「i」の成分を合算した値は「1」である。すなわち、「pi1+pi2+・・・+pin=1」である。
【0032】
次いで、CPUは、次式(2)に従って規格化定数「CN」を算出する(ステップS3)。
【数2】
式(2)からわかるように、規格化定数「CN」は、モデル間遷移確率「p」の転置行列と、モデル確率「μ」との積で表される。
【0033】
次いで、CPUは、次式(3)に従って、合成確率「γ」を算出する(ステップS5)。
【数3】
式(3)からわかるように、合成確率「γ」は、モデル間遷移確率「p」の各成分と、モデル確率「μ」の各成分と、規格化定数「CN」の各成分とを用いて算出される。
【0034】
その後、CPUは、次式(4)及び(5)に従って、KF処理における状態ベクトルの初期値「X0」及び誤差共分散の初期値「P0」を算出する(ステップS7)。
【数4】
【数5】
【0035】
次いで、CPUは、各KFモデルそれぞれについて、ループAの処理を実行する(ステップS9〜S23)。ループAでは、CPUは、次式(6)及び(7)に従って予測演算を行って、状態ベクトル及び誤差共分散を予測する(ステップS11)。
【数6】
【数7】
但し、添え字の「−」は予測値であることを示している。また、「φ」及び「Q」は、それぞれ状態遷移行列及びプロセスノイズと呼ばれる行列であり、各KFモデル毎に設定される行列である。
【0036】
次いで、CPUは、外部(例えば外部観測量を検出するセンサ)からメジャメント実測値「Z」を取得する(ステップS13)。また、CPUは、ステップS11で予測した状態ベクトルの予測値「X−」を用いて、外部観測量の予測値(以下、「メジャメント予測値」と称す。)を算出する(ステップS15)。メジャメント予測値は、所定の観測行列「H」と状態ベクトルの予測値「X−」との積「HX−」で表される。
【0037】
その後、CPUは、補正演算を行って、ステップS11で予測した状態ベクトル及び誤差共分散を補正する(ステップS17)。具体的には、先ず次式(8)に従って、カルマンゲイン「K」と呼ばれるゲインを算出する。
【数8】
但し、E[・]は平均値(期待値)を示している。また、「R」は測定誤差行列と呼ばれるメジャメント実測値「Z」に含まれる誤差を示す行列である。
【0038】
そして、CPUは、メジャメント実測値「Z」とメジャメント予測値「HX−」とを用いて、次式(9)及び(10)に従って、状態ベクトル及び誤差共分散を補正する。
【数9】
【数10】
但し、添え字の「+」は補正値であることを示している。また、「I」は単位行列である。
【0039】
次いで、CPUは、次式(11)に従って、尤度指標値「Vx」を算出する。
【数11】
【0040】
また、CPUは、次式(12)に従って、重み係数「β」を算出する。
【数12】
但し、「N」は取得したメジャメント実測値の個数を示している。
【0041】
全てのKFモデルについてステップS11〜S21の処理を行った後、CPUは、ループAを終了する。その後、CPUは、各KFモデル同士の相対的な組合せそれぞれにおける尤度指標値「Vx」の相対値を算出する(ステップS25)。例えば、第1KFモデル〜第3KFモデルの3つのKFモデルが存在する場合は、「Vx1−Vx2」、「Vx2−Vx1」、「Vx1−Vx3」、「Vx3−Vx1」、「Vx2−Vx3」及び「Vx3−Vx2」の6つの相対的な組合せに対して、それぞれの値を、尤度指標値の相対値として算出する。
【0042】
その後、CPUは、ステップS25で算出した相対値それぞれが、所定の最大値相当値(例えば「+20」)以上となったか、また、所定の最小値相当値(例えば「−20」)以下となったか否かを判定する(ステップS27)。
【0043】
そして、CPUは、次式(13−2)で与えられるモデル確率「μ」の式の分母において、相対値が最大値相当値以上となったと判定した項は「+∞」、相対値が最小値相当値以下となったと判定した項は「0」として、各KFモデルそれぞれについてモデル確率「μ」を算出する(ステップS29)。
【数13】
【0044】
より具体的には、相対値が最大値相当値以上となった項が1つでも存在する場合は、モデル確率「μ」を「0」とし、全ての項の相対値が最小値相当値以下となった場合は、モデル確率「μ」を「1」とする。
【0045】
尚、式(13−1)における「λ」は、次式(14)で与えられる尤度であり、「C」は、次式(15)で与えられる規格化定数である。
【数14】
【数15】
【0046】
次いで、CPUは、ステップS29で算出したモデル確率「μ」を用いて、次式(16)及び(17)に従って、状態ベクトル及び誤差共分散の出力値「X」及び「P」を決定する(ステップS31)。そして、CPUは、IMM演算処理を終了する。
【数16】
【数17】
【0047】
2.実験結果
次に、KFモデルが、第1KFモデル及び第2KFモデルの2つである場合に、各KFモデルそれぞれについてモデル確率「μ」を算出した場合の実験結果について説明する。ここでは、第1KFモデルの尤度指標値「Vx1」を「−20」、第1KFモデルの尤度指標値「Vx2」を「−21」として実験を行った。KFモデルが2つである場合は、次式(18)及び(19)に従ってモデル確率「μ1」及び「μ2」を算出する。
【数18】
【数19】
【0048】
図4は、従来の手法に則って、式(18−1)及び(19―1)によりモデル確率「μ1」及び「μ2」を算出した場合の各種パラメータの値を示した図である。この結果を見ると、「λ1」及び「λ2」が共に「0」であるために、「λ1C1」及び「λ2C2」が「0」となり、「C=λ1C1+λ2C2」も「0」となっている。これにより、式(18−1)及び(19−1)の分母がそれぞれ「0」となるため、モデル確率「μ1」及び「μ2」は不定となっている。
【0049】
この様子を定性的に示したグラフが、図6である。同図において、横軸は「Vx1」、縦軸は「y=exp(Vx1)」をそれぞれ示している。上記の実験では、「Vx1」の値が「−20」であるが、このグラフを見ると、「y」の値はほぼ「0」となることがわかる。また、「Vx2」の値は「−21」であるが、この場合も同様である。
【0050】
すなわち、従来の手法では、各KFモデルそれぞれについて算出された尤度指標値「Vx」が非常に小さな負の領域(図6のハッチング領域)に集中すると、EXP関数の値が演算可能な数値範囲の関係で「0」となり、式(14)により算出される尤度「λ」は「0」になってしまうという問題がある。尤度が「0」になると、式(13−1)によってモデル確率「μ」を算出することができない。
【0051】
図5は、本実施形態の手法に則って、式(18−2)及び(19−2)によりモデル確率「μ1」及び「μ2」を算出した場合の各種パラメータの値を示した図である。この結果を見ると、尤度指標値「Vx1」及び「Vx2」ではなく、その相対値「Vx2−Vx1」及び「Vx1−Vx2」を用いて計算を行ったことで、モデル確率「μ1」及び「μ2」が正しく求められていることがわかる。
【0052】
この様子を定性的に示したグラフが、図7である。同図において、横軸は「Vx1−Vx2」、縦軸は「y=exp(Vx1−Vx2)」をそれぞれ示している。上記の実験では、「Vx1」の値が「−20」であり、「Vx2」の値が「−21」であるため、「Vx1−Vx2」の値は「1」となるが、この場合は「y」の値を正しく求めることができる。また、「Vx2−Vx1」の値は「−1」となるが、この場合も同様に「y」の値を正しく求めることができる。
【0053】
本願発明者は、各KFモデルそれぞれについて算出された尤度指標値「Vx」が非常に小さな値であったとしても、尤度指標値の相対値は概ね「0」の近傍の領域(図7のハッチング領域)に集中する点に着目した。そして、式(14)に従って尤度「λ」を算出することなく、式(13−1)を展開・変形することで得られる式(13−2)を用いて、尤度指標値の相対値からモデル確率「μ」を直接算出することで、尤度指標値「Vx」の大きさに左右されずにモデル確率「μ」を求めることができることを発見した。
【0054】
3.実施例
次に、測位装置を備えた電子機器の一種である携帯型電話機に上述した原理を適用した場合の実施例について説明する。但し、上述した原理の適用可能な実施例がこれに限定されるわけではない。
【0055】
3−1.機能構成
図8は、本実施例における携帯型電話機1の機能構成を示すブロック図である。携帯型電話機1は、GPSアンテナ10と、GPS受信部20と、TCXO(Temperature Compensated Crystal Oscillator)40と、ホストCPU(Central Processing Unit)50と、操作部60と、表示部70と、携帯電話用アンテナ80と、携帯電話用無線通信回路部90と、ROM(Read Only Memory)100と、RAM(Random Access Memory)110とを備えて構成される。
【0056】
GPSアンテナ10は、GPS衛星から発信されているGPS衛星信号を含むRF(Radio Frequency)信号を受信するアンテナであり、受信した信号をGPS受信部20に出力する。尚、GPS衛星信号は、衛星毎に異なる拡散符号の一種であるPRN(Pseudo Random Noise)コードで直接スペクトラム拡散方式により変調された1.57542[GHz]の通信信号である。PRNコードは、コード長1023チップを1PNフレームとする繰返し周期1msの擬似ランダム雑音符号である。
【0057】
GPS受信部20は、GPSアンテナ10から出力された信号に基づいて携帯型電話機1の現在位置を測位する測位回路であり、いわゆるGPS受信機に相当する機能ブロックである。GPS受信部20は、RF(Radio Frequency)受信回路部21と、ベースバンド処理回路部30とを備えて構成される。尚、RF受信回路部21と、ベースバンド処理回路部30とは、それぞれ別のLSI(Large Scale Integration)として製造することも、1チップとして製造することも可能である。
【0058】
RF受信回路部21は、RF信号の処理回路ブロックであり、TCXO40により生成された発振信号を分周或いは逓倍することで、RF信号乗算用の発振信号を生成する。そして、生成した発振信号を、GPSアンテナ10から出力されたRF信号に乗算することで、RF信号を中間周波数の信号(以下、「IF(Intermediate Frequency)信号」と称す。)にダウンコンバートし、IF信号を増幅等した後、A/D変換器でデジタル信号に変換して、ベースバンド処理回路部30に出力する。
【0059】
ベースバンド処理回路部30は、RF受信回路部21から出力されたIF信号に対して相関処理等を行ってGPS衛星信号を捕捉・抽出し、データを復号して航法メッセージや時刻情報等を取り出して測位演算を行う回路部である。ベースバンド処理回路部30は、演算制御部31と、ROM35と、RAM37とを備えて構成される。また、演算制御部31は、メジャメント取得演算部32と、測位演算部33とを備えて構成される。
【0060】
尚、メジャメント取得演算部32と、測位演算部33とは、それぞれ別のLSIとして製造することも、1チップとして製造することも可能である。また、本実施形態においては現在位置の測位演算そのものは測位演算部33で実行することとして説明するが、測位演算部33で実行する処理の一部又は全部をホストCPU50で実行することとしてもよいのは勿論である。
【0061】
メジャメント取得演算部32は、RF受信回路部21から出力された受信信号(IF信号)から、GPS衛星信号の捕捉・追尾を行う回路部であり、相関演算部321を備えて構成されている。メジャメント取得演算部32は、捕捉・追尾したGPS衛星信号の受信周波数やコード位相等の情報を取得し、メジャメント実測値として測位演算部33に出力する。
【0062】
相関演算部321は、受信信号に含まれるPRNコードとレプリカコードとの相関を、例えばFFT演算を用いて算出するコヒーレント処理を行い、このコヒーレント処理の結果である相関値を所定秒数分(例えば「1秒分」)積算して積算相関値を算出するインコヒーレント処理を行うことで、GPS衛星信号を捕捉する。レプリカコードとは、擬似的に発生させた捕捉しようとするGPS衛星信号に含まれるPRNコードを模擬した信号である。
【0063】
捕捉しようとするGPS衛星信号が間違いなければ、そのGPS衛星信号に含まれるPRNコードとレプリカコードとは一致し(捕捉成功)、間違っていれば一致しない(捕捉失敗)。そのため、算出された積算相関値のピークを判定することによってGPS衛星信号の捕捉が成功したか否かを判定でき、レプリカコードを次々に変更して、同じ受信信号との相関演算を行うことで、GPS衛星信号を捕捉することが可能となる。
【0064】
また、相関演算部321は、上述したコヒーレント処理を、レプリカコードの発生信号の周波数、及び、PRNコードとレプリカコードとを相関演算する際の位相を変更しつつ行っている。レプリカコードの発生信号の周波数と受信信号の周波数とが一致し、且つ、PRNコードとレプリカコードとの位相が一致した場合に、積算相関値が最大となる。
【0065】
より具体的には、捕捉対象のGPS衛星信号に応じた所定の周波数及びコード位相の範囲をサーチ範囲として設定する。そして、このサーチ範囲内で、PRNコードの開始位置(コード位相)を検出するための位相方向の相関演算と、周波数を検出するための周波数方向の相関演算とを行う。サーチ範囲は、周波数についてはGPS衛星信号の搬送波周波数である1.57542[GHz]を中心とする所定の周波数掃引範囲、コード位相についてはPRNコードのチップ長である1023チップのコード位相範囲内に定められる。
【0066】
測位演算部33は、メジャメント取得演算部32から入力したメジャメント実測値を用いてIMM演算を行って携帯型電話機1の現在位置を測位する測位演算を行うプロセッサである。
【0067】
TCXO40は、所定の発振周波数で発振信号を生成する温度補償型水晶発振器であり、生成した発振信号をRF受信回路部21及びベースバンド処理回路部30に出力する。
【0068】
ホストCPU50は、ROM100に記憶されているシステムプログラム等の各種プログラムに従って携帯型電話機1の各部を統括的に制御するプロセッサである。ホストCPU50は、測位演算部33から入力した出力位置をプロットしたナビゲーション画面を、表示部70に表示させる。
【0069】
操作部60は、例えばタッチパネルやボタンスイッチ等により構成される入力装置であり、押下されたキーやボタンの信号をホストCPU50に出力する。この操作部60の操作により、通話要求やメールの送受信要求等の各種指示入力がなされる。
【0070】
表示部70は、LCD(Liquid Crystal Display)等により構成され、ホストCPU50から入力される表示信号に基づいた各種表示を行う表示装置である。表示部70には、ナビゲーション画面や時刻情報等が表示される。
【0071】
携帯電話用アンテナ80は、携帯型電話機1の通信サービス事業者が設置した無線基地局との間で携帯電話用無線信号の送受信を行うアンテナである。
【0072】
携帯電話用無線通信回路部90は、RF変換回路、ベースバンド処理回路等によって構成される携帯電話の通信回路部であり、携帯電話用無線信号の変調・復調等を行うことで、通話やメールの送受信等を実現する。
【0073】
ROM100は、ホストCPU50が携帯型電話機1を制御するためのシステムプログラムや、ナビゲーション機能を実現するための各種プログラムやデータ等を記憶している。
【0074】
RAM110は、ホストCPU50により実行されるシステムプログラム、各種処理プログラム、各種処理の処理中データ、処理結果などを一時的に記憶するワークエリアを形成している。
【0075】
3−2.データ構成
図9は、ベースバンド処理回路部30のROM35に格納されたデータの一例を示す図である。ROM35には、測位演算部33により読み出され、ベースバンド処理(図13参照)として実行されるベースバンド処理プログラム351が記憶されている。また、ベースバンド処理プログラム351には、IMM演算処理(図2及び図3参照)として実行されるIMM演算プログラム3511がサブルーチンとして含まれている。
【0076】
ベースバンド処理とは、測位演算部33が、IMM演算処理を行って出力位置を決定し、当該出力位置をホストCPU50に出力する処理である。ベースバンド処理については、フローチャートを用いて詳細に後述する。
【0077】
IMM演算処理とは、測位演算部33が、複数のKFモデルに基づくKF処理の処理結果を、算出・更新したモデル確率で合成することで、出力位置を決定する処理である。本実施例では、図11に示すように、携帯型電話機1の移動状態を示すKFモデルとして、停止状態にあることを示す定点KFモデルと、等速移動状態にあることを示す等速度KFモデルと、定加速度状態にあることを示す定加速度KFモデルとの3種類のKFモデルの処理結果に基づくIMM演算を行って現在位置を測位する。
【0078】
図10は、ベースバンド処理回路部30のRAM37に格納されるデータの一例を示す図である。RAM37には、IMMパラメータデータ371と、捕捉衛星別メジャメントデータ373と、出力位置データ375とが記憶される。
【0079】
IMMパラメータデータ371は、IMM演算処理において用いられるIMM演算の各種パラメータ(以下、「IMMパラメータ」と称す。)の値が記憶されたデータであり、IMM演算処理において測位演算部33により更新される。
【0080】
図12は、捕捉衛星別メジャメントデータ373のデータ構成例を示す図である。捕捉衛星別メジャメントデータ373は、IMM演算処理に使用するメジャメント情報についてのデータであり、捕捉衛星3731と、メジャメント実測値3733と、メジャメント予測値3735とが対応付けて記憶される。捕捉衛星3731には、当該捕捉衛星の番号が記憶され、メジャメント実測値3733及びメジャメント予測値3735には、当該捕捉衛星から受信したGPS衛星信号の受信周波数やコード位相の実測値及び予測値がそれぞれ記憶される。
【0081】
例えば、捕捉衛星「S1」についてのメジャメント実測値は、受信周波数が「SFreq1」、コード位相が「SCP1」であり、メジャメント予測値は、受信周波数が「EFreq1」、コード位相が「ECP1」である。
【0082】
IMM演算処理では、測位演算部33は、メジャメント取得演算部32により取得演算されたGPS衛星信号の受信周波数及びコード位相の情報を、メジャメント実測値として取得する。また、測位演算部33は、KF処理において予測演算により予測した状態ベクトルの予測値と観測行列とを用いてGPS衛星信号の受信周波数及びコード位相の情報を予測して、メジャメント予測値とする。
【0083】
出力位置データ375は、ホストCPU50に出力する位置として決定された出力位置のデータであり、IMM演算処理において測位演算部33により更新される。
【0084】
3−3.処理の流れ
図13は、測位演算部33によりROM35に記憶されているベースバンド処理プログラム351が読み出されて実行されることで、携帯型電話機1において実行されるベースバンド処理の流れを示すフローチャートである。ベースバンド処理は、RF受信回路部21によるGPS衛星信号の受信と併せて、測位演算部33が、操作部60に測位開始指示の操作がなされたことを検出した場合に実行を開始する処理であり、各種アプリケーションの実行といった各種の処理と並行して行われる処理である。
【0085】
尚、携帯型電話機1の電源のON/OFFとGPSの起動/停止とを連動させ、携帯型電話機1の電源投入操作を検出した場合に処理の実行を開始させることにしてもよい。原則として、測位演算は「1秒」毎に行われるものとする。また、特に説明しないが、以下のベースバンド処理の実行中は、GPSアンテナ10によるRF信号の受信や、RF受信回路部21によるIF信号へのダウンコンバート、メジャメント取得演算部32によるGPS衛星信号の受信周波数やコード位相の情報の取得・算出等が随時行われている状態にあるものとする。
【0086】
先ず、測位演算部33は、初期設定を行う(ステップA1)。具体的には、IMMパラメータを初期化し、RAM37のIMMパラメータデータ371に記憶させる。次いで、測位演算部33は、ROM35に記憶されているIMM演算プログラム3511を読み出して実行することで、IMM演算処理を行う(ステップA3)。
【0087】
ステップA3のIMM演算処理では、測位演算部33は、図2及び図3のフローチャートに従って処理を実行する。この場合において、測位演算部33は、携帯型電話機1の3次元の位置「(x,y,z)」、3次元の速度「(u,v,w)」、3次元の加速度「(ax,ay,az)」、クロックバイアス「b」及びクロックドリフト「d」を成分とする次式(20)で表される11次元の状態ベクトル「X」を用いてIMM演算を行う。
【数20】
【0088】
また、測位演算部33は、定点KFモデル用の状態遷移行列として次式(21)で与えられる「φ1」、等速度KFモデル用の状態遷移行列として次式(22)で与えられる「φ2」、定加速度KFモデル用の状態遷移行列として次式(23)で与えられる「φ3」を用いてKF処理を行う。
【0089】
【数21】
【数22】
【数23】
【0090】
より具体的には、測位演算部33は、図2のIMM演算処理のステップS11における予測演算において、状態遷移行列「φ1」〜「φ3」を用いて、式(6)及び式(7)に従って状態ベクトル及び誤差共分散の予測値「X−:X−1,X−2,X−3」及び「P−:P−1,P−2,P−3」を算出する。尚、状態遷移行列は、11×11の行列であり、その行と列の並びは、式(20)の状態ベクトル「X」の成分の並びに対応している。
【0091】
式(21)の状態遷移行列「φ1」では、3次元の位置に対応する3×3の行列部分の対角成分は「1」になっているが、3次元の速度及び加速度に対応する6×6の行列部分の成分は全て「0」になっており、携帯型電話機1が停止状態にあることを表していることがわかる。
【0092】
式(22)の状態遷移行列「φ2」では、3次元の位置及び速度に対応する6×6の行列部分の対角成分は「1」になっているが、3次元の加速度に対応する3×3の行列部分の成分は全て「0」になっており、携帯型電話機1が等速移動状態にあることを表していることがわかる。
【0093】
式(23)の状態遷移行列「φ3」では、3次元の位置、速度及び加速度に対応する9×9の行列部分の対角成分が「1」となっており、携帯型電話機1が定加速度状態にあることを表していることがわかる。
【0094】
また、測位演算部33は、定点KFモデル、等速度KFモデル及び定加速度KFモデルそれぞれについて、次式(24)で表される観測行列「H」を用いて計算を行う。
【数24】
【0095】
より具体的には、測位演算部33は、図2のIMM演算処理のステップS15において、観測行列「H」を用いてメジャメント予測値「HX−:H1X−1,H2X−2,H3X−3」を算出する。また、ステップS17の補正演算において、観測行列「H」を用いて式(8)〜式(10)に従って状態ベクトル及び誤差共分散の補正値「X:X1,X2,X3」及び「P:P1,P2,P3」を算出する。そして、ステップS19及びS21において、観測行列「H」を用いて式(11)及び式(12)に従って尤度指標値「Vx:Vx1,Vx2,Vx3」及び重み係数「β:β1,β2,β3」を算出する。
【0096】
ここで、観測行列「H」に含まれる「Dx」、「Dy」及び「Dz」は、次式(25−1)〜(25−3)でそれぞれ与えられる。
【数25】
【0097】
但し、「xu」、「yu」及び「zu」は、それぞれKF処理の予測演算により予測された携帯型電話機1の予測位置の3次元の成分をそれぞれ示しており、「xs」、「ys」及び「zs」は、観測対象とするGPS衛星の位置の3次元の成分をそれぞれ示している。また、「R」は、携帯型電話機1の予測位置と観測対象とするGPS衛星の位置間の距離を示している。
【0098】
そして、測位演算部33は、定点KFモデル、等速度KFモデル及び定加速度KFモデルそれぞれについてステップS19で算出した尤度指標値「Vx」の相対値「Vx1−Vx2」、「Vx2−Vx1」、「Vx1−Vx3」、「Vx3−Vx1」、「Vx2−Vx3」及び「Vx3−Vx2」を算出し(ステップS25)、これらの相対値と、ステップS21で算出した重み係数「β:β1,β2,β3」と、ステップS3で算出した規格化定数「CN:C1,C2,C3」とを用いて式(13−2)に従ってモデル確率「μ:μ1,μ2,μ3」を算出する(ステップS27、S29)。
【0099】
そして、測位演算部33は、算出したモデル確率「μ:μ1,μ2,μ3」を用いて、式(16)及び式(17)に従って状態ベクトル「X:X1,X2,X3」及び誤差共分散「P:P1,P2,P3」を算出することで、状態ベクトルの出力値「X」及び誤差共分散の出力値「P」を決定する(ステップS31)。
【0100】
ステップA3でIMM演算処理を行った後、測位演算部33は、IMM演算処理で決定した状態ベクトルの出力値「X」に含まれる携帯型電話機1の3次元の位置「(x,y,z)」を出力位置に決定し、ホストCPU50に出力する(ステップA5)。
【0101】
その後、測位演算部33は、操作部60を介してユーザにより測位終了指示がなされたか否かを判定し(ステップA7)、なされなかったと判定した場合は(ステップA7;No)、ステップA3に戻る。また、測位終了指示がなされたと判定した場合は(ステップA7;Yes)、ベースバンド処理を終了する。
【0102】
4.作用効果
本実施形態によれば、カルマンフィルタ処理それぞれの尤度指標値「Vx」が算出され、この尤度指標値「Vx」のカルマンフィルタ処理間の相対値が算出される。そして、算出された相対値を用いてカルマンフィルタ処理それぞれのモデル確率「μ」が算出され、このモデル確率「μ」を用いてカルマンフィルタ処理それぞれの処理結果が合成されることで測位位置が決定される。
【0103】
ここで、尤度指標値「Vx」とは、カルマンフィルタの入力値となる外部観測量の尤度「λ」を算出するための尤度関数の指数部分のことを指す。IMM演算におけるモデル確率「μ」は、尤度指標値「Vx」を用いて算出した外部観測量の尤度「λ」を基に算出することができるが、尤度指標値「Vx」が非常に小さな値である場合には、計算機で計算しようとしても尤度「λ」の返し値が「0」になってしまい、モデル確率「μ」を正しく求めることができない。
【0104】
この問題を解決するため、尤度指標値「Vx」をそのまま用いるのではなく、複数のカルマンフィルタ処理間の尤度指標値「Vx」の相対値を用いる。これは、個々の尤度指標値「Vx」が非常に小さな値であったとしても、尤度指標値「Vx」の相対値は概ね「0」の近傍の値となり、指数関数に当該相対値を与えて計算すると「0」とはならない点に着目したものである。かかる原理により、モデル確率「μ」を正しく算出することが可能となり、その結果、正確な測位位置の算出を実現し得る。
【0105】
5.変形例
5−1.電子機器
本発明は、測位装置を備えた電子機器であれば何れの電子機器にも適用可能である。例えば、ノート型パソコンやPDA(Personal Digital Assistant)、カーナビゲーション装置等についても同様に適用可能である。
【0106】
5−2.衛星測位システム
上述した実施形態では、衛星測位システムとしてGPSを例に挙げて説明したが、WAAS(Wide Area Augmentation System)、QZSS(Quasi Zenith Satellite System)、GLONASS(GLObal NAvigation Satellite System)、GALILEO等の他の衛星測位システムであってもよい。
【0107】
5−3.処理の分化
測位演算部33が実行する処理の一部又は全部を、ホストCPU50が実行することにしてもよい。例えば、メジャメント取得演算部32から取得したメジャメント実測値を用いて、ホストCPU50がIMM演算を行って出力位置を決定し、当該出力位置を表示部70に表示させる。
【図面の簡単な説明】
【0108】
【図1】IMMの説明図。
【図2】IMM演算処理の流れを示すフローチャート。
【図3】IMM演算処理の流れを示すフローチャート。
【図4】従来の手法によりモデル確率を求めた実験結果を示す図。
【図5】実施形態の手法によりモデル確率を求めた実験結果を示す図。
【図6】従来の手法によるモデル確率算出の説明図。
【図7】実施形態の手法によるモデル確率算出の説明図。
【図8】携帯型電話機の機能構成を示すブロック図。
【図9】ベースバンド処理回路部のROMに格納されたデータの一例を示す図。
【図10】ベースバンド処理回路部のRAMに格納されるデータの一例を示す図。
【図11】実施例におけるIMMの説明図。
【図12】捕捉衛星別メジャメントデータのデータ構成の一例を示す図。
【図13】ベースバンド処理の流れを示すフローチャート。
【符号の説明】
【0109】
1 携帯型電話機 、 10 GPSアンテナ、 20 GPS受信部、
21 RF受信回路部、 30 ベースバンド処理回路部、 31 演算制御部、
32 メジャメント取得演算部、 33 測位演算部、 35 ROM、
37 RAM、 40 TCXO、 50 ホストCPU、 60 操作部、
70 表示部、 80 携帯電話用アンテナ、 90 携帯電話用無線通信回路部、
100 ROM、 110 RAM
【特許請求の範囲】
【請求項1】
測位装置が、各カルマンフィルタ処理それぞれをモデルとみなし、前記カルマンフィルタ処理それぞれの処理結果を所定のモデル確率で重みづけして合成するインタラクティブミキシングモデル(Interactive Mixing Model)演算(以下「IMM演算」と称す。)を行って測位する測位方法であって、
前記カルマンフィルタ処理それぞれの尤度指標値を算出することと、
前記算出された尤度指標値の前記カルマンフィルタ処理間の相対値を算出することと、
前記算出された相対値を用いて前記カルマンフィルタ処理それぞれの前記モデル確率を算出することと、
前記算出されたモデル確率を用いて前記カルマンフィルタ処理それぞれの処理結果を合成することで測位位置を決定することと、
を含む測位方法。
【請求項2】
前記モデル確率を算出することは、前記相対値が小さいほど値が小さくなる所定の指数関数を用いて前記モデル確率を算出することである請求項1に記載の測位方法。
【請求項3】
前記モデル確率を算出することは、
前記相対値を用いて算出した前記所定の指数関数の値が極限値相当値を示す条件である所定の極限値条件を満たすか否かを判定することと、
前記所定の極限値条件を満たす場合に、前記指数関数の値を極限値とみなして前記モデル確率を算出することと、
を含む請求項2に記載の測位方法。
【請求項4】
前記所定の極限値条件には、最大値相当値を示す条件と最小値相当値を示す条件とが含まれ、
前記モデル確率を算出することは、前記所定の極限値条件を満たす場合に、前記最大値相当値を示す条件を満たすか、前記最小値相当値を示す条件を満たすかに応じて、前記モデル確率を1又は0とすることを含む、
請求項3に記載の測位方法。
【請求項5】
前記複数のカルマンフィルタ処理には、前記測位装置の移動状態として、停止状態にあることを前提としたカルマンフィルタ処理と、等速移動状態にあることを前提としたカルマンフィルタ処理と、定加速度状態にあることを前提としたカルマンフィルタ処理との3種類のカルマンフィルタ処理が少なくとも含まれる、
請求項1〜4の何れか一項に記載の測位方法。
【請求項6】
請求項1〜5の何れか一項に記載の測位方法を前記測位装置に内蔵されたコンピュータに実行させるためのプログラム。
【請求項7】
各カルマンフィルタ処理それぞれをモデルとみなし、前記カルマンフィルタ処理それぞれの処理結果を所定のモデル確率で重みづけして合成するIMM演算を行って測位する測位装置であって、
前記カルマンフィルタ処理それぞれの尤度指標値を算出する指標値算出部と、
前記算出された尤度指標値の前記カルマンフィルタ処理間の相対値を算出する相対値算出部と、
前記算出された相対値を用いて前記カルマンフィルタ処理それぞれの前記モデル確率を算出するモデル確率算出部と、
前記算出されたモデル確率を用いて前記カルマンフィルタ処理それぞれの処理結果を合成することで測位位置を決定する測位位置決定部と、
を備えた測位装置。
【請求項1】
測位装置が、各カルマンフィルタ処理それぞれをモデルとみなし、前記カルマンフィルタ処理それぞれの処理結果を所定のモデル確率で重みづけして合成するインタラクティブミキシングモデル(Interactive Mixing Model)演算(以下「IMM演算」と称す。)を行って測位する測位方法であって、
前記カルマンフィルタ処理それぞれの尤度指標値を算出することと、
前記算出された尤度指標値の前記カルマンフィルタ処理間の相対値を算出することと、
前記算出された相対値を用いて前記カルマンフィルタ処理それぞれの前記モデル確率を算出することと、
前記算出されたモデル確率を用いて前記カルマンフィルタ処理それぞれの処理結果を合成することで測位位置を決定することと、
を含む測位方法。
【請求項2】
前記モデル確率を算出することは、前記相対値が小さいほど値が小さくなる所定の指数関数を用いて前記モデル確率を算出することである請求項1に記載の測位方法。
【請求項3】
前記モデル確率を算出することは、
前記相対値を用いて算出した前記所定の指数関数の値が極限値相当値を示す条件である所定の極限値条件を満たすか否かを判定することと、
前記所定の極限値条件を満たす場合に、前記指数関数の値を極限値とみなして前記モデル確率を算出することと、
を含む請求項2に記載の測位方法。
【請求項4】
前記所定の極限値条件には、最大値相当値を示す条件と最小値相当値を示す条件とが含まれ、
前記モデル確率を算出することは、前記所定の極限値条件を満たす場合に、前記最大値相当値を示す条件を満たすか、前記最小値相当値を示す条件を満たすかに応じて、前記モデル確率を1又は0とすることを含む、
請求項3に記載の測位方法。
【請求項5】
前記複数のカルマンフィルタ処理には、前記測位装置の移動状態として、停止状態にあることを前提としたカルマンフィルタ処理と、等速移動状態にあることを前提としたカルマンフィルタ処理と、定加速度状態にあることを前提としたカルマンフィルタ処理との3種類のカルマンフィルタ処理が少なくとも含まれる、
請求項1〜4の何れか一項に記載の測位方法。
【請求項6】
請求項1〜5の何れか一項に記載の測位方法を前記測位装置に内蔵されたコンピュータに実行させるためのプログラム。
【請求項7】
各カルマンフィルタ処理それぞれをモデルとみなし、前記カルマンフィルタ処理それぞれの処理結果を所定のモデル確率で重みづけして合成するIMM演算を行って測位する測位装置であって、
前記カルマンフィルタ処理それぞれの尤度指標値を算出する指標値算出部と、
前記算出された尤度指標値の前記カルマンフィルタ処理間の相対値を算出する相対値算出部と、
前記算出された相対値を用いて前記カルマンフィルタ処理それぞれの前記モデル確率を算出するモデル確率算出部と、
前記算出されたモデル確率を用いて前記カルマンフィルタ処理それぞれの処理結果を合成することで測位位置を決定する測位位置決定部と、
を備えた測位装置。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【公開番号】特開2009−250619(P2009−250619A)
【公開日】平成21年10月29日(2009.10.29)
【国際特許分類】
【出願番号】特願2008−94954(P2008−94954)
【出願日】平成20年4月1日(2008.4.1)
【出願人】(000002369)セイコーエプソン株式会社 (51,324)
【Fターム(参考)】
【公開日】平成21年10月29日(2009.10.29)
【国際特許分類】
【出願日】平成20年4月1日(2008.4.1)
【出願人】(000002369)セイコーエプソン株式会社 (51,324)
【Fターム(参考)】
[ Back to top ]