(57)【要約】【課題】必要な計算量が少なく、信頼性の高い測定結果を得ることができる位置検出装置を提供する。【解決手段】装置は、各ランドマーク装置との距離の履歴に基づき、新たに測定した各ランドマーク装置との距離が異常であるか否かを判定する距離異常判定手段と、各ランドマーク装置の位置情報と、各ランドマーク装置との距離に基づき、カルマン・フィルタにより現在位置を求めて出力する位置判定手段とを備えており、位置判定手段は、新たに測定した第１のランドマーク装置との距離が異常である場合、新たに測定した第１のランドマーク装置との距離から、カルマン・フィルタによる、その予測値を引いた値が大きい程、小さくなる重み係数を算出し、算出した重み係数によりカルマン・ゲインを調整し、調整後のカルマン・ゲインにより現在位置を求める。【選択図】図１
2012002734.tif 000002
【特許請求の範囲】
【請求項１】
ランドマーク装置から、該ランドマーク装置の位置情報を受信する受信手段と、
ランドマーク装置との距離を測定する測位手段と、
各ランドマーク装置との距離の履歴に基づき、新たに測定した各ランドマーク装置との距離が異常であるか否かを判定する距離異常判定手段と、
各ランドマーク装置の位置情報と、各ランドマーク装置との距離に基づき、カルマン・フィルタにより現在位置を求めて出力する位置判定手段と、
を備えており、
位置判定手段は、新たに測定した第１のランドマーク装置との距離が異常である場合、新たに測定した第１のランドマーク装置との距離から、カルマン・フィルタによる、その予測値を引いた値が大きい程、小さくなる重み係数を算出し、算出した重み係数によりカルマン・ゲインを調整し、調整後のカルマン・ゲインにより現在位置を求める、
位置検出装置。
【請求項２】
加速度を測定する手段と、
加速度の履歴に基づき、新たに測定した加速度が異常であるか否かを判定する手段と、
をさらに備えており、
位置判定手段は、新たに測定した各ランドマーク装置との距離のいずれかが異常であり、かつ、新たに測定した加速度が異常である場合、現在位置が測定不可であると判定する、
請求項１に記載の位置検出装置。
【請求項３】
距離異常判定手段は、異常ではないと判定したときの、測位手段が測定した各ランドマーク装置との距離と、カルマン・フィルタによる、その予測値との差の平均値である第１の値を保持しており、新たに測定した各ランドマーク装置との距離と、その予測値との差である第２の値に基づき、前記平均値を更新し、新たに測定した第１のランドマーク装置との距離と、前記更新後の平均値との差である第３の値を求め、第３の値に基づく値を閾値判定して、新たに測定した第１のランドマーク装置との距離が異常であるか否かを判定する、
請求項１又は２に記載の位置検出装置。
【請求項４】
距離異常判定手段は、測位手段が測定した各ランドマーク装置との距離と、その予測値との差の標準偏差値である第４の値を、前記第１の値及び前記第２の値に基づき計算し、前記第３の値を、前記第４の値で除した値である第５の値を求め、第５の値に基づく値を閾値判定して、新たに測定した第１のランドマーク装置との距離が異常であるか否かを判定する、
請求項３に記載の位置検出装置。
【請求項５】
距離異常判定手段は、平均及び標準偏差が、前記更新後の平均値及び第４の値である正規分布において、その平均値を中心とした積分値が前記第５の値の２倍から１を引いた値に等しくなる位置における確率密度を求め、求めた確率密度にランドマーク装置の数及び距離異常判定手段が異常ではないと判定した回数を乗じた第６の値を求め、第６の値を閾値判定して、新たに測定した第１のランドマーク装置との距離が異常であるか否かを判定する、
請求項４記載の位置検出装置。
【請求項６】
請求項１から５のいずれか１項に記載の位置検出装置としてコンピュータを機能させるプログラム。
【請求項７】
複数のランドマーク装置からの信号を受信する位置検出装置における位置検出方法であって、
各ランドマーク装置から、各ランドマーク装置の位置情報を受信し、各ランドマーク装置との距離を測定するステップと、
各ランドマーク装置との距離の履歴に基づき、新たに測定した各ランドマーク装置との距離が異常であるか否かを判定するステップと、
各ランドマーク装置の位置情報と、各ランドマーク装置との距離に基づき、カルマン・フィルタにより現在位置を求めて出力するステップと、
を備えており、
新たに測定した第１のランドマーク装置との距離が異常である場合、新たに測定した第１のランドマーク装置との距離から、カルマン・フィルタによる、その予測値を引いた値が大きい程、小さくなる重み係数を算出し、算出した重み係数によりカルマン・ゲインを調整し、調整後のカルマン・ゲインにより現在位置を求める、
位置検出方法。
【請求項８】
加速度を測定するステップと、
加速度の履歴に基づき、新たに測定した加速度が異常であるか否かを判定するステップと、
をさらに備えており、
新たに測定した各ランドマーク装置との距離のいずれかが異常であり、かつ、新たに測定した加速度が異常である場合には、現在位置を測定不可であると判定する、
請求項７に記載の位置検出方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、屋内において現在の位置を検出する位置検出技術に関する。
【背景技術】
【０００２】
ＧＰＳ（Ｇｌｏｂａｌ  Ｐｏｓｉｔｉｏｎｉｎｇ  Ｓｙｓｔｅｍ）信号が受信できない屋内等において、現在位置を測定する方法としてＴＤＯＡ（Ｔｉｍｅ  Ｄｉｆｆｅｒｅｎｃｅ  Ｏｆ  Ａｒｒｉｖａｌ）システム等が知られている。
【０００３】
ＴＤＯＡシステムにおいては、適当な間隔にて設置された複数のランドマーク装置が、周期的に距離測定のための信号、例えば、音波信号と無線信号を含むＴＤＯＡ信号を送信する。なお、各ランドマーク装置は、ほぼ同時にＴＤＯＡ信号を送信する様に制御されており、ＴＤＯＡ信号に含まれる無線信号には、当該無線信号を送信したランドマーク装置の設置位置についての情報が含まれている。
【０００４】
位置検出装置が、あるランドマーク装置からの無線信号を時刻ｔ１に、このランドマーク装置から音波信号を時刻ｔ２に受信したものとすると、位置検出装置と当該ランドマーク装置との距離は、音速×（ｔ２−ｔ１）で計算することができる。ランドマーク装置からの無線信号には、当該ランドマークの位置情報が含まれているため、位置検出装置は３つ以上のランドマーク装置との距離を測定することで、自身の現在位置を計算することができる。
【０００５】
ただし、位置検出装置を携帯して移動しながら位置を測定する場合には、測定値をそのまま使用するのではなく、通常、カルマン・フィルタ等を用いて現在位置の推定を行い、推定値により補正した測定値を出力することが行われている（例えば、非特許文献１〜６、参照。）。
【先行技術文献】
【非特許文献】
【０００６】
【非特許文献１】Ａ．Ｈ．Ｍｏｈａｍｅｄ，  ｅｔ  ａｌ．，  “Ａｄａｐｔｉｖｅ  Ｋａｌｍａｎ  Ｆｉｌｔｅｒｉｎｇ  ｆｏｒ  ＩＮＳ／ＧＰＳ，”  Ｊｏｕｒｎａｌ  ｏｆ  Ｇｅｏｄｅｓｙ，  １９９９年
【非特許文献２】Ａ．Ｓｍｉｔｈ，  ｅｔ  ａｌ．，  “Ｔｒａｃｋｉｎｇ  Ｍｏｖｉｎｇ  Ｄｅｖｉｃｅｓ  ｗｉｔｈ  ｔｈｅ  Ｃｒｉｃｋｅｔ  Ｌｏｃａｔｉｏｎ  Ｓｙｓｔｅｍ，”  Ｍｏｂｉｓｙｓ，  ２００４年
【非特許文献３】Ｓ．Ｃ．  Ｃｈａｎ，  ｅｔ  ａｌ．，  “Ａ  Ｎｅｗ  Ｒｏｂｕｓｔ  Ｋａｌｍａｎ  Ｆｉｌｔｅｒ  Ａｌｇｏｒｉｔｈｍ  Ｕｎｄｅｒ  Ｏｕｔｌｉｅｒｓ  ａｎｄ  Ｓｙｓｔｅｍ  Ｕｎｃｅｒｔａｉｎｔｉｅｓ，”  ＩＥＥＥ  Ｉｎｔｅｒｎａｔｉｏｎａｌ  Ｓｙｍｐｏｓｉｕｍ  ｏｎ  Ｃｉｒｃｕｉｔｓ  ａｎｄ  Ｓｙｓｔｅｍｓ，  ２００５年
【非特許文献４】Ｔ．Ｐｅｒａｌａ，  ｅｔ  ａｌ，  “Ｒｏｂｕｓｔ  Ｅｘｔｅｎｄｅｄ  Ｋａｌｍａｎ  Ｆｉｌｔｅｒｉｎｇ  ｉｎ  Ｈｙｂｒｉｄ  Ｐｏｓｉｔｉｏｎｉｎｇ  Ａｐｐｌｉｃａｔｉｏｎｓ，”  ４ｔｈ  Ｗｏｒｋｓｈｏｐ  ｏｎ  ｐｏｓｉｔｉｏｎｉｎｇ，  Ｎａｖｉｇａｔｉｏｎ  ａｎｄ  Ｃｏｍｍｕｎｉｃａｔｉｏｎ，  ２００７年
【非特許文献５】Ｊ．Ｔｉｎｇ，  ｅｔ  ａｌ．，  “Ａ Ｋａｌｍａｎ  Ｆｉｌｔｅｒ  ｆｏｒ  Ｒｏｂｕｓｔ  Ｏｕｔｌｉｅｒ  Ｄｅｔｅｃｔｉｏｎ，”  ＩＥＥＥ／ＲＳＪ  Ｉｎｔｅｒｎａｔｉｏｎａｌ  Ｃｏｎｆｅｒｅｎｃｅ  ｏｎ  Ｉｎｔｅｌｌｉｇｅｎｔ  Ｒｏｂｏｔｓ  ａｎｄ  Ｓｙｓｔｅｍｓ，  ２００７年
【非特許文献６】Ｍ．Ｓｈｕａｉ，  ｅｔ  ａｌ．，  “Ａ  Ｋａｌｍａｎ  Ｆｉｌｔｅｒ  Ｂａｓｅｄ  Ａｐｐｒｏａｃｈ  ｆｏｒ  Ｏｕｔｌｉｅｒ  Ｄｅｔｅｃｔｉｏｎ  ｉｎ  Ｓｅｎｓｏｒ  Ｎｅｔｗｏｒｋｓ，”  Ｉｎｔｅｒｎａｔｉｏｎａｌ  Ｃｏｎｆｅｒｅｎｃｅ  ｏｎ  Ｃｏｍｐｕｔｅｒ  Ｓｃｉｅｎｃｅ  ａｎｄ  Ｓｏｆｔｗａｒｅ  Ｅｎｇｉｎｅｅｒｉｎｇ，  ２００８年
【非特許文献７】Ｒ．Ｍｅｈｒａ，  “Ｏｎ  ｔｈｅ  ｉｄｅｎｔｉｆｉｃａｔｉｏｎ  ｏｆ  ｖａｒｉａｎｃｅｓ  ａｎｄ  ａｄａｐｔｉｖｅ  Ｋａｌｍａｎ  ｆｉｌｔｅｒｉｎｇ，”  Ａｕｔｏｍａｔｉｃ  Ｃｏｎｔｒｏｌ，  ＩＥＥＥ  Ｔｒａｎｓａｃｔｉｏｎｓ  ｏｎ，  Ｖｏｌ．１５，  Ｎｏ．２．（１９７０），  ｐｐ．１７５−１８４
【発明の概要】
【発明が解決しようとする課題】
【０００７】
しかしながら、移動中には障害物やマルチパス等の影響により、ランドマーク装置からのＴＤＯＡ信号が不安定となり、測定精度の劣化の原因となっている。
【０００８】
このため、例えば、非特許文献２ではＬＳＱ（Ｌｅａｓｔ  Ｓｑｕａｒｅｓ  Ｍｉｎｉｍｉｚａｔｉｏｎ）アルゴリズムの利用を開示しているが、引用文献２に記載の方法は、ある種の想定条件に基づくものあり、想定条件から大きく外れた場合には、精度も大きく劣化するという問題がある。
【０００９】
また、非特許文献１及び３〜５は、特殊な統計的仮定に基づき、積分処理や反復計算処理を行うことを開示しているが、計算能力の低い携帯機器への実装には適するものではないという問題がある。
【００１０】
したがって、本発明は、必要な計算量が少なく、信頼性の高い測定結果を得ることができる位置検出装置、方法及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【００１１】
本発明における位置検出装置によれば、
ランドマーク装置から、該ランドマーク装置の位置情報を受信する受信手段と、ランドマーク装置との距離を測定する測位手段と、各ランドマーク装置との距離の履歴に基づき、新たに測定した各ランドマーク装置との距離が異常であるか否かを判定する距離異常判定手段と、各ランドマーク装置の位置情報と、各ランドマーク装置との距離に基づき、カルマン・フィルタにより現在位置を求めて出力する位置判定手段とを備えており、位置判定手段は、新たに測定した第１のランドマーク装置との距離が異常である場合、新たに測定した第１のランドマーク装置との距離から、カルマン・フィルタによる、その予測値を引いた値が大きい程、小さくなる重み係数を算出し、算出した重み係数によりカルマン・ゲインを調整し、調整後のカルマン・ゲインにより現在位置を求めることを特徴とする。
【００１２】
本発明による位置検出装置の他の実施形態によれば、
加速度を測定する手段と、加速度の履歴に基づき、新たに測定した加速度が異常であるか否かを判定する手段とをさらに備えており、位置判定手段は、新たに測定した各ランドマーク装置との距離のいずれかが異常であり、かつ、新たに測定した加速度が異常である場合、現在位置が測定不可であると判定することも好ましい。
【００１３】
また、本発明による位置検出装置の他の実施形態によれば、
距離異常判定手段は、異常ではないと判定したときの、測位手段が測定した各ランドマーク装置との距離と、カルマン・フィルタによる、その予測値との差の平均値である第１の値を保持しており、新たに測定した各ランドマーク装置との距離と、その予測値との差である第２の値に基づき、前記平均値を更新し、新たに測定した第１のランドマーク装置との距離と、前記更新後の平均値との差である第３の値を求め、第３の値に基づく値を閾値判定して、新たに測定した第１のランドマーク装置との距離が異常であるか否かを判定することも好ましい。
【００１４】
さらに、本発明による位置検出装置の他の実施形態によれば、
距離異常判定手段は、測位手段が測定した各ランドマーク装置との距離と、その予測値との差の標準偏差値である第４の値を、前記第１の値及び前記第２の値に基づき計算し、前記第３の値を、前記第４の値で除した値である第５の値を求め、第５の値に基づく値を閾値判定して、新たに測定した第１のランドマーク装置との距離が異常であるか否かを判定することも好ましい。
【００１５】
さらに、本発明による位置検出装置の他の実施形態によれば、
距離異常判定手段は、平均及び標準偏差が、前記更新後の平均値及び第４の値である正規分布において、その平均値を中心とした積分値が前記第５の値の２倍から１を引いた値に等しくなる位置における確率密度を求め、求めた確率密度にランドマーク装置の数及び距離異常判定手段が異常ではないと判定した回数を乗じた第６の値を求め、第６の値を閾値判定して、新たに測定した第１のランドマーク装置との距離が異常であるか否かを判定することも好ましい。
【００１６】
本発明におけるプログラムによれば、
上記位置検出装置としてコンピュータを機能させることを特徴とする。
【００１７】
本発明における位置検出方法によれば、
複数のランドマーク装置からの信号を受信する位置検出装置における位置検出方法であって、各ランドマーク装置から、各ランドマーク装置の位置情報を受信し、各ランドマーク装置との距離を測定するステップと、各ランドマーク装置との距離の履歴に基づき、新たに測定した各ランドマーク装置との距離が異常であるか否かを判定するステップと、各ランドマーク装置の位置情報と、各ランドマーク装置との距離に基づき、カルマン・フィルタにより現在位置を求めて出力するステップとを備えており、新たに測定した第１のランドマーク装置との距離が異常である場合、新たに測定した第１のランドマーク装置との距離から、カルマン・フィルタによる、その予測値を引いた値が大きい程、小さくなる重み係数を算出し、算出した重み係数によりカルマン・ゲインを調整し、調整後のカルマン・ゲインにより現在位置を求めることを特徴とする。
【００１８】
本発明による位置検出方法の他の実施形態によれば、
加速度を測定するステップと、加速度の履歴に基づき、新たに測定した加速度が異常であるか否かを判定するステップとをさらに備えており、新たに測定した各ランドマーク装置との距離のいずれかが異常であり、かつ、新たに測定した加速度が異常である場合には、現在位置を測定不可であると判定することも好ましい。
【発明の効果】
【００１９】
移動により、各ランドマーク装置との距離の測定が不安定となったとしても、測定精度を劣化させることなく位置を推定することができる。本発明による位置検出に必要な計算処理は少なく、本発明は、携帯型機器への実装に適したものである。
【図面の簡単な説明】
【００２０】
【図１】本発明による位置検出装置の構成図である。
【図２】本発明による位置検出システムの構成図である。
【図３】本発明による位置検出方法のフロー図である。
【図４】異常判定処理を説明する図である。
【発明を実施するための形態】
【００２１】
本発明を実施するための形態について、以下では図面を用いて詳細に説明する。
【００２２】
図２に示す様に、本発明による位置検出システムは、複数のランドマーク装置２０−１〜２０−ｎと、位置検出装置１０とを含んでおり、各ランドマーク装置２０−１〜２０−ｎは、周期的に、音波による信号と無線信号を同時に送信する。つまり、距離測定のためのＴＤＯＡ信号を送信する。好ましくは、各ランドマーク装置２０−１〜２０−ｎからのＴＤＯＡ信号の送信は、同期、つまりほぼ同時刻に生じる様に制御される。また、各ＴＤＯＡ信号に含まれる無線信号は、送信したランドマーク装置の設置位置についての情報を含んでいる。なお、位置検出装置１０が静止している場合において、位置検出装置１０の位置を確定させるためにはｎは３以上でなければならないが、本発明においては、利用者が位置検出装置１０を携帯して移動することを想定しており、現在位置の決定に過去の履歴を使用するため、ｎは２以上であれば良い。
【００２３】
図１は、本発明による位置検出装置の構成図である。図１によると、位置検出装置１０は、受信部１と、測距部２と、距離異常判定部３と、加速度異常判定部４と、加速度測定部５と、位置判定部６とを備えている。受信部１は、各ランドマーク装置からのＴＤＯＡ信号を受信して、ＴＤＯＡ信号に含まれる無線信号から各ランドマーク装置の設置位置情報を取得し、取得した各ランドマーク装置の設置位置情報を距離異常判定部３及び位置判定部６に通知する（図示せず。）。
【００２４】
測距部２は、各ランドマーク装置からＴＤＯＡ信号を受信する度に、受信したＴＤＯＡ信号に基づき各ランドマーク装置との距離を測定し、測定した距離を距離異常判定部３と、位置判定部６に出力する。また、加速度測定部５は、２軸又は３軸の加速度センサを有しており、周期的に測定した加速度を加速度異常判定部４に出力する。
【００２５】
距離異常判定部３は、各ランドマーク装置との距離の履歴から、各周期において測距部２から入力された距離が異常であるか否かを判定し、判定結果を位置判定部６に出力する。同様に、加速度異常判定部４は、加速度の履歴から、各周期において加速度測定部５から入力された加速度が異常であるか否かを判定し、判定結果を位置判定部６に出力する。
【００２６】
図３は、位置判定部６における処理フロー図である。なお、以下の説明において、ある時刻において、ＴＤＯＡ信号により測定した各ランドマーク装置との距離を満たす位置を“測定位置”と呼び、当該時点において、位置検出装置１０が予測した位置を“予測位置”と呼び、当該時点における測定位置及び予測位置から求めた位置を“最適位置”と呼ぶものとする。なお、位置判定部６は、最適位置を、当該時点における位置として外部に出力する。以下に、図３の処理フローを説明する。
【００２７】
位置判定部６は、周期的に、測距部２から各ランドマーク装置との距離を、距離異常判定部３及び加速度異常判定部４から判定結果を受信する（Ｓ３１）。距離異常判定部３からの判定結果が正常である場合には（Ｓ３２）、第１の位置判定処理により最適位置を求めて（Ｓ３４）、求めた最適位置を出力する（Ｓ３５）。一方、距離異常判定部３からの判定結果が異常である場合には（Ｓ３２）、加速度異常判定部４からの判定結果が異常であるか否かを確認する（Ｓ３３）。加速度異常判定部４からの判定結果が正常である場合には、第２の位置判定処理により最適位置を求めて（Ｓ３６）、求めた最適位置を出力する（Ｓ３７）。一方、加速度異常判定部４からの判定結果が異常である場合には、測定不可を出力する（Ｓ３８）。位置判定部６は、Ｓ３１からＳ３５、Ｓ３７又はＳ３８までの処理を、位置検出処理の終了指示を外部から受け取るまで、周期的に繰り返し行う。
【００２８】
第１の位置判定処理は、ある処理時点における各ランドマーク装置との距離が過去の履歴から異常ではないと判定された場合の処理である。つまり、受信しているＴＤＯＡ信号が信頼できる場合の処理であり、カルマン・フィルタにより最適位置を算出するものである。第１の位置判定処理を説明する前に、本発明の説明に必要な各パラメータについて説明する。まず、時刻ｋにおける位置検出装置１０の位置を（ｐｘ（ｋ），ｐｙ（ｋ））、位置検出装置１０の速度を（ｖｘ（ｋ），ｖｙ（ｋ））とし、
【００２９】
【数１】
2012002734.tif 000003
とすると、Ｘ（ｋ）は、以下の式で表されることになる。なお、Δｔは測定周期である。
Ｘ（ｋ）＝ＡＸ（ｋ−１）＋μ（ｋ）
【００３０】
なお、上式のμ（ｋ）は時間遷移に関する雑音であり、共分散行列Ｑかつ正規分布に従う。共分散行列Ｑは、固定値であってもよいが、非特許文献７に記載されている方法に従い時間により変化させてもよい。
【００３１】
時刻ｋにおいて、位置検出装置１０がｎ個のランドマーク装置からＴＤＯＡ信号を受信し、ｉ番目のランドマーク装置の位置を（ｐｘｉ，ｐｙｉ）、ＴＤＯＡ信号から計算したｉ番目のランドマーク装置と位置検出装置１０との距離をｄｉとし、
【００３２】
【数２】
2012002734.tif 000004
とすると、Ｚ（ｋ）は、以下の式で表されることになる。
Ｚ（ｋ）＝Ｂ（ｋ）＋ｖ（ｋ）
【００３３】
なお、上式において、ｖ（ｋ）は観測雑音であり、共分散行列Ｒかつ零平均の多変数正規分布に従う。共分散行列Ｒは、固定値であってもよいが、非特許文献７に記載されている方法に従い時間により変化させてもよい。本発明においては、観測モデルＨ（ｋ）は、Ｂ（ｋ）の各要素を偏微分して得られる以下の行列として定義する。
【００３４】
【数３】
2012002734.tif 000005
【００３５】
（第１の位置判定処理）続いて、第１の位置判定処理について説明する。位置判定部６は、時刻ｋにおける予測行列Ｘｐ（ｋ）を、時刻ｋ−１における最適行列Ｘｏ（ｋ−１）から、以下の式により求める。
Ｘｐ（ｋ）＝ＡＸｏ（ｋ−１）
なお、予測行列Ｘｐ及び最適行列Ｘｏは、４行１列であり、１行目及び３行目の要素がそれぞれ予測位置及び最適位置のｘ座標及びｙ座標を示し、２行目及び４行目の要素がそれぞれｘ方向及びｙ方向の速度成分を示している。また、最適行列Ｘｏの初期値は、最終的には収束するため任意であり、例えば、総ての要素を０とする。
【００３６】
続いて、時刻ｋにおける予測誤差行列Ｐ（ｋ）と、カルマン・ゲインＫ（ｋ）を以下の式により求める。
Ｐ（ｋ）＝ＡＣ（ｋ−１）ＡＴ＋Ｑ
Ｋ（ｋ）＝Ｐ（ｋ）Ｈ（ｋ）Ｔ（Ｈ（ｋ）Ｐ（ｋ）Ｈ（ｋ）Ｔ＋Ｒ）−１
ここで、Ｃ（ｋ−１）は、以下の式で求められる。
Ｃ（ｋ−１）＝（１−Ｋ（ｋ−１）Ｈ（ｋ−１））Ｐ（ｋ−１）
なお、Ｃ（ｋ）の初期値は、最終的には収束するため任意であり、例えば、４行４列の単位行列とする。
【００３７】
最後に、位置判定部６は、時刻ｋにおける最適行列Ｘｏ（ｋ）を以下の式により求め、求めたＸｏ（ｋ）の第１行及び第３行を取り出し、最適位置として出力する。
Ｘｏ（ｋ）＝Ｘｐ（ｋ）＋Ｋ（ｋ）（Ｚ（ｋ）−Ｈ（ｋ）Ｘｐ（ｋ））
また、時刻ｋ＋１での処理のため、
Ｃ（ｋ）＝（１−Ｋ（ｋ）Ｈ（ｋ））Ｐ（ｋ）を求めておく。
【００３８】
位置検出装置１０が各ランドマーク装置からの信号を常に安定して受信できる場合、上述した第１の位置判定処理を繰り返すのみでよいが、現実的には、障害物の影響等により受信状態が不安定になる。この不安定な状態にて検出した異常値をそのまま第１の位置判定処理にて使用するとその精度が劣化するため、本発明においては、測定距離が異常である場合（図３のＳ３２）には、第１の位置判定処理による最適位置の決定は行わず、加速度が異常であるか否かに応じて（図３のＳ３３）、第２の位置判定処理による最適位置の決定又は測定不可の出力を行う。
【００３９】
（距離異常判定処理）以下、距離異常判定部３による距離異常判定処理について説明する。なお、位置判定部６は、時刻ｋ−１の処理終了後、時刻ｋにおける予測行列Ｘｐ（ｋ）を算出して、その１行目及び３行目の要素で表される位置、つまり、時刻ｋにおける予測位置を、距離異常判定部３に出力しておく。
【００４０】
距離異常判定部３は、時刻ｋにおける予測位置に基づき、ｉ番目のランドマーク装置との距離ｄｐｉ（ｋ）を求め（ｉはｎ以下の自然数）、Ｚ（ｋ）の各要素ｄｉ（ｋ）との差γｉ（ｋ）＝ｄｉ（ｋ）−ｄｐｉ（ｋ）を求める。
【００４１】
また、距離異常判定部３は、時刻ｋ−１までの各時刻における距離異常判定処理にて、異常ではないと判断した回数を記録しており、異常では無い時の処理にて求めた各γの総ての平均値
【００４２】
【数４】
2012002734.tif 000006
と、時刻ｋにおけるγｉ（ｋ）から、時刻ｋにおけるγの平均値
【００４３】
【数５】
2012002734.tif 000007
を、以下の式により求める。
【００４４】
【数６】
2012002734.tif 000008
なお、時刻ｋ−１までの各時刻における距離異常判定処理にて、異常ではないと判断した回数をｐ−１とし、平均値の初期値は、以下の式で表される。
【数７】
2012002734.tif 000009
さらに、距離異常判定部３は、時刻ｋにおけるγの分散σ２（ｋ）を以下の式により求める。
【００４５】
【数８】
2012002734.tif 000010
【００４６】
距離異常判定部３は、各γｉ（ｋ）について、以下の式によりΦの値を求め、続いて、２Φ−１の値を求める。
【００４７】
【数９】
2012002734.tif 000011
続いて、距離異常判定部３は、平均及び分散がそれぞれ
【００４８】
【数１０】
2012002734.tif 000012
である正規分布において、その平均値を中心とした積分値が２Φ−１となる位置における確率密度を求める。例えば、Φの値が０．９５であり、図４が求めた正規分布であり、符号６１から６２に示す範囲の積分値が０．９であるものとすると、２Φ−１の値は０．９であるから、図４に示すαに対応する値を求める。
【００４９】
最後に、距離異常判定部３は、求めた確率密度の値をｎｐ倍し、この値と所定の閾値、例えば、０．５とを比較し、ｎｐ倍した値が所定の閾値より小さい場合、異常であると判定し、そうでない場合には正常であると判定する。つまり、図４において符号６１及び６２で示す位置が、平均から所定の距離より離れている場合に、異常であると判定する。
【００５０】
（加速度異常判定処理）続いて、加速度異常判定部４における加速度異常判定処理について説明する。まず、加速度異常判定部４は、加速度測定部５から取得する時刻ｋにおける直交する２方向の加速度の値ａｘ（ｋ）及びａｙ（ｋ）から、時刻ｋにおける加速度の値ａ（ｋ）を以下の式により求める。
【００５１】
【数１１】
2012002734.tif 000013
【００５２】
なお、ａｘ（ｋ）及びａｙ（ｋ）は、加速度測定部５が加速度センサから取得した時刻ｋにおける瞬時値であってもよいが、加速度測定部５は、時刻ｋ−１から時刻ｋまでのある期間における加速度センサが出力する値を積分して、積分した値をａｘ（ｋ）及びａｙ（ｋ）として加速度異常判定部４に出力することが好ましい。
【００５３】
続いて、加速度異常判定部４は、時刻ｋにおける加速度の値ａ（ｋ−１）から、時刻ｋにおける加速度の予測値ａｐ（ｋ）を以下の式により求める。
ａｐ（ｋ）＝ａ（ｋ−１）＋Δτ
なお、Δτは、以下の式であらわされる値である。
Δτ＝ｓｇｎ（ａ（ｋ−１）−ａ（ｋ−２））*Δτ´
ここで、Δτ´は以下の式に示す様に、時刻ｋ−Ｍから時刻ｋ−１までの加速度の変化の平均値である。
【００５４】
【数１２】
2012002734.tif 000014
また、ｓｇｎ（）は、引数の符号を戻り値とする関数であり、具体的には、引数が正の値のときは１、引数が負の値のときは−１、引数が零のときは零がそれぞれ戻り値となる。
【００５５】
続いて、加速度異常判定部４は、時刻ｋにおける加速度の予測誤差ｄｉｆｆ（ｋ）を以下の式により求める。
ｄｉｆｆ（ｋ）＝ａ（ｋ）−ａｐ（ｋ）
【００５６】
加速度異常判定部４は、時刻ｋ−１までの各時刻における加速度異常判定処理にて、異常ではないと判断された回数を記録しており、異常では無い時の処理にて求めた各予測誤差ｄｉｆｆの総ての平均値
【００５７】
【数１３】
2012002734.tif 000015
と、時刻ｋにおけるｄｉｆｆ（ｋ）から、時刻ｋにおけるｄｉｆｆの平均値
【００５８】
【数１４】
2012002734.tif 000016
を、以下の式により求める。
【００５９】
【数１５】
2012002734.tif 000017
なお、時刻ｋ−１までの各時刻における加速度異常判定処理にて、異常ではないと判断された回数はｑ−１とする。さらに、加速度異常判定部４は、時刻ｋにおける予測誤差の分散δ２（ｋ）を以下の式により求める。
【００６０】
【数１６】
2012002734.tif 000018
以下、距離異常判定処理と同じく、加速度異常判定部４は、以下の式によりΨの値を求め、続いて、２Ψ−１の値を求める。
【００６１】
【数１７】
2012002734.tif 000019
加速度異常判定部４は、平均及び分散がそれぞれ
【００６２】
【数１８】
2012002734.tif 000020
である正規分布において、その平均値を中心とした積分値が２Ψ−１となる位置における確率密度を求め、求めた確率密度の値をｑ倍し、この値と所定の閾値とを比較し、ｑ倍した値が所定の閾値より小さい場合、異常であると判定する。
【００６３】
以上、本発明においては、距離異常判定処理にて測定位置の異常を検出した場合、加速度異常判定処理にて、加速度の異常が発生しているか否かを判定し、加速度の異常が発生していなければ、位置検出装置を携帯している利用者の動作に大きな変化が発生していないものとして、以下に説明する第２の位置判定処理を行う。これに対して、加速度の異常が発生している場合には、位置の判定は不可であると判定して、測定不可を出力する。
【００６４】
（第２の位置判定処理）続いて、第２の位置判定処理について説明する。第２の位置判定処理において、位置判定部６は、距離異常判定部３から、距離異常判定部３が判定に使用した各パラメータを取得し、時刻ｋにおける各γｉ（ｋ）について、マハラビノス距離Ｍｉ（ｋ）を、以下の式により求める。
【００６５】
【数１９】
2012002734.tif 000021
【００６６】
続いて、位置判定部６は、重み行列Ｗ（ｋ）を、以下の様に求める。
【００６７】
【数２０】
2012002734.tif 000022
なお、重み係数ｗｉ（ｋ）は、以下の式で表される。
【００６８】
【数２１】
2012002734.tif 000023
【００６９】
位置判定部６は、以下の通り、重み行列により、Ｚ（ｋ）、Ｂ（ｋ）、Ｈ（ｋ）及びＲを調整する。
Ｚ´（ｋ）＝Ｗ（ｋ）Ｚ（ｋ）
Ｂ´（ｋ）＝Ｗ（ｋ）Ｂ（ｋ）
Ｈ´（ｋ）＝Ｗ（ｋ）Ｈ（ｋ）
Ｒ´＝ｃｏｖ［Ｗ（ｋ）ｖ（ｋ）］
【００７０】
さらに、位置判定部６は、重み行列で調整したカルマン・ゲインＫ´（ｋ）を以下の式により求める。
Ｋ´（ｋ）＝Ｐ（ｋ）Ｈ´（ｋ）Ｔ（Ｈ´（ｋ）Ｐ（ｋ）Ｈ´（ｋ）Ｔ＋Ｒ´）−１
【００７１】
最後に、位置判定部６は、時刻ｋにおける最適行列Ｘｏ（ｋ）を以下の式により求め、求めたＸｏ（ｋ）の第１行及び第３行を取り出し、最適位置として出力する。
Ｘｏ（ｋ）＝Ｘｐ（ｋ）＋Ｋ´（ｋ）（Ｚ´（ｋ）−Ｂ´（ｋ））
また、時刻ｋ＋１での処理のため、
Ｃ（ｋ）＝（１−Ｋ´（ｋ）Ｈ´（ｋ））Ｐ（ｋ）を求めておく。
【００７２】
以上、位置検出装置１０は、ＴＤＯＡ信号により求めた距離に異常を検出した場合、測定位置の予測位置との差と、その差の過去の平均との差による重み係数を求め、求めた重み係数でカルマン・ゲインを調整して最適位置を算出することで、異常値を検出した場合であっても測定精度を劣化させることなく位置を推定することができる。本発明による最適位置の算出に必要な計算処理負荷は少なく、本発明は、携帯型機器への実装に適したものである。
【００７３】
なお、上述した実施形態においては、音波信号及び無線信号を含むＴＤＯＡ信号により位置検出装置１０は各ランドマーク装置との距離を測定していたが、位置検出装置１０は、ランドマーク装置までの距離を測定する公知の種々の方法を使用することができる。
【００７４】
また、本発明による位置検出装置１０は、コンピュータを図１の各部として機能させるプログラムにより実現することができる。これらコンピュータプログラムは、コンピュータが読み取り可能な記憶媒体に記憶されて、又は、ネットワーク経由で配布が可能なものである。さらに、本発明は、ハードウェア及びソフトウェアの組合せによっても実現可能である。
【符号の説明】
【００７５】
１  受信部
２  測距部
３  距離異常判定部
４  加速度異常判定部
５  加速度測定部
６  位置判定部
１０  位置検出装置
２０−１〜２０−ｎ  ランドマーク装置
【図１】
2012002734.tif 000024
【図２】
2012002734.tif 000025
【図３】
2012002734.tif 000026
【図４】
2012002734.tif 000027