"位置検出対象の移動部分に装着された送信機から発信された信号を前記位置検出対象の参照部分に装着された3個の受信機で受信して前記移動部分の位置を検出する位置検出装置であって、前記移動部分の3次元空間の移動範囲を固定サイズのグリッドで分割し、グリッド中心の3次元座標とグリッド中心から前記3個の受信機までの3つの距離をベクトルとしたKD木のデータベースが予め登録された記憶部と、前記3個の受信機が出力する、前記送信機と各受信機との間の距離に依存する距離情報に基づいて前記記憶部内の前記データベースを探索して反復法ソルバの初期位置を決定し、前記初期位置を用いて連立非線形方程式を解いて前記移動部分の位置を検出するプロセッサを備え、前記プロセッサは、前記距離情報をフィルタリングして前記データベースの探索用距離情報を取得する第1のカルマンフィルタと、前記探索用距離情報を用いて前記データベースから最短隣グリッドを最短隣検索アルゴリズムに基づいて特定し、最短隣グリッドの中心位置を前記反復法ソルバの初期位置として代入して前記連立非線形方程式を解き、前記反復法ソルバが収束するか否かを判定する判定手段と、前記反復法ソルバが収束する場合は、前記第1のカルマンフィルタの推定状態のモデルのノイズレベルに応じて動的に調整されて前記移動部分の位置状態を推定する位置状態推定用の第2のカルマンフィルタを有することを特徴とする、位置検出装置。前記プロセッサは、前記反復法ソルバが収束しない場合は、前記最短隣グリッドに隣接する隣接グリッドを特定して各隣接グリッドの中心位置を前記反復法ソルバの初期位置として代入して前記連立非線形方程式を解いて再度解を探す探索手段を更に有することを特徴とする、請求項1記載の位置検出装置。前記プロセッサは、前記探索手段が解を探すことができないと、解が得られた位置の平均から近似解を求めて前記第2のカルマンフィルタに出力する手段を更に有することを特徴とする、請求項2記載の位置検出装置。位置検出対象の移動部分に装着される送信機と、前記位置検出対象の参照部分に装着され、前記送信機から発信された信号を受信する3個の受信機と、前記移動部分の3次元空間の移動範囲を固定サイズのグリッドで分割し、グリッド中心の3次元座標とグリッド中心から前記3個の受信機までの3つの距離をベクトルとしたKD木のデータベースが予め登録された記憶部と、前記3個の受信機が出力する、前記送信機と各受信機との間の距離に依存する距離情報に基づいて前記記憶部内の前記データベースを探索して反復法ソルバの初期位置を決定し、前記初期位置を用いて連立非線形方程式を解いて前記移動部分の位置を検出する検知処理部を備え、前記検知処理部は、前記距離情報をフィルタリングして前記データベースの探索用距離情報を取得する第1のカルマンフィルタと、前記探索用距離情報を用いて前記データベースから最短隣グリッドを最短隣検索アルゴリズムに基づいて特定し、最短隣グリッドの中心位置を前記反復法ソルバの初期位置として代入して前記連立非線形方程式を解き、前記反復法ソルバが収束するか否かを判定する判定手段と、前記反復法ソルバが収束する場合は、前記第1のカルマンフィルタの推定状態のモデルのノイズレベルに応じて動的に調整されて前記移動部分の位置状態を推定する位置状態推定用の第2のカルマンフィルタを有することを特徴とする、ウェアラブルセンサシステム。コンピュータに、位置検出対象の移動部分の位置を検出する位置検出処理を実行させるプログラムであって、前記移動部分の3次元空間の移動範囲を固定サイズのグリッドで分割し、グリッド中心の3次元座標とグリッド中心から、前記位置検出対象の参照部分に装着された3個の受信機までの3つの距離をベクトルとしたKD木のデータベースが予め登録された記憶部を用い、前記移動部分に装着された送信機から発信された信号を受信する前記3個の受信機が出力する、前記送信機と各受信機との間の距離に依存する距離情報に基づいて前記記憶部内の前記データベースを探索して反復法ソルバの初期位置を決定し、前記初期位置を用いて連立非線形方程式を解いて前記移動部分の位置を検出する検出手順を前記コンピュータに実行させ、前記検出手順は、前記距離情報を第1のカルマンフィルタによりフィルタリングして前記データベースの探索用距離情報を取得する第1のフィルタリング手順と、前記探索用距離情報を用いて前記データベースから最短隣グリッドを最短隣検索アルゴリズムに基づいて特定し、最短隣グリッドの中心位置を前記反復法ソルバの初期位置として代入して前記連立非線形方程式を解き、前記反復法ソルバが収束するか否かを判定する判定手順と、前記反復法ソルバが収束する場合は、前記第1のカルマンフィルタの推定状態のモデルのノイズレベルに応じて動的に調整されて前記移動部分の位置状態を推定する位置状態推定用の第2のカルマンフィルタによるフィルタリングを行う第2のフィルタリング手順を含むことを特徴とする、プログラム。従来のウェアラブルセンサシステムでは、比較的簡単、且つ、安価な構成で比較的高精度に位置検出を行うことは難しいという問題があった。そこで、本発明は、比較的簡単、且つ、安価な構成で比較的高精度に位置検出を行うことが可能な位置検出装置、ウェアラブルセンサシステム及びプログラムを提供することを目的とする。本発明の一観点によれば、位置検出対象の移動部分に装着された送信機から発信された信号を前記位置検出対象の参照部分に装着された3個の受信機で受信して前記移動部分の位置を検出する位置検出装置であって、前記移動部分の3次元空間の移動範囲を固定サイズのグリッドで分割し、グリッド中心の3次元座標とグリッド中心から前記3個の受信機までの3つの距離をベクトルとしたKD木のデータベースが予め登録された記憶部と、前記3個の受信機が出力する、前記送信機と各受信機との間の距離に依存する距離情報に基づいて前記記憶部内の前記データベースを探索して反復法ソルバの初期位置を決定し、前記初期位置を用いて連立非線形方程式を解いて前記移動部分の位置を検出するプロセッサを備え、前記プロセッサは、前記距離情報をフィルタリングして前記データベースの探索用距離情報を取得する第1のカルマンフィルタと、前記探索用距離情報を用いて前記データベースから最短隣グリッドを最短隣検索アルゴリズムに基づいて特定し、最短隣グリッドの中心位置を前記反復法ソルバの初期位置として代入して前記連立非線形方程式を解き、前記反復法ソルバが収束するか否かを判定する判定手段と、前記反復法ソルバが収束する場合は、前記第1のカルマンフィルタの推定状態のモデルのノイズレベルに応じて動的に調整されて前記移動部分の位置状態を推定する位置状態推定用の第2のカルマンフィルタを有することを特徴とする位置検出装置が提供される。本発明の一観点によれば、位置検出対象の移動部分に装着される送信機と、前記位置検出対象の参照部分に装着され、前記送信機から発信された信号を受信する3個の受信機と、前記移動部分の3次元空間の移動範囲を固定サイズのグリッドで分割し、グリッド中心の3次元座標とグリッド中心から前記3個の受信機までの3つの距離をベクトルとしたKD木のデータベースが予め登録された記憶部と、前記3個の受信機が出力する、前記送信機と各受信機との間の距離に依存する距離情報に基づいて前記記憶部内の前記データベースを探索して反復法ソルバの初期位置を決定し、前記初期位置を用いて連立非線形方程式を解いて前記移動部分の位置を検出する検知処理部を備え、前記検知処理部は、前記距離情報をフィルタリングして前記データベースの探索用距離情報を取得する第1のカルマンフィルタと、前記探索用距離情報を用いて前記データベースから最短隣グリッドを最短隣検索アルゴリズムに基づいて特定し、最短隣グリッドの中心位置を前記反復法ソルバの初期位置として代入して前記連立非線形方程式を解き、前記反復法ソルバが収束するか否かを判定する判定手段と、前記反復法ソルバが収束する場合は、前記第1のカルマンフィルタの推定状態のモデルのノイズレベルに応じて動的に調整されて前記移動部分の位置状態を推定する位置状態推定用の第2のカルマンフィルタを有することを特徴とするウェアラブルセンサシステムが提供される。本発明の一観点によれば、コンピュータに、位置検出対象の移動部分の位置を検出する位置検出処理を実行させるプログラムであって、前記移動部分の3次元空間の移動範囲を固定サイズのグリッドで分割し、グリッド中心の3次元座標とグリッド中心から、前記位置検出対象の参照部分に装着された3個の受信機までの3つの距離をベクトルとしたKD木のデータベースが予め登録された記憶部を用い、前記移動部分に装着された送信機から発信された信号を受信する前記3個の受信機が出力する、前記送信機と各受信機との間の距離に依存する距離情報に基づいて前記記憶部内の前記データベースを探索して反復法ソルバの初期位置を決定し、前記初期位置を用いて連立非線形方程式を解いて前記移動部分の位置を検出する検出手順を前記コンピュータに実行させ、前記検出手順は、前記距離情報を第1のカルマンフィルタによりフィルタリングして前記データベースの探索用距離情報を取得する第1のフィルタリング手順と、前記探索用距離情報を用いて前記データベースから最短隣グリッドを最短隣検索アルゴリズムに基づいて特定し、最短隣グリッドの中心位置を前記反復法ソルバの初期位置として代入して前記連立非線形方程式を解き、前記反復法ソルバが収束するか否かを判定する判定手順と、前記反復法ソルバが収束する場合は、前記第1のカルマンフィルタの推定状態のモデルのノイズレベルに応じて動的に調整されて前記移動部分の位置状態を推定する位置状態推定用の第2のカルマンフィルタによるフィルタリングを行う第2のフィルタリング手順を含むことを特徴とするプログラムが提供される。開示の位置検出装置、ウェアラブルセンサシステム及びプログラムによれば、比較的簡単、且つ、安価な構成で比較的高精度に位置検出を行うことができる。開示の位置検出装置、ウェアラブルセンサシステム及びプログラムでは、位置検出対象の移動部分に装着された送信機から発信された信号を位置検出対象の参照部分に装着された3個の受信機で受信して前記移動部分の位置を検出する。位置検出装置は、前記移動部分の3次元空間の移動範囲を固定サイズのグリッドで分割し、グリッド中心の3次元座標とグリッド中心から3個の受信機までの3つの距離をベクトルとしたKD木のデータベースが予め登録された記憶部と、3個の受信機が出力する送信機と各受信機との間の距離に依存する距離情報に基づいてデータベースを探索して反復法ソルバの初期位置を決定し、初期位置を用いて連立非線形方程式を解いて移動部分の位置を検出する制御装置を備える。制御装置は、距離情報をフィルタリングしてデータベースの探索用距離情報を取得する第1のカルマンフィルタと、探索用距離情報を用いてデータベースから最短隣グリッドを最短隣検索アルゴリズムに基づいて特定し、最短隣グリッドの中心位置を反復法ソルバの初期位置として代入して前記連立非線形方程式を解き、前記反復法ソルバが収束するか否かを判定する判定部と、反復法ソルバが収束する場合は第1のカルマンフィルタの推定状態のモデルのノイズレベルに応じて動的に調整されて移動部分の位置状態を推定する位置状態推定用の第2のカルマンフィルタを有する。全てのグリッドで解が得られない場合は、第2のカルマンフィルタの予測値とグリッド探索の全ての最終点の平均を計算し、第1及び第2のカルマンフィルタの観測及び予測の共分散情報に基づいて第1及び第2のカルマンフィルタによる2つの位置推定を再度重み付けで平均化するようにしても良い。以下に、開示の位置検出装置、ウェアラブルセンサシステム及びプログラムの各実施例を図面と共に説明する。第1実施例図1は、本発明の第1実施例における位置検出装置の一例を示すブロック図である。本実施例では、位置検出装置はウェアラブルセンサシステムの一部を形成する。図1に示すウェアラブルセンサシステムは、ウェアラブルシステム1、通信部2、及び制御装置3を有する。ウェアラブルシステム1は、データ採取部の一例であり、超音波発信部11、同期信号及びクロック生成部12、及び超音波受信部13を有する。超音波発信部11は発信機の一例であり、超音波受信部13はセンサ又は、センシングデバイス等の受信機の一例である。超音波発信部11から超音波を発信すると、発信タイミングが同期信号及びクロック生成部12に供給されて発信タイミングに基づいた同期信号が生成される。又、同期信号及びクロック生成部12は、クロックを生成する。超音波発信部11から発信された超音波は、同期信号及びクロック生成部12からの同期信号に同期して同期信号及びクロック生成部12からのクロックで決定されるタイミングで超音波受信部13により受信される。超音波受信部13は、3個の超音波センサを有し、各超音波センサが同期信号に同期してクロックに基づいて超音波を受信することで、各超音波センサが超音波発信部11からの距離に応じたタイミングで超音波を受信する。従って、超音波受信部13からは、各超音波センサの超音波検出出力に基づいて、各超音波センサの超音波発信部11からの距離に依存する距離情報又は、距離信号が出力される。制御装置3は、例えばパーソナルコンピュータPC:PersonalComputer等の周知の汎用コンピュータ、或いは、スマートフォンSmartphone、PDAPersonalDigitalAssistant等の携帯端末で形成可能である。この例では、制御装置3はデータベースDB:DataBase作成部31、外部ストレージDB32、及び検知処理部33を有する。検知処理部33は、プロセッサの一例であるCPUCentralProcessingUnit331、記憶部の一例であるメモリ332、及び通信部333を有する。CPU331は、メモリ332に格納されたプログラムを実行することにより制御装置3全体を制御する。メモリ332は、半導体記憶装置、磁気記録媒体、光記録媒体、光磁気記録媒体等で形成可能であり、上記のプログラムや各種データを格納すると共に、CPU331が実行する演算の中間結果や演算結果等を一時的に格納する一時メモリとしても機能する。CPU331は、メモリ332に格納されたプログラムを実行することにより、制御装置コンピュータ3を位置検出装置として機能させる。プログラムは、CPU331に通信部2から受信した距離情報に基づいて少なくとも図4と共に後述する位置検出処理の手順を実行させるものであり、メモリ332を含む適切なコンピュータ読み取り可能な記憶媒体に格納されていても良い。DB作成部31は、図3と共に後述するグリッド分割及び情報登録処理を実行し、後述する位置推定処理に必要なKD木KDTreeのデータベース以下、単にKDTreeDBとも言うを作成し、外部ストレージ32に格納する。後述するように、検知処理部33による位置検出処理は、外部ストレージ32に格納されたKDTreeDBを用いて実行される。DB作成部31は、例えばCPUにより形成可能であるが、検知処理部33内のCPU331がDB作成部31としても機能可能な場合には、DB作成部31は省略可能である。通信部2は、超音波受信部13からの距離情報を例えばケーブルを介して受信する。又、通信部2は、受信した距離情報を、無線ネットワークを介して制御装置3に送信する。この例では、通信部2と制御装置3との間の通信は、検知処理部33内の通信部333を介して行われる。尚、リアルタイムで位置検出処理を行う必要がない場合には、通信部2はUSBUniversalSerialBus等の接続部或いは、接続手段を介して制御装置3即ち、検知処理部33内の通信部333に接続可能である。尚、各超音波センサの超音波発信部11からの距離に依存する距離情報は、超音波受信部13内で各超音波センサの出力に基づいて算出しても、制御装置3の検知処理部33において算出しても良い。検知処理部33において距離情報を算出する場合は、超音波受信部13は距離情報として超音波センサの出力をそのまま出力可能である。この場合も、超音波センサの出力即ち、距離情報が各超音波センサの超音波発信部11からの距離に依存することには変わりない。図2は、ウェアラブルシステムの一例を示す図である。図2に示すように、ウェアラブルシステム1の超音波発信部11は、第1の装着部110を有する。第1の装着部110は、超音波発信機111と、同期信号及びクロック生成部12、オン/オフスイッチ図示せず等を含む回路部分112を有する。第1の装着部110は、ユーザの手首への装着を容易にするため、例えば腕時計の如くベルトで装着可能は構造を有する。ユーザの手首は、位置検出対象の移動部分の一例である。一方、ウェアラブルシステム1の超音波受信部13は、第2の装着部130を有する。第2の装着部130は、略V字形状を有し、例えば互いに等距離の位置に設けられた3個の超音波センサ131,132,133と、紐又はチェーン134と、回路部分135を有する。第2の装着部130は、ユーザへの装着を容易にするため、例えばユーザがネックレスのように首からかけられる構造を有し、装着状態ではユーザの胴体即ち、胸の部分に配置される。尚、紐又はチェーン134は、第2の装着部130をユーザの胴体に装着可能なベルトの如き構造を有しても良いことは言うまでもない。ユーザの胴体は、位置検出対象の参照点を含む参照部分の一例である。回路部分135は、各超音波センサ131,132,133の超音波検出出力をそのまま距離情報として出力しても、各超音波センサ131,132,133の超音波検出出力に基づいて各超音波センサ131,132,133の超音波発信機111からの距離を示す距離情報を算出して出力する回路を含んでも良い。又、回路部分135は、通信部2を含んでも良い。同期信号及びクロック生成部12は、第1の装着部110の超音波発信機111と第2の装着部130の各超音波センサ131,132,133とを接続部或いは、接続手段を介して接続される。接続部は、有線又は無線で第1及び第2の装着部110,130を接続する。接続部が第1及び第2の装着部110,130間を無線接続する場合、。又、この例では、説明の便宜上、通信部2が第2の装着部130に設けられているものとするが、通信部2は例えばケーブル図示せずを介して第2の装着部130に接続されていても良いことは言うまでもない。ウェアラブルセンサシステムは、比較的簡単、且つ、安価な構成で比較的高精度に位置検出を行うことが望ましい。又、ウェアラブルセンサシステムにおいて、電力消耗、サイズ、重量、デザイン等の要因から、超音波センサ等のセンサの数は最小限に抑えたい。従って、図2に示すように3個の超音波センサ131~133を使用する場合、超音波発信機111から各超音波センサ131~133即ち、各参照点の位置までの3つの距離を超音波の到着時間より計測し、計測された距離から超音波発信機111の位置、即ち、第1の装着部110を装着したユーザの手首又は、手先の3次元位置を計測することが考えられる。この場合、次の連立方程式1を解くことになる。しかし、次の理由で、上記連立方程式1を解くことは難しい。第1に、強い非線形特性を持つ解析解が存在しないからである。第2に、多変数反復ニュートンラフソンMultivariableIterationNewtonRaphson法を利用する際、検出対象となる実際の位置付近に初期探索始点を決定する必要があるものの、ユーザの手先の動きを検知するための探索を開始するべきスタート地点が予め分かっていない。第3に、特に超音波センサの場合は、温度、風速、音波の干渉等の影響により超音波センサの出力にノイズが混入し易いので、このノイズにより一般的な非線形方程式のソルバSolverでは単一解が得られない場合がある。そこで、本実施例では、次のような手順ST1~ST6を用いてユーザの手先の位置を推定する。ST1:距離観測用のカルマンフィルタKalmanFilterKF1を用いて観測モデル又は、誤差モデルに基づいた観測距離のフィルタリングFilteringを行う。ST2:予めユーザの手先の移動範囲を固定サイズのグリッドで分割し、グリッド中心の3次元座標とグリッド中心から3個の超音波センサ131~133までの3つの距離をベクトルとして、KDtreeに登録する。ST3:3つの距離の観測結果を取得する際、KDtreeの最短隣探索で最短隣グリッドを特定し、最短隣グリッドの中心の座標を探索始点とする。ST4:探索始点を初期解とし、NewtonRaphson法で上記連立方程式1を解き、解が得られた場合は、別の位置状態推定用の適応カルマンフィルタAdaptiveKalmanFilterKF2を用いて位置状態を推定する。この適応カルマンフィルタKF2のモデルは、カルマンフィルタKF1の推定状態のモデルのノイズレベルに応じて動的に調整される。ST5:手順ST3において解が得られない場合は、グリッドに隣接する例えば8つの隣接グリッドを特定し、隣接グリッドの中心座標から再度解を探す。ST6:全てのグリッドでの探索が成功しない即ち、解が得られない場合は、適応カルマンフィルタKF2の予測値と、グリッド探索の全ての最終点の平均を計算し、カルマンフィルタKF1,KF2の観測及び予測の共分散情報に基づいてカルマンフィルタKF1,KF2による2つの位置推定を再度重み付けで平均化する。これにより、ユーザの手先の移動範囲内において、常に3次元位置を解くための唯一の初期探索始点を特定することができる。又、超音波センサ131~133の出力にノイズが混入していても、各方向から探索することで解を探すことが可能となり、解が収束しない場合であっても幾何平均と状態予測で近似解を得ることができる。更に、階層化されたフィルタによる統合で、適応的な状態推定を行うことができ、比較的簡単、且つ、安価な構成で位置測定の精度を向上することが可能となる。本実施例では、ユーザの手先の位置以下、単に手先位置とも言うを高精度に推定するために、オフライン処理とオンライン処理の2つの処理を行う。図3は、グリッド分割及び情報登録処理を説明するフローチャートであり、DB作成部31が実行するオフライン処理の概要を示す。オフライン処理では、オンライン処理が行う位置推定処理に必要なKDtreeDBを作成する。図3のオフライン処理を説明する前に、手先位置の推定について説明する。図4は、位置検知における座標の定義を説明する図である。図4に示すように、3次元空間中の3つの超音波センサ131~133の参照点RP1RP2RP3の座標を定義する。手先位置に装着される超音波発信機111の位置を、推定目標となる手先位置gxyzと定義する。超音波発信機111からの超音波が超音波センサ131~133に到着する時間に基づいて、超音波発信機111からの各超音波センサ131~133までの距離を計算することができる。このため、ここでは説明の便宜上、超音波発信機111からの超音波が超音波センサ131~133に到着する時間ではなく、超音波発信機111から各超音波センサ131~133までの3つの距離を示す距離情報が観測結果として検知処理部33に入力されるものとする。尚、図5中、破線は手先位置gxyzの移動経路の一例を示す。3つの参照点RP1RP2RP3の座標xiyiziと3つの距離情報よりi123、理論上次の連立非線形方程式2を反復多変数NewtonRaphson法で解くことができ、手先位置を決定することができる。しかし、手先の動きを検知するための探索を開始するべきスタート地点が予め分かっていないため、反復法ソルバに必要な初期探索始点を特定できない。そこで、本実施例では、次のような方法で初期探索始点を特定する。次に、反復法ソルバに必要な初期探索始点の特定方法について説明する。先ず、3次元空間中の任意の場所にグローバル座標系を定義し、3つの参照点RP1RP2RP3の座標を定義する。更に、手先の移動空間をサイズLのグリッドで分割する。分割した全てのグリッドの中心位置の座標Giを取得する。又、グリッドの中心位置の座標Giから3つの超音波センサ131~133、即ち、3つの参照点RP1RP2RP3までの距離ri1ri2ri3を計算する。図5は、移動空間のグリッド化を説明する図である。説明の便宜上、図5の上部に、分割したグリッドの一部と参照点RP1RP2RP3との距離ri1ri2ri3を示す。分割したグリッドiの中心位置GxiGyiGziと、この中心位置GxiGyiGziから各参照点RP1RP2RP3までの距離情報を利用して、次のような探索用特徴ベクトルViを作成する。ここで、Kは常数であり、例えば1以下である。Viri1ri2ri3KGxiKGyiKGzi上記の全てのグリッドで作成した探索用特徴ベクトルViをKDtreeへ登録し、KDtreeDBを作成する。KDtree自体、及び、KDtreeによる最短隣探索自体は周知であり、その詳細な説明は省略する。尚、グリッドのサイズLの設定について、推定目標の近傍から手先の動きを検知するための探索を開始することが望ましいが、グリッドのサイズLが小さすぎるとKDTreeのサイズが膨大となり、最短隣探索に時間がかかってしまう。そこで、手先の移動範囲や観測結果に含まれるノイズ以下、観測ノイズとも言うの評価も考慮した場合、グリッドのサイズLは例えば10cm~20cmに設定可能である。KDtreeの最短隣探索アルゴリズムを利用して後述するオンライン処理で手先位置を検出する際、3つの距離情報から手先が存在するグリッドの中心位置を特定することができる。グリッドの中心位置を特定してから、この中心位置の座標GixGiyGizを反復法ソルバの初期探索始点に設定することにより、反復法で解を探索することが可能になる。具体的には、上記KDtreeの探索によるグリッドの中心位置の特定は、最短隣探索用距離の定義に応じて例えば次の2つの方法のいずれかを採用することができる。第1の方法は、標準ユークリッド距離を定義して用いる。多くのKDtreeの最短隣探索アルゴリズムは、次のD1の如きユークリッドノルムEuclideanNormで探索を行う。D1v1v22このような距離D1の定義で、多くのKDtreeのリソースをそのまま利用できると言うメリットがある。ただし、上記の位置推定処理において、後述するオンライン処理での計測情報は3つの距離情報しかないため、直接KDtreeに登録した探索用特徴ベクトルViとの距離の計算はできない。そこで、直接KDtreeに登録した探索用特徴ベクトルViとの距離の計算ができるように、オンライン処理で3つの距離情報ri1ri2ri3に基づいて次のようなベクトルwdを作成する。wdri1ri2ri3000このベクトルwdでKDtreeDBから最短隣グリッドを探索する際、KDtreeDBの作成時のベクトルのパラメータKを小さい常数例えば、1×10-6とする。つまり、最短隣探索のときの実際の距離riが探索の要素となるように、グリッドの中心位置に小さいパラメータKを乗算し、距離情報に比べて非常に小さくする。この結果、本来であれば直接探索できないグリッドの中心座標を3つの距離情報の探索を経由することにより間接的に取得することができる。このような方法で、リアルタイムに取得した任意の3つの距離情報から、予め用意したKDtreeDBから一番距離が短いグリッドを特定することができるので、特定したグリッドの中心位置に1/Kを乗算することで元の中心位置を求めてソルバの初期探索始点に設定する。第2の方法は、カスタマイズした距離を定義して用いる。上記第1の方法による距離の定義は、汎用的な定義でリソースを流用する上でメリットがあるが、最短隣探索時の距離の計算には、省略可能な情報も含まれている。そこで、距離の計算の効率を更に向上するために、距離の定義を以下の如く変更する。具体的には、KDtreeDBの登録用ベクトルの定義にあるパラメータKを1に設定する。Viri1ri2ri3GxiGyiGzi又、オンライン処理での距離の計測結果に基づいて次のように探索用特徴ベクトルViを定義する。Virj1rj2rj3そして、最短隣探索時の距離D1を次のように定義する。D1sqrt222このような距離の定義で、取得しようとしているグリッドの中心位置をKDtreeDBに登録するが、最短隣探索に影響しないようにベクトルの距離計算を修正することにより、上記第1の方法と同様にソルバの初期探索始点を設定することができる。図6は、KDTreeへ登録するデータを説明する図である。図6では説明の便宜上、参照点座標がRP1RP2RP3、手先移動範囲がグリッドの中心から±04m、グリッドサイズLがL=01m、パラメータKがK=10の場合を示す。図6が示すKDTreeへ登録したデータの一例には、手先位置gと各参照点RP1RP2RP3との距離ri1ri2ri3、及び分割したグリッドiの中心位置GxiGyiGziが含まれる。上記のような探索用特徴ベクトルViを作成してKDTreeへ登録することにより、オンライン処理では3次元観測がないため本来であれば直接探索できない3次元空間における位置を、一旦観測できる距離情報を経由して探索用特徴ベクトルViを求めることができる。このため、この探索用特徴ベクトルViに基づいて3次元空間における3次元位置情報の抽出及び復円を行うことで、3次元空間における位置の間接的な探索を実現できる。図3のオフライン処理の説明に戻るに、ステップS1は、3次元空間中の任意の場所にグローバル座標系を定義し、3つの参照点RP1RP2RP3の座標を定義し、手先の移動空間をサイズLのグリッドで分割する。ステップS2は、分割した全てのグリッドの中心位置の座標Giを取得する。ステップS3は、グリッドの中心位置の座標Giから3つの超音波センサ131~133、即ち、3つの参照点RP1RP2RP3までの距離ri1ri2ri3を計算する。ステップS4は、分割したグリッドiの中心位置GxiGyiGziと、この中心位置GxiGyiGziから各参照点RP1RP2RP3までの距離情報を利用して、上記の如き探索用特徴ベクトルViri1ri2ri3KGxiKGyiKGziを作成する。ステップS5は、グリッドiで作成した探索用特徴ベクトルViをKDtreeへ登録し、KDtreeDB601を外部ストレージ32内に作成する。KDtree自体、及び、KDtreeによる最短隣探索自体は周知であり、その詳細な説明は省略する。ステップS6は、全てのグリッドiで作成した探索用特徴ベクトルViをKDtreeへ登録したか否かを判定し、判定結果がNOであると、処理はステップS7へ進む。ステップS7は、処理の対象を次に処理するべきグリッドに設定し、処理はステップS2へ戻る。ステップS6の判定結果がYESになると、オフライン処理は終了する。図7は、位置推定処理を説明するフローチャートであり、検知処理部33即ち、CPU331が実行するオンライン処理の概要を示す。オンライン処理では、超音波センサ131~133の出力に基づいて求めた、超音波発信機111から超音波センサ131~133までの3つの距離情報と、KDtreeDBの最短隣探索から、非線形ソルバの初期位置を特定する。非線形ソルバが収束する場合の解と、収束しない場合の解を求める。更に精度良く手先位置を探索又は、トラッキングするために、階層化されたカルマンフィルタの適応調整を行い、調整後のカルマンフィルタの出力を最後の推定結果、即ち、最終の位置出力とする。図7のオンライン処理を説明する前に、反復法ソルバによる手先位置の特定と、手先位置の推定のためのフィルタリングについて説明する。反復法ソルバによる手先位置の特定は、以下のように行うことができる。先ず、上記の如く作成したKDtreeDBと3つの距離r1r2r3の観測結果より、オンライン処理で手先位置を特定する。具体的には、先ず、オンライン処理で観測した3つの距離情報r1r2r3を利用し、KDtreeDBからグリッドGiの中心位置の座標を探索し、探索した結果をX0とする。上記X0を反復法ソルバの初期位置として代入し、次のような方法で上記の連立非線形方程式を解く。このような方法で連立非線形方程式を解くこと自体は周知である。上記反復探索を行う際、関数行列Fが閾値ε以下、又は、δxがε以下なった場合は、反復探索が収束と判定する。ここで、εは予め設定したパラメータである。反復探索が収束した時点のxnewを、ソルバの新たな解として出力する。ただし、距離観測時のノイズ等の混入により、上記反復探索が収束しない場合もある。そこで、反復探索が収束しないときは、次のような方法で近似解を計算する。非線形反復法ソルバが収束しない場合は、別の初期位置を用いて探索することになるが、本実施例では次のような規則に従って新探索位置を決定する。先ず、上記反復法ソルバに必要な初期探索始点の特定方法で特定したグリッドをGiとし、このグリッドGiと隣接する隣接グリッドの中心位置を新探索位置に設定する。例えば、グリッドGiの面と接する6個のグリッドや、グリッドGiの中心から8個の頂点の延長線が通過する8個のグリッド等といった規則又は、ポリシーに従い、隣接グリッドを特定する。特定した隣接グリッドの中心位置から、上記反復法ソルバに必要な初期探索始点の特定方法で解を反復探索する。反復探索が収束しない場合は、最後の解が得られた探索位置を終了位置Siとしてメモリ332に記録し、次の隣接グリッドの中心位置から再度探索する。この再度の探索の結果、あるグリッドGjでの探索が収束した場合、その解を手先位置とする。全ての隣接グリッドの中心からの探索が収束しない場合は、各探索終了位置の平均位置で次式に従って多グリッド近似解Sを求める。SΣSi/N上記多グリッド近似解Sをそのまま使用することも可能であるが、位置検出精度を向上させるために、次のように、手先位置の位置状態推定用の適応カルマンフィルタKF2の予測位置Zpと統合するようにしても良い。PSwZpここで、Sは多グリッド近似解、Zpは位置状態推定用の適応カルマンフィルタFK2の予測位置、Pは最後の解が得られた探索位置での位置出力である。重み係数wの範囲は例えば01であり、位置状態推定用の適応カルマンフィルタFK2は距離観測用のカルマンフィルタKF1の観測予測の共分散と残差による計算を行う。図8は、隣接グリッドを用いて近似解を特定する方法を説明する図である。説明の便宜上、図8の上部に、隣接グリッドGiGjGkを示す。又、手先の前回の位置を破線の丸印、手先の予測位置をハッチング付きの丸印、手先の実位置をクロスハッチングで示す。位置状態推定用の適応カルマンフィルタFK2によるフィルタリングは、以下のように行うことができる。上記の反復法ソルバによる手先位置の特定により得られた結果をそのまま手先位置として利用することは可能であるが、超音波センサ131~133の観測ノイズの影響を抑えるために、カルマンフィルタを利用したフィルタリングを行う。更に、本実施例では、超音波センサ131~133の距離観測用のカルマンフィルタKF1と位置状態推定用の適応カルマンフィルタKF2を含む階層化されたフィルタリングを行い、距離観測用のカルマンフィルタKF1の内部状態に従って位置状態推定用の適応カルマンフィルタKF2のモデルを適応的に変化させ、位置検出精度の向上を図る。尚、カルマンフィルタ自体、及び、適応カルマンフィルタ自体は周知であるため、その詳細な説明は省略する。距離観測用のカルマンフィルタKF1は、超音波センサ131~133による観測距離rが定速度で変化していくという想定下でプロセスモデルを作成する。3つの観測が別々のフィルタとなる。各フィルタ用のプロセスモデルは、例えば以下の通りである。距離観測用のカルマンフィルタKF1の観測入力は、超音波センサ131~133が出力する生の距離情報である。距離情報をフィルタリングした結果は、KDtreeDBの探索用距離情報riである。一方、位置状態推定用の適応カルマンフィルタKF2は、手先が定速度移動という想定下で例えば以下の如きプロセスモデルを作成する。勿論、定加速度モデル等を作成しても良い。プロセスと観測モデルのノイズは、後述するパラメータQ,Rで調整できる。位置状態推定用の適応カルマンフィルタKF2の観測入力は、上記の如き反復法ソルバによる手先位置の特定で説明した、非線形ソルバが見つけた解収束の場合、或いは、多グリッドと予測位置の平均位置である。位置状態推定用の適応カルマンフィルタKF2の出力は、位置推定処理による手先位置の最後の推定結果となる。多グリッドと位置状態推定用の適応カルマンフィルタKF2の予測位置の平均位置を計算するための重みwは、以下のように計算可能である。先ず、位置状態推定の安定度に応じて重みwを推定位置に割り当てる。位置状態推定用の適応カルマンフィルタKF2が安定している状態で、推定位置の重みwを大きくする。位置状態推定用の適応カルマンフィルタKF2の安定性の確認は、例えば次のような予測共分散行列を用いて行うことができる。上記予測共分散行列において、Pが一定の閾値δより小さい場合は、位置状態推定用の適応カルマンフィルタKF2の予測される安定性が高いと判別し、推定位置の重みwを大きくする。通常、プロセスノイズwを示す共分散Qは固定であるが、手先の予測できない動きにより、定速度移動モデルとマッチングが取れない場合がある。特に超音波測位の場合は、手先が素早い、或いは、素早く複雑な動きをする際、反射や周り風速の影響で観測ノイズが発生しやすい。このような場合、位置状態推定用の適応カルマンフィルタKF2のノイズモデルの適応調整において共分散Qを大きな値に調整することで、低速度移動モデルとのマッチングの誤差を抑えることが可能となる。このようにマッチングの誤差を抑えるために、上記の距離観測用のカルマンフィルタKF1の観測予測の共分散と残差により、距離観測の安定度を先に評価する。上記閾値γを利用し、不確定性の性能関数fを作成することができる。例えば、2つの閾値γ1γ2で上記不確定の程度を3つのレベル大、中、小で定義することが可能である。尚、一定時間内の移動平均等、閾値γの値を平均することも可能である。図7のオンライン処理の説明に戻るに、ステップS11は、超音波受信部13の超音波センサ131~133が出力する生の距離情報をウェアラブルシステム1から取得する。ステップS12は、ステップS11で取得した距離情報即ち、観測入力を距離観測用のカルマンフィルタKF1でフィルタリングして、KDtreeDB601の探索用距離情報riを取得する。ステップS13は、探索用距離情報riを用いてKDtreeDB601から最短隣グリッドGiを周知の最短隣検索アルゴリズムに基づいて特定する。ステップS14は、最短隣グリッドの中心位置を反復法ソルバの初期位置として代入し、上記の如き連立非線形方程式を解き、反復法ソルバが収束するか否かを判定する。ステップS14の判定結果がNOであると、ステップS15は、ステップS13で特定したグリッドGiと隣接する隣接グリッド上記の例では8つまでの隣接グリッドを特定し、処理はステップS16へ進む。ステップS16は、隣接グリッドの中心位置を反復法ソルバの更新した初期位置として代入し、上記の如き連立非線形方程式を解き、反復法ソルバが収束するか否かを判定する。ステップS16の判定結果がNOであると、ステップS17は、最後の解が得られた探索位置を終了位置Siとしてメモリ332に記録し、処理はステップS18へ進む。ステップS18は、未処理の隣接グリッドがあるか否かを判定し、判定結果がYESであると処理はステップS15へ戻り、判定結果がNOであると処理はステップS19へ進む。ステップS19は、解が得られた位置の平均を近似解Sとする。即ち、ステップS19は、各探索終了位置の平均位置で上記の式に従って多グリッド近似解Sを求める。一方、ステップS12の後、ステップS22は、距離観測用のカルマンフィルタKF1の観測モデルを作成し、処理はステップS21へ進む。ステップS14又はステップS16の判定結果がYES、或いは、ステップS19の後、処理はステップS21へ進む。ステップS21は、適応カルマンフィルタKF2のモデルを、カルマンフィルタKF1の推定状態のモデルのノイズレベルに応じて動的に調整する。上記の如くステップS19で求めた多グリッド近似解Sをそのまま使用することも可能であるが、この例では、位置検出精度を向上させるために、上記の式PSwZpと共に説明したように、手先位置の位置状態推定用の適応カルマンフィルタKF2の予測位置Zpと統合する。全てのグリッドでの探索が成功しない即ち、解が得られない場合は、適応カルマンフィルタKF2の予測値と、グリッド探索の全ての最終点の平均を計算し、カルマンフィルタKF1,KF2の観測及び予測の共分散情報に基づいてカルマンフィルタKF1,KF2による2つの位置推定を再度重み付けで平均化する。つまり、位置状態推定用の適応カルマンフィルタKF2により、距離観測用のカルマンフィルタKF1の観測予測の共分散と残差による計算を行い、手先が定速度移動という想定下で上記の如きプロセスモデルを作成する。ステップS21の後、次のサイクルの処理を行う場合は処理がステップS11へ戻るが、次のサイクルの処理がない場合は調整後の位置状態推定用の適応カルマンフィルタKF2の出力を最後の推定結果、即ち、最終の位置出力とする。第2実施例図9は、本発明の第2実施例における位置検出装置の一例を示すブロック図である。図9中、図1と同一部分には同一符号を付し、その説明は省略する。図9において、ウェアラブルシステム51は、位置検出装置の一例である。ウェアラブルシステム51において、超音波受信部13からの距離情報は、メモリ52に格納される。つまり、上記第1実施例では、図1に示す検知処理部33がウェアラブルシステム1とは別体であったのに対し、本実施例では、図9に示すように検知処理部53がウェアラブルシステム51の一部を形成する。図9に示すメモリ52は、半導体記憶装置、磁気記録媒体、光記録媒体、光磁気記録媒体等で形成可能である。検知処理部53は、組み込み用マイクロコンピュータマイコン531と、ストレージ532を有し、マイコン531は図1の検知処理部33と同様にCPU及びメモリを含む構成を有する。マイコン531は、メモリ52から読み出した距離情報に基づいて位置検出処理を行うことができる。尚、マイコン531内のメモリは、メモリ52又はストレージ532を代わりに用いることで省略可能である。ストレージ532は、図1の外部ストレージ32と同様の機能を有する。メモリ52及びストレージ532の一方を他方として機能させることが可能な場合には、他方を省略しても良い。又、DB作成部600は、図1のDB作成部31と同様の機能を有するが、ウェアラブルシステム51に対して接続可能な外部装置である。図1の第1実施例では、図3及び図7の処理がいずれも位置検出装置内で実行されるのに対し、図9の第2実施例では、図3の処理は位置検出装置内で実行され、図7の処理が位置検出装置に接続される外部装置即ち、DB作成部600により実行される。以上の実施例を含む実施形態に関し、更に以下の付記を開示する。付記1位置検出対象の移動部分に装着された送信機から発信された信号を前記位置検出対象の参照部分に装着された3個の受信機で受信して前記移動部分の位置を検出する位置検出装置であって、前記移動部分の3次元空間の移動範囲を固定サイズのグリッドで分割し、グリッド中心の3次元座標とグリッド中心から前記3個の受信機までの3つの距離をベクトルとしたKD木のデータベースが予め登録された記憶部と、前記3個の受信機が出力する、前記送信機と各受信機との間の距離に依存する距離情報に基づいて前記記憶部内の前記データベースを探索して反復法ソルバの初期位置を決定し、前記初期位置を用いて連立非線形方程式を解いて前記移動部分の位置を検出するプロセッサを備え、前記プロセッサは、前記距離情報をフィルタリングして前記データベースの探索用距離情報を取得する第1のカルマンフィルタと、前記探索用距離情報を用いて前記データベースから最短隣グリッドを最短隣検索アルゴリズムに基づいて特定し、最短隣グリッドの中心位置を前記反復法ソルバの初期位置として代入して前記連立非線形方程式を解き、前記反復法ソルバが収束するか否かを判定する判定手段と、前記反復法ソルバが収束する場合は、前記第1のカルマンフィルタの推定状態のモデルのノイズレベルに応じて動的に調整されて前記移動部分の位置状態を推定する位置状態推定用の第2のカルマンフィルタを有することを特徴とする、位置検出装置。付記2前記プロセッサは、前記反復法ソルバが収束しない場合は、前記最短隣グリッドに隣接する隣接グリッドを特定して各隣接グリッドの中心位置を前記反復法ソルバの初期位置として代入して前記連立非線形方程式を解いて再度解を探す探索手段を更に有することを特徴とする、付記1記載の位置検出装置。付記3前記プロセッサは、前記探索手段が解を探すことができないと、解が得られた位置の平均から近似解を求めて前記第2のカルマンフィルタに出力する手段を更に有することを特徴とする、付記2記載の位置検出装置。付記4全てのグリッドで解が得られない場合は、前記第2のカルマンフィルタの予測値とグリッド探索の全ての最終点の平均を計算し、前記第1及び第2のカルマンフィルタの観測及び予測の共分散情報に基づいて前記第1及び第2のカルマンフィルタによる2つの位置推定を再度重み付けで平均化することを特徴とする、付記1乃至3のいずれか1項記載の位置検出装置。付記5前記送信機及び前記3個の受信機は、前記位置検出対象に装着されるウェアラブルシステムを形成し、前記位置検出装置は、前記ウェアラブルシステムとは別体であることを特徴とする、付記1乃至4のいずれか1項記載の位置検出装置。付記6前記送信機、前記3個の受信機、及び前記位置検出装置は、前記位置検出対象に装着されるウェアラブルシステムを形成することを特徴とする、付記1乃至4のいずれか1項記載の位置検出装置。付記7位置検出対象の移動部分に装着される送信機と、前記位置検出対象の参照部分に装着され、前記送信機から発信された信号を受信する3個の受信機と、前記移動部分の3次元空間の移動範囲を固定サイズのグリッドで分割し、グリッド中心の3次元座標とグリッド中心から前記3個の受信機までの3つの距離をベクトルとしたKD木のデータベースが予め登録された記憶部と、前記3個の受信機が出力する、前記送信機と各受信機との間の距離に依存する距離情報に基づいて前記記憶部内の前記データベースを探索して反復法ソルバの初期位置を決定し、前記初期位置を用いて連立非線形方程式を解いて前記移動部分の位置を検出する検知処理部を備え、前記検知処理部は、前記距離情報をフィルタリングして前記データベースの探索用距離情報を取得する第1のカルマンフィルタと、前記探索用距離情報を用いて前記データベースから最短隣グリッドを最短隣検索アルゴリズムに基づいて特定し、最短隣グリッドの中心位置を前記反復法ソルバの初期位置として代入して前記連立非線形方程式を解き、前記反復法ソルバが収束するか否かを判定する判定手段と、前記反復法ソルバが収束する場合は、前記第1のカルマンフィルタの推定状態のモデルのノイズレベルに応じて動的に調整されて前記移動部分の位置状態を推定する位置状態推定用の第2のカルマンフィルタを有することを特徴とする、ウェアラブルセンサシステム。付記8前記検知処理部は、前記反復法ソルバが収束しない場合は、前記最短隣グリッドに隣接する隣接グリッドを特定して各隣接グリッドの中心位置を前記反復法ソルバの初期位置として代入して前記連立非線形方程式を解いて再度解を探す探索手段を更に有することを特徴とする、付記7記載のウェアラブルセンサシステム。付記9前記検知処理部は、前記探索手段が解を探すことができないと、解が得られた位置の平均から近似解を求めて前記第2のカルマンフィルタに出力する手段を更に有することを特徴とする、付記8記載のウェアラブルセンサシステム。付記10全てのグリッドで解が得られない場合は、前記第2のカルマンフィルタの予測値とグリッド探索の全ての最終点の平均を計算し、前記第1及び第2のカルマンフィルタの観測及び予測の共分散情報に基づいて前記第1及び第2のカルマンフィルタによる2つの位置推定を再度重み付けで平均化することを特徴とする、付記7乃至9のいずれか1項記載のウェアラブルセンサシステム。付記11前記送信機は、前記位置検出対象の手首に装着される第1の装着部に設けられており、前記3個の受信機は、前記位置検出対象の胴体に装着される第2の装着部に設けられていることを特徴とする、付記7乃至10のいずれか1項記載のウェアラブルセンサシステム。付記12コンピュータに、位置検出対象の移動部分の位置を検出する位置検出処理を実行させるプログラムであって、前記移動部分の3次元空間の移動範囲を固定サイズのグリッドで分割し、グリッド中心の3次元座標とグリッド中心から、前記位置検出対象の参照部分に装着された3個の受信機までの3つの距離をベクトルとしたKD木のデータベースが予め登録された記憶部を用い、前記移動部分に装着された送信機から発信された信号を受信する前記3個の受信機が出力する、前記送信機と各受信機との間の距離に依存する距離情報に基づいて前記記憶部内の前記データベースを探索して反復法ソルバの初期位置を決定し、前記初期位置を用いて連立非線形方程式を解いて前記移動部分の位置を検出する検出手順を前記コンピュータに実行させ、前記検出手順は、前記距離情報を第1のカルマンフィルタによりフィルタリングして前記データベースの探索用距離情報を取得する第1のフィルタリング手順と、前記探索用距離情報を用いて前記データベースから最短隣グリッドを最短隣検索アルゴリズムに基づいて特定し、最短隣グリッドの中心位置を前記反復法ソルバの初期位置として代入して前記連立非線形方程式を解き、前記反復法ソルバが収束するか否かを判定する判定手順と、前記反復法ソルバが収束する場合は、前記第1のカルマンフィルタの推定状態のモデルのノイズレベルに応じて動的に調整されて前記移動部分の位置状態を推定する位置状態推定用の第2のカルマンフィルタによるフィルタリングを行う第2のフィルタリング手順を含むことを特徴とする、プログラム。付記13前記検出手順は、前記反復法ソルバが収束しない場合は、前記最短隣グリッドに隣接する隣接グリッドを特定して各隣接グリッドの中心位置を前記反復法ソルバの初期位置として代入して前記連立非線形方程式を解いて再度解を探す探索手順を更に含むことを特徴とする、付記12記載のプログラム。付記14前記検出手順は、前記探索手段が解を探すことができないと、解が得られた位置の平均から近似解を求めて前記第2のカルマンフィルタに出力する手順を更に含むことを特徴とする、付記13記載のプログラム。付記15全てのグリッドで解が得られない場合は、前記第2のカルマンフィルタの予測値とグリッド探索の全ての最終点の平均を計算し、前記第1及び第2のカルマンフィルタの観測及び予測の共分散情報に基づいて前記第1及び第2のカルマンフィルタによる2つの位置推定を再度重み付けで平均化することを特徴とする、付記12乃至14のいずれか1項記載のプログラム。以上、開示の位置検出装置、ウェアラブルセンサシステム及びプログラムを実施例により説明したが、本発明は上記実施例に限定されるものではなく、本発明の範囲内で種々の変形及び改良が可能であることは言うまでもない。"