JP2000213906A - 移動物体位置のトラッキング装置およびトラッキング方法 - Google Patents
移動物体位置のトラッキング装置およびトラッキング方法Info
- Publication number
- JP2000213906A JP2000213906A JP11373155A JP37315599A JP2000213906A JP 2000213906 A JP2000213906 A JP 2000213906A JP 11373155 A JP11373155 A JP 11373155A JP 37315599 A JP37315599 A JP 37315599A JP 2000213906 A JP2000213906 A JP 2000213906A
- Authority
- JP
- Japan
- Prior art keywords
- moving object
- tracking
- magnetic flux
- equation
- kalman filter
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Landscapes
- Measurement Of Length, Angles, Or The Like Using Electric Or Magnetic Means (AREA)
- Feedback Control In General (AREA)
Abstract
(57)【要約】
【課題】移動物体の近傍において測定した磁束の測定値
を、非線形カルマンフィルタを応用して処理することに
よって、移動物体の位置を検出し、追跡する、移動物体
位置のトラッキング装置およびトラッキング方法を提供
する。 【解決手段】移動物体の近傍において測定された磁束の
測定値を受けて、上記測定値に非線形カルマンフィルタ
を適用し、上記移動物体の位置に関する情報を出力す
る、非線形カルマンフィルタベースのトラッカ８０と、
上記移動物体の位置に関する上記情報を表示する位置表
示器と、を含んでなる。
を、非線形カルマンフィルタを応用して処理することに
よって、移動物体の位置を検出し、追跡する、移動物体
位置のトラッキング装置およびトラッキング方法を提供
する。 【解決手段】移動物体の近傍において測定された磁束の
測定値を受けて、上記測定値に非線形カルマンフィルタ
を適用し、上記移動物体の位置に関する情報を出力す
る、非線形カルマンフィルタベースのトラッカ８０と、
上記移動物体の位置に関する上記情報を表示する位置表
示器と、を含んでなる。
Description
【０００１】
【発明の属する技術分野】本発明は移動物体位置のトラ
ッキング装置およびトラッキング方法に関するものであ
る。
ッキング装置およびトラッキング方法に関するものであ
る。
【０００２】
【従来の技術】非線形フィルタの理論とその応用は、次
の文献で論じられている。
の文献で論じられている。
【０００３】エイチ・ジェイ・クシュナ著、「最適非線
形フィルタへの接近」、アイ、イー、イー、トランスア
クション、エイ、シー、ＡＣ−１２巻、第５号、１９６
７年１０月（Ｈ．Ｊ．Ｋｕｓｈｎｅｒ，“Ａｐｐｒｏｘ
ｉｍａｔｉｏｎｓ ｔｏ Ｏｐｔｉｍａｌ Ｎｏｎｌｉ
ｎｅａｒ Ｆｉｌｔｅｒｓ”，ＩＥＥＥ Ｔｒａｎｓ．
Ａ．Ｃ．，Ｖｏｌ．ＡＣ−１２，Ｎｏ．５，Ｏｃｔｏｂ
ｅｒ １９６７）、エイ・ゲルブ、ジェイ・エフ・カス
パ・ジュニア、アール・エイ・ナッシュ・ジュニア、シ
ー・イー・プライス、エイ・エイ・サザランド・ジュニ
ア著、「応用最適推定」、マサチューセッツ州ケンブリ
ッジ、エム・アイ・ティ・プレス、１９７４年（Ａ．Ｇ
ｅｌｂ，Ｊ．Ｆ．Ｋａｓｐａｒ，Ｊｒ．，Ｒ．Ａ．Ｎａ
ｓｈ，Ｊｒ．，Ｃ．Ｅ．Ｐｒｉｃｅ， ａｎｄ Ａ．
Ａ．Ｓｏｕｔｈｅｒｌａｎｄ，Ｊｒ．，“Ａｐｐｌｉｅ
ｄ Ｏｐｔｉｍａｌ Ｅｓｔｉｍａｔｉｏｎ”，Ｍ．
Ｉ．Ｔ．Ｐｒｅｓｓ，Ｃａｍｂｒｉｄｇｅ， Ｍａｓ
ｓ．，１９７４）、ビー・ディー・オー・アンダスン、
ジェイ・ビー・ムーア著、「最適フィルタ」、ニュージ
ャージー州、イングルウッドクリフス、プレンティスホ
ール、１９７９年（Ｂ．Ｄ．Ｏ．Ａｎｄｅｒｓｏｎ，
ａｎｄ Ｊ．Ｂ．Ｍｏｏｒｅ，“Ｏｐｔｉｍａｌ Ｆｉ
ｌｔｅｒｉｎｇ”，Ｐｒｅｎｔｉｃｅ−Ｈａｌｌ，Ｅｎ
ｇｌｅｗｏｏｄ Ｃｌｉｆｆｓ，Ｎｅｗ Ｊｅｒｓｅ
ｙ，１９７９）、エイ・エイチ・ジャツウィンスキ著、
「確率過程とフィルタ理論」、ニューヨーク、アカデミ
ックプレス、１９７１年（Ａ．Ｈ．Ｊａｚｗｉｎｓｋ
ｉ，“Ｓｔｏｃｈａｓｔｉｃ Ｐｒｏｃｅｓｓｅｓ ａ
ｎｄ Ｆｉｌｔｅｒｉｎｇ Ｔｈｅｏｒｙ”，Ａｃａｄ
ｅｍｉｃ Ｐｒｅｓｓ，Ｎｅｗ Ｙｏｒｋ，１９７
１）、エム・エス・グリュウワル、エイ・ピー・アンド
リュウス著、「カルマンフィルタ」、ニュージャージー
州、アッパーサドルリバー、プレンティスホール、１９
９３年（Ｍ．Ｓ．Ｇｒｅｗａｌ， ａｎｄ Ａ．Ｐ．Ａ
ｎｄｒｅｗｓ，“Ｋａｌｍａｎ Ｆｉｌｔｅｒｉｎ
ｇ”，Ｐｒｅｎｔｉｃｅ−Ｈａｌｌ，ＵｐｐｅｒＳａｄ
ｄｌｅ Ｒｉｖｅｒ，Ｎｅｗ Ｊｅｒｓｅｙ， １９９
３）。
形フィルタへの接近」、アイ、イー、イー、トランスア
クション、エイ、シー、ＡＣ−１２巻、第５号、１９６
７年１０月（Ｈ．Ｊ．Ｋｕｓｈｎｅｒ，“Ａｐｐｒｏｘ
ｉｍａｔｉｏｎｓ ｔｏ Ｏｐｔｉｍａｌ Ｎｏｎｌｉ
ｎｅａｒ Ｆｉｌｔｅｒｓ”，ＩＥＥＥ Ｔｒａｎｓ．
Ａ．Ｃ．，Ｖｏｌ．ＡＣ−１２，Ｎｏ．５，Ｏｃｔｏｂ
ｅｒ １９６７）、エイ・ゲルブ、ジェイ・エフ・カス
パ・ジュニア、アール・エイ・ナッシュ・ジュニア、シ
ー・イー・プライス、エイ・エイ・サザランド・ジュニ
ア著、「応用最適推定」、マサチューセッツ州ケンブリ
ッジ、エム・アイ・ティ・プレス、１９７４年（Ａ．Ｇ
ｅｌｂ，Ｊ．Ｆ．Ｋａｓｐａｒ，Ｊｒ．，Ｒ．Ａ．Ｎａ
ｓｈ，Ｊｒ．，Ｃ．Ｅ．Ｐｒｉｃｅ， ａｎｄ Ａ．
Ａ．Ｓｏｕｔｈｅｒｌａｎｄ，Ｊｒ．，“Ａｐｐｌｉｅ
ｄ Ｏｐｔｉｍａｌ Ｅｓｔｉｍａｔｉｏｎ”，Ｍ．
Ｉ．Ｔ．Ｐｒｅｓｓ，Ｃａｍｂｒｉｄｇｅ， Ｍａｓ
ｓ．，１９７４）、ビー・ディー・オー・アンダスン、
ジェイ・ビー・ムーア著、「最適フィルタ」、ニュージ
ャージー州、イングルウッドクリフス、プレンティスホ
ール、１９７９年（Ｂ．Ｄ．Ｏ．Ａｎｄｅｒｓｏｎ，
ａｎｄ Ｊ．Ｂ．Ｍｏｏｒｅ，“Ｏｐｔｉｍａｌ Ｆｉ
ｌｔｅｒｉｎｇ”，Ｐｒｅｎｔｉｃｅ−Ｈａｌｌ，Ｅｎ
ｇｌｅｗｏｏｄ Ｃｌｉｆｆｓ，Ｎｅｗ Ｊｅｒｓｅ
ｙ，１９７９）、エイ・エイチ・ジャツウィンスキ著、
「確率過程とフィルタ理論」、ニューヨーク、アカデミ
ックプレス、１９７１年（Ａ．Ｈ．Ｊａｚｗｉｎｓｋ
ｉ，“Ｓｔｏｃｈａｓｔｉｃ Ｐｒｏｃｅｓｓｅｓ ａ
ｎｄ Ｆｉｌｔｅｒｉｎｇ Ｔｈｅｏｒｙ”，Ａｃａｄ
ｅｍｉｃ Ｐｒｅｓｓ，Ｎｅｗ Ｙｏｒｋ，１９７
１）、エム・エス・グリュウワル、エイ・ピー・アンド
リュウス著、「カルマンフィルタ」、ニュージャージー
州、アッパーサドルリバー、プレンティスホール、１９
９３年（Ｍ．Ｓ．Ｇｒｅｗａｌ， ａｎｄ Ａ．Ｐ．Ａ
ｎｄｒｅｗｓ，“Ｋａｌｍａｎ Ｆｉｌｔｅｒｉｎ
ｇ”，Ｐｒｅｎｔｉｃｅ−Ｈａｌｌ，ＵｐｐｅｒＳａｄ
ｄｌｅ Ｒｉｖｅｒ，Ｎｅｗ Ｊｅｒｓｅｙ， １９９
３）。
【０００４】また、ビオ−サバールの法則については、
次の文献で論じられている。
次の文献で論じられている。
【０００５】ジェイ・ディー・ジャクソン著、「古典電
気力学」、ニューヨーク州、ニューヨーク、ジョン・ワ
イリー・アンド・サンズ、１９７５年（Ｊ．Ｄ．Ｊａｃ
ｋｓｏｎ，“Ｃｌａｓｓｉｃａｌ Ｅｌｅｃｔｒｏｄｙ
ｎａｍｉｃｓ”，ＪｏｈｎＷｉｌｌｅｙ ａｎｄ Ｓｏ
ｎｓ，Ｎｅｗ Ｙｏｒｋ，Ｎｅｗ Ｙｏｒｋ，１９７
５）。
気力学」、ニューヨーク州、ニューヨーク、ジョン・ワ
イリー・アンド・サンズ、１９７５年（Ｊ．Ｄ．Ｊａｃ
ｋｓｏｎ，“Ｃｌａｓｓｉｃａｌ Ｅｌｅｃｔｒｏｄｙ
ｎａｍｉｃｓ”，ＪｏｈｎＷｉｌｌｅｙ ａｎｄ Ｓｏ
ｎｓ，Ｎｅｗ Ｙｏｒｋ，Ｎｅｗ Ｙｏｒｋ，１９７
５）。
【０００６】拡張カルマンフィルタ（Ｅｘｔｅｎｄｅｄ
Ｋａｌｍａｎ Ｆｉｌｔｅｒｓ）のレーダでのトラッ
キングへの応用は、例えば米国特許第５，０７５，６９
４号、第４，１７９，６９６号、第３，９５２，３０４
号、第３，９３２，５７２号において論じられている。
Ｋａｌｍａｎ Ｆｉｌｔｅｒｓ）のレーダでのトラッ
キングへの応用は、例えば米国特許第５，０７５，６９
４号、第４，１７９，６９６号、第３，９５２，３０４
号、第３，９３２，５７２号において論じられている。
【０００７】その他のトラッキング装置は、例えば米国
特許第５，０９５，４６７号、第４，８５５、９３２号
において論じられている。
特許第５，０９５，４６７号、第４，８５５、９３２号
において論じられている。
【０００８】カルマンフィルタは、異なるセンサの「デ
ータ融合」のための標準ツールである。米国特許第５，
４１６，７１２号においては、ＧＰＳの信号と推測航法
とがカルマンフィルタによって結合され、またジャイロ
バイアスも校正されている。米国特許第５，６４５，０
７７号においては、自動ドリフト補正が論じられてい
る。
ータ融合」のための標準ツールである。米国特許第５，
４１６，７１２号においては、ＧＰＳの信号と推測航法
とがカルマンフィルタによって結合され、またジャイロ
バイアスも校正されている。米国特許第５，６４５，０
７７号においては、自動ドリフト補正が論じられてい
る。
【０００９】本明細書において言及する刊行物の全ての
開示と、本明細書において引用する刊行物の全ての開示
は、ここに参照によって組み入れられる。
開示と、本明細書において引用する刊行物の全ての開示
は、ここに参照によって組み入れられる。
【００１０】
【発明が解決しようとする課題】本発明は、移動物体の
近傍において測定した磁束の測定値を、上述の非線形カ
ルマンフィルタを応用して処理することによって、移動
物体の位置を検出し、追跡する、移動物体位置のトラッ
キング装置およびトラッキング方法を提供することを目
的とする。
近傍において測定した磁束の測定値を、上述の非線形カ
ルマンフィルタを応用して処理することによって、移動
物体の位置を検出し、追跡する、移動物体位置のトラッ
キング装置およびトラッキング方法を提供することを目
的とする。
【００１１】
【課題を解決するための手段】この目的を達成するた
め、本発明の望ましい実施の形態によれば、移動物体位
置のトラッキング装置は、移動物体の近傍において測定
した磁束の値に基づいて移動物体の位置を追跡する移動
物体位置のトラッキング装置において、上記移動物体の
近傍において測定された磁束の測定値を受けて、上記測
定値に非線形カルマンフィルタを適用し、上記移動物体
の位置に関する情報を出力する、非線形カルマンフィル
タベースのトラッカと、上記移動物体の位置に関する上
記情報を表示する位置表示器と、を含んでなる。
め、本発明の望ましい実施の形態によれば、移動物体位
置のトラッキング装置は、移動物体の近傍において測定
した磁束の値に基づいて移動物体の位置を追跡する移動
物体位置のトラッキング装置において、上記移動物体の
近傍において測定された磁束の測定値を受けて、上記測
定値に非線形カルマンフィルタを適用し、上記移動物体
の位置に関する情報を出力する、非線形カルマンフィル
タベースのトラッカと、上記移動物体の位置に関する上
記情報を表示する位置表示器と、を含んでなる。
【００１２】また本発明の望ましい実施の形態によれ
ば、非線形カルマンフィルタベースの上記トラッカは、
拡張カルマンフィルタベースを含む。
ば、非線形カルマンフィルタベースの上記トラッカは、
拡張カルマンフィルタベースを含む。
【００１３】なお、本発明の望ましい実施の形態によれ
ば、上記非線形カルマンフィルタは、要素が位置座標と
位置座標の１次微分とを含む状態ベクトルに作用する。
ば、上記非線形カルマンフィルタは、要素が位置座標と
位置座標の１次微分とを含む状態ベクトルに作用する。
【００１４】更に、本発明の望ましい実施の形態によれ
ば、上記位置座標は、３個の空間座標と２個の方向座標
とからなる。
ば、上記位置座標は、３個の空間座標と２個の方向座標
とからなる。
【００１５】更に、本発明の望ましい実施の形態によれ
ば、上記移動物体位置のトラッキング装置は、上記移動
物体の近傍に磁束を誘起する６個以下の有効な送信機か
らなる送信機アレーをさらに含んでなる。
ば、上記移動物体位置のトラッキング装置は、上記移動
物体の近傍に磁束を誘起する６個以下の有効な送信機か
らなる送信機アレーをさらに含んでなる。
【００１６】更に、本発明の望ましい実施の形態によれ
ば、非線形カルマンフィルタベースの上記トラッカは、
上記移動物体の上記位置から上記移動物体の近傍におい
て測定した上記磁束の値へビオ−サバール変換を適用す
る。
ば、非線形カルマンフィルタベースの上記トラッカは、
上記移動物体の上記位置から上記移動物体の近傍におい
て測定した上記磁束の値へビオ−サバール変換を適用す
る。
【００１７】更に、本発明の望ましい実施の形態によれ
ば、上記ビオ−サバール変換を適用する過程は、次式に
示す状態ベクトル↑ξの測定感度関数↑ｈの計算からな
る。
ば、上記ビオ−サバール変換を適用する過程は、次式に
示す状態ベクトル↑ξの測定感度関数↑ｈの計算からな
る。
【００１８】
【数１７】
【００１９】ここに、Ｃ0は係数、Ｒは、上記磁束を誘
起する複数の送信機からなる送信機アレーからの上記磁
束の上記測定値を検知する検知器と各上記送信機との間
の距離、および、
起する複数の送信機からなる送信機アレーからの上記磁
束の上記測定値を検知する検知器と各上記送信機との間
の距離、および、
【００２０】
【数１８】
【００２１】であり、ここに検知器の位置は（ｘd，
ｙd，ｚd，φd，θd）、送信機の位置は（ｘs，ｙs，ｚ
s，φs，θs）であり、δｘ、δｙ、δｚ、は、それぞ
れ検知器の位置と送信機の位置との間の距離のｘ、ｙ、
ｚ成分を表す。
ｙd，ｚd，φd，θd）、送信機の位置は（ｘs，ｙs，ｚ
s，φs，θs）であり、δｘ、δｙ、δｚ、は、それぞ
れ検知器の位置と送信機の位置との間の距離のｘ、ｙ、
ｚ成分を表す。
【００２２】なお、本発明の望ましい実施の形態によれ
ば、非線形カルマンフィルタベースの上記トラッカは、
少なくとも上記移動物体が上記磁束を誘起する複数の送
信機からなる送信機アレー内の１つの送信機に近接した
ときに、楕円積分を表すテイラー展開数列の第１項と第
２項とを計算することにより、上記楕円積分を近似す
る。
ば、非線形カルマンフィルタベースの上記トラッカは、
少なくとも上記移動物体が上記磁束を誘起する複数の送
信機からなる送信機アレー内の１つの送信機に近接した
ときに、楕円積分を表すテイラー展開数列の第１項と第
２項とを計算することにより、上記楕円積分を近似す
る。
【００２３】なお、本発明の望ましい実施の形態によれ
ば、上記楕円積分の近似は、上記Ａ1、Ａ3に次式の修正
を含む。
ば、上記楕円積分の近似は、上記Ａ1、Ａ3に次式の修正
を含む。
【００２４】
【数１９】
【００２５】ここに、ρは送信機のコイルの半径、Ｒ
は、上記磁束の上記測定値を検知する検知器と上記測定
値によって測定される上記磁束を誘起する送信機との間
の距離である。
は、上記磁束の上記測定値を検知する検知器と上記測定
値によって測定される上記磁束を誘起する送信機との間
の距離である。
【００２６】更に、本発明の望ましい実施の形態によれ
ば、上記移動物体の上記位置の方向成分は、時間につい
て連続な２つの角度、θ′とφ′とによって表され、
θ′とφ′の従来の極座標θとφとの関係は、次式のと
おりである。
ば、上記移動物体の上記位置の方向成分は、時間につい
て連続な２つの角度、θ′とφ′とによって表され、
θ′とφ′の従来の極座標θとφとの関係は、次式のと
おりである。
【００２７】
【数２０】
【００２８】更に、本発明の望ましい実施の形態によれ
ば、非線形カルマンフィルタベースの上記トラッカは、
特異性を避けるために、上記移動物体の上記位置の方向
成分のダイナミックオフセットを使用し、上記ダイナミ
ックオフセットは、下記の変換によって記述される。
ば、非線形カルマンフィルタベースの上記トラッカは、
特異性を避けるために、上記移動物体の上記位置の方向
成分のダイナミックオフセットを使用し、上記ダイナミ
ックオフセットは、下記の変換によって記述される。
【００２９】
【数２１】
【００３０】ここに、θとφは、ダイナミックオフセッ
ト後の上記移動物体の上記位置の方向成分からなり、
θ′とφ′は、ダイナミックオフセット前の上記移動物
体の上記位置の方向成分からなる。
ト後の上記移動物体の上記位置の方向成分からなり、
θ′とφ′は、ダイナミックオフセット前の上記移動物
体の上記位置の方向成分からなる。
【００３１】更に、本発明の望ましい実施の形態によれ
ば、上記非線形カルマンフィルタは、以下のマトリクス
と操作を適用する。
ば、上記非線形カルマンフィルタは、以下のマトリクス
と操作を適用する。
【００３２】
【数２２】
【００３３】ここに、ｋは時間ステップのインデック
ス、↑ξk（−）は状態ベクトルの予測値、↑ξk（＋）
は状態ベクトルの修正後の予測値、↑Φは状態遷移マト
リクス、
ス、↑ξk（−）は状態ベクトルの予測値、↑ξk（＋）
は状態ベクトルの修正後の予測値、↑Φは状態遷移マト
リクス、
【００３４】
【数２３】
【００３５】ここに、↑Ｐ（−）は推定誤差共変マトリ
クスの予測値、↑Ｐ（＋）は推定誤差共変マトリクスの
修正値、↑Ｑは過程ノイズ共変マトリクス、
クスの予測値、↑Ｐ（＋）は推定誤差共変マトリクスの
修正値、↑Ｑは過程ノイズ共変マトリクス、
【００３６】
【数２４】
【００３７】ここに、↑ｈは測定感度関数、↑ξは状態
ベクトル、
ベクトル、
【００３８】
【数２５】
【００３９】ここに、↑Ｒkは測定ノイズ共変マトリク
ス、
ス、
【００４０】
【数２６】
【００４１】ここに、↑ζは、上記移動物体の近傍にお
いて測定された上記磁束の測定値を表し、
いて測定された上記磁束の測定値を表し、
【００４２】
【数２７】
【００４３】である。
【００４４】更に、本発明の望ましい実施の形態によれ
ば、上記磁束の測定値は、上記移動物体の近傍における
６個以下の磁束の測定値からなる。
ば、上記磁束の測定値は、上記移動物体の近傍における
６個以下の磁束の測定値からなる。
【００４５】なお、本発明の望ましい実施の形態によれ
ば、非線形カルマンフィルタベースの上記トラッカは、
測定ノイズ共変マトリクス↑Ｒと過程ノイズ共変マトリ
クス↑Ｑを時間変更させるように作用すること。
ば、非線形カルマンフィルタベースの上記トラッカは、
測定ノイズ共変マトリクス↑Ｒと過程ノイズ共変マトリ
クス↑Ｑを時間変更させるように作用すること。
【００４６】更に、本発明の望ましい実施の形態によれ
ば、上記測定ノイズ共変マトリクス↑Ｒと上記過程ノイ
ズ共変マトリクス↑Ｑの上記時間変更は、次式からな
る。
ば、上記測定ノイズ共変マトリクス↑Ｒと上記過程ノイ
ズ共変マトリクス↑Ｑの上記時間変更は、次式からな
る。
【００４７】
【数２８】
【００４８】ここに、下添字ｋとｋ−１はｋ番目とｋ−
１番目の時間ステップのインデックスを、右肩の添字ｄ
ｅｃは減衰を、右肩の添字ｉｎｆは十分な時間経過後の
定常状態を、τdecayは減衰の時定数を、Ｔは時刻を、
それぞれ表す。
１番目の時間ステップのインデックスを、右肩の添字ｄ
ｅｃは減衰を、右肩の添字ｉｎｆは十分な時間経過後の
定常状態を、τdecayは減衰の時定数を、Ｔは時刻を、
それぞれ表す。
【００４９】更に、本発明の望ましい実施の形態によれ
ば、上記測定値によって計量される磁束を誘起する少な
くとも１個の送信機は、自己校正される。
ば、上記測定値によって計量される磁束を誘起する少な
くとも１個の送信機は、自己校正される。
【００５０】なお、本発明の望ましい実施の形態によれ
ば、上記非線形カルマンフィルタは、各上記自己校正送
信機の位置を校正するように作用する。
ば、上記非線形カルマンフィルタは、各上記自己校正送
信機の位置を校正するように作用する。
【００５１】更に、本発明の望ましい実施の形態によれ
ば、上記非線形カルマンフィルタは、各上記自己校正送
信機の発振強度を校正するように作用する。
ば、上記非線形カルマンフィルタは、各上記自己校正送
信機の発振強度を校正するように作用する。
【００５２】更に、本発明の望ましい実施の形態によれ
ば、上記非線形カルマンフィルタは、各上記自己校正送
信機の半径を校正するように作用する。
ば、上記非線形カルマンフィルタは、各上記自己校正送
信機の半径を校正するように作用する。
【００５３】なお、本発明の望ましい実施の形態によれ
ば、上記非線形カルマンフィルタは、各上記自己校正送
信機の向きを校正するように作用する。
ば、上記非線形カルマンフィルタは、各上記自己校正送
信機の向きを校正するように作用する。
【００５４】更に、本発明の望ましい実施の形態によれ
ば、上記トラッカは、成分が上記測定値によって計量さ
れる磁界を誘起する上記自己校正送信機の特性からなる
状態ベクトルを用い、上記特性の少なくとも１つは自己
校正される。
ば、上記トラッカは、成分が上記測定値によって計量さ
れる磁界を誘起する上記自己校正送信機の特性からなる
状態ベクトルを用い、上記特性の少なくとも１つは自己
校正される。
【００５５】更に、本発明の望ましい実施の形態によれ
ば、非線形カルマンフィルタベースの上記トラッカは、
少なくとも１つは適応過程によって発生される測定ノイ
ズ共変マトリクス↑Ｒと過程ノイズ共変マトリクス↑Ｑ
を使用する。
ば、非線形カルマンフィルタベースの上記トラッカは、
少なくとも１つは適応過程によって発生される測定ノイ
ズ共変マトリクス↑Ｒと過程ノイズ共変マトリクス↑Ｑ
を使用する。
【００５６】なお、本発明の望ましい実施の形態によれ
ば、上記過程ノイズ共変マトリクス↑Ｑのための上記適
応過程は、次式からなる。
ば、上記過程ノイズ共変マトリクス↑Ｑのための上記適
応過程は、次式からなる。
【００５７】
【数２９】
【００５８】ここに、↑Ｑaccは、状態ベクトルの中に
見られる最高次の時間微分に対応する↑Ｑの成分、下添
字ｋとｋ−１はｋ番目とｋ−１番目の時間ステップのイ
ンデックス、↑ξは状態ベクトル、右肩の添字ｖｅｌｏ
ｃｉｔｙは微分値すなわち速度を示し、Ｔは時刻、αは
ゲイン係数である。
見られる最高次の時間微分に対応する↑Ｑの成分、下添
字ｋとｋ−１はｋ番目とｋ−１番目の時間ステップのイ
ンデックス、↑ξは状態ベクトル、右肩の添字ｖｅｌｏ
ｃｉｔｙは微分値すなわち速度を示し、Ｔは時刻、αは
ゲイン係数である。
【００５９】更に、本発明の望ましい実施の形態によれ
ば、上記測定ノイズ共変マトリクス↑Ｒのための上記適
応過程は、次式からなる。
ば、上記測定ノイズ共変マトリクス↑Ｒのための上記適
応過程は、次式からなる。
【００６０】
【数３０】
【００６１】ここに、右肩の添字ｓは測定ノイズ共変マ
トリクス↑Ｒを計算する送信機を表し、下添字ｋとｋ−
１はｋ番目とｋ−１番目の時間ステップのインデック
ス、↑ξは状態ベクトル、↑ξk（−）は状態ベクトル
の予測値、↑ζは磁束の測定値、↑ｈは測定感度関数、
βはゲイン係数である。
トリクス↑Ｒを計算する送信機を表し、下添字ｋとｋ−
１はｋ番目とｋ−１番目の時間ステップのインデック
ス、↑ξは状態ベクトル、↑ξk（−）は状態ベクトル
の予測値、↑ζは磁束の測定値、↑ｈは測定感度関数、
βはゲイン係数である。
【００６２】また、本発明の望ましい実施の形態によれ
ば、移動物体位置のトラッキング方法は、移動物体の近
傍において測定した磁束の値に基づいて移動物体の位置
を追跡する移動物体位置のトラッキング方法において、
上記移動物体の近傍において測定された磁束の測定値を
受け、上記測定値に非線形カルマンフィルタを適用し、
上記移動物体の位置に関する情報を出力し、上記移動物
体の位置に関する上記情報の出力表示を供給する。
ば、移動物体位置のトラッキング方法は、移動物体の近
傍において測定した磁束の値に基づいて移動物体の位置
を追跡する移動物体位置のトラッキング方法において、
上記移動物体の近傍において測定された磁束の測定値を
受け、上記測定値に非線形カルマンフィルタを適用し、
上記移動物体の位置に関する情報を出力し、上記移動物
体の位置に関する上記情報の出力表示を供給する。
【００６３】
【発明の実施の形態】用語の定義： 位置（ポーズ、Ｐｏｓｅ）：３次元空間における場所を
記述する３座標と、方向を記述する２座標とを含む、円
対称物体の５次元座標である。
記述する３座標と、方向を記述する２座標とを含む、円
対称物体の５次元座標である。
【００６４】方向（Ｏｒｉｅｎｔａｔｉｏｎ）：３次元
空間における固定の方向に相対的な方向を記述する、物
体の２次元の角度方向位置である。
空間における固定の方向に相対的な方向を記述する、物
体の２次元の角度方向位置である。
【００６５】送信機：磁束を生じる磁界を発する装置で
ある。
ある。
【００６６】検知器：検知面積を横切る磁束を電圧に変
換する装置である。
換する装置である。
【００６７】図１〜３は、ビオ−サバールの法則を基礎
とする（以下ビオ−サバールベースと記す。）拡張カル
マンフィルタ型のトラッキングフィルタ（ｔｒａｃｋｉ
ｎｇｆｉｌｔｅｒ）の望ましい運用方法の流れ図であ
る。
とする（以下ビオ−サバールベースと記す。）拡張カル
マンフィルタ型のトラッキングフィルタ（ｔｒａｃｋｉ
ｎｇｆｉｌｔｅｒ）の望ましい運用方法の流れ図であ
る。
【００６８】図４は、図１〜３に示す方法によって運用
する、ビオ−サバールベースの、拡張カルマンフィルタ
型のトラッキングフィルタを含むトラッキング装置の１
例の簡単化したブロック図である。
する、ビオ−サバールベースの、拡張カルマンフィルタ
型のトラッキングフィルタを含むトラッキング装置の１
例の簡単化したブロック図である。
【００６９】図４に示す装置は、望ましくは、以下の機
能単位から成る。
能単位から成る。
【００７０】１．多数の電気コイルと電気コイルを駆動
する電子回路である駆動器とを含む送信機４０。
する電子回路である駆動器とを含む送信機４０。
【００７１】２．コイルと、例えば前置増幅、入力制
御、フィルタ等の前処理を提供する前置電子回路とを含
む検知器５０。
御、フィルタ等の前処理を提供する前置電子回路とを含
む検知器５０。
【００７２】３．同期装置６０。
【００７３】４．輪郭検出器７０。
【００７４】５．トラッキング非線形フィルタ（トラッ
カー）８０。
カー）８０。
【００７５】６．位置に関する情報を表示する位置表示
器。（図示しない。）各送信機４０は、交流駆動の従来
のソレノイドで構成することができ、移動物体の位置を
測定する空間内の適当な場所に固定して配設する。
器。（図示しない。）各送信機４０は、交流駆動の従来
のソレノイドで構成することができ、移動物体の位置を
測定する空間内の適当な場所に固定して配設する。
【００７６】検知器５０は、例えば一軸磁界検出器と信
号調整Ａ／Ｄ変換器からなり、例えば一軸アンテナコイ
ルまたはホール効果検出器、半導体部品、磁気抵抗検出
器、磁気ダイオード、磁気トランジスタ等から構成さ
れ、移動物体の適当な箇所に取り付ける。
号調整Ａ／Ｄ変換器からなり、例えば一軸アンテナコイ
ルまたはホール効果検出器、半導体部品、磁気抵抗検出
器、磁気ダイオード、磁気トランジスタ等から構成さ
れ、移動物体の適当な箇所に取り付ける。
【００７７】同期装置６０は、例えばシステム同期ユニ
ットの一種である。
ットの一種である。
【００７８】輪郭検出器７０は、例えば、並列に作動す
るＮ個の輪郭検出モジュールからなり、各輪郭検出モジ
ュールは２つのサブモジュール、フェーズロックループ
サブモジュールとシステム同期ユニットとからなる、Ｎ
（Ｎは任意の整数）個のＲＦ送信機から受信したＮ個の
磁気信号の輪郭の振幅（大きさ及び符号）Ｃ1，…，ＣN
を定める輪郭検出器の一種である。
るＮ個の輪郭検出モジュールからなり、各輪郭検出モジ
ュールは２つのサブモジュール、フェーズロックループ
サブモジュールとシステム同期ユニットとからなる、Ｎ
（Ｎは任意の整数）個のＲＦ送信機から受信したＮ個の
磁気信号の輪郭の振幅（大きさ及び符号）Ｃ1，…，ＣN
を定める輪郭検出器の一種である。
【００７９】トラッキング非線形フィルタ８０は、輪郭
検出器から供給された符号付きの振幅値に基づいて作動
し、６個の未知数を含むＮ個の解析方程式を解いて、検
知器５０の位置を出力する、位置決定ユニットの一種で
ある。
検出器から供給された符号付きの振幅値に基づいて作動
し、６個の未知数を含むＮ個の解析方程式を解いて、検
知器５０の位置を出力する、位置決定ユニットの一種で
ある。
【００８０】ビオ−サバールベースの拡張カルマンフィ
ルタ型のトラッキング非線形フィルタ８０の望ましい使
用方法を図１〜３を参照して詳細に説明する。例とし
て、磁界を誘起する送信機アレーの場合について説明す
る。重畳された磁束が磁束検知器（検知器５０）によっ
て検出され、輪郭検出器７０によって個々の磁束に分解
されるものとして記述する。他の型式の磁束誘起、磁束
測定装置等を用いてもよい。
ルタ型のトラッキング非線形フィルタ８０の望ましい使
用方法を図１〜３を参照して詳細に説明する。例とし
て、磁界を誘起する送信機アレーの場合について説明す
る。重畳された磁束が磁束検知器（検知器５０）によっ
て検出され、輪郭検出器７０によって個々の磁束に分解
されるものとして記述する。他の型式の磁束誘起、磁束
測定装置等を用いてもよい。
【００８１】ダイナミック系の状態空間モデル 従来のカルマンフィルタは、系のノイズの測定に基づく
線形系の状態の最適予測と、系の運動方程式のモデル
と、以前の状態の予測とを発生する、シーケンシャルト
ラッカー（ｓｅｑｕｅｎｔｉａｌ ｔｒａｃｋｅｒ）で
ある。関心の対象である系が非線形の場合には、カルマ
ンフィルタの一つの変形である拡張カルマンフィルタが
一般的に用いられる。
線形系の状態の最適予測と、系の運動方程式のモデル
と、以前の状態の予測とを発生する、シーケンシャルト
ラッカー（ｓｅｑｕｅｎｔｉａｌ ｔｒａｃｋｅｒ）で
ある。関心の対象である系が非線形の場合には、カルマ
ンフィルタの一つの変形である拡張カルマンフィルタが
一般的に用いられる。
【００８２】従来のカルマンフィルタは、複数のマトリ
クスを用いて、状態ベクトルの最適推定値を計算する。
これらのマトリクスは、例えば次のように系の運動方程
式から導かれる。（本明細書において、ベクトル、マト
リクスは、数式及び図面においては太文字で表示し、文
章内では文字の左に付けた↑で表示する。また、マトリ
クスの右肩の添字ｔ、Ｔは、転置マトリクスを示す。）
系の動力学的状態を次式で近似する。
クスを用いて、状態ベクトルの最適推定値を計算する。
これらのマトリクスは、例えば次のように系の運動方程
式から導かれる。（本明細書において、ベクトル、マト
リクスは、数式及び図面においては太文字で表示し、文
章内では文字の左に付けた↑で表示する。また、マトリ
クスの右肩の添字ｔ、Ｔは、転置マトリクスを示す。）
系の動力学的状態を次式で近似する。
【００８３】
【数３１】
【００８４】数３１式において、ｔは時間、↑ξ（ｔ）
は状態ベクトル、↑Ｆは微分方程式の係数マトリクス、
↑ｗ（ｔ）は系ノイズ、↑ζ（ｔ）は測定値、↑ｈ［↑
ξ（ｔ）］は測定感度関数、↑ｖ（ｔ）は測定ノイズで
ある。↑ｈ［↑ξ（ｔ）］は、次元が測定値の数である
ベクトル関数である。測定値↑ζは、輪郭検出器７０の
出力であり、トラッキング非線形フィルタ８０の入力で
ある前記のＣ1，…，ＣNと同じである。測定ノイズと系
ノイズの両者は、不変白色ガウス分布過程（ｉｎｖａｒ
ｉａｎｔ ｗｈｉｔｅ Ｇａｕｓｓｉａｎ ｄｉｓｔｒ
ｉｂｕｔｅｄｐｒｏｃｅｓｓｅｓ）であると仮定され、
互いに独立である。
は状態ベクトル、↑Ｆは微分方程式の係数マトリクス、
↑ｗ（ｔ）は系ノイズ、↑ζ（ｔ）は測定値、↑ｈ［↑
ξ（ｔ）］は測定感度関数、↑ｖ（ｔ）は測定ノイズで
ある。↑ｈ［↑ξ（ｔ）］は、次元が測定値の数である
ベクトル関数である。測定値↑ζは、輪郭検出器７０の
出力であり、トラッキング非線形フィルタ８０の入力で
ある前記のＣ1，…，ＣNと同じである。測定ノイズと系
ノイズの両者は、不変白色ガウス分布過程（ｉｎｖａｒ
ｉａｎｔ ｗｈｉｔｅ Ｇａｕｓｓｉａｎ ｄｉｓｔｒ
ｉｂｕｔｅｄｐｒｏｃｅｓｓｅｓ）であると仮定され、
互いに独立である。
【００８５】数３１式の離散時間形は、
【００８６】
【数３２】
【００８７】である。ここに、↑ξkは時間ステップｋ
における状態ベクトル、↑Φは時間ステップｋにおける
状態遷移マトリクス、↑ｗkは時間ステップｋにおける
系ノイズ、↑ｈ［↑ξk］は時間ステップｋにおける測
定感度関数、↑ζkは時間ステップｋにおける測定ベク
トル、↑ｖkは時間ステップｋにおける測定ノイズであ
る。系ノイズ↑ｗkと測定ノイズ↑ｖkは独立不変白色ガ
ウス分布過程である。
における状態ベクトル、↑Φは時間ステップｋにおける
状態遷移マトリクス、↑ｗkは時間ステップｋにおける
系ノイズ、↑ｈ［↑ξk］は時間ステップｋにおける測
定感度関数、↑ζkは時間ステップｋにおける測定ベク
トル、↑ｖkは時間ステップｋにおける測定ノイズであ
る。系ノイズ↑ｗkと測定ノイズ↑ｖkは独立不変白色ガ
ウス分布過程である。
【００８８】所望の位置を計算するのに十分な情報を含
むなら、一般に状態ベクトルは任意に選んでよい。可能
な一つの選択は、
むなら、一般に状態ベクトルは任意に選んでよい。可能
な一つの選択は、
【００８９】
【数３３】
【００９０】である。ここに、（ｘ，ｙ，ｚ，φ，θ）
は、図５に示すような検知器の位置の５座標であり、変
数の上の点（ドット）は変数の時間微分を示す。高次微
分の影響は、系ノイズに暗に含ませる。
は、図５に示すような検知器の位置の５座標であり、変
数の上の点（ドット）は変数の時間微分を示す。高次微
分の影響は、系ノイズに暗に含ませる。
【００９１】カルマンフィルタは、ガウスノイズである
系ノイズ↑ｗk、測定ノイズ↑ｖkに関する統計学の知識
に基づき、
系ノイズ↑ｗk、測定ノイズ↑ｖkに関する統計学の知識
に基づき、
【００９２】
【数３４】
【００９３】である。ここに、δk,iはクロネッカのデ
ルタ関数（Ｋｒｏｎｅｃｋｅｒ ｄｅｌｔａ ｆｕｎｃ
ｔｉｏｎ）である。
ルタ関数（Ｋｒｏｎｅｃｋｅｒ ｄｅｌｔａ ｆｕｎｃ
ｔｉｏｎ）である。
【００９４】連続な時間から離散時間への移行は、次の
関係を使用して行う。
関係を使用して行う。
【００９５】
【数３５】
【００９６】数３５式の最後の２つの表現は、唯一の近
似である。前者は（ｔk−ｔk-1）の非線形項を無視し、
後者はセンサがｔk-1とｔkの間にわたってノイズを積分
する場合には正確である。
似である。前者は（ｔk−ｔk-1）の非線形項を無視し、
後者はセンサがｔk-1とｔkの間にわたってノイズを積分
する場合には正確である。
【００９７】磁気トラッキングのための拡張カルマンフ
ィルタの使用 測定感度関数↑ｈ［↑ξ（ｔ）］が誘起電圧を磁界に関
連づけることは、ビオ−サバールの法則として知られて
おり、閉鎖形（ｃｌｏｓｅｄ ｆｏｒｍ）で表現するこ
とは困難である。従って、近似が望ましく、最も簡単な
近似は、
ィルタの使用 測定感度関数↑ｈ［↑ξ（ｔ）］が誘起電圧を磁界に関
連づけることは、ビオ−サバールの法則として知られて
おり、閉鎖形（ｃｌｏｓｅｄ ｆｏｒｍ）で表現するこ
とは困難である。従って、近似が望ましく、最も簡単な
近似は、
【００９８】
【数３６】
【００９９】である。ここに、↑ｒs、↑ｒdは送信機と
検知器の位置ベクトルであり、↑ｎs、↑ｎdは↑ｒs、
↑ｒdのそれぞれに対応する平面単位ベクトル（平面ベ
クトルは、その面に垂直な方向のベクトルである。）で
ある。これらの表記は、図６に図示されている。測定感
度関数↑ｈ（↑ξ）は、その要素の各々が単体の送信機
との相互作用であるベクトル関数である。ベクトル関数
の各要素について、値↑ｒs、↑ｎs、比例係数Ｃ0はそ
れらの対応する値を取る。
検知器の位置ベクトルであり、↑ｎs、↑ｎdは↑ｒs、
↑ｒdのそれぞれに対応する平面単位ベクトル（平面ベ
クトルは、その面に垂直な方向のベクトルである。）で
ある。これらの表記は、図６に図示されている。測定感
度関数↑ｈ（↑ξ）は、その要素の各々が単体の送信機
との相互作用であるベクトル関数である。ベクトル関数
の各要素について、値↑ｒs、↑ｎs、比例係数Ｃ0はそ
れらの対応する値を取る。
【０１００】トラッカー（トラッキング非線形フィルタ
８０）の機能は、カルマンフィルタへの幾つかの任意の
拡張を用いて定義することができる。拡張カルマンフィ
ルタを用いる本発明の望ましい実施の形態は、図１〜３
に示されており、以下に説明する。
８０）の機能は、カルマンフィルタへの幾つかの任意の
拡張を用いて定義することができる。拡張カルマンフィ
ルタを用いる本発明の望ましい実施の形態は、図１〜３
に示されており、以下に説明する。
【０１０１】この手順は、一種の逐次手順であって、そ
れぞれ前の推定値に基づいて次々に推定値を提供する。
初期計算２００は、一般に５個の初期値のアレイ、↑Ｑ
0，↑Ｒ0，↑Ｐ0，↑Φ，↑ξ0、を供給する。各ブロッ
クの役割は以下のとおりである。
れぞれ前の推定値に基づいて次々に推定値を提供する。
初期計算２００は、一般に５個の初期値のアレイ、↑Ｑ
0，↑Ｒ0，↑Ｐ0，↑Φ，↑ξ0、を供給する。各ブロッ
クの役割は以下のとおりである。
【０１０２】予測状態ベクトル２０５は、時間ステップ
ｋ−ｌにおける状態ベクトルの推定値を与えられ、時間
ステップｋにおける状態ベクトルの推定値、
ｋ−ｌにおける状態ベクトルの推定値を与えられ、時間
ステップｋにおける状態ベクトルの推定値、
【０１０３】
【数３７】
【０１０４】を発生するように作られている。（−）と
（＋）の表記は、それぞれ測定前の推定（予測）と測定
後の推定（訂正）を示す。数３３式に置いて提案された
状態ベクトルについて、マトリクス↑Φは、
（＋）の表記は、それぞれ測定前の推定（予測）と測定
後の推定（訂正）を示す。数３３式に置いて提案された
状態ベクトルについて、マトリクス↑Φは、
【０１０５】
【数３８】
【０１０６】の形をとる。
【０１０７】推定誤差共変マトリクス２１０は、推定誤
差共変マトリクス（ｅｓｔｉｍａｔｅ ｅｒｒｏｒ ｃ
ｏｖａｒｉａｎｃｅ−ｍａｔｒｉｘ）、
差共変マトリクス（ｅｓｔｉｍａｔｅ ｅｒｒｏｒ ｃ
ｏｖａｒｉａｎｃｅ−ｍａｔｒｉｘ）、
【０１０８】
【数３９】
【０１０９】を発生するように作られている。
【０１１０】この例におけるマトリクス↑Ｑは、
【０１１１】
【数４０】
【０１１２】の形をとる。
【０１１３】線形近似の測定感度関数２１５は、測定感
度関数を構築するように作られている。数３３式に置い
て提案された状態ベクトルについて、９個の送信機から
なる系の例においては、この近似は、
度関数を構築するように作られている。数３３式に置い
て提案された状態ベクトルについて、９個の送信機から
なる系の例においては、この近似は、
【０１１４】
【数４１】
【０１１５】になる。ここに、↑ｈl x,kは、時間ステッ
プｋにおいて推定された座標においてとられたｘに関す
る送信機ｌとの検知器の磁気相互作用の微分を示す。同
様に、他の項は、他の座標に関する他の相互作用の微分
を示す。速度の磁気相互作用への寄与は無視され、従っ
て速度に関する微分値はゼロになるので、数４１式の右
辺のマトリクスの右半分の項はゼロである。もし必要な
ら、磁気相互作用への速度の寄与を取り込むことは、簡
単である。
プｋにおいて推定された座標においてとられたｘに関す
る送信機ｌとの検知器の磁気相互作用の微分を示す。同
様に、他の項は、他の座標に関する他の相互作用の微分
を示す。速度の磁気相互作用への寄与は無視され、従っ
て速度に関する微分値はゼロになるので、数４１式の右
辺のマトリクスの右半分の項はゼロである。もし必要な
ら、磁気相互作用への速度の寄与を取り込むことは、簡
単である。
【０１１６】カルマンゲインマトリクス２２０の計算に
は、次の近似、
は、次の近似、
【０１１７】
【数４２】
【０１１８】を使用することができる。
【０１１９】例えば、９個の送信機からなる系において
は、↑Ｒは、
は、↑Ｒは、
【０１２０】
【数４３】
【０１２１】の形をとる。ここに、↑Ｒsは送信機ｓの
測定誤差の共変（ｃｏｖａｒｉａｎｃｅ）を表し、異な
る送信機の測定誤差は相関しないと仮定する。
測定誤差の共変（ｃｏｖａｒｉａｎｃｅ）を表し、異な
る送信機の測定誤差は相関しないと仮定する。
【０１２２】カルマンゲインマトリクス２２０は、順
次、予測状態ベクトル修正２２５と、推定誤差共変マト
リクス修正２３０に用いられる。予測状態ベクトル修正
２２５は、ゲインと、測定値と状態ベクトルに基づく予
測値との差（イノベーション（ｉｎｎｏｖａｔｉｏｎ
ｓ））との乗算、
次、予測状態ベクトル修正２２５と、推定誤差共変マト
リクス修正２３０に用いられる。予測状態ベクトル修正
２２５は、ゲインと、測定値と状態ベクトルに基づく予
測値との差（イノベーション（ｉｎｎｏｖａｔｉｏｎ
ｓ））との乗算、
【０１２３】
【数４４】
【０１２４】である。
【０１２５】推定誤差共変マトリクス修正２３０は、カ
ルマンゲインマトリクス２２０と測定感度関数２１５の
微分を含む
ルマンゲインマトリクス２２０と測定感度関数２１５の
微分を含む
【０１２６】
【数４５】
【０１２７】である。
【０１２８】このように、状態ベクトルと、誤差共変マ
トリクス↑Ｐ0と、↑Ｑと↑Ｒと↑ｈを用いたモデルへ
の初期計算２００の近似を行うことによって、↑ζkの
測定値に基づく一連の最適推定がもたらされる。
トリクス↑Ｐ0と、↑Ｑと↑Ｒと↑ｈを用いたモデルへ
の初期計算２００の近似を行うことによって、↑ζkの
測定値に基づく一連の最適推定がもたらされる。
【０１２９】初期計算２００の初期アレーの正確な値は
重要ではない。マトリクス↑Ｑと↑Ｒの値は系の以前の
知識から推定できる。すなわち、↑Ｒの値は測定ノイズ
から、↑Ｑの値は系における予期する加速度から計算で
きる。↑Ｐ0は任意の代表的な値をとる。例えば、毎秒
５ｍの速度が予期されれば、速度に関する↑Ｐ0の要素
は、毎秒２５ｍの速度をとるであろう。同様に、↑ξ0
も典型的な状態の値をとり、例えば、トラッキング範囲
が１立方ｍの範囲に限定され、速度が毎秒１ｍの範囲に
限定されなら、↑ξ0は１のベクトルになる。
重要ではない。マトリクス↑Ｑと↑Ｒの値は系の以前の
知識から推定できる。すなわち、↑Ｒの値は測定ノイズ
から、↑Ｑの値は系における予期する加速度から計算で
きる。↑Ｐ0は任意の代表的な値をとる。例えば、毎秒
５ｍの速度が予期されれば、速度に関する↑Ｐ0の要素
は、毎秒２５ｍの速度をとるであろう。同様に、↑ξ0
も典型的な状態の値をとり、例えば、トラッキング範囲
が１立方ｍの範囲に限定され、速度が毎秒１ｍの範囲に
限定されなら、↑ξ0は１のベクトルになる。
【０１３０】数３６式のビオ−サバールの法則の近似は
計算には便利ではないかもしれない。計算を容易にする
ために、検知器５０の位置（ｘd，ｙd，ｚd，φd，
θd）と送信機４０の位置（ｘs，ｙs，ｚs，φs，θs）
を用いてビオ−サバールの法則を近似する。その変換と
距離は次のように計算される。
計算には便利ではないかもしれない。計算を容易にする
ために、検知器５０の位置（ｘd，ｙd，ｚd，φd，
θd）と送信機４０の位置（ｘs，ｙs，ｚs，φs，θs）
を用いてビオ−サバールの法則を近似する。その変換と
距離は次のように計算される。
【０１３１】
【数４６】
【０１３２】これらの値は、３つの仮の値、
【０１３３】
【数４７】
【０１３４】を計算するのに用いられる。
【０１３５】磁気相互作用は、次のように計算される。
【０１３６】
【数４８】
【０１３７】ここに、Ｃ0は、望ましくは実験で定めら
れる係数である。検知器５０の位置と送信機４０の位置
とを知り、係数Ｃ0が分かれば、磁気相互作用は計算可
能である。
れる係数である。検知器５０の位置と送信機４０の位置
とを知り、係数Ｃ0が分かれば、磁気相互作用は計算可
能である。
【０１３８】改良ビオ−サバール近似 状態ベクトル、数３３式、が既知であれば、数４６〜４
８式は使用するのに便利であるが、依然貧弱な近似であ
る。物理的効果をより多く考慮して、磁気相互作用モデ
ルを改良すると、よりよいトラッキングの実行が可能に
なる。例えば、送信機のコイルの半径が、送信機と検知
器の間の距離に比して小さいと仮定する。本発明のこの
望ましい実施の形態においては、強い磁界の誘起が容易
であるので、大きい送信機を使用することが望ましい。
従って、検知器が送信機に接近したときには、Ａ1とＡ3
に次のような修正を施す。
８式は使用するのに便利であるが、依然貧弱な近似であ
る。物理的効果をより多く考慮して、磁気相互作用モデ
ルを改良すると、よりよいトラッキングの実行が可能に
なる。例えば、送信機のコイルの半径が、送信機と検知
器の間の距離に比して小さいと仮定する。本発明のこの
望ましい実施の形態においては、強い磁界の誘起が容易
であるので、大きい送信機を使用することが望ましい。
従って、検知器が送信機に接近したときには、Ａ1とＡ3
に次のような修正を施す。
【０１３９】
【数４９】
【０１４０】ここに、ρは送信機のコイルの半径であ
り、Ｒは数４６式によって定義されている。数４８、４
９式は、共にビオ−サバールの法則の近似である。数４
８式は、送信機と検知器が共に無限小の半径を有する場
合には厳密解に近づく。送信機の半径が増加すると、ビ
オ−サバールの法則の厳密解は、楕円積分の形で表され
る。数４９式は、テーラ展開数列のρ／Ｒにおける第１
項と第２項のみがとられたときの厳密解の近似である。
り、Ｒは数４６式によって定義されている。数４８、４
９式は、共にビオ−サバールの法則の近似である。数４
８式は、送信機と検知器が共に無限小の半径を有する場
合には厳密解に近づく。送信機の半径が増加すると、ビ
オ−サバールの法則の厳密解は、楕円積分の形で表され
る。数４９式は、テーラ展開数列のρ／Ｒにおける第１
項と第２項のみがとられたときの厳密解の近似である。
【０１４１】トラッキングを改良するための冗長測定の
使用 正確なトラッキングを得るのに必要な測定の数は現実的
に重要な問題である。ハードウエアの製造コストと計算
ロードを減少するためには、最小限の送信機数を有する
トラッキング装置を作ることが望ましい。全過程におい
て最もロードの大きい計算は、カルマンゲインマトリク
ス２２０の計算ステップの数４２式における逆マトリク
ス（ｉｎｖｅｒｓｅ ｍａｔｒｉｘ）の計算であり、逆
マトリクスのオーダは送信機の数に等しいので、送信機
の数を減らすと計算ロードは減少する。
使用 正確なトラッキングを得るのに必要な測定の数は現実的
に重要な問題である。ハードウエアの製造コストと計算
ロードを減少するためには、最小限の送信機数を有する
トラッキング装置を作ることが望ましい。全過程におい
て最もロードの大きい計算は、カルマンゲインマトリク
ス２２０の計算ステップの数４２式における逆マトリク
ス（ｉｎｖｅｒｓｅ ｍａｔｒｉｘ）の計算であり、逆
マトリクスのオーダは送信機の数に等しいので、送信機
の数を減らすと計算ロードは減少する。
【０１４２】このトラッキング装置は、５個の未知変数
座標があるけれども、５個の測定値は要しない。トラッ
キング装置は、現在の測定値のみならず過去の測定値も
最適に使用する。測定値の数が少ないほどトラッキング
は劣るようになる。しかし、最小の誤差推定のみが作り
出されるので、完全な解が得られなくても適度のトラッ
キングを得ることはできる。
座標があるけれども、５個の測定値は要しない。トラッ
キング装置は、現在の測定値のみならず過去の測定値も
最適に使用する。測定値の数が少ないほどトラッキング
は劣るようになる。しかし、最小の誤差推定のみが作り
出されるので、完全な解が得られなくても適度のトラッ
キングを得ることはできる。
【０１４３】可能な限り正確なトラッキングを提供する
ためには、測定数を増加することが望ましい。実際に
は、例えば送信機の位置が機械的衝撃の結果変化した
り、電子的不完全さや温度変化によって送信機の発振強
度が変化する結果、１またはそれ以上の送信機のパラメ
タの劣化がある場合がある。多数測定の装置では、カル
マンフィルタは、誤差が最小な出力を供給するように作
用し、不良送信機は幾らかの誤差をもたらす。送信機の
数が多いほど、誤差最小化における各送信機のウエイト
は小さくなり、推定がより正確になる。従って、送信機
の数を増加するとトラッキングの性能が改良される。装
置へ任意の数の送信機を組み込むことができるというこ
とは、本発明の望ましい実施の形態の明らかな利点であ
る。一般に、送信機の数を増加した場合、望ましくは検
知器のデジタル信号処理装置（ＤＳＰ）９０で実行され
る、ソフトウエアの変更のみで済む。
ためには、測定数を増加することが望ましい。実際に
は、例えば送信機の位置が機械的衝撃の結果変化した
り、電子的不完全さや温度変化によって送信機の発振強
度が変化する結果、１またはそれ以上の送信機のパラメ
タの劣化がある場合がある。多数測定の装置では、カル
マンフィルタは、誤差が最小な出力を供給するように作
用し、不良送信機は幾らかの誤差をもたらす。送信機の
数が多いほど、誤差最小化における各送信機のウエイト
は小さくなり、推定がより正確になる。従って、送信機
の数を増加するとトラッキングの性能が改良される。装
置へ任意の数の送信機を組み込むことができるというこ
とは、本発明の望ましい実施の形態の明らかな利点であ
る。一般に、送信機の数を増加した場合、望ましくは検
知器のデジタル信号処理装置（ＤＳＰ）９０で実行され
る、ソフトウエアの変更のみで済む。
【０１４４】トラッキング問題に最適化された状態ベク
トルの選択 数３３式の状態ベクトルは、それぞれの意味が図５に示
されている２つの方向座標φとθを含んでいる。これら
の座標は、角度θがｚ軸と方向ベクトルとの間の角度を
表し、角度φがｘ軸と方向ベクトルのｘ−ｙ面上への投
影との間の角度である、極座標表示である。θはゼロと
πラジアンの範囲にあり、φはゼロと２πラジアンの範
囲にある。
トルの選択 数３３式の状態ベクトルは、それぞれの意味が図５に示
されている２つの方向座標φとθを含んでいる。これら
の座標は、角度θがｚ軸と方向ベクトルとの間の角度を
表し、角度φがｘ軸と方向ベクトルのｘ−ｙ面上への投
影との間の角度である、極座標表示である。θはゼロと
πラジアンの範囲にあり、φはゼロと２πラジアンの範
囲にある。
【０１４５】この従来技術において普遍的な表現には問
題がある。図７は、漸次ｚ軸の方向に接近する方向ベク
トルを示し、そのθ座標は、φ座標は一定の間に、一定
の速度（負の）で減少する。方向ベクトルがθ＝０の点
を通過すると、座標は再び増加しはじめる。このよう
に、速度は瞬間的に符号を変え、速度の不連続をもたら
す。更に、φ座標に関しては、方向ベクトルがθ＝０の
点を通過すると、そのｘ−ｙ面上への投影は、反対の４
分円へ運動し、瞬間的な符号変化をもたらす。従って、
速度のみならず座標も不連続である。これらの座標の従
来のダイナミクス（ｄｙｎａｍｉｃｓ）を図８に示す。
題がある。図７は、漸次ｚ軸の方向に接近する方向ベク
トルを示し、そのθ座標は、φ座標は一定の間に、一定
の速度（負の）で減少する。方向ベクトルがθ＝０の点
を通過すると、座標は再び増加しはじめる。このよう
に、速度は瞬間的に符号を変え、速度の不連続をもたら
す。更に、φ座標に関しては、方向ベクトルがθ＝０の
点を通過すると、そのｘ−ｙ面上への投影は、反対の４
分円へ運動し、瞬間的な符号変化をもたらす。従って、
速度のみならず座標も不連続である。これらの座標の従
来のダイナミクス（ｄｙｎａｍｉｃｓ）を図８に示す。
【０１４６】本発明の望ましい実施の形態においては、
異なった方向座標系を用いる。この系（θ′，φ′）
は、次式、数５０式に示す写像（ｍａｐｐｉｎｇ）２３
５によって通常の極座標系（θ，φ）へ最も良く書き替
えられる。
異なった方向座標系を用いる。この系（θ′，φ′）
は、次式、数５０式に示す写像（ｍａｐｐｉｎｇ）２３
５によって通常の極座標系（θ，φ）へ最も良く書き替
えられる。
【０１４７】
【数５０】
【０１４８】この座標系を用いると、図７において示し
た運動に伴う不連続の除去が確かめられる。方向ベクト
ルはθ′＝０の点へ負の速度を持って接近し、そのθ′
座標は負になり、ゼロの速度を持つφ′座標はそのまま
の値を保つ。しかしながら、従来の表現へ変換されたと
きは、θ＝−θ′、φ＝φ′＋πである。
た運動に伴う不連続の除去が確かめられる。方向ベクト
ルはθ′＝０の点へ負の速度を持って接近し、そのθ′
座標は負になり、ゼロの速度を持つφ′座標はそのまま
の値を保つ。しかしながら、従来の表現へ変換されたと
きは、θ＝−θ′、φ＝φ′＋πである。
【０１４９】このダイナミクスを図９に示す。本発明の
この望ましい実施の形態においては、位置ベクトルの両
コンポーネントは正、負の任意の値をとり得、両座標と
それらの速度は常に連続である。
この望ましい実施の形態においては、位置ベクトルの両
コンポーネントは正、負の任意の値をとり得、両座標と
それらの速度は常に連続である。
【０１５０】従来の表現も、上に示し説明した本発明の
望ましい実施の形態の表現も、θ＝０のときのφ座標の
曖昧さを除去することはない。この特異性は、もし検知
器がθ＝０において止まれば、トラッキングの不連続と
して自身を表示し、物理的重要性を有しないφの値を発
生させ、運動を再開する。
望ましい実施の形態の表現も、θ＝０のときのφ座標の
曖昧さを除去することはない。この特異性は、もし検知
器がθ＝０において止まれば、トラッキングの不連続と
して自身を表示し、物理的重要性を有しないφの値を発
生させ、運動を再開する。
【０１５１】この座標系のダイナミックオフセット２４
０は、送信機の向きが上記の特異性に近づいたときに
は、極座標表示の軸を回転することによって、この問題
を解決する。この変換は、例えばｘ軸とｚ軸が取り替え
られるような、ｚ軸が異なる方向を指すような座表系の
回転に相当する。この特定の例における新しい系への変
換は、
０は、送信機の向きが上記の特異性に近づいたときに
は、極座標表示の軸を回転することによって、この問題
を解決する。この変換は、例えばｘ軸とｚ軸が取り替え
られるような、ｚ軸が異なる方向を指すような座表系の
回転に相当する。この特定の例における新しい系への変
換は、
【０１５２】
【数５１】
【０１５３】である。
【０１５４】この変換は、送信機のθが例えば１０°以
下になる毎に適用する。勿論、θの大きさをモニタし、
数５１式の変換を適用することは、数５０式の変換を適
用するときには避けられる計算上の負担をもたらすの
で、実行時に数学的曖昧さが現れたときのみ適用する。
下になる毎に適用する。勿論、θの大きさをモニタし、
数５１式の変換を適用することは、数５０式の変換を適
用するときには避けられる計算上の負担をもたらすの
で、実行時に数学的曖昧さが現れたときのみ適用する。
【０１５５】時間変更フィルタを使用する位置取得 数３７〜４５式で示したフィルタは、状態ベクトルの良
好な初期推定が可能であると仮定し、以後のトラッキン
グを予測−修正の逐次繰返しによって進めている。初期
推定が得られなければ、フィルタは発散するかもしれな
い。非線形の系への適用においては、初期推定が位相空
間における誤差関数の局部的最低値にくるかもしれない
ので、発散問題はより深刻になる。従って検知器の位置
決め問題は、２つに分離した問題、捕捉とトラッキング
になる。
好な初期推定が可能であると仮定し、以後のトラッキン
グを予測−修正の逐次繰返しによって進めている。初期
推定が得られなければ、フィルタは発散するかもしれな
い。非線形の系への適用においては、初期推定が位相空
間における誤差関数の局部的最低値にくるかもしれない
ので、発散問題はより深刻になる。従って検知器の位置
決め問題は、２つに分離した問題、捕捉とトラッキング
になる。
【０１５６】検知器５０の正確な位置は完全には決して
知られない。従って、たとえ劣ったものであっても任意
の初期推定値が過程を開始するのに役に立つと考えられ
るかも知れない。しかしながら、フィルタが発散しない
ようにするためには、方程式の一貫性を維持することが
望ましい。
知られない。従って、たとえ劣ったものであっても任意
の初期推定値が過程を開始するのに役に立つと考えられ
るかも知れない。しかしながら、フィルタが発散しない
ようにするためには、方程式の一貫性を維持することが
望ましい。
【０１５７】フィルタは、２つの織り混ぜられた動作、
状態ベクトルのトラッキングと、リカッチ（Ｒｉｃａｔ
ｔｉ）の差分方程式を解くこと、すなわち、↑Ｐについ
ての差分方程式を数値的に解くこととを実行する。リカ
ッチの方程式は、数３９式と数４５式とを用いて更新
（ｕｐｄａｔｅ）され、その解はその依存性を介して、
状態ベクトルの推定値に対して弱いフィードバックのみ
を有する。従って、結果の精度に殆ど関わりなく、自身
のペースで進行する。一方、状態ベクトルの推定は、↑
Ｐマトリクスが正確であると仮定して、↑Ｐマトリクス
を利用する。初期推定が極めて劣っていても、リカッチ
の方程式は収束し、誤差は未だ極めて大きいままで小さ
い↑Ｐが現れる。フィルタは、誤差は小さく、仮定は極
めて安定であるとみなして、大きなゲインがもたらされ
る。大きなゲインと大きなイノベーション（ｉｎｎｏｖ
ａｔｉｏｎ）とが組み合わされるとフィルタの発散が生
じる可能性がある。
状態ベクトルのトラッキングと、リカッチ（Ｒｉｃａｔ
ｔｉ）の差分方程式を解くこと、すなわち、↑Ｐについ
ての差分方程式を数値的に解くこととを実行する。リカ
ッチの方程式は、数３９式と数４５式とを用いて更新
（ｕｐｄａｔｅ）され、その解はその依存性を介して、
状態ベクトルの推定値に対して弱いフィードバックのみ
を有する。従って、結果の精度に殆ど関わりなく、自身
のペースで進行する。一方、状態ベクトルの推定は、↑
Ｐマトリクスが正確であると仮定して、↑Ｐマトリクス
を利用する。初期推定が極めて劣っていても、リカッチ
の方程式は収束し、誤差は未だ極めて大きいままで小さ
い↑Ｐが現れる。フィルタは、誤差は小さく、仮定は極
めて安定であるとみなして、大きなゲインがもたらされ
る。大きなゲインと大きなイノベーション（ｉｎｎｏｖ
ａｔｉｏｎ）とが組み合わされるとフィルタの発散が生
じる可能性がある。
【０１５８】劣った初期推定値を以ってフィルタがスタ
ートするのを許容するために、大きな↑Ｒまたは大きな
↑Ｑを人為的に維持することによって、リカッチの方程
式の収束を望ましくは遅くさせる。↑Ｒが大きいと、ゲ
インの式における逆マトリクス計算における↑Ｒの箇所
のために積分時間が長くなる。↑Ｑが大きいと、予測値
に↑Ｑは常に加算されるので、定常解は大きくなる。
ートするのを許容するために、大きな↑Ｒまたは大きな
↑Ｑを人為的に維持することによって、リカッチの方程
式の収束を望ましくは遅くさせる。↑Ｒが大きいと、ゲ
インの式における逆マトリクス計算における↑Ｒの箇所
のために積分時間が長くなる。↑Ｑが大きいと、予測値
に↑Ｑは常に加算されるので、定常解は大きくなる。
【０１５９】フィルタが進行し、より多くの測定値を
得、よりよい推定値を発生すると、大きな共変マトリク
スが障害になる。正確な予測が可能になれば、より簡潔
な予測ができ、従って測定値が十分に利用できる小さい
↑Ｒが望ましい。望ましくは、フィルタに、収束が卓越
する取得モードから、収束が保証され、精度がより重要
な、トラッキングモードへ切り換えられる機構を設け
る。この切り換えは、フィルタパラメタの急激な変化も
良くないので、望ましくはスムーズにする。
得、よりよい推定値を発生すると、大きな共変マトリク
スが障害になる。正確な予測が可能になれば、より簡潔
な予測ができ、従って測定値が十分に利用できる小さい
↑Ｒが望ましい。望ましくは、フィルタに、収束が卓越
する取得モードから、収束が保証され、精度がより重要
な、トラッキングモードへ切り換えられる機構を設け
る。この切り換えは、フィルタパラメタの急激な変化も
良くないので、望ましくはスムーズにする。
【０１６０】このような考慮から、時間変更パラメタ
（ｔｉｍｅ−ｖａｒｙｉｎｇ ｐａｒａｍｅｔｅｒｓ）
２４５の解が導かれる。状態ベクトルが良くは分かって
いない開始時点において、大きい↑Ｒと大きい↑Ｑと
が、大きい初期↑Ｐと共に供給される。しかし、↑Ｒと
↑Ｑは、系の真のノイズをより良く反映するコンポーネ
ントのみを残す、指数関数的に減衰するコンポーネント
からなる。例えば、↑Ｒと↑Ｑの時間依存性は次式の形
をとる。
（ｔｉｍｅ−ｖａｒｙｉｎｇ ｐａｒａｍｅｔｅｒｓ）
２４５の解が導かれる。状態ベクトルが良くは分かって
いない開始時点において、大きい↑Ｒと大きい↑Ｑと
が、大きい初期↑Ｐと共に供給される。しかし、↑Ｒと
↑Ｑは、系の真のノイズをより良く反映するコンポーネ
ントのみを残す、指数関数的に減衰するコンポーネント
からなる。例えば、↑Ｒと↑Ｑの時間依存性は次式の形
をとる。
【０１６１】
【数５２】
【０１６２】ここに、下添字ｋとｋ−１はｋ番目とｋ−
１番目の時間ステップのインデックスを、右肩の添字ｄ
ｅｃは減衰を、右肩の添字ｉｎｆは十分な時間経過後の
定常状態を、τdecayは減衰の時定数を、Ｔは時刻を、
それぞれ表す。
１番目の時間ステップのインデックスを、右肩の添字ｄ
ｅｃは減衰を、右肩の添字ｉｎｆは十分な時間経過後の
定常状態を、τdecayは減衰の時定数を、Ｔは時刻を、
それぞれ表す。
【０１６３】勿論、↑Ｐはリカッチの方程式の解の結果
として、漸次減衰する。↑Ｒと↑Ｑは、状態ベクトルに
ついての情報が最初に入手できないときに、取得モード
からトラッキングモードへの良好でスムーズな遷移がで
きるように、数百ミリ秒の時定数で減衰させる。
として、漸次減衰する。↑Ｒと↑Ｑは、状態ベクトルに
ついての情報が最初に入手できないときに、取得モード
からトラッキングモードへの良好でスムーズな遷移がで
きるように、数百ミリ秒の時定数で減衰させる。
【０１６４】自己校正の使用 数３７〜４５式に基づくトラッキングの精度は、全送信
機の位置の正確な認識に依存する。各送信機の５座標位
置は、その発振強度と半径と共に知られていることが望
ましい。送信機の位置はダイポール磁界を計算するのに
使用され、発振強度は比例係数Ｃ0に寄与し、半径は数
４９式におけるダイポール項への修正を計算するために
使用される。これらの各パラメタは、良好な校正を行っ
て直接測定される。実際には、例えば、プラスティック
の保護ケースに収納された電気コイルの物理的な寸法を
測定することは困難であり、正確な校正は面倒で、時に
は限定される。さらに、ほとんど全てのパラメタは、温
度変化による変動を受ける。したがって、装置に大まか
な校正を施して、パラメタを自動的に精密調整すること
が望ましい。
機の位置の正確な認識に依存する。各送信機の５座標位
置は、その発振強度と半径と共に知られていることが望
ましい。送信機の位置はダイポール磁界を計算するのに
使用され、発振強度は比例係数Ｃ0に寄与し、半径は数
４９式におけるダイポール項への修正を計算するために
使用される。これらの各パラメタは、良好な校正を行っ
て直接測定される。実際には、例えば、プラスティック
の保護ケースに収納された電気コイルの物理的な寸法を
測定することは困難であり、正確な校正は面倒で、時に
は限定される。さらに、ほとんど全てのパラメタは、温
度変化による変動を受ける。したがって、装置に大まか
な校正を施して、パラメタを自動的に精密調整すること
が望ましい。
【０１６５】比例係数Ｃ0の校正においては、Ｃ0の直接
測定が可能である。所望の精度が高いほど実験に時間が
かかる。代替的に、各送信機の状態要素Ｃiを加え、こ
れらのパラメタを最適化する新しいカルマンフィルタを
構築する。新しい状態ベクトルは望ましくは次式の形を
とる。
測定が可能である。所望の精度が高いほど実験に時間が
かかる。代替的に、各送信機の状態要素Ｃiを加え、こ
れらのパラメタを最適化する新しいカルマンフィルタを
構築する。新しい状態ベクトルは望ましくは次式の形を
とる。
【０１６６】
【数５３】
【０１６７】状態ベクトルが増加されたときには、マト
リクス↑Ｐ0、↑Ｑ、↑Φも次式のように増加されるの
が望ましい。
リクス↑Ｐ0、↑Ｑ、↑Φも次式のように増加されるの
が望ましい。
【０１６８】
【数５４】
【０１６９】ここに、↑Ｑcoorは上記のように定義され
たマトリクス↑Ｑであり、↑Ｑscは自己校正過程ノイズ
共変マトリクス（ｓｅｌｆ−ｃａｒｉｂｒａｔｉｏｎ
ｐｒｏｃｅｓｓ−ｎｏｉｓｅ ｃｏｖａｒｉａｎｃｅ−
ｍａｔｒｉｘ）である。このマトリクスは、比例係数Ｃ
0の最大許容変化率から計算されるか、一定の校正が要
求されるときにはゼロにセットされる。状態遷移マトリ
クス↑Φは、次式の形をとる。
たマトリクス↑Ｑであり、↑Ｑscは自己校正過程ノイズ
共変マトリクス（ｓｅｌｆ−ｃａｒｉｂｒａｔｉｏｎ
ｐｒｏｃｅｓｓ−ｎｏｉｓｅ ｃｏｖａｒｉａｎｃｅ−
ｍａｔｒｉｘ）である。このマトリクスは、比例係数Ｃ
0の最大許容変化率から計算されるか、一定の校正が要
求されるときにはゼロにセットされる。状態遷移マトリ
クス↑Φは、次式の形をとる。
【０１７０】
【数５５】
【０１７１】ここに、↑Φcoorは上記のように定義され
たマトリクス↑Φであり、↑Φscは係数遷移マトリクス
（ｃｏｅｆｆｉｃｉｅｎｔ ｔｒａｎｓｉｔｉｏｎ ｍ
ａｔｒｉｘ）である。後者は、状態に比例係数Ｃ0の時
間微分が含まれていなければ、一般に単位マトリクスに
等しい。
たマトリクス↑Φであり、↑Φscは係数遷移マトリクス
（ｃｏｅｆｆｉｃｉｅｎｔ ｔｒａｎｓｉｔｉｏｎ ｍ
ａｔｒｉｘ）である。後者は、状態に比例係数Ｃ0の時
間微分が含まれていなければ、一般に単位マトリクスに
等しい。
【０１７２】マトリクス↑Ｋ、↑Ｈも増加されるのが望
ましい。このためには、↑ｈの新しい状態要素（ｎｅｗ
ｓｔａｔｅ ｍｅｍｂｅｒｓ）に関する微分が一般に
要求される。この微分は、数４６〜４８式から解析的ま
たは数値計算的に計算され、それによってこの微分はよ
り便利または計算が早くなる。↑Ｐ0のサブマトリクス
は、校正の不確かさのサイズに依存する。比例係数Ｃ0
の自己校正の例については、マトリクス↑Ｈは次式の形
をとる。
ましい。このためには、↑ｈの新しい状態要素（ｎｅｗ
ｓｔａｔｅ ｍｅｍｂｅｒｓ）に関する微分が一般に
要求される。この微分は、数４６〜４８式から解析的ま
たは数値計算的に計算され、それによってこの微分はよ
り便利または計算が早くなる。↑Ｐ0のサブマトリクス
は、校正の不確かさのサイズに依存する。比例係数Ｃ0
の自己校正の例については、マトリクス↑Ｈは次式の形
をとる。
【０１７３】
【数５６】
【０１７４】同様に、他のパラメタの校正のために状態
要素を加えてもよい。例えば、ソースの方向φを考慮す
ると、
要素を加えてもよい。例えば、ソースの方向φを考慮す
ると、
【０１７５】
【数５７】
【０１７６】となり、あるいは任意の校正パラメタの組
合せがある。各組合せについて、マトリクス↑Ｐ0、↑
Ｑ、↑Φをそれに応じて構築するのが望ましい。
合せがある。各組合せについて、マトリクス↑Ｐ0、↑
Ｑ、↑Φをそれに応じて構築するのが望ましい。
【０１７７】自己校正が使用されるときには、送信機の
校正データも、図３の状態ベクトル２５５を用いて更新
することが望ましい。望ましくは、この更新は各逐次繰
返しごとに行う。しかし、更新が大幅でないと予期され
るなら、更新頻度を遅くする。
校正データも、図３の状態ベクトル２５５を用いて更新
することが望ましい。望ましくは、この更新は各逐次繰
返しごとに行う。しかし、更新が大幅でないと予期され
るなら、更新頻度を遅くする。
【０１７８】適応（ａｄａｐｔｉｖｅ）カルマンフィル
タの使用 フィルタパラメタの決定論的な時間減衰（ｄｅｔｅｒｍ
ｉｎｉｓｔｉｃ ｔｉｍｅ−ｄｅｃａｙ）は、適応的ア
プローチ（ａｄａｐｔｉｖｅ ａｐｐｒｏａｃｈ）へ一
般化される。例えば、トラッキングの結果からマトリク
ス↑Ｑ、↑Ｒを推定し、適応過程により適応的なマトリ
クス↑Ｑ、↑Ｒを導く。この例においては、マトリクス
↑Ｑは、運動方程式によってモデル化されない各運動要
素（ｋｉｎｅｔｉｃ ｃｏｍｐｏｎｅｎｔ）を記述し、
白色ノイズとしてモデル化する。
タの使用 フィルタパラメタの決定論的な時間減衰（ｄｅｔｅｒｍ
ｉｎｉｓｔｉｃ ｔｉｍｅ−ｄｅｃａｙ）は、適応的ア
プローチ（ａｄａｐｔｉｖｅ ａｐｐｒｏａｃｈ）へ一
般化される。例えば、トラッキングの結果からマトリク
ス↑Ｑ、↑Ｒを推定し、適応過程により適応的なマトリ
クス↑Ｑ、↑Ｒを導く。この例においては、マトリクス
↑Ｑは、運動方程式によってモデル化されない各運動要
素（ｋｉｎｅｔｉｃ ｃｏｍｐｏｎｅｎｔ）を記述し、
白色ノイズとしてモデル化する。
【０１７９】例えば、数３３式の状態ベクトルが用いら
れる場合には、２次時間微分即ち加速度は、マトリクス
↑Ｑの中に過程ノイズとして含まれるであろう。通常、
このマトリクスには、最悪のケースのシナリオに基づく
値が割り当てられる。予期される最大の加速度に基づい
てマトリクス↑Ｑを推定する。この選択は、↑Ｑが大き
いと大きな定常状態の↑Ｐとノイズに敏感なフィルタが
もたらされるので、常に最適とは限らない。他方、↑Ｑ
が小さいと、急速な位置変化が見落とされる可能性があ
る。
れる場合には、２次時間微分即ち加速度は、マトリクス
↑Ｑの中に過程ノイズとして含まれるであろう。通常、
このマトリクスには、最悪のケースのシナリオに基づく
値が割り当てられる。予期される最大の加速度に基づい
てマトリクス↑Ｑを推定する。この選択は、↑Ｑが大き
いと大きな定常状態の↑Ｐとノイズに敏感なフィルタが
もたらされるので、常に最適とは限らない。他方、↑Ｑ
が小さいと、急速な位置変化が見落とされる可能性があ
る。
【０１８０】数３３式の状態ベクトルにおいては、図３
の適応推定２５５は、次式の形をとる。
の適応推定２５５は、次式の形をとる。
【０１８１】
【数５８】
【０１８２】ここに、↑Ｑaccは、状態ベクトルの中に
見られる最高次の時間微分すなわち速度の微分に対応す
る↑Ｑの成分を表す。２乗の項は、検知器の加速度の推
定値である。定数αはゲイン係数であり、ゲインが高く
なるほど加速度の推定値が平均化される周期が短くな
る。↑Ｑ0 accは、定数で、時間に独立な項である。
見られる最高次の時間微分すなわち速度の微分に対応す
る↑Ｑの成分を表す。２乗の項は、検知器の加速度の推
定値である。定数αはゲイン係数であり、ゲインが高く
なるほど加速度の推定値が平均化される周期が短くな
る。↑Ｑ0 accは、定数で、時間に独立な項である。
【０１８３】同様に、マトリクス↑Ｒも望ましくは適応
推定２５０のようなアプローチの恩恵を受けることがで
きる。↑Ｒが大き過ぎる場合には、装置は検知器のコー
スの変化にゆっくりにしか反応しない。↑Ｒが小さ過ぎ
る場合には、フィルタが発散する可能性がある。このマ
トリクスは、系の物理モデルに含まれない効果を白色ノ
イズとしてモデル化するので、その適応（ａｄａｐｔａ
ｔｉｏｎ）は、イノベーション（ｉｎｎｏｖａｔｉｏ
ｎ）｛↑ζk−↑ｈ［↑ξk（−）］｝に依存し、
推定２５０のようなアプローチの恩恵を受けることがで
きる。↑Ｒが大き過ぎる場合には、装置は検知器のコー
スの変化にゆっくりにしか反応しない。↑Ｒが小さ過ぎ
る場合には、フィルタが発散する可能性がある。このマ
トリクスは、系の物理モデルに含まれない効果を白色ノ
イズとしてモデル化するので、その適応（ａｄａｐｔａ
ｔｉｏｎ）は、イノベーション（ｉｎｎｏｖａｔｉｏ
ｎ）｛↑ζk−↑ｈ［↑ξk（−）］｝に依存し、
【０１８４】
【数５９】
【０１８５】である。ここに、表記は数５８式と類似で
あり、右肩の添字ｓは、測定ノイズ共変を計算する送信
機を表す。↑Ｒ0 accは、定数で、時間に独立な項であ
る。
あり、右肩の添字ｓは、測定ノイズ共変を計算する送信
機を表す。↑Ｒ0 accは、定数で、時間に独立な項であ
る。
【０１８６】本明細書において、マトリクスの右肩の添
字ｔ、Ｔは、転置マトリクスを示す。 上記説明した本
発明の望ましい実施の形態は、拡張カルマンフィルタ、
すなわち最適非線形フィルタへの１次近似のみを考慮し
た。改良フィルタ（例えば従来技術で挙げたクシュナの
１９６７年の著作参照。）、拡張カルマンフィルタの２
次近似とガウスフィルタを含むもの（共に、従来技術で
挙げたジャツウィンスキの１９７１年の著作参照。）、
ガウス合計フィルタ（従来技術で挙げたアンダスンとム
ーアの１９７９年の著作参照。）及びその他の既知の近
似の変形版は、本願発明の代替の実施の形態へ容易に適
用可能であり、その誘導は簡単である。
字ｔ、Ｔは、転置マトリクスを示す。 上記説明した本
発明の望ましい実施の形態は、拡張カルマンフィルタ、
すなわち最適非線形フィルタへの１次近似のみを考慮し
た。改良フィルタ（例えば従来技術で挙げたクシュナの
１９６７年の著作参照。）、拡張カルマンフィルタの２
次近似とガウスフィルタを含むもの（共に、従来技術で
挙げたジャツウィンスキの１９７１年の著作参照。）、
ガウス合計フィルタ（従来技術で挙げたアンダスンとム
ーアの１９７９年の著作参照。）及びその他の既知の近
似の変形版は、本願発明の代替の実施の形態へ容易に適
用可能であり、その誘導は簡単である。
【０１８７】本願発明のソフトウエアの部分は、要すれ
ば、ＲＯＭ（呼出し専用メモリ）に書き込んで実行でき
ることは容易に理解されるであろう。本願発明のソフト
ウエアの部分は、一般的に、要すれば、従来の技術を用
いてハードウエアに組み込んで実行してもよい。
ば、ＲＯＭ（呼出し専用メモリ）に書き込んで実行でき
ることは容易に理解されるであろう。本願発明のソフト
ウエアの部分は、一般的に、要すれば、従来の技術を用
いてハードウエアに組み込んで実行してもよい。
【０１８８】簡単にするために、別々の実施の形態とし
て説明した本発明の種々の特徴を一つの実施の形態に組
み合わせて実行してもよい。逆に、簡単にするために、
一つの実施の形態として記述された種々の特徴を、別々
に、或いは部分的に組み合わせて実行してもよい。
て説明した本発明の種々の特徴を一つの実施の形態に組
み合わせて実行してもよい。逆に、簡単にするために、
一つの実施の形態として記述された種々の特徴を、別々
に、或いは部分的に組み合わせて実行してもよい。
【０１８９】当業者は、本発明は上記に特に示し記載し
たものに限定されるものではないことを理解できるであ
ろう。本発明の範囲は、請求の範囲によってのみ限定さ
れるものである。
たものに限定されるものではないことを理解できるであ
ろう。本発明の範囲は、請求の範囲によってのみ限定さ
れるものである。
【０１９０】
【発明の効果】以上説明したように、本発明に係る移動
物体位置のトラッキング装置およびトラッキング方法に
よれば、移動物体の近傍において測定した磁束の測定値
を上述の非線形カルマンフィルタを応用して処理するこ
とによって、移動物体の位置を検出し、追跡することが
容易になるという効果がある。
物体位置のトラッキング装置およびトラッキング方法に
よれば、移動物体の近傍において測定した磁束の測定値
を上述の非線形カルマンフィルタを応用して処理するこ
とによって、移動物体の位置を検出し、追跡することが
容易になるという効果がある。
【図１】ビオ−サバールベースの拡張カルマンフィルタ
型のトラッキングフィルタの望ましい実施手順を示す簡
単化した流れ図の一部である。
型のトラッキングフィルタの望ましい実施手順を示す簡
単化した流れ図の一部である。
【図２】ビオ−サバールベースの拡張カルマンフィルタ
型のトラッキングフィルタの望ましい実施手順を示す簡
単化した流れ図の一部である。
型のトラッキングフィルタの望ましい実施手順を示す簡
単化した流れ図の一部である。
【図３】ビオ−サバールベースの拡張カルマンフィルタ
型のトラッキングフィルタの望ましい実施手順を示す簡
単化した流れ図の一部である。
型のトラッキングフィルタの望ましい実施手順を示す簡
単化した流れ図の一部である。
【図４】図１〜３に示す手順によって実行するビオ−サ
バールベースの拡張カルマンフィルタ型のトラッキング
フィルタを含むトラッキング装置の１例の簡単化したブ
ロック図である。
バールベースの拡張カルマンフィルタ型のトラッキング
フィルタを含むトラッキング装置の１例の簡単化したブ
ロック図である。
【図５】状態ベクトルの方向を示すベクトル図である。
【図６】ビオ−サバールの法則を図示するベクトル図で
ある。
ある。
【図７】特異点を通過する検知器の状態を簡単化して示
す図である。
す図である。
【図８】図７に示す従来の極方向の座標のダイナミクス
を簡単化して示す図である。
を簡単化して示す図である。
【図９】本発明の望ましい実施の形態による、図７に示
す極方向の座標のダイナミクスを簡単化して示す図であ
る。
す極方向の座標のダイナミクスを簡単化して示す図であ
る。
４０…送信機 ５０…検知器 ６０…同期装置 ７０…輪郭検出器 ８０…トラッキング非線形フィルタ（トラッカー） ９０…デジタル信号処理装置（ＤＳＰ） ２００…初期計算 ２０５…予測状態ベクトル ２１０…推定誤差共変マトリクス ２１５…測定感度関数 ２２０…カルマンゲインマトリクス ２２５…予測状態ベクトル修正 ２３０…推定誤差共変マトリクス修正 ２４０…ダイナミックオフセット ２４５…時間変更パラメタ ２５０…適応推定 ２５５…状態ベクトル
───────────────────────────────────────────────────── フロントページの続き (72)発明者 エユーダ アルベック イスラエル国 マール アドミム 90610 ハーローテム ストリート 70 (72)発明者 ボウアッズ スピーヴァク イスラエル国 ペタク ティクヴァ 49272 ジョナ グリーン ストリート 20
Claims (30)
- 【請求項１】移動物体の近傍において測定した磁束の値
に基づいて移動物体の位置を追跡する移動物体位置のト
ラッキング装置において、 上記移動物体の近傍において測定された磁束の測定値を
受けて、上記測定値に非線形カルマンフィルタを適用
し、上記移動物体の位置に関する情報を出力する、非線
形カルマンフィルタベースのトラッカと、 上記移動物体の位置に関する上記情報を表示する位置表
示器と、 を含んでなることを特徴とする移動物体位置のトラッキ
ング装置。 - 【請求項２】非線形カルマンフィルタベースの上記トラ
ッカは、拡張カルマンフィルタベースのトラッカである
ことを特徴とする請求項１に記載の移動物体位置のトラ
ッキング装置。 - 【請求項３】非線形カルマンフィルタベースの上記トラ
ッカは、上記移動物体の上記位置から上記移動物体の近
傍において測定した上記磁束の値へビオ−サバール変換
を適用することを特徴とする請求項１に記載の移動物体
位置のトラッキング装置。 - 【請求項４】上記非線形カルマンフィルタは、要素が位
置座標と位置座標の１次微分とを含む状態ベクトルに作
用することを特徴とする請求項１に記載の移動物体位置
のトラッキング装置。 - 【請求項５】上記位置座標は、３個の空間座標と２個の
方向座標とからなることを特徴とする請求項４に記載の
移動物体位置のトラッキング装置。 - 【請求項６】上記ビオ−サバール変換を適用する過程
は、次式に示す状態ベクトル↑ξの測定感度関数↑ｈの
計算からなることを特徴とする請求項３に記載の移動物
体位置のトラッキング装置。 【数１】 ここに、Ｃ0は係数、Ｒは、上記磁束を誘起する複数の
送信機からなる送信機アレーからの上記磁束の上記測定
値を検知する検知器と各上記送信機との間の距離、およ
び、 【数２】 であり、ここに検知器の位置は｛ｘd，ｙd，ｚd，φd，
θd｝、送信機の位置は｛ｘs，ｙs，ｚs，φs，θs｝で
あり、δｘ、δｙ、δｚは、それぞれ検知器の位置と送
信機の位置との間の距離のｘ、ｙ、ｚ成分を表す。 - 【請求項７】非線形カルマンフィルタベースの上記トラ
ッカは、少なくとも上記移動物体が上記磁束を誘起する
複数の送信機からなる送信機アレー内の１つの送信機に
近接したときに、楕円積分を表すテイラー展開数列の第
１項と第２項とを計算することにより、上記楕円積分を
近似することを特徴とする請求項１に記載の移動物体位
置のトラッキング装置。 - 【請求項８】上記移動物体の近傍に磁束を誘起する６個
以下の有効な送信機からなる送信機アレーをさらに含ん
でなることを特徴とする請求項１に記載の移動物体位置
のトラッキング装置。 - 【請求項９】移動物体の近傍において測定した磁束の値
に基づいて移動物体の位置を追跡する移動物体位置のト
ラッキング方法において、 上記移動物体の近傍において測定された磁束の測定値を
受け、上記測定値に非線形カルマンフィルタを適用し、
上記移動物体の位置に関する情報を出力し、 上記移動物体の位置に関する上記情報の出力表示を供給
する、 ことを含んでなることを特徴とする移動物体位置のトラ
ッキング方法。 - 【請求項１０】上記非線形カルマンフィルタは、以下の
マトリクスと操作を適用することを特徴とする請求項１
に記載の移動物体位置のトラッキング装置。 【数３】 ここに、ｋは時間ステップのインデックス、↑ξ
k（−）は状態ベクトルの予測値、↑ξk（＋）は状態ベ
クトルの修正後の予測値、↑Φは状態遷移マトリクス、 【数４】 ここに、↑Ｐ（−）は推定誤差共変マトリクスの予測
値、↑Ｐ（＋）は推定誤差共変マトリクスの修正値、↑
Ｑは過程ノイズ共変マトリクス、 【数５】 ここに、↑ｈは測定感度関数、↑ξは状態ベクトル、 【数６】 ここに、↑Ｒkは測定ノイズ共変マトリクス、 【数７】 ここに、↑ζは、上記移動物体の近傍において測定され
た上記磁束の測定値を表し、 【数８】 である。 - 【請求項１１】非線形カルマンフィルタベースの上記ト
ラッカは、上記移動物体の上記位置から、及び上記測定
値によって計量される磁束を誘起する磁気送信機のアレ
ーの位置から、上記移動物体の近傍において測定した上
記磁束の値へ、少なくとも近似されたビオ−サバール変
換を適用することを特徴とする請求項１に記載の移動物
体位置のトラッキング装置。 - 【請求項１２】上記少なくとも近似されたビオ−サバー
ル変換は、近似されたビオ−サバール変換からなること
を特徴とする請求項１１に記載の移動物体位置のトラッ
キング装置。 - 【請求項１３】非線形カルマンフィルタベースの上記ト
ラッカは、状態ベクトル↑ξの測定感度関数↑ｈを、以
下のように計算するように作用することを特徴とする請
求項１に記載の移動物体位置のトラッキング装置。 【数９】 ここに、Ｃ0は係数、Ｒは、上記磁束を誘起する複数の
送信機からなる送信機アレーの上記磁束の上記測定値を
検知する検知器と各上記送信機との間の距離、および、 【数１０】 ここに、検知器の位置は（ｘd，ｙd，ｚd，φd，θd）
であり、送信機の位置は（ｘs，ｙs，ｚs，φs，θs）
であり、δｘ、δｙ、δｚ、は、それぞれ検知器の位置
と送信機の位置との間の距離のｘ、ｙ、ｚ成分を表す。 - 【請求項１４】非線形カルマンフィルタベースの上記ト
ラッカは、少なくとも上記移動物体が上記測定値によっ
て計量される磁束を誘起する送信機に近接したときに、
楕円積分を表すテイラー展開数列の第１項と第２項とを
計算することにより、上記楕円積分の近似を計算するよ
うに作用することを特徴とする請求項１に記載の移動物
体位置のトラッキング装置。 - 【請求項１５】上記楕円積分の近似は、次式からなるこ
とを特徴とする請求項１４に記載の移動物体位置のトラ
ッキング装置。 【数１１】 ここに、ρは送信機のコイルの半径、Ｒは、上記磁束の
上記測定値を検知する検知器と上記測定値によって測定
される上記磁束を誘起する上記送信機との間の距離であ
る。 - 【請求項１６】上記磁束の測定値は、上記移動物体の近
傍における６個以下の磁束の測定値からなることを特徴
とする請求項１に記載の移動物体位置のトラッキング装
置。 - 【請求項１７】上記移動物体の上記位置の方向成分は、
時間について連続な２つの角度、θ′とφ′とによって
表され、θ′とφ′の従来の極座標θとφとの関係は、
次式のとおりであることを特徴とする請求項１に記載の
移動物体位置のトラッキング装置。 【数１２】 - 【請求項１８】非線形カルマンフィルタベースの上記ト
ラッカは、特異性を避けるために、上記移動物体の上記
位置の方向成分のダイナミックオフセットを使用するこ
とを特徴とする請求項１に記載の移動物体位置のトラッ
キング装置。 - 【請求項１９】上記ダイナミックオフセットは、下記の
変換によって記述されることを特徴とする請求項１８に
記載の移動物体位置のトラッキング装置。 【数１３】 ここに、θとφは、ダイナミックオフセット後の上記移
動物体の上記位置の方向成分からなり、θ′とφ′は、
ダイナミックオフセット前の上記移動物体の上記位置の
方向成分からなる。 - 【請求項２０】非線形カルマンフィルタベースの上記ト
ラッカは、測定ノイズ共変マトリクス↑Ｒと過程ノイズ
共変マトリクス↑Ｑを時間変更させるように作用するこ
とを特徴とする請求項１に記載の移動物体位置のトラッ
キング装置。 - 【請求項２１】上記測定ノイズ共変マトリクス↑Ｒと上
記過程ノイズ共変マトリクス↑Ｑの上記時間変更は、次
式からなることを特徴とする請求項２０に記載の移動物
体位置のトラッキング装置。 【数１４】 ここに、下添字ｋとｋ−１はｋ番目とｋ−１番目の時間
ステップのインデックスを、右肩の添字ｄｅｃは減衰
を、右肩の添字ｉｎｆは十分な時間経過後の定常状態
を、τdecayは減衰の時定数を、Ｔは時刻を、それぞれ
表す。 - 【請求項２２】上記測定値によって計量される磁束を誘
起する少なくとも１個の自己校正送信機を更に含んでな
ることを特徴とする請求項１に記載の移動物体位置のト
ラッキング装置。 - 【請求項２３】上記非線形カルマンフィルタは、各上記
自己校正送信機の位置を校正するように作用することを
特徴とする請求項２２に記載の移動物体位置のトラッキ
ング装置。 - 【請求項２４】上記非線形カルマンフィルタは、各上記
自己校正送信機の発振強度を校正するように作用するこ
とを特徴とする請求項２２に記載の移動物体位置のトラ
ッキング装置。 - 【請求項２５】上記非線形カルマンフィルタは、各上記
自己校正送信機の半径を校正するように作用することを
特徴とする請求項２２に記載の移動物体位置のトラッキ
ング装置。 - 【請求項２６】上記非線形カルマンフィルタは、各上記
自己校正送信機の向きを校正するように作用することを
特徴とする請求項２２に記載の移動物体位置のトラッキ
ング装置。 - 【請求項２７】上記トラッカは、成分が上記自己校正送
信機の特性からなる状態ベクトルを用い、上記特性の少
なくとも１つは自己校正されることを特徴とする請求項
２２に記載の移動物体位置のトラッキング装置。 - 【請求項２８】非線形カルマンフィルタベースの上記ト
ラッカは、少なくとも１つは適応過程によって発生され
る測定ノイズ共変マトリクス↑Ｒと過程ノイズ共変マト
リクス↑Ｑを使用することを特徴とする請求項１に記載
の移動物体位置のトラッキング装置。 - 【請求項２９】上記過程ノイズ共変マトリクス↑Ｑのた
めの上記適応過程は、次式からなることを特徴とする請
求項２８に記載の移動物体位置のトラッキング装置。 【数１５】 ここに、↑Ｑaccは、状態ベクトルの中に見られる最高
次の時間微分に対応する↑Ｑの成分、下添字ｋとｋ−１
はｋ番目とｋ−１番目の時間ステップのインデックス、
↑ξは状態ベクトル、右肩の添字ｖｅｌｏｃｉｔｙは微
分値すなわ速度を示し、Ｔは時刻、αはゲイン係数であ
る。 - 【請求項３０】上記測定ノイズ共変マトリクス↑Ｒのた
めの上記適応過程は、次式からなることを特徴とする請
求項２８または２９に記載の移動物体位置のトラッキン
グ装置。 【数１６】 ここに、右肩の添字ｓは測定ノイズ共変マトリクス↑Ｒ
を計算する送信機を表し、下添字ｋとｋ−１はｋ番目と
ｋ−１番目の時間ステップのインデックス、↑ξは状態
ベクトル、↑ξk（−）は状態ベクトルの予測値、↑ζ
は磁束の測定値、↑ｈは測定感度関数、βはゲイン係数
である。
Applications Claiming Priority (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| IL127868 | 1998-12-30 | ||
| IL12786898A IL127868A0 (en) | 1998-12-30 | 1998-12-30 | Localization and tracking |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| JP2000213906A true JP2000213906A (ja) | 2000-08-04 |
Family
ID=11072324
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP11373155A Pending JP2000213906A (ja) | 1998-12-30 | 1999-12-28 | 移動物体位置のトラッキング装置およびトラッキング方法 |
Country Status (2)
| Country | Link |
|---|---|
| JP (1) | JP2000213906A (ja) |
| IL (1) | IL127868A0 (ja) |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2008064555A (ja) * | 2006-09-06 | 2008-03-21 | Japan Radio Co Ltd | 移動体姿勢計測装置 |
| JP2009509153A (ja) * | 2005-09-23 | 2009-03-05 | エディサンス | センサ、及び位置及び速度を測定するための方法 |
| CN104377447A (zh) * | 2014-10-27 | 2015-02-25 | 北京遥测技术研究所 | 一种基于Kalman滤波器的机动目标跟踪控制系统 |
| CN115790589A (zh) * | 2023-01-09 | 2023-03-14 | 西北工业大学 | 一种发射系无误差捷联惯性导航方法 |
| CN115964603A (zh) * | 2023-02-10 | 2023-04-14 | 成都理工大学 | 一种基于改进Kalman滤波的机动目标跟踪方法 |
| CN117388748A (zh) * | 2023-12-11 | 2024-01-12 | 天津航天瑞莱科技有限公司 | 一种船用逆变电源装置的综合环境试验系统 |
-
1998
- 1998-12-30 IL IL12786898A patent/IL127868A0/xx unknown
-
1999
- 1999-12-28 JP JP11373155A patent/JP2000213906A/ja active Pending
Cited By (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2009509153A (ja) * | 2005-09-23 | 2009-03-05 | エディサンス | センサ、及び位置及び速度を測定するための方法 |
| JP2008064555A (ja) * | 2006-09-06 | 2008-03-21 | Japan Radio Co Ltd | 移動体姿勢計測装置 |
| CN104377447A (zh) * | 2014-10-27 | 2015-02-25 | 北京遥测技术研究所 | 一种基于Kalman滤波器的机动目标跟踪控制系统 |
| CN115790589A (zh) * | 2023-01-09 | 2023-03-14 | 西北工业大学 | 一种发射系无误差捷联惯性导航方法 |
| CN115790589B (zh) * | 2023-01-09 | 2023-05-02 | 西北工业大学 | 一种发射系无误差捷联惯性导航方法 |
| CN115964603A (zh) * | 2023-02-10 | 2023-04-14 | 成都理工大学 | 一种基于改进Kalman滤波的机动目标跟踪方法 |
| CN115964603B (zh) * | 2023-02-10 | 2023-05-30 | 成都理工大学 | 一种基于改进Kalman滤波的机动目标跟踪方法 |
| CN117388748A (zh) * | 2023-12-11 | 2024-01-12 | 天津航天瑞莱科技有限公司 | 一种船用逆变电源装置的综合环境试验系统 |
| CN117388748B (zh) * | 2023-12-11 | 2024-05-28 | 天津航天瑞莱科技有限公司 | 一种船用逆变电源装置的综合环境试验系统 |
Also Published As
| Publication number | Publication date |
|---|---|
| IL127868A0 (en) | 2001-01-28 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Nazarahari et al. | 40 years of sensor fusion for orientation tracking via magnetic and inertial measurement units: Methods, lessons learned, and future challenges | |
| US6487516B1 (en) | System for three dimensional positioning and tracking with dynamic range extension | |
| US6484131B1 (en) | Localization and tracking system | |
| US6691074B1 (en) | System for three dimensional positioning and tracking | |
| US7970491B2 (en) | Robot localization system | |
| US10408627B2 (en) | Inertial device to estimate position based on corrected movement velocity | |
| US8374624B2 (en) | Location measurement method based on predictive filter | |
| US7991576B2 (en) | Indoor navigation system and method | |
| US5060175A (en) | Measurement and control system for scanning sensors | |
| US4520445A (en) | Method of determining the position and velocity of a vehicle | |
| WO1987002469A1 (en) | Signal processor for inertial measurement using coriolis force sensing accelerometer arrangements | |
| CN105928514A (zh) | 基于图像与惯性技术的agv复合导引系统 | |
| JP2003521697A (ja) | 変形４元数データ表記法を用いる傾動可能な物体の姿勢推定 | |
| CN105136145A (zh) | 一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法 | |
| JP2011503571A (ja) | 物体の方位測定 | |
| KR19980042029A (ko) | 스트랩다운 관성 항법 시스템에서의 스컬링 보정을 위한 방법 및 장치 | |
| US5023799A (en) | Vehicular traveling direction measuring system | |
| He et al. | Adaptive error-state Kalman filter for attitude determination on a moving platform | |
| CA2477677A1 (en) | Autonomous velocity estimation and navigation | |
| JP2000213906A (ja) | 移動物体位置のトラッキング装置およびトラッキング方法 | |
| Wang et al. | A robust backtracking CKF based on Krein space theory for in-motion alignment process | |
| CN114459466A (zh) | 一种基于模糊控制的mems多传感器数据融合处理方法 | |
| CN111854741B (zh) | 一种gnss/ins紧组合滤波器及导航方法 | |
| Yuan et al. | Reaearch on underwater integrated navigation system based on SINS/DVL/magnetometer/depth-sensor | |
| CN106872933A (zh) | 一种电磁矢量传感器阵元姿态位置六维误差自校正方法 |