(57)【要約】【課題】全送信機の中にマルチパスの影響を受けたものがあったり、送信機から受信機までの伝播経路上に問題がある場合においても、精度の高い測位を行うことが可能な位置推定装置を得る。【解決手段】位置が既知である複数の送信機１から送信された電波を、位置が未知である受信機２で受信して、受信機位置を推定する位置推定装置であって、受信可能な全送信機１の中から、所定の最低数以上の送信機の組み合わせを生成する送信機組み合わせ変更器３と、当該組み合わせ毎に、送信機１から受信機２までの伝播時間を計測して得る送受信機間の距離を基に測位解を算出する測位解算出器６と、過去に計測された受信機の位置から現在の位置を予測する予測器８と、当該予測位置と当該測位解との予測残差に基づいて、送信機組み合わせ変更手段３による送信機１の組み合わせを選択する測位解選択器９とを備えている。【選択図】図１
2005024535.tif 000002
【特許請求の範囲】
【請求項１】
位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に受信機位置を推定する位置推定装置であって、
受信可能な全送信機の中から、受信機位置の測定に必要な所定の最低数以上の送信機の組み合わせを生成する送信機組み合わせ変更手段と、
上記送信機組み合わせ変更手段による上記送信機の組み合わせ毎に、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、上記受信機位置の測位解を算出する測位解算出手段と、
過去に計測された受信機の位置から現在の受信機の位置を予測する予測手段と、
上記予測手段で算出された予測位置と上記測位解算出手段で算出された上記測位解との予測残差に基づいて、上記送信機組み合わせ変更手段による上記送信機の組み合わせを選択する選択手段と
を備えたことを特徴とする位置推定装置。
【請求項２】
位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に受信機位置を推定する位置推定装置であって、
受信可能な全送信機の中から、受信機位置の測定に必要な所定の最低数以上の送信機の組み合わせを生成する送信機組み合わせ変更手段と、
上記送信機組み合わせ変更手段による上記送信機の組み合わせ毎に、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、上記受信機位置の測位解を算出する測位解算出手段と、
上記送信機組み合わせ変更手段による上記送信機の組み合わせ毎に、上記測位解算出手段で算出された上記測位解の予測確率分布を算出する予測確率分布算出手段と、
上記予測確率分布における上記測位解の確率密度を基に、上記送信機組み合わせ変更手段による上記送信機の組み合わせを選択する選択手段と
を備えたことを特徴とする位置推定装置。
【請求項３】
位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に受信機位置を推定する位置推定装置であって、
受信可能な全送信機の中から、受信機位置の測定に必要な所定の最低数以上の送信機の組み合わせを生成する送信機組み合わせ変更手段と、
上記送信機組み合わせ変更手段による上記送信機の組み合わせ毎に、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、上記受信機位置の測位解を算出する測位解算出手段と、
上記送信機組み合わせ変更手段による上記送信機の組み合わせ毎に、上記測位解算出手段による上記測位解の予測確率分布を算出する予測確率分布算出手段と、
上記予測確率分布における上記測位解の確率密度に基いて、上記測位解の信頼度を算出する信頼度算出手段と、
上記信頼度により上記測位解を重み付けする重み付け手段と
を備えたことを特徴とする位置推定装置。
【請求項４】
位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に受信機位置を推定する位置推定装置であって、
受信可能な全送信機の中から、受信機位置の測定に必要な所定の最低数以上の送信機の組み合わせを生成する送信機組み合わせ変更手段と、
上記送信機から上記受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に、最小二乗法を用いて上記受信機位置の測位解を算出する最小二乗測位解算出手段と、
上記送信機組み合わせ変更手段による上記送信機の組み合わせ毎に、送信機から受信機までの伝播時間から計算される伝播距離と上記測位解との距離残差の確率分布を算出する確率分布算出手段と、
上記確率分布における上記距離残差の確率密度を基に、上記送信機組み合わせ変更手段による上記送信機の組み合わせを選択する選択手段と
を備えたことを特徴とする位置推定装置。
【請求項５】
位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に受信機位置を推定する位置推定装置であって、
受信可能な全送信機の中から、受信機位置の測定に必要な所定の最低数以上の送信機の組み合わせを生成する送信機組み合わせ変更手段と、
上記送信機から上記受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に、最小二乗法を用いて上記受信機位置の測位解を算出する最小二乗測位解算出手段と、
上記送信機組み合わせ変更手段による上記送信機の組み合わせ毎に、送信機から受信機までの伝播時間から計算される伝播距離と上記測位解との距離残差の確率分布を算出する確率分布算出手段と、
上記確率分布における上記距離残差の確率密度を基に、上記測位解の信頼度を算出する信頼度算出手段と、
上記信頼度により測位解を重み付けする重み付け手段と
を備えたことを特徴とする位置推定装置。
【請求項６】
位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に送信機位置を推定する位置推定装置であって、
受信可能な全受信機の中から、送信機位置の測定に必要な所定の最低数以上の受信機の組み合わせを生成する受信機組み合わせ変更手段と、
上記受信機組み合わせ変更手段による上記受信機の組み合わせ毎に、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、上記送信機位置の測位解を算出する測位解算出手段と、
過去に計測された送信機の位置から現在の位置を予測する予測手段と、
上記予測手段で算出された予測位置と上記測位解算出手段で算出された上記測位解との予測残差に基づいて、上記受信機組み合わせ変更手段による上記受信機の組み合わせを選択する選択手段と
を備えたことを特徴とする位置推定装置。
【請求項７】
位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に送信機位置を推定する位置推定装置であって、
受信可能な全受信機の中から、送信機位置の測定に必要な所定の最低数以上の受信機の組み合わせを変更する受信機組み合わせ変更手段と、
上記受信機組み合わせ変更手段による上記受信機の組み合わせ毎に、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、上記送信機位置の測位解を算出する測位解算出手段と、
上記受信機組み合わせ変更手段による上記受信機の組み合わせ毎に、上記測位解算出手段で算出された上記測位解の予測確率分布を算出する予測確率分布算出手段と、
上記予測確率分布における上記測位解の確率密度を基に、上記受信機組み合わせ変更手段による上記受信機の組み合わせを選択する選択手段と
を備えたことを特徴とする位置推定装置。
【請求項８】
位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に送信機位置を推定する位置推定装置であって、
受信可能な全受信機の中から、送信機位置の測定に必要な所定の最低数以上の受信機の組み合わせを生成する受信機組み合わせ変更手段と、
上記受信機組み合わせ変更手段による上記受信機の組み合わせ毎に、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、上記送信機位置の測位解を算出する測位解算出手段と、
上記受信機組み合わせ変更手段による上記受信機の組み合わせ毎に、上記測位解算出手段による上記測位解の予測確率分布を算出する予測確率分布算出手段と、
上記予測確率分布における上記測位解の確率密度に基いて、上記測位解の信頼度を算出する信頼度算出手段と、
上記信頼度により上記測位解を重み付けする重み付け手段と
を備えたことを特徴とする位置推定装置。
【請求項９】
位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に送信機位置を推定する位置推定装置であって、
受信可能な全受信機の中から、送信機位置の測定に必要な所定の最低数以上の受信機の組み合わせを生成する受信機組み合わせ変更手段と、
上記送信機から上記受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に、最小二乗法を用いて上記送信機位置の測位解を算出する最小二乗測位解算出手段と、
上記受信機組み合わせ変更手段による上記受信機の組み合わせ毎に、送信機から受信機までの伝播時間から計算される伝播距離と上記測位解との距離残差の確率分布を算出する確率分布算出手段と、
上記確率分布における上記距離残差の確率密度を基に、上記受信機組み合わせ変更手段による上記受信機の組み合わせを選択する選択手段と
を備えたことを特徴とする位置推定装置。
【請求項１０】
位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に送信機位置を推定する位置推定装置であって、
受信可能な全受信機の中から、送信機位置の測定に必要な所定の最低数以上の受信機の組み合わせを生成する受信機組み合わせ変更手段と、
上記送信機から上記受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に、最小二乗法を用いて上記送信機位置の測位解を算出する最小二乗測位解算出手段と、
上記受信機組み合わせ変更手段による上記受信機の組み合わせ毎に、送信機から受信機までの伝播時間から計算される伝播距離と上記測位解との距離残差の確率分布を算出する確率分布算出手段と、
上記確率分布における上記距離残差の確率密度を基に、上記測位解の信頼度を算出する信頼度算出手段と、
上記信頼度により上記測位解を重み付けする重み付け手段と
を備えたことを特徴とする位置推定装置。
【請求項１１】
位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に受信機位置を推定する位置推定装置であって、
受信可能な全送信機の中から、受信機位置の測定に必要な所定の最低数以上の送信機の組み合わせを生成する送信機組み合わせ変更手段と、
上記送信機組み合わせ変更手段による上記送信機の組み合わせ毎に、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に上記受信機位置の測位解を算出する測位解算出手段と、
過去に計測された受信機の位置から現在の位置を予測する予測手段と、
上記予測手段で算出された予測位置と上記測位解算出手段で算出された上記測位解との予測残差に基づいて、上記送信機組み合わせ変更手段による上記送信機の組み合わせを選択する選択手段と
を備えたことを特徴とする位置推定装置。
【請求項１２】
位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に受信機位置を推定する位置推定装置であって、
受信可能な全送信機の中から、受信機位置の測定に必要な所定の最低数以上の送信機の組み合わせを生成する送信機組み合わせ変更手段と、
上記送信機組み合わせ変更手段による上記送信機の組み合わせ毎に、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に上記受信機位置の測位解を算出する測位解算出手段と、
上記送信機組み合わせ変更手段による上記送信機の組み合わせ毎に、上記測位解算出手段で算出された上記測位解の予測確率分布を算出する予測確率分布算出手段と、
上記予測確率分布における上記測位解の確率密度を基に、上記送信機組み合わせ変更手段による上記送信機の組み合わせを選択する選択手段と
を備えたことを特徴とする位置推定装置。
【請求項１３】
位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に受信機位置を推定する位置推定装置であって、
受信可能な全送信機の中から、受信機位置の測定に必要な所定の最低数以上の送信機の組み合わせを生成する送信機組み合わせ変更手段と、
上記送信機組み合わせ変更手段による上記送信機の組み合わせ毎に、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に上記受信機位置の測位解を算出する測位解算出手段と、
上記送信機組み合わせ変更手段による上記送信機の組み合わせ毎に、上記測位解算出手段による上記測位解の予測確率分布を算出する予測確率分布算出手段と、
上記予測確率分布における上記測位解の確率密度に基いて、上記測位解の信頼度を算出する信頼度算出手段と、
上記信頼度により上記測位解を重み付けする重み付け手段と
を備えたことを特徴とする位置推定装置。
【請求項１４】
位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に受信機位置を推定する位置推定装置であって、
受信可能な全送信機の中から、受信機位置の測定に必要な所定の最低数以上の送信機の組み合わせを生成する送信機組み合わせ変更手段と、
受信機位置での各送信機からの信号間の伝播時間差または位相差に基づいて、最小二乗法を用いて上記受信機位置の測位解を算出する最小二乗測位解算出手段と、
上記送信機組み合わせ変更手段による上記送信機の組み合わせ毎に、上記伝播時間差または上記位相差から計算される伝播距離差と上記測位解との距離残差の確率分布を算出する確率分布算出手段と、
上記確率分布における上記距離残差の確率密度を基に、上記送信機組み合わせ変更手段による上記送信機の組み合わせを選択する選択手段と
を備えたことを特徴とする位置推定装置。
【請求項１５】
位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に受信機位置を推定する位置推定装置であって、
受信可能な全送信機の中から、受信機位置の測定に必要な所定の最低数以上の送信機の組み合わせを生成する送信機組み合わせ変更手段と、
受信機位置での各送信機からの信号間の伝播時間差または位相差に基づいて、最小二乗法を用いて上記受信機位置の測位解を算出する最小二乗測位解算出手段と、
上記送信機組み合わせ変更手段による上記送信機の組み合わせ毎に、上記伝播時間差または上記位相差から計算される伝播距離差と上記測位解との距離残差の確率分布を算出する確率分布算出手段と、
上記確率分布における上記距離残差の確率密度を基に、上記測位解の信頼度を算出する信頼度算出手段と、
上記信頼度により測位解を重み付けする重み付け手段と
を備えたことを特徴とする位置推定装置。
【請求項１６】
位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、各受信機における受信信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に送信機位置を推定する位置推定装置であって、
受信可能な全受信機の中から、送信機位置の測定に必要な所定の最低数以上の受信機の組み合わせを生成する受信機組み合わせ変更手段と、
上記受信機組み合わせ変更手段による上記受信機の組み合わせ毎に、各受信機における受信信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に上記送信機位置の測位解を算出する測位解算出手段と、
過去に計測された送信機の位置から現在の位置を予測する予測手段と、
上記予測手段で算出された予測位置と上記測位解算出手段で算出された上記測位解との予測残差に基づいて、上記受信機組み合わせ変更手段による上記受信機の組み合わせを選択する選択手段と
を備えたことを特徴とする位置推定装置。
【請求項１７】
位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、各受信機における受信信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に送信機位置を推定する位置推定装置であって、
受信可能な全受信機の中から、送信機位置の測定に必要な所定の最低数以上の受信機の組み合わせを変更する受信機組み合わせ変更手段と、
上記受信機組み合わせ変更手段による上記受信機の組み合わせ毎に、各受信機における受信信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に上記送信機位置の測位解を算出する測位解算出手段と、
上記受信機組み合わせ変更手段による上記受信機の組み合わせ毎に、上記測位解算出手段で算出された上記測位解の予測確率分布を算出する予測確率分布算出手段と、
上記予測確率分布における上記測位解の確率密度を基に、上記受信機組み合わせ変更手段による上記受信機の組み合わせを選択する選択手段と
を備えたことを特徴とする位置推定装置。
【請求項１８】
位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、各受信機における受信信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に送信機位置を推定する位置推定装置であって、
受信可能な全受信機の中から、送信機位置の測定に必要な所定の最低数以上の受信機の組み合わせを生成する受信機組み合わせ変更手段と、
上記受信機組み合わせ変更手段による上記受信機の組み合わせ毎に、各受信機における受信信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に上記送信機位置の測位解を算出する測位解算出手段と、
上記受信機組み合わせ変更手段による上記受信機の組み合わせ毎に、上記測位解算出手段による上記測位解の予測確率分布を算出する予測確率分布算出手段と、
上記予測確率分布における上記測位解の確率密度を基いて、上記測位解の信頼度を算出する信頼度算出手段と、
上記信頼度により上記測位解を重み付けする重み付け手段と
を備えたことを特徴とする位置推定装置。
【請求項１９】
位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、各受信機における受信信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に送信機位置を推定する位置推定装置であって、
受信可能な全受信機の中から、送信機位置の測定に必要な所定の最低数以上の受信機の組み合わせを生成する受信機組み合わせ変更手段と、
各受信機における受信信号間の伝播時間差または位相差に基づいて、最小二乗法を用いて上記送信機位置の測位解を算出する最小二乗測位解算出手段と、
上記受信機組み合わせ変更手段による上記受信機の組み合わせ毎に、各受信機における受信信号間の伝播時間差または位相差を計測して、上記伝播時間差または上記位相差から計算される伝播距離差と上記測位解との距離残差の確率分布を算出する確率分布算出手段と、
上記確率分布における上記距離残差の確率密度を基に、上記受信機組み合わせ変更手段による上記送信機の組み合わせを選択する選択手段と
を備えたことを特徴とする位置推定装置。
【請求項２０】
位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、各受信機における受信信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に送信機位置を推定する位置推定装置であって、
受信可能な全受信機の中から、送信機位置の測定に必要な所定の最低数以上の受信機の組み合わせを生成する受信機組み合わせ変更手段と、
上記受信機における受信信号間の伝播時間差または位相差に基づいて、最小二乗法を用いて上記送信機位置の測位解を算出する最小二乗測位解算出手段と、
上記受信機組み合わせ変更手段による上記受信機の組み合わせ毎に、各受信機における受信信号間の伝播時間差または位相差を計測して、上記伝播時間差または上記位相差から計算される伝播距離差と上記測位解との距離残差の確率分布を算出する確率分布算出手段と、
上記確率分布における上記距離残差の確率密度を基に、上記測位解の信頼度を算出する信頼度算出手段と、
上記信頼度により上記測位解を重み付けする重み付け手段と
を備えたことを特徴とする位置推定装置。
【請求項２１】
位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に受信機位置を推定する位置推定装置であって、
過去に計測された受信機の位置から現在の各送信機と受信機の間の観測距離を予測する予測手段と、
上記予測手段で算出された予測距離と観測された送受信機間の距離との予測残差に基づいて、送信機を選択する選択手段と、
上記選択手段で選択された送信機を用いて、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、上記受信機位置の測位解を算出する測位解算出手段と
を備えたことを特徴とする位置推定装置。
【請求項２２】
位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に受信機位置を推定する位置推定装置であって、
過去に計測された受信機の位置から現在の各送信機と受信機の間の距離を予測する予測手段と、
上記予測手段で算出された予測距離と観測された送受信機間の観測距離との予測残差の確率分布を算出する予測確率分布算出手段と、
上記予測確率分布における予測残差の確率密度に基づいて、送信機を選択する選択手段と、
上記選択手段で選択された送信機を用いて、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、上記受信機位置の測位解を算出する測位解算出手段と
を備えたことを特徴とする位置推定装置。
【請求項２３】
位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に受信機位置を推定する位置推定装置であって、
過去に計測された受信機の位置から現在の各送信機と受信機の間の距離を予測する予測手段と、
上記予測手段で算出された予測距離と観測された送受信機間の観測距離との予測残差に基づいて、送信機を選択する選択手段と、
上記選択手段で選択された送信機の数に応じて、第１および第２の平滑手段のいずれか一方に切り替えるフィルタ切り替え手段と、
上記選択手段で選択された送信機を用いて、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、上記受信機位置の測位解を算出する測位解算出手段と、
上記選択手段で選択された送信機を用いて、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、受信機の位置を推定する距離による位置推定手段と
を備え、
上記第１の平滑手段は、上記測位解と上記予測距離とに基づいて、平滑距離を算出して、上記予測手段に出力し、
上記第２の平滑手段は、疑似距離と上記予測距離と上記観測距離とに基づいて、１サンプリング前の平滑距離を算出して、上記予測手段に出力する
ことを特徴とする位置推定装置。
【請求項２４】
位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に受信機位置を推定する位置推定装置であって、
過去に計測された受信機の位置から現在の各送信機と受信機の間の距離を予測する予測手段と、
上記予測手段で算出された予測距離と観測された送受信機間の観測距離との予測残差の確率分布を算出する予測確率分布算出手段と、
上記予測確率分布における予測残差の確率密度に基づいて、送信機を選択する選択手段と、
上記選択手段で選択された送信機の数に応じて、第１および第２の平滑手段のいずれか一方に切り替えるフィルタ切り替え手段と、
上記選択手段で選択された送信機を用いて、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、上記受信機位置の測位解を算出する測位解算出手段と、
上記選択手段で選択された送信機を用いて、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、受信機の位置を推定する距離による位置推定手段と
を備え、
上記第１の平滑手段は、上記測位解と上記予測距離とに基づいて、平滑距離を算出して、上記予測手段に出力し、
上記第２の平滑手段は、疑似距離と上記予測距離と上記観測距離とに基づいて、１サンプリング前の平滑距離を算出して、上記予測手段に出力する
ことを特徴とする位置推定装置。
【請求項２５】
位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に送信機位置を推定する位置推定装置であって、
過去に計測された送信機の位置から現在の各受信機と送信機の間の距離を予測する予測手段と、
上記予測手段で算出された予測距離と観測された送受信機間の観測距離との予測残差に基づいて、受信機を選択する選択手段と、
上記選択手段で選択された受信機を用いて、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、上記送信機位置の測位解を算出する測位解算出手段と
を備えたことを特徴とする位置推定装置。
【請求項２６】
位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に送信機位置を推定する位置推定装置であって、
過去に計測された送信機の位置から現在の各送信機と受信機の間の距離を予測する予測手段と、
上記予測手段で算出された予測距離と観測された送受信機間の観測距離との予測残差の確率分布を算出する予測確率分布算出手段と、
上記予測確率分布における予測残差の確率密度に基づいて、受信機を選択する選択手段と、
上記選択手段で選択された受信機を用いて、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、上記送信機位置の測位解を算出する測位解算出手段と
を備えたことを特徴とする位置推定装置。
【請求項２７】
位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に送信機位置を推定する位置推定装置であって、
過去に計測された送信機の位置から現在の各受信機と送信機の間の距離を予測する予測手段と、
上記予測手段で算出された予測距離と観測された送受信機間の観測距離との予測残差に基づいて、受信機を選択する選択手段と、
上記選択手段で選択された受信機の数に応じて、第１および第２の平滑手段のいずれか一方に切り替えるフィルタ切り替え手段と、
上記選択手段で選択された受信機を用いて、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、上記送信機位置の測位解を算出する測位解算出手段と、
上記選択手段で選択された受信機を用いて、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、送信機の位置を推定する距離による位置推定手段と、
を備え、
上記第１の平滑手段は、上記測位解と上記予測距離とに基づいて、平滑距離を算出して、上記予測手段に出力し、
上記第２の平滑手段は、疑似距離と上記予測距離と上記観測距離とに基づいて、１サンプリング前の平滑距離を算出して、上記予測手段に出力する
ことを特徴とする位置推定装置。
【請求項２８】
位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に送信機位置を推定する位置推定装置であって、
過去に計測された送信機の位置から現在の各送信機と受信機の間の距離を予測する予測手段と、
上記予測手段で算出された予測距離と観測された送受信機間の観測距離との予測残差の確率分布を算出する予測確率分布算出手段と、
上記予測確率分布における予測残差の確率密度に基づいて、受信機を選択する選択手段と、
上記選択手段で選択された受信機の数に応じて、第１および第２の平滑手段のいずれか一方に切り替えるフィルタ切り替え手段と、
上記選択手段で選択された受信機を用いて、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、上記送信機位置の測位解を算出する測位解算出手段と、
上記選択手段で選択された受信機を用いて、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、送信機の位置を推定する距離による位置推定手段と
を備え、
上記第１の平滑手段は、上記測位解と上記予測距離とに基づいて、平滑距離を算出して、上記予測手段に出力し、
上記第２の平滑手段は、疑似距離と上記予測距離と上記観測距離とに基づいて、１サンプリング前の平滑距離を算出して、上記予測手段に出力する
ことを特徴とする位置推定装置。
【請求項２９】
位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に受信機位置を推定する位置推定装置であって、
過去に計測された受信機の位置から現在の受信機位置での各送信機からの信号間の伝搬時間差または位相差を予測する予測手段と、
上記予測手段で算出された予測時間差または予測位相差と観測された時間差または位相差との予測残差に基づいて、送信機を選択する選択手段と、
上記選択手段で選択された送信機を用いて、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に上記受信機位置の測位解を算出する測位解算出手段と
を備えたことを特徴とする位置推定装置。
【請求項３０】
位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に受信機位置を推定する位置推定装置であって、
過去に計測された受信機の位置から現在の受信機位置での各送信機からの信号間の伝搬時間差または位相差を予測する予測手段と、
上記予測手段で算出された予測時間差または予測位相差と観測された時間差または位相差との予測残差の確率分布を算出する予測確率分布算出手段と、
上記予測確率分布における予測残差の確率密度に基づいて、送信機を選択する選択手段と、
上記選択手段で選択された送信機を用いて、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に上記受信機位置の測位解を算出する測位解算出手段と
を備えたことを特徴とする位置推定装置。
【請求項３１】
位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に受信機位置を推定する位置推定装置であって、
過去に計測された受信機の位置から現在の受信機位置での各送信機からの信号間の伝搬時間差または位相差を予測する予測手段と、
上記予測手段で算出された予測時間差または予測位相差と観測された時間差または位相差との予測残差に基づいて、送信機を選択する選択手段と、
上記選択手段で選択された送信機の数に応じて、第１および第３の平滑手段のいずれか一方に切り替えるフィルタ切り替え手段と、
上記選択手段で選択された送信機を用いて、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に上記受信機位置の測位解を算出する測位解算出手段と、
上記選択手段で選択された送信機を用いて、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に、受信機の位置を推定する時間差または位相差による位置推定手段と、
を備え、
上記第１の平滑手段は、上記測位解と上記予測距離とに基づいて、平滑距離を算出して、上記予測手段に出力し、
上記第３の平滑手段は、疑似距離差と上記予測距離と上記観測距離の距離差とに基づいて、１サンプリング前の平滑距離を算出して、上記予測手段に出力する
ことを特徴とする位置推定装置。
【請求項３２】
位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に受信機位置を推定する位置推定装置であって、
過去に計測された受信機の位置から現在の受信機位置での各送信機からの信号間の伝搬時間差または位相差を予測する予測手段と、
上記予測手段で算出された予測時間差または予測位相差と観測された時間差または位相差との予測残差の確率分布を算出する予測確率分布算出手段と、
上記予測確率分布における予測残差の確率密度に基づいて、送信機を選択する選択手段と、
上記選択手段で選択された送信機の数に応じて、第１および第２の平滑手段のいずれか一方に切り替えるフィルタ切り替え手段と、
上記選択手段で選択された送信機を用いて、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に、上記受信機位置の測位解を算出する測位解算出手段と、
上記選択手段で選択された送信機を用いて、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に、受信機の位置を推定する時間差または位相差による位置推定手段と、
を備え、
上記第１の平滑手段は、上記測位解と上記予測距離とに基づいて、平滑距離を算出して、上記予測手段に出力し、
上記第３の平滑手段は、疑似距離差と上記予測距離と上記観測距離の距離差とに基づいて、１サンプリング前の平滑距離を算出して、上記予測手段に出力する
ことを特徴とする位置推定装置。
【請求項３３】
位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、各受信機における受信信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に送信機位置を推定する位置推定装置であって、
過去に計測された送信機の位置から現在の受信機位置での各送信機からの信号間の伝搬時間差または位相差を予測する予測手段と、
上記予測手段で算出された予測時間差または予測位相差と観測された時間差または位相差との予測残差に基づいて、受信機を選択する選択手段と、
上記選択手段で選択された受信機を用いて、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に上記送信機位置の測位解を算出する測位解算出手段と、
を備えたことを特徴とする位置推定装置。
【請求項３４】
位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、各受信機における受信信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に送信機位置を推定する位置推定装置であって、
過去に計測された送信機の位置から現在の受信機位置での各送信機からの信号間の伝搬時間差または位相差を予測する予測手段と、
上記予測手段で算出された予測時間差または予測位相差と観測された時間差または位相差との予測残差の確率分布を算出する予測確率分布算出手段と、
上記予測確率分布における予測残差の確率密度に基づいて、受信機を選択する選択手段と、
上記選択手段で選択された受信機を用いて、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に上記送信機位置の測位解を算出する測位解算出手段と、
を備えたことを特徴とする位置推定装置。
【請求項３５】
位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、各受信機における受信信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に送信機位置を推定する位置推定装置であって、
過去に計測された送信機の位置から現在の受信機位置での各送信機からの信号間の伝搬時間差または位相差を予測する予測手段と、
上記予測手段で算出された予測時間差または予測位相差と観測された時間差または位相差との予測残差に基づいて、受信機を選択する選択手段と、
上記選択手段で選択された受信機の数に応じて、第１および第３の平滑手段のいずれか一方に切り替えるフィルタ切り替え手段と、
上記選択手段で選択された受信機を用いて、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に上記送信機位置の測位解を算出する測位解算出手段と、
上記選択手段で選択された受信機を用いて、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に、送信機の位置を推定する時間差または位相差による位置推定手段と
を備え、
上記第１の平滑手段は、上記測位解と上記予測距離とに基づいて、平滑距離を算出して、上記予測手段に出力し、
上記第３の平滑手段は、疑似距離差と上記予測距離と上記観測距離の距離差とに基づいて、１サンプリング前の平滑距離を算出して、上記予測手段に出力する
ことを特徴とする位置推定装置。
【請求項３６】
位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、各受信機における受信信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に送信機位置を推定する位置推定装置であって、
過去に計測された送信機の位置から現在の受信機位置での各送信機からの信号間の伝搬時間差または位相差を予測する予測手段と、
上記予測手段で算出された予測時間差または予測位相差と観測された時間差または位相差との予測残差の確率分布を算出する予測確率分布算出手段と、
上記予測確率分布における予測残差の確率密度に基づいて、受信機を選択する選択手段と、
上記選択手段で選択された受信機の数に応じて、第１および第３の平滑手段のいずれか一方に切り替えるフィルタ切り替え手段と、
上記選択手段で選択された受信機を用いて、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に上記送信機位置の測位解を算出する測位解算出手段と、
上記選択手段で選択された受信機を用いて、受信機位置での各送信機からの信号間の伝播時間差または位相差を計測して、当該伝播時間差または当該位相差を基に、送信機の位置を推定する時間差または位相差による位置推定手段と、
を備え、
上記第１の平滑手段は、上記測位解と上記予測距離とに基づいて、平滑距離を算出して、上記予測手段に出力し、
上記第３の平滑手段は、疑似距離差と上記予測距離と上記観測距離の距離差とに基づいて、１サンプリング前の平滑距離を算出して、上記予測手段に出力する
ことを特徴とする位置推定装置。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は位置推定装置に関し、特に、ＧＰＳ測位システムなどに使用され、電波の伝播時間または時間差や位相差を利用して受信機位置または送信機位置の位置を推定するための位置推定装置に関するものである。
【背景技術】
【０００２】
位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、送信機から受信機までの伝播時間を計測して受信機位置を測位するという従来の方式が提案されている（例えば、非特許文献１参照。）。
【０００３】
上記非特許文献１に示されるように、この種の従来の方式においては、次の式（１）〜式（１０）のような測位方程式を解く方式が一般的である。式（１）は測位方程式である。
【０００４】
【数１】
2005024535.tif 000003
【０００５】
ここで、δｒは擬似距離誤差ベクトル、Ａは方向余弦ベクトル、δｕは受信機位置誤差ベクトルを表す。
擬似距離誤差ベクトルは式（２）のとおりである。
【０００６】
【数２】
2005024535.tif 000004
【０００７】
式（２）のｒｉ（ｉ＝１，・・・，Ｎの自然数であって、Ｎ≧４；３次元測位の場合）は、ｉ番目の送信機位置から受信機位置までの擬似距離であり、伝播時間と光速の積から求める。
式（２）のｆｉ（ｘ０，ｙ０，ｚ０，ｔ０）（ｉ＝１，・・・，Ｎの自然数であって、Ｎ≧４；３次元測位の場合）は概算距離であり、反復演算で得られる前回の受信機位置の測位値（ｘ０，ｙ０，ｚ０）と受信機時計誤差ｔ０、既知であるｉ番目の送信機位置（ｘｉ，ｙｉ，ｚｉ）から式（３）を用いて算出される。ｃは光速である。
【０００８】
【数３】
2005024535.tif 000005
【０００９】
方向余弦ベクトルは式（４）のとおりである。
【００１０】
【数４】
2005024535.tif 000006
【００１１】
方向余弦ベクトルの各要素Ａｉ，ｘ，Ａｉ，ｙ，Ａｉ，ｚ（ｉ＝１，・・・，Ｎの自然数であって、Ｎ≧４；３次元測位の場合）は、それぞれ、式（５）、式（６）、式（７）のとおりである。
【００１２】
【数５】
2005024535.tif 000007
【００１３】
受信機位置誤差ベクトルは、式（８）のとおりである。
【００１４】
【数６】
2005024535.tif 000008
【００１５】
式（８）の（ｘハット，ｙハット，ｚハット）が算出する測位解であり、ｔハットは副次的に求められる受信機時計誤差である。
【００１６】
３次元空間での測位においては、
もし、受信できる送信機数が３個以下の場合、式（１）の方程式は不定解となり、測位は不可能である。
もし、受信できる送信機数が４個の場合、式（９）のように式（１）の解を求め、測位することができる。
【００１７】
【数７】
2005024535.tif 000009
【００１８】
もし、受信できる送信機数が５個以上の場合、式（１０）のように式（１）の最小二乗解を求め、測位することができる。
【００１９】
【数８】
2005024535.tif 000010
【００２０】
なお、２次元空間での測位においては、
もし、受信できる送信機数が２個以下の場合、式（１）の方程式は不定解となり、測位は不可能である。
もし、受信できる送信機数が３個の場合、式（９）のように式（１）の解を求め、測位することができる。
もし、受信できる送信機数が４個以上の場合、式（１０）のように式（１）の最小二乗解を求め、測位することができる。
【００２１】
受信できる送信機数が、測位に必要な最低数（３次元測位の場合は４個、２次元測位の場合は３個）より多い場合は式（１０）に示した最小二乗法を用いて測位解を求めることができることを上で述べたが、上記の非特許文献１のように全ての送信機を用いて測位解を求める方法の他に、送信機を適宜選択して測位解を求める方式もよく用いられる（例えば、特許文献１参照。）。
【００２２】
上記の特許文献１に示された従来の方式では、８個の衛星（送信機）から、受信可能な任意の４個の衛星を組み合わせて測位解を求め、それぞれの測位解に対する誤差の評価指標ＤＯＰとＵＥＲＥの積算値を元に各測位解の誤差を評価し、積算値の最も小さい測位解を、測位結果として選択する。
ここで、ＤＯＰは衛星の配置に依存する測位精度の評価指標であり、例えば、３次元測位の場合、式（１１）とおくと、式（１２）で与えられる。
【００２３】
【数９】
2005024535.tif 000011
【００２４】
また、ＵＥＲＥは、衛星の内部時計の誤差や軌道のずれ等の情報として衛星から放送されるＳＶａｃｃｕｒａｃｙの値に応じた誤差評価指標である。
【００２５】
【特許文献１】特開平６−５９０１４号公報
【非特許文献１】日本測地学会編著、「ＧＰＳ−人工衛星による精密測位システム−」第３版、初版１９８９年１１月１５日、ｐ．１２４−１３１
【発明の開示】
【発明が解決しようとする課題】
【００２６】
上記の非特許文献１に示されている従来の方式は、常に全送信機を用いて測位演算を行うため、送信機の中にマルチパスの影響を受けた送信機があったり、伝播経路上に問題のある送信機があったりすると測位精度が劣化するという問題点があった。
【００２７】
また、上記の特許文献１に示された他の従来の方式は、冗長に用意された送信機数から、測位に必要な数の任意の送信機の組み合わせ毎に測位解を求め、送信機の配置による測位誤差や衛星の内部時計の誤差や軌道のずれ等、衛星自らが提供するＳＶａｃｃｕｒａｃｙ情報に関する誤差に起因して発生する測位誤差の評価指標を送信機の選択基準として、これらの誤差により不具合となった送信機を含まない組み合わせを選択しているが、上記マルチパスや伝搬経路上の問題により測位精度が劣化する場合を評価することができないという問題点があった。
【００２８】
この発明はかかる問題点を解決するためになされたもので、全送信機の中の一個または複数個の送信機にマルチパスの影響を受けたものがあったり、送信機から受信機までの伝播経路上に問題がある場合においても、精度の高い測位を行うことが可能な位置推定装置を得ることを目的とする。
【課題を解決するための手段】
【００２９】
この発明は、位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に受信機位置を推定する位置推定装置であって、受信可能な全送信機の中から、受信機位置の測定に必要な所定の最低数以上の送信機の組み合わせを生成する送信機組み合わせ変更手段と、上記送信機組み合わせ変更手段による上記送信機の組み合わせ毎に、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、上記受信機位置の測位解を算出する測位解算出手段と、過去に計測された受信機の位置から現在の受信機の位置を予測する予測手段と、上記予測手段で算出された予測位置と上記測位解算出手段で算出された上記測位解との予測残差に基づいて、上記送信機組み合わせ変更手段による上記送信機の組み合わせを選択する選択手段とを備えた位置推定装置である。
【発明の効果】
【００３０】
この発明は、位置が既知である複数の送信機から送信された電波を、位置が未知である受信機で受信して、送信機から受信機までの伝播時間を計測することにより得られる複数の送受信機間の距離を基に受信機位置を推定する位置推定装置であって、受信可能な全送信機の中から、受信機位置の測定に必要な所定の最低数以上の送信機の組み合わせを生成する送信機組み合わせ変更手段と、上記送信機組み合わせ変更手段による上記送信機の組み合わせ毎に、送信機から受信機までの伝播時間を計測することにより得られる送受信機間の距離を基に、上記受信機位置の測位解を算出する測位解算出手段と、過去に計測された受信機の位置から現在の受信機の位置を予測する予測手段と、上記予測手段で算出された予測位置と上記測位解算出手段で算出された上記測位解との予測残差に基づいて、上記送信機組み合わせ変更手段による上記送信機の組み合わせを選択する選択手段とを備えた位置推定装置であるので、全送信機の中の一個または複数個の送信機にマルチパスの影響を受けたものがあったり、送信機から受信機までの伝播経路上に問題がある場合においても、精度の高い測位を行うことが可能である。
【発明を実施するための最良の形態】
【００３１】
実施の形態１．
図１はこの発明の実施の形態１による位置推定装置を示す構成図であり、図において、１は空間に電波を放出する複数の送信機、２は送信機１からの電波を受信する受信機、３は測位を行うための送信機１の組み合わせを変更する送信機組み合わせ変更器、４は組み合わせ変更器３で組み合わせた送信機１から受信機２までの伝播時間から、送受信機１，２間の擬似距離を算出する擬似距離算出器、５は送受信機１，２の位置情報を用いて方向余弦ベクトルを算出する方向余弦ベクトル算出器、６は擬似距離と方向余弦ベクトルとから測位解を算出する最小二乗測位解算出器、７は最小二乗測位解算出器６で算出された測位解を記憶する測位解記憶器、８は平滑器１０で算出された平滑ベクトルが遅延回路１１を介して入力され、受信機２の位置の予測ベクトル及び予測誤差共分散行列を算出する予測器、９は、予測器８から予測ベクトルを入力し、測位解記憶器７から測位解を入力して、予測位置に近い測位解を選択する測位解選択器、１０は測位解選択器９から入力された測位解と、予測器８から入力された予測ベクトル及び予測誤差共分散行列とから平滑ベクトル及び平滑誤差共分散行列を算出する平滑器、１１は平滑器１０で算出した平滑ベクトル及び平滑誤差共分散行列を１サンプリング時刻だけ遅延する遅延回路である。
【００３２】
次に動作について説明する。
最初に、この実施の形態１による位置推定装置の動作原理を説明する。なお、ここでは、３次元位置を推定する場合を仮定する。
位置が既知である複数の送信機１から送信された電波を、位置が未知である受信機２で受信して得られる送受信機１，２間の伝播時間から、式（１）の測位方程式を解くことによって受信機２の位置を測位する原理は、上記の非特許文献１に記載された従来の技術と同じである。
受信機２の運動を、過去の測位結果を用いて予測する方法について説明する。
受信機２の運動モデルを式（１３）に示す。ここで、ｘｋアンダーバーはサンプリング時刻ｔｋにおける受信機２の運動諸元の真値を表す状態ベクトルであり、受信機２の位置ベクトルを式（１４）、速度ベクトルを式（１５）とすると、受信機２の状態ベクトルは式（１６）で表される。ここで、サンプリング時刻ｔｋ（ｋ＝１，２，・・・，ｎ）は、測位が行われるタイミングを表す離散時刻であり、以降、時刻ｔｋと呼ぶ。
【００３３】
【数１０】
2005024535.tif 000012
【００３４】
Φｋ−１は時刻ｔｋ−１から時刻ｔｋへの状態ベクトルの推移行列であり、式（１７）で表される。また、ｗｋアンダーバーは時刻ｔｋにおける駆動雑音ベクトルであり、Γ１（ｋ）は時刻ｔｋにおける駆動雑音ベクトルの変換行列である。例えば、受信機の運動モデルを等速直線運動と仮定したことによる打ち切り誤差項をΓ１（ｋ−１）ｗｋ−１アンダーバーとみれば、ｗｋアンダーバーは加速度ベクトル相当であり、Γ１（ｋ−１）は式（１８）で表される。なお、Ｔはサンプリング間隔、Ｉは３行３列の単位行列である。
【００３５】
【数１１】
2005024535.tif 000013
【００３６】
また、平均を表す記号としてＥを用いると、ｗｋアンダーバーは平均の３次元正規分布白色雑音であり、式（１９）及び式（２０）とする。ただし、０アンダーバーは零ベクトルであり、Ｑｋは時刻ｔｋにおける駆動雑音共分散行列である。
【００３７】
【数１２】
2005024535.tif 000014
【００３８】
受信機２の位置の観測モデルを式（２１）で表す。ここで、ｕｋアンダーバーは測位解選択器９によって選択された測位解、Ｈは観測行列で、式（２２）で表される。
また、ｖｋアンダーバーは時刻ｔｋにおいて選択された送信機１と受信機２の距離の観測雑音ベクトルであり、式（２３）で表す。観測雑音ベクトルは平均０アンダーバーの３次元正規分布白色雑音に従うものと仮定し、式（２４）及び式（２５）とする。なお、Ｒｋは時刻ｔｋにおける送信機１と受信機２の距離の観測誤差共分散行列である。時刻ｔｋまでの間に受信機２の追尾に用いた測位解の全体をＵｋとし、式（２６）で表す。
【００３９】
【数１３】
2005024535.tif 000015
【００４０】
また、Γ２（ｋ）はＮ個の送受信機１，２間距離から３次元位置への観測雑音ベクトルの変換行列であり、式（２７）、式（２８）で表される。
【００４１】
【数１４】
2005024535.tif 000016
【００４２】
ここで、ｈｉ（ｘｋアンダーバー）はｉ番目の送信機１と受信機２の距離であり、式（２９）で表される。また、行列Ｂのｉ行目の要素は式（３０）〜式（３２）で表される。ｘｋアンダーバー（−）は時刻ｔｋにおける受信機位置の予測ベクトルであり、算出方法は次節で説明する。
【００４３】
【数１５】
2005024535.tif 000017
【００４４】
次に、時刻ｔｋ−１までの測位解Ｕｋ−１が得られているときの予測ベクトルの算出方法について述べる。時刻ｔｋおける受信機の状態ベクトルｘｋアンダーバーの予測ベクトルを（ｘｋアンダーバー）ハット（−）、予測誤差共分散行列をＰｋ（−）とすると、それぞれ、条件付平均ベクトルおよび条件付共分散行列で定義され、式（３３）及び式（３４）で表される。ここで、（ｘｋ−１アンダーバー）ハット（＋）及びＰｋ−１（＋）は、それぞれ、時刻ｔｋ−１の平滑ベクトル及び平滑誤差共分散行列である。これらの算出方法については次に述べる。
【００４５】
【数１６】
2005024535.tif 000018
【００４６】
以上、受信機２の運動を、過去の測位結果を用いて予測する方法について説明した。
【００４７】
次に、測位解記憶器７に記憶された時刻ｔｋにおける複数の測位解の選択方法について述べる。測位解の選択には、式（３４）で表される過去の測位情報に基づく受信機２の現時刻の位置予測ベクトルｕｋアンダーバー（−）を利用する。
【００４８】
【数１７】
2005024535.tif 000019
【００４９】
すなわち、測位解記憶器７に記憶された時刻ｔｋにおけるｍ番目の測位解をｕｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）とすると、式（３６）における予測残差ｄｋ，ｍアンダーバーが最小となる測位解ｕｋアンダーバーを測位精度が最も高いものと判断し、選択する。なお、予測残差ｄｋ，ｍアンダーバーとは、すなわち、受信機２の予測位置と測定位置との残差である。もし、送信機組み合わせ変更器３で選択された送信機１の組み合わせの中の１個または複数個の送信機１にマルチパスの影響を受けた送信機があったり、伝播経路上に問題のある送信機があると、その予測残差は大きな値となるので、予測残差の最も小さい送信機１の組み合わせを選択すれば全ての送信機が正常で伝播経路上にも問題がない送信機の組み合わせを決定することができ、受信機２の位置をより高精度に推定することができる。
【００５０】
【数１８】
2005024535.tif 000020
【００５１】
以上、測位解の選択方法について述べた。
【００５２】
次に、平滑ベクトルの算出方法について述べる。ゲイン行列Ｋｋ、平滑ベクトル（ｘｋアンダーバー）ハット（＋）及び平滑誤差共分散行列Ｐｋ（＋）は通常のカルマンフィルタの理論により、式（３７）〜式（３９）で与えられる。ここで、Ｒｋは式（２５）で表される、時刻ｔｋにおける送信機１と受信機２の距離の観測誤差共分散行列である。以上、平滑ベクトルの算出方法について述べた。
【００５３】
【数１９】
2005024535.tif 000021
【００５４】
以上、実施の形態１による位置推定装置の動作原理について説明した。
【００５５】
次に、実施の形態１による位置推定装置の具体的な動作を図１を用いて説明する。
複数の送信機１から発射された電波は、送信機１から受信機２への距離に基づく遅延時間後に受信機２に到達する。
受信機２では各々の送信機１からの信号を弁別して受信する。送信機組み合わせ変更器３では、その全受信可能な送信機１の中から、測位に必要な所定の最低数以上、全発信機数以下の数で、適当な送信機１の組み合わせを選択する。擬似距離算出器４では、選択した組み合わせの送信機１の信号の伝播遅延時間から擬似距離ｒｉ（ｉ＝１，２，・・・，Ｎ）を算出する。
【００５６】
方向余弦ベクトル算出器５では、予め設定された送信機１の位置情報から式（４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、擬似距離ｒｉ（ｉ＝１，２，・・・，Ｎ）と方向余弦ベクトルＡを式（１０）に代入して、最小二乗測位解ｕｋ、ｍアンダーバーを算出する。送信機組み合わせ変更器３により送信機１の組み合わせを変えて、以上の測位演算と距離残差の算出を所定のＭ回繰り返し、その測位解ｕｋ、ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）をそれぞれ測位解記憶器７に記憶する。
【００５７】
予測器８では、１サンプリング前の平滑ベクトル（ｘｋ−１アンダーバー）ハット（＋）と平滑誤差共分散行列Ｐｋ−１（＋）を遅延回路１１を介して入力し、式（３３）〜式（３５）に従い、予測ベクトル（ｘｋアンダーバー）ハット（−）、予測誤差共分散行列Ｐｋ−１（−）、位置予測ベクトル（ｕｋアンダーバー）（−）をそれぞれ算出する。測位解選択器９では、予測器８から位置予測ベクトル（ｕｋアンダーバー）（−）を入力し、測位解記憶器７に記憶された測位解ｕｋ、ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）の中から、式（３６）で表される予測残差ｄｋ、ｍが最も小さい送信機１の組み合わせの測位解ｕｋアンダーバーを選択して、最終的な測位解とする。
【００５８】
平滑器１０では、測位解選択器で選択した測位解ｕｋアンダーバーと、予測器８で算出した予測ベクトル（ｘｋアンダーバー）ハット（−）及び予測誤差共分散行列Ｐｋ（−）を用いて平滑ベクトル（ｘｋアンダーバー）ハット（＋）及び平滑誤差共分散行列Ｐｋ（＋）を算出する。
【００５９】
以上のように、本実施の形態に係る位置推定装置は、予測残差の最も小さい送信機１の組み合わせを選択するので、従来の方式に比べ、信頼性が高く、全ての送信機が正常で伝播経路上にも問題ないと考えられる送信機の組み合わせを選択して受信機の位置を推定することができる。
【００６０】
実施の形態２．
図２は、本実施の形態２による位置推定装置を示す構成図であり、図において、図１と同一符号である１〜８、１０、１１は実施の形態１と同一または相当部分を示すのでその説明を省略する。
【００６１】
１２は平滑器１０で算出した平滑誤差共分散行列を遅延回路を介して入力し、測位解の予測残差の確率密度関数を算出する予測残差確率密度算出器、１３は予測器８から予測ベクトルを、測位解記憶器７から測位解を、予測残差確率密度算出器１２から測位解の予測残差の確率密度関数をそれぞれ入力し、各測位解の予測残差の確率密度から測位に適さない測位解を排除するゲート判定器、１４はゲート判定器１３において排除されなかった測位解と、予測残差確率密度算出器１２で算出した測位解の予測残差の確率密度関数をそれぞれ入力し、測位解の信頼度を算出する測位解信頼度算出器である。
【００６２】
次に動作について説明する。
最初に、この実施の形態２による位置推定装置の動作原理を説明する。なお、ここでは、３次元位置を推定する場合を仮定する。
位置が既知である複数の送信機１から送信された電波を、位置が未知である受信機２で受信して得られる送受信機間の伝播時間から、（１）式の測位方程式を解くことによって受信機位置を測位する原理は従来の技術における非特許文献１と同じである。
【００６３】
また、測位対象の運動を、過去の測位結果を用いて予測する方法は、上述の実施の形態１と同じである。
【００６４】
次に、測位に適さない測位解を排除する方法について述べる。実施の形態１では、予測残差の最も小さい測位解のみを選択して位置を推定しているが、予測残差の大きさはマルチパス等のセンサ不具合による影響以外に、送信機１と受信機２の幾何学的配置関係にも影響を受ける。センサ不具合がなく、送受信機の幾何学的配置関係のみにより予測残差が大きくなっている測位解は、利用した方が高い精度が得られる可能性がある。そこで、送受信機の幾何学的配置関係による影響を考慮し、センサ不具合の影響を受けた測位解のみを排除する。
【００６５】
過去の測位情報Ｕｋ−１より得られる測位解の予測確率分布Ｐ［ｕｋ，ｍアンダーバー｜Ｕｋ−１］は、式（４０）に示す条件付確率密度関数で表される。すなわち、測位解は、式（３５）で与えられる位置予測ベクトルｕｋアンダーバー（−）を平均とし、式（４１）で与えられるＳｋを共分散行列とする３次元正規分布ｇ３（ｕｋ，ｍアンダーバー；ｕｋ（−），Ｓｋ）に従うとする。この確率密度関数の式（４１）の右辺第２項は送受信機の幾何学的配置関係を考慮した項となっている。そして、式（４２）を満たす測位解を測位に利用し、満たさない測位解を排除する。ここで、ｑは目標との相関範囲を決めるパラメータであり、自由度３のχ自乗分布により算出する。
【００６６】
【数２０】
2005024535.tif 000022
【００６７】
次に、式（４２）を満たす全ての測位解に対し（便宜上、その総数をＭとする）、測位解の確率密度ｒｋ，ｍを式（４３）に従い算出し、これを正規化した式（４４）から当該測位解の信頼度βｋ，ｍを算出する。そして、式（４５）に従い、この信頼度で測位解を重み付けした平均値ｕｋアンダーバーを受信機２の推定位置とする。
【００６８】
【数２１】
2005024535.tif 000023
【００６９】
信頼度で測位解を重み付けした平均値ｕｋアンダーバーを用いて平滑ベクトルを算出する方法は実施の形態１と同じである。なお、平滑ベクトルの位置ベクトルを受信機２の推定位置としてもよい。
以上、実施の形態２による位置推定装置の動作原理について説明した。
【００７０】
次に、実施の形態２による位置推定装置の具体的な動作を図２に従い説明する。
複数の送信機１から発射された電波は、送信機１から受信機２への距離に基づく遅延時間後に受信機２に到達する。受信機２では各々の送信機１からの信号を弁別して受信する。送信機組み合わせ変更器３では、その全受信可能な送信機１の中から、測位に必要な所定の最低数以上、全発信機数以下の数で、適当な送信機１の組み合わせを選択する。擬似距離算出器４では、選択した組み合わせの送信機１の信号の伝播遅延時間から擬似距離ｒｉ（ｉ＝１，２，・・・，Ｎ）を算出する。
【００７１】
方向余弦ベクトル算出器５では、予め設定された送信機の位置情報から式（４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、擬似距離ｒｉ（ｉ＝１，２，・・・，Ｎ）と方向余弦ベクトルＡを式（１０）に代入して、最小二乗測位解ｕｋ，ｍアンダーバーを算出する。送信機組み合わせ変更器３により送信機１の組み合わせを変えて、以上の測位演算と距離残差の算出をＭ回繰り返し、その測位解ｕｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）をそれぞれ測位解記憶器７に記憶する。
【００７２】
予測器８では、１サンプリング前の平滑ベクトル（（ｘｋ−１アンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐｋ−１（＋）を遅延回路１１を介して入力し、式（３３）〜式（３５）に従い予測ベクトル（（ｘｋアンダーバー）ハット）（−）、予測誤差共分散行列Ｐｋ（−）、位置予測ベクトル（ｕｋアンダーバー）（−）をそれぞれ算出する。
【００７３】
予測残差確率密度算出器１２では、予測器８から予測誤差共分散行列Ｐｋ（−）を入力し、これと、予め設定された送信機と受信機の距離の観測誤差共分散行列とから、式（２７）及び式（４１）に従い、測位解の予測確率密度関数Ｐ［ｕｋ，ｍアンダーバー｜Ｕｋ−１］の共分散行列Ｓｋを算出する。ゲート判定器１３では、予測器８から位置予測ベクトルｕｋアンダーバー（−）を、予測残差確率密度算出器１２から共分散行列Ｓｋを入力し、測位解記憶器７に記憶された測位解ｕｋ、ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）の中から、式（４２）を満たすものを選択し、満たさないものを排除する。
【００７４】
測位解信頼度算出器１４では、予測器８で算出した位置予測ベクトルｕｋアンダーバー（−）と予測残差確率密度算出器１２で算出した共分散行列Ｓｋとをゲート判定器１３を介して入力し、式（４２）を満たす測位解ｕｋ、ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）の信頼度βｋ、ｍを、式（４４）に従い算出するとともに、この信頼度で測位解を重み付けした平均値ｕｋアンダーバーを式（４５）に従い算出する。
【００７５】
平滑器１０では、測位解信頼度算出器１４で算出した測位解ｕｋアンダーバーと、予測器８で算出した予測ベクトル（（ｘｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐｋ（−）を用いて平滑ベクトル（（ｘｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐｋ（＋）を算出する。
【００７６】
以上のように本実施の形態は、測位解の予測確率分布を利用して送受信機１，２の幾何学的配置関係のみによって測位誤差が大きくなっている測位解と、センサ不具合の影響を受けた測位解を判別し、後者のみを排除した残りの測位解の全てを用いて受信機２の位置を推定するので、上記の実施の形態２より高い精度で受信機２の位置を推定することができる可能性が高い。また、予測確率分布を利用して算出した信頼度による測位解の重み付き平均値を推定結果としているため、信頼性の低い測位解の悪影響を少なくすることができる。
【００７７】
実施の形態３．
図３はこの実施の形態３による位置推定装置を示す構成図であり、図において、図１と同一符号である１〜７は実施の形態１と同一または相当部分を示すのでその説明を省略する。
【００７８】
１５は最小二乗測位解算出器６で算出した距離残差を記憶する距離残差記憶器、１３は測位解記憶器７から測位解を、距離残差記憶器１５から距離残差を入力し、各測位解の距離残差の確率密度から測位に適さない測位解を排除するゲート判定器、１４はゲート判定器１３において排除されなかった測位解とをそれぞれ入力し、測位解の信頼度を算出する測位解信頼度算出器である。
【００７９】
次に動作について説明する。
最初に、この実施の形態３による位置推定装置の動作原理を説明する。なお、ここでは、３次元位置を推定する場合を仮定する。位置が既知である複数の送信機１から送信された電波を、位置が未知である受信機２で受信して得られる送受信機間の伝播時間から、（１）式の測位方程式を解くことによって受信機位置を測位する原理は従来の技術における従来の方式１と同じである。
【００８０】
次に、測位に適さない測位解を排除する方法について述べる。実施の形態１では、予測残差大きさを基に測位解の精度を判断しているが、マルチパス等のセンサ不具合の影響により測位解の精度が悪くなっている場合には、式（１０）に従って測位解を算出する際に得られる距離残差も大きくなっていると考えられる。そこで、この実施の形態３では、距離残差を利用することによって、測位に適さない測位解を判定する。
【００８１】
すなわち、距離残差の確率分布を、平均０、分散μ２の１次元正規分布と仮定すると、式（４６）で表される各測位解毎に得られる擬似距離ｒｉと式（１０）の反復演算により得られた測位解ｆｉ（ｘハット，ｙハット，ｚハット，ｔハット）の差のベクトルである距離残差の２乗ノルムρｍ２が式（４７）を満たす測位解を測位に利用し、満たさない測位解を排除する（ｍは送信機１の組み合わせ番号）。ここで、ｗは目標との相関範囲を決めるパラメータであり、自由度３のχ自乗分布により算出する。
【００８２】
【数２２】
2005024535.tif 000024
【００８３】
次に、式（４７）を満たす全ての測位解に対し（便宜上、その総数をＭとする）、測位解の確率密度ｒ’ｋ、ｍを式（４８）に従い算出し、これを正規化した式（４９）から当該測位解の信頼度β’ｋ、ｍを算出する。そして、式（５０）に従い、この信頼度で測位解を重み付けした平均値ｕｋアンダーバーを受信機２の推定位置とする。
【００８４】
【数２３】
2005024535.tif 000025
【００８５】
信頼度で測位解を重み付けした平均値ｕｋアンダーバーを用いて平滑ベクトルを算出する方法は実施の形態１と同じである。
以上、実施の形態３による位置推定装置の動作原理について説明した。
【００８６】
次に、実施の形態３による位置推定装置の具体的な動作を図３に従い説明する。複数の送信機１から発射された電波は、送信機１から受信機２への距離に基づく遅延時間後に受信機２に到達する。受信機２では各々の送信機１からの信号を弁別して受信する。送信機組み合わせ変更器３では、その全受信可能な送信機１の中から、測位に必要な所定の最低数以上、全発信機数以下の数で、適当な送信機１の組み合わせを選択する。擬似距離算出器４では、選択した組み合わせの送信機１の信号の伝播遅延時間から擬似距離を算出する。
【００８７】
方向余弦ベクトル算出器５では、予め設定された送信機１の位置情報から式（４７）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、擬似距離ｒｉ（ｉ＝１，２，・・・，Ｎ）と方向余弦ベクトルＡを式（１０）に代入して、最小二乗測位解ｕｋ、ｍアンダーバーを算出する。送信機組み合わせ変更器３により送信機１の組み合わせを変えて、以上の測位演算と距離残差の算出をＭ回繰り返し、その測位解ｕｋ、ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）をそれぞれ測位解記憶器７に記憶する。
【００８８】
距離残差記憶器１５では、最小二乗測位解算出器６から、擬似距離ｒｉと測位解ｆｉ（ｘハット，ｙハット，ｚハット，ｔハット）を入力し、式（４６）に従い距離残差の２乗ノルムρｍ２を算出し、これを記憶する。ゲート判定器１３では、距離残差記憶器１５から入力した距離残差の２乗ノルムρｍ２と、予め設定された距離残差の確率分布の分散μ２とから、測位解記憶器７に記憶された測位解ｕｋ、ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）の中から、式（４７）を満たすものを選択し、満たさないものを排除する。
【００８９】
測位解信頼度算出器１４では、距離残差の２乗ノルムρｍ２と、予め設定された距離残差の確率分布の分散μ２をゲート判定器１３を介して入力し、式（４７）を満たす全ての測位解に対し、測位解の信頼度βｋ，ｍを、式（４８）〜式（４９）に従い算出するとともに、この信頼度で測位解を重み付けした平均値ｕｋアンダーバーを受信機２の推定位置として式（５０）に従い算出する。
【００９０】
以上のように、本実施の形態は、測位解の距離残差の確率分布を利用して不具合の影響を受けた測位解を判別し、後者のみを排除した残りの測位解の全てを用いて受信機２の位置を推定するので、従来の方式に比べ、信頼性が高く、全ての送信機１が正常で伝播経路上にも問題ないと考えられる送信機１の組み合わせを選択して受信機２の位置を推定することができる。
【００９１】
実施の形態４．
上記の実施の形態１では、位置が既知である複数の送信機１からの電波を、位置が未知である受信機２で受信して、送信機１から受信機２までの伝播時間を計測して受信機位置を測位する方式への本発明の応用例を示したが、本実施の形態は、逆に、位置が未知である送信機１からの電波を、位置が既知である複数の受信機２で受信して、送信機１から受信機２までの伝播時間を計測して送信機位置を測位する方式へ本発明を適用した適用例について説明する。
【００９２】
図４はこの実施の形態４による位置推定装置を示す構成図であり、図において、図１と同一符号である１、２、４〜１１は実施の形態１と同一または相当部分を示すのでその説明を省略する。
【００９３】
１６は測位を行うための受信機２の組み合わせを変更する受信機組み合わせ変更器である。
【００９４】
次に、実施の形態４による位置推定装置の具体的な動作を図４に従い説明する。送信機１から発射された電波は、各々の伝播経路の伝播時間後に複数の受信機２で各々の送信機１からの信号を弁別して受信する。受信機組み合わせ変更器１６では、その全受信可能な受信機２の中から、測位に必要な所定の最低数以上、全発信機数以下の数で、適当な受信機２の組み合わせを選択する。擬似距離算出器４では、選択した組み合わせの受信機２の信号の伝播遅延時間から擬似距離ｒｉ（ｉ＝１，２，・・・，Ｎ）を算出する。
【００９５】
方向余弦ベクトル算出器５では、予め設定された受信機の位置情報から式（４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、擬似距離ｒｉ（ｉ＝１，２，・・・，Ｎ）と方向余弦ベクトルＡを式（１０）に代入して、最小二乗測位解ｕｋ，ｍアンダーバーを算出する。受信機組み合わせ変更器１６により受信機２の組み合わせを変えて、以上の測位演算と距離残差の算出をＭ回繰り返し、その測位解ｕｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）をそれぞれ測位解記憶器７に記憶する。
【００９６】
予測器８では、１サンプリング前の平滑ベクトル（（ｘｋ−１アンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐｋ−１（＋）を遅延回路１１を介して入力し、式（３３）〜式（３５）に従い予測ベクトル（（ｘｋアンダーバー）ハット）（−）、予測誤差共分散行列Ｐｋ（−）、位置予測ベクトル（ｕｋアンダーバー）（−）をそれぞれ算出する。測位解選択器９では、予測器８から位置予測ベクトル（ｕｋアンダーバー）（−）を入力し、測位解記憶器７に記憶された測位解ｕｋ、ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）の中から、式（３６）で表される予測残差ｄｋ，ｍが最も小さい受信機の組み合わせの測位解ｕｋアンダーバーを選択して、最終的な測位解とする。
【００９７】
平滑器１０では、測位解選択器で選択した測位解ｕｋアンダーバーと、予測器８で算出した予測ベクトル（（ｘｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐｋ（−）を用いて平滑ベクトル（（ｘｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐｋ（＋）を算出する。
【００９８】
以上のように、位置が未知である送信機１からの電波を、位置が既知である複数の受信機２で受信して、送信機１から受信機２までの伝播時間を計測して送信機位置を測位する方式に本発明を応用しても、実施の形態１と同様に、送信機１の位置をより高精度に測位することができる。
【００９９】
実施の形態５．
実施の形態２では、位置が既知である複数の送信機１からの電波を、位置が未知である受信機２で受信して、送信機１から受信機２までの伝播時間を計測して受信機位置を測位する方式への応用例であるが、本発明は、位置が未知である送信機１からの電波を、位置が既知である複数の受信機２で受信して、送信機１から受信機２までの伝播時間を計測して送信機位置を測位する方式にも適用できるので、本実施の形態においては、当該適用例について説明する。
【０１００】
図５はこの実施の形態５による位置推定装置を示す構成図であり、図において、図１と同一符号である１、２、４〜８、１０〜１４は実施の形態２と同一または相当部分を示すのでその説明を省略する。
【０１０１】
１６は測位を行うための受信機２の組み合わせを変更する受信機組み合わせ変更器である。
【０１０２】
次に、実施の形態５による位置推定装置の具体的な動作を図５に従い説明する。送信機１から発射された電波は、各々の伝播経路の伝播時間後に複数の受信機２で各々の送信機からの信号を弁別して受信する。受信機組み合わせ変更器１６では、その全受信可能な受信機２の中から、測位に必要な所定の最低数以上、全受信機数以下の数で、適当な受信機２の組み合わせを選択する。擬似距離算出器４では、選択した組み合わせの送信機１の信号の伝播遅延時間から擬似距離ｒｉ（ｉ＝１，２，・・・，Ｎ）を算出する。
【０１０３】
方向余弦ベクトル算出器５では、予め設定された送信機の位置情報から式（４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、擬似距離ｒｉ（ｉ＝１，２，・・・，Ｎ）と方向余弦ベクトルＡを式（１０）に代入して、最小二乗測位解ｕｋ，ｍアンダーバーを算出する。受信機組み合わせ変更器１６により受信機２の組み合わせを変えて、以上の測位演算と距離残差の算出をＭ回繰り返し、その測位解ｕｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）をそれぞれ測位解記憶器７に記憶する。
【０１０４】
予測器８では、１サンプリング前の平滑ベクトル（（ｘｋ−１アンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐｋ−１（＋）を遅延回路１１を介して入力し、式（３３）〜式（３５）に従い予測ベクトル（（ｘｋアンダーバー）ハット）（−）、予測誤差共分散行列Ｐｋ（−）、位置予測ベクトルｕｋアンダーバー（−）をそれぞれ算出する。
【０１０５】
予測残差確率密度算出器１２では、予測器８から予測誤差共分散行列Ｐｋ（−）を入力し、これと、予め設定された送信機１と受信機２の距離の観測誤差共分散行列とから、式（２７）及び式（４１）に従い、測位解の予測確率密度関数
Ｐ［ｕｋ，ｍアンダーバー｜Ｕｋ−１］の共分散行列Ｓｋを算出する。ゲート判定器１３では、予測器８から位置予測ベクトルｕｋアンダーバー（−）を、予測残差確率密度算出器１２から共分散行列Ｓｋを入力し、測位解記憶器７に記憶された測位解ｕｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）の中から、式（４２）を満たすものを選択し、満たさないものを排除する。
【０１０６】
測位解信頼度算出器１４では、予測器８で算出した位置予測ベクトルｕｋアンダーバー（−）と予測残差確率密度算出器１２で算出した共分散行列Ｓｋとをゲート判定器１３を介して入力し、式（４２）を満たす測位解ｕｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）の信頼度βｋ，ｍを、式（４４）に従い算出するとともに、この信頼度で測位解を重み付けした平均値ｕｋアンダーバーを式（４５）に従い算出する。
【０１０７】
平滑器１０では、測位解信頼度算出器１４で算出した測位解ｕｋアンダーバーと、予測器８で算出した予測ベクトル（（ｘｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐｋ（−）を用いて平滑ベクトル（（ｘｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐｋ（＋）を算出する。
【０１０８】
以上のようにして、位置が未知である送信機１からの電波を、位置が既知である複数の受信機２で受信して、送信機１から受信機２までの伝播時間を計測して送信機位置を測位する方式に本発明を応用しても、実施の形態２と同様に、送信機１の位置をより高精度に測位することができる。
【０１０９】
実施の形態６．
実施の形態３では、位置が既知である複数の送信機１からの電波を、位置が未知である受信機２で受信して、送信機１から受信機２までの伝播時間を計測して受信機位置を測位する方式への応用例であるが、本発明は、位置が未知である送信機１からの電波を、位置が既知である複数の受信機２で受信して、送信機１から受信機２までの伝播時間を計測して送信機位置を測位する方式にも適用できるので、本実施の形態においては、当該適用例について説明する。
【０１１０】
図６はこの実施の形態６による位置推定装置を示す構成図であり、図において、図１と同一符号である１、２、４〜７、１３〜１５は実施の形態３と同一または相当部分を示すのでその説明を省略する。
【０１１１】
１６は測位を行うための受信機２の組み合わせを変更する受信機組み合わせ変更器である。
【０１１２】
次に、実施の形態６による位置推定装置の具体的な動作を図６に従い説明する。送信機１から発射された電波は、各々の伝播経路の伝播時間後に複数の受信機２で各々の送信機からの信号を弁別して受信する。受信機組み合わせ変更器１６では、その全受信可能な受信機２の中から、測位に必要な所定の最低数以上、全受信機数以下の数で、適当な受信機２の組み合わせを選択する。擬似距離算出器４では、選択した組み合わせの送信機１の信号の伝播遅延時間から擬似距離
ｒｉ（ｉ＝１，２，・・・，Ｎ）を算出する。
【０１１３】
方向余弦ベクトル算出器５では、予め設定された送信機の位置情報から式（４７）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、擬似距離ｒｉ（ｉ＝１，２，・・・，Ｎ）と方向余弦ベクトルＡを式（１０）に代入して、最小二乗測位解ｕｋ，ｍアンダーバーを算出する。受信機組み合わせ変更器１６により受信機の組み合わせを変えて、以上の測位演算と距離残差の算出をＭ回繰り返し、その測位解ｕｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）をそれぞれ測位解記憶器７に記憶する。
【０１１４】
距離残差記憶器１５では、最小二乗測位解算出器６から、擬似距離ｒｉと測位解ｆｉ（ｘハット，ｙハット，ｚハット，ｔハット）を入力し、式（４６）に従い距離残差の２乗ノルムρｍ２を算出し、これを記憶する。ゲート判定器１３では、距離残差記憶器１５から入力した距離残差の２乗ノルムρｍ２と、予め設定された距離残差の確率分布の分散μ２とから、測位解記憶器７に記憶された測位解ｕｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）の中から、式（４７）を満たすものを選択し、満たさないものを排除する。
【０１１５】
測位解信頼度算出器１４では、距離残差の２乗ノルムρｍ２と、予め設定された距離残差の確率分布の分散μ２をゲート判定器１３を介して入力し、式（４７）を満たす全ての測位解に対し、測位解の信頼度βｋ，ｍを、式（４８）〜式（４９）に従い算出するとともに、この信頼度で測位解を重み付けした平均値ｕｋアンダーバーを受信機の推定位置として式（５０）に従い算出する。
【０１１６】
以上のようにして、位置が未知である送信機１からの電波を、位置が既知である複数の受信機２で受信して、送信機１から受信機２までの伝播時間を計測して送信機位置を測位する方式に本発明を応用しても、実施の形態３と同様に、送信機の位置をより高精度に測位することができる。
【０１１７】
実施の形態７．
実施の形態１は、位置が既知である複数の送信機１からの電波を、位置が未知である受信機２で受信して、送信機１から受信機２までの伝播時間を計測して受信機位置を測位する方式において、電波の伝播時間を用いた測位方式に本発明を適用したものであるが、本発明は、複数の送信機１から受信機２への電波の伝播時間差や位相差を用いた測位方式にも適用できるので、本実施の形態においては、当該適用例について説明する。
【０１１８】
図７はこの実施の形態７による位置推定装置を示す構成図であり、図において、図１と同一符号である１〜３、５〜１１は実施の形態１と同一または相当部分を示すのでその説明を省略する。
【０１１９】
１７は送信機組み合わせ変更器３で組み合わせた送信機から受信機への電波の伝播時間差もしくは位相差から、送受信機間の擬似距離差を算出する擬似距離差算出器である。
【０１２０】
次に複数の送信機１から受信機２への電波の伝播時間差や位相差を用いた測位方法について説明する。測位演算は式（１０）の代わりに式（５１）を用いる。
【０１２１】
【数２４】
2005024535.tif 000026
【０１２２】
【数２５】
2005024535.tif 000027
【０１２３】
但し、式（５１）のδｕ、Ａ、δｒはそれぞれ式（５２）、式（５３）、および式（５４）である。
【０１２４】
式（５３）のΔｒｉ，ｊ（ｉ，ｊ＝１，・・・，Ｎの自然数であって、Ｎ≧４；三次元測位の場合）はｉ番目の送信機１から受信機２までの距離とｊ番目の送信機から受信機までの距離との差であり、伝播時間差による測位の場合は伝播時間差と光速の積から求める。また、電波の位相差を用いる測位の場合には位相差と電波の波長の積から求める。
【０１２５】
式（５３）、式（５４）のΔｆｉ，ｊ（ｘ０，ｙ０，ｚ０）はｉ番目の送信機位置から受信機位置までの距離とｊ番目の送信機位置から受信機位置までの距離の概算距離差であり、反復演算で得られる前回の測位値（ｘ０，ｙ０，ｚ０）と、既知であるｉ番目の送信機位置（ｘｉ，ｙｉ，ｚｉ）と、同じく既知であるｊ番目の発信機位置（ｘｊ，ｙｊ，ｚｊ）から、式（５５）を用いて算出する。
【０１２６】
【数２６】
2005024535.tif 000028
【０１２７】
次に、実施の形態７による位置推定装置の具体的な動作を図７に従い説明する。複数の送信機１から発射された電波は、送信機から受信機への距離に基づく遅延時間後に受信機２に到達する。受信機２では各々の送信機からの信号を弁別して受信する。送信機組み合わせ変更器３では、その全受信可能な送信機の中から、測位に必要な所定の最低数以上、全発信機数以下の数で、適当な送信機の組み合わせを選択する。擬似距離算出器１７では、選択した組み合わせの２個の送信機の信号の伝播時間差もしくは位相差から擬似距離差ｒｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）を算出する。
【０１２８】
方向余弦ベクトル算出器５では、予め設定された送信機の位置情報から式（５４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、擬似距離差ｒｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）と方向余弦ベクトルＡを式（５１）に代入して、最小二乗測位解ｕｋ，ｍアンダーバーを算出する。送信機組み合わせ変更器３により送信機の組み合わせを変えて、以上の測位演算と距離残差の算出をＭ回繰り返し、その測位解ｕｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）をそれぞれ測位解記憶器７に記憶する。
【０１２９】
予測器８では、１サンプリング前の平滑ベクトル（（ｘｋ−１アンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐｋ−１（＋）を遅延回路１１を介して入力し、式（３３）〜式（３５）に従い予測ベクトル（（ｘｋアンダーバー）ハット）（−）、予測誤差共分散行列Ｐｋ（−）、位置予測ベクトルｕｋ（−）をそれぞれ算出する。測位解選択器９では、予測器８から位置予測ベクトルｕｋアンダーバー（−）を入力し、測位解記憶器７に記憶された測位解ｕｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）の中から、式（３６）で表される予測残差ｄｋ，ｍが最も小さい送信機の組み合わせの測位解ｕｋアンダーバーを選択して、最終的な測位解とする。
【０１３０】
平滑器１０では、測位解選択器で選択した測位解ｕｋアンダーバーと、予測器８で算出した予測ベクトル（（ｘｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐｋ（−）を用いて平滑ベクトル（（ｘｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐｋ（＋）を算出する。
【０１３１】
以上のようにして、位置が既知である複数の送信機１から送信された電波を、位置が未知である受信機２で受信して、受信機位置での各送信機の信号間の伝播時間差または位相差を計測して受信機位置を測位する方式に本発明を応用しても、実施の形態１と同様に、受信機２の位置をより高精度に測位することができる。
【０１３２】
実施の形態８．
実施の形態２は、位置が既知である複数の送信機１からの電波を、位置が未知である受信機２で受信して、送信機１から受信機２までの伝播時間を計測して受信機位置を測位する方式において、電波の伝播時間を用いた測位方式に本発明を適用したものであるが、本発明は、複数の送信機１から受信機２への電波の伝播時間差や位相差を用いた測位方式にも適用できるので、本実施の形態においては、当該適用例について説明する。
【０１３３】
図８はこの実施の形態８による位置推定装置を示す構成図であり、図において、図２と同一符号である１〜３、５〜８、１０〜１４は実施の形態２と同一または相当部分を示すのでその説明を省略する。
【０１３４】
１７は送信機組み合わせ変更器３で組み合わせた送信機１から受信機２への電波の伝播時間差もしくは位相差から、送受信機間の擬似距離差を算出する擬似距離差算出器である。
【０１３５】
なお、複数の送信機１から受信機２への電波の伝播時間差や位相差を用いた測位方法は実施の形態７と同じである。
【０１３６】
次に、実施の形態８による位置推定装置の具体的な動作を図８に従い説明する。複数の送信機１から発射された電波は、送信機１から受信機２への距離に基づく遅延時間後に受信機２に到達する。受信機２では各々の送信機１からの信号を弁別して受信する。送信機組み合わせ変更器３では、その全受信可能な送信機１の中から、測位に必要な所定の最低数以上、全発信機数以下の数で、適当な送信機１の組み合わせを選択する。擬似距離差算出器１７では、選択した組み合わせの２個の送信機１の信号の伝播時間差もしくは位相差から擬似距離差ｒｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）を算出する。
【０１３７】
方向余弦ベクトル算出器５では、予め設定された送信機の位置情報から式（５４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、擬似距離差ｒｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）と方向余弦ベクトルＡを式（５１）に代入して、最小二乗測位解ｕｋ，ｍアンダーバーを算出する。送信機組み合わせ変更器３により送信機１の組み合わせを変えて、以上の測位演算と距離残差の算出をＭ回繰り返し、その測位解ｕｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）をそれぞれ測位解記憶器７に記憶する。
【０１３８】
予測器８では、１サンプリング前の平滑ベクトル（（ｘｋ−１アンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐｋ−１（＋）を遅延回路１１を介して入力し、式（３３）〜式（３５）に従い予測ベクトル（（ｘｋアンダーバー）ハット）（−）、予測誤差共分散行列Ｐｋ（−）、位置予測ベクトルｕｋアンダーバー（−）をそれぞれ算出する。
【０１３９】
予測残差確率密度算出器１２では、予測器８から予測誤差共分散行列Ｐｋ（−）を入力し、これと、予め設定された送信機１と受信機２の距離の観測誤差共分散行列とから、式（２７）及び式（４１）に従い、測位解の予測確率密度関数
Ｐ［ｕｋ，ｍアンダーバー｜Ｕｋ−１］の共分散行列Ｓｋを算出する。ゲート判定器１３では、予測器８から位置予測ベクトルｕｋアンダーバー（−）を、予測残差確率密度算出器１２から共分散行列Ｓｋを入力し、測位解記憶器７に記憶された測位解ｕｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）の中から、式（４２）を満たすものを選択し、満たさないものを排除する。
【０１４０】
測位解信頼度算出器１４では、予測器８で算出した位置予測ベクトルｕｋアンダーバー（−）と予測残差確率密度算出器１２で算出した共分散行列Ｓｋとをゲート判定器１３を介して入力し、式（４２）を満たす測位解ｕｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）の信頼度βｋ，ｍを、式（４４）に従い算出するとともに、この信頼度で測位解を重み付けした平均値ｕｋアンダーバーを式（４５）に従い算出する。
【０１４１】
平滑器１０では、測位解信頼度算出器１４で算出した測位解ｕｋアンダーバーと、予測器８で算出した予測ベクトル（（ｘｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐｋ（−）を用いて平滑ベクトル（（ｘｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐｋ（＋）を算出する。
【０１４２】
以上のようにして、位置が既知である複数の送信機１から送信された電波を、位置が未知である受信機２で受信して、受信機位置での各送信機１の信号間の伝播時間差または位相差を計測して受信機位置を測位する方式に本発明を応用しても、実施の形態２と同様に、受信機２の位置をより高精度に測位することができる。
【０１４３】
実施の形態９．
実施の形態３は、位置が既知である複数の送信機１からの電波を、位置が未知である受信機２で受信して、送信機１から受信機２までの伝播時間を計測して受信機位置を測位する方式において、電波の伝播時間を用いた測位方式に本発明を適用したものであるが、本発明は、複数の送信機１から受信機２への電波の伝播時間差や位相差を用いた測位方式にも適用できるので、本実施の形態においては、当該適用例について説明する。
【０１４４】
図９はこの実施の形態９による位置推定装置を示す構成図であり、図において、図３と同一符号である１〜３、５〜７、１３、１４は実施の形態３と同一または相当部分を示すのでその説明を省略する。
【０１４５】
１７は送信機組み合わせ変更器３で組み合わせた送信機１から受信機２への電波の伝播時間差もしくは位相差から、送受信機間の擬似距離差を算出する擬似距離差算出器、１８は最小二乗測位解算出器６で算出した距離差残差を記憶する距離差残差記憶器である。
【０１４６】
なお、複数の送信機１から受信機２への電波の伝播時間差や位相差を用いた測位方法は実施の形態７と同じである。また、距離差残差の２乗ノルムは式（５６）で表される。
【０１４７】
【数２７】
2005024535.tif 000029
【０１４８】
次に、実施の形態９による位置推定装置の具体的な動作を図９に従い説明する。複数の送信機１から発射された電波は、送信機１から受信機２への距離に基づく遅延時間後に受信機２に到達する。受信機２では各々の送信機１からの信号を弁別して受信する。送信機組み合わせ変更器３では、その全受信可能な送信機１の中から、測位に必要な所定の最低数以上、全発信機数以下の数で、適当な送信機１の組み合わせを選択する。擬似距離差算出器１７では、選択した組み合わせの２個の送信機１の信号の伝播時間差もしくは位相差から擬似距離差ｒｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）を算出する。
【０１４９】
方向余弦ベクトル算出器５では、予め設定された送信機の位置情報から式（５４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、擬似距離差ｒｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）と方向余弦ベクトルＡを式（５１）に代入して、最小二乗測位解ｕｋ，ｍアンダーバーを算出する。送信機組み合わせ変更器３により送信機の組み合わせを変えて、以上の測位演算と距離残差の算出をＭ回繰り返し、その測位解ｕｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）をそれぞれ測位解記憶器７に記憶する。
【０１５０】
距離残差記憶器１５では、最小二乗測位解算出器６から、擬似距離差ｒｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）と測位解ｆｉ，ｊ（ｘハット，ｙハット，ｚハット）を入力し、式（５６）に従い距離差残差の２乗ノルムρｍ２を算出し、これを記憶する。ゲート判定器１３では、距離残差記憶器１５から入力した距離残差の２乗ノルムρｍ２と、予め設定された距離差残差の確率分布の分散μ２とから、測位解記憶器７に記憶された測位解ｕｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）の中から、式（４７）を満たすものを選択し、満たさないものを排除する。
【０１５１】
測位解信頼度算出器１４では、距離差残差の２乗ノルムρｍ２と、予め設定された距離差残差の確率分布の分散μ２をゲート判定器１３を介して入力し、式（４７）を満たす全ての測位解に対し、測位解の信頼度βｋ，ｍを、式（４８）〜式（４９）に従い算出するとともに、この信頼度で測位解を重み付けした平均値ｕｋアンダーバーを受信機の推定位置として式（５０）に従い算出する。
【０１５２】
以上のようにして、位置が既知である複数の送信機１から送信された電波を、位置が未知である受信機２で受信して、受信機位置での各送信機１の信号間の伝播時間差または位相差を計測して受信機位置を測位する方式に本発明を応用しても、実施の形態３と同様に、受信機の位置をより高精度に測位することができる。
【０１５３】
実施の形態１０．
実施の形態４は、位置が未知である送信機１から送信された電波を、位置が既知である複数の受信機２で受信して、送信機１から受信機２までの伝播時間を計測して送信機位置を測位する方式において、電波の伝播時間を用いた測位方式に本発明を適用したものであるが、本発明は、複数の受信機から送信機への電波の伝播時間差や位相差を用いた測位方式にも適用できるので、本実施の形態においては、当該適用例について説明する。
【０１５４】
図１０はこの実施の形態１０による位置推定装置を示す構成図であり、図において、図４と同一符号である１、２、５〜１１、１６は実施の形態４と同一または相当部分を示すのでその説明を省略する。
【０１５５】
１７は受信機組み合わせ変更器１６で組み合わせた送信機から受信機への電波の伝播時間差もしくは位相差から、送受信機間の擬似距離差を算出する擬似距離差算出器である。
【０１５６】
なお、複数の送信機１から受信機２への電波の伝播時間差や位相差を用いた測位方法は実施の形態７と同じである。
【０１５７】
次に、実施の形態１０による位置推定装置の具体的な動作を図１０に従い説明する。送信機１から発射された電波は、各々の伝播経路の伝播時間後に複数の受信機２で送信機からの信号を受信する。受信機組み合わせ変更器１６では、その全受信可能な受信機２の中から、測位に必要な所定の最低数以上、全受信機数以下の数で、適当な受信機２の組み合わせを選択する。擬似距離差算出器１７では、選択した組み合わせの２個の送信機１の信号の伝播時間差もしくは位相差から擬似距離差ｒｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）を算出する。
【０１５８】
方向余弦ベクトル算出器５では、予め設定された送信機の位置情報から式（５４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、擬似距離差ｒｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）と方向余弦ベクトルＡを式（５１）に代入して、最小二乗測位解ｕｋ，ｍアンダーバーを算出する。受信機組み合わせ変更器１６により受信機の組み合わせを変えて、以上の測位演算と距離残差の算出をＭ回繰り返し、その測位解ｕｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）をそれぞれ測位解記憶器７に記憶する。
【０１５９】
予測器８では、１サンプリング前の平滑ベクトル（（ｘｋ−１アンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐｋ−１（＋）を遅延回路１１を介して入力し、式（３３）〜式（３５）に従い予測ベクトル（（ｘｋアンダーバー）ハット）（−）、予測誤差共分散行列Ｐｋ（−）、位置予測ベクトルｕｋアンダーバー（−）をそれぞれ算出する。測位解選択器９では、予測器８から位置予測ベクトルｕｋアンダーバー（−）を入力し、測位解記憶器７に記憶された測位解ｕｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）の中から、式（３６）で表される予測残差ｄｋ，ｍが最も小さい受信機の組み合わせの測位解ｕｋアンダーバーを選択して、最終的な測位解とする。
【０１６０】
平滑器１０では、測位解選択器で選択した測位解ｕｋアンダーバーと、予測器８で算出した予測ベクトル（（ｘｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐｋ（−）を用いて平滑ベクトル（（ｘｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐｋ（＋）を算出する。
【０１６１】
以上のようにして、位置が未知である送信機１から送信された電波を、位置が既知である複数の受信機２で受信して、受信機位置での送信機１の信号間の伝播時間差または位相差を計測して送信機位置を測位する方式に本発明を応用しても、実施の形態４と同様に、送信機１の位置をより高精度に測位することができる。
【０１６２】
実施の形態１１．
実施の形態５は、位置が未知である送信機１から送信された電波を、位置が既知である複数の受信機２で受信して、送信機１から受信機２までの伝播時間を計測して送信機位置を測位する方式において、電波の伝播時間を用いた測位方式に本発明を適用したものであるが、本発明は、複数の受信機２から送信機１への電波の伝播時間差や位相差を用いた測位方式にも適用できるので、本実施の形態においては、当該適用例について説明する。
【０１６３】
図１１はこの実施の形態１１による位置推定装置を示す構成図であり、図において、図５と同一符号である１、２、５〜８、１０〜１４、１６は実施の形態５と同一または相当部分を示すのでその説明を省略する。
【０１６４】
１７は送信機組み合わせ変更器３で組み合わせた送信機から受信機への電波の伝播時間差もしくは位相差から、送受信機間の擬似距離差を算出する擬似距離差算出器である。
【０１６５】
なお、複数の送信機１から受信機２への電波の伝播時間差や位相差を用いた測位方法は実施の形態７と同じである。
【０１６６】
次に、実施の形態１１による位置推定装置の具体的な動作を図１１に従い説明する。送信機１から発射された電波は、各々の伝播経路の伝播時間後に複数の受信機２で送信機１からの信号を受信する。受信機組み合わせ変更器１６では、その全受信可能な受信機２の中から、測位に必要な所定の最低数以上、全受信機数以下の数で、適当な受信機２の組み合わせを選択する。擬似距離差算出器１７では、選択した組み合わせの２個の送信機の信号の伝播時間差もしくは位相差から擬似距離差ｒｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）を算出する。
【０１６７】
方向余弦ベクトル算出器５では、予め設定された受信機の位置情報から式（５４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、擬似距離差ｒｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）と方向余弦ベクトルＡを式（５１）に代入して、最小二乗測位解ｕｋ，ｍアンダーバーを算出する。受信機組み合わせ変更器１６により受信機２の組み合わせを変えて、以上の測位演算と距離差残差の算出をＭ回繰り返し、その測位解ｕｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）をそれぞれ測位解記憶器７に記憶する。
【０１６８】
予測器８では、１サンプリング前の平滑ベクトル（（ｘｋ−１アンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐｋ−１（＋）を遅延回路１１を介して入力し、式（３３）〜式（３５）に従い予測ベクトル（（ｘｋアンダーバー）ハット）（−）、予測誤差共分散行列Ｐｋ（−）、位置予測ベクトルｕｋアンダーバー（−）をそれぞれ算出する。
【０１６９】
予測残差確率密度算出器１２では、予測器８から予測誤差共分散行列Ｐｋ（−）を入力し、これと、予め設定された送信機と受信機の距離の観測誤差共分散行列とから、式（２７）及び式（４１）に従い、測位解の予測確率密度関数
Ｐ［ｕｋ，ｍアンダーバー｜Ｕｋ−１］の共分散行列Ｓｋを算出する。ゲート判定器１３では、予測器８から位置予測ベクトルｕｋアンダーバー（−）を、予測残差確率密度算出器１２から共分散行列Ｓｋを入力し、測位解記憶器７に記憶された測位解ｕｋ，ｍ（ｍ＝１，２，・・・，Ｍ）の中から、式（４２）を満たすものを選択し、満たさないものを排除する。
【０１７０】
測位解信頼度算出器１４では、予測器８で算出した位置予測ベクトルｕｋアンダーバー（−）と予測残差確率密度算出器１２で算出した共分散行列Ｓｋとをゲート判定器１３を介して入力し、式（４２）を満たす測位解ｕｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）の信頼度βｋ，ｍを、式（４４）に従い算出するとともに、この信頼度で測位解を重み付けした平均値ｕｋアンダーバーを式（４５）に従い算出する。
【０１７１】
平滑器１０では、測位解信頼度算出器１４で算出した測位解ｕｋアンダーバーと、予測器８で算出した予測ベクトル（（ｘｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐｋ（−）を用いて平滑ベクトル（（ｘｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐｋ（＋）を算出する。
【０１７２】
以上のようにして、位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、受信機位置での送信機の信号間の伝播時間差または位相差を計測して送信機位置を測位する方式に本発明を応用しても、実施の形態５と同様に、送信機の位置をより高精度に測位することができる。
【０１７３】
実施の形態１２．
実施の形態６は、位置が未知である送信機１から送信された電波を、位置が既知である複数の受信機２で受信して、送信機１から受信機２までの伝播時間を計測して送信機位置を測位する方式において、電波の伝播時間を用いた測位方式に本発明を適用したものであるが、本発明は、複数の受信機から送信機への電波の伝播時間差や位相差を用いた測位方式にも適用できるので、本実施の形態においては、当該適用例について説明する。
【０１７４】
図１２はこの実施の形態１２による位置推定装置を示す構成図であり、図において、図６と同一符号である１、２、５〜７、１３、１４、１６は実施の形態６と同一または相当部分を示すのでその説明を省略する。
【０１７５】
１７は受信機組み合わせ変更器１６で組み合わせた送信機１から受信機２への電波の伝播時間差もしくは位相差から、送受信機間の擬似距離差を算出する擬似距離差算出器である。
【０１７６】
なお、複数の送信機１から受信機２への電波の伝播時間差や位相差を用いた測位方法は実施の形態７と同じである。また、距離差残差の２乗ノルムは実施の形態９と同じである。
【０１７７】
次に、実施の形態１２による位置推定装置の具体的な動作を図１２に従い説明する。送信機１から発射された電波は、各々の伝播経路の伝播時間後に複数の受信機２で送信機１からの信号を受信する。受信機組み合わせ変更器１６では、その全受信可能な受信機２の中から、測位に必要な所定の最低数以上、全受信機数以下の数で、適当な受信機２の組み合わせを選択する。擬似距離差算出器１７では、選択した組み合わせの２個の送信機１の信号の伝播時間差もしくは位相差から擬似距離差ｒｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）を算出する。
【０１７８】
方向余弦ベクトル算出器５では、予め設定された送信機の位置情報から式（５４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、擬似距離差ｒｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）と方向余弦ベクトルＡを式（５１）に代入して、最小二乗測位解ｕｋ，ｍアンダーバーを算出する。受信機組み合わせ変更器１６により受信機の組み合わせを変えて、以上の測位演算と距離残差の算出をＭ回繰り返し、その測位解ｕｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）をそれぞれ測位解記憶器７に記憶する。
【０１７９】
距離残差記憶器１５では、最小二乗測位解算出器６から、擬似距離差ｒｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）と測位解ｆｉ，ｊ（ｘハット，ｙハット，ｚハット）を入力し、式（５６）に従い距離差残差の２乗ノルムρｍ２を算出し、これを記憶する。ゲート判定器１３では、距離残差記憶器１５から入力した距離差残差の２乗ノルムρｍ２と、予め設定された距離差残差の確率分布の分散μ２とから、測位解記憶器７に記憶された測位解ｕｋ，ｍアンダーバー（ｍ＝１，２，・・・，Ｍ）の中から、式（４７）を満たすものを選択し、満たさないものを排除する。
【０１８０】
測位解信頼度算出器１４では、距離差残差の２乗ノルムρｍ２と、予め設定された距離差残差の確率分布の分散μ２をゲート判定器１３を介して入力し、式（４７）を満たす全ての測位解に対し、測位解の信頼度βｋ，ｍを、式（４８）〜式（４９）に従い算出するとともに、この信頼度で測位解を重み付けした平均値ｕｋアンダーバーを受信機の推定位置として式（５０）に従い算出する。
【０１８１】
以上のようにして、位置が未知である送信機から送信された電波を、位置が既知である複数の受信機で受信して、受信機位置での送信機の信号間の伝播時間差または位相差を計測して送信機位置を測位する方式に本発明を応用しても、実施の形態６と同様に、送信機の位置をより高精度に測位することができる。
【０１８２】
実施の形態１３．
図１３は、本実施の形態１３による位置推定装置を示す構成図であり、図において、図１と同一符号である１、２、４〜６、８、１０、１１は実施の形態１と同一または相当部分を示すのでその説明を省略する。
【０１８３】
１８は予測距離残差確率密度算出器２０の出力する予測距離残差の確率密度関数を入力し、受信機２の出力する各送信機と受信機との間の距離のうち測位に適する距離を選択して送信機を選択する距離による送信機選択器、１９は予測器８の出力する予測位置を各送信機と受信機との間の予測距離に変換する距離変換器、２０は距離変換器１９の出力する各送信機と受信機との間の予測距離を入力し、予測距離残差の確率密度関数を算出する予測距離残差確率密度算出器である。
【０１８４】
次に動作について説明する。
最初に、この実施の形態１３による位置推定装置の動作原理を説明する。なお、ここでは、３次元位置を推定する場合を仮定する。
【０１８５】
測位対象の運動を、過去の測位結果を用いて予測する方法は、実施の形態１と同じである。
【０１８６】
次に、マルチパス等の影響を受けていない測位に適した送信機１を判定する方法について述べる。実施の形態１では、送信機組み合わせ変更器３により送信機１の組み合わせを変えて算出した複数の測位解を判定及び選択の対象としているが、この実施の形態１３では、測位解を求める前の段階、すなわち、受信機２の出力する送受信機１、２間の距離を判定及び選択の対象とする。
【０１８７】
ｉ番目の送信機１に対する予測距離ｒｋ，ｉ（−）は、式（２９）より式（５７）で表される。もし、送信機１の一部がマルチパスの影響を受けていたり、伝播経路上に問題があると、その距離の予測残差は大きな値となる。そこで、予測距離と、ｉ番目の送信機１と受信機２の擬似距離ｒｋ，ｉ（ｉ＝１，２，・・・，Ｎ）との差である予測残差ｄ’ｋ，ｉがパラメータξに対して式（５８）を満たす擬似距離を測位の対象として選択する。これにより、送信機が正常で伝播経路上にも問題がない送信機の組み合わせを決定することができ、これらの擬似距離を用いて測位を行うことにより受信機２の位置をより高精度に推定することができる。
【０１８８】
【数２８】
2005024535.tif 000030
【０１８９】
一方、送信機を選択する別な方法として距離の予測残差の確率分布を利用する方法がある。過去の測位情報Ｕｋ−１より得られる距離残差の予測確率分布Ｐ［ｒｋ，ｉ｜Ｕｋ―１］は、式（５９）に示す条件付確率密度関数で表される。すなわち、距離残差は、式（５７）で与えられる予測距離ｒｋ，ｉ（−）を平均とし、式（６０）で与えられるＳ’ｋ，ｉを分散とする１次元正規分布ｇ１（ｒｋ，ｉ；ｒｋ，ｉ（−），Ｓ’ｋ，ｉ）に従うとする。そして、式（６１）を満たす擬似距離を測位に利用し、満たさない擬似距離を排除する。ここで、ｑ’は目標との相関範囲を決めるパラメータであり、自由度１のχ自乗分布により算出する。Ｈ２，ｉ（（（ｘｋアンダーバー）ハット）（−））は、３次元直交座標の誤差共分散をｉ番目の送信機１と受信機２の距離の分散に変換する行列であり、式（６２）及び式（６３）に示すように３次元直交座標の誤差を予測ベクトル（（ｘｋアンダーバー）ハット）（−）の周りで線形近似することによって得る。式（６３）の行列の要素の算出式は式（３０）〜式（３２）を参照する。
【０１９０】
【数２９】
2005024535.tif 000031
【０１９１】
以上、距離による送信機１の選択方法について述べた。
【０１９２】
次に、前述の方法で選択された送信機１の擬似距離を用いて、受信機２の位置を測位する。この原理は従来の技術における非特許文献１と同じであるため省略する。
【０１９３】
次に、前述の測位結果を用いて平滑ベクトルを算出する。この原理は実施の形態１と同じであるため省略する。
【０１９４】
以上、実施の形態１３による位置推定装置の動作原理について説明した。
【０１９５】
次に、実施の形態１３による位置推定装置の具体的な動作を図１３に従い説明する。
複数の送信機１から発射された電波は、送信機１から受信機２への距離に基づく遅延時間後に受信機２に到達する。受信機２では各々の送信機１からの信号を弁別して受信する。擬似距離算出器４では、送信機１の信号の伝播遅延時間から擬似距離ｒｋ，ｉ（ｉ＝１，２，・・・，Ｎ）を算出する。
【０１９６】
距離による送信機選択器１８では、予測距離ｒｋ，ｉ（−）と距離の予測残差の確率密度関数の分散Ｓ’ｋ，ｉを予測距離残差確率密度算出器２０から入力し、擬似距離ｒｋ，ｉ（ｉ＝１，２，・・・，Ｎ）の中から、式（６１）を満たすものを選択し、満たさないものを排除する。
【０１９７】
方向余弦ベクトル算出器５では、予め設定された送信機の位置情報から式（４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、距離による送信機選択器１８で選択された擬似距離ｒｋ，ｉと方向余弦ベクトルＡを式（１０）に代入して、最小二乗測位解ｕｋアンダーバーを算出する。
【０１９８】
平滑器１０では、最小二乗測位解算出器６で算出した測位解ｕｋアンダーバーと、予測器８で算出した予測ベクトル（（ｘｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐｋ（−）を用いて平滑ベクトル（ｘｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列（Ｐｋ（＋））を算出する。
【０１９９】
予測器８では、１サンプリング前の平滑ベクトル（ｘｋ−１アンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐｋ−１（＋）を遅延回路１１を介して入力し、式（３３）〜式（３５）に従い予測ベクトル（（ｘｋアンダーバー）ハット）（−）、予測誤差共分散行列Ｐｋ（−）、位置予測ベクトル（ｕｋアンダーバー）（−）をそれぞれ算出する。距離変換機１９では、予測器８で算出した予測ベクトル（（ｘｋアンダーバー）ハット）（−）を式（５７）に従い距離の予測ベクトルに変換する。
【０２００】
予測距離残差確率密度算出器２０では、予測器８から予測ベクトル（（ｘｋアンダーバー）ハット）（−）と予測誤差共分散行列Ｐｋ（−）を入力し、これと、予め設定された送信機１と受信機２の距離の観測誤差分散σｋ，ｉとから、式（６０）に従い、距離の予測残差の確率密度関数Ｐ［ｒｋ，ｉ｜Ｕｋ−１］の分散Ｓ’ｋ，ｉを算出する。
【０２０１】
以上のように、本実施の形態は、距離の予測残差もしくは距離の予測残差の確率密度関数を利用して選択した送信機１の擬似距離を用いて受信機２の位置を推定するので、動作が異常であったり伝播経路上に問題がある送信機の影響による測位精度の劣化を防ぐことができる。また、実施の形態１〜１２のように、複数の測角解を求める必要がないため演算負荷が軽い。
【０２０２】
実施の形態１４．
図１４は、本実施の形態１４による位置推定装置を示す構成図であり、図において、図１と同一符号である１、２、４〜６、８、１０、１１は実施の形態１と、図１３と同一符号である１８〜２０は実施の形態１３と同一または相当部分を示すのでその説明を省略する。
【０２０３】
２１は、距離による送信機選択器１８において選択された送信機の数によって、平滑する方法を切り替えるフィルタ切り替え器、２２は、距離の観測行列を算出する距離観測行列算出器、２３は、距離による送信機選択器１８の出力する擬似距離と、距離観測行列算出器２２の出力する距離の観測行列と、予測器の出力する前サンプリング時刻の予測ベクトルと予測誤差共分散行列とを入力し、平滑ベクトルを算出する距離による位置平滑器である。
【０２０４】
次に動作について説明する。
最初に、この実施の形態１４による位置推定装置の動作原理を説明する。なお、ここでは、３次元位置を推定する場合を仮定する。
【０２０５】
測位対象の運動を、過去の測位結果を用いて予測する方法は、実施の形態１と同じである。
【０２０６】
また、マルチパス等の影響を受けていない測位に適した送信機１を判定し、選択する方法は実施の形態１３と同じである。
【０２０７】
さて、上述の実施の形態１３では、測位に適した送信機１を選択した結果、その数が３次元測位を行う上で最低必要な４個未満の場合には測位できない。この場合は、選択された擬似距離を直接用いて平滑ベクトルを算出する。次にこの方法について説明する。
選択された送信機１に対応する擬似距離をｒｋ，ｉ（ｉ＝１，２，・・・，ｌ）とし、これを式（６４）に示すようにベクトル表記する。この場合、観測モデルは式（６５）で表される。
【０２０８】
【数３０】
2005024535.tif 000032
【０２０９】
式（６５）で表される距離の観測モデルは非線形であるため、通常のカルマンフィルタでは平滑ベクトルを算出できない。そこで、式（６７）及び式（６８）で示すように、この観測モデルを予測ベクトル（（ｘｋアンダーバー）ハット）（−）の周りで線形近似した拡張カルマンフィルタを利用する。
【０２１０】
【数３１】
2005024535.tif 000033
【０２１１】
拡張カルマンフィルタの理論より、ゲイン行列Ｋｋ、平滑ベクトル（（ｘｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐｋ（＋）は、式（６９）〜式（７１）で与えられる。以上、平滑ベクトルの算出方法について述べた。
【０２１２】
【数３２】
2005024535.tif 000034
【０２１３】
次に、送信機１を選択した結果、その数が４個以上の場合の処理について説明する。この場合、非特許文献１と同様に選択した送信機１の擬似距離を用いて、受信機２の位置を測位する。この原理は従来の技術における非特許文献１と同じであるため省略する。また、この測位結果を用いて平滑ベクトルを算出する原理は実施の形態１と同じであるため省略する。
【０２１４】
以上、実施の形態１４による位置推定装置の動作原理について説明した。
【０２１５】
次に、実施の形態１４による位置推定装置の具体的な動作を図１４に従い説明する。
複数の送信機１から発射された電波は、送信機１から受信機２への距離に基づく遅延時間後に受信機２に到達する。受信機２では各々の送信機１からの信号を弁別して受信する。擬似距離算出器４では、送信機１の信号の伝播遅延時間から擬似距離ｒｋ，ｉ（ｉ＝１，２，・・・，Ｎ）を算出する。
【０２１６】
距離による送信機選択器１８では、予測距離ｒｋ，ｉ（−）と距離の予測残差の確率密度関数の分散Ｓ’ｋ，ｉを予測距離残差確率密度算出器２０から入力し、擬似距離ｒｋ，ｉ（ｉ＝１，２，・・・，Ｎ）の中から、式（６１）を満たすものを選択し、満たさないものを排除する。
【０２１７】
フィルタ切り替え器２１では、距離による送信機選択器１８で送信機１を選択した結果、その数が測位を行う上で十分な数である４個以上の場合に方向余弦ベクトル算出器５に処理を移行し、４個未満の場合に距離観測行列算出器２２に処理を移行する。
【０２１８】
方向余弦ベクトル算出器５では、距離による送信機選択器１８で選択された送信機１の数が４個以上の場合に、予め設定された送信機の位置情報から式（４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、距離による送信機選択器１８で選択された擬似距離ｒｋ，ｉと方向余弦ベクトルＡを式（１０）に代入して、最小二乗測位解ｕｋアンダーバーを算出する。
【０２１９】
平滑器１０では、最小二乗測位解算出器６で算出した測位解ｕｋアンダーバーと、予測器８で算出した予測ベクトル（（ｘｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐｋ（−）を用いて平滑ベクトル（（ｘｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐｋ（＋）を算出する。
【０２２０】
距離観測行列算出器２２では、距離による送信機選択器１８で選択された送信機１の数が４個未満の場合に、予測器８で算出した予測ベクトル（（ｘｋアンダーバー）ハット）（−）から式（６８）に従い距離の観測行列Ｈ２（（（ｘｋアンダーバー）ハット）（−））を算出する。
【０２２１】
距離による位置平滑器２３では、距離による送信機選択器１８で選択された擬似距離ベクトルｒｋアンダーバーと、距離観測行列算出器２２で算出した距離の観測行列Ｈ２（（（ｘｋアンダーバー）ハット）（−））と、予測器８で算出した予測ベクトル（（ｘｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐｋ（−）を用いて平滑ベクトル（（ｘｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐｋ（＋）を算出する。
【０２２２】
予測器８では、距離による送信機選択器１８で選択された送信機１の数が４個以上の場合には平滑器１０で算出した遅延回路１１を介して入力し、一方、選択された送信機１の数が４個未満の場合には距離による位置平滑器２３で算出した１サンプリング前の平滑ベクトル（（ｘｋアンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐｋ−１（＋）を遅延回路１１を介して入力し、式（３３）〜式（３５）に従い予測ベクトル（（ｘｋアンダーバー）ハット）（−）、予測誤差共分散行列Ｐｋ（−）、位置予測ベクトル（ｕｋアンダーバー）（−）をそれぞれ算出する。距離変換機１９では、予測器８で算出した予測ベクトル（（ｘｋアンダーバー）ハット）（−）を式（５７）に従い距離の予測ベクトルに変換する。
【０２２３】
予測距離残差確率密度算出器２０では、予測器８から予測ベクトル（（ｘｋアンダーバー）ハット）（−）と予測誤差共分散行列Ｐｋ（−）を入力し、これと、予め設定された送信機１と受信機２の距離の観測誤差分散σｋ，ｉとから、式（６０）に従い、距離の予測残差の確率密度関数Ｐ［ｒｋ，ｉ｜Ｕｋ―１］の分散Ｓ’ｋ，ｉを算出する。
【０２２４】
以上のように本実施の形態は、距離の予測残差もしくは距離の予測残差の確率密度関数を利用して選択した送信機１の擬似距離を用いて受信機２の位置を推定するので、動作が異常であったり伝播経路上に問題がある送信機の影響による測位精度の劣化を防ぐことができる。また、実施の形態１〜１２のように、複数の測角解を求める必要がないため演算負荷が軽い。
更に、上述の実施の形態１３では、距離による送信機選択器１８において選択された送信機の数が４個未満の場合に測位ができない問題があるが、本実施例では距離による位置平滑器２３において、擬似距離を直接用いて位置を平滑する構成としているので、測位ができない場合においても、精度よく受信機の位置を推定することができる。
【０２２５】
実施の形態１５．
上記の実施の形態１３では、位置が既知である複数の送信機１からの電波を、位置が未知である受信機２で受信して、送信機１から受信機２までの伝播時間を計測して受信機位置を測位する方式への本発明の応用例を示したが、本実施の形態は、逆に、位置が未知である送信機１からの電波を、位置が既知である複数の受信機２で受信して、送信機１から受信機２までの伝播時間を計測して送信機位置を測位する方式へ本発明を適用した適用例について説明する。
【０２２６】
図１５はこの実施の形態１５による位置推定装置を示す構成図であり、図において、図１３と同一符号である１、２、４〜６、８、１０、１１、１９、２０は実施の形態１３と同一または相当部分を示すのでその説明を省略する。
【０２２７】
２４は予測距離残差確率密度算出器２０の出力する予測距離残差の確率密度関数を入力し、各受信機２の出力する送信機と各受信機の間の擬似距離のうち測位に適する擬似距離を選択する距離による受信機選択器である。
【０２２８】
次に、実施の形態１５による位置推定装置の具体的な動作を図１５に従い説明する。送信機１から発射された電波は、送信機１から受信機２への距離に基づく遅延時間後に受信機２に到達する。擬似距離算出器４では送信機１の信号の伝播遅延時間から擬似距離ｒｋ，ｉ（ｉ＝１，２，・・・，Ｎ）を算出する。
【０２２９】
距離による受信機選択器２４では、予測距離ｒｋ，ｉ（−）と距離の予測残差の確率密度関数の分散Ｓ’ｋ，ｉを予測距離残差確率密度算出器２０から入力し、擬似距離ｒｋ，ｉ（ｉ＝１，２，・・・，Ｎ）の中から、式（６１）を満たすものを選択し、満たさないものを排除する。
【０２３０】
方向余弦ベクトル算出器５では、予め設定された受信機の位置情報から式（４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、距離による送信機選択器１８で選択された擬似距離ｒｋ，ｉと方向余弦ベクトルＡを式（１０）に代入して、最小二乗測位解ｕｋアンダーバーを算出する。
【０２３１】
平滑器１０では、最小二乗測位解算出器６で算出した測位解ｕｋアンダーバーと、予測器８で算出した予測ベクトル（（ｘｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐｋ（−）を用いて平滑ベクトル（（ｘｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐｋ（＋）を算出する。
【０２３２】
予測器８では、１サンプリング前の平滑ベクトル（（ｘｋアンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐｋ―１（＋）を遅延回路１１を介して入力し、式（３３）〜式（３５）に従い予測ベクトル（（ｘｋアンダーバー）ハット）（−）、予測誤差共分散行列Ｐｋ（−）、位置予測ベクトル（ｕｋアンダーバー）（−）をそれぞれ算出する。距離変換機１９では、予測器８で算出した予測ベクトル（（ｘｋアンダーバー）ハット）（−）を式（５７）に従い距離の予測ベクトルに変換する。
【０２３３】
予測距離残差確率密度算出器２０では、予測器８から予測ベクトル（（ｘｋアンダーバー）ハット）（−）と予測誤差共分散行列Ｐｋ（−）を入力し、これと、予め設定された送信機１と受信機２の距離の観測誤差分散σｋ，ｉとから、式（６０）に従い、距離の予測残差の確率密度関数Ｐ［ｒｋ，ｉ｜Ｕｋ―１］の分散Ｓ’ｋ，ｉを算出する。
【０２３４】
以上のように、位置が未知である送信機１からの電波を、位置が既知である複数の受信機２で受信して、送信機１から受信機２までの伝播時間を計測して送信機位置を測位する方式に本発明を応用しても、実施の形態１３と同様に、送信機１の位置をより高精度に測位することができる。
【０２３５】
実施の形態１６．
上記の実施の形態１４では、位置が既知である複数の送信機１からの電波を、位置が未知である受信機２で受信して、送信機１から受信機２までの伝播時間を計測して受信機位置を測位する方式への応用例であるが、本発明は、位置が未知である送信機１からの電波を、位置が既知である複数の受信機２で受信して、送信機１から受信機２までの伝播時間を計測して送信機位置を測位する方式にも適用できるので、本実施の形態においては、当該適用例について説明する。
【０２３６】
図１６はこの実施の形態１６による位置推定装置を示す構成図であり、図において、図１４と同一符号である１、２、４〜６、８、１０、１１、１９〜２３は実施の形態１４と同一または相当部分を示すのでその説明を省略する。
【０２３７】
２４は予測距離残差確率密度算出器２０の出力する予測距離残差の確率密度関数を入力し、各受信機２の出力する送信機と各受信機の間の擬似距離のうち測位に適する擬似距離を選択する距離による受信機選択器である。
【０２３８】
次に、実施の形態１６による位置推定装置の具体的な動作を図１６に従い説明する。送信機１から発射された電波は、送信機１から受信機２への距離に基づく遅延時間後に受信機２に到達する。擬似距離算出器４では送信機１の信号の伝播遅延時間から擬似距離ｒｋ，ｉ（ｉ＝１，２，・・・，Ｎ）を算出する。
【０２３９】
距離による受信機選択器２４では、予測距離ｒｋ，ｉ（−）と距離の予測残差の確率密度関数の分散Ｓ’ｋ，ｉを予測距離残差確率密度算出器２０から入力し、擬似距離ｒｋ，ｉ（ｉ＝１，２，・・・，Ｎ）の中から、式（６１）を満たすものを選択し、満たさないものを排除する。
【０２４０】
フィルタ切り替え器２１では、距離による受信機選択器２４で受信機２を選択した結果、その数が測位を行う上で十分な数である４個以上の場合に方向余弦ベクトル算出器５に処理を移行し、４個未満の場合に距離観測行列算出器２２に処理を移行する。
【０２４１】
方向余弦ベクトル算出器５では、距離による受信機選択器２４で選択された受信機２の数が４個以上の場合に、予め設定された受信機の位置情報から式（４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、距離による受信機選択器２４で選択された擬似距離ｒｋ，ｉと方向余弦ベクトルＡを式（１０）に代入して、最小二乗測位解ｕｋアンダーバーを算出する。
【０２４２】
平滑器１０では、最小二乗測位解算出器６で算出した測位解ｕｋアンダーバーと、予測器８で算出した予測ベクトル（（ｘｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐｋ（−）を用いて平滑ベクトル（（ｘｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐｋ（＋）を算出する。
【０２４３】
距離観測行列算出器２２では、距離による受信機選択器２４で選択された受信機２の数が４個未満の場合に、予測器８で算出した予測ベクトル（（ｘｋアンダーバー）ハット）（−）から式（６８）に従い距離の観測行列Ｈ２（（（ｘｋアンダーバー）ハット）（−））を算出する。
【０２４４】
距離による位置平滑器２３では、距離による受信機選択器２４で選択された擬似距離ベクトルｒｋアンダーバーと、距離観測行列算出器２２で算出した距離の観測行列Ｈ２（（（ｘｋアンダーバー）ハット）（−））と、予測器８で算出した予測ベクトル（（ｘｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐｋ（−）を用いて平滑ベクトル（（ｘｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐｋ（＋）を算出する。
【０２４５】
予測器８では、距離による受信機選択器２４で選択された受信機２の数が４個以上の場合には平滑器１０で算出した遅延回路１１を介して入力し、一方、選択された受信機２の数が４個未満の場合には距離による位置平滑器２３で算出した１サンプリング前の平滑ベクトル（（ｘｋアンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐｋ―１（＋）を遅延回路１１を介して入力し、式（３３）〜式（３５）に従い予測ベクトル（（ｘｋアンダーバー）ハット）（−）、予測誤差共分散行列Ｐｋ（−）、位置予測ベクトルｕｋアンダーバー（−）をそれぞれ算出する。距離変換機１９では、予測器８で算出した予測ベクトル（（ｘｋアンダーバー）ハット）（−）を式（５７）に従い距離の予測ベクトルに変換する。
【０２４６】
予測距離残差確率密度算出器２０では、予測器８から予測ベクトル（（ｘｋアンダーバー）ハット）（−）と予測誤差共分散行列Ｐｋ（−）を入力し、これと、予め設定された送信機１と受信機２の距離の観測誤差分散σｋ，ｉとから、式（６０）に従い、距離の予測残差の確率密度関数Ｐ［ｒｋ，ｉ｜Ｕｋ―１］の分散Ｓ’ｋ，ｉを算出する。
【０２４７】
以上のように、位置が未知である送信機１からの電波を、位置が既知である複数の受信機２で受信して、送信機１から受信機２までの伝播時間を計測して送信機位置を測位する方式に本発明を応用しても、実施の形態１４と同様に、送信機１の位置をより高精度に測位することができる。
【０２４８】
実施の形態１７．
上述の実施の形態１３は、位置が既知である複数の送信機１からの電波を、位置が未知である受信機２で受信して、送信機１から受信機２までの伝播時間を計測して受信機位置を測位する方式において、電波の伝播時間を用いた測位方式に本発明を適用したものであるが、本発明は、複数の送信機１から受信機２への電波の伝播時間差や位相差を用いた測位方式にも適用できるので、本実施の形態においては、当該適用例について説明する。
【０２４９】
図１７はこの実施の形態１７による位置推定装置を示す構成図であり、図において、図１３と同一符号である１、２、５、６、８、１０、１１、１７は実施の形態１３と同一または相当部分を示すのでその説明を省略する。
【０２５０】
２５は予測距離差残差確率密度算出器２７の出力する予測距離差残差の確率密度関数を入力し、受信機２の出力する各送信機と受信機の間の擬似距離差のうち測位に適する擬似距離差を選択する距離差による送信機選択器、２６は予測器８の出力する予測位置を各送信機と受信機の予測距離差に変換する距離差変換器、２７は距離差変換器２６の出力する各送信機と受信機の予測距離差を入力し、予測距離差残差の確率密度関数を算出する予測距離差残差確率密度算出器である。
【０２５１】
複数の送信機１から受信機２への電波の伝播時間差や位相差を用いてマルチパス等の影響を受けていない測位に適した送信機１を判定する方法について述べる。
【０２５２】
ｉ番目の送信機１に対する予測距離差Δｒｋ，ｉ，ｊ（−）は既知であるｉ番目の送信機位置（ｘｉ，ｙｉ，ｚｉ）と、同じく既知であるｊ番目の発信機位置（ｘｊ，ｙｊ，ｚｊ）から、式（７２）を用いて算出する。もし、送信機１の一部がマルチパスの影響を受けていたり、伝播経路上に問題があると、その距離差の予測残差は大きな値となる。そこで、予測距離差と、ｉ番目の送信機１と受信機２の擬似距離差Δｒｋ，ｉ，ｊ（ｉ＝１，２，・・・，Ｎ）との差である予測残差τｋ，ｉがパラメータηに対して式（７３）を満たす擬似距離差を測位の対象として選択する。これにより、送信機が正常で伝播経路上にも問題がない送信機の組み合わせを決定することができ、これらの擬似距離差を用いて測位を行うことにより受信機２の位置をより高精度に推定することができる。
【０２５３】
【数３３】
2005024535.tif 000035
【０２５４】
一方、送信機を選択する別な方法として距離差の予測残差の確率分布を利用する方法がある。過去の測位情報Ｕｋ−１より得られる距離差残差の予測確率分布Ｐ［Δｒｋ，ｉ、ｊ｜Ｕｋ―１］は、式（７４）に示す条件付確率密度関数で表される。すなわち、距離差残差は、式（７２）で与えられる予測距離差Δｒｋ，ｉ、ｊ（−）を平均とし、式（７５）で与えられるΩｋ，ｉ，ｊを分散とする１次元正規分布ｇ１（Δｒｋ，ｉ、ｊ；Δｒｋ，ｉ、ｊ（−），Ωｋ，ｉ，ｊ）に従うとする。そして、式（７６）を満たす擬似距離差を測位に利用し、満たさない擬似距離差を排除する。ここで、κは目標との相関範囲を決めるパラメータであり、自由度１のχ自乗分布により算出する。Ｈ３，ｉ，ｊ（（（ｘｋアンダーバー）ハット）（−））は、３次元直交座標の誤差共分散をｉ番目の送信機と受信機２の距離と、ｊ番目の送信機と受信機２の距離との差の分散に変換する行列であり、式（７７）及び式（７８）に示すように３次元直交座標の誤差を予測ベクトル（（ｘｋアンダーバー）ハット）（−）の周りで線形近似することによって得る。
【０２５５】
【数３４】
2005024535.tif 000036
【０２５６】
以上、距離差による送信機１の選択方法について述べた。
【０２５７】
なお、前述の方法で選択された２つの送信機と受信機２の擬似距離差を用いて、受信機２の位置を測位する原理は実施の形態１７と同じ原理であるため省略する。
【０２５８】
次に、実施の形態１７による位置推定装置の具体的な動作を図１７に従い説明する。
複数の送信機１から発射された電波は、送信機１から受信機２への距離に基づく遅延時間後に受信機２に到達する。受信機２では各々の送信機１からの信号を弁別して受信する。擬似距離差算出器１７では、２つの送信機１の信号の伝播時間差もしくは位相差から擬似距離差Δｒｋ，ｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）を算出する。
【０２５９】
距離差による送信機選択器２５では、予測距離差Δｒｋ，ｉ，ｊ（−）と擬似距離差の予測残差の確率密度関数の分散Ωｋ，ｉ，ｊを予測距離差残差確率密度算出器２７から入力し、擬似距離差Δｒｋ，ｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）の中から、式（７６）を満たすものを選択し、満たさないものを排除する。
【０２６０】
方向余弦ベクトル算出器５では、予め設定された送信機の位置情報から式（５４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、距離差による送信機選択器２５で選択された擬似距離差Δｒｋ，ｉ，ｊと方向余弦ベクトルＡを式（５１）に代入して、最小二乗測位解ｕｋアンダーバーを算出する。
【０２６１】
平滑器１０では、最小二乗測位解算出器６で算出した測位解ｕｋアンダーバーと、予測器８で算出した予測ベクトル（（ｘｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐｋ（−）を用いて、式（３７）〜式（３９）に従い平滑ベクトル（（ｘｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐｋ（＋）を算出する。
【０２６２】
予測器８では、１サンプリング前の平滑ベクトル（（ｘｋアンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐｋ−１（＋）を遅延回路１１を介して入力し、式（３３）〜式（３５）に従い予測ベクトル（（ｘｋアンダーバー）ハット）（−）、予測誤差共分散行列Ｐｋ（−）、位置予測ベクトル（ｕｋアンダーバー）（−）をそれぞれ算出する。距離差変換機２６では、予測器８で算出した予測ベクトル（（ｘｋアンダーバー）ハット）（−）を式（７２）に従い距離差の予測ベクトルに変換する。
【０２６３】
予測距離差残差確率密度算出器２７では、予測器８から予測ベクトル（（ｘｋアンダーバー）ハット）（−）と予測誤差共分散行列Ｐｋ（−）を入力し、これと、予め設定された２つの送信機と受信機２の距離差の観測誤差分散σ’ｋ，ｉ，ｊとから、式（７５）に従い、距離差の予測残差の確率密度関数Ｐ［Δｒｋ，ｉ，ｊ｜Ｕｋ―１］の分散Ωｋ，ｉを算出する。
【０２６４】
以上のようにして、位置が既知である複数の送信機１から送信された電波を、位置が未知である受信機２で受信して、受信機位置での各送信機の信号間の伝播時間差または位相差を計測して受信機位置を測位する方式に本発明を応用しても、実施の形態１３と同様に、受信機２の位置をより高精度に測位することができる。
【０２６５】
実施の形態１８．
上記の実施の形態１４は、位置が既知である複数の送信機１からの電波を、位置が未知である受信機２で受信して、送信機１から受信機２までの伝播時間を計測して受信機位置を測位する方式において、電波の伝播時間を用いた測位方式に本発明を適用したものであるが、本発明は、複数の送信機１から受信機２への電波の伝播時間差や位相差を用いた測位方式にも適用できるので、本実施の形態においては、当該適用例について説明する。
【０２６６】
図１８はこの実施の形態１８による位置推定装置を示す構成図であり、図において、図１４と同一符号である１、２、５、６、８、１０、１１、１７は実施の形態１３と同一または相当部分を示すのでその説明を省略する。
【０２６７】
２５は予測距離差残差確率密度算出器２７の出力する予測距離差残差の確率密度関数を入力し、受信機２の出力する各送信機と受信機の間の擬似距離差のうち測位に適する擬似距離差を選択する距離差による送信機選択器、２６は予測器８の出力する予測位置を各送信機と受信機の予測距離差に変換する距離差変換器、２７は距離差変換器２６の出力する各送信機と受信機の予測距離差を入力し、予測距離差残差の確率密度関数を算出する予測距離差残差確率密度算出器、２８は距離差の観測行列を算出する距離差観測行列算出器、２９は距離差による送信機選択器２５の出力する擬似距離差と、距離差観測行列算出器２８の出力する距離差の観測行列と、予測器８の出力する前サンプリング時刻の予測ベクトルと予測誤差共分散行列とを入力し、平滑ベクトルを算出する距離差による位置平滑器である。
【０２６８】
距離差による送信機１の選択方法、及び、選択された２つの送信機と受信機２の擬似距離差を用いて、受信機２の位置を測位する原理は実施の形態１７と同じ原理であるため省略する。
【０２６９】
次に距離差の観測行列の算出方法及び距離差による位置平滑器の動作原理について説明する。
選択された送信機１に対応する擬似距離差をΔｒｋ，ｉ，ｊ（ｉ，ｊ＝１，２，・・・，ｌ）とし、これを式（７９）に示すようにベクトル表記する。この場合、観測モデルは式（８０）で表される。
【０２７０】
【数３５】
2005024535.tif 000037
【０２７１】
式（８０）で表される距離差の観測モデルは非線形であるため、通常のカルマンフィルタでは平滑ベクトルを算出できない。そこで、式（８２）及び式（８３）で示すように、この観測モデルを予測ベクトル（（ｘｋアンダーバー）ハット）（−）の周りで線形近似した拡張カルマンフィルタを利用する。
【０２７２】
【数３６】
2005024535.tif 000038
【０２７３】
拡張カルマンフィルタの理論より、ゲイン行列Ｋｋ、平滑ベクトル（（ｘｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐｋ（＋）は、式（８４）〜式（８６）で与えられる。以上、平滑ベクトルの算出方法について述べた。
【０２７４】
【数３７】
2005024535.tif 000039
【０２７５】
次に、実施の形態１８による位置推定装置の具体的な動作を図１８に従い説明する。
複数の送信機１から発射された電波は、送信機１から受信機２への距離に基づく遅延時間後に受信機２に到達する。受信機２では各々の送信機１からの信号を弁別して受信する。擬似距離差算出器１７では、２つの送信機１の信号の伝播時間差もしくは位相差から擬似距離差Δｒｋ，ｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）を算出する。
【０２７６】
距離差による送信機選択器２５では、予測距離差Δｒｋ，ｉ，ｊ（−）と擬似距離差の予測残差の確率密度関数の分散Ωｋ，ｉ，ｊを予測距離差残差確率密度算出器２７から入力し、擬似距離差Δｒｋ，ｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）の中から、式（７６）を満たすものを選択し、満たさないものを排除する。
【０２７７】
フィルタ切り替え器２１では、距離差による送信機選択器２５で送信機１のペアを選択した結果、そのペアの数が測位を行う上で十分な数である４個以上の場合に方向余弦ベクトル算出器５に処理を移行し、４個未満の場合に距離差観測行列算出器２８に処理を移行する。
【０２７８】
方向余弦ベクトル算出器５では、距離差による送信機選択器２５で選択された送信機１の数が４個以上の場合に、予め設定された送信機の位置情報から式（５４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、距離差による送信機選択器２５で選択された擬似距離差Δｒｋ，ｉ，ｊと方向余弦ベクトルＡを式（５１）に代入して、最小二乗測位解ｕｋアンダーバーを算出する。
【０２７９】
平滑器１０では、最小二乗測位解算出器６で算出した測位解ｕｋアンダーバーと、予測器８で算出した予測ベクトル（（ｘｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐｋ（−）を用いて、式（３７）〜式（３９）に従い平滑ベクトル（（ｘｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐｋ（＋）を算出する。
【０２８０】
距離差観測行列算出器２８では、距離差による送信機選択器２５で選択された送信機１の数が４個未満の場合に、予測器８で算出した予測ベクトル（（ｘｋアンダーバー）ハット）（−）から式（８３）に従い距離差の観測行列Ｈ４（（（ｘｋアンダーバー）ハット）（−））を算出する。
【０２８１】
距離差による位置平滑器２９では、距離差による送信機選択器２５で選択された擬似距離差ベクトルΔｒｋアンダーバーと、距離差観測行列算出器２８で算出した距離差の観測行列Ｈ４（（（ｘｋアンダーバー）ハット）（−））と、予測器８で算出した予測ベクトル（（ｘｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐｋ（−）を用いて、式（８４）〜式（８６）に従い平滑ベクトル（（ｘｋアンダーバー）ハット）（＋））及び平滑誤差共分散行列Ｐｋ（＋）を算出する。
【０２８２】
予測器８では、距離差による送信機選択器２５で選択された送信機１の数が４個以上の場合には平滑器１０で算出した遅延回路１１を介して入力し、一方、選択された送信機１の数が４個未満の場合には距離差による位置平滑器２９で算出した１サンプリング前の平滑ベクトル（（ｘｋアンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐｋ―１（＋）を遅延回路１１を介して入力し、式（３３）〜式（３５）に従い予測ベクトル（（ｘｋアンダーバー）ハット）（−）、予測誤差共分散行列Ｐｋ（−）、位置予測ベクトルｕｋアンダーバー（−）をそれぞれ算出する。距離差変換機２６では、予測器８で算出した予測ベクトル（（ｘｋアンダーバー）ハット）（−）を式（７２）に従い距離差の予測ベクトルに変換する。
【０２８３】
予測距離差残差確率密度算出器２７では、予測器８から予測ベクトル（（ｘｋアンダーバー）ハット）（−）と予測誤差共分散行列Ｐｋ（−）を入力し、これと、予め設定された２つの送信機と受信機２の距離差の観測誤差分散σ’ｋ，ｉ，ｊとから、式（７５）に従い、距離差の予測残差の確率密度関数Ｐ［Δｒｋ，ｉ，ｊ｜Ｕｋ―１］の分散Ωｋ，ｉを算出する。
【０２８４】
以上のようにして、位置が既知である複数の送信機１から送信された電波を、位置が未知である受信機２で受信して、受信機位置での各送信機の信号間の伝播時間差または位相差を計測して受信機位置を測位する方式に本発明を応用しても、実施の形態１４と同様に、受信機２の位置をより高精度に測位することができる。
【０２８５】
実施の形態１９．
上記の実施の形態１７では、位置が既知である複数の送信機１からの電波を、位置が未知である受信機２で受信して、送信機１から受信機２への電波の伝播時間差や位相差を計測して受信機位置を測位する方式への本発明の応用例を示したが、本実施の形態は、逆に、位置が未知である送信機１からの電波を、位置が既知である複数の受信機２で受信して、送信機１から受信機２送信機１から受信機２への電波の伝播時間差や位相差を計測して送信機位置を測位する方式へ本発明を適用した適用例について説明する。
【０２８６】
図１９はこの実施の形態１９による位置推定装置を示す構成図であり、図において、図１７と同一符号である１、２、５、６、８、１０、１１、１７、２６、２７は実施の形態１７と同一または相当部分を示すのでその説明を省略する。
【０２８７】
３０は予測距離差残差確率密度算出器２７の出力する予測距離差残差の確率密度関数を入力し、各受信機２の出力する送信機１と受信機２の擬似距離差のうち測位に適する擬似距離差を選択する距離差による受信機選択器である。
【０２８８】
次に、実施の形態１９による位置推定装置の具体的な動作を図１９に従い説明する。送信機１から発射された電波は、送信機１から受信機２への距離に基づく遅延時間後に受信機２に到達する。擬似距離差算出器１７では、２つの送信機１の信号の伝播時間差もしくは位相差から擬似距離差Δｒｋ，ｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）を算出する。
【０２８９】
距離差による受信機選択器３０では、予測距離差Δｒｋ，ｉ，ｊ（−）と擬似距離差の予測残差の確率密度関数の分散Ωｋ，ｉ，ｊを予測距離差残差確率密度算出器２７から入力し、擬似距離差Δｒｋ，ｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）の中から、式（７６）を満たすものを選択し、満たさないものを排除する。
【０２９０】
方向余弦ベクトル算出器５では、予め設定された受信機の位置情報から式（５４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、距離差による受信機選択器３０で選択された擬似距離差Δｒｋ，ｉ，ｊと方向余弦ベクトルＡを式（５１）に代入して、最小二乗測位解ｕｋアンダーバーを算出する。
【０２９１】
平滑器１０では、最小二乗測位解算出器６で算出した測位解ｕｋアンダーバーと、予測器８で算出した予測ベクトル（（ｘｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐｋ（−）を用いて、式（３７）〜式（３９）に従い平滑ベクトル（（ｘｋアンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐｋ（＋）を算出する。
【０２９２】
予測器８では、１サンプリング前の平滑ベクトル（（ｘｋ−１アンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐｋ−１（＋）を遅延回路１１を介して入力し、式（３３）〜式（３５）に従い予測ベクトル（（ｘｋアンダーバー）ハット）（−）、予測誤差共分散行列Ｐｋ（−）、位置予測ベクトル（（ｕｋアンダーバー）ハット）（−）をそれぞれ算出する。距離差変換機２６では、予測器８で算出した予測ベクトル（（ｘｋアンダーバー）ハット）（−）を式（７２）に従い距離差の予測ベクトルに変換する。
【０２９３】
予測距離差残差確率密度算出器２７では、予測器８から予測ベクトル（（ｘｋアンダーバー）ハット）（−）と予測誤差共分散行列Ｐｋ（−）を入力し、これと、予め設定された２つの送信機と受信機２の距離差の観測誤差分散σ’ｋ，ｉ，ｊとから、式（７５）に従い、距離差の予測残差の確率密度関数Ｐ［Δｒｋ，ｉ，ｊ｜Ｕｋ―１］の分散Ωｋ，ｉを算出する。
【０２９４】
以上のようにして、位置が未知である送信機１から送信された電波を、位置が既知である複数の受信機２で受信して、受信機位置での送信機１の信号間の伝播時間差または位相差を計測して送信機位置を測位する方式に本発明を応用しても、実施の形態１７と同様に、送信機１の位置をより高精度に測位することができる。
【０２９５】
実施の形態２０．
上記の実施の形態１８では、位置が既知である複数の送信機１からの電波を、位置が未知である受信機２で受信して、送信機１から受信機２までの伝播時間を計測して受信機位置を測位する方式への応用例であるが、本実施の形態は、逆に、位置が未知である送信機１からの電波を、位置が既知である複数の受信機２で受信して、送信機１から受信機２送信機１から受信機２への電波の伝播時間差や位相差を計測して送信機位置を測位する方式へ本発明を適用した適用例について説明する。
【０２９６】
図２０はこの実施の形態２０による位置推定装置を示す構成図であり、図において、図１８と同一符号である１、２、５、６、８、１０、１１、１７、２６〜２９は実施の形態１８と同一または相当部分を示すのでその説明を省略する。
【０２９７】
３０は予測距離差残差確率密度算出器２７の出力する予測距離差残差の確率密度関数を入力し、各受信機２の出力する送信機と各受信機の間の擬似距離差のうち測位に適する擬似距離差を選択する距離差による受信機選択器である。
【０２９８】
次に、実施の形態２０による位置推定装置の具体的な動作を図２０に従い説明する。送信機１から発射された電波は、送信機１から受信機２への距離に基づく遅延時間後に受信機２に到達する。擬似距離差算出器１７では、２つの送信機１の信号の伝播時間差もしくは位相差から擬似距離差Δｒｋ，ｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）を算出する。
【０２９９】
距離差による受信機選択器３０では、予測距離差Δｒｋ，ｉ，ｊ（−）と擬似距離差の予測残差の確率密度関数の分散Ωｋ，ｉ，ｊを予測距離差残差確率密度算出器２７から入力し、擬似距離差Δｒｋ，ｉ，ｊ（ｉ，ｊ＝１，２，・・・，Ｎ）の中から、式（７６）を満たすものを選択し、満たさないものを排除する。
【０３００】
フィルタ切り替え器２１では、距離差による受信機選択器３０で受信機１のペアを選択した結果、そのペアの数が測位を行う上で十分な数である４個以上の場合に方向余弦ベクトル算出器５に処理を移行し、４個未満の場合に距離差観測行列算出器２８に処理を移行する。
【０３０１】
方向余弦ベクトル算出器５では、距離差による受信機選択器３０で選択された受信機２の数が４個以上の場合に、予め設定された送信機の位置情報から式（５４）に従い、方向余弦ベクトルＡを算出する。最小二乗測位解算出器６では、距離差による受信機選択器３０で選択された擬似距離差Δｒｋ，ｉ，ｊと方向余弦ベクトルＡを式（５１）に代入して、最小二乗測位解ｕｋアンダーバーを算出する。
【０３０２】
平滑器１０では、最小二乗測位解算出器６で算出した測位解と、予測器８で算出した予測ベクトル（（ｘｋアンダーバー）ハット）（−）と予測誤差共分散行列Ｐｋ（−）を用いて、式（３７）〜式（３９）に従い、平滑ベクトル（（ｘｋアンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐｋ（＋）を算出する。
【０３０３】
距離差観測行列算出器２８では、距離差による受信機選択器３０で選択された受信機２の数が４個未満の場合に、予測器８で算出した予測ベクトル（（ｘｋアンダーバー）ハット）（−）から式（８３）に従い距離差の観測行列Ｈ４（（（ｘｋアンダーバー）ハット）（−））を算出する。
【０３０４】
距離差による位置平滑器２９では、距離差による受信機選択器３０で選択された擬似距離差ベクトルΔｒｋアンダーバーと、距離差観測行列算出器２８で算出した距離差の観測行列Ｈ４（（（ｘｋアンダーバー）ハット）（−））と、予測器８で算出した予測ベクトル（（ｘｋアンダーバー）ハット）（−）及び予測誤差共分散行列Ｐｋ（−）を用いて、式（８４）〜式（８６）に従い、平滑ベクトル（（ｘｋアンダーバー）ハット）（＋）及び平滑誤差共分散行列Ｐｋ（＋）を算出する。
【０３０５】
予測器８では、距離差による受信機選択器３０で選択された受信機２の数が４個以上の場合には平滑器１０で算出した遅延回路１１を介して入力し、一方、選択された受信機２の数が４個未満の場合には距離差による位置平滑器２９で算出した１サンプリング前の平滑ベクトル（（ｘｋ−１アンダーバー）ハット）（＋）と平滑誤差共分散行列Ｐｋ−１（＋）を遅延回路１１を介して入力し、式（３３）〜式（３５）に従い予測ベクトル（（ｘｋアンダーバー）ハット）（−）、予測誤差共分散行列Ｐｋ（−）、位置予測ベクトル（ｕｋアンダーバー）（−）をそれぞれ算出する。距離差変換機２６では、予測器８で算出した予測ベクトル（（ｘｋアンダーバー）ハット）（−）を式（７２）に従い距離差の予測ベクトルに変換する。
【０３０６】
予測距離差残差確率密度算出器２７では、予測器８から予測ベクトル（（ｘｋアンダーバー）ハット）（−）と予測誤差共分散行列Ｐｋ（−）を入力し、これと、予め設定された２つの送信機と受信機２の距離差の観測誤差分散σ’ｋ，ｉ，ｊとから、式（７５）に従い、距離差の予測残差の確率密度関数Ｐ［Δｒｋ，ｉ，ｊ｜Ｕｋ―１］の分散Ωｋ，ｉを算出する。
【０３０７】
以上のようにして、位置が未知である送信機１から送信された電波を、位置が既知である複数の受信機２で受信して、受信機位置での送信機１の信号間の伝播時間差または位相差を計測して送信機位置を測位する方式に本発明を応用しても、実施の形態１８と同様に、送信機１の位置をより高精度に測位することができる。
【図面の簡単な説明】
【０３０８】
【図１】この発明の実施の形態１に係る位置推定装置の構成を示したブロック図である。
【図２】この発明の実施の形態２に係る位置推定装置の構成を示したブロック図である。
【図３】この発明の実施の形態３に係る位置推定装置の構成を示したブロック図である。
【図４】この発明の実施の形態４に係る位置推定装置の構成を示したブロック図である。
【図５】この発明の実施の形態５に係る位置推定装置の構成を示したブロック図である。
【図６】この発明の実施の形態６に係る位置推定装置の構成を示したブロック図である。
【図７】この発明の実施の形態７に係る位置推定装置の構成を示したブロック図である。
【図８】この発明の実施の形態８に係る位置推定装置の構成を示したブロック図である。
【図９】この発明の実施の形態９に係る位置推定装置の構成を示したブロック図である。
【図１０】この発明の実施の形態１０に係る位置推定装置の構成を示したブロック図である。
【図１１】この発明の実施の形態１１に係る位置推定装置の構成を示したブロック図である。
【図１２】この発明の実施の形態１２に係る位置推定装置の構成を示したブロック図である。
【図１３】この発明の実施の形態１３に係る位置推定装置の構成を示したブロック図である。
【図１４】この発明の実施の形態１４に係る位置推定装置の構成を示したブロック図である。
【図１５】この発明の実施の形態１５に係る位置推定装置の構成を示したブロック図である。
【図１６】この発明の実施の形態１６に係る位置推定装置の構成を示したブロック図である。
【図１７】この発明の実施の形態１７に係る位置推定装置の構成を示したブロック図である。
【図１８】この発明の実施の形態１８に係る位置推定装置の構成を示したブロック図である。
【図１９】この発明の実施の形態１９に係る位置推定装置の構成を示したブロック図である。
【図２０】この発明の実施の形態２０に係る位置推定装置の構成を示したブロック図である。
【符号の説明】
【０３０９】
１  送信機、２  受信機、３  送信機組み合わせ変更器、４  疑似距離算出器、５  方向余弦ベクトル算出器、６  最小二乗測位解算出器、７  測位解記憶器、８  予測器、９  測位解選択器、１０  平滑器、１１  遅延回路、１２  予測残差確率密度算出器、１３  ゲート判定器、１４  測位解信頼度算出器、１５  距離残差記憶器、１６  受信機組み合わせ変更器、１７  疑似距離差算出器、１８  距離による送信機選択器、１９  距離変換器、２０  予測距離残差確率密度算出器、２１  フィルタ切り替え器、２２  距離観測行列算出器、２３  距離による位置平滑器、２４  距離による受信機選択器、２５  距離差による送信機選択器、２６  距離差変換器、２７  予測距離差残差確率密度算出器、２８  距離差観測行列算出器、２９  距離差による位置平滑器、３０  距離差による受信機選択器。
【図１】
2005024535.tif 000040
【図２】
2005024535.tif 000041
【図３】
2005024535.tif 000042
【図４】
2005024535.tif 000043
【図５】
2005024535.tif 000044
【図６】
2005024535.tif 000045
【図７】
2005024535.tif 000046
【図８】
2005024535.tif 000047
【図９】
2005024535.tif 000048
【図１０】
2005024535.tif 000049
【図１１】
2005024535.tif 000050
【図１２】
2005024535.tif 000051
【図１３】
2005024535.tif 000052
【図１４】
2005024535.tif 000053
【図１５】
2005024535.tif 000054
【図１６】
2005024535.tif 000055
【図１７】
2005024535.tif 000056
【図１８】
2005024535.tif 000057
【図１９】
2005024535.tif 000058
【図２０】
2005024535.tif 000059